milky time

scripts/speed_limit_vision/replay_route_runtime.py

@@ -61,10 +61,20 @@ class QlogRuntimeContext:
 
 
 class RouteReplayDaemon(slv.SpeedLimitVisionDaemon):
-  def __init__(self, runtime_context: QlogRuntimeContext | None, measured_inference_seconds: float):
+  def __init__(
+    self,
+    runtime_context: QlogRuntimeContext | None,
+    measured_inference_seconds: float,
+    measured_base_inference_seconds: float | None = None,
+    measured_classifier_forward_seconds: float = 0.0,
+  ):
     super().__init__(use_runtime=False)
     self.runtime_context = runtime_context
     self.measured_inference_seconds = max(float(measured_inference_seconds), 0.0)
+    self.measured_base_inference_seconds = (
+      max(float(measured_base_inference_seconds), 0.0) if measured_base_inference_seconds is not None else None
+    )
+    self.measured_classifier_forward_seconds = max(float(measured_classifier_forward_seconds), 0.0)
     self.next_available_at = -float("inf")
     self.now = 0.0
     self.sampled_frames = 0
@@ -131,9 +141,19 @@ class RouteReplayDaemon(slv.SpeedLimitVisionDaemon):
       return
 
     self.last_inference_at = now
-    self.next_available_at = now + self.measured_inference_seconds
     self.inference_frames += 1
+    self.last_detector_forward_count = 0
+    self.last_detector_forward_duration_s = 0.0
+    self.last_classifier_forward_count = 0
+    self.last_classifier_forward_duration_s = 0.0
     detection = self._detect_sign(frame_bgr)
+    inference_seconds = self.measured_inference_seconds
+    if self.measured_base_inference_seconds is not None:
+      inference_seconds = (
+        self.measured_base_inference_seconds +
+        self.last_classifier_forward_count * self.measured_classifier_forward_seconds
+      )
+    self.next_available_at = now + inference_seconds
     if detection is not None:
       self._update_detection(detection)
     elif self.published_speed_limit_mph > 0 and self._published_detection_stale(now):
@@ -153,6 +173,17 @@ def parse_args() -> argparse.Namespace:
   parser.add_argument("--fast-seek", action="store_true", help="Use VideoCapture seeks when skipping frames. Faster, but less faithful for HEVC.")
   parser.add_argument("--qlog-context", action="store_true", help="Replay with logged deviceState/livePose/mapdOut context for closer runtime cadence.")
   parser.add_argument("--measured-inference-seconds", type=float, default=0.0, help="Simulate wall-clock time spent inside one runtime inference on the comma.")
+  parser.add_argument(
+    "--measured-base-inference-seconds",
+    type=float,
+    help="Simulate a measured no-proposal inference cost; enables the dynamic comma cost model.",
+  )
+  parser.add_argument(
+    "--measured-classifier-forward-seconds",
+    type=float,
+    default=0.0,
+    help="Additional measured comma cost per classifier forward when the dynamic cost model is enabled.",
+  )
   parser.add_argument(
     "--detector-region-mode",
     choices=("full", "right_roi", "full_and_right_roi"),
@@ -314,8 +345,15 @@ def replay_route(
   progress: bool,
   fast_seek: bool,
   measured_inference_seconds: float,
+  measured_base_inference_seconds: float | None = None,
+  measured_classifier_forward_seconds: float = 0.0,
 ) -> tuple[RouteSummary, list[dict[str, str]]]:
-  daemon = RouteReplayDaemon(runtime_context, measured_inference_seconds)
+  daemon = RouteReplayDaemon(
+    runtime_context,
+    measured_inference_seconds,
+    measured_base_inference_seconds,
+    measured_classifier_forward_seconds,
+  )
   for segment_path in segments:
     segment = segment_index(segment_path)
     capture = cv2.VideoCapture(str(segment_path))
@@ -450,15 +488,20 @@ def main() -> int:
       args.progress,
       args.fast_seek,
       args.measured_inference_seconds,
+      args.measured_base_inference_seconds,
+      args.measured_classifier_forward_seconds,
     )
     all_events.extend((log_id, event) for event in events)
-    print(
-      f"{summary.route}: segments={summary.segments} qlog_context={int(summary.qlog_context)} sampled={summary.sampled_frames} "
-      f"inference={summary.inference_frames} candidate={summary.candidate_events} "
-      f"publish={summary.publish_events} stale_clear={summary.stale_clear_events} road_change={summary.road_change_events} "
-      f"measured_inference_s={args.measured_inference_seconds:.3f} region={slv.DETECTOR_CLASSIFIER_REGION_MODE}",
-      flush=True,
-    )
+    summary_line = "".join((
+      f"{summary.route}: segments={summary.segments} qlog_context={int(summary.qlog_context)} sampled={summary.sampled_frames} ",
+      f"inference={summary.inference_frames} candidate={summary.candidate_events} ",
+      f"publish={summary.publish_events} stale_clear={summary.stale_clear_events} road_change={summary.road_change_events} ",
+      f"measured_inference_s={args.measured_inference_seconds:.3f} ",
+      f"measured_base_s={args.measured_base_inference_seconds if args.measured_base_inference_seconds is not None else 'off'} ",
+      f"measured_classifier_forward_s={args.measured_classifier_forward_seconds:.3f} ",
+      f"region={slv.DETECTOR_CLASSIFIER_REGION_MODE}",
+    ))
+    print(summary_line, flush=True)
     publish_values = [event.get("speedLimitMph") for event in events if event["event"] == "publish"]
     if publish_values:
       print(f"  publishes: {', '.join(publish_values)}", flush=True)

selfdrive/controls/lib/longcontrol.py

@@ -323,7 +323,8 @@ class LongControl:
       return
     if self.last_output_accel <= 0.10:
       return
-    if a_target > 0.03:
+    light_accel_threshold = float(interp(CS.vEgo, [8.0, 15.0, 25.0], [0.03, 0.06, 0.10]))
+    if a_target > light_accel_threshold:
       return
     if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL:
       return
@@ -336,7 +337,7 @@ class LongControl:
     if authority_mismatch <= 0.08 and error > -0.08:
       return
 
-    target_factor = float(interp(a_target, [-0.30, -0.10, -0.02, 0.03], [0.20, 0.35, 0.60, 0.85]))
+    target_factor = float(interp(a_target, [-0.30, -0.10, -0.02, light_accel_threshold], [0.20, 0.35, 0.60, 0.98]))
     if error < -0.20:
       target_factor *= 0.75
     self.pid.i *= target_factor

selfdrive/controls/lib/longitudinal_planner.py

@@ -231,17 +231,9 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0
 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5
 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0
 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15
-RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA = 0.10
-RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL = 0.12
-RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN = 0.18
 RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75
 RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04
 RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75
-POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED = 12.0
-POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95
-POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35
-POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL = 0.25
-POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN = 0.10
 POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME = 75.0
 POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED = 8.0
 POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB = 0.9
@@ -250,16 +242,14 @@ POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED = 0.8
 POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE = 0.10
 POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN = 0.10
 POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN = 0.05
-COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB = 0.95
-COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA = -0.05
-COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL = 0.20
-COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN = 0.20
-SPACIOUS_TRACKED_FOLLOW_MIN_MODEL_PROB = 0.98
-SPACIOUS_TRACKED_FOLLOW_MIN_HEADWAY_MARGIN = 0.45
-SPACIOUS_TRACKED_FOLLOW_MAX_CLOSING_SPEED = 0.60
-SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE = 0.10
-SPACIOUS_TRACKED_FOLLOW_LATCH_TIME = 1.25
-SPACIOUS_TRACKED_FOLLOW_LATCH_MIN_LEAD_DELTA = 0.90
+FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED = 12.0
+FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN = 0.10
+FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN = 0.90
+FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN = 4.0
+FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED = 0.20
+FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED = 1.50
+FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE = 0.35
+FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL = 0.55
 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0
 LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85
 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20
@@ -286,13 +276,7 @@ CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9
 CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15
 CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5
 CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25
-CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET = 0.95
-CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED = 0.8
-CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE = 0.10
 CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18
-CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN = 0.25
-CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA = 0.35
-CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN = 1.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED = 12.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED = 22.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB = 0.9
@@ -618,7 +602,6 @@ class LongitudinalPlanner:
     self.manual_stop_resume_override_until = 0.0
     self.lead_depart_accel_hold_until = 0.0
     self.lead_depart_accel_hold_floor = None
-    self.spacious_follow_cap_bypass_until = 0.0
     self.post_departure_follow_settle_until = 0.0
     self.duplicate_vision_comfort_lead_source = None
 
@@ -1416,50 +1399,6 @@ class LongitudinalPlanner:
     brake_floor = -hold_brake
     return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor)
 
-  def is_stable_post_departure_pullaway(self, lead, v_ego, t_follow):
-    if lead is None or not lead.status or float(v_ego) < POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED:
-      return False
-
-    lead_radar = bool(getattr(lead, "radar", False))
-    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
-    if not lead_radar and lead_prob < POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB:
-      return False
-
-    if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
-      return False
-
-    lead_delta = float(lead.vLead) - float(v_ego)
-    lead_accel = float(getattr(lead, "aLeadK", 0.0))
-    if (lead_delta < POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA or
-        lead_accel < POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL):
-      return False
-
-    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
-    headway_margin = actual_headway - float(t_follow)
-    return headway_margin >= POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN
-
-  def is_comfortable_accelerating_away_follow(self, lead, v_ego, t_follow):
-    if lead is None or not lead.status or float(v_ego) < POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED:
-      return False
-
-    lead_radar = bool(getattr(lead, "radar", False))
-    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
-    if not lead_radar and lead_prob < COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB:
-      return False
-
-    if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
-      return False
-
-    lead_delta = float(lead.vLead) - float(v_ego)
-    lead_accel = float(getattr(lead, "aLeadK", 0.0))
-    if (lead_delta < COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA or
-        lead_accel < COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL):
-      return False
-
-    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
-    headway_margin = actual_headway - float(t_follow)
-    return headway_margin >= COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN
-
   def post_departure_follow_settle_active(self, lead, v_ego, t_follow):
     if lead is None or not lead.status:
       return False
@@ -1495,37 +1434,39 @@ class LongitudinalPlanner:
 
     return headway_margin >= POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN
 
-  def is_spacious_low_closure_follow(self, lead, v_ego, t_follow):
-    if lead is None or not lead.status or float(v_ego) < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED:
-      return False
-
-    lead_radar = bool(getattr(lead, "radar", False))
-    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
-    if not lead_radar and lead_prob < SPACIOUS_TRACKED_FOLLOW_MIN_MODEL_PROB:
-      return False
-
-    if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
-      return False
-
-    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
-    if lead_brake > SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE:
-      return False
+  @staticmethod
+  def get_follow_accel_cap_allowance(lead, v_ego, t_follow):
+    if lead is None or not lead.status or float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED:
+      return 0.0
 
     closing_speed = max(float(v_ego) - float(lead.vLead), 0.0)
-    if closing_speed > SPACIOUS_TRACKED_FOLLOW_MAX_CLOSING_SPEED:
-      return False
-
-    if self.raw_close_lead_needs_control(lead, v_ego):
-      return False
-
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
     actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
     headway_margin = actual_headway - float(t_follow)
-    return headway_margin >= SPACIOUS_TRACKED_FOLLOW_MIN_HEADWAY_MARGIN
+    if (headway_margin <= FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN or
+        closing_speed >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED or
+        lead_brake >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE):
+      return 0.0
 
-  def spacious_follow_cap_bypass_active(self, lead, v_ego, t_follow, tracking_lead_active):
-    if not tracking_lead_active or time.monotonic() > self.spacious_follow_cap_bypass_until:
-      return False
-    return self.is_spacious_low_closure_follow(lead, v_ego, t_follow)
+    headway_factor = float(np.clip(
+      (headway_margin - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN) /
+      (FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN),
+      0.0,
+      1.0,
+    ))
+    closing_factor = float(np.clip(
+      (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - closing_speed) /
+      (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED),
+      0.0,
+      1.0,
+    ))
+    brake_factor = float(np.clip(
+      (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE - lead_brake) /
+      FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE,
+      0.0,
+      1.0,
+    ))
+    return FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL * headway_factor * closing_factor * brake_factor
 
   def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow, current_source=None, tracking_lead_active=False):
     if lead is None or not lead.status:
@@ -1563,20 +1504,9 @@ class LongitudinalPlanner:
       tracking_lead_active and
       self.lead_is_matched_follow_window(lead, v_ego, t_follow)
     )
-    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
-    headway_margin = actual_headway - float(t_follow)
     if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN):
       return None
 
-    if (
-      radar_matched_follow_active and
-      current_source in ("lead0", "lead1") and
-      lead_delta >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA and
-      float(getattr(lead, "aLeadK", 0.0)) >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL and
-      headway_margin >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN
-    ):
-      return None
-
     if (radar_matched_follow_active and current_source == "cruise" and
         gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and
         lead_delta < min_lead_delta):
@@ -1588,17 +1518,8 @@ class LongitudinalPlanner:
     if gap_error > gap_buffer:
       return None
 
-    if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
-      return None
-    if current_source == "cruise" and self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active):
-      return None
-
-    if not low_speed_follow_window and self.is_stable_post_departure_pullaway(lead, v_ego, t_follow):
-      return None
-
-    # If the lead is already pace-matched or pulling away, keep any catch-up
-    # accel very small while we're near the follow target so we don't surge into
-    # the lead and immediately ask for brake again.
+    # Keep the near-target cap conservative, then continuously relax it when
+    # there is real headway to use. Avoid binary bypasses around zero vRel.
     if low_speed_follow_window:
       edge_cap = float(np.interp(lead_delta, [min_lead_delta, 0.0, 1.0, 2.0], [0.20, 0.24, 0.38, 0.55]))
       near_cap = min(edge_cap, 0.16)
@@ -1606,7 +1527,10 @@ class LongitudinalPlanner:
       edge_cap = float(np.interp(lead_delta, [-0.5, 0.0, 1.0], [0.16, 0.08, 0.02]))
       near_cap = min(edge_cap, 0.03)
     gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0))
-    return float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap]))
+    cap = float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap]))
+    allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0))
+    cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow)
+    return cap
 
   def get_low_speed_follow_transition_brake_cap(self, lead, v_ego, t_follow, prev_output_a_target, output_a_target):
     if lead is None or not lead.status:
@@ -1666,9 +1590,12 @@ class LongitudinalPlanner:
     if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED:
       return None
 
-    if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow):
-      return None
-    if self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active):
+    lead_accel = float(getattr(lead, "aLeadK", 0.0))
+    if (
+      float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED and
+      lead_delta >= 0.35 and
+      lead_accel >= 0.25
+    ):
       return None
 
     closing_speed = max(float(v_ego) - float(lead.vLead), 0.0)
@@ -1680,18 +1607,6 @@ class LongitudinalPlanner:
     if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead:
       return None
 
-    # Don't let a spacious, nearly pace-matched tracked lead toggle this cap on
-    # and off while cruise remains the source. That creates the square-wave
-    # accel "surge / give up / surge" behavior seen in real logs.
-    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
-    headway_margin = actual_headway - float(t_follow)
-    tracking_only_follow = tracking_lead_active and not raw_close_lead and not unresolved_slow_lead
-    if (tracking_only_follow and
-        headway_margin > CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET and
-        closing_speed < CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED and
-        lead_brake <= CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE):
-      return None
-
     desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
     gap_error = float(lead.dRel) - desired_gap
     gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN,
@@ -1699,18 +1614,6 @@ class LongitudinalPlanner:
     if gap_error > gap_buffer:
       return None
 
-    # If the same lead is already accelerating away and we're no longer tight to
-    # the follow target, don't slam the accel cap back on just because lead_delta
-    # momentarily falls near the pull-away threshold. That produces the repeated
-    # 0.18 m/s^2 "surge / give up / surge" behavior seen in real logs.
-    lead_accel = float(getattr(lead, "aLeadK", 0.0))
-    if self.is_stable_post_departure_pullaway(lead, v_ego, t_follow) or (
-      lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and
-      lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and
-      gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN
-    ):
-      return None
-
     base_cap = float(np.interp(
       lead_delta,
       [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED],
@@ -1722,11 +1625,10 @@ class LongitudinalPlanner:
     else:
       base_cap = min(base_cap, float(np.interp(closing_speed, [0.0, 1.0, 2.0], [0.18, 0.12, 0.06])))
 
-    if gap_error <= 0.0:
-      return max(0.0, base_cap)
-
     gap_factor = float(np.clip(gap_error / max(gap_buffer, 0.1), 0.0, 1.0))
     cap = min(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL, base_cap + 0.06 * gap_factor)
+    allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0))
+    cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow)
     return max(0.0, cap)
 
   def get_cruise_tracking_lead_accel_transition_target(self, lead, v_ego, t_follow,
@@ -2484,21 +2386,6 @@ class LongitudinalPlanner:
       not recently_braked
     )
 
-    if lead_one_active and self.mpc.source == "cruise":
-      lead_delta = float(self.lead_one.vLead) - float(scene_v_ego)
-      lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0)))
-      if (
-        self.is_spacious_low_closure_follow(self.lead_one, scene_v_ego, effective_t_follow) and
-        lead_brake <= SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE and
-        (
-          self.is_stable_post_departure_pullaway(self.lead_one, scene_v_ego, effective_t_follow) or
-          lead_delta >= SPACIOUS_TRACKED_FOLLOW_LATCH_MIN_LEAD_DELTA
-        )
-      ):
-        self.spacious_follow_cap_bypass_until = now_t + SPACIOUS_TRACKED_FOLLOW_LATCH_TIME
-    elif not lead_one_active:
-      self.spacious_follow_cap_bypass_until = 0.0
-
     # Calculate scene uncertainty from model desire prediction entropy and disengage predictions
     uncertainty = 0.0
     if hasattr(sm['modelV2'], 'meta'):

selfdrive/controls/tests/test_longcontrol.py

@@ -613,7 +613,7 @@ def test_gm_stock_truck_positive_i_bleeds_on_coast_request():
   assert lc.pid.i < 0.25
 
 
-def test_gm_stock_truck_positive_i_trim_skips_when_planner_still_requests_accel():
+def test_gm_stock_truck_positive_i_bleeds_during_light_highway_accel_request():
   CP = car.CarParams.new_message()
   CP.brand = "gm"
   CP.carFingerprint = "CHEVROLET_SILVERADO"
@@ -631,4 +631,46 @@ def test_gm_stock_truck_positive_i_trim_skips_when_planner_still_requests_accel(
 
   lc._trim_gm_truck_positive_hold_integrator(0.05, 0.05, CS)
 
+  assert lc.pid.i < 0.25
+
+
+def test_gm_stock_truck_positive_i_trim_keeps_meaningful_accel_request():
+  CP = car.CarParams.new_message()
+  CP.brand = "gm"
+  CP.carFingerprint = "CHEVROLET_SILVERADO"
+  CP.enableGasInterceptorDEPRECATED = False
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.02]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.28]
+
+  lc = LongControl(CP)
+  lc.pid.i = 0.25
+  lc.last_output_accel = 0.20
+  CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  lc._trim_gm_truck_positive_hold_integrator(0.12, 0.12, CS)
+
+  assert lc.pid.i == pytest.approx(0.25, abs=1e-9)
+
+
+def test_gm_stock_truck_positive_i_trim_preserves_low_speed_launch():
+  CP = car.CarParams.new_message()
+  CP.brand = "gm"
+  CP.carFingerprint = "CHEVROLET_SILVERADO"
+  CP.enableGasInterceptorDEPRECATED = False
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.02]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.28]
+
+  lc = LongControl(CP)
+  lc.pid.i = 0.25
+  lc.last_output_accel = 0.20
+  CS = car.CarState.new_message(vEgo=5.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  lc._trim_gm_truck_positive_hold_integrator(0.05, 0.05, CS)
+
   assert lc.pid.i == pytest.approx(0.25, abs=1e-9)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -2036,7 +2036,7 @@ def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup():
   assert 0.15 <= cap <= 0.45
 
 
-def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lead():
+def test_route_8bc6_post_departure_catchup_cap_uses_continuous_allowance_for_accelerating_radar_lead():
   v_ego = 19.03
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2046,10 +2046,11 @@ def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lea
 
   cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45)
 
-  assert cap is None
+  assert cap is not None
+  assert 0.1 < cap < 0.3
 
 
-def test_route_8bc6_cruise_tracking_cap_skips_comfortable_accelerating_radar_follow():
+def test_route_8bc6_cruise_tracking_cap_uses_continuous_allowance_for_accelerating_radar_follow():
   v_ego = 18.744474411010742
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2066,18 +2067,17 @@ def test_route_8bc6_cruise_tracking_cap_skips_comfortable_accelerating_radar_fol
     tracking_lead_active=True,
   )
 
-  assert cap is None
+  assert cap is not None
+  assert 0.3 < cap < 0.5
 
 
-def test_route_687_voacc_catchup_cap_skips_spacious_low_closure_follow_with_flat_lead_accel():
+def test_route_687_voacc_catchup_cap_uses_continuous_spacious_allowance():
   v_ego = 12.2
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
   lead = make_lead(
     status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10,
   )
-  planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0
-
   cap = planner.get_lead_catchup_accel_cap(
     lead,
     v_ego,
@@ -2086,18 +2086,17 @@ def test_route_687_voacc_catchup_cap_skips_spacious_low_closure_follow_with_flat
     tracking_lead_active=True,
   )
 
-  assert cap is None
+  assert cap is not None
+  assert 0.15 < cap < 0.3
 
 
-def test_route_687_voacc_cruise_tracking_cap_skips_spacious_low_closure_follow_with_flat_lead_accel():
+def test_route_687_voacc_cruise_tracking_cap_uses_continuous_spacious_allowance():
   v_ego = 12.2
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
   lead = make_lead(
     status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10,
   )
-  planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0
-
   cap = planner.get_cruise_tracking_lead_accel_cap(
     lead,
     v_ego,
@@ -2106,7 +2105,8 @@ def test_route_687_voacc_cruise_tracking_cap_skips_spacious_low_closure_follow_w
     tracking_lead_active=True,
   )
 
-  assert cap is None
+  assert cap is not None
+  assert 0.15 < cap < 0.3
 
 
 def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high():
@@ -2452,7 +2452,7 @@ def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble():
   )
 
   assert cap is not None
-  assert 0.05 <= cap <= 0.10
+  assert 0.3 <= cap <= 0.5
 
 
 def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst():
@@ -2507,7 +2507,7 @@ def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead():
   assert cap is None
 
 
-def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow():
+def test_cruise_tracking_lead_accel_cap_continuously_limits_spacious_tracking_only_follow():
   v_ego = 18.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2521,7 +2521,37 @@ def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow():
     tracking_lead_active=True,
   )
 
-  assert cap is None
+  assert cap is not None
+  assert 0.3 < cap < 0.55
+
+
+def test_route_8bc6_radar_follow_caps_do_not_flip_between_bypass_and_hold():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=17.35)
+  route_states = (
+    (17.35, 36.2, 16.55, 0.60),
+    (17.26, 34.8, 17.36, 0.40),
+    (18.78, 35.2, 18.58, 0.40),
+    (18.87, 35.0, 19.07, 0.40),
+  )
+
+  caps = []
+  for v_ego, d_rel, v_lead, a_lead in route_states:
+    lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead,
+                     radar=True, model_prob=1.0, y_rel=0.2)
+    cap = planner.get_cruise_tracking_lead_accel_cap(
+      lead,
+      v_ego,
+      1.25,
+      current_source="cruise",
+      tracking_lead_active=True,
+    )
+    assert cap is not None
+    caps.append(cap)
+
+  assert min(caps) > 0.25
+  assert max(caps) < 0.65
+  assert max(caps) - min(caps) < 0.35
 
 
 def test_inside_gap_closing_lead_cap_blocks_route_accel_burst():
@@ -2570,7 +2600,7 @@ def test_inside_gap_closing_lead_cap_does_not_touch_standstill_departure():
   assert planner.get_inside_gap_closing_lead_accel_cap(lead, 0.0, -1.0, 1.25) is None
 
 
-def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead():
+def test_route_8bc6_post_departure_cruise_cap_uses_continuous_allowance_for_accelerating_radar_lead():
   v_ego = 19.03
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2586,10 +2616,11 @@ def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead
     tracking_lead_active=True,
   )
 
-  assert cap is None
+  assert cap is not None
+  assert 0.2 < cap < 0.35
 
 
-def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow():
+def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow_outside_cap_window():
   v_ego = 24.108949661254883
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2629,7 +2660,7 @@ def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelera
   assert cap is None
 
 
-def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_flips_to_lead0():
+def test_route_8bc6_catchup_cap_continuously_limits_accelerating_lead_when_source_flips_to_lead0():
   v_ego = 24.361867904663086
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2646,11 +2677,11 @@ def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_
     tracking_lead_active=True,
   )
 
-  assert planner.is_comfortable_accelerating_away_follow(lead, v_ego, 1.3549551963806152)
-  assert cap is None
+  assert cap is not None
+  assert 0.03 < cap < 0.10
 
 
-def test_route_8bc6_radar_matched_follow_catchup_cap_skips_mild_pullaway_after_lead_lock():
+def test_route_8bc6_radar_matched_follow_catchup_cap_continuously_limits_mild_pullaway_after_lead_lock():
   v_ego = 23.96
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
   planner = LongitudinalPlanner(CP, init_v=v_ego)
@@ -2667,7 +2698,8 @@ def test_route_8bc6_radar_matched_follow_catchup_cap_skips_mild_pullaway_after_l
   )
 
   assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16)
-  assert cap is None
+  assert cap is not None
+  assert 0.1 < cap < 0.25
 
 
 def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed():
@@ -2786,7 +2818,8 @@ def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap
     tracking_lead_active=True,
   )
 
-  assert cap_without_latch == pytest.approx(0.10831585854957321, abs=1e-6)
+  assert cap_without_latch is not None
+  assert 0.10 < cap_without_latch < 0.25
   assert cap_with_latch is None
 
 

selfdrive/controls/tests/test_starpilot_acceleration.py

@@ -152,17 +152,15 @@ def test_slc_coast_window_disabled_when_target_drop_is_not_slc():
   assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
 
 
-def test_truck_tuning_standard_profile_limits_launch_spike():
-  assert get_max_accel_standard(0.0, ev_tuning=False, truck_tuning=True) < 3.0
+def test_truck_tuning_standard_profile_keeps_non_binding_launch_headroom():
+  assert get_max_accel_standard(0.0, ev_tuning=False, truck_tuning=True) == pytest.approx(6.0)
+  assert get_max_accel_standard(5.0, ev_tuning=False, truck_tuning=True) == pytest.approx(1.10)
 
 
-def test_truck_tuning_standard_profile_keeps_mid_speed_headroom():
-  truck = get_max_accel_standard(15.0, ev_tuning=False, truck_tuning=True)
-  gas = get_max_accel_standard(15.0, ev_tuning=False, truck_tuning=False)
-
-  assert truck >= gas - 0.05
-  assert truck > 1.25
+def test_truck_tuning_standard_profile_uses_proven_cruise_limits():
+  assert get_max_accel_standard(15.0, ev_tuning=False, truck_tuning=True) == pytest.approx(0.60)
+  assert get_max_accel_standard(25.0, ev_tuning=False, truck_tuning=True) == pytest.approx(0.45)
 
 
-def test_truck_tuning_standard_profile_does_not_fall_off_at_highway_speed():
-  assert get_max_accel_standard(25.0, ev_tuning=False, truck_tuning=True) >= 0.85
+def test_truck_tuning_standard_profile_limits_highway_run_up():
+  assert get_max_accel_standard(40.0, ev_tuning=False, truck_tuning=True) == pytest.approx(0.35)

starpilot/common/accel_profile.py

@@ -45,10 +45,10 @@ A_CRUISE_MAX_VALS_STANDARD_GAS =   [2.00, 1.80, 1.55, 1.30, 1.05, 0.85, 0.55]
 A_CRUISE_MAX_VALS_SPORT_GAS =      [2.50, 2.25, 1.95, 1.60, 1.30, 1.05, 0.75]
 A_CRUISE_MAX_VALS_SPORT_PLUS_GAS = [3.50, 3.20, 2.80, 2.35, 1.90, 1.55, 1.15]
 
-A_CRUISE_MAX_VALS_ECO_TRUCK = [2.00, 1.55, 1.27, 1.07, 0.90, 0.72, 0.46]
-A_CRUISE_MAX_VALS_STANDARD_TRUCK = [2.75, 2.05, 1.72, 1.42, 1.18, 0.98, 0.64]
-A_CRUISE_MAX_VALS_SPORT_TRUCK = [3.25, 2.45, 2.07, 1.72, 1.43, 1.20, 0.84]
-A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK = [4.00, 2.85, 2.42, 2.02, 1.68, 1.40, 1.04]
+A_CRUISE_MAX_VALS_ECO_TRUCK = [3.00, 1.05, 0.60, 0.50, 0.50, 0.45, 0.35]
+A_CRUISE_MAX_VALS_STANDARD_TRUCK = [6.00, 1.10, 0.70, 0.60, 0.55, 0.45, 0.35]
+A_CRUISE_MAX_VALS_SPORT_TRUCK = [6.00, 1.15, 0.75, 0.70, 0.60, 0.50, 0.40]
+A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK = [6.00, 1.30, 0.90, 0.80, 0.70, 0.60, 0.45]
 
 
 def akima_interp(x, xp, fp):

starpilot/system/speed_limit_vision.py

@@ -187,6 +187,10 @@ DETECTOR_CLASSIFIER_RESCUE_MIN_X_RATIO = 0.52
 DETECTOR_CLASSIFIER_RESCUE_MIN_SUPPORT = 2
 DETECTOR_CLASSIFIER_RESCUE_MIN_CONFIDENCE = 0.90
 DETECTOR_CLASSIFIER_RESCUE_MAX_SCORE = 0.64
+DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_SUPPORT = 3
+DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_PROPOSAL_CONFIDENCE = 0.60
+DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_READ_CONFIDENCE = 0.995
+DETECTOR_CLASSIFIER_STRONG_RESCUE_MAX_SCORE = 0.74
 DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_HEIGHT = 55
 DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_AREA_RATIO = 0.002
 DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_PROPOSAL_CONFIDENCE = 0.18
@@ -224,6 +228,7 @@ def device_cpu_usage_busy(cpu_usage):
 class Detection:
   speed_limit_mph: int
   confidence: float
+  strong_consensus: bool = False
 
 
 @dataclass
@@ -231,6 +236,7 @@ class HistoryEntry:
   speed_limit_mph: int
   confidence: float
   created_at: float
+  strong_consensus: bool = False
 
 
 class SpeedLimitVisionDaemon:
@@ -1535,6 +1541,7 @@ class SpeedLimitVisionDaemon:
             speed_limit_mph = competing_speed_limit_mph
       read_confidence = speed_best_confidences[speed_limit_mph]
       support_count = speed_support_counts[speed_limit_mph]
+      strong_rescue = False
       score = min(
         read_confidence * 0.72 +
         proposal_confidence * 0.24 +
@@ -1560,12 +1567,20 @@ class SpeedLimitVisionDaemon:
           continue
         if read_confidence < DETECTOR_CLASSIFIER_RESCUE_MIN_CONFIDENCE:
           continue
-        published_score = min(score, DETECTOR_CLASSIFIER_RESCUE_MAX_SCORE)
+        strong_rescue = (
+          speed_trusted_model_support.get(speed_limit_mph, 0) >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_SUPPORT and
+          proposal_confidence >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_PROPOSAL_CONFIDENCE and
+          read_confidence >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_READ_CONFIDENCE
+        )
+        rescue_max_score = (
+          DETECTOR_CLASSIFIER_STRONG_RESCUE_MAX_SCORE if strong_rescue else DETECTOR_CLASSIFIER_RESCUE_MAX_SCORE
+        )
+        published_score = min(score, rescue_max_score)
         if speed_trusted_model_support.get(speed_limit_mph, 0) < DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_SUPPORT:
           selection_score = published_score
       if selection_score > best_score:
         best_score = selection_score
-        best_detection = Detection(speed_limit_mph, published_score)
+        best_detection = Detection(speed_limit_mph, published_score, strong_rescue)
       if best_detection is not None and best_detection.confidence >= MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE:
         return best_detection
 
@@ -1839,22 +1854,25 @@ class SpeedLimitVisionDaemon:
     candidate_speed_limit, candidate_count = counts.most_common(1)[0]
     matching_entries = [entry for entry in self.history if entry.speed_limit_mph == candidate_speed_limit]
     best_confidence = max(entry.confidence for entry in matching_entries)
+    has_strong_consensus = any(entry.strong_consensus for entry in matching_entries)
     current_speed_limit = self.published_speed_limit_mph
     current_count = counts.get(current_speed_limit, 0) if current_speed_limit > 0 else 0
 
     if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit:
       required_count = CHANGE_CONSISTENT_DETECTIONS
+      allow_single_frame_consensus = has_strong_consensus
       if current_speed_limit >= 30 and candidate_speed_limit < 30:
         required_count = LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS
+        allow_single_frame_consensus = False
         if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE:
           return None
-      if candidate_count < required_count:
+      if candidate_count < required_count and not allow_single_frame_consensus:
         return None
       if candidate_count <= current_count:
         return None
       return candidate_speed_limit, best_confidence
 
-    if best_confidence >= STRONG_DETECTION_CONFIDENCE or candidate_count >= CONSISTENT_DETECTIONS:
+    if has_strong_consensus or best_confidence >= STRONG_DETECTION_CONFIDENCE or candidate_count >= CONSISTENT_DETECTIONS:
       return candidate_speed_limit, best_confidence
     return None
 
@@ -2058,7 +2076,7 @@ class SpeedLimitVisionDaemon:
         candidateConfidence=round(detection.confidence, 4),
       )
 
-    self.history.append(HistoryEntry(detection.speed_limit_mph, detection.confidence, now))
+    self.history.append(HistoryEntry(detection.speed_limit_mph, detection.confidence, now, detection.strong_consensus))
     self._prune_history(now)
 
     confirmed = self._confirm_detection()

starpilot/system/tests/test_speed_limit_vision.py

@@ -20,6 +20,12 @@ def test_speed_change_requires_two_matching_reads():
   assert daemon._confirm_detection() == pytest.approx((55, 0.76))
 
 
+def test_speed_change_accepts_single_strong_consensus_read():
+  daemon = daemon_with_history(70, [])
+  daemon.history.append(HistoryEntry(60, 0.74, 1.0, strong_consensus=True))
+  assert daemon._confirm_detection() == pytest.approx((60, 0.74))
+
+
 def test_low_speed_change_requires_three_high_confidence_reads():
   daemon = daemon_with_history(40, [(25, 0.95), (25, 0.96)])
   assert daemon._confirm_detection() is None
@@ -28,6 +34,12 @@ def test_low_speed_change_requires_three_high_confidence_reads():
   assert daemon._confirm_detection() == pytest.approx((25, 0.96))
 
 
+def test_low_speed_change_ignores_single_strong_consensus_read():
+  daemon = daemon_with_history(40, [])
+  daemon.history.append(HistoryEntry(25, 0.95, 1.0, strong_consensus=True))
+  assert daemon._confirm_detection() is None
+
+
 def test_low_speed_change_rejects_low_confidence_sequence():
   daemon = daemon_with_history(40, [(25, 0.82), (25, 0.88), (25, 0.89)])
   assert daemon._confirm_detection() is None

starpilot/system/the_galaxy/assets/components/router.js

@@ -19,7 +19,7 @@ import { ModelManager } from "/assets/components/tools/model_manager.js?v=202603
 import { LivePlots } from "/assets/components/tools/plots.js"
 import { ThemeMaker } from "/assets/components/tools/theme_maker.js"
 import { TestingGround } from "/assets/components/tools/testing_ground.js"
-import { Tuning } from "/assets/components/tools/tuning.js?v=ftm-workspace-5"
+import { Tuning } from "/assets/components/tools/tuning.js?v=ftm-workspace-6"
 import { Troubleshoot } from "/assets/components/tools/troubleshoot.js"
 import { TmuxLog } from "/assets/components/tools/tmux.js"
 import { ToggleControl } from "/assets/components/tools/toggles.js"

starpilot/system/the_galaxy/assets/components/tools/tuning.js

@@ -795,9 +795,10 @@ export function Tuning() {
               <p><strong>Analyzer Recommended:</strong> ${reportPaths().find((path) => path.isPrimary)?.title || "Recommendations"}</p>
               <p><strong>Active Path:</strong> ${primaryPath()?.title || "Recommendations"}</p>
               <p><strong>Path Choice:</strong> ${state.report.pathSelectionSource === "manual" ? "Manual override" : "Automatic"}</p>
-              <p><strong>Nonlinear Torque Map:</strong> ${state.report.capabilities?.nonlinearTorqueMap?.asymmetric ? "Asymmetric left/right siglin" : (state.report.capabilities?.nonlinearTorqueMap ? "Symmetric siglin" : "Not detected")}</p>
-              <p><strong>Live Learner Refits Map:</strong> ${state.report.capabilities?.nonlinearTorqueMap ? "No" : "Not applicable"}</p>
+              <p><strong>Nonlinear Torque Map:</strong> ${state.report.capabilities?.nonlinearTorqueMap?.type === "siglin" ? (state.report.capabilities.nonlinearTorqueMap.asymmetric ? "Asymmetric left/right siglin" : "Symmetric siglin") : "Not detected"}</p>
+              <p><strong>Live Learner Refits Map:</strong> ${state.report.capabilities?.nonlinearTorqueMap?.type === "siglin" ? "No" : "Not applicable"}</p>
               <p><strong>Processed Segments:</strong> ${safeCount(state.report.summary?.processedSegments)}</p>
+              <p><strong>Driver-Override Samples Excluded:</strong> ${safeCount(state.report.summary?.excludedDriverOverrideSamples)}</p>
               <p><strong>qlog Fallback:</strong> ${state.report.summary?.usedQlogFallback ? "Yes" : "No"}</p>
               <p><strong>Samples:</strong> ${safeCount(state.report.summary?.sampleCount)}</p>
             </div>

starpilot/system/the_galaxy/ftm_workspace.py

@@ -106,6 +106,9 @@ FTM_PATH_SPECS = {
   },
 }
 
+FTM_DRIVER_OVERRIDE_PRE_BUFFER_S = 0.35
+FTM_DRIVER_OVERRIDE_POST_BUFFER_S = 1.0
+
 
 @dataclass
 class RouteSource:
@@ -447,6 +450,39 @@ def _group_masked_events(samples: list[FTMSample], mask: list[bool], score_serie
   return events
 
 
+def _analysis_eligibility_mask(samples: list[FTMSample]) -> list[bool]:
+  eligible = [bool(sample.lat_active) for sample in samples]
+  group_start = 0
+  while group_start < len(samples):
+    group_key = (samples[group_start].route, samples[group_start].segment)
+    group_end = group_start + 1
+    while group_end < len(samples) and (samples[group_end].route, samples[group_end].segment) == group_key:
+      group_end += 1
+
+    last_override = -math.inf
+    for idx in range(group_start, group_end):
+      sample = samples[idx]
+      if sample.steering_pressed:
+        last_override = sample.t
+      if sample.steering_pressed or (sample.t - last_override) <= FTM_DRIVER_OVERRIDE_POST_BUFFER_S:
+        eligible[idx] = False
+
+    next_override = math.inf
+    for idx in range(group_end - 1, group_start - 1, -1):
+      sample = samples[idx]
+      if sample.steering_pressed:
+        next_override = sample.t
+      if sample.steering_pressed or (next_override - sample.t) <= FTM_DRIVER_OVERRIDE_PRE_BUFFER_S:
+        eligible[idx] = False
+
+    # Force an event boundary between route segments even when lateral control stays active.
+    eligible[group_start] = False
+    eligible[group_end - 1] = False
+    group_start = group_end
+
+  return eligible
+
+
 def _build_plot_svg(samples: list[FTMSample], event: dict[str, Any]) -> str:
   start_idx = max(0, event["startIdx"] - 12)
   end_idx = min(len(samples) - 1, event["endIdx"] + 12)
@@ -685,14 +721,14 @@ def _rich_profile_supports_knob(capabilities: dict[str, Any], suffix: str) -> bo
 
 
 def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any]], dict[str, Any]]:
-  active_samples = [sample for sample in samples if sample.lat_active and not sample.steering_pressed]
+  eligibility = _analysis_eligibility_mask(samples)
+  active_samples = [sample for sample, allowed in zip(samples, eligibility, strict=True) if allowed]
   if not active_samples:
     return [], {"sampleCount": 0}
 
-  over_error = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples]
-  v_ego = [sample.v_ego for sample in active_samples]
-  desired = [sample.desired_la for sample in active_samples]
-  jerk = [sample.desired_jerk for sample in active_samples]
+  over_error = [abs(sample.actual_la) - abs(sample.desired_la) for sample in samples]
+  desired = [sample.desired_la for sample in samples]
+  jerk = [sample.desired_jerk for sample in samples]
   angle = [sample.steering_angle_deg for sample in active_samples]
   output = [sample.output for sample in active_samples]
 
@@ -700,18 +736,22 @@ def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any
   unwind_phase = [(abs(d) > 0.25 and abs(j) > 0.20 and d * j < 0.0) for d, j in zip(desired, jerk, strict=True)]
   steady_curve_phase = [(abs(d) > 0.35 and abs(j) < 0.18) for d, j in zip(desired, jerk, strict=True)]
   saturation_phase = [
-    bool(sample.saturated and abs(sample.desired_la) > 0.30 and (abs(sample.desired_jerk) > 0.16 or abs(sample.actual_la) > 0.45))
-    for sample in active_samples
+    bool(allowed and sample.saturated and abs(sample.desired_la) > 0.30 and (abs(sample.desired_jerk) > 0.16 or abs(sample.actual_la) > 0.45))
+    for sample, allowed in zip(samples, eligibility, strict=True)
   ]
 
   base_masks = {
-    "understeer": [(phase and (ov < -0.20)) for phase, ov in zip(steady_curve_phase, over_error, strict=True)],
-    "oversteer": [(phase and (ov > 0.20)) for phase, ov in zip(steady_curve_phase, over_error, strict=True)],
-    "late_turn_in": [(phase and ov < -0.16) for phase, ov in zip(entry_phase, over_error, strict=True)],
-    "early_turn_in": [(phase and ov > 0.16) for phase, ov in zip(entry_phase, over_error, strict=True)],
-    "unwind_too_slow": [(phase and ov > 0.14) for phase, ov in zip(unwind_phase, over_error, strict=True)],
-    "unwind_too_fast": [(phase and ov < -0.14) for phase, ov in zip(unwind_phase, over_error, strict=True)],
-    "low_speed_unwillingness": [(s.v_ego < 6.0 and abs(s.desired_la) > 0.30 and abs(s.desired_jerk) > 0.18 and (abs(s.actual_la) + 0.18) < abs(s.desired_la)) for s in active_samples],
+    "understeer": [(allowed and phase and (ov < -0.20)) for allowed, phase, ov in zip(eligibility, steady_curve_phase, over_error, strict=True)],
+    "oversteer": [(allowed and phase and (ov > 0.20)) for allowed, phase, ov in zip(eligibility, steady_curve_phase, over_error, strict=True)],
+    "late_turn_in": [(allowed and phase and ov < -0.16) for allowed, phase, ov in zip(eligibility, entry_phase, over_error, strict=True)],
+    "early_turn_in": [(allowed and phase and ov > 0.16) for allowed, phase, ov in zip(eligibility, entry_phase, over_error, strict=True)],
+    "unwind_too_slow": [(allowed and phase and ov > 0.14) for allowed, phase, ov in zip(eligibility, unwind_phase, over_error, strict=True)],
+    "unwind_too_fast": [(allowed and phase and ov < -0.14) for allowed, phase, ov in zip(eligibility, unwind_phase, over_error, strict=True)],
+    "low_speed_unwillingness": [
+      bool(allowed and sample.v_ego < 6.0 and abs(sample.desired_la) > 0.30 and abs(sample.desired_jerk) > 0.18 and
+           (abs(sample.actual_la) + 0.18) < abs(sample.desired_la))
+      for sample, allowed in zip(samples, eligibility, strict=True)
+    ],
     "saturation_limited": saturation_phase,
   }
   score_map = {
@@ -721,16 +761,18 @@ def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any
     "early_turn_in": [max(ov, 0.0) + abs(j) * 0.1 for ov, j in zip(over_error, jerk, strict=True)],
     "unwind_too_slow": [max(ov, 0.0) for ov in over_error],
     "unwind_too_fast": [max((-ov), 0.0) for ov in over_error],
-    "low_speed_unwillingness": [max(abs(d) - abs(a), 0.0) for d, a in zip(desired, [sample.actual_la for sample in active_samples], strict=True)],
-    "saturation_limited": [1.0 if sample.saturated else 0.0 for sample in active_samples],
+    "low_speed_unwillingness": [max(abs(d) - abs(sample.actual_la), 0.0) for d, sample in zip(desired, samples, strict=True)],
+    "saturation_limited": [1.0 if sample.saturated else 0.0 for sample in samples],
   }
 
   # Straight-road chatter detection uses a simple 4-second window.
   straight_windows = []
-  for start_idx in range(0, max(len(active_samples) - 20, 1), 10):
-    window = active_samples[start_idx:start_idx + 40]
+  for start_idx in range(0, max(len(samples) - 20, 1), 10):
+    window = samples[start_idx:start_idx + 40]
     if len(window) < 20:
       continue
+    if not all(eligibility[start_idx:start_idx + len(window)]):
+      continue
     if float(np.mean([sample.v_ego for sample in window])) < 20.0:
       continue
     if float(np.mean([abs(sample.desired_la) for sample in window])) > 0.12:
@@ -753,10 +795,12 @@ def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any
       })
 
   curve_windows = []
-  for start_idx in range(0, max(len(active_samples) - 20, 1), 8):
-    window = active_samples[start_idx:start_idx + 36]
+  for start_idx in range(0, max(len(samples) - 20, 1), 8):
+    window = samples[start_idx:start_idx + 36]
     if len(window) < 18:
       continue
+    if not all(eligibility[start_idx:start_idx + len(window)]):
+      continue
     if float(np.mean([sample.v_ego for sample in window])) < 15.0:
       continue
     desired_sign = float(np.mean([sample.desired_la for sample in window]))
@@ -782,20 +826,21 @@ def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any
 
   summaries: list[dict[str, Any]] = []
   for bucket, mask in base_masks.items():
-    events = _group_masked_events(active_samples, mask, score_map[bucket])
+    events = _group_masked_events(samples, mask, score_map[bucket])
     if events:
-      summaries.extend(_summaries_from_events(bucket, active_samples, events))
+      summaries.extend(_summaries_from_events(bucket, samples, events))
   if straight_windows:
-    summaries.extend(_summaries_from_events("center_chatter", active_samples, straight_windows))
+    summaries.extend(_summaries_from_events("center_chatter", samples, straight_windows))
   if curve_windows:
-    summaries.extend(_summaries_from_events("notchy_mid_curve", active_samples, curve_windows))
+    summaries.extend(_summaries_from_events("notchy_mid_curve", samples, curve_windows))
 
   left_errors = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples if sample.desired_la > 0.25]
   right_errors = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples if sample.desired_la < -0.25]
   summary_stats = {
     "sampleCount": len(active_samples),
+    "excludedDriverOverrideSamples": sum(1 for sample, allowed in zip(samples, eligibility, strict=True) if sample.lat_active and not allowed),
     "qlogFallback": False,
-    "meanDesiredAbs": round(float(np.mean(np.abs(desired))), 4),
+    "meanDesiredAbs": round(float(np.mean(np.abs([sample.desired_la for sample in active_samples]))), 4),
     "meanErrorAbs": round(float(np.mean(np.abs([sample.actual_la - sample.desired_la for sample in active_samples]))), 4),
     "leftBias": round(float(np.mean(left_errors)), 4) if left_errors else 0.0,
     "rightBias": round(float(np.mean(right_errors)), 4) if right_errors else 0.0,
@@ -1376,6 +1421,7 @@ def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: d
   actionable = [
     summary for summary in summaries
     if summary.get("bucket") not in ("model_limited", "angle_control_diagnostic")
+    and float(summary.get("severity", 0.0)) >= 0.4
   ]
   if not actionable:
     return {
@@ -1393,6 +1439,30 @@ def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: d
     if summary.get("bucket") in ("understeer", "oversteer", "late_turn_in", "early_turn_in", "saturation_limited")
     and float(summary.get("severity", 0.0)) >= 0.85
   ]
+  severe_global_bands = {
+    (str(summary.get("direction", "center")), str(summary.get("speedBand", "mixed")))
+    for summary in severe_global
+  }
+  severe_global_segments = {
+    str(segment.get("label", ""))
+    for summary in severe_global
+    for segment in summary.get("evidence", {}).get("segments", [])
+    if segment.get("label")
+  }
+  severe_saturation = any(
+    summary.get("bucket") == "saturation_limited" and float(summary.get("severity", 0.0)) >= 0.85
+    for summary in actionable
+  )
+
+  if mean_error < 0.08 and not severe_saturation and not (
+    len(severe_global_bands) >= 2 and len(severe_global_segments) >= 2
+  ):
+    return {
+      "primaryPathKey": "cleanup_pass",
+      "alternatePathKey": "baseline_fix",
+      "reason": "Overall lateral-accel tracking is already strong. The remaining misses are isolated enough that changing the base tune would disturb more good behavior than it fixes.",
+      "baselineScore": 0,
+    }
 
   baseline_score = 0
   if mean_error >= 0.14:
@@ -1685,7 +1755,8 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d
   current_params = _current_param_state(car_params, params)
 
   if torque_control:
-    summaries, summary_stats = classify_torque_samples(all_samples)
+    raw_summaries, summary_stats = classify_torque_samples(all_samples)
+    summaries = _resolve_conflicting_actionable_suggestions(raw_summaries)
     paths_payload, path_decision = build_recommendation_paths(report_id, summaries, summary_stats, capabilities, current_params, feedback)
     primary_path = next((path for path in paths_payload if path.get("isPrimary")), paths_payload[0] if paths_payload else {})
     suggestions = list(primary_path.get("suggestions", []))
@@ -1761,6 +1832,7 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d
     "pathDecision": path_decision,
     "paths": paths_payload,
     "findings": summaries,
+    "rawFindings": raw_summaries if torque_control else summaries,
     "suggestions": suggestions,
     "profiles": profiles,
     "addTheseParametersAndStartHere": _add_parameters_start_here(capabilities, suggestions, path_decision["primaryPathKey"]),

starpilot/system/the_galaxy/tests/test_ftm_workspace.py

@@ -188,10 +188,44 @@ def test_classify_torque_samples_detects_center_chatter(tmp_path):
     ))
 
   summaries, stats = module.classify_torque_samples(samples)
-  assert stats["sampleCount"] == len(samples)
+  assert stats["sampleCount"] == len(samples) - 2  # Segment edges are event boundaries, not analysis samples.
   assert any(summary["bucket"] == "center_chatter" for summary in summaries)
 
 
+def test_analysis_eligibility_masks_driver_override_with_settle_buffer(tmp_path):
+  module, _ = _load_ftm_workspace_module(tmp_path)
+  samples = [
+    _sample(module, t=idx * 0.1, steering_pressed=(idx == 20))
+    for idx in range(50)
+  ]
+
+  eligible = module._analysis_eligibility_mask(samples)
+  assert eligible[16] is True
+  assert eligible[17] is False
+  assert eligible[20] is False
+  assert eligible[30] is False
+  assert eligible[31] is True
+
+
+def test_classify_torque_samples_does_not_bridge_driver_override(tmp_path):
+  module, _ = _load_ftm_workspace_module(tmp_path)
+  samples = []
+  for idx in range(80):
+    samples.append(_sample(
+      module,
+      t=idx * 0.1,
+      desired_la=-0.5,
+      actual_la=-0.1,
+      desired_jerk=-0.5,
+      steering_pressed=30 <= idx <= 40,
+    ))
+
+  summaries, stats = module.classify_torque_samples(samples)
+  late_events = [event for summary in summaries if summary["bucket"] == "late_turn_in" for event in summary["events"]]
+  assert stats["excludedDriverOverrideSamples"] > 11
+  assert all(event["endIdx"] < 27 or event["startIdx"] > 50 for event in late_events)
+
+
 def test_build_suggestions_prefers_rich_low_speed_turn_in_knob(tmp_path):
   module, _ = _load_ftm_workspace_module(tmp_path)
   summary = {
@@ -380,6 +414,36 @@ def test_select_primary_tuning_path_prefers_cleanup_for_localized_issue(tmp_path
   assert decision["alternatePathKey"] == "baseline_fix"
 
 
+def test_select_primary_tuning_path_vetoes_baseline_when_global_fit_is_strong(tmp_path):
+  module, _ = _load_ftm_workspace_module(tmp_path)
+  summaries = [
+    {
+      "bucket": "late_turn_in",
+      "severity": 1.05,
+      "direction": "right",
+      "speedBand": "mid",
+      "evidence": {"segments": [{"label": "route/37"}]},
+    },
+    {"bucket": "center_chatter", "severity": 0.55, "direction": "center", "speedBand": "highway", "evidence": {"segments": []}},
+    {"bucket": "unwind_too_slow", "severity": 0.6, "direction": "right", "speedBand": "mid", "evidence": {"segments": [{"label": "route/37"}]}},
+  ]
+
+  decision = module.select_primary_tuning_path(summaries, {"meanErrorAbs": 0.054})
+  assert decision["primaryPathKey"] == "cleanup_pass"
+  assert "already strong" in decision["reason"]
+
+
+def test_conflicting_summary_resolution_keeps_dominant_direction(tmp_path):
+  module, _ = _load_ftm_workspace_module(tmp_path)
+  summaries = [
+    {"bucket": "early_turn_in", "severity": 1.34, "evidence": {"directionBias": "right", "speedBand": "mid", "eventCount": 1}},
+    {"bucket": "late_turn_in", "severity": 1.03, "evidence": {"directionBias": "right", "speedBand": "mid", "eventCount": 18}},
+  ]
+
+  resolved = module._resolve_conflicting_actionable_suggestions(summaries)
+  assert [summary["bucket"] for summary in resolved] == ["late_turn_in"]
+
+
 def test_build_trial_profiles_suppresses_ignored_dimensions(tmp_path):
   module, _ = _load_ftm_workspace_module(tmp_path)
   suggestions = [