From b2586c56bb2b50aa26028c4a45fc31679bee6af0 Mon Sep 17 00:00:00 2001 From: Jacob Neulight Date: Sat, 18 Jul 2026 20:24:41 -0600 Subject: [PATCH] Ford: extend angle-mode stall rescue below 9 m/s with pinion measurement The post-override stall blip was gated on v_ego > 9.0 and on the deviation clip actually binding -- both because the yaw-derived measurement is untrustworthy at low speed. With the pinion measurement enabled those reasons vanish (the geometric measurement is at its best at low speed), and the old gating provably deadlocks: below the clip's own 9 m/s gate the clip can never bind, so the accumulator can never charge, observed on-road as a post-override PSCM stall through an entire ~60 m-radius turn at 19 mph that ended in a driver rescue. With STEER_ANGLE_CURVATURE enabled: the stall gate drops to 5 m/s (matching ford.h's path-offset low-speed floor) and the accumulator charges on the raw desired-vs-measured gap where the clip cannot bind. Replaying the stalled-turn segment: the old gating never fires; the new gating fires during the stall, before the driver intervention. Flag off: gating is bit-identical to before (unit-tested). --- .../sunnypilot/car/ford/lateral_angle_ext.py | 22 ++++++++++--- .../car/ford/tests/test_lateral_angle_ext.py | 31 +++++++++++++++++++ 2 files changed, 48 insertions(+), 5 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 42aa95ed59..b7e518822b 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -102,6 +102,11 @@ _STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL # 20 Hz lateral tick (matches human_turn.py) _STALL_GAP_MIN = 2.0 * CarControllerParams.CURVATURE_ERROR # desired must lead measured by 2x the clip tolerance _STALL_HOLD_S = 0.5 # accumulated clip-binding time before a pulse fires +_DEVIATION_CLIP_GATE_MS = 9.0 # m/s; below this the deviation clip (and stall detection on yaw) is inert +# With the pinion measurement the geometric source is trustworthy at low speed (best there, in +# fact), so stall detection can extend to the path-offset check's low-speed floor (5 m/s, +# ford.h FORD_PATH_OFFSET_LIMITS.angle_error_min_speed). On yaw the 9 m/s distrust stands. +_STALL_GATE_PINION_MS = 5.0 _STALL_BLIP_FRAMES = 6 # mode-0 pulse length (6 frames @ 20 Hz = 300 ms; PSCM acked mode 0 in ~150 ms on-road) _STALL_COOLDOWN_S = 2.0 # re-arm delay after a pulse (release ramp + PSCM response time) _STALL_MAX_BLIPS = 3 # give up on a stuck episode; devLim telemetry keeps recording the stall @@ -428,9 +433,9 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # Use planner / predicted κ directly for the κ → path_angle map; we are not sending κ on CAN. kappa_cmd = float(requested_curvature) - # BluePilot: clip kappa_cmd to current_curvature (measured, from yaw rate) +- CURVATURE_ERROR, + # BluePilot: clip kappa_cmd to current_curvature (measured, via get_current_curvature) +- CURVATURE_ERROR, # mirroring lateral_curv_ext.py's apply_ford_curvature_limits_ext exactly (same formula, same - # v_ego > 9 gate, same CarControllerParams.CURVATURE_ERROR tolerance). Without this, kappa_cmd + # _DEVIATION_CLIP_GATE_MS gate, same CarControllerParams.CURVATURE_ERROR tolerance). Without this, kappa_cmd # (and therefore path_angle, and the shadow_curvature sent to ford.h) can legitimately lead the # measured curvature by more than ford.h's angle-error tolerance during normal curve entry/exit # -- the shadow-curvature deviation check (ford_shadow_curvature_error_check) would then block @@ -439,7 +444,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # than only clipping the value reported to panda (which would make the check a no-op). current_curvature = self.get_current_curvature(CS) self.bp_curvature_deviation_limited = False - if v_ego > 9: + if v_ego > _DEVIATION_CLIP_GATE_MS: _kappa_cmd_pre_error_clip = kappa_cmd kappa_cmd = float(clip(kappa_cmd, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR)) @@ -536,13 +541,20 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # clip's tolerance while the clip was actually binding for _STALL_HOLD_S accumulated seconds. # devLim flickers mid-stall (~63% duty on the diagnosis route), so off frames hold the # accumulator rather than resetting it; a closed gap or driver press ends the episode. + # With the pinion measurement, detection extends below the deviation clip's own gate + # (see _STALL_GATE_PINION_MS): there the clip can never bind, so the accumulator charges on + # the raw gap instead -- observed on-road as a post-override PSCM stall through an entire + # ~60 m-radius turn at 19 mph that the current gating could never rescue. self.stall_blip_cooldown_s = max(0.0, self.stall_blip_cooldown_s - _STEER_DT) + _stall_gate_ms = _STALL_GATE_PINION_MS if self.bp_pinion_curvature_enabled else _DEVIATION_CLIP_GATE_MS _stall_gap = desired_curvature - current_curvature - _stalled = (not CS.out.steeringPressed and not self.lane_change and v_ego > 9.0 + _stalled = (not CS.out.steeringPressed and not self.lane_change and v_ego > _stall_gate_ms and abs(_stall_gap) > _STALL_GAP_MIN and abs(desired_curvature) > abs(current_curvature)) if _stalled: - if self.bp_curvature_deviation_limited and self.stall_blip_cooldown_s <= 0.0: + _clip_can_bind = v_ego > _DEVIATION_CLIP_GATE_MS + _charging = self.bp_curvature_deviation_limited or (self.bp_pinion_curvature_enabled and not _clip_can_bind) + if _charging and self.stall_blip_cooldown_s <= 0.0: self.stall_blip_hold_s += _STEER_DT if self.stall_blip_hold_s >= _STALL_HOLD_S and self.stall_blip_count < _STALL_MAX_BLIPS: self.stall_blip_frames_left = _STALL_BLIP_FRAMES diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py index a6650d9072..490890ab95 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py @@ -202,5 +202,36 @@ def test_safety_param_stays_a_plain_int(self): self.assertIs(type(CP_SP.safetyParam), int) +class TestLowSpeedStallRescue(unittest.TestCase): + """With the pinion measurement enabled, stall detection extends below the deviation + clip's 9 m/s gate (down to 5 m/s, the ford.h path-offset floor), charging on the raw + gap where the clip can never bind. With the flag off, gating is bit-identical to + before: nothing charges below 9 m/s (yaw measurement distrust stands).""" + + def _drive_stalled(self, ext, CP, v_ego, frames): + # hands-off, measured curvature 0 (wheel straight), planner asking 0.01 -> raw gap + # 0.01 > _STALL_GAP_MIN; below 9 m/s the deviation clip is inert so devLim stays False + cs = _CS(vEgoRaw=v_ego, vEgo=v_ego, yawRate=0.0, steeringAngleDeg=0.0) + for _ in range(frames): + ext.update_angle_strategy(_CC(latActive=True), cs, _Actuators(curvature=0.01), CP) + + def test_pinion_rescues_below_clip_gate(self): + ext, CP = _pinion_harness(flag=True) + self._drive_stalled(ext, CP, v_ego=6.0, frames=12) + self.assertEqual(ext.stall_blip_count, 1) # blip fired from gap-only accumulation + + def test_pinion_inert_below_stall_gate(self): + ext, CP = _pinion_harness(flag=True) + self._drive_stalled(ext, CP, v_ego=4.5, frames=12) + self.assertEqual(ext.stall_blip_count, 0) + self.assertEqual(ext.stall_blip_hold_s, 0.0) + + def test_yaw_mode_unchanged_below_gate(self): + ext, CP = _pinion_harness(flag=False) + self._drive_stalled(ext, CP, v_ego=6.0, frames=12) + self.assertEqual(ext.stall_blip_count, 0) + self.assertEqual(ext.stall_blip_hold_s, 0.0) + + if __name__ == '__main__': unittest.main()