Ford: keep the angle-mode shadow curvature truthful during overrides

[jrneulight]

Ford: keep the angle-mode shadow curvature truthful during overrides

Angle mode publishes a "shadow curvature" — the kappa its path_angle was derived from —
in Lane_Assist_Data1's unused bits, and ford.h judges it against measured curvature
(angle_meas). That is angle mode's only commanded-vs-measured deviation check. This PR
makes the published value honest in the situations where the planner kappa cannot
describe what the car is actually doing, and ships the replay tool that found (and now
guards) the underlying behavior.

The evidence that motivated this

We built a frame-exact replay of ford.h's LKA/LMC TX-hook semantics
(tools/ford_lmc_safety_replay.py, first commit of this PR) and ran it against ~4.5 h of
real Explorer rlogs (routes 00000001--be********, 00000002--71********,
00000003--a1********, 00000004--f0********; 72,782 LMC frames). Two properties make
its attributions trustworthy rather than guesswork:

• Ground truth independent of the simulation: a sendcan frame with no TX loopback
  echo in the can stream was actually blocked by the panda, and
  pandaStates.safetyTxBlocked counter deltas cross-check the totals frame-exactly.
• The reset-bypass latch is modeled. An earlier analysis omitted it and
  mis-attributed blocks; this tool exists so firmware semantics can never silently drift
  from the analysis again.

Findings:

1. Exactly two real lateral safety blocks occurred in 4.5 h — same mechanism both
   times. A driver correcting mid-curve with the mode still enabled: the
   deviation-clipped planner kappa can't follow the wheel, so the shadow exits the error
   band while a sustained curve has drained the latch.

   Route	t	shadow (CAN)	angle_meas window	speed	pressed	counter
   00000003	455.0 s	−630	[−795, −782]	11.2 m/s	yes	0→2 at 455.0
   00000004	1301.7 s	−766	[−942, −927]	10.0 m/s	yes	2→4 at 1301.7

2. 35 latent "re-engage race" frames, none of which ever blocked. On release of a
   human-turn override or stall blip, the old code zeroed the shadow, so the first
   re-enabled LMC frame (20 Hz) races LKA's 33 Hz shadow latch and can present a stale
   zero against real measured curvature. Every one of these frames was bypassed by the
   reset-bypass latch (all 72,782 LMC frames echoed to the bus). The race is real at the
   check-semantics level and would surface under any future latch tightening — but it
   has never blocked anything in current firmware.

3. The safetyTxBlocked folklore dissolved under attribution. The counter resets
   when the safety model is set, so the 5–9 counts seen at every ignition are
   pre-safety-mode init TX, not drive events; the only other in-drive increments were
   cancel-button frames gated at cruise-state edges (Steering_Data_FD1 passthrough,
   unrelated to lateral).

4. The latch itself does more than expected: in angle mode the wire curvature is
   always 0, so any straight-driving frame recharges the 60-frame full bypass, and the
   shadow check is only live during >3 s sustained curves (latch empty 23–60% of frames
   depending on route). Documented here honestly; narrowing it is future work this tool
   can validate.

Why the fix is shaped this way

• All measurement reads are routed through one accessor,
  LateralCurvExt.get_current_curvature(), so the shadow judged by the check and the
  check's own reference can never come from different measurement families.
• The shadow is published from the measured curvature whenever the planner kappa cannot
  honestly describe the car's steering: while the driver presses (fixes the two real
  blocks), and during human-turn/blip mode-0 pulses and inactive frames (kills the
  re-engage race at the source — the panda-latched shadow tracks reality continuously,
  so the first re-engaged frame compares measured-vs-measured).
• We deliberately did not add a firmware engage-edge grace: replay shows it would be
  dead code under the current latch. No dead code in safety files.
• Caveat stated plainly: while the driver presses, the deviation check becomes vacuous
  (shadow ≡ measurement). We consider that correct — during an override the honest
  "command" is the driver's curvature, and the driver has control — but it is a
  reviewer-visible trade-off, not a hidden one.

The evidence afterwards

Re-running the same tool with --truthful-shadow (the fix modeled at real LKA cadence,
so latch-age timing skew is included):

Route	shadow-check violations before	after	predicted real blocks before → after
00000001	2	0	0 → 0
00000002	2	0	0 → 0
00000003	10 (+1 real)	0	1 → 0
00000004	21 (+1 real)	0	1 → 0

No new violations are possible by construction: the published value sits inside
[meas_min − band, meas_max + band] at latch time. On-road confirmation: a ~27 min
acceptance drive on this code (demanding roads, driver corrections included) shows zero
lateral blocks and zero pre-latch shadow signatures — the latent class is empty in
live driving, not just in counterfactual replay. The only residual entries are six
engagement-transition controls_allowed sampling artifacts, unrelated to the shadow
system and all latch-masked. Hands-off behavior is unchanged (unit-tested: the clipped
planner kappa is still published when hands are off).

What each change is

1. tools/ford_lmc_safety_replay.py (first commit): the full CAN-path TX-hook port —
   value limits, steer_angle_cmd_checks including its controls-not-allowed gate and
   desired_angle_last reset semantics, the shadow check, ROC checks, and the
   reset-bypass latch — plus the echo-based ground truth and the --truthful-shadow
   counterfactual.
2. lateral_curv_ext.py: get_current_curvature(CS) accessor (yaw-rate
   implementation, identical math to the expressions it replaces).
3. lateral_angle_ext.py: the four publishing sites (inactive, human-turn override,
   stall blip, driver-pressed) switched from 0.0/clipped-kappa to the accessor.
4. tests/test_lateral_angle_ext.py: pins the truthfulness contract at all four
   sites and the unchanged hands-off path, with a harness mirroring CarController's
   mixin composition.

Test it yourself

# unit tests (no hardware)
pytest opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py

# safety suite: untouched by this PR (control-code only); not-worse-than-baseline
pytest opendbc_repo/opendbc/safety/tests/test_ford.py

# replay any of your own Ford angle-mode routes (rlogs required)
FORD_REPLAY_DONGLE_ID=<your_dongle> tools/ford_lmc_safety_replay.py <route> [--truthful-shadow]

The replay prints, per route: sim violations pre-latch (what would block with no latch),
post-latch (predicted real blocks), the per-check breakdown, the echo ground truth
(sent/echoed/no-echo per address), and every safetyTxBlocked counter increment with
route time — so you can reproduce the attribution table above on our routes or check
your own fleet's behavior before/after this change.

[jrneulight]

Closing: this already landed in bp-dev as part of #145.

Both commits are ancestors of bp-dev — ee1ff983e9 (LMC safety-check replay tool) and 699c17d9fd (truthful shadow curvature) rode in inside #145's merge, since this branch was #145's base.

Verified in bp-dev rather than assumed:

• lateral_angle_ext.py and tools/ford_lmc_safety_replay.py are byte-identical to this branch's copies.
• get_current_curvature() and its call site are present in lateral_curv_ext.py; its docstring was intentionally extended by Ford: opt-in pinion curvature measurement (Use Pinion Yaw Sensor) #145 when the measurement source became selectable.

GitHub couldn't auto-close this because I'd force-pushed a rebased copy of the branch, so the PR head SHAs no longer matched the ones that merged. That's on me — the content is in, nothing is lost.

No action needed; the follow-up work continues in #152 (restores the pinion toggle that the #145 merge dropped from settings_ui.json), #148, and #153.