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