No-uh

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)

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"

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