diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 2f7461a6e..cfd24c1d0 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -50,6 +50,18 @@ def make_sm(*, standstill=True, min_steer_speed=0.0): } +def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, controls_enabled=True): + return vcruise.update( + controls_enabled=controls_enabled, + now=now, + time_validated=True, + v_cruise=20.0, + v_ego=v_ego, + sm=sm, + starpilot_toggles=toggles, + ) + + def make_toggles(): return SimpleNamespace( force_stops=True, @@ -163,6 +175,20 @@ def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold() assert vcruise.tracked_model_length == pytest.approx(0.0) +def test_engage_from_aol_while_stopped_at_red_light_seeds_force_stop_hold(): + _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False) + sm = make_sm(standstill=True) + toggles = make_toggles() + + assert update_vcruise(vcruise, sm, toggles, now=0.0, controls_enabled=False) == pytest.approx(20.0) + assert not vcruise.standstill_force_stop_hold + + assert update_vcruise(vcruise, sm, toggles, now=0.05, controls_enabled=True) == pytest.approx(0.0) + assert vcruise.standstill_force_stop_hold + assert vcruise.standstill_force_stop_reason == "light" + assert vcruise.forcing_stop + + def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release(): planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) sm = make_sm(standstill=True) @@ -212,7 +238,7 @@ def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) sm = make_sm(standstill=True) toggles = make_toggles() - base = datetime.datetime(2026, 6, 18, tzinfo=datetime.timezone.utc) + base = datetime.datetime(2026, 6, 18, tzinfo=datetime.UTC) first = vcruise.update( controls_enabled=True, @@ -253,6 +279,63 @@ def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing assert not vcruise.forcing_stop +def test_standstill_light_hold_expires_and_does_not_rearm_from_stopped_model(): + planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False) + sm = make_sm(standstill=True) + toggles = make_toggles() + + assert update_vcruise(vcruise, sm, toggles, now=0.0) == pytest.approx(0.0) + assert vcruise.standstill_force_stop_reason == "light" + + assert update_vcruise(vcruise, sm, toggles, now=4.9) == pytest.approx(0.0) + assert vcruise.forcing_stop + + assert update_vcruise(vcruise, sm, toggles, now=5.1) == pytest.approx(20.0) + assert not vcruise.forcing_stop + assert not vcruise.standstill_force_stop_hold + + # The red-light model remains stopped, but Force Stop must stay released so + # Experimental Mode can own the red-to-green departure. + assert update_vcruise(vcruise, sm, toggles, now=5.2) == pytest.approx(20.0) + assert not vcruise.forcing_stop + + +def test_approach_light_force_stop_expires_without_rearming_at_standstill(): + _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=True) + toggles = make_toggles() + + update_vcruise(vcruise, make_sm(standstill=False), toggles, now=0.0, v_ego=1.0) + sm = make_sm(standstill=True) + for frame in range(60): + result = update_vcruise(vcruise, sm, toggles, now=(frame + 1) * 0.05) + + assert result == pytest.approx(20.0) + assert not vcruise.forcing_stop + assert not vcruise.standstill_force_stop_hold + + assert update_vcruise(vcruise, sm, toggles, now=3.1) == pytest.approx(20.0) + assert not vcruise.forcing_stop + + +def test_stop_sign_hold_persists_until_resume(): + _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False) + sm = make_sm(standstill=True) + sm["starpilotCarState"].dashboardStopSign = 1 + toggles = make_toggles() + + assert update_vcruise(vcruise, sm, toggles, now=0.0) == pytest.approx(0.0) + assert vcruise.standstill_force_stop_reason == "sign" + + sm["starpilotCarState"].dashboardStopSign = 0 + assert update_vcruise(vcruise, sm, toggles, now=8.0) == pytest.approx(0.0) + assert vcruise.forcing_stop + + sm["starpilotCarState"].accelPressed = True + assert update_vcruise(vcruise, sm, toggles, now=8.1) == pytest.approx(20.0) + assert not vcruise.forcing_stop + assert not vcruise.stop_sign_confirmed + + def test_nav_turn_speed_control_default_off(): _, vcruise = make_vcruise(nav_state={ "valid": True, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index fa4245652..a1a6c0737 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -5,13 +5,14 @@ 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, PLANNER_TIME +from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedController from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS OVERRIDE_FORCE_STOP_TIMER = 10 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75 +STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0 NAV_TURN_COMFORT_DECEL = 1.25 NAV_TURN_DISTANCE_BUFFER = 8.0 NAV_TURN_MIN_TARGET_DELTA = 0.25 @@ -67,6 +68,9 @@ class StarPilotVCruise: self.force_stop_timer = 0.0 self.standstill_force_stop_hold = False self.standstill_force_stop_clear_since = 0.0 + self.standstill_force_stop_started_at = None + self.standstill_force_stop_reason = None + self.controls_enabled_previously = False # Kinematic distance estimator. Same attribute also published as # starpilotPlan.forcingStopLength, so the existing reader keeps working. self.tracked_model_length = 0.0 @@ -105,6 +109,12 @@ class StarPilotVCruise: delta = now - since return delta.total_seconds() if hasattr(delta, "total_seconds") else float(delta) + def _clear_standstill_force_stop_hold(self): + self.standstill_force_stop_hold = False + self.standstill_force_stop_clear_since = 0.0 + self.standstill_force_stop_started_at = None + self.standstill_force_stop_reason = None + @staticmethod def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier): maneuver_type = str(maneuver_type or "").strip().lower() @@ -222,33 +232,44 @@ class StarPilotVCruise: raw_model_stopped = bool(getattr(self.starpilot_planner, "raw_model_stopped", False)) standstill_force_stop_scene_active = bool(force_stop_active or raw_model_stopped) + standstill = bool(sm["carState"].standstill) + engaged_at_standstill = controls_enabled and not self.controls_enabled_previously and standstill - # If the driver engages while already stopped at a red light / stop sign, seed - # the same stop-hold path openpilot would have had if it made the stop itself. - # Without this, a brief model-clear dropout can release the stop immediately. - if ( - controls_enabled and - sm["carState"].standstill and - standstill_force_stop_scene_active and - not self.forcing_stop and - self.force_stop_timer < 0.5 - ): + # Stop signs remain latched until the driver resumes. A light hold is only + # seeded on the engagement edge; otherwise the expired Force Stop would + # immediately re-arm itself from the still-short model trajectory. + stop_sign_hold_requested = controls_enabled and standstill and self.stop_sign_confirmed + light_hold_requested = engaged_at_standstill and standstill_force_stop_scene_active and not self.stop_sign_confirmed + if stop_sign_hold_requested and self.standstill_force_stop_reason != "sign": self.standstill_force_stop_hold = True self.standstill_force_stop_clear_since = 0.0 + self.standstill_force_stop_started_at = now + self.standstill_force_stop_reason = "sign" + self.tracked_model_length = 0.0 + elif light_hold_requested and not self.standstill_force_stop_hold: + self.standstill_force_stop_hold = True + self.standstill_force_stop_clear_since = 0.0 + self.standstill_force_stop_started_at = now + self.standstill_force_stop_reason = "light" self.tracked_model_length = 0.0 if self.standstill_force_stop_hold: pedal_override = bool(sm["carState"].gasPressed or sm["starpilotCarState"].accelPressed) - if (not controls_enabled) or (not sm["carState"].standstill) or lead_present or pedal_override: - self.standstill_force_stop_hold = False - self.standstill_force_stop_clear_since = 0.0 + light_hold_expired = ( + self.standstill_force_stop_reason == "light" and + self.standstill_force_stop_started_at is not None and + self._elapsed_seconds(now, self.standstill_force_stop_started_at) >= STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + ) + if pedal_override: + self.override_force_stop_timer = OVERRIDE_FORCE_STOP_TIMER + if (not controls_enabled) or (not standstill) or lead_present or pedal_override or light_hold_expired: + self._clear_standstill_force_stop_hold() elif standstill_force_stop_scene_active: self.standstill_force_stop_clear_since = 0.0 elif self.standstill_force_stop_clear_since == 0.0: self.standstill_force_stop_clear_since = now elif self._elapsed_seconds(now, self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME: - self.standstill_force_stop_hold = False - self.standstill_force_stop_clear_since = 0.0 + self._clear_standstill_force_stop_hold() # Timer ramp. Faster commitment when the dashboard confirms. if force_stop_active and not sm["carState"].standstill: @@ -364,8 +385,7 @@ class StarPilotVCruise: else: self.forcing_stop = False - self.standstill_force_stop_hold = False - self.standstill_force_stop_clear_since = 0.0 + self._clear_standstill_force_stop_hold() # Latch is only meaningful during an active force-stop cycle self.stop_sign_confirmed = False @@ -388,4 +408,5 @@ class StarPilotVCruise: targets.append(self.nav_turn_target) v_cruise = min(targets) + self.controls_enabled_previously = controls_enabled return v_cruise