From 5137636387c2723b3d79aea290dbe8f538044c23 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Thu, 23 Jul 2026 08:38:06 -0500 Subject: [PATCH] Model Stop Hysteresis --- starpilot/controls/lib/starpilot_vcruise.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index de73c8eb6..ea0bfe9ec 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -38,6 +38,7 @@ NAV_TURN_TARGET_SPEEDS = { FORCE_STOP_MODEL_APPROACH_DECEL = 0.65 FORCE_STOP_DASH_APPROACH_DECEL = 1.0 ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this +ACTIVATION_HYSTERESIS_M = 8.0 # m — release margin; absorbs model_length jitter at the gate MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap @@ -136,6 +137,7 @@ class StarPilotVCruise: self.override_force_stop_timer = 0 self.force_stop_timer = 0.0 + self.activation_gate_active = False self.standstill_force_stop_hold = False self.standstill_force_stop_clear_since = 0.0 self.standstill_force_stop_started_at = None @@ -307,9 +309,20 @@ class StarPilotVCruise: # Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path # in CEM can set stop_light_detected even with a lead present, which would incorrectly # activate Force Stop and stop the car far behind the lead instead of letting ACC handle it. - cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected + # + # Schmitt trigger: model_length can jitter several meters around ACTIVATION_M, which + # otherwise keeps resetting force_stop_timer's ramp. Scoped to a detected stop so the + # wider release threshold can't leak into ordinary slow driving. + stop_light_detected = self.starpilot_planner.starpilot_cem.stop_light_detected + if self.activation_gate_active and stop_light_detected: + model_length_active = self.starpilot_planner.model_length < ACTIVATION_M + ACTIVATION_HYSTERESIS_M + else: + model_length_active = self.starpilot_planner.model_length < ACTIVATION_M + self.activation_gate_active = model_length_active and stop_light_detected + + cem_path = (stop_light_detected and controls_enabled and starpilot_toggles.force_stops - and self.starpilot_planner.model_length < ACTIVATION_M + and model_length_active and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve and not curved_approach_scene