fix(long): remove RadarDistance vLead

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

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():