From 840e4814d035387ccbb21dafe5bf301fd855437d Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Thu, 4 Jun 2026 14:25:38 -0400 Subject: [PATCH] No-uh --- scripts/diagnose_slc_mapd.py | 72 +++++++++-- .../tests/test_speed_limit_controller.py | 118 ++++++++++++++++++ .../controls/lib/speed_limit_controller.py | 70 +++++++---- 3 files changed, 227 insertions(+), 33 deletions(-) diff --git a/scripts/diagnose_slc_mapd.py b/scripts/diagnose_slc_mapd.py index 28fdb134e..88c602439 100644 --- a/scripts/diagnose_slc_mapd.py +++ b/scripts/diagnose_slc_mapd.py @@ -137,6 +137,16 @@ class ScsSample: decel_pressed: bool +@dataclass +class LeadSample: + """One radarState lead-vehicle event from the log.""" + + log_mono_time: int + has_lead: bool + d_rel: float # metres ahead + v_lead: float # m/s absolute speed of lead + + @dataclass class GpsSample: """One gpsLocationExternal event from the log.""" @@ -251,6 +261,12 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: formatter_class=argparse.RawDescriptionHelpFormatter, epilog=JWT_HELP, ) + p.add_argument( + "route_pos", + nargs="?", + default=None, + help="Route name (positional format alternative)." + ) p.add_argument( "--route", default=None, @@ -268,7 +284,10 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: default=False, help="Display speeds in km/h (default: mph).", ) - return p.parse_args(argv) + args = p.parse_args(argv) + if args.route_pos: + args.route = args.route_pos + return args def resolve_route_identifier(raw: str) -> str: @@ -308,6 +327,8 @@ def resolve_route_identifier(raw: str) -> str: "Expected: dongle_id|log_id (16 hex chars | identifier)" ) dongle, log_id, suffix = m.group(1), m.group(2), m.group(3) or "" + if suffix: + suffix = re.sub(r"^/(\d+)/(\d+)$", r"/\1:\2", suffix) return f"{dongle}/{log_id}{suffix}" @@ -365,18 +386,19 @@ def parse_route_logs( list[CarSample], list[GpsSample], list[ScsSample], + list[LeadSample], list[int], ]: """Use LogReader to parse qlog and extract all speed-related messages. - Returns (mapd_events, splan_events, car_events, gps_events, scs_events, segments). + Returns (mapd_events, splan_events, car_events, gps_events, scs_events, lead_events, segments). """ mapd_events: list[MapdSample] = [] splan_events: list[SplanSample] = [] car_events: list[CarSample] = [] gps_events: list[GpsSample] = [] scs_events: list[ScsSample] = [] - segments_found: set[int] = set() + lead_events: list[LeadSample] = [] segments_found: set[int] = set() seg_of_msg: dict[int, int] = {} @@ -459,6 +481,17 @@ def parse_route_logs( decel_pressed=bool(s.decelPressed), ) ) + elif which == "radarState": + r = msg.radarState + lead = r.leadOne + lead_events.append( + LeadSample( + log_mono_time=t, + has_lead=bool(lead.status), + d_rel=float(lead.dRel or 0), + v_lead=float(lead.vLead or 0), + ) + ) if not all_msgs: print("Warning: no messages found in route logs.", file=sys.stderr) @@ -483,7 +516,7 @@ def parse_route_logs( segments = sorted(segments_found) if segments_found else [0] - return mapd_events, splan_events, car_events, gps_events, scs_events, segments + return mapd_events, splan_events, car_events, gps_events, scs_events, lead_events, segments # ============================================================================ @@ -885,9 +918,10 @@ def detect_changes( osm_ways: dict[int, OsmWay], gps_timeline: list[tuple[float, float, float]], base_time_ns: int, + lead_events: list[LeadSample] | None = None, ) -> list[ChangeRow]: """Walk starpilotPlan events to detect every SLC state transition, - correlating with mapd, carState, and starpilotCarState for full context. + correlating with mapd, carState, starpilotCarState, and radarState for full context. Produces a timeline of SLC decisions: limits, overrides, prompts, lookaheads. """ @@ -899,6 +933,9 @@ def detect_changes( mapd_sorted = ( sorted(mapd_events, key=lambda e: e.log_mono_time) if mapd_events else [] ) + lead_sorted = ( + sorted(lead_events, key=lambda e: e.log_mono_time) if lead_events else [] + ) prev_slc = -1.0 prev_source_key = "" @@ -921,6 +958,7 @@ def detect_changes( m = _nearest(mapd_sorted, t_ns) c = _nearest(car_events, t_ns) s = _nearest(scs_events, t_ns) + lv = _nearest(lead_sorted, t_ns) if lead_sorted else None mapd_sl = m.speed_limit if m else 0 mapd_next = m.next_speed_limit if m else 0 @@ -931,6 +969,9 @@ def detect_changes( gas = bool(c.gas_pressed) if c else False accel = bool(s.accel_pressed) if s else False decel = bool(s.decel_pressed) if s else False + has_lead = bool(lv.has_lead) if lv else False + lead_d_rel = lv.d_rel if lv and lv.has_lead else 0.0 + lead_v = lv.v_lead if lv and lv.has_lead else 0.0 lat, lon = gps_at_time(t_ns, gps_timeline) if gps_timeline else (0, 0) osm_sl, osm_name = ( @@ -1012,11 +1053,17 @@ def detect_changes( detail = f"gas pressed: {v_ego * KPH_TO_MPH * MS_TO_KPH:.0f} > {slc * KPH_TO_MPH * MS_TO_KPH:.0f}" if accel: detail += " (accel)" + if has_lead: + detail += f" [lead {lead_d_rel:.0f}m ahead]" + + elif ov_end and not limit_changed and has_lead and not gas: + event_type = "OVERRIDE CLEAR" + detail = f"ACC decel behind lead ({lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph) — not driver brake" elif ov_end and not limit_changed: if ov_phase: event_type = "OVERRIDE CLEAR" - detail = "override ended" + detail = "override ended" + (f" [lead {lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph]" if has_lead else "") elif active_ov and ov_phase != "active": event_type = "OVERRIDE ACTIVE" @@ -1025,6 +1072,8 @@ def detect_changes( elif stale_ov and ov_phase != "stale": event_type = "STALE OVERRIDE" detail = f"overridden > {slc * KPH_TO_MPH * MS_TO_KPH:.0f}, v_ego={v_ego * KPH_TO_MPH * MS_TO_KPH:.0f}" + if has_lead: + detail += f" [ACC: lead {lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph]" elif source_changed: event_type = "SOURCE" @@ -1102,14 +1151,14 @@ def fmt_speed(mps: float, use_mph: bool, width: int = 5) -> str: def print_header( - route_name: str, mapd_n: int, splan_n: int, gps_n: int, scs_n: int + route_name: str, mapd_n: int, splan_n: int, gps_n: int, scs_n: int, lead_n: int = 0 ) -> None: sep = "=" * 82 print(f"\n{sep}") print(f" SLC / mapd Diagnostic Timeline") print(f" Route: {route_name}") print( - f" Events: mapdOut={mapd_n} | starpilotPlan={splan_n} | starpilotCarState={scs_n} | GPS={gps_n}" + f" Events: mapdOut={mapd_n} | starpilotPlan={splan_n} | starpilotCarState={scs_n} | GPS={gps_n} | radarState={lead_n}" ) print(f"{sep}") @@ -1208,7 +1257,7 @@ def main(argv: list[str] | None = None) -> int: print(f"\nProcessing route: {canonical}", file=sys.stderr) # ── Parse qlog ────────────────────────────────────────────────── - mapd_ev, splan_ev, car_ev, gps_ev, scs_ev, segments = parse_route_logs( + mapd_ev, splan_ev, car_ev, gps_ev, scs_ev, lead_ev, segments = parse_route_logs( canonical ) @@ -1237,11 +1286,12 @@ def main(argv: list[str] | None = None) -> int: # ── Detect changes ────────────────────────────────────────── rows = detect_changes( - mapd_ev, splan_ev, car_ev, scs_ev, osm_ways, gps_timeline, base_time + mapd_ev, splan_ev, car_ev, scs_ev, osm_ways, gps_timeline, base_time, + lead_events=lead_ev, ) # ── Output ───────────────────────────────────────────────── - print_header(canonical, len(mapd_ev), len(splan_ev), len(gps_ev), len(scs_ev)) + print_header(canonical, len(mapd_ev), len(splan_ev), len(gps_ev), len(scs_ev), len(lead_ev)) print_table(rows, not args.kmh) stale = sum(1 for r in rows if r.stale) diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index bf34dfd74..3226b6845 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -114,11 +114,129 @@ def test_new_source_limit_clears_override_until_gas_release(): assert controller.overridden_speed == pytest.approx(mph(65)) assert controller.override_slc + + # --- Dropout / Fallback Test Condition --- + controller.starpilot_toggles = make_toggles(slc_fallback_set_speed=True) + # No limit available → falls back to v_cruise (75 mph) with source "None". + # Override persists because target_to_use resolves to last_valid_limit (45 mph) which is + # below overridden_speed (65 mph) — the sticky override_slc chain stays True. + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.target == pytest.approx(mph(75)) + assert controller.source == "None" + assert controller.overridden_speed == pytest.approx(mph(65)) + assert controller.override_slc + + # Recovery to a confirmed limit (55 mph) clears the override: this is a genuinely new + # speed zone (55 != last_valid 45), so clear_override_for_source_limit fires correctly. + controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.target == pytest.approx(mph(55)) + assert controller.source == "Dashboard" + assert controller.overridden_speed == 0 + assert not controller.override_slc + + # --- Override Clipping Check (set-speed fallback) --- + # Separate controller: active override, then fallback to v_cruise that is BELOW the override. + # overridden_speed clips to v_cruise (override_slc stays True via sticky chain, but + # np.clip clamps overridden_speed to the new target+offset). + clip_controller = make_controller(slc_fallback_set_speed=True) + try: + clip_controller.source = "Dashboard" + clip_controller.target = mph(55) + clip_controller.previous_source = "Dashboard" + clip_controller.previous_target = mph(55) + clip_controller.last_valid_limit = mph(55) + clip_controller.override_slc = True + clip_controller.overridden_speed = mph(65) + + sm_no_gas = make_sm(gas_pressed=False) + # v_cruise = 30 mph (below last_valid 55), so target_to_use returns mph(30). + # override_slc sticky: overridden_speed=65 > 30+0=30 > 0 — still True from chain. + # np.clip(65, 30, 30) = 30, so overridden_speed clips to mph(30). + clip_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(30), sm_no_gas) + clip_controller.update_override(mph(30), 0.0, mph(30), 0.0, sm_no_gas) + + assert clip_controller.target == pytest.approx(mph(30)) + # Clipped to v_cruise — not locked at mph(55) or mph(65) + assert clip_controller.overridden_speed == pytest.approx(mph(30)) + assert clip_controller.override_slc + finally: + clip_controller.shutdown() + + # --- Lost Speed Limit (no fallback) clears target to 0 --- + # When all limit sources drop to 0 with no fallback, target becomes 0 + # and override_slc is False (target_to_use=0, chain evaluates False). + lost_controller = make_controller(slc_fallback_set_speed=False, slc_fallback_previous_speed_limit=False) + try: + lost_controller.source = "Dashboard" + lost_controller.target = mph(45) + lost_controller.previous_source = "Dashboard" + lost_controller.previous_target = mph(45) + + sm_on = make_sm(gas_pressed=False) + lost_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm_on) + lost_controller.update_override(mph(75), 0.0, mph(65), 0.0, sm_on) + + assert lost_controller.target == 0 + assert lost_controller.overridden_speed == 0 + assert not lost_controller.override_slc + finally: + lost_controller.shutdown() finally: controller.shutdown() def test_unconfirmed_lower_limit_keeps_existing_override(): + # First, verify startup behavior where target is 0 and priority limit is detected + startup_controller = make_controller( + speed_limit_priority1="Dashboard", + slc_fallback_previous_speed_limit=True, + ) + try: + startup_controller.previous_target = mph(55) + startup_controller.previous_source = "Dashboard" + startup_controller.target = 0 + + sm = make_sm(gas_pressed=False) + startup_controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + + assert startup_controller.target == pytest.approx(mph(45)) + assert startup_controller.source == "Dashboard" + finally: + startup_controller.shutdown() + + # Verify Bug 3: Fallback transitions should bypass confirmation checks + fallback_confirm_controller = make_controller( + slc_fallback_set_speed=True, + speed_limit_confirmation_higher=True + ) + try: + fallback_confirm_controller.source = "Dashboard" + fallback_confirm_controller.target = mph(35) + fallback_confirm_controller.previous_target = mph(35) + + sm = make_sm(gas_pressed=False) + fallback_confirm_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(60), mph(35), sm) + + assert fallback_confirm_controller.target == pytest.approx(mph(60)) + assert fallback_confirm_controller.source == "None" + assert fallback_confirm_controller.unconfirmed_speed_limit == 0 + finally: + fallback_confirm_controller.shutdown() + + # Verify Bug 1: Boundaries are correctly mapped and not falling back to 0 + boundary_controller = make_controller() + boundary_controller.starpilot_toggles.speed_limit_offset1 = 1.0 + boundary_controller.starpilot_toggles.speed_limit_offset2 = 2.0 + + # Exact boundary speed: 11.2 m/s is the *start* of band 2 (25–34 mph range). + # With low <= target < high: 11.2 <= 11.2 < 15.2 → True → maps to offset2 (not 0). + offset = boundary_controller.get_offset(11.2) + assert offset != 0.0 + controller = make_controller(speed_limit_confirmation_lower=True) try: controller.source = "Dashboard" diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 27581a6ab..5a3e30c09 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -73,6 +73,7 @@ class SpeedLimitController: self.mapbox_token = self.starpilot_planner.params.get("MapboxSecretKey", encoding="utf-8") self.previous_target = self.starpilot_planner.params.get_float("PreviousSpeedLimit") + self.last_valid_limit = self.previous_target if self.previous_target > 0 else 0 self.executor = ThreadPoolExecutor(max_workers=1) self.mapbox_future = None @@ -90,11 +91,21 @@ class SpeedLimitController: return self.target == 0 and bool(getattr(self.starpilot_toggles, "slc_fallback_experimental_mode", False)) @property - def offset(self): + def target_to_use(self): + if self.source == "None" and self.target > 0 and self.last_valid_limit > 0: + if self.target >= self.last_valid_limit: + return self.last_valid_limit + return self.target + + def get_offset(self, target_speed): if self.starpilot_toggles is None: return 0 offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL - return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low < self.target < high), 0) + return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low <= target_speed < high), 0) + + @property + def offset(self): + return self.get_offset(self.target) @property def override_mode_enabled(self): @@ -103,7 +114,8 @@ class SpeedLimitController: return self.starpilot_toggles.speed_limit_controller_override_manual or self.starpilot_toggles.speed_limit_controller_override_set_speed def override_active(self, v_ego, gas_pressed): - target_with_offset = self.target + self.offset + target_to_use = self.target_to_use + target_with_offset = target_to_use + self.get_offset(target_to_use) if target_with_offset <= 0 or not self.override_mode_enabled: return False return self.overridden_speed > target_with_offset or (gas_pressed and v_ego > target_with_offset) @@ -113,6 +125,8 @@ class SpeedLimitController: return if not had_override and self.overridden_speed <= 0: return + if abs(desired_target - self.last_valid_limit) < 0.1: + return # A new posted limit starts a new segment, so the previous segment's gas override # should not carry through until the driver releases and reapplies the pedal. @@ -122,7 +136,7 @@ class SpeedLimitController: self.override_requires_gas_release = True def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm): - if not self.starpilot_planner.gps_valid or not self.mapbox_token or (sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45: + if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45: self.mapbox_limit = 0 self.segment_distance = 0 return @@ -144,6 +158,7 @@ class SpeedLimitController: try: if not is_url_pingable(self.mapbox_host): self.segment_distance = 1000 + successful = True return None if time_validated: @@ -156,7 +171,7 @@ class SpeedLimitController: }) self.mapbox_requests["total_requests"] += 1 - self.starpilot_planner.params.put_nonblocking("MapBoxRequests", self.mapbox_requests) + self.starpilot_planner.params.put_nonblocking("MapBoxRequests", json.dumps(self.mapbox_requests)) current_bearing = self.starpilot_planner.gps_position.get("bearing") current_latitude = self.starpilot_planner.gps_position.get("latitude") @@ -217,15 +232,20 @@ class SpeedLimitController: segment_distance = distances[0] speed_data = annotation.get("maxspeed", []) - speed_limit_kph = 0 if speed_data: first_segment_speed = speed_data[0] - speed_limit_kph = (first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0) or 0 - - if speed_limit_kph > 0: - self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS - self.segment_distance = segment_distance - return + try: + raw_speed = float(first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0.0) + except (ValueError, TypeError): + raw_speed = 0.0 + unit = first_segment_speed.get("unit", "km/h") + if raw_speed > 0: + if unit == "mph": + self.mapbox_limit = raw_speed * CV.MPH_TO_MS + else: + self.mapbox_limit = raw_speed * CV.KPH_TO_MS + self.segment_distance = segment_distance + return self.mapbox_limit = 0 self.segment_distance = v_ego @@ -275,12 +295,12 @@ class SpeedLimitController: self.previous_target = desired_target self.previous_road_name = current_road_name - elif desired_target < self.target and not self.starpilot_toggles.speed_limit_confirmation_lower: + elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower): self.source = desired_source self.target = desired_target self.clear_override_for_source_limit(desired_source, desired_target, had_override) - elif desired_target > self.target and not self.starpilot_toggles.speed_limit_confirmation_higher: + elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher): self.source = desired_source self.target = desired_target self.clear_override_for_source_limit(desired_source, desired_target, had_override) @@ -300,7 +320,7 @@ class SpeedLimitController: self.previous_target = self.target self.previous_road_name = current_road_name - self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target) + self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target)) def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False): self.update_map_speed_limit(v_ego, sm) @@ -343,7 +363,7 @@ class SpeedLimitController: desired_source = "None" desired_target = 0 - if desired_target == 0 or self.target == 0: + if desired_target == 0: if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.starpilot_toggles.slc_mapbox_filler: self.get_mapbox_speed_limit(now, time_validated, v_ego, sm) @@ -351,7 +371,7 @@ class SpeedLimitController: desired_source = "Mapbox" desired_target = self.mapbox_limit - if not display_only and (desired_target == 0 or self.target == 0): + if not display_only and desired_target == 0: if self.previous_target > 0 and self.starpilot_toggles.slc_fallback_previous_speed_limit: desired_source = self.previous_source desired_target = self.previous_target @@ -384,12 +404,16 @@ class SpeedLimitController: if abs(desired_target - self.previous_target) >= 1 or (current_road_name != self.previous_road_name and current_road_name != ""): self.handle_limit_change(desired_source, desired_target, current_road_name, v_ego, sm) - elif desired_source != self.source and abs(desired_target - self.target) < 1: + elif desired_source != self.source and (abs(desired_target - self.target) < 1 or self.target == 0): self.source = desired_source + self.target = desired_target else: self.speed_limit_changed_timer = 0 self.unconfirmed_speed_limit = 0 + if self.source != "None" and self.target > 0: + self.last_valid_limit = self.target + self._slc_adopt_counter += 1 if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"): self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit") @@ -402,7 +426,7 @@ class SpeedLimitController: self.previous_target = desired_target self.speed_limit_changed_timer = 0 self.unconfirmed_speed_limit = 0 - self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target) + self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target)) self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset) def update_map_speed_limit(self, v_ego, sm): @@ -439,14 +463,16 @@ class SpeedLimitController: if not sm["carState"].gasPressed: self.override_requires_gas_release = False - self.override_slc = self.overridden_speed > self.target + self.offset > 0 and v_ego > self.target + self.offset - self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > self.target + self.offset > 0 + target_to_use = self.target_to_use + offset = self.get_offset(target_to_use) + self.override_slc = self.override_slc and self.overridden_speed > target_to_use + offset > 0 + self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0 if self.override_slc: if self.starpilot_toggles.speed_limit_controller_override_manual: if sm["carState"].gasPressed: self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed) - self.overridden_speed = float(np.clip(self.overridden_speed, self.target + self.offset, v_cruise + v_cruise_diff)) + self.overridden_speed = float(np.clip(self.overridden_speed, target_to_use + offset, v_cruise + v_cruise_diff)) elif self.starpilot_toggles.speed_limit_controller_override_set_speed: self.overridden_speed = v_cruise + v_cruise_diff