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

[ghbarker]

The evidence that motivated this

• The angle-mode speed factors (FordLowSpeedFactor_ang / FordHighSpeedFactor_ang) correct the PSCM's imperfect path-angle-to-turn conversion, and today they are hand-tuned: watch the requested-vs-actual curvature traces, compare the tops and bottoms of the graphs, tap +/- until the peaks line up. That is per-car (tires and alignment shift it), easy to get subtly wrong (no error bars on an eyeball), and leaves no record of why a value was chosen.
• Mining the reference drive (00000006--31********) showed the raw material is there to automate it: steady engaged curves and matched command/measured curvature apexes carry direct gain information — but only after aggressive filtering. The same logs are full of counterfeits: bump flicks (measurement-rate spikes while the command is quiet), rough-surface stretches, tire-limit corners where cmd != meas is physics, longitudinal load transfer, sub-threshold driver grip, and a ~0.10 left/right gain split at the high anchor from road crown that a naive estimator happily averages in.
• Found while wiring telemetry, standalone-worthy: ControllerStateBP dataclass fields must be declared in opendbc/car/structs.py to survive convert_to_capnp — undeclared setattr is silently dropped and the capnp default gets logged. Three fields were being written by the publisher and never reached the wire, which made the device look disarmed in every log while the controller ran fine.
• A first on-road span (0000000b--d5********, ~1 h) surfaced one more: the low anchor accumulates ~5x less evidence than the high one (city curve frames are mostly grip/accel-rejected), so it wandered 0.98 -> 1.06 -> 1.00 inside one drive — felt from the seat as curve turn-in overshoot through the 1.30x curve branch.

Why the fix is shaped this way

• Continuous, not one-shot. An earlier converge-and-verify design kept evidence in memory and lost it at ignition-off ("drove, rebooted, nothing changed"). Here the estimator's sufficient statistics serialize into FordAngleAutoCalState (JSON, saved every 30 s of dirty state) and restore on arming — evidence spans drives and ignition cycles by construction.
• Apex evidence exists because winding roads never go steady. A dominant, prominent command extremum is matched to its same-sign measured twin inside the actuator-lag horizon, median-of-3 committed. Steady-state evidence alone would starve exactly where the factors matter most.
• Reject first, fit second. Every counterfeit above has a dedicated gate, each with a persisted per-cause counter so the offline analyzer can show what was rejected and why. Left/right divergence inflates effective stderr and blocks action until evidence balances — crown bias stalls the calibrator instead of poisoning it.
• Actuation delay is designed around, then gated anyway. Steady windows and amplitude-ratio apexes are delay-insensitive by construction; on top, evidence collects only while liveDelay.status == estimated (the measurement chain shares locationd's warmup), and a window-total drift bound caps the lag bias of slow ramps at any delay.
• Nudges are bounded and visible. At most 0.02 per step, 20 s apart, per-drive caps (0.04 low / 0.10 high — the low cap is the wander guard from the evidence above), only with sufficient anchor weight and stderr. It writes the same params the +/- buttons use, so the menu shows it working; a manual tap always wins (two-tick debounce against async-put lag, soft evidence reset, never a wipe).
• Lifecycle is its own class. AutoCalPipeline is pure math with no I/O, shared byte-for-byte with the offline analyzer; AutoCalController owns params, arm/disarm, persistence, edit debounce, nudge writes, and errors; the strategy is down to poll_params() / feed() / idle(). Errors report to their own FordAngleAutoCalError param — a transient write failure can never overwrite accumulated evidence.
• Locks when there is nothing left to do: both anchors with 60 s+ of weight, stderr in bounds, applied within 0.03 of target for 300 s of driving. Toggling off clears everything for a fresh start.

The evidence afterwards

• On-road (Mach-E GT, comma four): factors walked 1.00/1.00 to convergence over four drives with evidence persisting across ignition cycles, and on 0000000b--d5******** (~1 h) the calibrator locked at 1.00/1.14 (stderr 0.023/0.025, targets inside the deadband for 300 s). Independent check of the converged values: median actual/requested curvature ratio 1.000 (city band) / 0.993 (highway band).
• The wander guard, replayed over the same drive: low walks 0.96 -> 0.98 -> 1.00 monotonically to the identical lock — the 0.06 round trip is gone.
• 47 tests: estimator/peaks/quality/nudger/lock units, synthetic closed-loop truth recovery with injected bumps and grips, and onboard-glue tests driving the real strategy with type-checked mock params (the fork's params are typed FLOAT; an untyped mock once hid a string-write bug that silently killed every nudge on-device).
• Full card process replayed clean over 2.7 h of Mach-E rlogs with the calibrator armed; the analyzer replays the exact onboard pipeline (same imports) with virtual nudging for offline threshold work. The reference-route replay is byte-identical through all of the review-driven refactors below.
• One review finding reversed itself: the reported off-by-one in the safety.c debug accessors is a false positive — the pinion geometry table is sized COUNT+1 with slot 0 as the sentinel (the index rides bits 1-4 of the safety param). Verified by tightening the check and watching the geometry test fail on the last row; now documented at the accessors so it doesn't get "fixed" again.

What each change is

• opendbc/sunnypilot/car/ford/angle_autocal.py: the pure estimator — Frame dataclass, quality monitor, steady gate (with the drift bound), peak matcher, weighted LSQ with forgetting + L/R divergence, nudge policy, lock.
• opendbc/sunnypilot/car/ford/angle_autocal_controller.py: AutoCalController — params handle, arm/disarm, JSON persistence + save cadence, user-edit debounce, nudge writes, liveDelay warmup gate, error reporting, telemetry status.
• opendbc/sunnypilot/car/ford/lateral_angle_ext.py: three controller calls + Frame construction; the stall-blip branch now idles the pipeline and clears the saturation flag like the other early returns.
• opendbc/sunnypilot/car/ford/values_ext.py: the gain model (platform gain pairs, CAR sets, platform_gains(), V_LOW/V_HIGH/LOW_ANCHOR_BASE) lives with the strategy; calibrator and analyzer consume it.
• opendbc/car/structs.py: declare bmsAngleAutoCalibrate, bmsAngleAutoCalState, angleSaturated (the capnp-bridge fix).
• cereal/custom.capnp: the three telemetry fields.
• common/params_keys.h: register FordAngleAutoCal (BOOL), FordAngleAutoCalState (STRING), FordAngleAutoCalError (STRING, diagnostics-only channel).
• bluepilot/selfdrive/car/bp_card_publisher.py: publish armed/status from the live controller only (the param-snapshot copies were dead and a second source of truth).
• selfdrive/ui/bp/layouts/settings/bluepilot.py + mici/.../lateral_mici.py: the toggle on both settings surfaces, angle-mode gated.
• opendbc/safety/tests/libsafety/safety.c: documentation comment at the geometry accessors (see false positive above).
• opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py: the 47-test suite.

Test it yourself

• Driver-facing docs ship with the branch: docs/ford-angle-autocal.md — what to expect, timelines, troubleshooting.

• Toggle Auto-Calibrate Adjustment Factors in lateral tuning (angle mode) and drive: the +/- menu values move on their own in 0.02 steps; the telemetry status in logs (bmsAngleAutoCalState) reads armed n=... w=.../... applied=... nudges=..., then locked.

• Tap +/- mid-collection: your value sticks and the logged status reflects the soft reset.

• Offline, on any logged drive: python bp/angle_autocal_analyze.py <dir> <route> (bp-tools) replays the exact pipeline and writes an HTML report of every accepted/rejected sample plus the virtual nudge timeline.

• Toggle off and on: state clears, collection restarts from scratch.

Risks / limits

• Angle mode only; other brands and curvature mode never construct the pipeline — the toggle is greyed outside angle mode.
• Default OFF — zero behavior change until opted in; a locked calibration stops adjusting until the user toggles off/on.
• The low anchor learns slowly by design (city curves are mostly rejected as contaminated); the per-drive cap bounds any single drive's effect on curve feel to two steps.
• Evidence pauses (pipeline idles) until locationd's delay estimator reports estimated — costs the warmup minutes of each drive.
• MIN_KAPPA (0.001) must stay at or above the curvature-gain interp knee in the strategy — they are the same number by design and must move together (noted at both sites).

🤖 Generated with Claude Code

[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