Model Stop Hysteresis

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