From d0d3d22b8318c9bd2e557b653da941d5d1b09036 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 14 Jul 2026 15:42:22 -0700 Subject: [PATCH] fix(long): frame based staleness for jumpguard and lead-smoother --- .../lib/radar_distance/radar_distance.py | 61 ++++++++++-- .../tests/test_radar_distance.py | 99 ++++++++++++++++++- 2 files changed, 151 insertions(+), 9 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 7f96e4b341..39efdc6e27 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -51,6 +51,14 @@ SWITCH_DREL = 4.0 # m, dRel jump = a track switch (used by the ins # frames. Self-heals fast so a genuinely-departing lead is never held stale for long. JUMP_GUARD_MAX_HOLD = 10 # frames (~0.5s) +# How many frames a _JumpGuard/_LeadSmoother reference can go without a step()/update() call (below the speed +# gate that call is skipped entirely) before it's discarded as stale rather than diffed/EMA'd against. Must be +# small next to a real stop (the bug this guards against ran 3200+ frames/160s) but comfortably bigger than an +# incidental single skipped cycle from v_ego dithering right at LOW_SPEED_PASSTHROUGH_V/CREEP_PASSTHROUGH_V -- +# a flat ">1" here defeated the same-cycle farther-jump guard on every dithered cycle and let a real jitter +# glitch leak into the smoother's EMA for up to ~1s. +REFERENCE_STALE_FRAMES = 20 # ~1.0s + # Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead. STABILITY_WINDOW = 5 # frames (~0.25s @ 20Hz) VLEAD_SPREAD = 4.0 # m/s, vLead range over the window above which the lead is "unstable" @@ -149,19 +157,31 @@ class _LeadSmoother: self._vl = None self._vr = None self._hold = 0 + self._last_frame = 0 def reset(self): self._d = None self._vl = None self._vr = None self._hold = 0 + self._last_frame = 0 - def update(self, lead, noisy: bool): + def update(self, lead, noisy: bool, frame: int): # A held lead's dRel/vLead is a stale extrapolation -- feeding it into the EMA would both hide that from # downstream (wraps it into a _SmoothedLead) and pollute _d/_vl/_vr, lagging the real value's recovery. if isinstance(lead, _HeldLead): self.reset() return lead + # update() is only called above CREEP_PASSTHROUGH_V (see smooth_radarstate), so _hold and the EMA state + # (_d/_vl/_vr) freeze for the entire duration of any full standstill. Resuming and EMA-ing a real, opening + # lead against that frozen _d as if no time had passed lags dRel toward the stale, closer pre-stop value + # for up to LEAD_SMOOTH_TAU-ish seconds -- same bug class as _JumpGuard/_LeadHold's frame-based staleness + # fixes. Treat a gap since the last call larger than REFERENCE_STALE_FRAMES (not a flat 1) as no state at + # all, so an incidental single skipped cycle from v_ego dithering right at the gate doesn't itself wipe the + # EMA's short-term jitter-suppression memory. + if self._last_frame and frame - self._last_frame > REFERENCE_STALE_FRAMES: + self.reset() + self._last_frame = frame self._hold = LEAD_SMOOTH_HOLD if noisy else self._hold - 1 if self._hold <= 0 or not lead.status: self.reset() @@ -186,39 +206,54 @@ class _JumpGuard: # reading is no longer confirmed fresh, so it must not carry enough confidence to trip the stock FCW gate. def __init__(self): self._last = None + self._last_frame = 0 self._hold = 0 self._grace_used = False def reset(self): self._last = None + self._last_frame = 0 self._hold = 0 self._grace_used = False - def step(self, raw): + def step(self, raw, frame): if not raw.status: self.reset() return raw - if self._last is not None and (raw.dRel - self._last[0]) > SWITCH_DREL and self._hold < JUMP_GUARD_MAX_HOLD: + # _last is only a valid reference to diff against if it's recent. smooth_radarstate() stops calling step() + # below LOW_SPEED_PASSTHROUGH_V (see _LeadHold), so after any low-speed gap (a full stop, a slow zone) + # _last can be arbitrarily many real seconds old. Diffing a fresh reading against a stale one as if it were + # a same-cycle transient rejects a real, large, entirely legitimate change (e.g. a lead that pulled away + # during the gap) as a fusion glitch and holds a phantom lead -- measured causing a hard, unwarranted brake + # on a real route (launch from a stop after the lead had long since moved on). Treat a stale _last as no + # reference at all: pass raw through and re-baseline. REFERENCE_STALE_FRAMES (not a flat 1) so an + # incidental single skipped cycle from v_ego dithering right at the gate doesn't itself defeat the guard. + stale = self._last is not None and (frame - self._last_frame) > REFERENCE_STALE_FRAMES + + if not stale and self._last is not None and (raw.dRel - self._last[0]) > SWITCH_DREL and self._hold < JUMP_GUARD_MAX_HOLD: dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last self._hold += 1 held_dRel = max(MIN_HELD_DREL, dRel0 - max(-vRel0, 0.0) * DT_MDL) self._last = (held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, prob0) + self._last_frame = frame return _HeldLead(held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, min(prob0, FCW_PROB_CAP)) # Hold cap reached on a lead that was closing: self-healing straight onto raw here would adopt a farther # reading than the trajectory already tracked, i.e. report a farther lead than reality for at least one # more cycle. Take exactly one bounded extra cycle at the last-held value first -- never a second, so this # can't turn into an indefinite hold on a lead that genuinely departed. - if (self._hold >= JUMP_GUARD_MAX_HOLD and not self._grace_used and self._last is not None and + if (not stale and self._hold >= JUMP_GUARD_MAX_HOLD and not self._grace_used and self._last is not None and self._last[1] < 0.0 and (raw.dRel - self._last[0]) > SWITCH_DREL): dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last self._grace_used = True + self._last_frame = frame return _HeldLead(dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, min(prob0, FCW_PROB_CAP)) self._hold = 0 self._grace_used = False self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb) + self._last_frame = frame return raw @@ -236,6 +271,7 @@ class _LeadHold: self._real_frame = 0 self._armed = False self._held_dRel = 0.0 + self._holding = False # true once this hold episode has been reseeded from a real reading def reset(self): self.__init__() @@ -247,18 +283,27 @@ class _LeadHold: if self._sustained >= SUSTAIN_FRAMES: self._real_frame = frame self._armed = True + self._holding = False # back on a real sighting -- the next dropout starts a fresh hold episode return raw self._sustained = 0 since_real = frame - self._real_frame if self._armed and self._last is not None and since_real <= HOLD_MAX_FRAMES: dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last - if since_real == 1: + # Reseed _held_dRel from the real last-known value exactly once per hold episode, on whichever call + # first starts holding -- NOT on since_real==1: since_real is elapsed REAL frames (see class docstring), + # so any low-speed gap in between (step() skipped) makes since_real > 1 on the very first dropout call + # actually made, and comparing it to 1 would silently skip the reseed -- leaving _held_dRel at its + # stale/init value (0.0), which the next line's floor then clamps to MIN_HELD_DREL: a fabricated + # near-bumper phantom lead, not a dead-reckoned extrapolation of the real one. + if not self._holding: self._held_dRel = dRel0 + self._holding = True self._held_dRel = max(MIN_HELD_DREL, self._held_dRel - max(-vRel0, 0.0) * DT_MDL) return _HeldLead(self._held_dRel, vRel0, vLead0, min(aLeadK0, 0.0), aLeadTau0, min(prob0, FCW_PROB_CAP)) self._armed = False + self._holding = False return raw @@ -389,13 +434,13 @@ class RadarDistanceController: two = radarstate.leadTwo noisy = self._stability.churn or self._stability.same_track_noise if self._v_ego >= LOW_SPEED_PASSTHROUGH_V: - one = self._jump_guard.step(radarstate.leadOne) # reject a same-cycle farther-jump transient ... + one = self._jump_guard.step(radarstate.leadOne, self._frame) # reject a same-cycle farther-jump transient ... one = self._one.step(one, self._frame) # ... + flicker-hold ... two = self._two.step(radarstate.leadTwo, self._frame) - one = self._smoother.update(one, noisy) # ... + same-object de-jitter (anti follow-hunt) + one = self._smoother.update(one, noisy, self._frame) # ... + same-object de-jitter (anti follow-hunt) elif self._v_ego >= CREEP_PASSTHROUGH_V: # creep band: de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch) - one = self._smoother.update(radarstate.leadOne, noisy) + one = self._smoother.update(radarstate.leadOne, noisy, self._frame) else: one = radarstate.leadOne # full standstill: no hold/smoothing one = self._stop_gap_bias(one) # low-speed near-stopped: settle farther back 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 b4899dfdaa..6b7d1deefe 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 @@ -456,6 +456,44 @@ def test_jump_guard_boundary_not_triggered(): assert out.leadOne.dRel == pytest.approx(30.0 + SWITCH_DREL - 0.1) # under threshold -> passes through +def test_jump_guard_does_not_hold_a_stale_reference_after_an_extended_low_speed_gap(): + # Mirrors _LeadHold's identical bug (see test_hold_does_not_resurrect_a_stale_lead_after_an_extended_low_speed_gap): + # _jump_guard.step() is also only called above LOW_SPEED_PASSTHROUGH_V, so its _last reference used to + # freeze for the entire duration of any low-speed period with no elapsed-time awareness. Route + # 550a71ee4c7a7fbe/000004dc--c8c0867520, t~407.1: a lead tracked at dRel=11.72 while decelerating into a + # stop froze there through a ~160s standstill. On relaunch the real lead (now dRel=23.16, opening) was + # diffed against that 160s-stale reference as if it were a same-cycle transient, held as a fabricated + # closing lead, and fed the MPC a phantom near-collision course that produced a real, unwarranted hard + # brake on a real drive. + c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0) + c.smooth_radarstate(rs(lead(dRel=11.72, vRel=-2.33, vLead=2.72))) # last reading before decelerating to a stop + c._v_ego = CREEP_PASSTHROUGH_V - 0.5 # full stop: jump-guard.step() never called + for _ in range(HOLD_MAX_FRAMES * 10): # far longer than any hold cap -- a real gap + c.smooth_radarstate(rs(lead(dRel=3.68, vRel=0.0, vLead=0.0))) # the lead is stopped just ahead + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 # relaunch: real lead now far + opening + out = c.smooth_radarstate(rs(lead(dRel=23.16, vRel=4.72, vLead=9.77))).leadOne + assert out.dRel == pytest.approx(23.16) # fresh reading passes through, not held as a phantom jump + assert out.vRel == pytest.approx(4.72) + + +def test_jump_guard_survives_a_single_incidental_gap_without_losing_protection(): + # Regression: a flat ">1 frame gap -> stale" threshold discarded the trusted reference (and so skipped the + # SWITCH_DREL check entirely) after ANY single skipped call -- not just a real multi-second stop. That + # happens on every cycle of ordinary v_ego dithering right at LOW_SPEED_PASSTHROUGH_V (a car crawling near + # 5 m/s in stop-and-go traffic), permanently voiding the same-cycle fusion-transient guard for as long as the + # dithering continues and letting a real glitch straight through. A single one-frame dip (a momentary v_ego + # dip below the gate, then immediately back above it) must NOT be treated as stale -- the original farther- + # jump rejection must still fire right after it, same as it would with no gap at all. + c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0) + c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0, vLead=18.0))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0 # one incidental dip below the gate + c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 # immediately back above it + glitch = rs(lead(dRel=30.0 + SWITCH_DREL + 1.0, vRel=6.0, vLead=20.0)) # same-cycle fusion transient + out = c.smooth_radarstate(glitch).leadOne + assert out.dRel < 32.0 # still held near ~30, glitch did not leak through + + # --- flicker-hold ----------------------------------------------------------------------------------------- def test_holds_after_sustained_dropout(): @@ -493,7 +531,9 @@ def test_hold_does_not_resurrect_a_stale_lead_after_an_extended_low_speed_gap(): def test_hold_survives_a_brief_low_speed_dip_within_the_cap(): # A short dip below the gate (well under HOLD_MAX_FRAMES real cycles) is the case flicker-hold exists for -- - # it must still bridge, same as a same-speed dropout of the same real duration would. + # it must still bridge, same as a same-speed dropout of the same real duration would. dRel must stay close + # to the real last-known value (a genuine dead-reckoned extrapolation) -- NOT collapse toward MIN_HELD_DREL, + # which is what a broken reseed would produce (see test_hold_reseeds_correctly_after_any_low_speed_gap). c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0) for _ in range(3): c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-3.0, vLead=5.0))) @@ -504,6 +544,25 @@ def test_hold_survives_a_brief_low_speed_dip_within_the_cap(): out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) assert out.leadOne.status is True assert out.leadOne.dRel < 30.0 + assert out.leadOne.dRel > 29.0 # dead-reckoned from 30.0, not collapsed to MIN_HELD_DREL + + +def test_hold_reseeds_correctly_after_any_low_speed_gap(): + # Regression: _held_dRel used to be reseeded to the real last-known value only when since_real == 1. Once + # since_real became elapsed-REAL-frames (not a self-incrementing call counter), any skipped low-speed frame + # made since_real > 1 on the very first dropout call actually made, silently skipping the reseed -- leaving + # _held_dRel at its stale/init value (0.0), which the very next line's floor clamps to MIN_HELD_DREL: a + # fabricated near-bumper phantom lead fed straight to the MPC, not a dead-reckoned extrapolation of the real + # one. A single one-frame low-speed dip (the shortest possible gap, ~0.05s) is enough to trigger it. + c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0) + for _ in range(3): + c.smooth_radarstate(rs(lead(dRel=50.0, vRel=-3.0, vLead=15.0))) + c._v_ego = CREEP_PASSTHROUGH_V - 0.5 + c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) # exactly ONE low-speed frame + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 + out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne + assert out.status is True + assert out.dRel > 45.0 # must be near the real ~50m, not a fabricated 0.5m def test_releases_after_hold_cap(): @@ -702,3 +761,41 @@ def test_smoother_same_track_noise_ignores_drel_jump(): for i in range(30): out = c.smooth_radarstate(rs(lead(dRel=40.0 if i % 2 == 0 else 55.0, vLead=18.0, vRel=-1.0, radarTrackId=4))) assert out.leadOne.dRel < 45.0 # held near the trusted value by the jump-guard, not 55 + + +def test_smoother_does_not_lag_a_stale_ema_after_an_extended_low_speed_gap(): + # Mirrors the identical bug already fixed in _JumpGuard/_LeadHold: _smoother.update() is only called above + # CREEP_PASSTHROUGH_V (see smooth_radarstate), so its EMA state (_d/_vl/_vr) and _hold freeze for the entire + # duration of any full standstill. Resuming and EMA-ing a real, opening lead against that frozen state as if + # no time had passed lags dRel toward the stale, closer pre-stop value -- confirmed on the same real route as + # the _JumpGuard bug (550a71ee4c7a7fbe/000004dc--c8c0867520): pre-fix this reported 12.86m instead of the + # real 23.16m on relaunch. + c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0) + for i in range(12): + c.smooth_radarstate(rs(lead(dRel=11.72, vLead=2.72, vRel=-2.33, radarTrackId=1 if i % 2 else 2))) # arms churn + c._v_ego = CREEP_PASSTHROUGH_V - 0.5 # full stop: smoother.update() never called + for _ in range(60): + c.smooth_radarstate(rs(lead(dRel=3.68, vRel=0.0, vLead=0.0))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 # relaunch: real lead now far + opening + out = c.smooth_radarstate(rs(lead(dRel=23.16, vRel=4.72, vLead=9.77, radarTrackId=1))).leadOne + assert out.dRel == pytest.approx(23.16) # fresh reading passes through, not EMA-lagged stale + + +def test_single_incidental_gap_during_churn_does_not_leak_a_glitch_into_the_ema(): + # Regression, deeper than the single-mechanism cases above: a flat ">1 frame gap -> stale" threshold made + # _jump_guard treat ANY single skipped call as fully stale and skip the SWITCH_DREL check -- so a same-cycle + # fusion-transient glitch right after one incidental low-speed dip passed straight through _jump_guard + # unguarded, then got folded into the churn smoother's EMA (which was still live from before the dip), + # lagging vLead/dRel toward the glitch's inflated values for ~1s: a farther-and-faster-than-real lead, i.e. + # a real violation of this file's own invariant ("NEVER report a farther-or-faster lead than reality"). + c = ctrl(v_ego=10.0) + for i in range(6): # prime churn (real radar trackId-flip signature) + tid = 1 if i % 2 == 0 else 2 + c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-1.0, vLead=15.0, radarTrackId=tid))) + c._v_ego = 3.0 # one incidental dip into the creep band + c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-1.0, vLead=15.0, radarTrackId=1))) + c._v_ego = 10.0 # immediately back above the full-pipeline gate + glitch = lead(dRel=30.0 + SWITCH_DREL + 1.0, vRel=6.0, vLead=20.0, radarTrackId=1) + out = c.smooth_radarstate(rs(glitch)).leadOne + assert out.dRel < 31.0 # held near real ~30, not the glitch's 35.0 + assert out.vLead < 16.0 # held near real ~15, not the glitch's 20.0