From b30e52261ebf3a77093d6f10fbce2f5bb0d3e3ab Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 24 Jun 2026 19:32:33 -0700 Subject: [PATCH] fix(long): remove RadarDistance vLead --- .../lib/radar_distance/radar_distance.py | 53 ++++--------------- .../tests/test_radar_distance.py | 20 ++----- 2 files changed, 15 insertions(+), 58 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index e53ae48964..ae0b9064a8 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -4,13 +4,15 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. -RadarDistance: smooths the lead the longitudinal MPC follows, without ever reporting a farther-or-faster -lead than reality (so braking is always >= stock, never weaker). Three jobs, all above LOW_SPEED_PASSTHROUGH_V: - - flicker-hold: keep a just-dropped, recently-sustained lead alive through a brief radar dropout. - - lead-speed smoothing: lag the lead ACCELERATING (damps the catch-up surge -> less rubber-band) while - passing the lead SLOWING straight through (instant brake). -At/below LOW_SPEED_PASSTHROUGH_V (stop/creep) it returns the raw radarstate unchanged -> byte-stock stops. -Default off => stock passthrough. +RadarDistance: keep a just-dropped, recently-sustained lead alive through a brief radar dropout (flicker-hold), +so the MPC does not lose+regain a flickering lead. The held lead is obstacle-monotone (held obstacle <= last +real <= stock) -> braking is always >= stock, never weaker. Active only above LOW_SPEED_PASSTHROUGH_V; at/below +it (stop/creep) it returns the raw radarstate unchanged -> byte-stock stops. Default off => stock passthrough. + +NOTE: an earlier vLead "rise smoothing" was removed -- it lagged the lead's speed-up by ~1 s, so when a lead +pulled away in stop-and-go it reported the lead as still near-stopped (measured up to 11 m/s slower than real). +That fed the MPC a phantom-slow/stopped lead -> phantom hard braking + a launch rubber-band. The lead's real +speed is passed through unchanged now. """ from opendbc.car import structs @@ -26,27 +28,6 @@ MIN_HELD_DREL = 0.5 # Stop/creep regime: return the raw radarstate so stop distance is byte-identical to stock (off==on). LOW_SPEED_PASSTHROUGH_V = 5.0 # m/s -# Lead-speed smoothing: time constant for lagging a lead that is speeding up. Falls are instant, so the -# reported vLead is always <= real -> obstacle is never farther than stock -> braking never weaker. -VLEAD_RISE_TAU = 1.0 # s -_VLEAD_RISE_ALPHA = DT_MDL / VLEAD_RISE_TAU - - -class _LeadView: - # Mirror of a lead with a smoothed vLead (<= real). Used above the stop gate to damp the catch-up surge. - __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') - - def __init__(self, src, vlead): - self.status = src.status - self.dRel = src.dRel - self.yRel = src.yRel - self.vRel = src.vRel - self.vLead = vlead - self.vLeadK = vlead - self.aLeadK = src.aLeadK - self.aLeadTau = src.aLeadTau - self.modelProb = src.modelProb - class _HeldLead: __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') @@ -78,7 +59,6 @@ class _LeadHold: self._since_real = 0 self._armed = False self._held_dRel = 0.0 - self._vlead_f = None # smoothed vLead (lag-up / instant-down) def reset(self): self.__init__() @@ -106,19 +86,6 @@ class _LeadHold: self._armed = False return raw - def smooth_vlead(self, lead): - # Lag the lead speeding up; pass slowing through instantly. Reported vLead stays <= real => the MPC's - # obstacle is never farther than stock (never-weaker), and the catch-up surge to a fidgety lead is damped. - if not lead.status: - self._vlead_f = None - return lead - v = float(lead.vLead) - if self._vlead_f is None or v <= self._vlead_f: - self._vlead_f = v # instant on slow-down / first sample - return lead - self._vlead_f += (v - self._vlead_f) * _VLEAD_RISE_ALPHA # lag on speed-up - return _LeadView(lead, self._vlead_f) - class RadarDistanceController: def __init__(self, CP: structs.CarParams, params=None): @@ -153,4 +120,4 @@ class RadarDistanceController: two = self._two.step(radarstate.leadTwo) if self._v_ego < LOW_SPEED_PASSTHROUGH_V: # stop/creep -> raw (byte-stock stops) return radarstate - return _RadarStateProxy(self._one.smooth_vlead(one), self._two.smooth_vlead(two)) + return _RadarStateProxy(one, two) # flicker-hold only; lead speed passed through as-is 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 542552c967..44328a3a56 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 @@ -107,23 +107,13 @@ def test_low_speed_passthrough_but_hold_warmed_for_highway(): assert out.leadOne.status is True -def test_vlead_filter_lags_rise_instant_fall_never_weaker(): - # Above the stop gate: a lead speeding up is lagged (reported vLead < real -> damps the catch-up surge), - # a lead slowing is instant (reported == real -> brake now). Reported vLead is ALWAYS <= real (never weaker). +def test_vlead_passed_through_unchanged(): + # The lead's real speed is reported as-is above the gate (no rise-lag) -- a lead pulling away is NOT + # reported as still-slow, so no phantom-slow-lead braking / stop-and-go rubber-band. c = ctrl() # default _v_ego above the gate - c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0))) # seed the filter at 15 + c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0))) rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne - assert 15.0 <= rising.vLead < 25.0 # lagged below the real rise - falling = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=10.0))).leadOne - assert falling.vLead == pytest.approx(10.0, abs=1e-6) # instant on slow-down - - -def test_vlead_filter_off_below_gate(): - # Stop/creep regime: raw passthrough, filter does not act (stop distance stays byte-stock). - c = ctrl() - c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1 - one = lead(dRel=6.0, vLead=2.0) - assert c.smooth_radarstate(rs(one)).leadOne is one + assert rising.vLead == pytest.approx(25.0, abs=1e-6) # real speed, not lagged below it def test_obstacle_monotone_during_hold():