IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/car/tests/.gitignore

@@ -0,0 +1 @@
+*.bz2

iqpilot/selfdrive/car/tests/big_cars_test.sh

@@ -0,0 +1,11 @@
+#!/usr/bin/env bash
+
+SCRIPT_DIR=$(dirname "$0")
+BASEDIR=$(realpath "$SCRIPT_DIR/../../../../")
+cd $BASEDIR
+
+export MAX_EXAMPLES=300
+export INTERNAL_SEG_CNT=300
+export INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt
+
+cd iqpilot/selfdrive/car/tests && pytest test_models.py test_car_interfaces.py

iqpilot/selfdrive/car/tests/test_car_interfaces.py

@@ -0,0 +1,152 @@
+import os
+import pytest
+import hypothesis.strategies as st
+from hypothesis import Phase, given, settings
+from parameterized import parameterized
+from types import SimpleNamespace
+
+from iqpilot.cereal import car, custom
+from iqdbc.car import DT_CTRL
+from iqdbc.car.structs import CarParams
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.tests.test_car_interfaces import get_fuzzy_car_interface, get_fuzzy_strategy
+from iqdbc.car.mock.values import CAR as MOCK
+from iqdbc.car.values import PLATFORMS
+from iqpilot.selfdrive.car.card import run_optional_pre_init
+from iqpilot.selfdrive.car.helpers import convert_iq_car_control, convert_iq_car_control_compact
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl
+from iqpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator
+
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '60'))
+VW_MLB_LONG_APPLY_EXCLUDE = {"AUDI_Q5_MK1", "PORSCHE_MACAN_MK1"}
+SIGNED_RUNTIME_BRANDS = {"tesla", "volkswagen"}
+SIGNED_RUNTIME_PLATFORMS = tuple(car_name for car_name in sorted(PLATFORMS)
+                                 if interfaces[car_name].__module__.split(".")[-2] in SIGNED_RUNTIME_BRANDS)
+PUBLIC_INTERFACE_PLATFORMS = tuple(car_name for car_name in sorted(PLATFORMS)
+                                   if car_name not in SIGNED_RUNTIME_PLATFORMS)
+
+
+def exercise_car_interface(car_name, draw):
+  car_interface = get_fuzzy_car_interface(car_name, draw)
+  if car_name in VW_MLB_LONG_APPLY_EXCLUDE:
+    car_interface.CP.openpilotLongitudinalControl = False
+  car_params = car_interface.CP.as_reader()
+  car_params_iq = car_interface.CP_IQ
+  iqpilot_interfaces.apply_iq_car_config(car_interface)
+
+  cc_msg = FuzzyGenerator.get_random_msg(draw, car.CarControl, real_floats=True)
+  cc_sp_msg = FuzzyGenerator.get_random_msg(draw, custom.IQCarControl, real_floats=True)
+  now_nanos = 0
+  CC = car.CarControl.new_message(**cc_msg).as_reader()
+  CC_IQ = convert_iq_car_control(custom.IQCarControl.new_message(**cc_sp_msg).as_reader())
+  for _ in range(10):
+    car_interface.update([])
+    car_interface.apply(CC, CC_IQ, now_nanos)
+    now_nanos += DT_CTRL * 1e9
+
+  CC = car.CarControl.new_message(**cc_msg)
+  CC.enabled = True
+  CC.latActive = True
+  CC.longActive = True
+  CC = CC.as_reader()
+  for _ in range(10):
+    car_interface.update([])
+    car_interface.apply(CC, CC_IQ, now_nanos)
+    now_nanos += DT_CTRL * 1e9
+
+  LongControl(car_params, car_params_iq)
+  if car_params.steerControlType == CarParams.SteerControlType.angle:
+    LatControlAngle(car_params, car_params_iq, car_interface, DT_CTRL)
+  elif car_params.lateralTuning.which() == 'pid':
+    LatControlPID(car_params, car_params_iq, car_interface, DT_CTRL)
+  elif car_params.lateralTuning.which() == 'torque':
+    LatControlTorque(car_params, car_params_iq, car_interface, DT_CTRL)
+
+
+@pytest.mark.car_ports
+class TestCarInterfaces:
+  # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause
+  #  many generated examples to overrun when max_examples > ~20, don't use it
+  @parameterized.expand([(car,) for car in PUBLIC_INTERFACE_PLATFORMS] + [MOCK.MOCK])
+  @settings(max_examples=MAX_EXAMPLES, deadline=None,
+            phases=(Phase.reuse, Phase.generate, Phase.shrink))
+  @given(data=st.data())
+  def test_car_interfaces(self, car_name, data):
+    exercise_car_interface(car_name, data.draw)
+
+  @parameterized.expand([(car,) for car in SIGNED_RUNTIME_PLATFORMS])
+  @settings(max_examples=MAX_EXAMPLES, deadline=None,
+            phases=(Phase.reuse, Phase.generate, Phase.shrink))
+  @given(data=st.data())
+  def test_public_car_params(self, car_name, data):
+    params = data.draw(get_fuzzy_strategy())
+    params['fingerprints'] |= {key + 1: params['fingerprints'][0] for key in range(6)}
+    car_interface = interfaces[car_name]
+    car_params = car_interface.get_params(car_name, params['fingerprints'], params['car_fw'],
+                                           alpha_long=params['alpha_long'], is_release=False, docs=False)
+    car_params_iq = car_interface.get_params_iq(car_params, car_name, params['fingerprints'], params['car_fw'],
+                                                 alpha_long=params['alpha_long'], is_release_iq=False, docs=False)
+    assert car_params.mass > 1
+    assert car_params.wheelbase > 0
+    assert car_params.maxLateralAccel > 0
+    assert car_params_iq is not None
+
+
+def test_convert_iq_car_control_compact_skips_leads():
+  msg = custom.IQCarControl.new_message()
+  msg.aol.enabled = True
+  msg.aol.active = True
+  msg.aol.available = True
+  param = msg.init("params", 1)
+  param[0].key = "enhancedStockLongitudinalControl.setSpeedKph"
+  param[0].type = "float"
+  param[0].value = b"42.0"
+  msg.leadOne.dRel = 42.0
+  msg.leadOne.status = True
+  msg.leadTwo.dRel = 84.0
+  msg.leadTwo.status = True
+
+  compact = convert_iq_car_control_compact(msg.as_reader(), include_leads=False)
+  assert compact.aol.enabled
+  assert compact.aol.active
+  assert compact.aol.available
+  assert len(compact.params) == 1
+  assert compact.params[0].key == "enhancedStockLongitudinalControl.setSpeedKph"
+  assert compact.params[0].type == "float"
+  assert compact.params[0].value == b"42.0"
+  assert compact.leadOne.dRel == 0.0
+  assert not compact.leadOne.status
+  assert compact.leadTwo.dRel == 0.0
+  assert not compact.leadTwo.status
+
+  full = convert_iq_car_control_compact(msg.as_reader(), include_leads=True)
+  assert full.leadOne.dRel == 42.0
+  assert full.leadOne.status
+  assert full.leadTwo.dRel == 84.0
+  assert full.leadTwo.status
+
+
+def test_run_optional_pre_init_skips_missing_hook():
+  ci = SimpleNamespace()
+  run_optional_pre_init(ci, car.CarParams(), custom.IQCarParams(), (lambda wait_for_one=False: [], lambda msgs: None))
+
+
+def test_run_optional_pre_init_calls_hook():
+  called = []
+
+  class CI:
+    @staticmethod
+    def pre_init(CP, CP_IQ, can_recv, can_send):
+      called.append((CP, CP_IQ, can_recv, can_send))
+
+  can_callbacks = (lambda wait_for_one=False: [], lambda msgs: None)
+  cp = car.CarParams()
+  cp_iq = custom.IQCarParams()
+  run_optional_pre_init(CI(), cp, cp_iq, can_callbacks)
+
+  assert called == [(cp, cp_iq, *can_callbacks)]

iqpilot/selfdrive/car/tests/test_car_specific_events.py

@@ -0,0 +1,117 @@
+from types import SimpleNamespace
+
+from iqpilot.cereal import log
+from iqdbc.car import structs
+
+from iqpilot.selfdrive.car.car_specific import CarSpecificEvents
+
+EventName = log.OnroadEvent.EventName
+GearShifter = structs.CarState.GearShifter
+
+
+def make_car_state(**overrides):
+  base = dict(
+    doorOpen=False,
+    seatbeltUnlatched=False,
+    gearShifter=GearShifter.drive,
+    cruiseState=SimpleNamespace(available=True, enabled=False, nonAdaptive=False),
+    cruiseFaultLateralMode=False,
+    espDisabled=False,
+    espActive=False,
+    stockFcw=False,
+    stockAeb=False,
+    stockLkas=False,
+    vEgo=15.0,
+    brakeHoldActive=False,
+    parkingBrake=False,
+    accFaulted=False,
+    steeringPressed=False,
+    steeringDisengage=False,
+    brakePressed=False,
+    standstill=False,
+    gasPressed=False,
+    vehicleSensorsInvalid=False,
+    invalidLkasSetting=False,
+    lowSpeedAlert=False,
+    buttonEnable=False,
+    buttonEvents=[],
+    steerFaultTemporary=False,
+    steerFaultPermanent=False,
+    blockPcmEnable=False,
+  )
+  base.update(overrides)
+  return SimpleNamespace(**base)
+
+
+def test_pcm_disable_suppressed_during_cruise_fault_lateral_mode():
+  cp = SimpleNamespace(
+    brand="volkswagen",
+    openpilotLongitudinalControl=False,
+    minEnableSpeed=0.0,
+    pcmCruise=True,
+    carFingerprint="MOCK",
+  )
+  events = CarSpecificEvents(cp)
+
+  cs = make_car_state(cruiseFaultLateralMode=True)
+  cs_prev = make_car_state(cruiseState=SimpleNamespace(available=True, enabled=True, nonAdaptive=False))
+
+  out = events.update(cs, cs_prev, SimpleNamespace(actuators=SimpleNamespace(accel=0.0)))
+
+  assert not out.has(EventName.pcmDisable)
+
+
+def test_cruise_fault_lateral_mode_replaces_acc_faulted_alert():
+  cp = SimpleNamespace(
+    brand="volkswagen",
+    openpilotLongitudinalControl=False,
+    minEnableSpeed=0.0,
+    pcmCruise=True,
+    carFingerprint="MOCK",
+  )
+  events = CarSpecificEvents(cp)
+
+  cs = make_car_state(accFaulted=True, cruiseFaultLateralMode=True)
+  cs_prev = make_car_state()
+
+  out = events.update(cs, cs_prev, SimpleNamespace(actuators=SimpleNamespace(accel=0.0)))
+
+  assert out.has(EventName.cruiseFaultLateralAllowed)
+  assert not out.has(EventName.accFaulted)
+
+
+def test_acc_faulted_still_emitted_without_cruise_fault_lateral_mode():
+  cp = SimpleNamespace(
+    brand="volkswagen",
+    openpilotLongitudinalControl=False,
+    minEnableSpeed=0.0,
+    pcmCruise=True,
+    carFingerprint="MOCK",
+  )
+  events = CarSpecificEvents(cp)
+
+  cs = make_car_state(accFaulted=True)
+  cs_prev = make_car_state()
+
+  out = events.update(cs, cs_prev, SimpleNamespace(actuators=SimpleNamespace(accel=0.0)))
+
+  assert out.has(EventName.accFaulted)
+  assert not out.has(EventName.cruiseFaultLateralAllowed)
+
+
+def test_pcm_disable_still_emitted_without_cruise_fault_lateral_mode():
+  cp = SimpleNamespace(
+    brand="volkswagen",
+    openpilotLongitudinalControl=False,
+    minEnableSpeed=0.0,
+    pcmCruise=True,
+    carFingerprint="MOCK",
+  )
+  events = CarSpecificEvents(cp)
+
+  cs = make_car_state()
+  cs_prev = make_car_state(cruiseState=SimpleNamespace(available=True, enabled=True, nonAdaptive=False))
+
+  out = events.update(cs, cs_prev, SimpleNamespace(actuators=SimpleNamespace(accel=0.0)))
+
+  assert out.has(EventName.pcmDisable)

iqpilot/selfdrive/car/tests/test_cruise_speed.py

@@ -0,0 +1,175 @@
+import pytest
+import itertools
+import numpy as np
+
+from parameterized import parameterized_class
+from iqpilot.cereal import log
+from iqpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
+from iqpilot.cereal import car, custom
+from iqpilot.common.constants import CV
+from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+def run_cruise_simulation(cruise, e2e, personality, 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,
+    personality=personality,
+  )
+  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:
+  def test_cruise_speed(self):
+    print(f'Testing {self.speed} m/s')
+    cruise_speed = float(self.speed)
+
+    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
+    assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s'
+
+
+# TODO: test pcmCruise and pcmCruiseSpeed
+@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
+class TestVCruiseHelper:
+  def setup_method(self):
+    self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
+    self.CP_IQ = custom.IQCarParams(pcmCruiseSpeed=self.pcm_cruise_speed)
+    self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_IQ)
+    self.reset_cruise_speed_state()
+
+  def reset_cruise_speed_state(self):
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMode", 0)
+    self.v_cruise_helper.params.put_bool("IQE2ESetSpeedUseCurrent", False)
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMph", 65)
+    self.v_cruise_helper.read_custom_set_speed_params()
+    # 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, iq_dynamic_mode):
+    # Simulates user pressing set with a current speed
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, iq_dynamic_mode)
+
+  def test_adjust_speed(self):
+    """
+    Asserts speed changes on falling edges of buttons.
+    """
+
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, 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)
+        assert 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, False)
+
+      # Expected diff on enabling. Speed should not change on falling edge of pressed
+      assert 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, 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
+        assert 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, 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
+      assert 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 iq_dynamic_mode in (True, False):
+        for v_ego in np.linspace(0, 100, 101):
+          self.reset_cruise_speed_state()
+          assert not self.v_cruise_helper.v_cruise_initialized
+
+          self.enable(float(v_ego), experimental_mode, iq_dynamic_mode)
+          assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
+          assert self.v_cruise_helper.v_cruise_initialized
+
+  def test_iq_mode_fixed_set_speed(self):
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMode", 1)
+    self.v_cruise_helper.params.put_bool("IQE2ESetSpeedUseCurrent", False)
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMph", 72)
+    self.v_cruise_helper.read_custom_set_speed_params()
+
+    self.enable(30 * CV.MPH_TO_MS, True, False)
+    assert self.v_cruise_helper.v_cruise_kph == int(round(72 * CV.MPH_TO_KPH))
+
+  def test_iq_mode_current_speed_override(self):
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMode", 1)
+    self.v_cruise_helper.params.put_bool("IQE2ESetSpeedUseCurrent", True)
+    self.v_cruise_helper.params.put("IQE2ESetSpeedMph", 72)
+    self.v_cruise_helper.read_custom_set_speed_params()
+
+    self.enable(47 * CV.MPH_TO_MS, True, False)
+    assert self.v_cruise_helper.v_cruise_kph == int(round(47 * CV.MPH_TO_KPH))

iqpilot/selfdrive/car/tests/test_docs.py

@@ -0,0 +1,27 @@
+import os
+
+from iqpilot.common.basedir import BASEDIR
+from iqdbc.car.docs import generate_cars_md, get_all_car_docs
+from iqdbc.lvbs.car.car_catalog import build_car_catalog
+from iqpilot.selfdrive.debug.dump_car_docs import dump_car_docs
+from iqpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff
+from iqpilot.selfdrive.car.docs import CARS_MD_TEMPLATE
+from iqpilot.selfdrive.car.vehicle_catalog import load_catalog
+
+
+class TestCarDocs:
+  @classmethod
+  def setup_class(cls):
+    cls.all_cars = get_all_car_docs()
+
+  def test_generator(self):
+    generate_cars_md(self.all_cars, CARS_MD_TEMPLATE)
+
+  def test_docs_diff(self):
+    dump_path = os.path.join(BASEDIR, "iqpilot", "selfdrive", "car", "tests", "cars_dump")
+    dump_car_docs(dump_path)
+    print_car_docs_diff(dump_path)
+    os.remove(dump_path)
+
+  def test_vehicle_catalog(self):
+    assert load_catalog() == build_car_catalog()

iqpilot/selfdrive/car/tests/test_helpers.py

@@ -0,0 +1,50 @@
+from dataclasses import dataclass
+from enum import Enum
+
+import pytest
+
+from iqdbc.car import structs
+from iqdbc.car.hyundai.values import HyundaiFlagsIQ
+
+from iqpilot.selfdrive.car.helpers import asdictref, convert_to_capnp
+
+
+class SampleEnum(Enum):
+  value = 7
+
+
+@dataclass
+class SampleStruct:
+  enum: SampleEnum
+  values: tuple[int, ...]
+  mapping: dict[str, list[SampleEnum]]
+
+
+def test_convert_to_capnp_normalizes_enum_values():
+  params = structs.IQCarParams(flags=HyundaiFlagsIQ.HAS_LFA_BUTTON)
+  assert asdictref(params)["flags"] == HyundaiFlagsIQ.HAS_LFA_BUTTON.value
+  assert convert_to_capnp(params).flags == HyundaiFlagsIQ.HAS_LFA_BUTTON.value
+
+
+def test_asdictref_preserves_container_types_and_resolves_enums():
+  source = SampleStruct(SampleEnum.value, (1, 2), {"items": [SampleEnum.value]})
+  converted = asdictref(source)
+  assert converted == {"enum": 7, "values": (1, 2), "mapping": {"items": [7]}}
+  assert isinstance(converted["values"], tuple)
+  assert isinstance(converted["mapping"]["items"], list)
+
+
+def test_asdictref_rejects_non_dataclass_values():
+  with pytest.raises(TypeError, match="dataclass instances"):
+    asdictref(object())
+
+
+def test_convert_to_capnp_supports_iq_car_state():
+  state = convert_to_capnp(structs.IQCarState())
+  assert state.speedLimit == 0
+  assert not state.accelPressed
+
+
+def test_convert_to_capnp_rejects_unknown_dataclass():
+  with pytest.raises(ValueError, match="Unsupported struct type"):
+    convert_to_capnp(SampleStruct(SampleEnum.value, (), {}))

iqpilot/selfdrive/car/tests/test_longitudinal_mode_persistence.py

@@ -0,0 +1,38 @@
+from iqpilot.cereal import custom
+from iqdbc.car import structs
+
+from iqpilot.selfdrive.car.interfaces import _cleanup_unsupported_params
+
+
+class DummyParams:
+  def __init__(self):
+    self.removed: list[str] = []
+    self.values: dict[str, object] = {}
+
+  def remove(self, key: str) -> None:
+    self.removed.append(key)
+
+  def get_bool(self, key: str) -> bool:
+    return bool(self.values.get(key, False))
+
+  def get(self, key: str, return_default: bool = False):
+    return self.values.get(key)
+
+  def put(self, key: str, value) -> None:
+    self.values[key] = value
+
+
+class TestLongitudinalModePersistence:
+  def test_iq_dynamic_mode_is_not_removed_when_openpilot_long_is_unavailable(self):
+    params = DummyParams()
+    cp = structs.CarParams()
+    cp.openpilotLongitudinalControl = False
+    cp.steerControlType = structs.CarParams.SteerControlType.torque
+
+    cp_iq = custom.IQCarParams()
+    cp_iq.pcmCruiseSpeed = True
+
+    _cleanup_unsupported_params(cp, cp_iq, params)
+
+    assert "IQDynamicMode" not in params.removed
+    assert "LongIncrementsEnabled" in params.removed

iqpilot/selfdrive/car/tests/test_models.py

@@ -0,0 +1,547 @@
+import time
+import copy
+import os
+import pytest
+import random
+import unittest # noqa: TID251
+from collections import defaultdict, Counter
+import hypothesis.strategies as st
+from hypothesis import Phase, given, settings
+
+from iqdbc.car import DT_CTRL, gen_empty_fingerprint, structs
+from iqdbc.can.parser import MAX_BAD_COUNTER
+from iqdbc.car.can_definitions import CanData
+from iqdbc.car.car_helpers import FRAME_FINGERPRINT, interfaces
+from iqdbc.car.fingerprints import MIGRATION
+from iqdbc.car.honda.values import CAR as HONDA, HondaFlags
+from iqdbc.car.structs import car
+from iqdbc.car.tests.routes import routes, CarTestRoute
+from iqdbc.car.values import Platform
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.pandad import can_capnp_to_list
+from iqpilot.selfdrive.test.helpers import read_segment_list
+from iqpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
+from iqpilot.tools.lib.logreader import LogReader, LogsUnavailable, openpilotci_source, internal_source, comma_api_source
+from iqpilot.tools.lib.route import SegmentName
+
+SafetyModel = car.CarParams.SafetyModel
+SteerControlType = structs.CarParams.SteerControlType
+
+NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
+JOB_ID = int(os.environ.get("JOB_ID", "0"))
+INTERNAL_SEG_LIST = os.environ.get("INTERNAL_SEG_LIST", "")
+INTERNAL_SEG_CNT = int(os.environ.get("INTERNAL_SEG_CNT", "0"))
+MAX_EXAMPLES = int(os.environ.get("MAX_EXAMPLES", "300"))
+CI = os.environ.get("CI", None) is not None
+RELAY_TRANSITION_TIMEOUT_US = 10_000_000
+PRIVATE_RUNTIME_BRANDS = {"tesla", "volkswagen"}
+UNSUPPORTED_ROUTE_BRANDS = {"body"}
+DASHCAM_ONLY_PLATFORMS = {
+  "BUICK_REGAL",
+  "GMC_YUKON",
+  "MAZDA_3",
+  "MAZDA_6",
+  "MAZDA_CX5",
+  "MAZDA_CX9",
+  "PSA_PEUGEOT_208",
+  "SUBARU_ASCENT_2023",
+  "SUBARU_CROSSTREK_HYBRID",
+  "SUBARU_FORESTER_2022",
+  "SUBARU_OUTBACK_2023",
+}
+
+
+@pytest.fixture(autouse=True)
+def controls_ready_params(openpilot_function_fixture, tmp_path):
+  root = str(tmp_path)
+  os.mkdir(os.path.join(root, "d_tmp"))
+  os.symlink("d_tmp", os.path.join(root, "d"))
+  os.environ["PARAMS_ROOT"] = root
+  Params().put_bool("ControlsReady", True)
+  yield
+
+
+def normalize_can_buses(can: tuple[int, list[CanData]], raw_can_keys: set[tuple[int, int]]) -> tuple[int, list[CanData]]:
+  timestamp, messages = can
+  return timestamp, [CanData(msg.address, msg.dat, msg.src % 128) for msg in messages
+                     if msg.src < 128 or (msg.address, msg.src % 128) not in raw_can_keys]
+
+
+def get_test_cases() -> list[tuple[str, CarTestRoute | None]]:
+  test_cases = []
+  if not len(INTERNAL_SEG_LIST):
+    for i, route in enumerate(sorted(routes, key=lambda item: (str(item.car_model), item.route, item.segment or -1))):
+      brand = interfaces[str(route.car_model)].__module__.split(".")[-2]
+      if brand not in PRIVATE_RUNTIME_BRANDS | UNSUPPORTED_ROUTE_BRANDS and i % NUM_JOBS == JOB_ID:
+        test_cases.append((str(route.car_model), route))
+
+  else:
+    segment_list = read_segment_list(os.path.join(BASEDIR, INTERNAL_SEG_LIST))
+    segment_list = random.sample(segment_list, INTERNAL_SEG_CNT or len(segment_list))
+    for platform, segment in segment_list:
+      platform = MIGRATION.get(platform, platform)
+      segment_name = SegmentName(segment)
+      test_cases.append((platform, CarTestRoute(segment_name.route_name.canonical_name, platform,
+                                                segment=segment_name.segment_num)))
+  return test_cases
+
+
+@pytest.mark.slow
+@pytest.mark.shared_download_cache
+@pytest.mark.xdist_group_class_property('test_route')
+class CarModelTestBase(unittest.TestCase):
+  __test__ = False
+  platform: Platform | None = None
+  test_route: CarTestRoute | None = None
+
+  can_msgs: list[tuple[int, list[CanData]]]
+  fingerprint: dict[int, dict[int, int]]
+  elm_frame: int | None
+  car_safety_mode_frame: int | None
+
+  @classmethod
+  def get_testing_data_from_logreader(cls, lr):
+    car_fw = []
+    can_msgs = []
+    cls.elm_frame = None
+    cls.car_safety_mode_frame = None
+    cls.fingerprint = gen_empty_fingerprint()
+    alpha_long = False
+    for msg in lr:
+      if msg.which() == "can":
+        can = can_capnp_to_list((msg.as_builder().to_bytes(),))[0]
+        can_msgs.append((can[0], [CanData(*can) for can in can[1]]))
+        if len(can_msgs) <= FRAME_FINGERPRINT:
+          for m in msg.can:
+            if m.src < 64:
+              cls.fingerprint[m.src][m.address] = len(m.dat)
+
+      elif msg.which() == "carParams":
+        car_fw = msg.carParams.carFw
+        if msg.carParams.openpilotLongitudinalControl:
+          alpha_long = True
+        if cls.platform is None:
+          live_fingerprint = msg.carParams.carFingerprint
+          cls.platform = MIGRATION.get(live_fingerprint, live_fingerprint)
+
+      # Log which can frame the panda safety mode left ELM327, for CAN validity checks
+      elif msg.which() == 'pandaStates':
+        for ps in msg.pandaStates:
+          if cls.elm_frame is None and ps.safetyModel != SafetyModel.elm327:
+            cls.elm_frame = len(can_msgs)
+          if cls.car_safety_mode_frame is None and ps.safetyModel not in \
+            (SafetyModel.elm327, SafetyModel.noOutput):
+            cls.car_safety_mode_frame = len(can_msgs)
+
+      elif msg.which() == 'pandaStateDEPRECATED':
+        if cls.elm_frame is None and msg.pandaStateDEPRECATED.safetyModel != SafetyModel.elm327:
+          cls.elm_frame = len(can_msgs)
+        if cls.car_safety_mode_frame is None and msg.pandaStateDEPRECATED.safetyModel not in \
+          (SafetyModel.elm327, SafetyModel.noOutput):
+          cls.car_safety_mode_frame = len(can_msgs)
+
+    assert len(can_msgs) > int(50 / DT_CTRL), "no can data found"
+    return car_fw, can_msgs, alpha_long
+
+  @classmethod
+  def get_testing_data(cls):
+    test_segs = (2, 1, 0)
+    if cls.test_route.segment is not None:
+      test_segs = (cls.test_route.segment,)
+
+    for seg in test_segs:
+      segment_range = f"{cls.test_route.route}/{seg}"
+
+      try:
+        sources = [internal_source] if len(INTERNAL_SEG_LIST) else [openpilotci_source, comma_api_source]
+        lr = LogReader(segment_range, sources=sources, sort_by_time=True)
+        return cls.get_testing_data_from_logreader(lr)
+      except (LogsUnavailable, AssertionError):
+        pass
+
+    raise Exception(f"Route: {repr(cls.test_route.route)} with segments: {test_segs} not found or no CAN msgs found. Is it uploaded and public?")
+
+
+  @classmethod
+  def setUpClass(cls):
+    car_fw, cls.can_msgs, alpha_long = cls.get_testing_data()
+    cls.raw_can_keys = {(msg.address, msg.src) for _, messages in cls.can_msgs for msg in messages if msg.src < 128}
+
+    # if relay is expected to be open in the route
+    cls.openpilot_enabled = cls.car_safety_mode_frame is not None
+
+    cls.CarInterface = interfaces[cls.platform]
+    cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False)
+    cls.CP_IQ = cls.CarInterface.get_params_iq(cls.CP, cls.platform,  cls.fingerprint, car_fw, alpha_long, False, docs=False)
+    assert cls.CP
+    assert cls.CP_IQ
+    assert cls.CP.carFingerprint == cls.platform
+
+    os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT
+
+  @classmethod
+  def tearDownClass(cls):
+    del cls.can_msgs
+
+  def setUp(self):
+    from iqdbc.safety.tests.libsafety import libsafety_py
+
+    self.libsafety_py = libsafety_py
+    self.CI = self.CarInterface(self.CP.copy(), copy.deepcopy(self.CP_IQ))
+    assert self.CI
+
+    # TODO: check safetyModel is in release panda build
+    self.safety = libsafety_py.libsafety
+
+    safety_param_iq = self.CP_IQ.iqSafetyFlags
+    self.safety.set_current_safety_param_iq(safety_param_iq)
+
+    cfg = self.CP.safetyConfigs[-1]
+    set_status = self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam)
+    self.assertEqual(0, set_status, f"failed to set safetyModel {cfg}")
+    self.safety.init_tests()
+
+  def test_car_params(self):
+    self.assertFalse(self.CP.dashcamOnly)
+
+    # make sure car params are within a valid range
+    self.assertGreater(self.CP.mass, 1)
+
+    if self.CP.steerControlType != SteerControlType.angle:
+      tuning = self.CP.lateralTuning.which()
+      if tuning == 'pid':
+        self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
+      elif tuning == 'torque':
+        self.assertTrue(self.CP.lateralTuning.torque.latAccelFactor > 0)
+      else:
+        raise Exception("unknown tuning")
+
+  def test_car_interface(self):
+    can_invalid_cnt = 0
+    invalid_reasons = Counter()
+    CC = structs.CarControl().as_reader()
+    CC_IQ = structs.IQCarControl()
+
+    for i, msg in enumerate(self.can_msgs):
+      CS, _ = self.CI.update(normalize_can_buses(msg, self.raw_can_keys))
+      self.CI.apply(CC, CC_IQ, msg[0])
+
+      # wait max of 2s for low frequency msgs to be seen
+      if i > 250:
+        can_invalid_cnt += not CS.canValid
+        if not CS.canValid:
+          for bus, cp in self.CI.can_parsers.items():
+            bus_timeout = cp.bus_timeout
+            for state in cp.message_states.values():
+              if state.counter_fail >= MAX_BAD_COUNTER:
+                invalid_reasons[f"{bus}:{state.name}:counter"] += 1
+              if not state.valid(cp._last_update_nanos, bus_timeout):
+                invalid_reasons[f"{bus}:{state.name}:timeout"] += 1
+
+    self.assertEqual(can_invalid_cnt, 0, dict(invalid_reasons))
+
+  def test_radar_interface(self):
+    RI = self.CarInterface.RadarInterface(self.CP, self.CP_IQ)
+    assert RI
+
+    # Since OBD port is multiplexed to bus 1 (commonly radar bus) while fingerprinting,
+    # start parsing CAN messages after we've left ELM mode and can expect CAN traffic
+    error_cnt = 0
+    for i, msg in enumerate(self.can_msgs[self.elm_frame:]):
+      rr: structs.RadarData | None = RI.update(normalize_can_buses(msg, self.raw_can_keys))
+      if rr is not None and i > 50:
+        error_cnt += rr.errors.canError
+    self.assertEqual(error_cnt, 0)
+
+  def test_panda_safety_rx_checks(self):
+    start_ts = self.can_msgs[0][0]
+
+    failed_addrs = Counter()
+    last_relay_malfunction_us = 0.
+    relay_open_inferred = False
+    for can_idx, can in enumerate(self.can_msgs):
+      # update panda timer
+      t = (can[0] - start_ts) / 1e3
+      self.safety.set_timer(int(t))
+
+      # run all msgs through the safety RX hook
+      for msg in can[1]:
+        if msg.src >= 64:
+          continue
+
+        to_send = self.libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
+        if self.safety.safety_rx_hook(to_send) != 1:
+          failed_addrs[hex(msg.address)] += 1
+
+      relay_malfunction = self.safety.get_relay_malfunction()
+      for msg in can[1]:
+        if msg.src >= 128 and (msg.address, msg.src % 128) not in self.raw_can_keys:
+          to_send = self.libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
+          self.safety.safety_rx_hook(to_send)
+      self.safety.set_relay_malfunction(relay_malfunction)
+
+      # ensure all msgs defined in the addr checks are valid
+      self.safety.safety_tick_current_safety_config()
+      if t > 1e6:
+        self.assertTrue(self.safety.safety_config_valid())
+
+      if self.car_safety_mode_frame is not None:
+        if can_idx >= self.car_safety_mode_frame:
+          self.assertFalse(self.safety.get_relay_malfunction())
+        else:
+          self.safety.set_relay_malfunction(False)
+      elif relay_open_inferred:
+        self.assertFalse(self.safety.get_relay_malfunction())
+      elif self.safety.get_relay_malfunction():
+        last_relay_malfunction_us = t
+        self.safety.set_relay_malfunction(False)
+      elif t - last_relay_malfunction_us > RELAY_TRANSITION_TIMEOUT_US:
+        relay_open_inferred = True
+      else:
+        self.safety.set_relay_malfunction(False)
+
+    self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}")
+
+    # ensure RX checks go invalid after small time with no traffic
+    self.safety.set_timer(int(t + (2*1e6)))
+    self.safety.safety_tick_current_safety_config()
+    self.assertFalse(self.safety.safety_config_valid())
+
+  def test_panda_safety_tx_cases(self, data=None):
+    """Asserts we can tx common messages"""
+    def test_car_controller(car_control, car_control_iq):
+      def run_controller(CI):
+        now_nanos = 0
+        msgs_sent = 0
+        for _ in range(round(10.0 / DT_CTRL)):
+          CI.update([])
+          _, sendcan = CI.apply(car_control, car_control_iq, now_nanos)
+          now_nanos += DT_CTRL * 1e9
+          msgs_sent += len(sendcan)
+          for addr, dat, bus in sendcan:
+            to_send = self.libsafety_py.make_CANPacket(addr, bus % 4, dat)
+            self.assertTrue(self.safety.safety_tx_hook(to_send), (addr, dat, bus))
+        return msgs_sent
+
+      CI = self.CarInterface(self.CP, self.CP_IQ)
+      msgs_sent = run_controller(CI)
+      if msgs_sent == 0:
+        CI = self.CarInterface(self.CP, self.CP_IQ)
+        for can in self.can_msgs[self.elm_frame:]:
+          CI.update(normalize_can_buses(can, self.raw_can_keys))
+        msgs_sent = run_controller(CI)
+
+      # Make sure we attempted to send messages
+      self.assertGreater(msgs_sent, 50)
+
+    # Make sure we can send all messages while inactive
+    CC = structs.CarControl()
+    CC_IQ = structs.IQCarControl()
+    test_car_controller(CC.as_reader(), CC_IQ)
+
+    # Test cancel + general messages (controls_allowed=False & cruise_engaged=True)
+    self.safety.set_cruise_engaged_prev(True)
+    CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(cancel=True))
+    test_car_controller(CC.as_reader(), CC_IQ)
+
+    # Test resume + general messages (controls_allowed=True & cruise_engaged=True)
+    self.safety.set_controls_allowed(True)
+    CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(resume=True))
+    test_car_controller(CC.as_reader(), CC_IQ)
+
+  # Skip stdout/stderr capture with pytest, causes elevated memory usage
+  @pytest.mark.nocapture
+  @settings(max_examples=MAX_EXAMPLES, deadline=None,
+            phases=(Phase.reuse, Phase.generate, Phase.shrink))
+  @given(data=st.data())
+  def test_panda_safety_carstate_fuzzy(self, data):
+    """
+      For each example, pick a random CAN message on the bus and fuzz its data,
+      checking for panda state mismatches.
+    """
+
+    valid_addrs = [(addr, bus, size) for bus, addrs in self.fingerprint.items() for addr, size in addrs.items()]
+    address, bus, size = data.draw(st.sampled_from(valid_addrs))
+
+    msg_strategy = st.binary(min_size=size, max_size=size)
+    msgs = data.draw(st.lists(msg_strategy, min_size=20))
+
+    vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar
+
+    for n, dat in enumerate(msgs):
+      # due to panda updating state selectively, only edges are expected to match
+      # TODO: warm up CarState with real CAN messages to check edge of both sources
+      #  (eg. toyota's gasPressed is the inverse of a signal being set)
+      prev_panda_gas = self.safety.get_gas_pressed_prev()
+      prev_panda_brake = self.safety.get_brake_pressed_prev()
+      prev_panda_regen_braking = self.safety.get_regen_braking_prev()
+      prev_panda_steering_disengage = self.safety.get_steering_disengage_prev()
+      prev_panda_vehicle_moving = self.safety.get_vehicle_moving()
+      prev_panda_vehicle_speed_min = self.safety.get_vehicle_speed_min()
+      prev_panda_vehicle_speed_max = self.safety.get_vehicle_speed_max()
+      prev_panda_cruise_engaged = self.safety.get_cruise_engaged_prev()
+      prev_panda_acc_main_on = self.safety.get_acc_main_on()
+
+      to_send = self.libsafety_py.make_CANPacket(address, bus, dat)
+      self.safety.safety_rx_hook(to_send)
+
+      can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])]
+      CS, _ = self.CI.update(can)
+      if n < 5:  # CANParser warmup time
+        continue
+
+      if self.safety.get_gas_pressed_prev() != prev_panda_gas:
+        self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev())
+
+      if self.safety.get_brake_pressed_prev() != prev_panda_brake:
+        # TODO: remove this exception once this mismatch is resolved
+        brake_pressed = CS.brakePressed
+        if CS.brakePressed and not self.safety.get_brake_pressed_prev():
+          if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05:
+            brake_pressed = False
+
+        self.assertEqual(brake_pressed, self.safety.get_brake_pressed_prev())
+
+      if self.safety.get_regen_braking_prev() != prev_panda_regen_braking:
+        self.assertEqual(CS.regenBraking, self.safety.get_regen_braking_prev())
+
+      if self.safety.get_steering_disengage_prev() != prev_panda_steering_disengage:
+        self.assertEqual(CS.steeringDisengage, self.safety.get_steering_disengage_prev())
+
+      if self.safety.get_vehicle_moving() != prev_panda_vehicle_moving and not self.CP.notCar:
+        self.assertEqual(not CS.standstill, self.safety.get_vehicle_moving())
+
+      # check vehicle speed if angle control car or available
+      if self.safety.get_vehicle_speed_min() > 0 or self.safety.get_vehicle_speed_max() > 0:
+        vehicle_speed_seen = True
+
+      if vehicle_speed_seen and (self.safety.get_vehicle_speed_min() != prev_panda_vehicle_speed_min or
+                                 self.safety.get_vehicle_speed_max() != prev_panda_vehicle_speed_max):
+        v_ego_raw = CS.vEgoRaw / self.CP.wheelSpeedFactor
+        self.assertFalse(v_ego_raw > (self.safety.get_vehicle_speed_max() + 1e-3) or
+                         v_ego_raw < (self.safety.get_vehicle_speed_min() - 1e-3))
+
+      if not (self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH)):
+        if self.safety.get_cruise_engaged_prev() != prev_panda_cruise_engaged:
+          self.assertEqual(CS.cruiseState.enabled, self.safety.get_cruise_engaged_prev())
+
+      if self.CP.brand == "honda":
+        if self.safety.get_acc_main_on() != prev_panda_acc_main_on:
+          self.assertEqual(CS.cruiseState.available, self.safety.get_acc_main_on())
+
+  def test_panda_safety_carstate(self):
+    """
+      Assert that panda safety matches openpilot's carState
+    """
+    # warm up pass, as initial states may be different
+    for can in self.can_msgs[:300]:
+      self.CI.update(normalize_can_buses(can, self.raw_can_keys))
+      for msg in filter(lambda m: m.src < 64, can[1]):
+        to_send = self.libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
+        self.safety.safety_rx_hook(to_send)
+
+    controls_allowed_prev = False
+    CS_prev = car.CarState.new_message()
+    checks = defaultdict(int)
+    standstill_mismatches = []
+    vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar
+    for idx, can in enumerate(self.can_msgs[300:]):
+      CS, _ = self.CI.update(normalize_can_buses(can, self.raw_can_keys))
+      CS = CS.as_reader()
+      for msg in filter(lambda m: m.src < 64, can[1]):
+        to_send = self.libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
+        ret = self.safety.safety_rx_hook(to_send)
+        self.assertEqual(1, ret, f"safety rx failed ({ret=}): {(msg.address, msg.src % 4)}")
+
+      # Skip first frame so CS_prev is properly initialized
+      if idx == 0:
+        CS_prev = CS
+        # Button may be left pressed in warm up period
+        if not self.CP.pcmCruise:
+          self.safety.set_controls_allowed(0)
+        continue
+
+      # TODO: check rest of panda's carstate (steering, ACC main on, etc.)
+
+      checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev()
+      standstill_mismatch = CS.standstill == self.safety.get_vehicle_moving()
+      checks['standstill'] += standstill_mismatch and not self.CP.notCar
+      if standstill_mismatch and len(standstill_mismatches) < 10:
+        standstill_mismatches.append((idx, CS.standstill, self.safety.get_vehicle_moving(), CS.vEgoRaw,
+                                      self.safety.get_vehicle_speed_min(), self.safety.get_vehicle_speed_max()))
+
+      # check vehicle speed if angle control car or available
+      if self.safety.get_vehicle_speed_min() > 0 or self.safety.get_vehicle_speed_max() > 0:
+        vehicle_speed_seen = True
+
+      if vehicle_speed_seen:
+        v_ego_raw = CS.vEgoRaw / self.CP.wheelSpeedFactor
+        checks['vEgoRaw'] += (v_ego_raw > (self.safety.get_vehicle_speed_max() + 1e-3) or
+                              v_ego_raw < (self.safety.get_vehicle_speed_min() - 1e-3))
+
+      # TODO: remove this exception once this mismatch is resolved
+      brake_pressed = CS.brakePressed
+      if CS.brakePressed and not self.safety.get_brake_pressed_prev():
+        if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05:
+          brake_pressed = False
+      checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev()
+      checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev()
+      checks['steeringDisengage'] += CS.steeringDisengage != self.safety.get_steering_disengage_prev()
+
+      if self.CP.pcmCruise:
+        # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
+        # On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
+        # openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
+        if self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH):
+          # only the rising edges are expected to match
+          if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
+            checks['controlsAllowed'] += not self.safety.get_controls_allowed()
+        else:
+          checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
+
+        # TODO: fix notCar mismatch
+        if not self.CP.notCar:
+          checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev()
+      else:
+        # Check for user button enable on rising edge of controls allowed
+        button_enable = CS.buttonEnable and (not CS.brakePressed or CS.standstill)
+        mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev)
+        checks['controlsAllowed'] += mismatch
+        controls_allowed_prev = self.safety.get_controls_allowed()
+        if button_enable and not mismatch:
+          self.safety.set_controls_allowed(False)
+
+      if self.CP.brand == "honda":
+        checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
+
+      CS_prev = CS
+
+    failed_checks = {k: v for k, v in checks.items() if v > 0}
+    self.assertFalse(len(failed_checks),
+                     f"panda safety doesn't agree with openpilot: {failed_checks}, standstill={standstill_mismatches}")
+
+
+class DashcamCarModelTestBase(CarModelTestBase):
+  __test__ = False
+  test_panda_safety_rx_checks = None
+  test_panda_safety_tx_cases = None
+  test_panda_safety_carstate_fuzzy = None
+  test_panda_safety_carstate = None
+
+  def test_car_params(self):
+    self.assertTrue(self.CP.dashcamOnly)
+
+
+for case_index, (case_platform, case_route) in enumerate(get_test_cases()):
+  case_name = f"TestCarModel_{case_index}_{case_platform}"
+  base = DashcamCarModelTestBase if case_platform in DASHCAM_ONLY_PLATFORMS else CarModelTestBase
+  globals()[case_name] = type(case_name, (base,), {
+    "__test__": True,
+    "platform": case_platform,
+    "test_route": case_route,
+  })
+
+
+if __name__ == "__main__":
+  unittest.main()

iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py

@@ -0,0 +1,121 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.cereal import car, custom
+from iqpilot.common.constants import CV
+from iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from iqpilot.selfdrive.car.cruise import VCruiseHelper
+
+
+class TestSpeedLimitSetSpeedMirror:
+  def setup_method(self):
+    self.CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+    self.CP_IQ = custom.IQCarParams(pcmCruiseSpeed=True)
+    self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_IQ)
+    self.v_cruise_helper.set_speed_to_limit = True
+
+  @staticmethod
+  def _iq_plan(limit_mps: float, state) -> SimpleNamespace:
+    resolver = SimpleNamespace(
+      speedLimitValid=limit_mps > 0,
+      speedLimitLastValid=limit_mps > 0,
+      speedLimitFinalLast=limit_mps,
+    )
+    assist = SimpleNamespace(state=state)
+    return SimpleNamespace(speedLimit=SimpleNamespace(resolver=resolver, assist=assist))
+
+  def test_op_long_mirrors_active_speed_limit_target_into_cluster_speed(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.active))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_syncs_to_new_limit_even_when_assist_not_active(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_allows_manual_set_speed_changes_between_limit_changes(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    # First cycle after a valid limit appears will sync to the resolved target.
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    # On later cycles with the same limit, manual set speed changes should be preserved.
+    CS = car.CarState(cruiseState={"available": True, "speed": 15.64, "speedCluster": 15.64})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(15.64 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(15.64 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_resyncs_when_limit_changes(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 15.64, "speedCluster": 15.64})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(13.41, custom.IQPlan.SpeedLimit.AssistState.inactive))
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1)
+
+
+def test_set_speed_does_not_follow_limit_when_feature_off():
+  # Default off: set speed must stay the driver's value (limiter-only via planner min-blend).
+  CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+  CP_IQ = custom.IQCarParams(pcmCruiseSpeed=True)
+  helper = VCruiseHelper(CP, CP_IQ)
+  helper.set_speed_to_limit = False
+  helper.update_speed_limit_assist(False, TestSpeedLimitSetSpeedMirror._iq_plan(
+    17.88, custom.IQPlan.SpeedLimit.AssistState.active))
+
+  CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+  helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+  # Set speed tracks the car's cruise speed, NOT the 17.88 m/s limit.
+  assert helper.v_cruise_kph == pytest.approx(22.35 * CV.MS_TO_KPH, abs=0.1)
+
+
+def test_enhanced_stock_longitudinal_control_syncs_once_then_follows_cluster_speed():
+  CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+  resolver = SimpleNamespace(speedLimitFinalLast=17.88)
+  assist = SimpleNamespace(enabled=True)
+  iq_plan = SimpleNamespace(speedLimit=SimpleNamespace(resolver=resolver, assist=assist))
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=100.0, previous_sync_limit_kph=None, pending_sync_limit_kph=None
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=22.0, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=17.88 * CV.MS_TO_KPH, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit is None
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=22.0, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(22.0, abs=0.1)

iqpilot/selfdrive/car/tests/test_tesla_fsd_visualization.py

@@ -0,0 +1,26 @@
+from iqdbc.car import structs
+from iqdbc.lvbs.car.interfaces import apply_iq_car_config
+from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ, TeslaSafetyFlagsIQ
+from iqpilot.selfdrive.car.interfaces import initialize_params
+
+
+class ParamStore:
+  def get(self, name, return_default=False):
+    return name == "IQTeslaFsdVisualization"
+
+
+class CarInterface:
+  def get_longitudinal_tuning_iq(self, CP, CP_IQ):
+    return None
+
+
+def test_fsd_visualization_is_snapshotted():
+  snapshot = initialize_params(ParamStore())
+  params = {key: value for item in snapshot for key, value in item.items()}
+  assert params["IQTeslaFsdVisualization"] is True
+
+  CP = structs.CarParams(brand="tesla")
+  CP_IQ = structs.IQCarParams()
+  apply_iq_car_config(CarInterface(), CP, CP_IQ, snapshot)
+  assert CP_IQ.flags & TeslaFlagsIQ.FSD_VISUALIZATION
+  assert CP_IQ.iqSafetyFlags & TeslaSafetyFlagsIQ.FSD_VISUALIZATION