diff --git a/scripts/speed_limit_vision/replay_route_runtime.py b/scripts/speed_limit_vision/replay_route_runtime.py
index 3e2c2f4fd..e651c29b2 100644
--- a/scripts/speed_limit_vision/replay_route_runtime.py
+++ b/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)
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 5a5917ef6..4120cc47c 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/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
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 2865d1317..87fc73e93 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/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'):
diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py
index 71ba1748e..f4a0d14cb 100644
--- a/selfdrive/controls/tests/test_longcontrol.py
+++ b/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)
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index 1b9ce75cf..74a6527e7 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/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
diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py
index 005ed2f1d..3bdfb2009 100644
--- a/selfdrive/controls/tests/test_starpilot_acceleration.py
+++ b/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)
diff --git a/starpilot/common/accel_profile.py b/starpilot/common/accel_profile.py
index d669f0ab1..c14fe475e 100644
--- a/starpilot/common/accel_profile.py
+++ b/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):
diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py
index f97fa4f57..1ce095d1a 100644
--- a/starpilot/system/speed_limit_vision.py
+++ b/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()
diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py
index 592fe388d..019623ad6 100644
--- a/starpilot/system/tests/test_speed_limit_vision.py
+++ b/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
diff --git a/starpilot/system/the_galaxy/assets/components/router.js b/starpilot/system/the_galaxy/assets/components/router.js
index 33e7f8a54..2386a6012 100644
--- a/starpilot/system/the_galaxy/assets/components/router.js
+++ b/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"
diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.js b/starpilot/system/the_galaxy/assets/components/tools/tuning.js
index 433920327..8cb9bf033 100644
--- a/starpilot/system/the_galaxy/assets/components/tools/tuning.js
+++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.js
@@ -795,9 +795,10 @@ export function Tuning() {
Analyzer Recommended: ${reportPaths().find((path) => path.isPrimary)?.title || "Recommendations"}
Active Path: ${primaryPath()?.title || "Recommendations"}
Path Choice: ${state.report.pathSelectionSource === "manual" ? "Manual override" : "Automatic"}
- Nonlinear Torque Map: ${state.report.capabilities?.nonlinearTorqueMap?.asymmetric ? "Asymmetric left/right siglin" : (state.report.capabilities?.nonlinearTorqueMap ? "Symmetric siglin" : "Not detected")}
- Live Learner Refits Map: ${state.report.capabilities?.nonlinearTorqueMap ? "No" : "Not applicable"}
+ Nonlinear Torque Map: ${state.report.capabilities?.nonlinearTorqueMap?.type === "siglin" ? (state.report.capabilities.nonlinearTorqueMap.asymmetric ? "Asymmetric left/right siglin" : "Symmetric siglin") : "Not detected"}
+ Live Learner Refits Map: ${state.report.capabilities?.nonlinearTorqueMap?.type === "siglin" ? "No" : "Not applicable"}
Processed Segments: ${safeCount(state.report.summary?.processedSegments)}
+ Driver-Override Samples Excluded: ${safeCount(state.report.summary?.excludedDriverOverrideSamples)}
qlog Fallback: ${state.report.summary?.usedQlogFallback ? "Yes" : "No"}
Samples: ${safeCount(state.report.summary?.sampleCount)}
diff --git a/starpilot/system/the_galaxy/ftm_workspace.py b/starpilot/system/the_galaxy/ftm_workspace.py
index 7e9a5c2d5..6dfb3284d 100644
--- a/starpilot/system/the_galaxy/ftm_workspace.py
+++ b/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"]),
diff --git a/starpilot/system/the_galaxy/tests/test_ftm_workspace.py b/starpilot/system/the_galaxy/tests/test_ftm_workspace.py
index 22c5145f5..2558f7e6d 100644
--- a/starpilot/system/the_galaxy/tests/test_ftm_workspace.py
+++ b/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 = [