diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 07daa98bef..5d4a779af6 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -122,6 +122,44 @@ def test_active_slc_control_target_does_not_require_set_speed_limit(): assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) +@pytest.mark.parametrize( + ("slc_target_mph", "slc_offset_mph", "expected_v_cruise_mph"), + [ + (30.0, 0.0, 30.0), + (25.0, 0.0, 25.0), + (24.0, 0.0, 24.0), + (20.0, 0.0, 20.0), + (15.0, 0.0, 15.0), + (0.0, 0.0, 35.0), + (20.0, 5.0, 25.0), + ], +) +def test_active_slc_target_constrains_vcruise_below_csc_minimum(slc_target_mph, slc_offset_mph, expected_v_cruise_mph): + _, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.speed_limit_controller = True + toggles.is_metric = False + for index in range(1, 8): + setattr(toggles, f"speed_limit_offset{index}", slc_offset_mph * CV.MPH_TO_MS) + + vcruise.slc.target = slc_target_mph * CV.MPH_TO_MS + vcruise.slc.source = "Dashboard" + vcruise.slc.update_limits = lambda *_args, **_kwargs: None + vcruise.slc.update_override = lambda *_args, **_kwargs: None + + result = update_vcruise( + vcruise, + sm, + toggles, + now=10.0, + v_ego=35.0 * CV.MPH_TO_MS, + v_cruise=35.0 * CV.MPH_TO_MS, + ) + + assert result == pytest.approx(expected_v_cruise_mph * CV.MPH_TO_MS) + + def test_elantra_gets_lead_veto_margin_before_force_stop(): assert get_lead_veto_distance(SimpleNamespace(carFingerprint="HYUNDAI_ELANTRA_2021")) == pytest.approx(90.0) assert get_lead_veto_distance(SimpleNamespace(carFingerprint="OTHER_CAR")) == pytest.approx(75.0) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index c8268ca117..2d46e1f2de 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -5,11 +5,12 @@ import math from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED +from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_ACTIVE_OFF_DELTA, CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA, + CSC_MIN_SPEED, CurveSpeedController, is_manual_speed_control, ) @@ -21,7 +22,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_force_stop_reanchor_speed_tolerance, ) -CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS OVERRIDE_FORCE_STOP_TIMER = 10 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75 # Open-loop — green is undetectable at standstill, so this only needs to cover the @@ -764,7 +764,7 @@ class StarPilotVCruise: getattr(self.slc, "source", "None"), ) self._applied_slc_control_target = slc_control_target if slc_control_target > 0.0 else 0.0 - if slc_control_target >= CSC_MIN_SPEED: + if slc_control_target > 0.0: targets.append(slc_control_target) if self.nav_turn_target > 0.0: targets.append(self.nav_turn_target)