Release 260111

2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
--- a/system/sensord/sensors/init.py
+++ b/system/sensord/sensors/init.py
--- a/system/sensord/sensors/i2c_sensor.py
+++ b/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)
--- a/system/sensord/sensors/lsm6ds3_accel.py
+++ b/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()
--- a/system/sensord/sensors/lsm6ds3_gyro.py
+++ b/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()
--- a/system/sensord/sensors/lsm6ds3_temp.py
+++ b/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
--- a/system/sensord/sensors/mmc5603nj_magn.py
+++ b/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),
+    ))