sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

sensord: lower temperature sensor freq to 2Hz (#29563)

Browse Source
pull/29564/head
Adeeb Shihadeh 2 years ago committed by GitHub
parent e726505918
commit 42769345d5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 44 additions and 60 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 37
      system/sensord/sensors_qcom2.cc
  2. 65
      system/sensord/tests/test_sensord.py

37
system/sensord/sensors_qcom2.cc
Unescape View File

@ -7,6 +7,7 @@
#include <poll.h>
#include <linux/gpio.h>
#include "cereal/services.h"
#include "cereal/messaging/messaging.h"
#include "common/i2c.h"
#include "common/ratekeeper.h"
@ -27,11 +28,11 @@
ExitHandler do_exit;
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> sensors) {
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool>> sensors) {
PubMaster pm({"gyroscope", "accelerometer"});
int fd = -1;
for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
for (auto &[sensor, msg_name, required] : sensors) {
if (sensor->has_interrupt_enabled()) {
fd = sensor->gpio_fd;
break;
@ -71,7 +72,7 @@ void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> se
uint64_t offset = nanos_since_epoch() - nanos_since_boot();
uint64_t ts = evdata[num_events - 1].timestamp - offset;
for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
for (auto &[sensor, msg_name, required] : sensors) {
if (!sensor->has_interrupt_enabled()) {
continue;
}
@ -90,16 +91,16 @@ void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> se
}
// poweroff sensors, disable interrupts
for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
for (auto &[sensor, msg_name, required] : sensors) {
if (sensor->has_interrupt_enabled()) {
sensor->shutdown();
}
}
}
void polling_loop(Sensor *sensor, std::string msg_name, int frequency) {
void polling_loop(Sensor *sensor, std::string msg_name) {
PubMaster pm({msg_name.c_str()});
RateKeeper rk(msg_name, frequency);
RateKeeper rk(msg_name, services.at(msg_name).frequency);
while (!do_exit) {
MessageBuilder msg;
if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
@ -113,22 +114,22 @@ void polling_loop(Sensor *sensor, std::string msg_name, int frequency) {
int sensor_loop(I2CBus *i2c_bus_imu) {
// Sensor init
std::vector<std::tuple<Sensor *, std::string, bool, int>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2", false, 100},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2", false, 100},
{new BMX055_Magn(i2c_bus_imu), "magnetometer", false, 100},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2", false, 100},
std::vector<std::tuple<Sensor *, std::string, bool>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2", false},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2", false},
{new BMX055_Magn(i2c_bus_imu), "magnetometer", false},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2", false},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer", true, 100},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope", true, 100},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor", true, 100},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer", true},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope", true},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor", true},
{new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false, 100},
{new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false},
};
// Initialize sensors
std::vector<std::thread> threads;
for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) {
for (auto &[sensor, msg_name, required] : sensors_init) {
int err = sensor->init();
if (err < 0) {
if (required) {
@ -139,7 +140,7 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
}
if (!sensor->has_interrupt_enabled()) {
threads.emplace_back(polling_loop, sensor, msg_name, polling_freq);
threads.emplace_back(polling_loop, sensor, msg_name);
}
}
@ -156,7 +157,7 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
t.join();
}
for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) {
for (auto &[sensor, msg_name, required] : sensors_init) {
delete sensor;
}
return 0;

65
system/sensord/tests/test_sensord.py
Unescape View File

@ -7,6 +7,7 @@ from collections import namedtuple, defaultdict
import cereal.messaging as messaging
from cereal import log
from cereal.services import service_list
from openpilot.common.gpio import get_irqs_for_action
from openpilot.system.hardware import TICI
from openpilot.selfdrive.manager.process_config import managed_processes
@ -37,29 +38,29 @@ SENSOR_CONFIGURATIONS = (
)
Sensor = log.SensorEventData.SensorSource
SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max', 'expected_freq'])
SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
ALL_SENSORS = {
Sensor.lsm6ds3: {
SensorConfig("acceleration", 5, 15, 100),
SensorConfig("gyroUncalibrated", 0, .2, 100),
SensorConfig("temperature", 0, 60, 100),
SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("temperature", 0, 60),
},
Sensor.lsm6ds3trc: {
SensorConfig("acceleration", 5, 15, 104),
SensorConfig("gyroUncalibrated", 0, .2, 104),
SensorConfig("temperature", 0, 60, 100),
SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("temperature", 0, 60),
},
Sensor.bmx055: {
SensorConfig("acceleration", 5, 15, 100),
SensorConfig("gyroUncalibrated", 0, .2, 100),
SensorConfig("magneticUncalibrated", 0, 300, 100),
SensorConfig("temperature", 0, 60, 100),
SensorConfig("acceleration", 5, 15),
SensorConfig("gyroUncalibrated", 0, .2),
SensorConfig("magneticUncalibrated", 0, 300),
SensorConfig("temperature", 0, 60),
},
Sensor.mmc5603nj: {
SensorConfig("magneticUncalibrated", 0, 300, 100),
SensorConfig("magneticUncalibrated", 0, 300),
}
}
@ -85,7 +86,7 @@ def read_sensor_events(duration_sec):
assert sum(map(len, events.values())) != 0, "No sensor events collected!"
return events
return {k: v for k, v in events.items() if len(v) > 0}
class TestSensord(unittest.TestCase):
@classmethod
@ -119,7 +120,6 @@ class TestSensord(unittest.TestCase):
def test_sensors_present(self):
# verify correct sensors configuration
seen = set()
for etype in self.events:
for measurement in self.events[etype]:
@ -159,22 +159,12 @@ class TestSensord(unittest.TestCase):
stddev = np.std(tdiffs)
assert stddev < 2.0, f"Standard-dev to big {stddev}"
def test_events_check(self):
# verify if all sensors produce events
sensor_events = dict()
for etype in self.events:
for measurement in self.events[etype]:
m = getattr(measurement, measurement.which())
if m.type in sensor_events:
sensor_events[m.type] += 1
else:
sensor_events[m.type] = 1
for s in sensor_events:
err_msg = f"Sensor {s}: 200 < {sensor_events[s]}"
assert sensor_events[s] > 200, err_msg
def test_sensor_frequency(self):
for s, msgs in self.events.items():
with self.subTest(sensor=s):
freq = len(msgs) / self.sample_secs
ef = service_list[s].frequency
assert ef*0.85 <= freq <= ef*1.15
def test_logmonottime_timestamp_diff(self):
# ensure diff between the message logMonotime and sample timestamp is small
@ -193,16 +183,14 @@ class TestSensord(unittest.TestCase):
# before the sensor is read
tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
high_delay_diffs = set(filter(lambda d: d >= 15., tdiffs))
# some sensors have a read procedure that will introduce an expected diff on the order of 20ms
high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs))
assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}"
avg_diff = round(sum(tdiffs)/len(tdiffs), 4)
assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
stddev = np.std(tdiffs)
assert stddev < 2, f"Timing diffs have too high stddev: {stddev}"
def test_sensor_values_sanity_check(self):
def test_sensor_values(self):
sensor_values = dict()
for etype in self.events:
for measurement in self.events[etype]:
@ -219,18 +207,13 @@ class TestSensord(unittest.TestCase):
else:
sensor_values[key] = [values]
# Sanity check sensor values and counts
# Sanity check sensor values
for sensor, stype in sensor_values:
for s in ALL_SENSORS[sensor]:
if s.type != stype:
continue
key = (sensor, s.type)
val_cnt = len(sensor_values[key])
min_samples = self.sample_secs * s.expected_freq
err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {min_samples}"
assert min_samples*0.9 < val_cnt < min_samples*1.1, err_msg
mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg

Write Preview
Loading…
Cancel
Save