IQ.Pilot Release Commit @ 0798119

iqpilot/selfdrive/car/tests/test_longitudinal_mode_persistence.py

@@ -0,0 +1,38 @@
+from cereal import custom
+from iqdbc.car import structs
+
+from openpilot.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_speed_limit_set_speed.py

@@ -0,0 +1,121 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import car, custom
+from openpilot.common.constants import CV
+from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from openpilot.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)