Ford angle-mode lateral: closed-loop auto-calibration of the speed factors

[ghbarker]

The evidence that motivated this

• The two angle-mode speed factors (FordLowSpeedFactor_ang / FordHighSpeedFactor_ang) are per-car: pinion geometry, tires, and alignment move the true gains, and most drivers never find their values. Manual stepping is trial-and-error at 0.01 granularity across two coupled knobs.
• Lag-aligned measurement on real drives shows the stock factors leaving real error on the table: on the reference car the high anchor's measured/commanded curvature ratio sat near 0.85–0.95 at 1.00 and converged to ~1.0 only near 1.16 — the car was persistently turning less than asked until calibrated.
• The same measurement machinery that proves the deficit can close the loop: if evidence quality is gated hard enough, the car can find its own factors.

Why the fix is shaped this way

• A pure estimator with no I/O (angle_autocal.py): weighted least-squares fit of the two gain anchors from curve samples, y = g_applied / r per sample so the target is invariant to whatever factor was in force when the sample was taken. The same module runs offline against logged drives — every onboard decision is reproducible on a PC.
• Evidence is guarded before it is trusted: samples only count while the loop is quietly tracking (hunting can never read as gain), only above a curvature floor that sits exactly at the gain-interp knee (MIN_KAPPA — sub-knee under-delivery cannot contaminate the fit), only while liveDelay reports estimated, and lag-aligned against the command from lateralDelay ago so winding roads still count. Saturated frames, hands-on frames, and lat-accel-implausible samples are rejected.
• Adjust-then-verify, one damped step at a time: at most one bounded nudge per period toward the fit, and every step must be judged against fresh post-step evidence before the next. Evidence forgets on a ~2 h driving horizon so the calibration stays adaptive; a stability deadband plus a lock freezes it when there is nothing left to adjust.
• Lifecycle is separate from math (angle_autocal_controller.py): arm/disarm from the toggle, JSON persistence across drives, blocking nudge writes (single owner of the factor params), user-edit detection, an error channel that can never clobber evidence, and ground-truth telemetry from the live controller — not param snapshots.
• The sub-knee small-signal lift rides FordHighSpeedDampening_ang from Add High Speed Low Curve Tuning Option #166 (mechanically the same multiplier) — no duplicate param.

The evidence afterwards

• Full validation gate on this branch standalone: Ford suite green + the card process replayed over real drive segments with auto-cal armed, clean.
• On-car drive telemetry (route 0000001e, 32 segments): the calibrator proposed, stepped, and verify-confirmed the high anchor at 1.16 with the live response ratio settling at 0.996–1.013 — the car measurably doing what is asked after calibration.
• 975-line unit suite on the pure estimator (evidence gating, anchor solving, nudge damping, persistence round-trips, user-edit paths) + controller lifecycle tests; 105 Ford tests green on the stack.

What each change is

• opendbc/sunnypilot/car/ford/angle_autocal.py: the pure estimator — evidence gates, staged steady/apex collection, the two-anchor fit, nudger, verify, lock.
• opendbc/sunnypilot/car/ford/angle_autocal_controller.py: lifecycle — arm/disarm, persistence, blocking factor writes, error channel, telemetry status.
• opendbc/sunnypilot/car/ford/lateral_angle_ext.py: frame feed + factor read-back (the strategy stays the single reader of the params the nudger writes); platform gains extracted to values_ext.py.
• cereal/custom.capnp + opendbc/car/structs.py + bluepilot/selfdrive/car/bp_card_publisher.py: ground-truth auto-cal telemetry (bmsAngleAutoCalibrate, bmsAngleAutoCalState, angleSaturated).
• common/params_keys.h: the five FordAngleAutoCal* params.
• Both device menus + sunnylink: arm toggle, calibration lock, Erase Calibration Memory.
• docs/ford-angle-autocal.md: driver-facing guide.
• Tests: test_angle_autocal.py (estimator + controller), test_lateral_angle_ext.py (strategy glue).

Test it yourself

• Driver docs: docs/ford-angle-autocal.md.
• Leave the toggle off: nothing changes, anywhere. Arm it and drive normally — curves at city and highway speeds both feed it; the factors move in 0.01 steps you can watch in the menu, and Erase Calibration Memory returns everything to stock in one tap.
• Offline: point the estimator at any logged drive (bp-tools angle_autocal_analyze.py) and compare its fit to what the car converged on.

Risks / limits

• Angle-mode Ford only; everything is gated behind the primary-control-variable = angle setting. Off by default; factors stay inside the existing 0.5–1.5 clip.
• Known hardening in flight: a step that fails its post-step verification is currently held (blocked from further movement until fresh evidence) but not walked back — observed in the field as slow upward factor drift over multiple drives when evidence quality is poor. The walk-back + evidence-discount fix is queued as a follow-up commit on this PR; until then Erase Calibration Memory fully resets.
• Manual factor edits always win: the controller detects a hand-edit and soft-resets its confidence rather than fighting the driver.

Part 1 of 3: anti-weave smoothing (#171) and the on-device gauges (#172) are stacked on this PR.

[ghbarker]

Rebased onto latest bp-dev (picks up the FORD_EDGE_MK2 pinion-toggle commit that touches the same settings files) and addressed the full design-review findings with five new commits:

• Review blockers 1–3: dead human_turn/stall_blip parameters removed from the gate/pipeline signatures (those frames idle the pipeline before the call site); the stall-blip branch now clears the saturation flag and idles the pipeline like the other early returns; errors write to their own FordAngleAutoCalError param so a transient write failure can never overwrite accumulated evidence.
• Actuation-delay protections: evidence collects only while liveDelay.status == estimated (the measurement chain shares locationd's warmup), and a steady-window drift bound caps the lag bias of slow ramps regardless of the actual delay.
• 5.2: the gain model (platform gains, CAR sets, platform_gains(), V_LOW/V_HIGH/LOW_ANCHOR_BASE) moved to values_ext.py — strategy owns it, calibrator consumes it; the noqa'd back-import and an inline duplicate are gone.
• P4: new AutoCalController owns the whole lifecycle (params, arm/disarm, persistence, edit debounce, nudges, errors, telemetry); the strategy is down to poll_params()/feed()/idle() plus two read-only telemetry properties.
• 5.3 sweep: frozen Frame dataclass replaces the 12-parameter update clump; admission predicate computed once in the gate; NamedTuple peak buffer; float-tolerance user-edit detection; dead publisher param-cache entries dropped. The reported safety.c off-by-one turned out to be a false positive (the geometry table is 1-based with a sentinel row 0 — verified by tightening the check and watching the geometry test fail) and is now documented at the accessors.

On-road result: on a ~1 h drive (route 0000000b--d5********, Mach-E) the calibrator converged and locked at 1.00/1.14 (stderr 0.023/0.025, targets within deadband for 300 s). Also guarded here: the low anchor accumulates ~5× less evidence than the high one and wandered 0.98→1.06→1.00 within that drive (felt as curve turn-in overshoot through the 1.30× branch); MAX_DRIVE_DELTA_LOW = 0.04 bounds it, and the same drive now replays 0.96→0.98→1.00 monotonically to the identical lock.

47 tests (incl. the onboard-glue suite) pass; the reference-route replay is byte-identical through the refactors.

🤖 Generated with Claude Code

[ghbarker]

Two small additions since the reformat: the evidence Frame is now only constructed while the calibrator is armed (it was built and discarded every frame for drivers with the toggle off — caught by downstream sim tooling whose minimal mock had no wheelSpeeds), and the toggle is now exposed in sunnylink with the same angle-mode gating as the on-device surfaces.

The anti-weave smoothing feature is now up as #163, stacked on this branch — merging this first shrinks that one to its own two commits.

🤖 Generated with Claude Code