feat(long): RadarDistance lead instability

cereal/custom.capnp

@@ -309,6 +309,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     bypassed @6 :Bool;        # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e)
     comfortStopActive @7 :Bool;   # low-speed comfort decel-to-stop floor currently governing (behind a near-stopped lead)
     comfortStopFloor @8 :Float32; # comfort-stop floor commanded (m/s^2, negative; 0 when not engaged)
+    leadUnstable @9 :Bool;        # RadarDistance lead-instability telemetry (bimodal/bouncing radar lead; informational, no control effect yet)
   }
 
   enum AccelerationPersonality {

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -158,6 +158,7 @@ class LongitudinalPlannerSP:
     acceleration.bypassed = bool(self.accel.bypassed())
     acceleration.comfortStopActive = bool(self.accel.comfort_stop_active())
     acceleration.comfortStopFloor = float(self.accel.comfort_stop_floor())
+    acceleration.leadUnstable = bool(self.radar_distance.lead_unstable())
 
 
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py

@@ -13,6 +13,8 @@ Active only above LOW_SPEED_PASSTHROUGH_V; at/below it returns the raw radarstat
 Default off => stock passthrough.
 """
 
+from collections import deque
+
 from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
@@ -34,6 +36,12 @@ VLEAD_TAU = 0.4                 # s, lag on a speeding-up lead
 _VLEAD_ALPHA = DT_MDL / VLEAD_TAU
 SWITCH_DREL = 8.0              # m, dRel jump that means the radar switched to a different track -> reset the filter
 
+# Lead-instability detector (telemetry only, no control effect yet): flags a bimodal/bouncing radar lead --
+# the signature behind the residual goal-2 firm brakes (vLead jumping between two tracks, dRel stepping).
+# Validated on routes 047f/0480: fires 0.6-0.9% of following frames, concentrated at the firm-brake events.
+STABILITY_WINDOW = 5            # frames (~0.25s @ 20Hz)
+VLEAD_SPREAD = 4.0             # m/s, vLead range over the window above which the lead is "unstable"
+
 
 class _LeadView:
   __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
@@ -123,6 +131,34 @@ class _LeadHold:
     return _LeadView(lead, self._vlead_f)
 
 
+class _LeadStability:
+  # Read-only lead-quality monitor. Watches raw leadOne for the bimodal/bouncing signature (vLead range over a
+  # short window, or repeated dRel track-switch jumps). Pure telemetry -- it conditions nothing, just reports a
+  # flag so we can size how often the residual firm brakes are radar-instability driven before building a fix.
+  def __init__(self):
+    self._v = deque(maxlen=STABILITY_WINDOW)
+    self._d = deque(maxlen=STABILITY_WINDOW)
+    self.unstable = False
+
+  def reset(self):
+    self._v.clear()
+    self._d.clear()
+    self.unstable = False
+
+  def update(self, lead, v_ego: float) -> None:
+    if not lead.status or v_ego < LOW_SPEED_PASSTHROUGH_V:
+      self.reset()
+      return
+    self._v.append(float(lead.vLead))
+    self._d.append(float(lead.dRel))
+    if len(self._v) < STABILITY_WINDOW:
+      self.unstable = False
+      return
+    v_spread = max(self._v) - min(self._v)
+    d_jumps = sum(abs(b - a) > SWITCH_DREL for a, b in zip(self._d, list(self._d)[1:], strict=False))
+    self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2
+
+
 class RadarDistanceController:
   def __init__(self, CP: structs.CarParams, params=None):
     self._CP = CP
@@ -133,6 +169,7 @@ class RadarDistanceController:
     self._vlead_damp_enabled = VLEAD_DAMP_ENABLED   # speed-damp (B) gated off; flicker-hold (A) runs alone
     self._one = _LeadHold()
     self._two = _LeadHold()
+    self._stability = _LeadStability()   # lead-instability telemetry (informational, no control effect)
 
   def _read_params(self) -> None:
     enabled = self._params.get_bool("RadarDistance")
@@ -150,7 +187,11 @@ class RadarDistanceController:
   def enabled(self) -> bool:
     return self._enabled
 
+  def lead_unstable(self) -> bool:
+    return self._stability.unstable
+
   def smooth_radarstate(self, radarstate):
+    self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry; runs every cycle, even when disabled
     if not self._enabled:
       return radarstate
     one = self._one.step(radarstate.leadOne)

sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py

@@ -136,6 +136,35 @@ def test_vlead_damp_gated_off_reports_real_speed():
   assert rising.vLead == pytest.approx(25.0, abs=1e-6)          # no damp -> real speed
 
 
+# --- lead-instability detector (telemetry) -----------------------------------
+
+def test_stability_quiet_on_clean_lead():
+  c = ctrl()
+  for v in (18.0, 18.2, 17.9, 18.1, 18.0, 17.8):                # steady lead, small noise
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=v)))
+  assert not c.lead_unstable()                                  # range < VLEAD_SPREAD -> stable
+
+def test_stability_flags_bimodal_lead():
+  c = ctrl()
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):                        # bouncing between two tracks
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()                                      # range 10 m/s > VLEAD_SPREAD -> unstable
+
+def test_stability_resets_on_dropout():
+  c = ctrl()
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))   # lead drops
+  assert not c.lead_unstable()                                 # buffer cleared -> stable
+
+def test_stability_runs_even_when_disabled():
+  c = ctrl(enabled=False)                                      # telemetry runs regardless of RadarDistance gate
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()
+
+
 def test_obstacle_monotone_during_hold():
   c = ctrl()
   for _ in range(3):