I cannot stop planning long

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -9,31 +9,9 @@ from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer
 from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams
 from opendbc.car.vehicle_model import VehicleModel
 
-_PREAP_RATE_LIMIT_BP = [0., 5., 15.]
-_PREAP_RATE_LIMIT_UP = [5., 0.8, 0.15]
-_PREAP_RATE_LIMIT_DOWN = [5., 3.5, 0.4]
-_PREAP_EPS_ANGLE_ERROR_MAX = 20.0
-
-
-def _apply_preap_steer_angle_limits(desired_angle: float, last_angle: float, v_ego: float,
-                                    steering_angle: float, lat_active: bool) -> float:
-  if not lat_active:
-    return steering_angle
-
-  steer_up = (last_angle * desired_angle > 0.) and (abs(desired_angle) > abs(last_angle))
-  rate_limit = _PREAP_RATE_LIMIT_UP if steer_up else _PREAP_RATE_LIMIT_DOWN
-  max_angle_diff = float(np.interp(v_ego, _PREAP_RATE_LIMIT_BP, rate_limit))
-
-  apply_angle = float(np.clip(desired_angle, last_angle - max_angle_diff, last_angle + max_angle_diff))
-  # Match the older Tesla unity controller and stay close to measured EPAS angle to avoid rejection.
-  return float(np.clip(apply_angle,
-                       steering_angle - _PREAP_EPS_ANGLE_ERROR_MAX,
-                       steering_angle + _PREAP_EPS_ANGLE_ERROR_MAX))
-
-
 def get_safety_CP():
   from opendbc.car.tesla.interface import CarInterface
-  return CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP if getattr(get_safety_CP, "_preap", False) else CAR.TESLA_MODEL_Y)
+  return CarInterface.get_non_essential_params(CAR.TESLA_MODEL_Y)
 
 
 class CarController(CarControllerBase):
@@ -45,15 +23,15 @@ class CarController(CarControllerBase):
     self.preap_long = None
     self.stock_cc = None
 
+    # Vehicle model used for lateral limiting
+    self.VM = VehicleModel(get_safety_CP())
+
     if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
-      get_safety_CP._preap = True
       self.tesla_can = init_preap_can(dbc_names)
       self.preap_long = PreAPLongController()
       self.stock_cc = StockCCSpoofer()
-
-    # Vehicle model used for lateral limiting
-    self.VM = VehicleModel(get_safety_CP())
-    get_safety_CP._preap = False
+      from opendbc.car.tesla.interface import CarInterface
+      self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP))
 
   def update(self, CC, CS, now_nanos, starpilot_toggles):
     if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
@@ -117,8 +95,9 @@ class CarController(CarControllerBase):
         CS.engagement.pedal_speed_kph = 0.0
 
     if self.frame % 2 == 0:
-      self.apply_angle_last = _apply_preap_steer_angle_limits(
-        actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CS.out.steeringAngleDeg, lat_active,
+      self.apply_angle_last = apply_steer_angle_limits_vm(
+        actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
+        lat_active, CarControllerParams, self.VM,
       )
       cntr = (self.frame // 2) % 16
       can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_last, lat_active))

selfdrive/controls/lib/lead_behavior.py

@@ -7,6 +7,13 @@ VISION_LEAD_TRACK_MIN_DISTANCE = 25.0
 VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75
 VISION_LEAD_TRACK_CLOSING_GAIN = 0.20
 VISION_LEAD_TRACK_CLOSING_CAP = 2.50
+RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0
+RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0
+RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95
+RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET = 0.35
+RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET = 0.90
+RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35
+RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70
 
 
 def should_track_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float,
@@ -26,6 +33,27 @@ def should_track_lead(lead_status: bool, lead_distance: float, model_length: flo
   return float(lead_distance) < min(model_limit, vision_limit)
 
 
+def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *,
+                                       radar: bool = False, lead_brake: float = 0.0,
+                                       lead_prob: float = 0.0) -> bool:
+  if radar or float(t_follow) <= 0.0 or float(v_ego) < RADARLESS_MATCHED_FOLLOW_MIN_SPEED:
+    return False
+  if float(lead_prob) < RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB:
+    return False
+  if float(lead_brake) > RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE:
+    return False
+
+  relative_speed = float(v_ego) - float(v_lead)
+  if abs(relative_speed) > RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED:
+    return False
+
+  actual_headway = float(lead_distance) / max(float(v_ego), 1e-3)
+  min_headway = max(RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY,
+                    float(t_follow) - RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET)
+  max_headway = float(t_follow) + RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET
+  return min_headway <= actual_headway <= max_headway
+
+
 def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
                                   v_cruise: float | None = None) -> float:
   gap_error = lead_distance - desired_gap

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -13,7 +13,7 @@ from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias
+from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias, is_radarless_matched_follow_window
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 
@@ -77,6 +77,8 @@ FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0
 FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25
 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0
 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9
+RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN = 2.5
+RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.10
 
 # Function to get parameter value based on current speed
 def get_speed_based_param(speed_mph, param_array):
@@ -547,6 +549,25 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego)
     return lead_xv
 
+  @staticmethod
+  def get_radarless_matched_follow_cruise_hysteresis(lead, v_ego, t_follow):
+    if lead is None or not lead.status:
+      return 0.0
+
+    if not is_radarless_matched_follow_window(
+      v_ego,
+      lead.dRel,
+      lead.vLead,
+      t_follow,
+      radar=bool(getattr(lead, "radar", False)),
+      lead_brake=max(0.0, -float(getattr(lead, "aLeadK", 0.0))),
+      lead_prob=float(getattr(lead, "modelProb", 0.0)),
+    ):
+      return 0.0
+
+    return max(RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN,
+               RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego))
+
   def set_accel_limits(self, min_a, max_a):
     # TODO this sets a max accel limit, but the minimum limit is only for cruise decel
     # needs refactor
@@ -585,6 +606,11 @@ class LongitudinalMpc:
                                  v_lower,
                                  v_upper)
       cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
+      prev_source = self.source
+      if prev_source == 'lead0':
+        cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow)
+      elif prev_source == 'lead1':
+        cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow)
       if tracking_lead and lead_one.status:
         desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
         closing_speed = max(0.0, v_ego - lead_one.vLead)

selfdrive/controls/lib/longitudinal_planner.py

@@ -13,6 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
@@ -133,8 +134,26 @@ STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB = 0.7
 STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR = 0.24
 STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY = 1.05
 STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET = 0.90
+STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED = -1.2
+STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED = 2.2
+STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL = 0.12
+STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL = 0.08
 STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL = 0.18
 STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL = 0.32
+STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN = 0.45
+STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY = 1.80
+STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE = 0.15
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE = 80.0
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.1
+FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 2.0
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB = 0.95
+FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC = 7.5
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55
+FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00
+FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05
+FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18
+FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [3.5, 3.5, 3.2]
@@ -656,37 +675,149 @@ class LongitudinalPlanner:
     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]))
 
-  def get_matched_follow_brake_cap(self, lead, v_ego, base_t_follow):
+  def lead_is_matched_follow_window(self, lead, v_ego, base_t_follow):
     if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
-      return None
+      return False
 
-    closing_speed = max(0.0, float(v_ego) - float(lead.vLead))
-    if not (STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED):
-      return None
+    relative_speed = float(v_ego) - float(lead.vLead)
+    if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED):
+      return False
 
     lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
     if lead_brake > STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE:
-      return None
+      return False
 
-    lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0))
-    if not bool(getattr(lead, "radar", False)) and lead_prob < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB:
-      return None
+    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 not is_radarless_matched_follow_window(
+      v_ego,
+      lead.dRel,
+      lead.vLead,
+      base_t_follow,
+      radar=lead_radar,
+      lead_brake=lead_brake,
+      lead_prob=lead_prob,
+    ):
+      return False
 
     actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
     if actual_headway < max(STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY, float(base_t_follow) - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET):
-      return None
+      return False
     if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET:
+      return False
+    return True
+
+  def get_matched_follow_control_lead(self, v_ego, t_follow):
+    if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow):
+      return self.lead_two
+    if self.lead_is_matched_follow_window(self.lead_one, v_ego, t_follow):
+      return self.lead_one
+    if self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow):
+      return self.lead_two
+    return None
+
+  def get_follow_control_lead(self, lead_control_active, v_ego, t_follow):
+    matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow)
+    if matched_follow_lead is not None:
+      return matched_follow_lead
+
+    if not lead_control_active:
       return None
 
+    if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow):
+      return self.lead_two
+
+    if self.lead_one.status:
+      return self.lead_one
+    if self.lead_two.status:
+      return self.lead_two
+    return None
+
+  def lead_is_spacious_brake_cap_window(self, lead, v_ego, base_t_follow):
+    if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
+      return False
+
+    relative_speed = float(v_ego) - float(lead.vLead)
+    if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED):
+      return False
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE:
+      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 < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB:
+      return False
+
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    if actual_headway < max(float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN,
+                            STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY):
+      return False
+    if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET:
+      return False
+    return True
+
+  def get_matched_follow_brake_cap(self, lead, v_ego, base_t_follow):
+    if not (
+      self.lead_is_matched_follow_window(lead, v_ego, base_t_follow) or
+      self.lead_is_spacious_brake_cap_window(lead, v_ego, base_t_follow)
+    ):
+      return None
+
+    relative_speed = float(v_ego) - float(lead.vLead)
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
     cap_decel = float(np.interp(
-      closing_speed,
-      [STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED, STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED],
-      [STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL, STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL],
+      relative_speed,
+      [STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED, 0.0, STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED],
+      [STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL,
+       STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL,
+       STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL],
     ))
     headway_deficit = float(np.clip((float(base_t_follow) - actual_headway) / STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET, 0.0, 1.0))
     cap_decel = min(STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL, cap_decel + 0.05 * headway_deficit)
     return -cap_decel
 
+  def get_far_lead_brake_cap(self, lead, v_ego, base_t_follow):
+    if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
+      return None
+
+    if bool(getattr(lead, "radar", False)):
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB:
+      return None
+
+    relative_speed = float(v_ego) - float(lead.vLead)
+    if not (FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED):
+      return None
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE:
+      return None
+
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    headway_margin = actual_headway - float(base_t_follow)
+    if float(lead.dRel) < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN:
+      return None
+
+    ttc = float(lead.dRel) / max(relative_speed, 1e-3)
+    if ttc < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC:
+      return None
+
+    cap_decel = float(np.interp(
+      relative_speed,
+      [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED],
+      [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL],
+    ))
+    relax_decel = float(np.interp(
+      headway_margin,
+      [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN],
+      [0.0, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL],
+    ))
+    return -max(0.0, cap_decel - relax_decel)
+
   @staticmethod
   def raw_close_lead_needs_control(lead, v_ego):
     if lead is None or not lead.status:
@@ -720,6 +851,7 @@ class LongitudinalPlanner:
       accel_coast = ACCEL_MAX
 
     v_ego = get_planner_v_ego(self.CP, sm['carState'])
+    scene_v_ego = float(sm['carState'].vEgo)
     v_cruise = sm['starpilotPlan'].vCruise
     if not np.isfinite(v_cruise):
       cloudlog.error(f"Longitudinal planner received non-finite vCruise={v_cruise}, falling back to v_ego={v_ego:.2f}")
@@ -784,7 +916,7 @@ class LongitudinalPlanner:
     tracking_lead = bool(sm['starpilotPlan'].trackingLead)
     self.lead_one = sm['radarState'].leadOne
     self.lead_two = sm['radarState'].leadTwo
-    raw_close_lead_control = any(self.raw_close_lead_needs_control(lead, v_ego) for lead in (self.lead_one, self.lead_two))
+    raw_close_lead_control = any(self.raw_close_lead_needs_control(lead, scene_v_ego) for lead in (self.lead_one, self.lead_two))
     # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead
     # safety path so ACC/chill does not ignore a visible lead during that debounce.
     lead_control_active = tracking_lead or raw_close_lead_control
@@ -897,13 +1029,14 @@ class LongitudinalPlanner:
     closing_speed = 0.0
     if lead_one_active:
       desired_gap = float(desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow))
+      scene_desired_gap = float(desired_follow_distance(scene_v_ego, self.lead_one.vLead, effective_t_follow))
       close_gap_window = max(UNCERT_PANIC_MAX_GAP_BUFFER_MIN,
                              UNCERT_PANIC_MAX_GAP_BUFFER_GAIN * float(v_ego))
-      panic_close_window = float(self.lead_one.dRel) <= desired_gap + close_gap_window
-      closing_speed = max(0.0, v_ego - self.lead_one.vLead)
+      panic_close_window = float(self.lead_one.dRel) <= scene_desired_gap + close_gap_window
+      closing_speed = max(0.0, scene_v_ego - self.lead_one.vLead)
       closing_fast = closing_speed >= max(
         UNCERT_PANIC_MIN_CLOSING_SPEED,
-        UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN * float(v_ego),
+        UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN * float(scene_v_ego),
       )
 
     # Only bypass lead smoothing when we're closing meaningfully and already
@@ -916,16 +1049,27 @@ class LongitudinalPlanner:
     steady_follow_filter_floor = 0.0
     if lead_one_active and desired_gap is not None and not panic_bypass:
       lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0)))
-      lead_prob = float(getattr(self.lead_one, "modelProb", 1.0 if bool(getattr(self.lead_one, "radar", False)) else 0.0))
-      actual_headway = float(self.lead_one.dRel) / max(float(v_ego), 1e-3)
+      lead_radar = bool(getattr(self.lead_one, "radar", False))
+      lead_prob = float(getattr(self.lead_one, "modelProb", 1.0 if lead_radar else 0.0))
+      actual_headway = float(self.lead_one.dRel) / max(scene_v_ego, 1e-3)
       matched_follow_window = (
-        v_ego >= STEADY_FOLLOW_SMOOTHING_MIN_SPEED and
-        STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED and
-        actual_headway >= max(STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY,
-                              effective_t_follow - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET) and
-        actual_headway <= effective_t_follow + STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET and
-        lead_brake <= STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE and
-        (bool(getattr(self.lead_one, "radar", False)) or lead_prob >= STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB)
+        is_radarless_matched_follow_window(
+          scene_v_ego,
+          self.lead_one.dRel,
+          self.lead_one.vLead,
+          effective_t_follow,
+          radar=lead_radar,
+          lead_brake=lead_brake,
+          lead_prob=lead_prob,
+        ) or (
+          lead_radar and
+          scene_v_ego >= STEADY_FOLLOW_SMOOTHING_MIN_SPEED and
+          STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED and
+          actual_headway >= max(STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY,
+                                effective_t_follow - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET) and
+          actual_headway <= effective_t_follow + STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET and
+          lead_brake <= STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE
+        )
       )
       if matched_follow_window:
         steady_follow_filter_floor = STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR
@@ -1134,12 +1278,20 @@ class LongitudinalPlanner:
     if vision_brake_cap_active:
       output_accel_min = min(output_accel_min, vision_cap_accel_min)
 
-    if lead_one_active and not panic_bypass:
-      matched_follow_brake_cap = self.get_matched_follow_brake_cap(self.lead_one, v_ego, sm['starpilotPlan'].tFollow)
+    follow_control_lead = self.get_follow_control_lead(lead_control_active, scene_v_ego, effective_t_follow)
+    if follow_control_lead is not None and not panic_bypass:
+      matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow)
       if matched_follow_brake_cap is not None:
         self.a_desired = max(self.a_desired, matched_follow_brake_cap)
         output_a_target = max(output_a_target, matched_follow_brake_cap)
 
+    comfort_lead = self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one
+    if comfort_lead is not None and not panic_bypass:
+      far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow)
+      if far_lead_brake_cap is not None:
+        self.a_desired = max(self.a_desired, far_lead_brake_cap)
+        output_a_target = max(output_a_target, far_lead_brake_cap)
+
     output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
     output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
 

selfdrive/controls/tests/test_lead_behavior.py

@@ -1,5 +1,6 @@
 from openpilot.selfdrive.controls.lib.lead_behavior import (
   get_tracked_lead_catchup_bias,
+  is_radarless_matched_follow_window,
   should_track_lead,
   should_disable_far_lead_throttle,
 )
@@ -69,3 +70,19 @@ def test_should_track_lead_accepts_closer_vision_only_highway_lead():
 
 def test_should_track_lead_accepts_fast_closing_vision_lead_early():
   assert should_track_lead(True, 90.0, 140.0, 6.0, 20.0, v_lead=0.0, radar=False)
+
+
+def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow():
+  assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
+
+
+def test_radarless_matched_follow_window_rejects_large_relative_speed():
+  assert not is_radarless_matched_follow_window(31.0, 48.0, 27.5, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
+
+
+def test_radarless_matched_follow_window_rejects_far_headway():
+  assert not is_radarless_matched_follow_window(31.0, 82.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
+
+
+def test_radarless_matched_follow_window_rejects_low_confidence_lead():
+  assert not is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.55)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -995,3 +995,100 @@ def test_near_speed_follow_soft_brake_cap_limits_matched_follow_pulse():
 
   assert planner.mpc.filter_time_factor >= 0.24
   assert planner.output_a_target >= -0.33
+
+
+def test_near_speed_follow_soft_brake_cap_covers_slightly_opening_lead():
+  v_ego = 29.58
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=44.44, v_lead=30.65, radar=False, model_prob=0.98)
+
+  cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45)
+
+  assert cap is not None
+  assert cap >= -0.14
+  assert cap <= -0.06
+
+
+def test_near_speed_follow_soft_brake_cap_extends_to_spacious_modest_closing():
+  v_ego = 23.69
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=49.48, v_lead=21.64, a_lead=-0.014, radar=False, model_prob=0.998)
+
+  cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45)
+
+  assert cap is not None
+  assert cap >= -0.33
+  assert cap <= -0.22
+
+
+def test_near_speed_follow_soft_brake_cap_uses_raw_vehicle_speed_when_cluster_runs_high():
+  v_ego = 23.0073
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=47.52, v_lead=21.68, a_lead=-0.0646, radar=False, model_prob=0.998),
+  )
+  sm["carState"].vEgoCluster = 23.6931
+  sm["starpilotPlan"].maxAcceleration = 0.61
+
+  for _ in range(6):
+    planner.update(sm, make_toggles())
+
+  assert not planner.lead_is_matched_follow_window(sm["radarState"].leadOne, sm["carState"].vEgoCluster, 1.45)
+  assert planner.output_a_target > -0.35
+  assert planner.output_a_target < -0.22
+
+
+def test_near_speed_follow_soft_brake_cap_rejects_close_gap_even_with_modest_closing():
+  v_ego = 37.19
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=34.70, v_lead=35.88, radar=False, model_prob=0.99)
+
+  cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.0)
+
+  assert cap is None
+
+
+def test_follow_control_lead_prefers_active_lead1_for_matched_follow():
+  v_ego = 23.3
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6)
+  planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98)
+  planner.mpc.source = "lead1"
+
+  follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45)
+
+  assert follow_lead is planner.lead_two
+
+
+def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch():
+  v_ego = 27.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99)
+
+  follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45)
+
+  assert follow_lead is planner.lead_one
+
+
+def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead():
+  v_ego = 32.37
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=82.07, v_lead=30.63, a_lead=-0.01, radar=False, model_prob=0.99)
+
+  cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.70)
+
+  assert cap is not None
+  assert cap > -0.2
+  assert cap < -0.05

selfdrive/ui/layouts/settings/starpilot/system_settings.py

@@ -41,6 +41,7 @@ from openpilot.selfdrive.ui.layouts.settings.starpilot.aethergrid import (
   draw_soft_card,
   draw_tab_card,
 )
+from openpilot.starpilot.common.connect_server import prepare_konik_server_switch
 
 LEGACY_STARPILOT_PARAM_RENAMES = {
   "FrogPilotApiToken": "StarPilotApiToken",
@@ -981,7 +982,7 @@ class StarPilotSystemLayout(_SettingsPage):
     return self._params.get_bool("UseKonikServer")
 
   def _on_konik_toggle(self, state):
-    self._params.put_bool("UseKonikServer", state)
+    prepare_konik_server_switch(state, self._params)
     cache_path = Path("/cache/use_konik")
     if state:
       cache_path.parent.mkdir(parents=True, exist_ok=True)

selfdrive/ui/mici/layouts/settings/device.py

@@ -23,6 +23,7 @@ from openpilot.selfdrive.ui.ui_state import device, ui_state
 from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.system.ui.widgets.html_render import HtmlModal, HtmlRenderer
 from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
+from openpilot.starpilot.common.connect_server import prepare_konik_server_switch
 
 
 class ReviewTermsPage(TermsPage, NavScroller):
@@ -152,7 +153,7 @@ class ConnectServerBigButton(BigButton):
 
   def _apply_selection(self, selection: str):
     use_konik = selection == self._KONIK_OPTION
-    self._params.put_bool("UseKonikServer", use_konik)
+    prepare_konik_server_switch(use_konik, self._params)
 
     toggle_path = _konik_toggle_path()
     toggle_path.parent.mkdir(parents=True, exist_ok=True)

starpilot/common/connect_server.py

@@ -0,0 +1,79 @@
+from pathlib import Path
+
+from openpilot.common.params import Params
+from openpilot.system.athena.registration import register
+from openpilot.system.hardware import PC
+from openpilot.system.hardware.hw import Paths
+
+from openpilot.starpilot.common.starpilot_utilities import use_konik_server
+
+
+def _cache_params_path() -> str:
+  if PC:
+    return str(Path(Paths.comma_home()) / "cache" / "params")
+  return "/cache/params"
+
+
+def _normalize_dongle_id(value):
+  if isinstance(value, bytes):
+    value = value.decode("utf-8", errors="ignore")
+  if value is None:
+    return None
+  value = str(value).strip()
+  return value or None
+
+
+def _read_persisted_stock_dongle_id():
+  persisted_dongle_id_path = Path(Paths.persist_root()) / "comma" / "dongle_id"
+  if not persisted_dongle_id_path.is_file():
+    return None
+  return _normalize_dongle_id(persisted_dongle_id_path.read_text())
+
+
+def _remove_param_from_live_and_cache(key, params, params_cache=None):
+  if params_cache is None:
+    params_cache = Params(_cache_params_path())
+  params.remove(key)
+  params_cache.remove(key)
+
+
+def prepare_konik_server_switch(use_konik, params, params_cache=None):
+  params.put_bool("UseKonikServer", use_konik)
+  if use_konik:
+    _remove_param_from_live_and_cache("KonikDongleId", params, params_cache)
+  else:
+    _remove_param_from_live_and_cache("DongleId", params, params_cache)
+
+
+def _ensure_stock_dongle_id(params):
+  current_dongle_id = _normalize_dongle_id(params.get("DongleId"))
+  konik_dongle_id = _normalize_dongle_id(params.get("KonikDongleId"))
+  stock_dongle_id = _normalize_dongle_id(params.get("StockDongleId"))
+
+  if stock_dongle_id not in (None, konik_dongle_id):
+    return stock_dongle_id
+
+  candidate = _read_persisted_stock_dongle_id()
+  if candidate in (None, konik_dongle_id):
+    candidate = current_dongle_id if current_dongle_id != konik_dongle_id else None
+
+  if candidate is not None and candidate != stock_dongle_id:
+    params.put("StockDongleId", candidate)
+
+  return candidate
+
+
+def sync_konik_dongle_id(params):
+  current_dongle_id = _normalize_dongle_id(params.get("DongleId"))
+  konik_dongle_id = _normalize_dongle_id(params.get("KonikDongleId"))
+  stock_dongle_id = _ensure_stock_dongle_id(params)
+
+  if use_konik_server():
+    if konik_dongle_id is None:
+      konik_dongle_id = _normalize_dongle_id(register(show_spinner=True, register_konik=True))
+      if konik_dongle_id is not None:
+        params.put("KonikDongleId", konik_dongle_id)
+    if konik_dongle_id is not None and current_dongle_id != konik_dongle_id:
+      params.put("DongleId", konik_dongle_id)
+  elif current_dongle_id == konik_dongle_id and stock_dongle_id is not None:
+    params.put("DongleId", stock_dongle_id)

starpilot/common/starpilot_functions.py

@@ -12,72 +12,21 @@ from cereal import messaging
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
 from openpilot.common.time_helpers import system_time_valid
-from openpilot.system.athena.registration import register
 from openpilot.system.hardware import HARDWARE
-from openpilot.system.hardware.hw import Paths
 from openpilot.system.version import get_build_metadata
 
 from openpilot.starpilot.assets.theme_manager import ThemeManager
 from openpilot.starpilot.common.starpilot_backups import backup_starpilot
+from openpilot.starpilot.common.connect_server import sync_konik_dongle_id
 from openpilot.starpilot.common.maps_catalog import normalize_schedule_value, sanitize_selected_locations_csv
 from openpilot.starpilot.common.theme_asset_names import find_matching_theme_asset_file
-from openpilot.starpilot.common.starpilot_utilities import get_starpilot_api_info, is_FrogsGoMoo, is_url_pingable, run_cmd, use_konik_server
+from openpilot.starpilot.common.starpilot_utilities import get_starpilot_api_info, is_FrogsGoMoo, is_url_pingable, run_cmd
 from openpilot.starpilot.common.starpilot_variables import (
   ERROR_LOGS_PATH, STARPILOT_API, FROGS_GO_MOO_PATH, HD_LOGS_PATH, KONIK_LOGS_PATH, MAPS_PATH, THEME_SAVE_PATH,
   StarPilotVariables, get_starpilot_toggles
 )
 
 
-def _normalize_dongle_id(value):
-  if isinstance(value, bytes):
-    value = value.decode("utf-8", errors="ignore")
-  if value is None:
-    return None
-  value = str(value).strip()
-  return value or None
-
-
-def _read_persisted_stock_dongle_id():
-  persisted_dongle_id_path = Path(Paths.persist_root()) / "comma" / "dongle_id"
-  if not persisted_dongle_id_path.is_file():
-    return None
-  return _normalize_dongle_id(persisted_dongle_id_path.read_text())
-
-
-def _ensure_stock_dongle_id(params):
-  current_dongle_id = _normalize_dongle_id(params.get("DongleId"))
-  konik_dongle_id = _normalize_dongle_id(params.get("KonikDongleId"))
-  stock_dongle_id = _normalize_dongle_id(params.get("StockDongleId"))
-
-  if stock_dongle_id not in (None, konik_dongle_id):
-    return stock_dongle_id
-
-  candidate = _read_persisted_stock_dongle_id()
-  if candidate in (None, konik_dongle_id):
-    candidate = current_dongle_id if current_dongle_id != konik_dongle_id else None
-
-  if candidate is not None and candidate != stock_dongle_id:
-    params.put("StockDongleId", candidate)
-
-  return candidate
-
-
-def sync_konik_dongle_id(params):
-  current_dongle_id = _normalize_dongle_id(params.get("DongleId"))
-  konik_dongle_id = _normalize_dongle_id(params.get("KonikDongleId"))
-  stock_dongle_id = _ensure_stock_dongle_id(params)
-
-  if use_konik_server():
-    if konik_dongle_id is None:
-      konik_dongle_id = _normalize_dongle_id(register(show_spinner=True, register_konik=True))
-      if konik_dongle_id is not None:
-        params.put("KonikDongleId", konik_dongle_id)
-    if konik_dongle_id is not None and current_dongle_id != konik_dongle_id:
-      params.put("DongleId", konik_dongle_id)
-  elif current_dongle_id == konik_dongle_id and stock_dongle_id is not None:
-    params.put("DongleId", stock_dongle_id)
-
-
 def seed_desktop_theme_assets():
   params = Params()
   params_memory = Params(memory=True)

starpilot/common/starpilot_variables.py

@@ -457,7 +457,13 @@ class StarPilotVariables:
     current_value = self.params.get_float(key)
     if not math.isfinite(current_value):
       current_value = 0.0
-    if math.isclose(current_value, current_stock, abs_tol=1e-6) or math.isclose(current_stock, 0.0, abs_tol=1e-6):
+
+    # If the stock baseline was missing (0.0/unset), do not stomp an existing
+    # user override. Only backfill the live param when it was still effectively
+    # tracking the old stock value or was itself unset.
+    should_update_live_value = math.isclose(current_value, current_stock, abs_tol=1e-6)
+    should_update_live_value |= math.isclose(current_value, 0.0, abs_tol=1e-6)
+    if should_update_live_value:
       self.params.put_float(key, live_value)
 
     self.params.put_float(stock_key, live_value)

starpilot/common/tests/test_starpilot_functions.py

@@ -1,4 +1,4 @@
-from openpilot.starpilot.common import starpilot_functions as spf
+from openpilot.starpilot.common import connect_server as cs
 
 
 class FakeParams:
@@ -11,15 +11,21 @@ class FakeParams:
   def put(self, key, value):
     self.values[key] = value
 
+  def put_bool(self, key, value):
+    self.values[key] = b"1" if value else b"0"
+
+  def remove(self, key):
+    self.values.pop(key, None)
+
 
 def test_sync_konik_dongle_id_preserves_stock_id_before_switching(monkeypatch, tmp_path):
-  monkeypatch.setattr(spf.Paths, "persist_root", staticmethod(lambda: str(tmp_path)))
-  monkeypatch.setattr(spf, "use_konik_server", lambda: True)
-  monkeypatch.setattr(spf, "register", lambda **kwargs: "konik-dongle")
+  monkeypatch.setattr(cs.Paths, "persist_root", staticmethod(lambda: str(tmp_path)))
+  monkeypatch.setattr(cs, "use_konik_server", lambda: True)
+  monkeypatch.setattr(cs, "register", lambda **kwargs: "konik-dongle")
 
   params = FakeParams({"DongleId": "stock-dongle"})
 
-  spf.sync_konik_dongle_id(params)
+  cs.sync_konik_dongle_id(params)
 
   assert params.get("StockDongleId") == "stock-dongle"
   assert params.get("KonikDongleId") == "konik-dongle"
@@ -32,30 +38,52 @@ def test_sync_konik_dongle_id_restores_stock_id_from_persist(monkeypatch, tmp_pa
   persisted_dongle_id_path.parent.mkdir(parents=True, exist_ok=True)
   persisted_dongle_id_path.write_text("stock-dongle")
 
-  monkeypatch.setattr(spf.Paths, "persist_root", staticmethod(lambda: str(persist_root)))
-  monkeypatch.setattr(spf, "use_konik_server", lambda: False)
+  monkeypatch.setattr(cs.Paths, "persist_root", staticmethod(lambda: str(persist_root)))
+  monkeypatch.setattr(cs, "use_konik_server", lambda: False)
 
   params = FakeParams({
     "DongleId": "konik-dongle",
     "KonikDongleId": "konik-dongle",
   })
 
-  spf.sync_konik_dongle_id(params)
+  cs.sync_konik_dongle_id(params)
 
   assert params.get("StockDongleId") == "stock-dongle"
   assert params.get("DongleId") == "stock-dongle"
 
 
 def test_sync_konik_dongle_id_skips_missing_stock_backup(monkeypatch, tmp_path):
-  monkeypatch.setattr(spf.Paths, "persist_root", staticmethod(lambda: str(tmp_path)))
-  monkeypatch.setattr(spf, "use_konik_server", lambda: False)
+  monkeypatch.setattr(cs.Paths, "persist_root", staticmethod(lambda: str(tmp_path)))
+  monkeypatch.setattr(cs, "use_konik_server", lambda: False)
 
   params = FakeParams({
     "DongleId": "konik-dongle",
     "KonikDongleId": "konik-dongle",
   })
 
-  spf.sync_konik_dongle_id(params)
+  cs.sync_konik_dongle_id(params)
 
   assert params.get("DongleId") == "konik-dongle"
   assert params.get("StockDongleId") is None
+
+
+def test_prepare_konik_server_switch_clears_cached_konik_id():
+  params = FakeParams({"KonikDongleId": "konik-dongle"})
+  params_cache = FakeParams({"KonikDongleId": "konik-dongle"})
+
+  cs.prepare_konik_server_switch(True, params, params_cache)
+
+  assert params.get("UseKonikServer") == b"1"
+  assert params.get("KonikDongleId") is None
+  assert params_cache.get("KonikDongleId") is None
+
+
+def test_prepare_konik_server_switch_clears_cached_stock_id():
+  params = FakeParams({"DongleId": "konik-dongle"})
+  params_cache = FakeParams({"DongleId": "konik-dongle"})
+
+  cs.prepare_konik_server_switch(False, params, params_cache)
+
+  assert params.get("UseKonikServer") == b"0"
+  assert params.get("DongleId") is None
+  assert params_cache.get("DongleId") is None

starpilot/common/tests/test_starpilot_variables.py

@@ -27,3 +27,28 @@ def test_get_starpilot_toggles_uses_last_non_empty_broadcast(monkeypatch):
   assert first.always_on_lateral is True
   assert second.always_on_lateral is True
   assert second.vision_speed_limit_detection is True
+
+
+class _FakeParams:
+  def __init__(self, floats=None):
+    self.floats = dict(floats or {})
+
+  def get_float(self, key):
+    return float(self.floats.get(key, 0.0))
+
+  def put_float(self, key, value):
+    self.floats[key] = float(value)
+
+  def remove(self, key):
+    self.floats.pop(key, None)
+
+
+def test_sync_stock_param_does_not_stomp_existing_custom_value_when_stock_missing():
+  params = _FakeParams({"SteerDelay": 0.35, "SteerDelayStock": 0.0})
+  variables = object.__new__(spv.StarPilotVariables)
+  variables.params = params
+
+  variables._sync_stock_param("SteerDelay", "SteerDelayStock", 0.10)
+
+  assert params.get_float("SteerDelay") == 0.35
+  assert params.get_float("SteerDelayStock") == 0.10

starpilot/controls/starpilot_planner.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import json
 import math
+import time
 
 import cereal.messaging as messaging
 
@@ -10,7 +11,7 @@ from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
-from openpilot.selfdrive.controls.lib.lead_behavior import should_track_lead
+from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window, should_track_lead
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
 
 from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature
@@ -22,6 +23,8 @@ from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollow
 from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
 from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
 
+RADARLESS_TRACK_HOLD_TIME = 0.45
+
 
 def _sanitize_json_value(value):
   if isinstance(value, float):
@@ -71,6 +74,7 @@ class StarPilotPlanner:
     self.gps_location_service = get_gps_location_service(self.params)
 
     self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL)
+    self.radarless_follow_hold_until = 0.0
 
   def shutdown(self):
     self.starpilot_vcruise.slc.shutdown()
@@ -101,7 +105,7 @@ class StarPilotPlanner:
     }
     self.gps_valid = self.gps_position["latitude"] != 0 or self.gps_position["longitude"] != 0
     bearing = self.gps_position["bearing"]
-    if starpilot_toggles.compass and abs(bearing - self._prev_gps_bearing) > 0.5:
+    if getattr(starpilot_toggles, "compass", False) and abs(bearing - self._prev_gps_bearing) > 0.5:
       self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position))
       self._prev_gps_bearing = bearing
 
@@ -164,6 +168,26 @@ class StarPilotPlanner:
       v_lead=self.lead_one.vLead,
       radar=bool(getattr(self.lead_one, "radar", False)),
     )
+    now_t = time.monotonic()
+    lead_radar = bool(getattr(self.lead_one, "radar", False))
+    t_follow = max(float(getattr(self.starpilot_following, "t_follow", 0.0)), 1.45)
+    matched_follow_window = self.lead_one.status and is_radarless_matched_follow_window(
+      v_ego,
+      self.lead_one.dRel,
+      self.lead_one.vLead,
+      t_follow,
+      radar=lead_radar,
+      lead_brake=max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))),
+      lead_prob=float(getattr(self.lead_one, "modelProb", 0.0)),
+    )
+    if matched_follow_window and (following_lead or self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6):
+      self.radarless_follow_hold_until = now_t + RADARLESS_TRACK_HOLD_TIME
+    elif lead_radar or not self.lead_one.status:
+      self.radarless_follow_hold_until = 0.0
+
+    if not following_lead and matched_follow_window and now_t < self.radarless_follow_hold_until:
+      following_lead = True
+
     self.tracking_lead_filter.update(following_lead)
     return self.tracking_lead_filter.x >= THRESHOLD
 

starpilot/ui/qt/offroad/device_settings.cc

@@ -1,6 +1,27 @@
 #include "starpilot/ui/screenrecorder/screenrecorder.h"
 #include "starpilot/ui/qt/offroad/device_settings.h"
 
+namespace {
+
+std::string cacheParamsPath() {
+  return Hardware::PC() ? Path::comma_home() + "/cache/params" : "/cache/params";
+}
+
+void prepareKonikServerSwitch(bool use_konik) {
+  Params params;
+  Params params_cache(cacheParamsPath());
+
+  if (use_konik) {
+    params.remove("KonikDongleId");
+    params_cache.remove("KonikDongleId");
+  } else {
+    params.remove("DongleId");
+    params_cache.remove("DongleId");
+  }
+}
+
+}  // namespace
+
 StarPilotDevicePanel::StarPilotDevicePanel(StarPilotSettingsWindow *parent, bool forceOpen) : StarPilotListWidget(parent), parent(parent) {
   forceOpenDescriptions = forceOpen;
 
@@ -165,6 +186,7 @@ StarPilotDevicePanel::StarPilotDevicePanel(StarPilotSettingsWindow *parent, bool
         filePath = "/cache/use_HD";
       } else if (key == "UseKonikServer") {
         filePath = "/cache/use_konik";
+        prepareKonikServerSwitch(state);
       }
 
       if (!filePath.isEmpty()) {

tools/longitudinal/score_route_longitudinal.py

@@ -5,6 +5,7 @@ import math
 import os
 from collections import defaultdict
 from dataclasses import asdict, dataclass
+from pathlib import Path
 from types import SimpleNamespace
 
 import numpy as np
@@ -17,6 +18,7 @@ from openpilot.selfdrive.controls.lib import longitudinal_planner as longitudina
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from openpilot.tools.lib.logreader import LogReader
+from openpilot.tools.lib.url_file import hash_256
 
 
 for level_name in ("info", "warning", "error", "exception", "event"):
@@ -43,6 +45,10 @@ REQUIRED_SERVICES = {
   "selfdriveState",
   "starpilotPlan",
 }
+COMMA_DATA_CACHE_ROOT = Path("/tmp/comma_download_cache")
+ROUTE_CACHE_REBUILD_ROOT = Path("/tmp/starpilot_route_cache")
+MAX_CACHED_ROUTE_SEGMENTS = 255
+MAX_CONSECUTIVE_CACHE_MISSES = 4
 
 
 @dataclass
@@ -141,8 +147,117 @@ def is_vision_slow_lead(v_ego: float, lead_status: bool, lead_radar: bool, lead_
   )
 
 
-def score_route(route: str, capture_events: bool = False, max_events: int = 200) -> tuple[RouteMetrics, list[dict]]:
-  metrics = RouteMetrics(route=route)
+def cached_rlog_url(route: str, seg: int) -> str:
+  return f"https://commadata2.blob.core.windows.net/commadata2/{route}/{seg}/rlog.zst"
+
+
+def parse_route_identifier(identifier: str) -> tuple[str, str | None]:
+  parts = identifier.split("/")
+  if len(parts) < 2:
+    raise ValueError(f"Invalid route identifier: {identifier}")
+
+  route = "/".join(parts[:2])
+  remainder = parts[2:]
+  if not remainder:
+    return route, None
+
+  first = remainder[0]
+  if first in ("r", "q", "a", "i"):
+    return route, None
+  return route, first
+
+
+def cached_rlog_chunk_paths(route: str, seg: int, cache_root: Path = COMMA_DATA_CACHE_ROOT) -> list[Path]:
+  prefix = f"{hash_256(cached_rlog_url(route, seg))}_"
+  chunk_paths = []
+  for candidate in cache_root.glob(f"{prefix}*"):
+    suffix = candidate.name.removeprefix(prefix)
+    try:
+      chunk_index = float(suffix)
+    except ValueError:
+      continue
+    chunk_paths.append((chunk_index, candidate))
+  return [path for _, path in sorted(chunk_paths, key=lambda item: item[0])]
+
+
+def discover_cached_segment_indexes(route: str,
+                                    cache_root: Path = COMMA_DATA_CACHE_ROOT,
+                                    max_seg: int = MAX_CACHED_ROUTE_SEGMENTS) -> list[int]:
+  found: list[int] = []
+  consecutive_misses = 0
+  for seg in range(max_seg + 1):
+    if cached_rlog_chunk_paths(route, seg, cache_root):
+      found.append(seg)
+      consecutive_misses = 0
+    elif found:
+      consecutive_misses += 1
+      if consecutive_misses >= MAX_CONSECUTIVE_CACHE_MISSES:
+        break
+
+  if not found:
+    raise FileNotFoundError(f"No cached rlogs found for {route} under {cache_root}")
+  return found
+
+
+def select_segment_indexes(route: str, segment_slice: str | None, available_idxs: list[int]) -> list[int]:
+  if segment_slice is None:
+    return available_idxs
+
+  available_set = set(available_idxs)
+  if ":" not in segment_slice:
+    seg = int(segment_slice)
+    if seg not in available_set:
+      raise FileNotFoundError(f"Requested segment {seg} is not available in local cache for {route}")
+    return [seg]
+
+  raw_parts = segment_slice.split(":")
+  if len(raw_parts) > 3:
+    raise ValueError(f"Invalid segment slice: {segment_slice}")
+  parts = [int(part) if part else None for part in raw_parts]
+  while len(parts) < 3:
+    parts.append(None)
+  start, end, step = parts
+  max_seg = max(available_idxs)
+  if end is None or end < 0 or (start is not None and start < 0):
+    universe = list(range(max_seg + 1))
+  else:
+    universe = list(range(max(max_seg, end) + 1))
+  requested = universe[slice(start, end, step)]
+  missing = [seg for seg in requested if seg not in available_set]
+  if missing:
+    raise FileNotFoundError(f"Requested cached segments are unavailable for {route}: {missing[:8]}")
+  return requested
+
+
+def rebuild_cached_rlogs(identifier: str,
+                         cache_root: Path = COMMA_DATA_CACHE_ROOT,
+                         out_root: Path = ROUTE_CACHE_REBUILD_ROOT) -> tuple[str, list[str]]:
+  route, segment_slice = parse_route_identifier(identifier)
+  available_idxs = discover_cached_segment_indexes(route, cache_root)
+  requested_idxs = select_segment_indexes(route, segment_slice, available_idxs)
+  rebuilt_paths: list[str] = []
+
+  for seg in requested_idxs:
+    chunk_paths = cached_rlog_chunk_paths(route, seg, cache_root)
+    if not chunk_paths:
+      raise FileNotFoundError(f"Missing cached rlog chunks for {route}/{seg}")
+
+    out_path = out_root / route / str(seg) / "rlog.zst"
+    expected_size = sum(chunk_path.stat().st_size for chunk_path in chunk_paths)
+    if not out_path.exists() or out_path.stat().st_size != expected_size:
+      out_path.parent.mkdir(parents=True, exist_ok=True)
+      with out_path.open("wb") as out_file:
+        for chunk_path in chunk_paths:
+          with chunk_path.open("rb") as chunk_file:
+            out_file.write(chunk_file.read())
+    rebuilt_paths.append(str(out_path))
+
+  return route, rebuilt_paths
+
+
+def score_route(route: str, capture_events: bool = False, max_events: int = 200, use_cached_rlogs: bool = False) -> tuple[RouteMetrics, list[dict]]:
+  display_route = parse_route_identifier(route)[0] if use_cached_rlogs else route
+  metrics = RouteMetrics(route=display_route)
   planner = None
   toggles = default_toggles()
   state: dict[str, object] = {}
@@ -159,7 +274,13 @@ def score_route(route: str, capture_events: bool = False, max_events: int = 200)
   prev_plan_t: int | None = None
   prev_a_target: float | None = None
 
-  for msg in LogReader(route, sort_by_time=True):
+  logreader_input: str | list[str]
+  if use_cached_rlogs:
+    _, logreader_input = rebuild_cached_rlogs(route)
+  else:
+    logreader_input = route
+
+  for msg in LogReader(logreader_input, sort_by_time=True):
     which = msg.which()
     if which == "carParams" and planner is None:
       planner = LongitudinalPlanner(msg.carParams)
@@ -399,6 +520,8 @@ def parse_args() -> argparse.Namespace:
                       help="Total weight assigned to the priority subset")
   parser.add_argument("--planner-profile", choices=("current", "legacy_panic_bypass"), default="current",
                       help="Planner behavior profile used for local comparison runs")
+  parser.add_argument("--cached-rlogs", action="store_true",
+                      help="Replay from locally cached rlogs rebuilt from the download cache")
   parser.add_argument("--json-out", type=str,
                       help="Optional local output path for JSON results")
   parser.add_argument("--events-out", type=str,
@@ -412,7 +535,10 @@ def main() -> None:
   args = parse_args()
   configure_planner_profile(args.planner_profile)
 
-  scored = [score_route(route, capture_events=bool(args.events_out), max_events=args.max_events_per_route) for route in args.routes]
+  scored = [score_route(route,
+                        capture_events=bool(args.events_out),
+                        max_events=args.max_events_per_route,
+                        use_cached_rlogs=args.cached_rlogs) for route in args.routes]
   results = [result for result, _ in scored]
   events = {route: route_events for (result, route_events), route in zip(scored, args.routes)}
   payload = {