From f8372ccc4df0b5b0e49572f8817ce7d5b14c6db9 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 22 Apr 2026 12:53:07 -0700 Subject: [PATCH] sensord: remove mmc5603nj support (#37881) * sensord: remove mmc5603nj support * lil more * lil more --- cereal/services.py | 2 - system/sensord/sensord.py | 6 - system/sensord/sensors/mmc5603nj_magn.py | 76 ------------ system/sensord/tests/test_sensord.py | 140 ++++++++--------------- 4 files changed, 48 insertions(+), 176 deletions(-) delete mode 100644 system/sensord/sensors/mmc5603nj_magn.py diff --git a/cereal/services.py b/cereal/services.py index c8525c0dd..c43d3cb9b 100755 --- a/cereal/services.py +++ b/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), diff --git a/system/sensord/sensord.py b/system/sensord/sensord.py index 62908c6f1..d17d29695 100755 --- a/system/sensord/sensord.py +++ b/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: diff --git a/system/sensord/sensors/mmc5603nj_magn.py b/system/sensord/sensors/mmc5603nj_magn.py deleted file mode 100644 index 255e99eb3..000000000 --- a/system/sensord/sensors/mmc5603nj_magn.py +++ /dev/null @@ -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), - )) diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py index 9a66678ec..c82e59560 100644 --- a/system/sensord/tests/test_sensord.py +++ b/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()