#!/usr/bin/env python3 import time import unittest import numpy as np from collections import namedtuple from smbus2 import SMBus import cereal.messaging as messaging from cereal import log from system.hardware import TICI from selfdrive.test.helpers import with_processes from selfdrive.manager.process_config import managed_processes SENSOR_CONFIGURATIONS = ( { ('bmx055', 'acceleration'), ('bmx055', 'gyroUncalibrated'), ('bmx055', 'magneticUncalibrated'), ('bmx055', 'temperature'), ('lsm6ds3', 'acceleration'), ('lsm6ds3', 'gyroUncalibrated'), ('lsm6ds3', 'temperature'), ('rpr0521', 'light'), }, { ('lsm6ds3', 'acceleration'), ('lsm6ds3', 'gyroUncalibrated'), ('lsm6ds3', 'temperature'), ('mmc5603nj', 'magneticUncalibrated'), ('rpr0521', 'light'), }, { ('bmx055', 'acceleration'), ('bmx055', 'gyroUncalibrated'), ('bmx055', 'magneticUncalibrated'), ('bmx055', 'temperature'), ('lsm6ds3trc', 'acceleration'), ('lsm6ds3trc', 'gyroUncalibrated'), ('lsm6ds3trc', 'temperature'), ('rpr0521', 'light'), }, { ('lsm6ds3trc', 'acceleration'), ('lsm6ds3trc', 'gyroUncalibrated'), ('lsm6ds3trc', 'temperature'), ('mmc5603nj', 'magneticUncalibrated'), ('rpr0521', 'light'), }, ) Sensor = log.SensorEventData.SensorSource SensorConfig = namedtuple('SensorConfig', ['type', 'min_samples', 'sanity_min', 'sanity_max']) ALL_SENSORS = { Sensor.rpr0521: { SensorConfig("light", 100, 0, 150), }, Sensor.lsm6ds3: { SensorConfig("acceleration", 100, 5, 15), SensorConfig("gyroUncalibrated", 100, 0, .2), SensorConfig("temperature", 100, 0, 60), }, Sensor.lsm6ds3trc: { SensorConfig("acceleration", 100, 5, 15), SensorConfig("gyroUncalibrated", 100, 0, .2), SensorConfig("temperature", 100, 0, 60), }, Sensor.bmx055: { SensorConfig("acceleration", 100, 5, 15), SensorConfig("gyroUncalibrated", 100, 0, .2), SensorConfig("magneticUncalibrated", 100, 0, 300), SensorConfig("temperature", 100, 0, 60), }, Sensor.mmc5603nj: { SensorConfig("magneticUncalibrated", 100, 0, 300), } } 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") for line in lines: if f" {int_pin} " in line: return ''.join(list(filter(lambda e: e != '', line.split(' ')))[1:6]) return "" class TestSensord(unittest.TestCase): @classmethod def setUpClass(cls): if not TICI: raise unittest.SkipTest @with_processes(['sensord']) def test_sensors_present(self): # verify correct sensors configuration events = read_sensor_events(10) seen = set() for event in events: for measurement in event.sensorEvents: # filter unset events (bmx magn) if measurement.version == 0: continue seen.add((str(measurement.source), measurement.which())) self.assertIn(seen, SENSOR_CONFIGURATIONS) @with_processes(['sensord']) def test_lsm6ds3_100Hz(self): # verify measurements are sampled and published at a 100Hz rate events = read_sensor_events(3) # 3sec (about 300 measurements) data_points = set() for event in events: for measurement in event.sensorEvents: # skip lsm6ds3 temperature measurements if measurement.which() == 'temperature': continue if str(measurement.source).startswith("lsm6ds3"): data_points.add(measurement.timestamp) assert len(data_points) != 0, "No lsm6ds3 sensor events" data_list = list(data_points) data_list.sort() tdiffs = np.diff(data_list) high_delay_diffs = set(filter(lambda d: d >= 10.1*10**6, tdiffs)) assert len(high_delay_diffs) < 10, f"Too many high delay packages: {high_delay_diffs}" avg_diff = sum(tdiffs)/len(tdiffs) assert avg_diff > 9.6*10**6, f"avg difference {avg_diff}, below threshold" stddev = np.std(tdiffs) assert stddev < 1.5*10**6, f"Standard-dev to big {stddev}" @with_processes(['sensord']) def test_events_check(self): # verify if all sensors produce events events = read_sensor_events(3) sensor_events = dict() for event in events: for measurement in event.sensorEvents: # filter unset events (bmx magn) if measurement.version == 0: continue if measurement.type in sensor_events: sensor_events[measurement.type] += 1 else: sensor_events[measurement.type] = 1 for s in sensor_events: err_msg = f"Sensor {s}: 200 < {sensor_events[s]}" assert sensor_events[s] > 200, err_msg @with_processes(['sensord']) def test_logmonottime_timestamp_diff(self): # ensure diff between the message logMonotime and sample timestamp is small events = read_sensor_events(3) tdiffs = list() for event in events: for measurement in event.sensorEvents: # filter unset events (bmx magn) if measurement.version == 0: continue # 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 # negative values might occur, as non interrupt packages created # before the sensor is read tdiffs.append(abs(event.logMonoTime - measurement.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}" avg_diff = round(sum(tdiffs)/len(tdiffs), 4) assert avg_diff < 4*10**6, f"Avg packet diff: {avg_diff:.1f}ns" stddev = np.std(tdiffs) assert stddev < 2*10**6, f"Timing diffs have to high stddev: {stddev}" @with_processes(['sensord']) def test_sensor_values_sanity_check(self): events = read_sensor_events(2) sensor_values = dict() for event in events: for m in event.sensorEvents: # filter unset events (bmx magn) if m.version == 0: continue 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 and counts for sensor, stype in sensor_values: for s in ALL_SENSORS[sensor]: if s.type != stype: continue key = (sensor, s.type) val_cnt = len(sensor_values[key]) err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {s.min_samples}" assert val_cnt > s.min_samples, err_msg 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(1) # check if the interrupts are enableds with SMBus(SENSOR_BUS, force=True) as bus: int1_ctrl_reg = bus.read_byte_data(I2C_ADDR_LSM, 0x0D) assert int1_ctrl_reg == 3, "Interrupts not enabled!" # read /proc/interrupts to verify interrupts are received state_one = get_proc_interrupts(LSM_INT_GPIO) time.sleep(1) state_two = get_proc_interrupts(LSM_INT_GPIO) assert state_one != state_two, "no Interrupts received after sensord start!" managed_processes["sensord"].stop() # check if the interrupts got disabled with SMBus(SENSOR_BUS, force=True) as bus: int1_ctrl_reg = bus.read_byte_data(I2C_ADDR_LSM, 0x0D) assert int1_ctrl_reg == 0, "Interrupts not disabled!" # read /proc/interrupts to verify no more interrupts are received state_one = get_proc_interrupts(LSM_INT_GPIO) time.sleep(1) state_two = get_proc_interrupts(LSM_INT_GPIO) assert state_one == state_two, "Interrupts received after sensord stop!" if __name__ == "__main__": unittest.main()