sensord: remove mmc5603nj support (#37881)

* sensord: remove mmc5603nj support

* lil more

* lil more

cereal/services.py

@@ -24,8 +24,6 @@ _services: dict[str, tuple] = {
   # note: the "EncodeIdx" packets will still be in the log
   "gyroscope": (True, 104., 104),
   "accelerometer": (True, 104., 104),
-  "magnetometer": (True, 25.),
-  "lightSensor": (True, 100., 100),
   "temperatureSensor": (True, 2., 200),
   "gpsNMEA": (True, 9.),
   "deviceState": (True, 2., 1),

system/sensord/sensord.py

@@ -11,13 +11,11 @@ from openpilot.common.utils 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
 
@@ -97,10 +95,6 @@ def main() -> None:
     (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:

system/sensord/sensors/mmc5603nj_magn.py

@@ -1,76 +0,0 @@
-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

@@ -5,44 +5,19 @@ 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
 
-LSM = {
-  ('lsm6ds3', 'acceleration'),
-  ('lsm6ds3', 'gyroUncalibrated'),
-  ('lsm6ds3', 'temperature'),
-}
-LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
+SensorConfig = namedtuple('SensorConfig', ['service', 'measurement', 'sanity_min', 'sanity_max', 'std_max'])
 
-MMC = {
-  ('mmc5603nj', 'magneticUncalibrated'),
-}
-
-SENSOR_CONFIGURATIONS: list[set] = {
-  "mici": [LSM, LSM_C],
-  "tizi": [MMC | LSM, MMC | LSM_C],
-  "tici": [LSM, LSM_C, MMC | LSM, MMC | LSM_C],
-}.get(HARDWARE.get_device_type(), [])
-
-Sensor = log.SensorEventData.SensorSource
-SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max', 'std_max'])
-ALL_SENSORS = {
-  Sensor.lsm6ds3trc: {
-    SensorConfig("acceleration", 5, 15, 5),
-    SensorConfig("gyroUncalibrated", 0, .15, 0.5),
-    SensorConfig("temperature", 10, 40, 0.5),  # set for max range of our office
-  },
-
-  Sensor.mmc5603nj: {
-    SensorConfig("magneticUncalibrated", 0, 300, 10),
-  }
-}
-ALL_SENSORS[Sensor.lsm6ds3] = ALL_SENSORS[Sensor.lsm6ds3trc]
+SENSOR_CONFIGS = (
+  SensorConfig("accelerometer", "acceleration", 5, 15, 5),
+  SensorConfig("gyroscope", "gyroUncalibrated", 0, .15, 0.5),
+  SensorConfig("temperatureSensor", "temperature", 10, 40, 0.5),  # set for max range of our office
+)
+SENSOR_CONFIGS_BY_MEASUREMENT = {config.measurement: config for config in SENSOR_CONFIGS}
 
 def get_irq_count(irq: int):
   with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
@@ -50,12 +25,11 @@ def get_irq_count(irq: int):
     return sum(per_cpu)
 
 def read_sensor_events(duration_sec):
-  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'temperatureSensor',]
   socks = {}
   poller = messaging.Poller()
   events = defaultdict(list)
-  for stype in sensor_types:
-    socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100)
+  for config in SENSOR_CONFIGS:
+    socks[config.service] = messaging.sub_sock(config.service, poller=poller, timeout=100)
 
   # wait for sensors to come up
   with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"):
@@ -70,11 +44,15 @@ def read_sensor_events(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}
 
+def iter_measurements(events):
+  for msgs in events.values():
+    for measurement in msgs:
+      yield measurement, getattr(measurement, measurement.which())
+
 @pytest.mark.tici
 class TestSensord:
   @classmethod
@@ -102,15 +80,9 @@ class TestSensord:
   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_all_sensors_present(self):
+    missing = [config.service for config in SENSOR_CONFIGS if config.service not in self.events]
+    assert len(missing) == 0, f"missing sensors: {missing}"
 
   def test_lsm6ds3_timing(self, subtests):
     # verify measurements are sampled and published at 104Hz
@@ -120,13 +92,10 @@ class TestSensord:
       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 service in ('accelerometer', 'gyroscope'):
+      for measurement in self.events.get(service, []):
+        m = getattr(measurement, measurement.which())
+        sensor_t[m.sensor].append(m.timestamp)
 
     for s, vals in sensor_t.items():
       with subtests.test(sensor=s):
@@ -153,19 +122,16 @@ class TestSensord:
   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())
+    tdiffs = []
+    for measurement, m in iter_measurements(self.events):
+      # 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
 
-        # 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)
+      # 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))
@@ -175,39 +141,29 @@ class TestSensord:
     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())
+    sensor_values = defaultdict(list)
+    for _, m in iter_measurements(self.events):
+      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]
+      if hasattr(values, 'v'):
+        values = values.v
+      sensor_values[key].append(np.atleast_1d(values))
 
     # Sanity check sensor values
-    for sensor, stype in sensor_values:
-      for s in ALL_SENSORS[sensor]:
-        if s.type != stype:
-          continue
+    for (sensor, stype), values in sensor_values.items():
+      config = SENSOR_CONFIGS_BY_MEASUREMENT[stype]
 
-        key = (sensor, s.type)
-        if s.type == 'temperature':
-          measurement_stat = np.mean(sensor_values[key])
-        else:
-          measurement_stat = np.mean(np.linalg.norm(sensor_values[key], axis=1))
-        err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {measurement_stat} is not between {s.sanity_min} and {s.sanity_max}"
-        assert s.sanity_min <= measurement_stat <= s.sanity_max, err_msg
+      if config.measurement == 'temperature':
+        measurement_stat = np.mean(values)
+      else:
+        measurement_stat = np.mean(np.linalg.norm(values, axis=1))
+      err_msg = f"Sensor '{sensor} {config.measurement}' failed sanity checks {measurement_stat} is not between {config.sanity_min} and {config.sanity_max}"
+      assert config.sanity_min <= measurement_stat <= config.sanity_max, err_msg
 
-        std_dev = np.std(sensor_values[key], axis=0)
-        err_msg = f"Sensor '{sensor} {s.type}' failed std dev test {std_dev} is not under {s.std_max}"
-        assert np.all(std_dev <= s.std_max), err_msg
+      std_dev = np.std(values, axis=0)
+      err_msg = f"Sensor '{sensor} {config.measurement}' failed std dev test {std_dev} is not under {config.std_max}"
+      assert np.all(std_dev <= config.std_max), err_msg
 
   def test_sensor_verify_no_interrupts_after_stop(self):
     managed_processes["sensord"].start()