Sensor events splitup (#25714)

* PoC of reading sensors via interrupts instead of polling

* add Gyro and draft for magn

* add more functionality to gpio.cc

* change LSM gyro to interrupt

* resolve rebase conflict

* update BMX accel interrupt impl

* add interrupt collector thread to fetch in parallel

* change get_event interface to return true on successful read

* update BMX gyro interrupt impl

* update gpio.h/.cc according to comments

* address comments, rename Edgetype enum

* Edgetype to EdgeType

* update sensor interrupt interface

* add error handling, and read fd on trigger

* avoid sending empty messages

* fix build

* use gpiochip

* less diff

* gpiochip on both edges, but skip falling edge if rising edge is detected

* init last_ts with 0

* update sensord testcases

* update sensord testsweet

* test for pipeline

* readd with_process

* add null check

* move tests update to seperate PR

* sensord: improve test coverage (#25683)

* update sensord-interrupt testsweet

* address review comments

* inc stddev threshold

* fix format string

* add version 0 check again

* relax strictness after c3 with bmx tests

* relax strictness after tests

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>

* address PR comments

* fix typo

* remove 4ms limit, and skip first 0.5sec of data

* revert disable_interuppt change to destructor

* fix and remove timing skip

* make gpiochip generic

* sensord port

* change from sensorEvents to separated events

* fix gyro usage

* add splitted sensor tests

* modify debug script sensor_data_to_hist.py

* refactor get_event interface to remove sensorEvent message type

* update locationd to non sensorEvent usage

* tmp commit

* fix replay

* fix accelerometer type

* fix sensor to hist debug script

* update sensord tests to split events

* remove rebase artifacts

* port test_sensord.py

* small clean up

* change cereal to sensorEvents-splitup branch

* upate sensorEvents in regen

* fix route generation for splitted sensor events

* regen cleanUp from sensorEvents change

* .

* remove light and temp from locationd

* add generic init delay per sensor

* .

* update routes

* move bmx gyro/accel to its own channel

* adopt sensor tests to bmx channel

* remove rebase artifacts

* fix sensord test

* handle bmx not present

* add bmx sockets to regen

* .

* .

* code cleanUp

* .

* address PR comments

* address PR comments

* address PR comments

* lsm clean up

* readd sensorEvents

* rever regen.py

* .

* update replay refs

* move channels

* fix artifact

* bump cereal

* update refs

* fix timing issue

Co-authored-by: Bruce Wayne <batman@workstation-eu-intern2.eu.local>
Co-authored-by: gast04 <kurt.nistelberger@gmail.com>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

cereal

@@ -1 +1 @@
-Subproject commit 5aa49864bce38f520705b6ed0b98e7cf9560ed0a
+Subproject commit d4cf8728e2fa2d87d90098efa7ddeaf8f98a03db

selfdrive/car/mock/interface.py

@@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase):
 
     cloudlog.debug("Using Mock Car Interface")
 
-    self.sensor = messaging.sub_sock('sensorEvents')
+    self.gyro = messaging.sub_sock('gyroscope')
     self.gps = messaging.sub_sock('gpsLocationExternal')
 
     self.speed = 0.
@@ -46,11 +46,9 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
     # get basic data from phone and gps since CAN isn't connected
-    sensors = messaging.recv_sock(self.sensor)
-    if sensors is not None:
-      for sensor in sensors.sensorEvents:
-        if sensor.type == 4:  # gyro
-          self.yaw_rate_meas = -sensor.gyro.v[0]
+    gyro_sensor = messaging.recv_sock(self.gyro)
+    if gyro_sensor is not None:
+      self.yaw_rate_meas = -gyro_sensor.gyroscope.gyroUncalibrated.v[0]
 
     gps = messaging.recv_sock(self.gps)
     if gps is not None:

selfdrive/debug/sensor_data_to_hist.py

@@ -7,6 +7,7 @@ get interrupts in a 2kHz rate.
 
 import argparse
 import sys
+import numpy as np
 from collections import defaultdict
 
 from tools.lib.logreader import LogReader
@@ -23,28 +24,22 @@ def parseEvents(log_reader):
   lsm_data = defaultdict(list)
 
   for m in log_reader:
-    # only sensorEvents
-    if m.which() != 'sensorEvents':
+    if m.which() not in ['accelerometer', 'gyroscope']:
       continue
 
-    for se in m.sensorEvents:
-      # convert data to dictionary
-      d = se.to_dict()
+    d = getattr(m, m.which()).to_dict()
 
-      if d["timestamp"] == 0:
-        continue # empty event?
+    if d["source"] == SRC_BMX and "acceleration" in d:
+      bmx_data["accel"].append(d["timestamp"] / 1e9)
 
-      if d["source"] == SRC_BMX and "acceleration" in d:
-        bmx_data["accel"].append(d["timestamp"] / 1e9)
+    if d["source"] == SRC_BMX and "gyroUncalibrated" in d:
+      bmx_data["gyro"].append(d["timestamp"] / 1e9)
 
-      if d["source"] == SRC_BMX and "gyroUncalibrated" in d:
-        bmx_data["gyro"].append(d["timestamp"] / 1e9)
+    if d["source"] == SRC_LSM and "acceleration" in d:
+      lsm_data["accel"].append(d["timestamp"] / 1e9)
 
-      if d["source"] == SRC_LSM and "acceleration" in d:
-        lsm_data["accel"].append(d["timestamp"] / 1e9)
-
-      if d["source"] == SRC_LSM and "gyroUncalibrated" in d:
-        lsm_data["gyro"].append(d["timestamp"] / 1e9)
+    if d["source"] == SRC_LSM and "gyroUncalibrated" in d:
+      lsm_data["gyro"].append(d["timestamp"] / 1e9)
 
   return bmx_data, lsm_data
 
@@ -54,11 +49,7 @@ def cleanData(data):
     return [], []
 
   data.sort()
-  prev = data[0]
-  diffs = []
-  for v in data[1:]:
-    diffs.append(v - prev)
-    prev = v
+  diffs = np.diff(data)
   return data, diffs
 
 
@@ -118,4 +109,3 @@ if __name__ == "__main__":
 
   print("check plot...")
   plt.show()
-

selfdrive/locationd/locationd.cc

@@ -195,51 +195,48 @@ VectorXd Localizer::get_stdev() {
   return this->kf->get_P().diagonal().array().sqrt();
 }
 
-void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
+void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
   // TODO does not yet account for double sensor readings in the log
-  for (int i = 0; i < log.size(); i++) {
-    const cereal::SensorEventData::Reader& sensor_reading = log[i];
 
-    // Ignore empty readings (e.g. in case the magnetometer had no data ready)
-    if (sensor_reading.getTimestamp() == 0) {
-      continue;
-    }
+  // Ignore empty readings (e.g. in case the magnetometer had no data ready)
+  if (log.getTimestamp() == 0) {
+    return;
+  }
 
-    double sensor_time = 1e-9 * sensor_reading.getTimestamp();
+  double sensor_time = 1e-9 * log.getTimestamp();
 
-    // sensor time and log time should be close
-    if (std::abs(current_time - sensor_time) > 0.1) {
-      LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
-      return;
-    }
+  // sensor time and log time should be close
+  if (std::abs(current_time - sensor_time) > 0.1) {
+    LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
+    return;
+  }
 
-      // TODO: handle messages from two IMUs at the same time
-    if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
-      continue;
-    }
+  // TODO: handle messages from two IMUs at the same time
+  if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
+    return;
+  }
 
-    // Gyro Uncalibrated
-    if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
-      auto v = sensor_reading.getGyroUncalibrated().getV();
-      auto meas = Vector3d(-v[2], -v[1], -v[0]);
-      if (meas.norm() < ROTATION_SANITY_CHECK) {
-        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
-      }
+  // Gyro Uncalibrated
+  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+    auto v = log.getGyroUncalibrated().getV();
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+    if (meas.norm() < ROTATION_SANITY_CHECK) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
     }
+  }
 
-    // Accelerometer
-    if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
-      auto v = sensor_reading.getAcceleration().getV();
+  // Accelerometer
+  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
+    auto v = log.getAcceleration().getV();
 
-      // TODO: reduce false positives and re-enable this check
-      // check if device fell, estimate 10 for g
-      // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
-      //this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
+    // TODO: reduce false positives and re-enable this check
+    // check if device fell, estimate 10 for g
+    // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
+    // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
 
-      auto meas = Vector3d(-v[2], -v[1], -v[0]);
-      if (meas.norm() < ACCEL_SANITY_CHECK) {
-        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
-      }
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+    if (meas.norm() < ACCEL_SANITY_CHECK) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
     }
   }
 }
@@ -441,8 +438,10 @@ void Localizer::handle_msg_bytes(const char *data, const size_t size) {
 void Localizer::handle_msg(const cereal::Event::Reader& log) {
   double t = log.getLogMonoTime() * 1e-9;
   this->time_check(t);
-  if (log.isSensorEvents()) {
-    this->handle_sensors(t, log.getSensorEvents());
+  if (log.isAccelerometer()) {
+    this->handle_sensor(t, log.getAccelerometer());
+  } else if (log.isGyroscope()) {
+    this->handle_sensor(t, log.getGyroscope());
   } else if (log.isGpsLocation()) {
     this->handle_gps(t, log.getGpsLocation());
   } else if (log.isGpsLocationExternal()) {
@@ -498,7 +497,9 @@ int Localizer::locationd_thread() {
   } else {
     gps_location_socket = "gpsLocationExternal";
   }
-  const std::initializer_list<const char *> service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
+  const std::initializer_list<const char *> service_list = {gps_location_socket,
+    "cameraOdometry", "liveCalibration", "carState", "carParams",
+    "accelerometer", "gyroscope", "magnetometer"};
   PubMaster pm({"liveLocationKalman"});
 
   // TODO: remove carParams once we're always sending at 100Hz
@@ -521,11 +522,11 @@ int Localizer::locationd_thread() {
     }
 
     // 100Hz publish for notcars, 20Hz for cars
-    const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry";
+    const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry";
     if (sm.updated(trigger_msg)) {
       bool inputsOK = sm.allAliveAndValid();
-      bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
       bool gpsOK = this->isGpsOK();
+      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope", "magnetometer"});
 
       MessageBuilder msg_builder;
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);

selfdrive/locationd/locationd.h

@@ -45,7 +45,7 @@ public:
 
   void handle_msg_bytes(const char *data, const size_t size);
   void handle_msg(const cereal::Event::Reader& log);
-  void handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log);
+  void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
   void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log);
   void handle_car_state(double current_time, const cereal::CarState::Reader& log);
   void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);

selfdrive/locationd/test/_test_locationd_lib.py

@@ -51,23 +51,23 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
 
   @unittest.skip("temporarily disabled due to false positives")
   def test_device_fell(self):
-    msg = messaging.new_message('sensorEvents', 1)
-    msg.sensorEvents[0].sensor = 1
-    msg.sensorEvents[0].timestamp = msg.logMonoTime
-    msg.sensorEvents[0].type = 1
-    msg.sensorEvents[0].init('acceleration')
-    msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
+    msg = messaging.new_message('accelerometer')
+    msg.accelerometer.sensor = 1
+    msg.accelerometer.timestamp = msg.logMonoTime
+    msg.accelerometer.type = 1
+    msg.accelerometer.init('acceleration')
+    msg.accelerometer.acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
     self.localizer_handle_msg(msg)
 
     ret = self.localizer_get_msg()
     self.assertTrue(ret.liveLocationKalman.deviceStable)
 
-    msg = messaging.new_message('sensorEvents', 1)
-    msg.sensorEvents[0].sensor = 1
-    msg.sensorEvents[0].timestamp = msg.logMonoTime
-    msg.sensorEvents[0].type = 1
-    msg.sensorEvents[0].init('acceleration')
-    msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2
+    msg = messaging.new_message('accelerometer')
+    msg.accelerometer.sensor = 1
+    msg.accelerometer.timestamp = msg.logMonoTime
+    msg.accelerometer.type = 1
+    msg.accelerometer.init('acceleration')
+    msg.accelerometer.acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2
     self.localizer_handle_msg(msg)
 
     ret = self.localizer_get_msg()

selfdrive/locationd/test/test_locationd.py

@@ -14,7 +14,8 @@ from selfdrive.manager.process_config import managed_processes
 
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
-  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration']
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
 
   def setUp(self):
     random.seed(123489234)

selfdrive/sensord/sensors/bmx055_accel.cc

@@ -42,6 +42,7 @@ int BMX055_Accel::init() {
   if (ret < 0) {
     goto fail;
   }
+
   ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
   if (ret < 0) {
     goto fail;
@@ -61,7 +62,7 @@ int BMX055_Accel::shutdown()  {
   return ret;
 }
 
-bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) {
+bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[6];
   int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
@@ -73,6 +74,7 @@ bool BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) {
   float y = -read_12_bit(buffer[2], buffer[3]) * scale;
   float z = read_12_bit(buffer[4], buffer[5]) * scale;
 
+  auto event = msg.initEvent().initAccelerometer2();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
   event.setVersion(1);
   event.setSensor(SENSOR_ACCELEROMETER);

selfdrive/sensord/sensors/bmx055_accel.h

@@ -36,6 +36,6 @@ class BMX055_Accel : public I2CSensor {
 public:
   BMX055_Accel(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/bmx055_gyro.cc

@@ -72,7 +72,7 @@ int BMX055_Gyro::shutdown()  {
   return ret;
 }
 
-bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) {
+bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[6];
   int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
@@ -84,6 +84,7 @@ bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) {
   float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
   float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
 
+  auto event = msg.initEvent().initGyroscope2();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
   event.setVersion(1);
   event.setSensor(SENSOR_GYRO_UNCALIBRATED);
@@ -94,5 +95,6 @@ bool BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) {
   auto svec = event.initGyroUncalibrated();
   svec.setV(xyz);
   svec.setStatus(true);
+
   return true;
 }

selfdrive/sensord/sensors/bmx055_gyro.h

@@ -36,6 +36,6 @@ class BMX055_Gyro : public I2CSensor {
 public:
   BMX055_Gyro(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/bmx055_magn.cc

@@ -223,7 +223,7 @@ bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *
 }
 
 
-bool BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) {
+bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[8];
   int16_t _x, _y, x, y, z;
@@ -233,6 +233,8 @@ bool BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) {
 
   bool parsed = parse_xyz(buffer, &_x, &_y, &z);
   if (parsed) {
+
+    auto event = msg.initEvent().initMagnetometer();
     event.setSource(cereal::SensorEventData::SensorSource::BMX055);
     event.setVersion(2);
     event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);

selfdrive/sensord/sensors/bmx055_magn.h

@@ -59,6 +59,6 @@ class BMX055_Magn : public I2CSensor{
 public:
   BMX055_Magn(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/bmx055_temp.cc

@@ -28,7 +28,7 @@ fail:
   return ret;
 }
 
-bool BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) {
+bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[1];
   int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
@@ -36,10 +36,12 @@ bool BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) {
 
   float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
 
+  auto event = msg.initEvent().initTemperatureSensor();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
   event.setVersion(1);
   event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
   event.setTimestamp(start_time);
   event.setTemperature(temp);
+
   return true;
 }

selfdrive/sensord/sensors/bmx055_temp.h

@@ -8,6 +8,6 @@ class BMX055_Temp : public I2CSensor {
 public:
   BMX055_Temp(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown() { return 0; }
 };

selfdrive/sensord/sensors/file_sensor.cc

@@ -2,8 +2,7 @@
 
 #include <string>
 
-FileSensor::FileSensor(std::string filename) : file(filename) {
-}
+FileSensor::FileSensor(std::string filename) : file(filename) {}
 
 int FileSensor::init() {
   return file.is_open() ? 0 : 1;

selfdrive/sensord/sensors/file_sensor.h

@@ -15,5 +15,5 @@ public:
   ~FileSensor();
   int init();
   bool has_interrupt_enabled();
-  virtual bool get_event(cereal::SensorEventData::Builder &event) = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
 };

selfdrive/sensord/sensors/i2c_sensor.cc

@@ -15,7 +15,8 @@ int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
   return int32_t(combined) / (1 << 4);
 }
 
-I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
+I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
 
 I2CSensor::~I2CSensor() {
   if (gpio_fd != -1) {

selfdrive/sensord/sensors/i2c_sensor.h

@@ -31,6 +31,6 @@ public:
   int init_gpio();
   bool has_interrupt_enabled();
   virtual int init() = 0;
-  virtual bool get_event(cereal::SensorEventData::Builder &event) = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
   virtual int shutdown() = 0;
 };

selfdrive/sensord/sensors/light_sensor.cc

@@ -5,7 +5,9 @@
 #include "common/timing.h"
 #include "selfdrive/sensord/sensors/constants.h"
 
-bool LightSensor::get_event(cereal::SensorEventData::Builder &event) {
+LightSensor::LightSensor(std::string filename) : FileSensor(filename) {}
+
+bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   file.clear();
   file.seekg(0);
@@ -13,6 +15,7 @@ bool LightSensor::get_event(cereal::SensorEventData::Builder &event) {
   int value;
   file >> value;
 
+  auto event = msg.initEvent().initLightSensor();
   event.setSource(cereal::SensorEventData::SensorSource::RPR0521);
   event.setVersion(1);
   event.setSensor(SENSOR_LIGHT);

selfdrive/sensord/sensors/light_sensor.h

@@ -3,7 +3,7 @@
 
 class LightSensor : public FileSensor {
 public:
-  LightSensor(std::string filename) : FileSensor(filename){};
-  bool get_event(cereal::SensorEventData::Builder &event);
+  LightSensor(std::string filename);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown() { return 0; }
 };

selfdrive/sensord/sensors/lsm6ds3_accel.cc

@@ -5,7 +5,8 @@
 #include "common/swaglog.h"
 #include "common/timing.h"
 
-LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : I2CSensor(bus, gpio_nr, shared_gpio) {}
+LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
 
 int LSM6DS3_Accel::init() {
   int ret = 0;
@@ -93,15 +94,13 @@ fail:
   return ret;
 }
 
-bool LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) {
+bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
 
-  if (has_interrupt_enabled()) {
-    // INT1 shared with gyro, check STATUS_REG who triggered
-    uint8_t status_reg = 0;
-    read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
-    if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
-      return false;
-    }
+  // INT1 shared with gyro, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
+    return false;
   }
 
   uint8_t buffer[6];
@@ -113,10 +112,12 @@ bool LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) {
   float y = read_16_bit(buffer[2], buffer[3]) * scale;
   float z = read_16_bit(buffer[4], buffer[5]) * scale;
 
+  auto event = msg.initEvent().initAccelerometer();
   event.setSource(source);
   event.setVersion(1);
   event.setSensor(SENSOR_ACCELEROMETER);
   event.setType(SENSOR_TYPE_ACCELEROMETER);
+  event.setTimestamp(ts);
 
   float xyz[] = {y, -x, z};
   auto svec = event.initAcceleration();

selfdrive/sensord/sensors/lsm6ds3_accel.h

@@ -28,6 +28,6 @@ class LSM6DS3_Accel : public I2CSensor {
 public:
   LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@@ -8,7 +8,8 @@
 
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 
-LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : I2CSensor(bus, gpio_nr, shared_gpio) {}
+LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
+  I2CSensor(bus, gpio_nr, shared_gpio) {}
 
 int LSM6DS3_Gyro::init() {
   int ret = 0;
@@ -96,15 +97,13 @@ fail:
   return ret;
 }
 
-bool LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) {
+bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
 
-  if (has_interrupt_enabled()) {
-    // INT1 shared with accel, check STATUS_REG who triggered
-    uint8_t status_reg = 0;
-    read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
-    if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
-      return false;
-    }
+  // INT1 shared with accel, check STATUS_REG who triggered
+  uint8_t status_reg = 0;
+  read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
+  if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
+    return false;
   }
 
   uint8_t buffer[6];
@@ -116,10 +115,12 @@ bool LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) {
   float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
   float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
 
+  auto event = msg.initEvent().initGyroscope();
   event.setSource(source);
   event.setVersion(2);
   event.setSensor(SENSOR_GYRO_UNCALIBRATED);
   event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  event.setTimestamp(ts);
 
   float xyz[] = {y, -x, z};
   auto svec = event.initGyroUncalibrated();

selfdrive/sensord/sensors/lsm6ds3_gyro.h

@@ -28,6 +28,6 @@ class LSM6DS3_Gyro : public I2CSensor {
 public:
   LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/lsm6ds3_temp.cc

@@ -31,8 +31,7 @@ fail:
   return ret;
 }
 
-bool LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) {
-
+bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[2];
   int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
@@ -41,6 +40,7 @@ bool LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) {
   float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
   float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
 
+  auto event = msg.initEvent().initTemperatureSensor();
   event.setSource(source);
   event.setVersion(1);
   event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);

selfdrive/sensord/sensors/lsm6ds3_temp.h

@@ -21,6 +21,6 @@ class LSM6DS3_Temp : public I2CSensor {
 public:
   LSM6DS3_Temp(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown() { return 0; }
 };

selfdrive/sensord/sensors/mmc5603nj_magn.cc

@@ -79,8 +79,7 @@ fail:
   return ret;
 }
 
-bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) {
-
+bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
   uint64_t start_time = nanos_since_boot();
   uint8_t buffer[9];
   int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
@@ -91,6 +90,7 @@ bool MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) {
   float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale;
   float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale;
 
+  auto event = msg.initEvent().initMagnetometer();
   event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
   event.setVersion(1);
   event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);

selfdrive/sensord/sensors/mmc5603nj_magn.h

@@ -25,6 +25,6 @@ class MMC5603NJ_Magn : public I2CSensor {
 public:
   MMC5603NJ_Magn(I2CBus *bus);
   int init();
-  bool get_event(cereal::SensorEventData::Builder &event);
+  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
   int shutdown();
 };

selfdrive/sensord/sensors/sensor.h

@@ -1,13 +1,18 @@
 #pragma once
 
-#include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/messaging/messaging.h"
 
 class Sensor {
 public:
   int gpio_fd = -1;
+  uint64_t init_delay = 500e6; // default dealy 500ms
   virtual ~Sensor() {};
   virtual int init() = 0;
-  virtual bool get_event(cereal::SensorEventData::Builder &event) = 0;
+  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
   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;
+  }
 };

selfdrive/sensord/sensors_qcom2.cc

@@ -3,6 +3,7 @@
 #include <chrono>
 #include <thread>
 #include <vector>
+#include <map>
 #include <poll.h>
 #include <linux/gpio.h>
 
@@ -27,18 +28,18 @@
 
 ExitHandler do_exit;
 std::mutex pm_mutex;
-
-// filter first values (0.5sec) as those may contain inaccuracies
 uint64_t init_ts = 0;
-constexpr uint64_t init_delay = 500*1e6;
 
-void interrupt_loop(int fd, std::vector<Sensor *>& sensors, PubMaster& pm) {
+void interrupt_loop(std::vector<Sensor *>& sensors,
+                    std::map<Sensor*, std::string>& sensor_service)
+{
+  PubMaster pm_int({"gyroscope", "accelerometer"});
+
+  int fd = sensors[0]->gpio_fd;
   struct pollfd fd_list[1] = {0};
   fd_list[0].fd = fd;
   fd_list[0].events = POLLIN | POLLPRI;
 
-  uint64_t offset = nanos_since_epoch() - nanos_since_boot();
-
   while (!do_exit) {
     int err = poll(fd_list, 1, 100);
     if (err == -1) {
@@ -65,39 +66,21 @@ void interrupt_loop(int fd, std::vector<Sensor *>& sensors, PubMaster& pm) {
     }
 
     int num_events = err / sizeof(*evdata);
+    uint64_t offset = nanos_since_epoch() - nanos_since_boot();
     uint64_t ts = evdata[num_events - 1].timestamp - offset;
 
-    MessageBuilder msg;
-    auto orphanage = msg.getOrphanage();
-    std::vector<capnp::Orphan<cereal::SensorEventData>> collected_events;
-    collected_events.reserve(sensors.size());
-
     for (Sensor *sensor : sensors) {
-      auto orphan = orphanage.newOrphan<cereal::SensorEventData>();
-      auto event = orphan.get();
-      if (!sensor->get_event(event)) {
+      MessageBuilder msg;
+      if (!sensor->get_event(msg, ts)) {
         continue;
       }
 
-      event.setTimestamp(ts);
-      collected_events.push_back(kj::mv(orphan));
-    }
-
-    if (collected_events.size() == 0) {
-      continue;
-    }
-
-    auto events = msg.initEvent().initSensorEvents(collected_events.size());
-    for (int i = 0; i < collected_events.size(); ++i) {
-      events.adoptWithCaveats(i, kj::mv(collected_events[i]));
-    }
+      if (!sensor->is_data_valid(init_ts, ts)) {
+        continue;
+      }
 
-    if (ts - init_ts < init_delay) {
-      continue;
+      pm_int.send(sensor_service[sensor].c_str(), msg);
     }
-
-    std::lock_guard<std::mutex> lock(pm_mutex);
-    pm.send("sensorEvents", msg);
   }
 
   // poweroff sensors, disable interrupts
@@ -106,16 +89,7 @@ void interrupt_loop(int fd, std::vector<Sensor *>& sensors, PubMaster& pm) {
   }
 }
 
-int sensor_loop() {
-  I2CBus *i2c_bus_imu;
-
-  try {
-    i2c_bus_imu = new I2CBus(I2C_BUS_IMU);
-  } catch (std::exception &e) {
-    LOGE("I2CBus init failed");
-    return -1;
-  }
-
+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);
@@ -129,6 +103,20 @@ int sensor_loop() {
 
   LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
 
+  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"},
+    {&light, "lightSensor"}
+  };
+
   // Sensor init
   std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
   sensors_init.push_back({&bmx055_accel, false});
@@ -148,29 +136,27 @@ int sensor_loop() {
 
   // Initialize sensors
   std::vector<Sensor *> sensors;
-  for (auto &sensor : sensors_init) {
-    int err = sensor.first->init();
+  for (auto &[sensor, required] : sensors_init) {
+    int err = sensor->init();
     if (err < 0) {
-      // Fail on required sensors
-      if (sensor.second) {
+      if (required) {
         LOGE("Error initializing sensors");
-        delete i2c_bus_imu;
         return -1;
       }
     } else {
-      if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) {
+
+      if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) {
         has_magnetometer = true;
       }
 
-      if (!sensor.first->has_interrupt_enabled()) {
-        sensors.push_back(sensor.first);
+      if (!sensor->has_interrupt_enabled()) {
+        sensors.push_back(sensor);
       }
     }
   }
 
   if (!has_magnetometer) {
     LOGE("No magnetometer present");
-    delete i2c_bus_imu;
     return -1;
   }
 
@@ -179,33 +165,30 @@ int sensor_loop() {
   util::set_core_affinity({1});
   std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'");
 
-  PubMaster pm({"sensorEvents"});
+  PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor",
+                        "lightSensor", "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, lsm6ds3_accel.gpio_fd, std::ref(lsm_interrupt_sensors), std::ref(pm));
+  std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors),
+                                   std::ref(sensor_service));
 
   // polling loop for non interrupt handled sensors
   while (!do_exit) {
     std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
 
-    const int num_events = sensors.size();
-    MessageBuilder msg;
-    auto sensor_events = msg.initEvent().initSensorEvents(num_events);
-
-    for (int i = 0; i < num_events; i++) {
-      auto event = sensor_events[i];
-      sensors[i]->get_event(event);
-    }
+    for (Sensor *sensor : sensors) {
+      MessageBuilder msg;
+      if (!sensor->get_event(msg)) {
+        continue;
+      }
 
-    if (nanos_since_boot() - init_ts < init_delay) {
-      continue;
-    }
+      if (!sensor->is_data_valid(init_ts, nanos_since_boot())) {
+        continue;
+      }
 
-    {
-      std::lock_guard<std::mutex> lock(pm_mutex);
-      pm.send("sensorEvents", msg);
+      pm_non_int.send(sensor_service[sensor].c_str(), msg);
     }
 
     std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
@@ -217,10 +200,15 @@ int sensor_loop() {
   }
 
   lsm_interrupt_thread.join();
-  delete i2c_bus_imu;
   return 0;
 }
 
 int main(int argc, char *argv[]) {
-  return sensor_loop();
+  try {
+    auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
+    return sensor_loop(i2c_bus_imu.get());
+  } catch (std::exception &e) {
+    LOGE("I2CBus init failed");
+    return -1;
+  }
 }

selfdrive/sensord/tests/test_sensord.py

@@ -3,7 +3,7 @@ import os
 import time
 import unittest
 import numpy as np
-from collections import namedtuple
+from collections import namedtuple, defaultdict
 
 import cereal.messaging as messaging
 from cereal import log
@@ -78,21 +78,8 @@ ALL_SENSORS = {
   }
 }
 
-SENSOR_BUS = 1
-I2C_ADDR_LSM = 0x6A
 LSM_INT_GPIO = 84
 
-def read_sensor_events(duration_sec):
-  sensor_events = messaging.sub_sock("sensorEvents", timeout=0.1)
-  start_time_sec = time.monotonic()
-  events = []
-  while time.monotonic() - start_time_sec < duration_sec:
-    events += messaging.drain_sock(sensor_events)
-    time.sleep(0.01)
-
-  assert len(events) != 0, "No sensor events collected"
-  return events
-
 def get_proc_interrupts(int_pin):
   with open("/proc/interrupts") as f:
     lines = f.read().split("\n")
@@ -100,9 +87,27 @@ def get_proc_interrupts(int_pin):
   for line in lines:
     if f" {int_pin} " in line:
       return ''.join(list(filter(lambda e: e != '', line.split(' ')))[1:6])
-
   return ""
 
+def read_sensor_events(duration_sec):
+
+  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
+                  'gyroscope2', 'lightSensor', 'temperatureSensor']
+  esocks = {}
+  events = defaultdict(list)
+  for stype in sensor_types:
+    esocks[stype] = messaging.sub_sock(stype, timeout=0.1)
+
+  start_time_sec = time.monotonic()
+  while time.monotonic() - start_time_sec < duration_sec:
+    for esock in esocks:
+      events[esock] += messaging.drain_sock(esocks[esock])
+    time.sleep(0.1)
+
+  for etype in events:
+    assert len(events[etype]) != 0, f"No {etype} events collected"
+
+  return events
 
 class TestSensord(unittest.TestCase):
   @classmethod
@@ -132,12 +137,10 @@ class TestSensord(unittest.TestCase):
     # verify correct sensors configuration
 
     seen = set()
-    for event in self.events:
-      for measurement in event.sensorEvents:
-        # filter unset events (bmx magn)
-        if measurement.version == 0:
-          continue
-        seen.add((str(measurement.source), measurement.which()))
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        seen.add((str(m.source), m.which()))
 
     self.assertIn(seen, SENSOR_CONFIGURATIONS)
 
@@ -148,10 +151,14 @@ class TestSensord(unittest.TestCase):
       1: [], # accel
       5: [], # gyro
     }
-    for event in self.events:
-      for measurement in event.sensorEvents:
-        if str(measurement.source).startswith("lsm6ds3") and measurement.sensor in sensor_t:
-          sensor_t[measurement.sensor].append(measurement.timestamp)
+
+    for measurement in self.events['accelerometer']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
+
+    for measurement in self.events['gyroscope']:
+      m = getattr(measurement, measurement.which())
+      sensor_t[m.sensor].append(m.timestamp)
 
     for s, vals in sensor_t.items():
       with self.subTest(sensor=s):
@@ -172,17 +179,14 @@ class TestSensord(unittest.TestCase):
     # verify if all sensors produce events
 
     sensor_events = dict()
-    for event in  self.events:
-      for measurement in event.sensorEvents:
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
 
-        # filter unset events (bmx magn)
-        if measurement.version == 0:
-          continue
-
-        if measurement.type in sensor_events:
-          sensor_events[measurement.type] += 1
+        if m.type in sensor_events:
+          sensor_events[m.type] += 1
         else:
-          sensor_events[measurement.type] = 1
+          sensor_events[m.type] = 1
 
     for s in sensor_events:
       err_msg = f"Sensor {s}: 200 < {sensor_events[s]}"
@@ -192,22 +196,19 @@ class TestSensord(unittest.TestCase):
     # ensure diff between the message logMonotime and sample timestamp is small
 
     tdiffs = list()
-    for event in self.events:
-      for measurement in event.sensorEvents:
-
-        # filter unset events (bmx magn)
-        if measurement.version == 0:
-          continue
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
 
         # check if gyro and accel timestamps are before logMonoTime
-        if str(measurement.source).startswith("lsm6ds3"):
-          if measurement.which() != 'temperature':
-            err_msg = f"Timestamp after logMonoTime: {measurement.timestamp} > {event.logMonoTime}"
-            assert measurement.timestamp < event.logMonoTime, err_msg
+        if str(m.source).startswith("lsm6ds3"):
+          if m.which() != 'temperature':
+            err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
+            assert m.timestamp < measurement.logMonoTime, err_msg
 
         # negative values might occur, as non interrupt packages created
         # before the sensor is read
-        tdiffs.append(abs(event.logMonoTime - measurement.timestamp))
+        tdiffs.append(abs(measurement.logMonoTime - m.timestamp))
 
     high_delay_diffs = set(filter(lambda d: d >= 10*10**6, tdiffs))
     assert len(high_delay_diffs) < 15, f"Too many high delay packages: {high_delay_diffs}"
@@ -219,16 +220,14 @@ class TestSensord(unittest.TestCase):
     assert stddev < 2*10**6, f"Timing diffs have to high stddev: {stddev}"
 
   def test_sensor_values_sanity_check(self):
-    sensor_values = dict()
-    for event in self.events:
-      for m in event.sensorEvents:
-
-        # filter unset events (bmx magn)
-        if m.version == 0:
-          continue
 
+    sensor_values = dict()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
         key = (m.source.raw, m.which())
         values = getattr(m, m.which())
+
         if hasattr(values, 'v'):
           values = values.v
         values = np.atleast_1d(values)
@@ -265,7 +264,8 @@ class TestSensord(unittest.TestCase):
     time.sleep(1)
     state_two = get_proc_interrupts(LSM_INT_GPIO)
 
-    assert state_one != state_two, f"no interrupts received after sensord start!\n{state_one} {state_two}"
+    error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}"
+    assert state_one != state_two, error_msg
 
     managed_processes["sensord"].stop()
     time.sleep(1)
@@ -276,6 +276,5 @@ class TestSensord(unittest.TestCase):
     state_two = get_proc_interrupts(LSM_INT_GPIO)
     assert state_one == state_two, "Interrupts received after sensord stop!"
 
-
 if __name__ == "__main__":
   unittest.main()

selfdrive/test/process_replay/process_replay.py

@@ -328,7 +328,8 @@ CONFIGS = [
     proc_name="locationd",
     pub_sub={
       "cameraOdometry": ["liveLocationKalman"],
-      "sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
+      "accelerometer": [], "gyroscope": [], "magnetometer": [],
+      "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
     },
     ignore=["logMonoTime", "valid"],
     init_callback=get_car_params,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-9098c1cf6993598071c3e27448356eef86660d02
+761eada809a0eaa67989e6e435042633f965d1fe

selfdrive/test/process_replay/regen.py

@@ -90,30 +90,10 @@ def replay_device_state(s, msgs):
       rk.keep_time()
 
 
-def replay_sensor_events(s, msgs):
-  pm = messaging.PubMaster([s, ])
-  rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
-  smsgs = [m for m in msgs if m.which() == s]
-  while True:
-    for m in smsgs:
-      new_m = m.as_builder()
-      new_m.logMonoTime = int(sec_since_boot() * 1e9)
-
-      for evt in new_m.sensorEvents:
-        evt.timestamp = new_m.logMonoTime
-
-      pm.send(s, new_m)
-      rk.keep_time()
-
-
 def replay_sensor_event(s, msgs):
-  smsgs = [m for m in msgs if m.which() == s]
-  #if len(smsgs) == 0:
-  #  return
-
   pm = messaging.PubMaster([s, ])
   rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
-
+  smsgs = [m for m in msgs if m.which() == s]
   while True:
     for m in smsgs:
       m = m.as_builder()
@@ -213,12 +193,12 @@ def migrate_carparams(lr):
 def migrate_sensorEvents(lr):
   all_msgs = []
   for msg in lr:
-    if msg.which() != 'sensorEvents':
+    if msg.which() != 'sensorEventsDEPRECATED':
       all_msgs.append(msg)
       continue
 
     # migrate to split sensor events
-    for evt in msg.sensorEvents:
+    for evt in msg.sensorEventsDEPRECATED:
       # build new message for each sensor type
       sensor_service = ''
       if evt.which() == 'acceleration':
@@ -244,9 +224,6 @@ def migrate_sensorEvents(lr):
 
       all_msgs.append(m.as_reader())
 
-    # append also legacy sensorEvents, to have both (remove later)
-    all_msgs.append(msg)
-
   return all_msgs
 
 
@@ -270,7 +247,6 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False):
   vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm)
   fake_daemons = {
     'sensord': [
-      multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)),
       multiprocessing.Process(target=replay_sensor_event, args=('accelerometer', lr)),
       multiprocessing.Process(target=replay_sensor_event, args=('gyroscope', lr)),
       multiprocessing.Process(target=replay_sensor_event, args=('magnetometer', lr)),

tools/sim/bridge.py

@@ -29,7 +29,7 @@ REPEAT_COUNTER = 5
 PRINT_DECIMATION = 100
 STEER_RATIO = 15.
 
-pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"])
+pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"])
 sm = messaging.SubMaster(['carControl', 'controlsState'])
 
 def parse_args(add_args=None):
@@ -123,17 +123,20 @@ class Camerad:
 
 def imu_callback(imu, vehicle_state):
   vehicle_state.bearing_deg = math.degrees(imu.compass)
-  dat = messaging.new_message('sensorEvents', 2)
-  dat.sensorEvents[0].sensor = 4
-  dat.sensorEvents[0].type = 0x10
-  dat.sensorEvents[0].init('acceleration')
-  dat.sensorEvents[0].acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
+  dat = messaging.new_message('accelerometer')
+  dat.accelerometer.sensor = 4
+  dat.accelerometer.type = 0x1
+  dat.accelerometer.init('acceleration')
+  dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
+  pm.send('accelerometer', dat)
+
   # copied these numbers from locationd
-  dat.sensorEvents[1].sensor = 5
-  dat.sensorEvents[1].type = 0x10
-  dat.sensorEvents[1].init('gyroUncalibrated')
-  dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
-  pm.send('sensorEvents', dat)
+  dat = messaging.new_message('gyroscope')
+  dat.gyroscope.sensor = 5
+  dat.gyroscope.type = 0x10
+  dat.gyroscope.init('gyroUncalibrated')
+  dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
+  pm.send('gyroscope', dat)
 
 
 def panda_state_function(vs: VehicleState, exit_event: threading.Event):