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) @pytest.mark.parametrize("pcm_cruise", [False, True]) @pytest.mark.parametrize("is_metric", [False, True]) def test_driver_increase_marker_survives_button_release(pcm_cruise, is_metric): helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True)) helper.set_speed_to_limit = False helper.v_cruise_kph = helper.v_cruise_cluster_kph = 80.0 state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS}) helper.update_v_cruise(state, True, is_metric) state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)] helper.update_v_cruise(state, True, is_metric) assert helper.slc_set_speed_request_id == 0 state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)] if pcm_cruise: state.cruiseState.speed = state.cruiseState.speedCluster = 81 * CV.KPH_TO_MS helper.update_v_cruise(state, True, is_metric) request = custom.IQCarState.new_message( slcSetSpeedRequestId=helper.slc_set_speed_request_id, slcSetSpeedGestureId=helper.slc_set_speed_gesture_id, slcSetSpeedRequestKph=helper.slc_set_speed_request_kph, ) assert request.slcSetSpeedRequestId == 1 assert request.slcSetSpeedRequestKph == pytest.approx(helper.v_cruise_kph) state.buttonEvents = [] for _ in range(100): helper.update_v_cruise(state, True, is_metric) assert helper.slc_set_speed_request_id == request.slcSetSpeedRequestId assert helper.slc_set_speed_gesture_id == request.slcSetSpeedGestureId def test_stock_cruise_sync_is_not_a_driver_increase(): helper = VCruiseHelper(car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True), custom.IQCarParams(pcmCruiseSpeed=True)) helper.set_speed_to_limit = True helper.update_speed_limit_assist(True, TestSpeedLimitSetSpeedMirror._iq_plan( 50 * CV.KPH_TO_MS, custom.IQPlan.SpeedLimit.AssistState.active)) state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS}) for _ in range(10): helper.update_v_cruise(state, True, True) assert helper.slc_set_speed_request_id == 0 state.cruiseState.speed = state.cruiseState.speedCluster = 90 * CV.KPH_TO_MS helper.update_v_cruise(state, True, True) assert helper.slc_set_speed_request_id == 0 def test_held_increase_uses_one_gesture_and_multiple_requests(): helper = VCruiseHelper(car.CarParams(pcmCruise=False), custom.IQCarParams(pcmCruiseSpeed=True)) helper.set_speed_to_limit = False helper.v_cruise_kph = 80.0 state = car.CarState(cruiseState={"available": True}) state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)] helper.update_v_cruise(state, True, True) state.buttonEvents = [] for _ in range(110): helper.update_v_cruise(state, True, True) assert helper.slc_set_speed_gesture_id == 1 assert helper.slc_set_speed_request_id >= 2 assert helper.slc_set_speed_request_kph == pytest.approx(helper.v_cruise_kph) 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)