Release 260111

system/sensord/sensord.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+import os
+import time
+import ctypes
+import select
+import threading
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.util import sudo_write
+from openpilot.common.realtime import config_realtime_process, Ratekeeper
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.gpio import gpiochip_get_ro_value_fd, gpioevent_data
+from openpilot.system.hardware import HARDWARE
+
+from openpilot.system.sensord.sensors.i2c_sensor import Sensor
+from openpilot.system.sensord.sensors.lsm6ds3_accel import LSM6DS3_Accel
+from openpilot.system.sensord.sensors.lsm6ds3_gyro import LSM6DS3_Gyro
+from openpilot.system.sensord.sensors.lsm6ds3_temp import LSM6DS3_Temp
+from openpilot.system.sensord.sensors.mmc5603nj_magn import MMC5603NJ_Magn
+
+I2C_BUS_IMU = 1
+
+def interrupt_loop(sensors: list[tuple[Sensor, str, bool]], event) -> None:
+  pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt])
+
+  # Requesting both edges as the data ready pulse from the lsm6ds sensor is
+  # very short (75us) and is mostly detected as falling edge instead of rising.
+  # So if it is detected as rising the following falling edge is skipped.
+  fd = gpiochip_get_ro_value_fd("sensord", 0, 84)
+
+  # Configure IRQ affinity
+  irq_path = "/proc/irq/336/smp_affinity_list"
+  if not os.path.exists(irq_path):
+    irq_path = "/proc/irq/335/smp_affinity_list"
+  if os.path.exists(irq_path):
+    sudo_write('1\n', irq_path)
+
+  offset = time.time_ns() - time.monotonic_ns()
+
+  poller = select.poll()
+  poller.register(fd, select.POLLIN | select.POLLPRI)
+  while not event.is_set():
+    events = poller.poll(100)
+    if not events:
+      cloudlog.error("poll timed out")
+      continue
+    if not (events[0][1] & (select.POLLIN | select.POLLPRI)):
+      cloudlog.error("no poll events set")
+      continue
+
+    dat = os.read(fd, ctypes.sizeof(gpioevent_data)*16)
+    evd = gpioevent_data.from_buffer_copy(dat)
+
+    cur_offset = time.time_ns() - time.monotonic_ns()
+    if abs(cur_offset - offset) > 10 * 1e6:  # ms
+      cloudlog.warning(f"time jumped: {cur_offset} {offset}")
+      offset = cur_offset
+      continue
+
+    ts = evd.timestamp - cur_offset
+    for sensor, service, interrupt in sensors:
+      if interrupt:
+        try:
+          evt = sensor.get_event(ts)
+          if not sensor.is_data_valid():
+            continue
+          msg = messaging.new_message(service, valid=True)
+          setattr(msg, service, evt)
+          pm.send(service, msg)
+        except Sensor.DataNotReady:
+          pass
+        except Exception:
+          cloudlog.exception(f"Error processing {service}")
+
+
+def polling_loop(sensor: Sensor, service: str, event: threading.Event) -> None:
+  pm = messaging.PubMaster([service])
+  rk = Ratekeeper(SERVICE_LIST[service].frequency, print_delay_threshold=None)
+  while not event.is_set():
+    try:
+      evt = sensor.get_event()
+      if not sensor.is_data_valid():
+        continue
+      msg = messaging.new_message(service, valid=True)
+      setattr(msg, service, evt)
+      pm.send(service, msg)
+    except Exception:
+      cloudlog.exception(f"Error in {service} polling loop")
+    rk.keep_time()
+
+def main() -> None:
+  config_realtime_process([1, ], 1)
+
+  sensors_cfg = [
+    (LSM6DS3_Accel(I2C_BUS_IMU), "accelerometer", True),
+    (LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True),
+    (LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False),
+  ]
+  if HARDWARE.get_device_type() == "tizi":
+    sensors_cfg.append(
+      (MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False),
+    )
+
+  # Reset sensors
+  for sensor, _, _ in sensors_cfg:
+    try:
+      sensor.reset()
+    except Exception:
+      cloudlog.exception(f"Error initializing {sensor} sensor")
+
+  # Initialize sensors
+  exit_event = threading.Event()
+  threads = [
+    threading.Thread(target=interrupt_loop, args=(sensors_cfg, exit_event), daemon=True)
+  ]
+  for sensor, service, interrupt in sensors_cfg:
+    try:
+      sensor.init()
+      if not interrupt:
+        # Start polling thread for sensors without interrupts
+        threads.append(threading.Thread(
+          target=polling_loop,
+          args=(sensor, service, exit_event),
+          daemon=True
+        ))
+    except Exception:
+      cloudlog.exception(f"Error initializing {service} sensor")
+
+  try:
+    for t in threads:
+      t.start()
+    while any(t.is_alive() for t in threads):
+      time.sleep(1)
+  except KeyboardInterrupt:
+    pass
+  finally:
+    exit_event.set()
+    for t in threads:
+      if t.is_alive():
+        t.join()
+
+    for sensor, _, _ in sensors_cfg:
+      try:
+        sensor.shutdown()
+      except Exception:
+        cloudlog.exception("Error shutting down sensor")
+
+if __name__ == "__main__":
+  main()

system/sensord/sensors/i2c_sensor.py

@@ -0,0 +1,77 @@
+import time
+import smbus2
+import ctypes
+from collections.abc import Iterable
+
+from cereal import log
+
+class Sensor:
+  class SensorException(Exception):
+    pass
+
+  class DataNotReady(SensorException):
+    pass
+
+  def __init__(self, bus: int) -> None:
+    self.bus = smbus2.SMBus(bus)
+    self.source = log.SensorEventData.SensorSource.velodyne  # unknown
+    self.start_ts = 0.
+
+  def __del__(self):
+    self.bus.close()
+
+  def read(self, addr: int, length: int) -> bytes:
+    return bytes(self.bus.read_i2c_block_data(self.device_address, addr, length))
+
+  def write(self, addr: int, data: int) -> None:
+    self.bus.write_byte_data(self.device_address, addr, data)
+
+  def writes(self, writes: Iterable[tuple[int, int]]) -> None:
+    for addr, data in writes:
+      self.write(addr, data)
+
+  def verify_chip_id(self, address: int, expected_ids: list[int]) -> int:
+    chip_id = self.read(address, 1)[0]
+    assert chip_id in expected_ids
+    return chip_id
+
+  # Abstract methods that must be implemented by subclasses
+  @property
+  def device_address(self) -> int:
+    raise NotImplementedError
+
+  def reset(self) -> None:
+    # optional.
+    # not part of init due to shared registers
+    pass
+
+  def init(self) -> None:
+    raise NotImplementedError
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    raise NotImplementedError
+
+  def shutdown(self) -> None:
+    raise NotImplementedError
+
+  def is_data_valid(self) -> bool:
+    if self.start_ts == 0:
+      self.start_ts = time.monotonic()
+
+    # unclear whether we need this...
+    return (time.monotonic() - self.start_ts) > 0.5
+
+  # *** helpers ***
+  @staticmethod
+  def wait():
+    # a standard small sleep
+    time.sleep(0.005)
+
+  @staticmethod
+  def parse_16bit(lsb: int, msb: int) -> int:
+    return ctypes.c_int16((msb << 8) | lsb).value
+
+  @staticmethod
+  def parse_20bit(b2: int, b1: int, b0: int) -> int:
+    combined = ctypes.c_uint32((b0 << 16) | (b1 << 8) | b2).value
+    return ctypes.c_int32(combined).value // (1 << 4)

system/sensord/sensors/lsm6ds3_accel.py

@@ -0,0 +1,161 @@
+import os
+import time
+
+from cereal import log
+from openpilot.system.sensord.sensors.i2c_sensor import Sensor
+
+class LSM6DS3_Accel(Sensor):
+  LSM6DS3_ACCEL_I2C_REG_DRDY_CFG  = 0x0B
+  LSM6DS3_ACCEL_I2C_REG_INT1_CTRL = 0x0D
+  LSM6DS3_ACCEL_I2C_REG_CTRL1_XL  = 0x10
+  LSM6DS3_ACCEL_I2C_REG_CTRL3_C   = 0x12
+  LSM6DS3_ACCEL_I2C_REG_CTRL5_C   = 0x14
+  LSM6DS3_ACCEL_I2C_REG_STAT_REG  = 0x1E
+  LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL = 0x28
+
+  LSM6DS3_ACCEL_ODR_104HZ       = (0b0100 << 4)
+  LSM6DS3_ACCEL_INT1_DRDY_XL    = 0b1
+  LSM6DS3_ACCEL_DRDY_XLDA       = 0b1
+  LSM6DS3_ACCEL_DRDY_PULSE_MODE = (1 << 7)
+  LSM6DS3_ACCEL_IF_INC          = 0b00000100
+
+  LSM6DS3_ACCEL_ODR_52HZ        = (0b0011 << 4)
+  LSM6DS3_ACCEL_FS_4G           = (0b10 << 2)
+  LSM6DS3_ACCEL_IF_INC_BDU      = 0b01000100
+  LSM6DS3_ACCEL_POSITIVE_TEST   = 0b01
+  LSM6DS3_ACCEL_NEGATIVE_TEST   = 0b10
+  LSM6DS3_ACCEL_MIN_ST_LIMIT_mg = 90.0
+  LSM6DS3_ACCEL_MAX_ST_LIMIT_mg = 1700.0
+
+  @property
+  def device_address(self) -> int:
+    return 0x6A
+
+  def reset(self):
+    self.write(0x12, 0x1)
+    time.sleep(0.1)
+
+  def init(self):
+    chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
+    if chip_id == 0x6A:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3trc
+    else:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3
+
+    # self-test
+    if os.getenv("LSM_SELF_TEST") == "1":
+      self.self_test(self.LSM6DS3_ACCEL_POSITIVE_TEST)
+      self.self_test(self.LSM6DS3_ACCEL_NEGATIVE_TEST)
+
+    # actual init
+    int1 = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0]
+    int1 |= self.LSM6DS3_ACCEL_INT1_DRDY_XL
+    self.writes((
+      # Enable continuous update and automatic address increment
+      (self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC),
+      # Set ODR to 104 Hz, FS to ±2g (default)
+      (self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, self.LSM6DS3_ACCEL_ODR_104HZ),
+      # Configure data ready signal to pulse mode
+      (self.LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, self.LSM6DS3_ACCEL_DRDY_PULSE_MODE),
+      # Enable data ready interrupt on INT1 without resetting existing interrupts
+      (self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, int1),
+    ))
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    assert ts is not None  # must come from the IRQ event
+
+    # Check if data is ready since IRQ is shared with gyro
+    status_reg = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0]
+    if (status_reg & self.LSM6DS3_ACCEL_DRDY_XLDA) == 0:
+      raise self.DataNotReady
+
+    scale = 9.81 * 2.0 / (1 << 15)
+    b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6)
+    x = self.parse_16bit(b[0], b[1]) * scale
+    y = self.parse_16bit(b[2], b[3]) * scale
+    z = self.parse_16bit(b[4], b[5]) * scale
+
+    event = log.SensorEventData.new_message()
+    event.timestamp = ts
+    event.version = 1
+    event.sensor = 1  # SENSOR_ACCELEROMETER
+    event.type = 1    # SENSOR_TYPE_ACCELEROMETER
+    event.source = self.source
+    a = event.init('acceleration')
+    a.v = [y, -x, z]
+    a.status = 1
+    return event
+
+  def shutdown(self) -> None:
+    # Disable data ready interrupt on INT1
+    value = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0]
+    value &= ~self.LSM6DS3_ACCEL_INT1_DRDY_XL
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value)
+
+    # Power down by clearing ODR bits
+    value = self.read(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 1)[0]
+    value &= 0x0F
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value)
+
+  # *** self-test stuff ***
+  def _wait_for_data_ready(self):
+    while True:
+      drdy = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0]
+      if drdy & self.LSM6DS3_ACCEL_DRDY_XLDA:
+        break
+
+  def _read_and_avg_data(self, scaling: float) -> list[float]:
+    out_buf = [0.0, 0.0, 0.0]
+    for _ in range(5):
+      self._wait_for_data_ready()
+      b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6)
+      for j in range(3):
+        val = self.parse_16bit(b[j*2], b[j*2+1]) * scaling
+        out_buf[j] += val
+    return [x / 5.0 for x in out_buf]
+
+  def self_test(self, test_type: int) -> None:
+    # Prepare sensor for self-test
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC_BDU)
+
+    # Configure ODR and full scale based on sensor type
+    if self.source == log.SensorEventData.SensorSource.lsm6ds3trc:
+      odr_fs = self.LSM6DS3_ACCEL_FS_4G | self.LSM6DS3_ACCEL_ODR_52HZ
+      scaling = 0.122  # mg/LSB for ±4g
+    else:
+      odr_fs = self.LSM6DS3_ACCEL_ODR_52HZ
+      scaling = 0.061  # mg/LSB for ±2g
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, odr_fs)
+
+    # Wait for stable output
+    time.sleep(0.1)
+    self._wait_for_data_ready()
+    val_st_off = self._read_and_avg_data(scaling)
+
+    # Enable self-test
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type)
+
+    # Wait for stable output
+    time.sleep(0.1)
+    self._wait_for_data_ready()
+    val_st_on = self._read_and_avg_data(scaling)
+
+    # Disable sensor and self-test
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0)
+    self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0)
+
+    # Calculate differences and check limits
+    test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)]
+    for val in test_val:
+      if val < self.LSM6DS3_ACCEL_MIN_ST_LIMIT_mg or val > self.LSM6DS3_ACCEL_MAX_ST_LIMIT_mg:
+        raise self.SensorException(f"Accelerometer self-test failed for test type {test_type}")
+
+if __name__ == "__main__":
+  import numpy as np
+  s = LSM6DS3_Accel(1)
+  s.init()
+  time.sleep(0.2)
+  e = s.get_event(0)
+  print(e)
+  print(np.linalg.norm(e.acceleration.v))
+  s.shutdown()

system/sensord/sensors/lsm6ds3_gyro.py

@@ -0,0 +1,145 @@
+import os
+import math
+import time
+
+from cereal import log
+from openpilot.system.sensord.sensors.i2c_sensor import Sensor
+
+class LSM6DS3_Gyro(Sensor):
+  LSM6DS3_GYRO_I2C_REG_DRDY_CFG  = 0x0B
+  LSM6DS3_GYRO_I2C_REG_INT1_CTRL = 0x0D
+  LSM6DS3_GYRO_I2C_REG_CTRL2_G   = 0x11
+  LSM6DS3_GYRO_I2C_REG_CTRL5_C   = 0x14
+  LSM6DS3_GYRO_I2C_REG_STAT_REG  = 0x1E
+  LSM6DS3_GYRO_I2C_REG_OUTX_L_G  = 0x22
+
+  LSM6DS3_GYRO_ODR_104HZ       = (0b0100 << 4)
+  LSM6DS3_GYRO_INT1_DRDY_G     = 0b10
+  LSM6DS3_GYRO_DRDY_GDA        = 0b10
+  LSM6DS3_GYRO_DRDY_PULSE_MODE = (1 << 7)
+
+  LSM6DS3_GYRO_ODR_208HZ       = (0b0101 << 4)
+  LSM6DS3_GYRO_FS_2000dps      = (0b11 << 2)
+  LSM6DS3_GYRO_POSITIVE_TEST   = (0b01 << 2)
+  LSM6DS3_GYRO_NEGATIVE_TEST   = (0b11 << 2)
+  LSM6DS3_GYRO_MIN_ST_LIMIT_mdps = 150000.0
+  LSM6DS3_GYRO_MAX_ST_LIMIT_mdps = 700000.0
+
+  @property
+  def device_address(self) -> int:
+    return 0x6A
+
+  def reset(self):
+    self.write(0x12, 0x1)
+    time.sleep(0.1)
+
+  def init(self):
+    chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
+    if chip_id == 0x6A:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3trc
+    else:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3
+
+    # self-test
+    if "LSM_SELF_TEST" in os.environ:
+      self.self_test(self.LSM6DS3_GYRO_POSITIVE_TEST)
+      self.self_test(self.LSM6DS3_GYRO_NEGATIVE_TEST)
+
+    # actual init
+    self.writes((
+      # TODO: set scale. Default is +- 250 deg/s
+      (self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_104HZ),
+      # Configure data ready signal to pulse mode
+      (self.LSM6DS3_GYRO_I2C_REG_DRDY_CFG, self.LSM6DS3_GYRO_DRDY_PULSE_MODE),
+    ))
+    value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0]
+    value |= self.LSM6DS3_GYRO_INT1_DRDY_G
+    self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value)
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    assert ts is not None  # must come from the IRQ event
+
+    # Check if gyroscope data is ready, since it's shared with accelerometer
+    status_reg = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0]
+    if not (status_reg & self.LSM6DS3_GYRO_DRDY_GDA):
+      raise self.DataNotReady
+
+    b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6)
+    x = self.parse_16bit(b[0], b[1])
+    y = self.parse_16bit(b[2], b[3])
+    z = self.parse_16bit(b[4], b[5])
+    scale = (8.75 / 1000.0) * (math.pi / 180.0)
+    xyz = [y * scale, -x * scale, z * scale]
+
+    event = log.SensorEventData.new_message()
+    event.timestamp = ts
+    event.version = 2
+    event.sensor = 5  # SENSOR_GYRO_UNCALIBRATED
+    event.type = 16   # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED
+    event.source = self.source
+    g = event.init('gyroUncalibrated')
+    g.v = xyz
+    g.status = 1
+    return event
+
+  def shutdown(self) -> None:
+    # Disable data ready interrupt on INT1
+    value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0]
+    value &= ~self.LSM6DS3_GYRO_INT1_DRDY_G
+    self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value)
+
+    # Power down by clearing ODR bits
+    value = self.read(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 1)[0]
+    value &= 0x0F
+    self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, value)
+
+  # *** self-test stuff ***
+  def _wait_for_data_ready(self):
+    while True:
+      drdy = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0]
+      if drdy & self.LSM6DS3_GYRO_DRDY_GDA:
+        break
+
+  def _read_and_avg_data(self) -> list[float]:
+    out_buf = [0.0, 0.0, 0.0]
+    for _ in range(5):
+      self._wait_for_data_ready()
+      b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6)
+      for j in range(3):
+        val = self.parse_16bit(b[j*2], b[j*2+1]) * 70.0  # mdps/LSB for 2000 dps
+        out_buf[j] += val
+    return [x / 5.0 for x in out_buf]
+
+  def self_test(self, test_type: int):
+    # Set ODR to 208Hz, FS to 2000dps
+    self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_208HZ | self.LSM6DS3_GYRO_FS_2000dps)
+
+    # Wait for stable output
+    time.sleep(0.15)
+    self._wait_for_data_ready()
+    val_st_off = self._read_and_avg_data()
+
+    # Enable self-test
+    self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type)
+
+    # Wait for stable output
+    time.sleep(0.05)
+    self._wait_for_data_ready()
+    val_st_on = self._read_and_avg_data()
+
+    # Disable sensor and self-test
+    self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0)
+    self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0)
+
+    # Calculate differences and check limits
+    test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)]
+    for val in test_val:
+      if val < self.LSM6DS3_GYRO_MIN_ST_LIMIT_mdps or val > self.LSM6DS3_GYRO_MAX_ST_LIMIT_mdps:
+        raise Exception(f"Gyroscope self-test failed for test type {test_type}")
+
+if __name__ == "__main__":
+  s = LSM6DS3_Gyro(1)
+  s.init()
+  time.sleep(0.1)
+  print(s.get_event(0))
+  s.shutdown()

system/sensord/sensors/lsm6ds3_temp.py

@@ -0,0 +1,33 @@
+import time
+
+from cereal import log
+from openpilot.system.sensord.sensors.i2c_sensor import Sensor
+
+# https://content.arduino.cc/assets/st_imu_lsm6ds3_datasheet.pdf
+class LSM6DS3_Temp(Sensor):
+  @property
+  def device_address(self) -> int:
+    return 0x6A
+
+  def _read_temperature(self) -> float:
+    scale = 16.0 if self.source == log.SensorEventData.SensorSource.lsm6ds3 else 256.0
+    data = self.read(0x20, 2)
+    return 25 + (self.parse_16bit(data[0], data[1]) / scale)
+
+  def init(self):
+    chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
+    if chip_id == 0x6A:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3trc
+    else:
+      self.source = log.SensorEventData.SensorSource.lsm6ds3
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    event = log.SensorEventData.new_message()
+    event.version = 1
+    event.timestamp = int(time.monotonic() * 1e9)
+    event.source = self.source
+    event.temperature = self._read_temperature()
+    return event
+
+  def shutdown(self) -> None:
+    pass

system/sensord/sensors/mmc5603nj_magn.py

@@ -0,0 +1,76 @@
+import time
+
+from cereal import log
+from openpilot.system.sensord.sensors.i2c_sensor import Sensor
+
+# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf
+
+# Register addresses
+REG_ODR = 0x1A
+REG_INTERNAL_0 = 0x1B
+REG_INTERNAL_1 = 0x1C
+
+# Control register settings
+CMM_FREQ_EN = (1 << 7)
+AUTO_SR_EN  = (1 << 5)
+SET         = (1 << 3)
+RESET       = (1 << 4)
+
+class MMC5603NJ_Magn(Sensor):
+  @property
+  def device_address(self) -> int:
+    return 0x30
+
+  def init(self):
+    self.verify_chip_id(0x39, [0x10, ])
+    self.writes((
+      (REG_ODR, 0),
+
+      # Set BW to 0b01 for 1-150 Hz operation
+      (REG_INTERNAL_1, 0b01),
+    ))
+
+  def _read_data(self, cycle) -> list[float]:
+    # start measurement
+    self.write(REG_INTERNAL_0, cycle)
+    self.wait()
+
+    # read out XYZ
+    scale = 1.0 / 16384.0
+    b = self.read(0x00, 9)
+    return [
+      (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0,
+      (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0,
+      (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0,
+    ]
+
+  def get_event(self, ts: int | None = None) -> log.SensorEventData:
+    ts = time.monotonic_ns()
+
+    # SET - RESET cycle
+    xyz = self._read_data(SET)
+    reset_xyz = self._read_data(RESET)
+    vals = [*xyz, *reset_xyz]
+
+    event = log.SensorEventData.new_message()
+    event.timestamp = ts
+    event.version = 1
+    event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED
+    event.type = 14  # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED
+    event.source = log.SensorEventData.SensorSource.mmc5603nj
+
+    m = event.init('magneticUncalibrated')
+    m.v = vals
+    m.status = int(all(int(v) != -32 for v in vals))
+
+    return event
+
+  def shutdown(self) -> None:
+    v = self.read(REG_INTERNAL_0, 1)[0]
+    self.writes((
+      # disable auto-reset of measurements
+      (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))),
+
+      # disable continuous mode
+      (REG_ODR, 0),
+    ))

system/sensord/tests/test_sensord.py

@@ -0,0 +1,252 @@
+import os
+import pytest
+import time
+import numpy as np
+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.common.timeout import Timeout
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.manager.process_config import managed_processes
+
+BMX = {
+  ('bmx055', 'acceleration'),
+  ('bmx055', 'gyroUncalibrated'),
+  ('bmx055', 'magneticUncalibrated'),
+  ('bmx055', 'temperature'),
+}
+
+LSM = {
+  ('lsm6ds3', 'acceleration'),
+  ('lsm6ds3', 'gyroUncalibrated'),
+  ('lsm6ds3', 'temperature'),
+}
+LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
+
+MMC = {
+  ('mmc5603nj', 'magneticUncalibrated'),
+}
+
+SENSOR_CONFIGURATIONS: list[set] = [
+  BMX | LSM,
+  MMC | LSM,
+  BMX | LSM_C,
+  MMC| LSM_C,
+]
+if HARDWARE.get_device_type() == "mici":
+  SENSOR_CONFIGURATIONS = [
+    LSM,
+    LSM_C,
+  ]
+
+Sensor = log.SensorEventData.SensorSource
+SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
+ALL_SENSORS = {
+  Sensor.lsm6ds3: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.lsm6ds3trc: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.bmx055: {
+    SensorConfig("acceleration", 5, 15),
+    SensorConfig("gyroUncalibrated", 0, .2),
+    SensorConfig("magneticUncalibrated", 0, 300),
+    SensorConfig("temperature", 0, 60),
+  },
+
+  Sensor.mmc5603nj: {
+    SensorConfig("magneticUncalibrated", 0, 300),
+  }
+}
+
+
+def get_irq_count(irq: int):
+  with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
+    per_cpu = map(int, f.read().split(","))
+    return sum(per_cpu)
+
+def read_sensor_events(duration_sec):
+  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
+                  'gyroscope2', 'temperatureSensor', 'temperatureSensor2']
+  socks = {}
+  poller = messaging.Poller()
+  events = defaultdict(list)
+  for stype in sensor_types:
+    socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100)
+
+  # wait for sensors to come up
+  with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"):
+    while len(poller.poll(250)) == 0:
+      pass
+  time.sleep(1)
+  for s in socks.values():
+    messaging.drain_sock_raw(s)
+
+  st = time.monotonic()
+  while time.monotonic() - st < duration_sec:
+    for s in socks:
+      events[s] += messaging.drain_sock(socks[s])
+    time.sleep(0.1)
+
+  assert sum(map(len, events.values())) != 0, "No sensor events collected!"
+
+  return {k: v for k, v in events.items() if len(v) > 0}
+
+@pytest.mark.tici
+class TestSensord:
+  @classmethod
+  def setup_class(cls):
+    # enable LSM self test
+    os.environ["LSM_SELF_TEST"] = "1"
+
+    # read initial sensor values every test case can use
+    os.system("pkill -f \\\\./sensord")
+    try:
+      managed_processes["sensord"].start()
+      cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10"))
+      cls.events = read_sensor_events(cls.sample_secs)
+
+      # determine sensord's irq
+      cls.sensord_irq = get_irqs_for_action("sensord")[0]
+    finally:
+      # teardown won't run if this doesn't succeed
+      managed_processes["sensord"].stop()
+
+  @classmethod
+  def teardown_class(cls):
+    managed_processes["sensord"].stop()
+
+  def teardown_method(self):
+    managed_processes["sensord"].stop()
+
+  def test_sensors_present(self):
+    # verify correct sensors configuration
+    seen = set()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+        seen.add((str(m.source), m.which()))
+
+    assert seen in SENSOR_CONFIGURATIONS
+
+  def test_lsm6ds3_timing(self, subtests):
+    # verify measurements are sampled and published at 104Hz
+
+    sensor_t = {
+      1: [], # accel
+      5: [], # gyro
+    }
+
+    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 subtests.test(sensor=s):
+        assert len(vals) > 0
+        tdiffs = np.diff(vals) / 1e6 # millis
+
+        high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs))
+        assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}"
+
+        avg_diff = sum(tdiffs)/len(tdiffs)
+        avg_freq = 1. / (avg_diff * 1e-3)
+        assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz"
+
+        stddev = np.std(tdiffs)
+        assert stddev < 2.0, f"Standard-dev to big {stddev}"
+
+  def test_sensor_frequency(self, subtests):
+    for s, msgs in self.events.items():
+      with subtests.test(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
+
+    tdiffs = list()
+    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(m.source).startswith("lsm6ds3") and 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(measurement.logMonoTime - m.timestamp) / 1e6)
+
+    # 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"
+
+  def test_sensor_values(self):
+    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)
+
+        if key in sensor_values:
+          sensor_values[key].append(values)
+        else:
+          sensor_values[key] = [values]
+
+    # 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)
+        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
+
+  def test_sensor_verify_no_interrupts_after_stop(self):
+    managed_processes["sensord"].start()
+    time.sleep(3)
+
+    # read /proc/interrupts to verify interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+
+    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)
+
+    # read /proc/interrupts to verify no more interrupts are received
+    state_one = get_irq_count(self.sensord_irq)
+    time.sleep(1)
+    state_two = get_irq_count(self.sensord_irq)
+    assert state_one == state_two, "Interrupts received after sensord stop!"
+

system/sensord/tests/ttff_test.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import time
+import atexit
+
+from cereal import messaging
+from openpilot.system.manager.process_config import managed_processes
+
+TIMEOUT = 10*60
+
+def kill():
+  for proc in ['ubloxd', 'pigeond']:
+    managed_processes[proc].stop(retry=True, block=True)
+
+if __name__ == "__main__":
+  # start ubloxd
+  managed_processes['ubloxd'].start()
+  atexit.register(kill)
+
+  sm = messaging.SubMaster(['ubloxGnss'])
+
+  times = []
+  for i in range(20):
+    # start pigeond
+    st = time.monotonic()
+    managed_processes['pigeond'].start()
+
+    # wait for a >4 satellite fix
+    while True:
+      sm.update(0)
+      msg = sm['ubloxGnss']
+      if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
+        report = msg.measurementReport
+        if report.numMeas > 4:
+          times.append(time.monotonic() - st)
+          print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
+          break
+
+      if time.monotonic() - st > TIMEOUT:
+        raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
+
+      time.sleep(0.1)
+
+    # stop pigeond
+    managed_processes['pigeond'].stop(retry=True, block=True)
+    time.sleep(20)
+
+  print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")