Force Stop Tweaks

cereal/custom.capnp

@@ -224,6 +224,16 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
 struct StarPilotRadarState @0xb86e6369214c01c8 {
   leadLeft @0 :LeadData;
   leadRight @1 :LeadData;
+  adjacentStopped @2 :AdjacentStopped;
+
+  # A vehicle in an adjacent lane that was observed MOVING and then came to rest.
+  # Distinct from leadLeft/leadRight, which are moving-target-only by design.
+  struct AdjacentStopped {
+    status @0 :Bool;
+    dRel @1 :Float32;
+    yRel @2 :Float32;
+    radarTrackId @3 :Int32 = -1;
+  }
 
   struct LeadData {
     dRel @0 :Float32;

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -835,7 +835,8 @@ class LongitudinalMpc:
 
   def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
              personality=log.LongitudinalPersonality.standard, tracking_lead=True,
-             optional_far_lead_comfort=True, smooth_duplicate_vision=False):
+             optional_far_lead_comfort=True, smooth_duplicate_vision=False,
+             stop_x=None):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
@@ -890,6 +891,11 @@ class LongitudinalMpc:
         lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego)
         lead_0_obstacle = lead_0_obstacle + lead_0_bias
         lead_1_obstacle = lead_1_obstacle + lead_1_bias
+      # A forced stop is a position constraint, not a speed one. Folded in here rather than
+      # as a 4th column so the SOURCES[argmin] below keeps working. Lets the solver plan the
+      # stop directly instead of chasing a descending speed ceiling with a persistent lag.
+      if stop_x is not None:
+        cruise_obstacle = np.minimum(cruise_obstacle, stop_x)
       x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
       candidate_source = SOURCES[np.argmin(x_obstacles[0])]
       sticky_source = None
@@ -939,6 +945,8 @@ class LongitudinalMpc:
       xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
       x = np.cumsum(np.insert(xforward, 0, x[0]))
 
+      if stop_x is not None:
+        cruise_target = np.minimum(cruise_target, stop_x)
       x_and_cruise = np.column_stack([x, cruise_target])
       x = np.min(x_and_cruise, axis=1)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -3095,11 +3095,20 @@ class LongitudinalPlanner:
     dec_mpc_mode = self.get_mpc_mode()
     if not self.mlsim:
       self.mpc.mode = dec_mpc_mode
+    # Hand the forced stop to the solver as a position. The obstacle sits STOP_DISTANCE
+    # beyond the line because the safe-distance term already includes it — placing it on
+    # the line parks us short. Below that the existing v_cruise=0 path finishes the stop,
+    # since forcingStopLength is decaying to zero and the obstacle would land behind us.
+    force_stop_x = None
+    if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > STOP_DISTANCE:
+      force_stop_x = float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE
+
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
                     sm['starpilotPlan'].dangerFactor, effective_t_follow,
                     personality=personality, tracking_lead=lead_control_active,
                     optional_far_lead_comfort=True,
-                    smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass)
+                    smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
+                    stop_x=force_stop_x)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/controls/radard.py

@@ -30,6 +30,18 @@ RADAR_TO_CAMERA = 1.52  # RADAR is ~ 1.5m ahead from center of mesh frame
 G90_RADAR_LOW_SPEED_MAX_DIST = 12.0
 G90_RADAR_LOW_SPEED_MAX_Y = 0.6
 
+# Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light
+# approaches. The qualifier is the DECELERATION HISTORY, not the current speed: roadside
+# furniture and curb-parked cars never show a moving -> stopped transition, so testing
+# for "anything slow in the next lane" instead would brake us early for parked cars.
+ADJACENT_STOP_MOVING_V = 5.0      # m/s — must have genuinely been moving
+ADJACENT_STOP_REST_V = 1.5        # m/s — and then genuinely at rest
+ADJACENT_STOP_MOVING_FRAMES = 15  # 0.75 s at 20 Hz, both ways: rejects speed noise
+ADJACENT_STOP_REST_FRAMES = 15
+ADJACENT_STOP_MIN_Y = 1.8         # m — inside this is our own lane
+ADJACENT_STOP_MAX_Y = 7.5         # m — beyond this is roadside, not an adjacent lane
+ADJACENT_STOP_MAX_D = 110.0       # m
+
 
 class KalmanParams:
   def __init__(self, dt: float):
@@ -62,6 +74,11 @@ class Track:
 
     self.leadTrackID = 0
 
+    # deceleration history for the adjacent-lane stopped-vehicle detector
+    self.moving_frames = 0
+    self.rest_frames = 0
+    self.seen_moving = False
+
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
     self.dRel = d_rel   # LONG_DIST
@@ -83,6 +100,21 @@ class Track:
     else:
       self.aLeadTau.update(0.0)
 
+    # Track the moving -> stopped transition. Only sustained runs count, so one noisy
+    # speed sample can neither arm nor trip the detector.
+    if self.vLead > ADJACENT_STOP_MOVING_V:
+      self.moving_frames += 1
+      self.rest_frames = 0
+      if self.moving_frames >= ADJACENT_STOP_MOVING_FRAMES:
+        self.seen_moving = True
+    elif abs(self.vLead) < ADJACENT_STOP_REST_V:
+      self.moving_frames = 0
+      self.rest_frames += 1
+    else:
+      # coasting between the two bands: hold state, restart both runs
+      self.moving_frames = 0
+      self.rest_frames = 0
+
     self.cnt += 1
 
   def get_RadarState(self, model_prob: float = 0.0):
@@ -111,6 +143,31 @@ class Track:
     right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
     return -self.yRel > right_lane
 
+  def is_adjacent_stopped(self, model_data: capnp._DynamicStructReader):
+    """A neighbouring-lane vehicle that was seen moving and has now come to rest.
+
+    Deliberately not potential_adjacent_lead, which is moving-target-only and would have
+    to be loosened to "anything slow" to catch these. Lane geometry mirrors it (model
+    y == -yRel, laneLines[1] left boundary and [2] right), plus an outer bound so
+    roadside returns past the neighbouring lane don't qualify.
+    """
+    if not (self.seen_moving and self.rest_frames >= ADJACENT_STOP_REST_FRAMES):
+      return False
+
+    if self.leadTrackID == self.identifier:
+      return False
+
+    if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
+      return False
+
+    if not (0.0 < self.dRel < ADJACENT_STOP_MAX_D):
+      return False
+
+    model_y = -self.yRel
+    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+    return bool(model_y < left_lane or model_y > right_lane)
+
   def potential_low_speed_lead(self, v_ego: float):
     # stop for stuff in front of you and low speed, even without model confirmation
     # Radar points closer than 0.75, are almost always glitches on toyota radars
@@ -270,6 +327,29 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
   return lead_dict
 
 
+def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
+  """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
+
+  Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the
+  rest are closer to us, so the nearest one underestimates the distance. The consumer only
+  shortens with this, so underestimating is the harmful direction.
+  """
+  if len(model_data.laneLines) < 4:
+    return {'status': False}
+
+  candidates = [c for c in tracks.values() if c.is_adjacent_stopped(model_data)]
+  if not candidates:
+    return {'status': False}
+
+  furthest = max(candidates, key=lambda c: c.dRel)
+  return {
+    'status': True,
+    'dRel': float(furthest.dRel),
+    'yRel': float(furthest.yRel),
+    'radarTrackId': int(furthest.identifier),
+  }
+
+
 class RadarD:
   def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False):
     self.current_time = 0.0
@@ -360,6 +440,12 @@ class RadarD:
       self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
       self.starpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=False)
 
+    # Not gated on the adjacent-lead toggles: this is a separate signal with a separate
+    # consumer (Force Stop), and leaving leadLeft/leadRight untouched keeps existing
+    # lane-change and UI behaviour unchanged.
+    if self.ready:
+      self.starpilot_radar_state.adjacentStopped = get_adjacent_stopped(self.tracks, sm['modelV2'])
+
     self.starpilot_toggles = get_starpilot_toggles(sm)
 
   def publish(self, pm: messaging.PubMaster):

starpilot/controls/lib/starpilot_vcruise.py

@@ -37,12 +37,24 @@ NAV_TURN_TARGET_SPEEDS = {
 # Smaller values pull speed down earlier on approach.
 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_M = 75.0       # m — CEM/model path activates when model_length < this.
+                          # Don't raise: forcing_stop latches until standstill, so a brief
+                          # red-light blip at longer range commits to a stop we can't release.
+ACTIVATION_HYSTERESIS_M = 8.0  # m — release margin; absorbs model_length jitter at the gate
+LEAD_VETO_M = 75.0        # m — lead proximity that vetoes Force Stop (kept off ACTIVATION_M
+                          # so raising activation can't silently widen the veto)
 MPC_HANDOFF_M = 6.0       # m — below this, command 0 and let MPC finish the stop
+FORCE_STOP_APPROACH_DECEL = 0.75  # m/s^2 — speed ceiling before commit. LOWER = more early
+                          # braking. Must stay above FORCE_STOP_MODEL_APPROACH_DECEL or the
+                          # pre-commit ceiling is stricter than the stop itself.
 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
                           # tracked length closer when dashboard confirms a sign
+DASH_MODEL_AGREE_M = 50.0 # m — dash arm/snap needs model_length under this; a lone dash bit
+                          # against a long model path is a phantom stop
 FT_TO_M = 0.3048
+ADJACENT_STOP_MIN_USE_M = 10.0  # m — inside this the MPC already owns the stop; a
+                                # late-arriving hint could only jerk it
 FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS
 # Real-turn steering angle. A stop-then-turn is still ~straight on approach, so a low
 # threshold caused legit stops to be skipped when the blinker came on early. Only suppress
@@ -136,6 +148,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
@@ -190,6 +203,26 @@ class StarPilotVCruise:
     self.standstill_force_stop_started_at = None
     self.standstill_force_stop_reason = None
 
+  @staticmethod
+  def _get_adjacent_stop_distance(sm):
+    """dRel of a vehicle that decelerated to a stop in an adjacent lane, or None.
+
+    The model's own distance runs long on a clear-lane approach; a car stopped alongside
+    is physically at (or just behind) the stop bar. Radar-only, so it holds for any
+    driving model.
+    """
+    try:
+      radar_state = sm["starpilotRadarState"]
+    except (KeyError, IndexError, TypeError, AttributeError):
+      return None
+
+    adjacent = getattr(radar_state, "adjacentStopped", None)
+    if adjacent is None or not getattr(adjacent, "status", False):
+      return None
+
+    d_rel = float(getattr(adjacent, "dRel", 0.0))
+    return d_rel if d_rel > ADJACENT_STOP_MIN_USE_M else None
+
   @staticmethod
   def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier):
     maneuver_type = str(maneuver_type or "").strip().lower()
@@ -296,7 +329,7 @@ class StarPilotVCruise:
     # during the filter's settling window and stay committed for the whole stop.
     lead = self.starpilot_planner.lead_one
     lead_present = (bool(getattr(lead, "status", False))
-                    and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M
+                    and float(getattr(lead, "dRel", float("inf"))) < LEAD_VETO_M
                     and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0)
     curved_approach_scene = (
       abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
@@ -307,9 +340,19 @@ 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 jitters around ACTIVATION_M and 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
@@ -323,6 +366,7 @@ class StarPilotVCruise:
     dash_active = dash_value > 0
     dash_path = (dash_active and controls_enabled and starpilot_toggles.force_stops
                  and v_ego < ADAS_MAX_MS
+                 and self.starpilot_planner.model_length < DASH_MODEL_AGREE_M
                  and self.override_force_stop_timer <= 0
                  and not self.starpilot_planner.driving_in_curve
                  and not turn_scene_active
@@ -488,11 +532,22 @@ class StarPilotVCruise:
         # Kinematic distance estimator (also published as forcingStopLength).
         # Decay one-to-one with motion, clamp by current model_length so we adopt
         # the model's view when it regains sight, and snap closer to DASH_SEED_M
-        # whenever the dashboard signal is active.
+        # when the dashboard signal is active and the model agrees a stop is near.
         self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
         self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length)
         if dash_active:
-          self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
+          if self.starpilot_planner.model_length < DASH_MODEL_AGREE_M:
+            self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
+          # inside the seed the model range is the better line estimate; letting it pull
+          # tracked back up is what keeps an early snap from parking us short of the sign
+          if self.starpilot_planner.model_length < DASH_SEED_M:
+            self.tracked_model_length = self.starpilot_planner.model_length
+
+        # A car stopped in the next lane marks the stop bar better than the model does.
+        # Shortening clamp only — it can pull the stop in, never push it out.
+        adjacent_stop_d = self._get_adjacent_stop_distance(sm)
+        if adjacent_stop_d is not None:
+          self.tracked_model_length = min(self.tracked_model_length, adjacent_stop_d)
 
         # Kinematic profile with user offset. Positive offset shifts the perceived
         # line further down the road -> car rolls further before commanding 0.
@@ -538,6 +593,27 @@ class StarPilotVCruise:
         targets.append(slc_control_target)
       if self.nav_turn_target > 0.0:
         targets.append(self.nav_turn_target)
+
+      # Far-approach envelope: bleed speed off before commit so the car isn't still at
+      # cruise when the kinematic curve takes over. Same vetoes as the activation paths;
+      # no latch, recomputed each frame, releases on green.
+      if (stop_light_detected
+          and controls_enabled and starpilot_toggles.force_stops
+          and self.override_force_stop_timer <= 0
+          and not self.starpilot_planner.driving_in_curve
+          and not curved_approach_scene
+          and not turn_scene_active
+          and not self.starpilot_planner.tracking_lead
+          and not lead_present):
+        # adjacent-stopped hint caps the model distance; shorten-only, self-clearing
+        approach_d = self.starpilot_planner.model_length
+        adjacent_stop_d = self._get_adjacent_stop_distance(sm)
+        if adjacent_stop_d is not None:
+          approach_d = min(approach_d, adjacent_stop_d)
+        approach_d += offset_m
+        if approach_d > MPC_HANDOFF_M:
+          targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - MPC_HANDOFF_M)))
+
       v_cruise = min(targets)
 
     self.controls_enabled_previously = controls_enabled

starpilot/controls/starpilot_planner.py

@@ -30,6 +30,7 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
 from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
 
 RADARLESS_TRACK_HOLD_TIME = 0.45
+FORCE_STOP_JERK_SCALE = 0.32  # accel-change cost multiplier while forcing_stop (125 -> ~40)
 
 
 def _sanitize_json_value(value):
@@ -282,7 +283,10 @@ class StarPilotPlanner:
     starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])
     starpilotPlan = starpilot_plan_send.starpilotPlan
 
-    starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk)
+    # While committed to a Force Stop, cut the MPC's accel-change penalty so terminal
+    # braking can ramp faster. 0.32 lands near 40, what long_mpc uses in blended mode.
+    jerk_scale = FORCE_STOP_JERK_SCALE if self.starpilot_vcruise.forcing_stop else 1.0
+    starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk * jerk_scale)
     starpilotPlan.dangerFactor = float(self.starpilot_following.danger_factor)
     starpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.starpilot_following.danger_jerk)
     starpilotPlan.speedJerk = float(J_EGO_COST * self.starpilot_following.speed_jerk)