sensord: setup per-sensor threads (#29558)

* sensord: setup per-sensor threads

* revert cereal

* more cleanup

* bump cereal

* update test

---------

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 6a66b7555679c5cd3511e03ba6e1cb528706f84e

system/sensord/sensors/lsm6ds3_accel.cc

@@ -121,7 +121,7 @@ int LSM6DS3_Accel::init() {
   uint8_t value = 0;
   bool do_self_test = false;
 
-  const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
   if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
     do_self_test = true;
   }

system/sensord/sensors/sensor.h

@@ -5,6 +5,7 @@
 class Sensor {
 public:
   int gpio_fd = -1;
+  uint64_t start_ts = 0;
   uint64_t init_delay = 500e6; // default dealy 500ms
   virtual ~Sensor() {}
   virtual int init() = 0;
@@ -12,7 +13,10 @@ public:
   virtual bool has_interrupt_enabled() = 0;
   virtual int shutdown() = 0;
 
-  virtual bool is_data_valid(uint64_t st, uint64_t ct) {
-    return (ct - st) > init_delay;
+  virtual bool is_data_valid(uint64_t current_ts) {
+    if (start_ts == 0) {
+      start_ts = current_ts;
+    }
+    return (current_ts - start_ts) > init_delay;
   }
 };

system/sensord/sensors_qcom2.cc

@@ -22,19 +22,22 @@
 #include "system/sensord/sensors/lsm6ds3_gyro.h"
 #include "system/sensord/sensors/lsm6ds3_temp.h"
 #include "system/sensord/sensors/mmc5603nj_magn.h"
-#include "system/sensord/sensors/sensor.h"
 
 #define I2C_BUS_IMU 1
 
 ExitHandler do_exit;
-uint64_t init_ts = 0;
 
-void interrupt_loop(std::vector<Sensor *>& sensors,
-                    std::map<Sensor*, std::string>& sensor_service)
-{
-  PubMaster pm_int({"gyroscope", "accelerometer"});
+void interrupt_loop(std::vector<std::tuple<Sensor *, std::string, bool, int>> sensors) {
+  PubMaster pm({"gyroscope", "accelerometer"});
+
+  int fd = -1;
+  for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
+    if (sensor->has_interrupt_enabled()) {
+      fd = sensor->gpio_fd;
+      break;
+    }
+  }
 
-  int fd = sensors[0]->gpio_fd;
   struct pollfd fd_list[1] = {0};
   fd_list[0].fd = fd;
   fd_list[0].events = POLLIN | POLLPRI;
@@ -68,90 +71,76 @@ void interrupt_loop(std::vector<Sensor *>& sensors,
     uint64_t offset = nanos_since_epoch() - nanos_since_boot();
     uint64_t ts = evdata[num_events - 1].timestamp - offset;
 
-    for (Sensor *sensor : sensors) {
+    for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
+      if (!sensor->has_interrupt_enabled()) {
+        continue;
+      }
+
       MessageBuilder msg;
       if (!sensor->get_event(msg, ts)) {
         continue;
       }
 
-      if (!sensor->is_data_valid(init_ts, ts)) {
+      if (!sensor->is_data_valid(ts)) {
         continue;
       }
 
-      pm_int.send(sensor_service[sensor].c_str(), msg);
+      pm.send(msg_name.c_str(), msg);
     }
   }
 
   // poweroff sensors, disable interrupts
-  for (Sensor *sensor : sensors) {
-    sensor->shutdown();
+  for (auto &[sensor, msg_name, required, polling_freq] : sensors) {
+    if (sensor->has_interrupt_enabled()) {
+      sensor->shutdown();
+    }
   }
 }
 
+void polling_loop(Sensor *sensor, std::string msg_name, int frequency) {
+  PubMaster pm({msg_name.c_str()});
+  RateKeeper rk(msg_name, frequency);
+  while (!do_exit) {
+    MessageBuilder msg;
+    if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
+      pm.send(msg_name.c_str(), msg);
+    }
+    rk.keepTime();
+  }
+
+  sensor->shutdown();
+}
+
 int sensor_loop(I2CBus *i2c_bus_imu) {
-  BMX055_Accel bmx055_accel(i2c_bus_imu);
-  BMX055_Gyro bmx055_gyro(i2c_bus_imu);
-  BMX055_Magn bmx055_magn(i2c_bus_imu);
-  BMX055_Temp bmx055_temp(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},
 
-  LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT);
-  LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel
-  LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu);
+    {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},
 
-  MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu);
-
-  std::map<Sensor*, std::string> sensor_service = {
-    {&bmx055_accel, "accelerometer2"},
-    {&bmx055_gyro, "gyroscope2"},
-    {&bmx055_magn, "magnetometer"},
-    {&bmx055_temp, "temperatureSensor"},
-
-    {&lsm6ds3_accel, "accelerometer"},
-    {&lsm6ds3_gyro, "gyroscope"},
-    {&lsm6ds3_temp, "temperatureSensor"},
-
-    {&mmc5603nj_magn, "magnetometer"},
+    {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer", false, 100},
   };
 
-  // Sensor init
-  std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
-  sensors_init.push_back({&bmx055_accel, false});
-  sensors_init.push_back({&bmx055_gyro, false});
-  sensors_init.push_back({&bmx055_magn, false});
-  sensors_init.push_back({&bmx055_temp, false});
-
-  sensors_init.push_back({&lsm6ds3_accel, true});
-  sensors_init.push_back({&lsm6ds3_gyro, true});
-  sensors_init.push_back({&lsm6ds3_temp, true});
-
-  sensors_init.push_back({&mmc5603nj_magn, false});
-
-  bool has_magnetometer = false;
-
   // Initialize sensors
-  std::vector<Sensor *> sensors;
-  for (auto &[sensor, required] : sensors_init) {
+  std::vector<std::thread> threads;
+  for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) {
     int err = sensor->init();
     if (err < 0) {
       if (required) {
         LOGE("Error initializing sensors");
         return -1;
       }
-    } else {
-
-      if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) {
-        has_magnetometer = true;
-      }
-
-      if (!sensor->has_interrupt_enabled()) {
-        sensors.push_back(sensor);
-      }
+      continue;
     }
-  }
 
-  if (!has_magnetometer) {
-    LOGE("No magnetometer present");
-    return -1;
+    if (!sensor->has_interrupt_enabled()) {
+      threads.emplace_back(polling_loop, sensor, msg_name, polling_freq);
+    }
   }
 
   // increase interrupt quality by pinning interrupt and process to core 1
@@ -159,39 +148,17 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
   util::set_core_affinity({1});
   std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'");
 
-  PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor", "magnetometer"});
-  init_ts = nanos_since_boot();
-
   // thread for reading events via interrupts
-  std::vector<Sensor *> lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro};
-  std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors),
-                                   std::ref(sensor_service));
+  threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
 
-  RateKeeper rk("sensord", 100);
-
-  // polling loop for non interrupt handled sensors
-  while (!do_exit) {
-    for (Sensor *sensor : sensors) {
-      MessageBuilder msg;
-      if (!sensor->get_event(msg)) {
-        continue;
-      }
-
-      if (!sensor->is_data_valid(init_ts, nanos_since_boot())) {
-        continue;
-      }
-
-      pm_non_int.send(sensor_service[sensor].c_str(), msg);
-    }
-
-    rk.keepTime();
+  // wait for all threads to finish
+  for (auto &t : threads) {
+    t.join();
   }
 
-  for (Sensor *sensor : sensors) {
-    sensor->shutdown();
+  for (auto &[sensor, msg_name, required, polling_freq] : sensors_init) {
+    delete sensor;
   }
-
-  lsm_interrupt_thread.join();
   return 0;
 }
 

system/sensord/tests/test_sensord.py

@@ -71,7 +71,7 @@ def get_irq_count(irq: int):
 
 def read_sensor_events(duration_sec):
   sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
-                  'gyroscope2', 'temperatureSensor']
+                  'gyroscope2', 'temperatureSensor', 'temperatureSensor2']
   esocks = {}
   events = defaultdict(list)
   for stype in sensor_types: