From 93520b7e993b07bb85abed8e4ceb224767c62d0b Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 11 Jul 2026 12:52:02 -0700 Subject: [PATCH] fix(long): jump-guard self-heal onto farther-than-held value on a closing lead A spurious closer misread can poison the guard's anchor, making the lead's own real, continuing (farther) trajectory read as a false farther jump and get held to the cap. Self-heal then jumped straight onto whatever farther transitional reading showed up next, which _LeadHold could seed a dropout extrapolation from -- reporting a lead opening up right before a real catch-up brake (confirmed on route 000004c6, t~1338, -3.94 m/s^2). Add one bounded grace cycle before self-heal on a closing lead so the handoff into a dropout hold stays close to the real trajectory instead. --- .../lib/radar_distance/radar_distance.py | 13 +++++ .../tests/test_radar_distance.py | 53 ++++++++++++++++++- 2 files changed, 65 insertions(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 86a5d7381c..649d8c37ac 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -187,10 +187,12 @@ class _JumpGuard: def __init__(self): self._last = None self._hold = 0 + self._grace_used = False def reset(self): self._last = None self._hold = 0 + self._grace_used = False def step(self, raw): if not raw.status: @@ -204,7 +206,18 @@ class _JumpGuard: self._last = (held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, prob0) 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 + 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 + 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) return raw 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 cc1a198853..88c608d3e1 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 @@ -344,8 +344,59 @@ def test_jump_guard_self_heals_after_cap(): for _ in range(JUMP_GUARD_MAX_HOLD): out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0))) assert out.leadOne.dRel < 40.0 # held while under the cap + # cap just reached on a lead that was closing -- one bounded grace cycle before accepting a farther raw value out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0))) - assert out.leadOne.dRel == pytest.approx(40.0) # cap exceeded -> accepts the real (departing) value + assert out.leadOne.dRel < 40.0 # grace cycle: still held, not yet accepted + out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0))) + assert out.leadOne.dRel == pytest.approx(40.0) # grace spent -> accepts the real (departing) value + + +def test_jump_guard_self_heals_immediately_when_not_closing(): + # The grace cycle only protects a lead that was closing when the cap was hit -- a lead that was already + # steady/opening (vRel >= 0) self-heals on the very first cap-exceeding frame, same as before this fix. + c = ctrl() + c.smooth_radarstate(rs(lead(dRel=20.0, vRel=0.5, vLead=19.0))) + for _ in range(JUMP_GUARD_MAX_HOLD): + out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=0.5, vLead=19.0))) + assert out.leadOne.dRel < 40.0 + out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=0.5, vLead=19.0))) + assert out.leadOne.dRel == pytest.approx(40.0) # no grace needed -> heals immediately, unchanged behavior + + +def test_jump_guard_grace_is_used_at_most_once_per_hold_episode(): + # The grace cycle must be bounded -- a lead that keeps reading farther after the grace is spent must not + # get a second grace before genuinely accepting the new value (else a departed lead could be held forever). + c = ctrl() + c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0))) + for _ in range(JUMP_GUARD_MAX_HOLD): + c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0))) + c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0))) # grace cycle, spent + out = c.smooth_radarstate(rs(lead(dRel=70.0, vRel=-1.0, vLead=19.0))) + assert out.leadOne.dRel == pytest.approx(70.0) # grace already spent this episode -> accepts immediately + + +def test_jump_guard_replays_real_route_dropout_catchup(): + # route 550a71ee4c7a7fbe/000004c6--ed1b6d7f95, t~1337.9-1338.5: a spurious closer misread (31.08 -> 24.94) + # passes through immediately (closer always does), poisoning the guard's anchor. The lead's real, continuing + # trajectory (~31m, closing) then reads as a farther jump against that bad anchor and gets held for the full + # cap. Without the grace cycle, the guard self-healed straight onto a farther transitional misread (56.52) + # right as a real dropout began, and _LeadHold then flicker-held THAT value through the whole dropout -- + # reporting a lead ~2x farther and opening instead of closing, easing the MPC off right before a real + # catch-up brake. The grace cycle must keep the held value close to the real trajectory across this handoff. + c = ctrl(v_ego=14.4) + raw = [ + (30.62, -0.45, 1), (38.36, -3.33, -1), (38.20, -3.42, -1), (38.08, -3.45, -1), (37.88, -3.53, -1), + (37.72, -3.58, -1), (47.53, 0.35, -1), (24.94, -1.90, -1), (31.44, -3.72, -1), (31.20, -3.97, -1), + (31.08, -3.95, 2), (74.32, 3.65, 3), (74.52, 3.70, 3), (74.92, 3.85, 3), (75.12, 3.88, 3), + (75.28, 3.90, 3), (75.64, 3.95, 3), (75.64, 3.95, 3), (56.52, -2.09, -1), + ] + out = None + for dRel, vRel, tid in raw: + out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=10.5, radarTrackId=tid))) + assert out.leadOne.dRel < 30.0 # grace cycle: still held near the real trajectory + dropout_held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne + assert dropout_held.status is True + assert dropout_held.dRel < 30.0 # flicker-hold seeds from the grace-held value, not 56.52 def test_jump_guard_resets_on_dropout():