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

old-commit-hash: 42769345d505cc30978aaf9ed735cb8d644b13aa

system/sensord/sensors_qcom2.cc

@@ -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;

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