Ford: extend the angle-mode stall rescue below 9 m/s with the pinion measurement

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

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