diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 649d8c37ac..7f96e4b341 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -223,30 +223,37 @@ class _JumpGuard: class _LeadHold: + # step() takes the caller's absolute frame counter rather than counting its own calls: below + # LOW_SPEED_PASSTHROUGH_V the caller stops calling step() at all (see smooth_radarstate), and a + # self-incrementing counter would then stay frozen at whatever it was for however long that lasts -- on + # resume it would read as "just a few frames since the last real sighting" no matter how much real time + # (a full stop, a slow zone) actually passed, and could hand HOLD_MAX_FRAMES worth of stale credit to a + # sighting from arbitrarily long ago. Comparing against the caller's frame counter makes the elapsed-frames + # check correct regardless of how many cycles were skipped in between. def __init__(self): self._last = None self._sustained = 0 - self._since_real = 0 + self._real_frame = 0 self._armed = False self._held_dRel = 0.0 def reset(self): self.__init__() - def step(self, raw): + def step(self, raw, frame): if raw.status and raw.dRel > DROPOUT_DREL: self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb) self._sustained += 1 if self._sustained >= SUSTAIN_FRAMES: - self._since_real = 0 + self._real_frame = frame self._armed = True return raw self._sustained = 0 - self._since_real += 1 - if self._armed and self._last is not None and self._since_real <= HOLD_MAX_FRAMES: + 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 self._since_real == 1: + if since_real == 1: self._held_dRel = dRel0 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)) @@ -333,7 +340,6 @@ class RadarDistanceController: if self._frame % int(1. / DT_MDL) == 0: self._read_params() self._v_ego = float(sm['carState'].vEgo) - self._frame += 1 def enabled(self) -> bool: return self._enabled @@ -376,6 +382,7 @@ class RadarDistanceController: return _BiasedLead(lead, max(lead.dRel - offset, STOP_GAP_MIN_DREL)) def smooth_radarstate(self, radarstate): + self._frame += 1 # step()'s elapsed-frames basis; see _LeadHold self._stability.update(radarstate.leadOne, self._v_ego) # telemetry, runs every cycle if not self._enabled: return radarstate # off: byte-stock passthrough @@ -383,8 +390,8 @@ class RadarDistanceController: 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._one.step(one) # ... + flicker-hold ... - two = self._two.step(radarstate.leadTwo) + 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) elif self._v_ego >= CREEP_PASSTHROUGH_V: # creep band: de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch) 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 88c608d3e1..b4899dfdaa 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 @@ -475,6 +475,37 @@ def test_no_hold_without_sustained_lead(): assert out.leadOne.status is False # no hold armed +def test_hold_does_not_resurrect_a_stale_lead_after_an_extended_low_speed_gap(): + # Below LOW_SPEED_PASSTHROUGH_V the hold is never stepped at all (see smooth_radarstate), so an elapsed- + # frames check must be based on real cycles, not "cycles since step() was last called" -- otherwise resuming + # above the gate looks like no time passed no matter how long the low-speed period actually was, and a hold + # armed on a real lead long before the gap can resurrect as if it were still fresh. + 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))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0 # below the gate: step() stops being called on the hold + for _ in range(HOLD_MAX_FRAMES * 3): + c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 # back above the gate, lead still gone + out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) + assert out.leadOne.status is False # must not resurrect the old hold + + +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. + 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))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0 + for _ in range(3): + c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0 + 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 + + def test_releases_after_hold_cap(): c = ctrl() for _ in range(3):