Add openpilot tests

selfdrive/controls/tests/test_alerts.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+import copy
+import json
+import os
+import unittest
+import random
+from PIL import Image, ImageDraw, ImageFont
+
+from cereal import log, car
+from cereal.messaging import SubMaster
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
+
+AlertSize = log.ControlsState.AlertSize
+
+OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")
+
+# TODO: add callback alerts
+ALERTS = []
+for event_types in EVENTS.values():
+  for alert in event_types.values():
+    ALERTS.append(alert)
+
+
+class TestAlerts(unittest.TestCase):
+
+  @classmethod
+  def setUpClass(cls):
+    with open(OFFROAD_ALERTS_PATH) as f:
+      cls.offroad_alerts = json.loads(f.read())
+
+      # Create fake objects for callback
+      cls.CS = car.CarState.new_message()
+      cls.CP = car.CarParams.new_message()
+      cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0]
+      cls.sm = SubMaster(cfg.pubs)
+
+  def test_events_defined(self):
+    # Ensure all events in capnp schema are defined in events.py
+    events = car.CarEvent.EventName.schema.enumerants
+
+    for name, e in events.items():
+      if not name.endswith("DEPRECATED"):
+        fail_msg = "%s @%d not in EVENTS" % (name, e)
+        self.assertTrue(e in EVENTS.keys(), msg=fail_msg)
+
+  # ensure alert text doesn't exceed allowed width
+  def test_alert_text_length(self):
+    font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts")
+    regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
+    bold_font_path = os.path.join(font_path, "Inter-Bold.ttf")
+    semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
+
+    max_text_width = 2160 - 300  # full screen width is usable, minus sidebar
+    draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
+
+    fonts = {
+      AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)],
+      AlertSize.mid: [ImageFont.truetype(bold_font_path, 88),
+                      ImageFont.truetype(regular_font_path, 66)],
+    }
+
+    for alert in ALERTS:
+      if not isinstance(alert, Alert):
+        alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100)
+
+      # for full size alerts, both text fields wrap the text,
+      # so it's unlikely that they  would go past the max width
+      if alert.alert_size in (AlertSize.none, AlertSize.full):
+        continue
+
+      for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]):
+        if i >= len(fonts[alert.alert_size]):
+          break
+
+        font = fonts[alert.alert_size][i]
+        left, _, right, _ = draw.textbbox((0, 0), txt, font)
+        width = right - left
+        msg = f"type: {alert.alert_type} msg: {txt}"
+        self.assertLessEqual(width, max_text_width, msg=msg)
+
+  def test_alert_sanity_check(self):
+    for event_types in EVENTS.values():
+      for event_type, a in event_types.items():
+        # TODO: add callback alerts
+        if not isinstance(a, Alert):
+          continue
+
+        if a.alert_size == AlertSize.none:
+          self.assertEqual(len(a.alert_text_1), 0)
+          self.assertEqual(len(a.alert_text_2), 0)
+        elif a.alert_size == AlertSize.small:
+          self.assertGreater(len(a.alert_text_1), 0)
+          self.assertEqual(len(a.alert_text_2), 0)
+        elif a.alert_size == AlertSize.mid:
+          self.assertGreater(len(a.alert_text_1), 0)
+          self.assertGreater(len(a.alert_text_2), 0)
+        else:
+          self.assertGreater(len(a.alert_text_1), 0)
+
+        self.assertGreaterEqual(a.duration, 0.)
+
+        if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE):
+          self.assertEqual(a.creation_delay, 0.)
+
+  def test_offroad_alerts(self):
+    params = Params()
+    for a in self.offroad_alerts:
+      # set the alert
+      alert = copy.copy(self.offroad_alerts[a])
+      set_offroad_alert(a, True)
+      alert['extra'] = ''
+      self.assertTrue(json.dumps(alert) == params.get(a, encoding='utf8'))
+
+      # then delete it
+      set_offroad_alert(a, False)
+      self.assertTrue(params.get(a) is None)
+
+  def test_offroad_alerts_extra_text(self):
+    params = Params()
+    for i in range(50):
+      # set the alert
+      a = random.choice(list(self.offroad_alerts))
+      alert = self.offroad_alerts[a]
+      set_offroad_alert(a, True, extra_text="a"*i)
+
+      written_alert = json.loads(params.get(a, encoding='utf8'))
+      self.assertTrue("a"*i == written_alert['extra'])
+      self.assertTrue(alert["text"] == written_alert['text'])
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_cruise_speed.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+import itertools
+import numpy as np
+import unittest
+
+from parameterized import parameterized_class
+from cereal import log
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
+from cereal import car
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+def run_cruise_simulation(cruise, e2e, t_end=20.):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=max(cruise - 1., 0.0),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    cruise_values=[cruise],
+    prob_lead_values=[0.0],
+    breakpoints=[0.],
+    e2e=e2e,
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1, 3]
+
+
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
+                      log.LongitudinalPersonality.schema.enumerants, # personality
+                      [5,35])) # speed
+class TestCruiseSpeed(unittest.TestCase):
+  def test_cruise_speed(self):
+    params = Params()
+    params.put("LongitudinalPersonality", str(self.personality))
+    print(f'Testing {self.speed} m/s')
+    cruise_speed = float(self.speed)
+
+    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e)
+    self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s')
+
+
+# TODO: test pcmCruise
+@parameterized_class(('pcm_cruise',), [(False,)])
+class TestVCruiseHelper(unittest.TestCase):
+  def setUp(self):
+    self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
+    self.v_cruise_helper = VCruiseHelper(self.CP)
+    self.reset_cruise_speed_state()
+
+  def reset_cruise_speed_state(self):
+    # Two resets previous cruise speed
+    for _ in range(2):
+      self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
+
+  def enable(self, v_ego, experimental_mode):
+    # Simulates user pressing set with a current speed
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
+
+  def test_adjust_speed(self):
+    """
+    Asserts speed changes on falling edges of buttons.
+    """
+
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+
+    for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
+      for pressed in (True, False):
+        CS = car.CarState(cruiseState={"available": True})
+        CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)]
+
+        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+        self.assertEqual(pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
+
+  def test_rising_edge_enable(self):
+    """
+    Some car interfaces may enable on rising edge of a button,
+    ensure we don't adjust speed if enabled changes mid-press.
+    """
+
+    # NOTE: enabled is always one frame behind the result from button press in controlsd
+    for enabled, pressed in ((False, False),
+                             (False, True),
+                             (True, False)):
+      CS = car.CarState(cruiseState={"available": True})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
+      self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
+      if pressed:
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+
+      # Expected diff on enabling. Speed should not change on falling edge of pressed
+      self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
+
+  def test_resume_in_standstill(self):
+    """
+    Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
+    """
+
+    self.enable(0, False)
+
+    for standstill in (True, False):
+      for pressed in (True, False):
+        CS = car.CarState(cruiseState={"available": True, "standstill": standstill})
+        CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
+        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+        # speed should only update if not at standstill and button falling edge
+        should_equal = standstill or pressed
+        self.assertEqual(should_equal, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
+
+  def test_set_gas_pressed(self):
+    """
+    Asserts pressing set while enabled with gas pressed sets
+    the speed to the maximum of vEgo and current cruise speed.
+    """
+
+    for v_ego in np.linspace(0, 100, 101):
+      self.reset_cruise_speed_state()
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+
+      # first decrement speed, then perform gas pressed logic
+      expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
+      expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH)  # clip to min of vEgo
+      expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))
+
+      CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
+      self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+      # TODO: fix skipping first run due to enabled on rising edge exception
+      if v_ego == 0.0:
+        continue
+      self.assertEqual(expected_v_cruise_kph, self.v_cruise_helper.v_cruise_kph)
+
+  def test_initialize_v_cruise(self):
+    """
+    Asserts allowed cruise speeds on enabling with SET.
+    """
+
+    for experimental_mode in (True, False):
+      for v_ego in np.linspace(0, 100, 101):
+        self.reset_cruise_speed_state()
+        self.assertFalse(self.v_cruise_helper.v_cruise_initialized)
+
+        self.enable(float(v_ego), experimental_mode)
+        self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
+        self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+import unittest
+import itertools
+from parameterized import parameterized_class
+
+from openpilot.common.params import Params
+from cereal import log
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
+from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=float(v_lead),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    speed_lead_values=[v_lead],
+    breakpoints=[0.],
+    e2e=e2e,
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,2] - output[-1,1]
+
+
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
+                      [log.LongitudinalPersonality.relaxed, # personality
+                       log.LongitudinalPersonality.standard,
+                       log.LongitudinalPersonality.aggressive],
+                      [0,10,35])) # speed
+class TestFollowingDistance(unittest.TestCase):
+  def test_following_distance(self):
+    params = Params()
+    params.put("LongitudinalPersonality", str(self.personality))
+    v_lead = float(self.speed)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e)
+    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    err_ratio = 0.2 if self.e2e else 0.1
+    self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_lateral_mpc.py

@@ -0,0 +1,89 @@
+import unittest
+import numpy as np
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from openpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+
+
+def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
+            lane_width=3.6, poly_shift=0.):
+
+  if lat_mpc is None:
+    lat_mpc = LateralMpc()
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
+
+  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
+  heading_pts = np.zeros(LAT_MPC_N + 1)
+  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
+
+  x0 = np.array([x_init, y_init, psi_init, curvature_init])
+  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
+                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
+
+  # converge in no more than 10 iterations
+  for _ in range(10):
+    lat_mpc.run(x0, p,
+                y_pts, heading_pts, curv_rate_pts)
+  return lat_mpc.x_sol
+
+
+class TestLateralMpc(unittest.TestCase):
+
+  def _assert_null(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      self.assertAlmostEqual(sol[0,i,1], 0., delta=curvature)
+      self.assertAlmostEqual(sol[0,i,2], 0., delta=curvature)
+      self.assertAlmostEqual(sol[0,i,3], 0., delta=curvature)
+
+  def _assert_simmetry(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      self.assertAlmostEqual(sol[0,i,1], -sol[1,i,1], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,2], -sol[1,i,2], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,3], -sol[1,i,3], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,0], sol[1,i,0], delta=curvature)
+
+  def test_straight(self):
+    sol = run_mpc()
+    self._assert_null(np.array([sol]))
+
+  def test_y_symmetry(self):
+    sol = []
+    for y_init in [-0.5, 0.5]:
+      sol.append(run_mpc(y_init=y_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_poly_symmetry(self):
+    sol = []
+    for poly_shift in [-1., 1.]:
+      sol.append(run_mpc(poly_shift=poly_shift))
+    self._assert_simmetry(np.array(sol))
+
+  def test_curvature_symmetry(self):
+    sol = []
+    for curvature_init in [-0.1, 0.1]:
+      sol.append(run_mpc(curvature_init=curvature_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_psi_symmetry(self):
+    sol = []
+    for psi_init in [-0.1, 0.1]:
+      sol.append(run_mpc(psi_init=psi_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_no_overshoot(self):
+    y_init = 1.
+    sol = run_mpc(y_init=y_init)
+    for y in list(sol[:,1]):
+      self.assertGreaterEqual(y_init, abs(y))
+
+  def test_switch_convergence(self):
+    lat_mpc = LateralMpc()
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
+    right_psi_deg = np.degrees(sol[:,2])
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
+    left_psi_deg = np.degrees(sol[:,2])
+    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_leads.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+import unittest
+
+import cereal.messaging as messaging
+
+from openpilot.selfdrive.test.process_replay import replay_process_with_name
+from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA
+
+
+class TestLeads(unittest.TestCase):
+  def test_radar_fault(self):
+    # if there's no radar-related can traffic, radard should either not respond or respond with an error
+    # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
+    def single_iter_pkg():
+      # single iter package, with meaningless cans and empty carState/modelV2
+      msgs = []
+      for _ in range(5):
+        can = messaging.new_message("can", 1)
+        cs = messaging.new_message("carState")
+        msgs.append(can.as_reader())
+        msgs.append(cs.as_reader())
+      model = messaging.new_message("modelV2")
+      msgs.append(model.as_reader())
+
+      return msgs
+
+    msgs = [m for _ in range(3) for m in single_iter_pkg()]
+    out = replay_process_with_name("radard", msgs, fingerprint=TOYOTA.COROLLA_TSS2)
+    states = [m for m in out if m.which() == "radarState"]
+    failures = [not state.valid and len(state.radarState.radarErrors) for state in states]
+
+    self.assertTrue(len(states) == 0 or all(failures))
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_startup.py

@@ -0,0 +1,120 @@
+import os
+from parameterized import parameterized
+
+from cereal import log, car
+import cereal.messaging as messaging
+from openpilot.common.params import Params
+from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
+from openpilot.selfdrive.car.fingerprints import _FINGERPRINTS
+from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA
+from openpilot.selfdrive.car.mazda.values import CAR as MAZDA
+from openpilot.selfdrive.controls.lib.events import EVENT_NAME
+from openpilot.selfdrive.manager.process_config import managed_processes
+
+EventName = car.CarEvent.EventName
+Ecu = car.CarParams.Ecu
+
+COROLLA_FW_VERSIONS = [
+  (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'),
+  (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'),
+  (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'),
+  (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'),
+  (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'),
+]
+COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')]
+COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1]
+
+CX5_FW_VERSIONS = [
+  (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+  (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
+]
+
+
+@parameterized.expand([
+  # TODO: test EventName.startup for release branches
+
+  # officially supported car
+  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"),
+  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"),
+
+  # dashcamOnly car
+  (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"),
+  (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"),
+
+  # unrecognized car with no fw
+  (EventName.startupNoFw, None, None, ""),
+  (EventName.startupNoFw, None, None, ""),
+
+  # unrecognized car
+  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
+  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
+
+  # fuzzy match
+  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
+  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
+])
+def test_startup_alert(expected_event, car_model, fw_versions, brand):
+  controls_sock = messaging.sub_sock("controlsState")
+  pm = messaging.PubMaster(['can', 'pandaStates'])
+
+  params = Params()
+  params.put_bool("OpenpilotEnabledToggle", True)
+
+  # Build capnn version of FW array
+  if fw_versions is not None:
+    car_fw = []
+    cp = car.CarParams.new_message()
+    for ecu, addr, subaddress, version in fw_versions:
+      f = car.CarParams.CarFw.new_message()
+      f.ecu = ecu
+      f.address = addr
+      f.fwVersion = version
+      f.brand = brand
+
+      if subaddress is not None:
+        f.subAddress = subaddress
+
+      car_fw.append(f)
+    cp.carVin = "1" * 17
+    cp.carFw = car_fw
+    params.put("CarParamsCache", cp.to_bytes())
+  else:
+    os.environ['SKIP_FW_QUERY'] = '1'
+
+  managed_processes['controlsd'].start()
+
+  assert pm.wait_for_readers_to_update('can', 5)
+  pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]]))
+
+  assert pm.wait_for_readers_to_update('pandaStates', 5)
+  msg = messaging.new_message('pandaStates', 1)
+  msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
+  pm.send('pandaStates', msg)
+
+  # fingerprint
+  if (car_model is None) or (fw_versions is not None):
+    finger = {addr: 1 for addr in range(1, 100)}
+  else:
+    finger = _FINGERPRINTS[car_model][0]
+
+  msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()]
+  for _ in range(1000):
+    # controlsd waits for boardd to echo back that it has changed the multiplexing mode
+    if not params.get_bool("ObdMultiplexingChanged"):
+      params.put_bool("ObdMultiplexingChanged", True)
+
+    pm.send('can', can_list_to_can_capnp(msgs))
+    assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}"
+
+    ctrls = messaging.drain_sock(controls_sock)
+    if len(ctrls):
+      event_name = ctrls[0].controlsState.alertType.split("/")[0]
+      assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}"
+      break
+  else:
+    raise Exception(f"failed to fingerprint {car_model}")

selfdrive/controls/tests/test_state_machine.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+import unittest
+
+from cereal import car, log
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.car_helpers import interfaces
+from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME
+from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \
+                                          AudibleAlert, EVENTS
+from openpilot.selfdrive.car.mock.values import CAR as MOCK
+
+State = log.ControlsState.OpenpilotState
+
+# The event types that maintain the current state
+MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
+                   State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)}
+ALL_STATES = tuple(State.schema.enumerants.values())
+# The event types checked in DISABLED section of state machine
+ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)
+
+
+def make_event(event_types):
+  event = {}
+  for ev in event_types:
+    event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW,
+                      VisualAlert.none, AudibleAlert.none, 1.)
+  EVENTS[0] = event
+  return 0
+
+
+class TestStateMachine(unittest.TestCase):
+
+  def setUp(self):
+    CarInterface, CarController, CarState = interfaces[MOCK.MOCK]
+    CP = CarInterface.get_non_essential_params(MOCK.MOCK)
+    CI = CarInterface(CP, CarController, CarState)
+
+    self.controlsd = Controls(CI=CI)
+    self.controlsd.events = Events()
+    self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+    self.CS = car.CarState()
+
+  def test_immediate_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
+        self.controlsd.state = state
+        self.controlsd.state_transition(self.CS)
+        self.assertEqual(State.disabled, self.controlsd.state)
+        self.controlsd.events.clear()
+
+  def test_user_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.controlsd.events.add(make_event([et, ET.USER_DISABLE]))
+        self.controlsd.state = state
+        self.controlsd.state_transition(self.CS)
+        self.assertEqual(State.disabled, self.controlsd.state)
+        self.controlsd.events.clear()
+
+  def test_soft_disable(self):
+    for state in ALL_STATES:
+      if state == State.preEnabled:  # preEnabled considers NO_ENTRY instead
+        continue
+      for et in MAINTAIN_STATES[state]:
+        self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE]))
+        self.controlsd.state = state
+        self.controlsd.state_transition(self.CS)
+        self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling)
+        self.controlsd.events.clear()
+
+  def test_soft_disable_timer(self):
+    self.controlsd.state = State.enabled
+    self.controlsd.events.add(make_event([ET.SOFT_DISABLE]))
+    self.controlsd.state_transition(self.CS)
+    for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
+      self.assertEqual(self.controlsd.state, State.softDisabling)
+      self.controlsd.state_transition(self.CS)
+
+    self.assertEqual(self.controlsd.state, State.disabled)
+
+  def test_no_entry(self):
+    # Make sure noEntry keeps us disabled
+    for et in ENABLE_EVENT_TYPES:
+      self.controlsd.events.add(make_event([ET.NO_ENTRY, et]))
+      self.controlsd.state_transition(self.CS)
+      self.assertEqual(self.controlsd.state, State.disabled)
+      self.controlsd.events.clear()
+
+  def test_no_entry_pre_enable(self):
+    # preEnabled with noEntry event
+    self.controlsd.state = State.preEnabled
+    self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
+    self.controlsd.state_transition(self.CS)
+    self.assertEqual(self.controlsd.state, State.preEnabled)
+
+  def test_maintain_states(self):
+    # Given current state's event type, we should maintain state
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.controlsd.state = state
+        self.controlsd.events.add(make_event([et]))
+        self.controlsd.state_transition(self.CS)
+        self.assertEqual(self.controlsd.state, state)
+        self.controlsd.events.clear()
+
+
+if __name__ == "__main__":
+  unittest.main()