From 4d351bdcadce607f779fb58b7149d86fff0e259c Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 27 Jun 2026 11:04:25 -0700 Subject: [PATCH] feat(long): RadarDistance lead instability --- cereal/custom.capnp | 1 + .../controls/lib/longitudinal_planner.py | 1 + .../lib/radar_distance/radar_distance.py | 41 +++++++++++++++++++ .../tests/test_radar_distance.py | 29 +++++++++++++ 4 files changed, 72 insertions(+) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3d0314d63b..d3dc52dee9 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index c0b988bf8c..4796bac48e 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index c7b410338b..348e32c7bc 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 295b18d422..6a8c6551d5 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/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):