diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc index 313dc50095..7a7e5d1e80 100644 --- a/system/sensord/sensors_qcom2.cc +++ b/system/sensord/sensors_qcom2.cc @@ -7,6 +7,7 @@ #include #include +#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> sensors) { +void interrupt_loop(std::vector> 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> 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> 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> 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> 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 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; diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py index 815dd8b444..e3cd77a1a3 100755 --- a/system/sensord/tests/test_sensord.py +++ b/system/sensord/tests/test_sensord.py @@ -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)) - assert len(high_delay_diffs) < 20, f"Too many measurements published : {high_delay_diffs}" + # 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