Force Stop Tweaks

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -844,7 +844,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
@@ -899,6 +900,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
@@ -948,6 +954,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):