Longfixes

starpilot/controls/lib/conditional_experimental_mode.py

@@ -239,7 +239,12 @@ class ConditionalExperimentalMode:
       return
 
     slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead
-    stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1
+    stopped_lead = bool(
+      starpilot_toggles.conditional_stopped_lead and
+      lead_status and
+      lead_speed < 1 and
+      lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME)
+    )
     vision_slow_lead_candidate = bool(
       lead_status and
       lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and

starpilot/controls/lib/starpilot_vcruise.py

@@ -103,10 +103,15 @@ class StarPilotVCruise:
     if dash_path:
       self.stop_sign_confirmed = True
 
+    raw_model_stopped = bool(getattr(self.starpilot_planner, "raw_model_stopped", False))
+
     # Timer ramp. Faster commitment when the dashboard confirms.
     if force_stop_active and not sm["carState"].standstill:
       rate = DT_MDL * 2 if dash_active else DT_MDL
       self.force_stop_timer = min(self.force_stop_timer + rate, 2.0)
+    elif (self.forcing_stop and sm["carState"].standstill and not dash_active and
+          not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped):
+      self.force_stop_timer = 0.0
     else:
       self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0)
 

starpilot/controls/starpilot_planner.py

@@ -56,6 +56,7 @@ class StarPilotPlanner:
     self.gps_valid = False
     self.lateral_check = False
     self.model_stopped = False
+    self.raw_model_stopped = False
     self.road_curvature_detected = False
     self.tracking_lead = False
     self._prev_gps_bearing = 0
@@ -129,8 +130,8 @@ class StarPilotPlanner:
 
     self.model_length = sm["modelV2"].position.x[-1]
 
-    self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
-    self.model_stopped |= self.starpilot_vcruise.forcing_stop
+    self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+    self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop
 
     self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"], v_ego)