diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 9f90c4f7b..afabe58df 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -522,14 +522,14 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens(): def test_force_stop_reanchors_when_model_reopens_path_without_stop_action(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) - planner.model_length = 40.0 - vcruise.tracked_model_length = 10.0 + planner.model_length = 90.0 + vcruise.tracked_model_length = 60.0 sm = make_sm(standstill=False) sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5) - assert vcruise.tracked_model_length == pytest.approx(40.0) + assert vcruise.tracked_model_length == pytest.approx(90.0) assert result > 5.0 @@ -561,6 +561,18 @@ def test_santa_fe_force_stop_holds_through_low_speed_detector_dropout(): assert result == pytest.approx(0.0) +def test_force_stop_does_not_reanchor_inside_reanchor_floor(): + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 90.0 + vcruise.tracked_model_length = 25.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5) + + assert vcruise.tracked_model_length < 25.0 + + def test_force_stop_does_not_reanchor_committed_model_stop(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 40.0 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index b2664a081..0aa713a37 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -76,6 +76,7 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003 FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise +FORCE_STOP_REANCHOR_MIN_M = 40.0 # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -632,7 +633,7 @@ class StarPilotVCruise: model_wants_stop = False if ( not dash_active and - self.tracked_model_length > force_stop_handoff_m and + self.tracked_model_length > max(force_stop_handoff_m, FORCE_STOP_REANCHOR_MIN_M) and not model_wants_stop and model_length > self.tracked_model_length + FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP and (