Ford: anti-weave smoothing of the angle-mode command path (opt-in strength dial)

bluepilot/selfdrive/car/bp_card_publisher.py

@@ -104,6 +104,9 @@ def _refresh_settings_cache() -> dict:
     "bmsMinimumSpeedToPauseLaneChange": _get_int(p, "BlinkerMinLateralControlSpeed", 20),
     "bmsShowLateralControlMode":       _get_bool(p, "BpShowLateralControl"),
     # --- Angle Tuning ---
+    # bmsAngleAutoCalibrate / bmsAngleAutoCalState are intentionally NOT here: they are
+    # ground truth from the live controller (set below from CI.CC every publish) — a
+    # param-snapshot copy would be a second source of truth that is silently overwritten.
     "bmsLowSpeedAdjustmentFactor":     _get_float(p, "FordLowSpeedFactor_ang", 1.0),
     "bmsHighSpeedAdjustmentFactor":    _get_float(p, "FordHighSpeedFactor_ang", 1.0),
     "bmsLaneChangeFactorHighAngle":    _get_float(p, "lane_change_factor_high_ang", 1.0),
@@ -131,6 +134,7 @@ def publish_controller_state_bp(CI, pm):
     cs_bp.curvatureDeviationLimited = getattr(CI.CC, "curvatureDeviationLimited", False)
     cs_bp.humanTurnLateralPaused = bool(getattr(CI.CC, "humanTurnLateralPaused", False))
     cs_bp.stallBlipActive = bool(getattr(CI.CC, "stallBlipActive", False))
+    cs_bp.angleSaturated = bool(getattr(CI.CC, "bp_angle_saturated", False))
 
     # BluePilot: settings snapshot -- refreshed at most every _SETTINGS_INTERVAL s so Params
     # reads don't add latency to every card.py tick.
@@ -145,6 +149,17 @@ def publish_controller_state_bp(CI, pm):
       for field, value in _settings_cache.items():
         setattr(cs_bp, field, value)
 
+    # BluePilot: auto-cal fields are GROUND TRUTH from the live controller, not the param
+    # snapshot — a device once had params armed while the controller ran disarmed, and the
+    # param-sourced telemetry made that undiagnosable from logs. bp_autocal_status carries
+    # the controller's own view (armed/evidence/nudges, "off", "locked", or an error).
+    cc = CI.CC
+    if hasattr(cc, "autocal_enabled"):
+      cs_bp.bmsAngleAutoCalibrate = bool(cc.autocal_enabled)
+    status = getattr(cc, "bp_autocal_status", "")
+    if status:
+      cs_bp.bmsAngleAutoCalState = str(status)
+
     # BluePilot: fingerprint info -- plain attribute reads on CarParams, no Params round-trip
     # needed, so no caching required (fingerprint never changes after startup).
     CP = getattr(CI, "CP", None)

cereal/custom.capnp

@@ -548,6 +548,9 @@ struct ControllerStateBP @0xcd96dafb67a082d0 {
   # --- Fingerprint (not a menu item, but requested alongside the settings snapshot) ---
   bmsFingerprintForced @52 :Bool;  # true when CarParams.fingerprintSource == fixed (CarPlatformBundle / FINGERPRINT env)
   bmsFingerprint @53 :Text;  # CarParams.carFingerprint
+  bmsAngleAutoCalibrate @54 :Bool;  # FordAngleAutoCal (one-time speed-factor auto-calibration)
+  bmsAngleAutoCalState @55 :Text;  # FordAngleAutoCalState ("" collecting, "done ..." locked)
+  angleSaturated @56 :Bool;  # angle mode: PSCM authority limit or DBC clamp modified this frame's command
 }
 
 struct CarStateBP @0xb057204d7deadf3f {

common/params_keys.h

@@ -309,6 +309,11 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LC_PID_gain_UI_curv", {PERSISTENT | BACKUP, FLOAT, "3.0"}},
     {"FordLowSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
     {"FordHighSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
+    {"FordAngleAutoCal", {PERSISTENT | BACKUP, BOOL, "0"}},           // one-time auto-calibration of the angle speed factors
+    {"FordAngleAutoCalState", {PERSISTENT | BACKUP, STRING, ""}},     // "" = collecting; JSON = evidence; "locked"/"done ..." = finished
+    {"FordAngleAutoCalError", {PERSISTENT, STRING, ""}},              // diagnostics only — separate channel so an error can never clobber evidence
+    {"FordAngleSmoothing", {PERSISTENT | BACKUP, BOOL, "1"}},         // anti-weave smoothing of the angle command path
+    {"FordAngleSmoothStrength", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // 0.0=minimal .. 1.0=tuned default .. 1.5=strong
     {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}},
 
     {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}},

docs/ford-angle-autocal.md

@@ -0,0 +1,130 @@
+# Ford Angle-Mode Auto-Calibration — User Guide
+
+BluePilot can tune your car's two angle-mode adjustment factors for you, while you
+drive, using exactly the comparison you'd do by hand — and stop when it's done.
+
+---
+
+## What it does (and why you'd want it)
+
+On Fords running **angle mode**, BluePilot sends the car a target steering angle and the
+car's power-steering computer (the PSCM) turns the wheel. That conversion isn't perfectly
+1:1, and it drifts from car to car with tires, alignment, and platform. Two menu values
+correct for it:
+
+- **Low Speed Adjustment Factor** (`FordLowSpeedFactor_ang`)
+- **High Speed Adjustment Factor** (`FordHighSpeedFactor_ang`)
+
+The manual tuning method is: drive, plot requested vs. actual turn, compare the tops and
+bottoms of the two curves, tap +/- until the peaks line up, repeat. It works, but it's
+per-car, slow, and easy to get subtly wrong.
+
+**Auto-calibration automates that exact loop.** It watches requested vs. actual curvature
+in real time, collects evidence only from clean cornering, and nudges the same two menu
+values you would have tapped — in small steps, with statistical error bars instead of an
+eyeball. When there's nothing left to adjust, it **locks** and stops touching anything.
+
+## Requirements
+
+- A Ford running BluePilot with **Lateral Control set to Angle** (the toggle is greyed
+  out in curvature mode).
+- Nothing else. It's **off by default** and changes nothing until you turn it on.
+
+## Turning it on
+
+**comma 3X:** Settings → BluePilot → Lateral Tuning → **Auto-Calibrate Adjustment Factors**
+
+**comma four:** Lateral menu → **Auto-Calibrate Factors**
+
+**Sunnylink:** [Lateral Tuning] Auto-Calibrate Adjustment Factors
+
+Then just drive normally with lateral engaged.
+
+## What you'll see
+
+Open the Lateral Tuning menu during or after a drive: the low/high factor values **move on
+their own**, at most 0.02 at a time. That's it working. There's no ceremony — the
+calibrator uses the same values the +/- buttons use, so the menu is always the truth.
+
+What it's doing underneath:
+
+- Evidence comes from **steady engaged curves** and from **curve apexes** (the "tops and
+  bottoms of the graphs"), so winding roads count even when nothing is steady.
+- Everything suspicious is thrown away: potholes and bump-flicks, rough washboard
+  surfaces, hard braking/accelerating, tire-limit cornering, any moment your hands are on
+  the wheel (plus a cooldown after), and crowned/banked roads that push all the evidence
+  to one side.
+- Evidence **survives ignition cycles** — progress is saved every 30 seconds and picked
+  up on the next drive.
+
+## How long does it take?
+
+Honest answer from real drives: **roughly an hour of mixed driving**, but it depends
+heavily on the roads.
+
+- **Highway curves calibrate the high factor quickly** — sweeping interstate curves are
+  ideal evidence and pile up fast.
+- **The low factor is slower on purpose.** City cornering is exactly where hands, bumps,
+  and sharp maneuvers contaminate the data, so most of it is rejected. Gentle 25–45 mph
+  curvy roads with hands off are what it wants.
+- The first few minutes of every drive contribute nothing — the calibrator waits for the
+  car's own sensor-calibration stack to warm up before trusting any measurement.
+
+You don't have to do anything special. It gets there on normal driving; special trips
+just get there sooner.
+
+## Your +/- buttons still win
+
+Tap +/- any time, calibrating or not. Your value is adopted immediately and the
+calibrator treats it as a strong hint — it softens its accumulated evidence rather than
+fighting you. It will only move the value again if fresh driving data genuinely disagrees.
+
+## Locking
+
+When both factors have solid evidence behind them and the applied values have sat within
+0.03 of the statistical target for 5 minutes of driving, the calibration **locks**:
+
+- The factors stop changing. Permanently, for this car.
+- The toggle stays on but does nothing further.
+
+**To recalibrate** (new tires, alignment work, seasonal tire swap, or you just want a
+fresh pass): toggle it **off and back on**. That clears everything and starts a clean
+collection.
+
+## What it will never do
+
+- It never moves a factor more than **0.02 per step**, **0.10 per drive** for the high
+  factor and **0.04 per drive** for the low factor — one drive can't transform how your
+  car steers.
+- It never acts on thin data: each factor needs sustained clean evidence and a tight
+  error bar before its first nudge.
+- It never runs in curvature mode, never runs while locked, and never runs before the
+  measurement stack is warmed up.
+- Turning the toggle off stops it instantly and clears its state.
+
+## Troubleshooting
+
+| Symptom | Likely reason |
+|---|---|
+| Factors never move | Normal for the first drives — evidence takes time, and city-heavy driving is mostly rejected by design. Check you're in angle mode and the toggle is on. Highway curves speed things up. |
+| Factors moved, then stopped | It probably **locked** — that's success. Toggle off/on if you want a re-run. |
+| Low factor barely changes while high converged | Expected — see "How long does it take?". Gentle mid-speed curves with hands off are the low anchor's food. |
+| A value looks wrong after calibration | Tap +/- to your preferred value; the calibrator adopts it. If it drifts back, the data disagrees with you — try a re-run after checking tire pressures/alignment. |
+| Suspected fault | The calibrator writes any internal error to the `FordAngleAutoCalError` param (visible in logs) instead of failing silently — include it when reporting. |
+
+## For the curious
+
+The estimator is pure math shared byte-for-byte with an offline analyzer. If you upload
+your drives, anyone can replay exactly what the car's calibrator saw — every accepted
+sample, every rejection and its reason, and the nudge-by-nudge timeline:
+
+```
+python bp/angle_autocal_analyze.py <folder-with-rlogs> <route-id>
+```
+
+(from the [bp-tools](https://github.com/ghbarker/bp-tools) repo; writes a self-contained
+HTML report.)
+
+That analyzer is also how the feature was tuned and validated: thresholds were chosen on
+logged reference drives, and every code change is checked by replaying a known drive and
+confirming the calibrator's decisions are unchanged.

docs/ford-angle-smoothing.md

@@ -0,0 +1,111 @@
+# Ford Angle-Mode Anti-Weave Smoothing — User Guide
+
+BluePilot can damp the slow left-right "rhythmic centering" weave some Fords show in
+angle mode on straight roads — with a strength dial you control, and a guarantee that
+the neutral setting is exactly stock steering.
+
+---
+
+## What it does (and why you'd want it)
+
+Some angle-mode Fords develop a gentle, rhythmic side-to-side motion on straights — the
+car works the wheel every few seconds even though the lane is dead straight. Log analysis
+traced it to a feedback loop: tiny curvature noise crosses internal thresholds, those
+crossings modulate the steering command, and the car's power-steering computer integrates
+the result into motion you can feel.
+
+Smoothing breaks that loop at its sources: it filters the noise that feeds the thresholds
+and holds the steering command steady when changes are smaller than what the car can even
+represent on the wire. It does **not** slow the steering down — all shaping is on the
+input side, and curve entry is deliberately left fast at every strength.
+
+Measured on the same car, straight-road driving, with and without:
+
+| | Without | With (menu 1.8) |
+|---|---|---|
+| Slow lane sway (the weave) | ±0.46 m | **±0.26 m (−43%)** |
+| Fast wheel-working dither | ±0.033 m | ±0.022 m (−33%) |
+
+Curve entry and exit were checked specifically: entries are unaffected by design, and
+exits measured *cleaner* with smoothing on.
+
+## The strength dial
+
+**Settings → BluePilot → Lateral Tuning → Smooth Steering (Anti-Weave)** — a master
+toggle plus a **Smoothing Strength** stepper. Also on the comma four lateral menu and in
+Sunnylink.
+
+The scale is deliberately simple:
+
+- **1.0 — stock.** Not "a little smoothing": *bit-for-bit identical* to the feature not
+  existing. This is verified by an automated test on every change.
+- **1.1 – 1.9** — increasing damping.
+- **2.0 — the tuned setting.** Chosen on logged drives; this is where the numbers in the
+  table above come from (measured at 1.8, tuned default 2.0).
+- **2.5 — maximum.** More damping, with a measurable cost (below). For cars that still
+  weave at 2.0.
+
+The toggle defaults ON with strength 1.0 — which means **stock behavior until you step
+the strength up**. Damping is always your explicit choice.
+
+## What it costs
+
+Nothing is free in a control loop. The closed-loop simulator puts numbers on it:
+
+| Setting | Lane-keeping tightness (std) |
+|---|---|
+| 1.0 / off | 0.039 m |
+| 2.0 | 0.042 m (+3 mm) |
+| 2.5 | 0.047 m (+8 mm) |
+
+At the tuned setting you trade **three millimeters** of station-keeping for roughly half
+the weave. At 2.5 the cost triples for diminishing extra damping — try 2.0 first.
+
+## How to find your setting
+
+1. Leave strength at 1.0 for a drive or two so you know your baseline.
+2. If you feel the slow weave on straights, step to **2.0** and drive the same roads.
+3. Still feel it? Step toward 2.5 one notch at a time.
+4. If the car ever feels *lazier* than you like, step back down — every value between
+   1.0 and your current setting is a valid operating point.
+
+Changes take effect within a second (no reboot), and stepping strength mid-drive is safe:
+the filters are built to pick up from the live steering state, never from stale values.
+
+## What it will never do
+
+- **1.0 is stock, provably.** The passthrough is tested for bit-identity, not "close".
+- It never adds lag on top of the steering output — that specific design was tested in a
+  closed-loop simulator, measured to *hurt* lane-keeping 2.5x, and rejected. Input-side
+  shaping only.
+- Curve entry speed is independent of strength: the entry filter is fixed-fast, and an
+  automated test pins it.
+- All of it disengages instantly with lateral control, and every filter resets across
+  takeovers — no state survives a disengagement, a driver override, or a steering pause.
+
+## Troubleshooting
+
+| Symptom | Likely reason |
+|---|---|
+| No difference at 1.0 | Correct — 1.0 *is* stock. Step up to feel the feature. |
+| Still weaving at 2.0 | Step toward 2.5. If maxed and still weaving, report it with a route ID — your car may need the underlying factors calibrated first (see the auto-calibration guide). |
+| Feels slow into curves | Not a smoothing effect at any strength (entry is fixed-fast) — check your speed-factor calibration instead. |
+| Wandering within the lane | Distinguish: the weave is *rhythmic* (a steady few-second cycle); random wander is usually crosswind, crown, or camera calibration. Smoothing targets the rhythm. |
+
+## Relationship to auto-calibration
+
+They're complementary and independent. Auto-calibration fixes the *average* correction
+(how much turn you get per command); smoothing fixes the *oscillation* around it. A car
+with badly-off factors can weave for that reason alone — calibrate first, then judge how
+much smoothing you still want.
+
+## For the curious
+
+The smoothing math lives in one pure, unit-tested module
+(`opendbc/sunnypilot/car/ford/angle_smoothing.py`) with five elements: a hysteresis on
+the curve-entry decision, a low-pass on the model's predicted curvature, a slew on the
+exit blend, an asymmetric filter on the gain schedule (the primary fix — fast attack,
+strength-scaled release), and a one-LSB hold on the outgoing wire value. The measurement
+tooling — spectral weave analysis of any logged drive, and the closed-loop simulator used
+to bound the costs above — lives in [bp-tools](https://github.com/ghbarker/bp-tools)
+(`bp/angle_weave_analyze.py`, `sim/closed_loop_weave.py`).

opendbc_repo/opendbc/car/structs.py

@@ -181,6 +181,7 @@ class ControllerStateBP:
   curvatureDeviationLimited: bool = False  # current_curvature error-clip constrained the command this frame
   humanTurnLateralPaused: bool = False  # angle mode: lateral forced inactive (mode 0) during a manual turn
   stallBlipActive: bool = False  # angle mode: brief mode-0 pulse resetting PSCM authority after a post-override stall
+  angleSaturated: bool = False  # angle mode: PSCM authority limit or DBC clamp modified this frame's command
 
   # BluePilot: full BluePilot-menu settings snapshot -- see custom.capnp ControllerStateBP for
   # field-by-field param-key mapping and the field-retirement convention.
@@ -224,6 +225,11 @@ class ControllerStateBP:
   bmsMinimumSpeedToPauseLaneChange: int = 20
   bmsShowLateralControlMode: bool = False
   # --- Angle Tuning ---
+  # NOTE: a field must be declared HERE to survive convert_to_capnp — the publisher's
+  # setattr on an undeclared name is silently dropped at conversion, publishing the capnp
+  # default instead. That gap muted the auto-cal telemetry on every build until 2026-07-22.
+  bmsAngleAutoCalibrate: bool = False
+  bmsAngleAutoCalState: str = ""
   bmsLowSpeedAdjustmentFactor: float = 1.0
   bmsHighSpeedAdjustmentFactor: float = 1.0
   bmsLaneChangeFactorHighAngle: float = 1.0

opendbc_repo/opendbc/safety/tests/libsafety/safety.c

@@ -259,6 +259,10 @@ uint16_t get_current_safety_param_sp(void){
 }
 
 // BluePilot: debug getters for the Ford pinion geometry table (ALLOW_DEBUG builds only).
+// NOTE: idx <= COUNT is NOT an off-by-one. The table is declared
+// ford_pinion_geometry[FORD_PINION_GEOMETRY_COUNT + 1U]: slot 0 is the disabled/sentinel
+// row and real rows are 1..COUNT (the index rides bits 1-4 of the safety param, where 0
+// means "no row"). ford.h's own bounds check is (index == 0 || index > COUNT).
 // Consumed by test_ford.py's geometry-consistency test, which compares every firmware row
 // against CarSpecs + calc_slip_factor(VehicleModel(CP)) so the table cannot rot as
 // platforms change -- without fragile header parsing.