181 lines
9.4 KiB
Python
181 lines
9.4 KiB
Python
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)
|