sensord: move to system/ (#27531)

* sensord: move to system/ * add gitignore to releaes files old-commit-hash: 6f40f0d4427f4238c3939a64d1d6baa7d77f1ecc
2026-09-30 19:33:45 +08:00 · 2023-03-08 10:56:54 -08:00
parent a9b38c148d
commit 47d30174d3
50 changed files with 51 additions and 51 deletions
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+import time
+import unittest
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+from common.gpio import gpio_read
+from selfdrive.test.helpers import with_processes
+from selfdrive.manager.process_config import managed_processes
+from system.hardware import TICI
+from system.hardware.tici.pins import GPIO
+
+
+# TODO: test TTFF when we have good A-GNSS
+class TestPigeond(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    if not TICI:
+      raise unittest.SkipTest
+
+  def tearDown(self):
+    managed_processes['pigeond'].stop()
+
+  @with_processes(['pigeond'])
+  def test_frequency(self):
+    sm = messaging.SubMaster(['ubloxRaw'])
+
+    # setup time
+    time.sleep(2)
+    sm.update()
+
+    for _ in range(int(10 * service_list['ubloxRaw'].frequency)):
+      sm.update()
+      assert sm.all_checks()
+
+  def test_startup_time(self):
+    for _ in range(5):
+      sm = messaging.SubMaster(['ubloxRaw'])
+      managed_processes['pigeond'].start()
+
+      start_time = time.monotonic()
+      for __ in range(10):
+        sm.update(1 * 1000)
+        if sm.updated['ubloxRaw']:
+          break
+      assert sm.rcv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time"
+
+      et = time.monotonic() - start_time
+      assert et < 5, f"pigeond took {et:.1f}s to start"
+      managed_processes['pigeond'].stop()
+
+  def test_turns_off_ublox(self):
+    for s in (0.1, 0.5, 1, 5):
+      managed_processes['pigeond'].start()
+      time.sleep(s)
+      managed_processes['pigeond'].stop()
+
+      assert gpio_read(GPIO.UBLOX_RST_N) == 0
+      assert gpio_read(GPIO.UBLOX_PWR_EN) == 0
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,271 @@
+#!/usr/bin/env python3
+import os
+import time
+import unittest
+import numpy as np
+from collections import namedtuple, defaultdict
+
+import cereal.messaging as messaging
+from cereal import log
+from system.hardware import TICI, HARDWARE
+from selfdrive.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'),
+}
+
+RPR = {
+  ('rpr0521', 'light'),
+}
+
+SENSOR_CONFIGURATIONS = (
+  (BMX | LSM | RPR),
+  (MMC | LSM | RPR),
+  (BMX | LSM_C | RPR),
+  (MMC| LSM_C | RPR),
+)
+
+Sensor = log.SensorEventData.SensorSource
+SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
+ALL_SENSORS = {
+  Sensor.rpr0521: {
+    SensorConfig("light", 0, 1023),
+  },
+
+  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),
+  }
+}
+
+LSM_IRQ = 336
+
+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', 'lightSensor', 'temperatureSensor']
+  esocks = {}
+  events = defaultdict(list)
+  for stype in sensor_types:
+    esocks[stype] = messaging.sub_sock(stype, timeout=0.1)
+
+  start_time_sec = time.monotonic()
+  while time.monotonic() - start_time_sec < duration_sec:
+    for esock in esocks:
+      events[esock] += messaging.drain_sock(esocks[esock])
+    time.sleep(0.1)
+
+  assert sum(map(len, events.values())) != 0, "No sensor events collected!"
+
+  return events
+
+class TestSensord(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    if not TICI:
+      raise unittest.SkipTest
+
+    # make sure gpiochip0 is readable
+    HARDWARE.initialize_hardware()
+
+    # 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()
+      time.sleep(3)
+      cls.sample_secs = 10
+      cls.events = read_sensor_events(cls.sample_secs)
+    finally:
+      # teardown won't run if this doesn't succeed
+      managed_processes["sensord"].stop()
+
+  @classmethod
+  def tearDownClass(cls):
+    managed_processes["sensord"].stop()
+    if "LSM_SELF_TEST" in os.environ:
+      del os.environ['LSM_SELF_TEST']
+
+  def tearDown(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()))
+
+    self.assertIn(seen, SENSOR_CONFIGURATIONS)
+
+  def test_lsm6ds3_timing(self):
+    # 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 self.subTest(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_events_check(self):
+    # verify if all sensors produce events
+
+    sensor_events = dict()
+    for etype in self.events:
+      for measurement in self.events[etype]:
+        m = getattr(measurement, measurement.which())
+
+        if m.type in sensor_events:
+          sensor_events[m.type] += 1
+        else:
+          sensor_events[m.type] = 1
+
+    for s in sensor_events:
+      err_msg = f"Sensor {s}: 200 < {sensor_events[s]}"
+      assert sensor_events[s] > 200, err_msg
+
+  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)
+
+    high_delay_diffs = set(filter(lambda d: d >= 15., 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"
+
+    stddev = np.std(tdiffs)
+    assert stddev < 2, f"Timing diffs have too high stddev: {stddev}"
+
+  def test_sensor_values_sanity_check(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 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])
+        min_samples = self.sample_secs * 100  # Hz
+        err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {min_samples}"
+        assert min_samples*0.9 < val_cnt < min_samples*1.1, 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(3)
+
+    # read /proc/interrupts to verify interrupts are received
+    state_one = get_irq_count(LSM_IRQ)
+    time.sleep(1)
+    state_two = get_irq_count(LSM_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(LSM_IRQ)
+    time.sleep(1)
+    state_two = get_irq_count(LSM_IRQ)
+    assert state_one == state_two, "Interrupts received after sensord stop!"
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import time
+import atexit
+
+from cereal import messaging
+from selfdrive.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")