From 1b8caa231c68184f404256d9d13d823a1f8f9866 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Thu, 30 Jul 2026 00:30:45 -0400 Subject: [PATCH] Ford angle-mode lateral: closed-loop auto-calibration of the speed factors MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A pure estimator (angle_autocal.py) fits the two gain anchors from clean cornering evidence — steady curve segments and matched command/measured apexes — and nudges the applied factors toward the fit in small damped steps, verifying each step against fresh evidence before the next. Evidence is only taken while the loop is quietly tracking (hunting can never read as gain) and while the actuation delay is estimated, aligned against the command from lateral_delay ago so a moving command on winding roads still counts. Lifecycle (arm/disarm, persistence, nudge writes, lock) lives in a controller; the same estimator runs offline against logged drives. The sub-knee small-signal lift rides the FordHighSpeedDampening_ang knob merged in #166 (mechanically the same multiplier); no duplicate param is added. Settings are on the on-device menus (comma 3X + comma four) and sunnylink. Menu neutral values leave steering bit-identical to today. --- bluepilot/selfdrive/car/bp_card_publisher.py | 15 + cereal/custom.capnp | 4 + common/params_keys.h | 5 + docs/ford-angle-autocal.md | 180 +++ opendbc_repo/opendbc/car/structs.py | 6 + .../opendbc/safety/tests/libsafety/safety.c | 4 + .../sunnypilot/car/ford/angle_autocal.py | 1033 +++++++++++++++++ .../car/ford/angle_autocal_controller.py | 229 ++++ .../sunnypilot/car/ford/lateral_angle_ext.py | 244 ++-- .../car/ford/tests/test_angle_autocal.py | 975 ++++++++++++++++ .../opendbc/sunnypilot/car/ford/values_ext.py | 39 + selfdrive/ui/bp/layouts/settings/bluepilot.py | 63 + .../bp/mici/layouts/settings/lateral_mici.py | 48 +- sunnypilot/sunnylink/settings_ui.json | 13 + .../settings_ui_src/pages/vehicle.yaml | 8 + 15 files changed, 2750 insertions(+), 116 deletions(-) create mode 100644 docs/ford-angle-autocal.md create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py diff --git a/bluepilot/selfdrive/car/bp_card_publisher.py b/bluepilot/selfdrive/car/bp_card_publisher.py index 7dfa8c5853..ad17c6ff7d 100644 --- a/bluepilot/selfdrive/car/bp_card_publisher.py +++ b/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: mode the controller actually ran, straight off the car controller (not Params). if getattr(CI.CC, "disable_BP_lat_UI", True): cs_bp.activeLateralMode = structs.ControllerStateBP.LateralMode.openpilot @@ -152,6 +156,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) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 99bc6d8bdf..93f725e068 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -558,6 +558,10 @@ struct ControllerStateBP @0xcd96dafb67a082d0 { curvature @1; angle @2; } + + bmsAngleAutoCalibrate @55 :Bool; # FordAngleAutoCal toggle state + bmsAngleAutoCalState @56 :Text; # live controller status (bp_autocal_status): "off"/"locked"/"reset" or armed JSON + angleSaturated @57 :Bool; # angle mode: PSCM authority limit or DBC clamp modified this frame's command } struct CarStateBP @0xb057204d7deadf3f { diff --git a/common/params_keys.h b/common/params_keys.h index 3248d6e8d3..a20961c03d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -310,6 +310,11 @@ inline static std::unordered_map keys = { {"FordLowSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, {"FordHighSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, {"FordHighSpeedDampening_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 + {"FordAngleAutoCalReset", {PERSISTENT, BOOL, "0"}}, // erase calibration memory: controller wipes evidence + resets factors to 1.00, then clears this + {"FordAngleAutoCalLock", {PERSISTENT | BACKUP, BOOL, "1"}}, // on: calibration freezes when stable (default); off: never locks, keeps adapting — turning off an existing lock resumes it {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/docs/ford-angle-autocal.md b/docs/ford-angle-autocal.md new file mode 100644 index 0000000000..857ad63969 --- /dev/null +++ b/docs/ford-angle-autocal.md @@ -0,0 +1,180 @@ +# 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 engaged curves — including **winding roads where the wheel never + stops moving**: the comparison is made against the command from the car's own measured + steering delay ago, so a continuously changing curve reads correctly instead of being + discarded. Curve **apexes** (the "tops and bottoms of the graphs") count separately. +- 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. +- **Only calm data counts.** Evidence is taken solely while the steering loop is quietly + tracking — the moments when the car is swinging wide or catching itself back are the + loop's dynamics, not the car's gain, and they are refused outright. A step takes as + many curve passes as calm data requires; a slower right answer beats a faster wrong one. +- **Every adjustment is checked before the next one.** After a step, the calibrator + collects a fresh batch of clean curves *at the new value* and confirms the step + actually brought the car **closer to doing exactly what's asked** (the measured + response strictly nearer 100% of requested than before). Confirmed → it keeps going. + Contradicted → it stops moving that factor and demands twice the evidence before + trying again. Poll a couple turns, adjust, poll some more — enforced, not hoped. +- Evidence **survives ignition cycles** — progress is saved every 30 seconds and picked + up on the next drive. + +## Watching it live from your phone + +The [phone graph page](lateral-phone-graph.md) (`http://192.168.43.1:8088/lateral` on the +device hotspot) shows a **calibration dashboard** whenever the calibrator is armed: one +card per speed band (low, under 30 mph / high, over 60 mph) with + +- how much clean-curve evidence each band has collected (and how much it needs), +- what the car is measured doing right now — e.g. **"turns 93% of requested"**, +- the current factor and the step it wants to try next — **"factor 1.00 → try 1.08"**, +- live *checking…* progress while a fresh step is being verified, and whether the last + step **confirmed ✓** or didn't. + +A pill in the corner shows which band your current speed is feeding ("42 mph · blend +zone"). Between 30 and 60 mph evidence splits between both anchors. + +## 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. + +**The lock is optional.** A **Calibration Lock** toggle sits next to the main switch +(default on). Turn it off and the calibrator never freezes — it keeps adapting for as +long as the main toggle is on. Turning the lock off on an *already locked* car resumes +calibration from its saved evidence, losing nothing; turning it back on re-enables +freezing once things are stable again. + +**To recalibrate** (new tires, alignment work, seasonal tire swap, or you just want a +fresh pass): toggle it **off and back on**. That clears the evidence and starts a clean +collection *from the current factor values*. + +## Erase Calibration Memory + +Next to the toggle sits **Erase Calibration Memory** — the full do-over. One tap: + +- wipes all collected evidence and any lock, +- clears the calibrator's error log, +- and puts **both factors back to 1.00** (stock). + +Use it when a calibration run went somewhere you don't trust and you want to retry from +a clean slate rather than from wherever the factors ended up. It works offroad or +mid-drive (takes effect within a second while driving), and the phone dashboard shows +"memory erased" when it lands. + +## What it will never do + +- It never moves a factor more than **0.02 per step**, and never steps the same factor + again until fresh driving data at the new value has confirmed the previous step. + There is deliberately **no cap on total movement** — a car that is genuinely far off + is allowed to walk all the way to its fit — because every step of that walk has to + keep verifying against the road. +- 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. | +| Whole run went somewhere you don't trust | **Erase Calibration Memory** — factors back to 1.00, evidence wiped, clean retry. | +| Steps keep showing "didn't verify" on the phone dashboard | The car's measured response is contradicting the model — usually bad data conditions (crosswind, rough roads, constant light grip). The calibrator is protecting you by refusing to walk further; give it cleaner roads. | +| 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 +``` + +(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. diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index ff03ba9ed8..5260ad4d86 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/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 diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index bbf30c89ea..7ad3595c56 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/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. diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py new file mode 100644 index 0000000000..b9bb9388b4 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -0,0 +1,1033 @@ +"""BluePilot: continuous auto-calibration of the angle-mode speed adjustment factors. + +The angle strategy (lateral_angle_ext.py) computes, for the high-curvature branch: + + factor(v) = interp(v, [V_LOW, V_HIGH], [1.30 * LOW_FACTOR, platform_gain * HIGH_FACTOR]) + path_angle = kappa_cmd * v * factor + +LOW_FACTOR / HIGH_FACTOR (FordLowSpeedFactor_ang / FordHighSpeedFactor_ang) are per-car +constants the driver is asked to hand-tune with the +/- buttons by comparing requested turn +to actual turn — watching the tops and bottoms of the two curvature traces and bumping the +factor until the peaks line up. This module automates exactly that loop, continuously: + + - evidence comes from curve apexes (matched command/measured curvature extrema, the + "tops and bottoms of the graphs") and from steady-state curve segments; + - a quality layer admits only clean linear-regime cornering — road bumps, rough + surface, tire-limit turns, longitudinal load transfer, driver grip and bank-biased + one-sided evidence are excised (with per-cause counters, so the offline analyzer can + show exactly what was rejected and why); + - as evidence accrues the applied factors are nudged in small bounded steps toward the + estimate — visible live in the lateral tuning menu — backing off automatically when a + step overshoots (each sample records the gain in force when it was taken, so the + estimate of the IDEAL gain is invariant to the nudge trajectory); + - the estimator's sufficient statistics serialize to/from the FordAngleAutoCalState + param, so evidence spans drives and ignition cycles; + - when there is nothing left to adjust for LOCK_STABLE_S of driving, the calibration + locks (per-car, not per-drive). Toggling the setting off clears everything. + +The estimator is pure math with no I/O so the exact same code runs in two places: + - offline, in bp-tools/bp/angle_autocal_analyze.py, replaying logged drives so the + behavior (and every accepted/rejected sample) can be inspected before touching the car + - onboard, fed from lateral_angle_ext during normal driving +""" +import math +from dataclasses import dataclass +from typing import NamedTuple + +# The strategy owns the gain model; this module (and the offline analyzer) consume it. +from opendbc.sunnypilot.car.ford.values_ext import V_LOW, V_HIGH, LOW_ANCHOR_BASE + + +@dataclass(frozen=True) +class Frame: + """One 20 Hz lateral frame of evidence inputs, shared verbatim by the onboard + controller and the offline analyzer. Every field is required — defaults on physical + signals (a_ego, saturated, ...) were a silent-wrong-answer risk whenever the two + consumers drifted on parameter order.""" + v_ego: float + kappa_cmd: float + kappa_meas: float + steering_pressed: bool + angle_rate_limited: bool + deviation_limited: bool + saturated: bool + driver_torque: float + a_ego: float + ws_spread: float | None + low_factor: float + high_factor: float + lateral_delay: float # liveDelay.lateralDelay (s) — evidence is aligned against cmd(t - delay) + +# Sample admission gates (mirrored by both the offline analyzer and the onboard hook). +MIN_SPEED = 9.5 # m/s; below this the deviation clip is off and measurement is noisy +MIN_KAPPA = 0.001 # 1/m; fully inside the high-curvature branch the factors scale +# --- Lag-aligned steadiness -------------------------------------------------------------- +# The measurement lags the command by the actuation delay (liveDelay: ~0.15-0.30s typical, +# up to ~0.42s observed). Evidence ratios are therefore taken against the command from +# lateral_delay seconds AGO (a short ring buffer in the pipeline), which removes the +# first-order lag bias outright instead of demanding a nearly frozen command. What remains +# is the DELAY ESTIMATE's own error (~±0.1s), so the bounds below only need to keep +# kappa's fractional change over that residual small: bias ≈ tau_err * |dk/dt| / |k| — +# at the 0.5/s relative bound and 0.1s residual, <= 5%, sign-symmetric over entries and +# exits, inside the stderr machinery. Validated on-road 2026-07-22 (routes 0b/12/13): +# vs the old frozen-command gate this recovers 3-5x the evidence on winding roads with +# the pooled fit unchanged (high 1.127 before and after, stderr 0.033 -> 0.012). +LAG_MIN_S, LAG_MAX_S = 0.10, 0.42 # trust clamp for the liveDelay estimate +# --- Loop-quietness admission ------------------------------------------------------------ +# Steady evidence is taken only while the tracking loop is CALM: the smoothed +# |meas - aligned cmd| error trend must be flat. Samples taken mid-excursion or +# mid-correction carry the LOOP's dynamics, not the plant's gain — audited on-road +# 2026-07-23: 39% of admissions were non-quiet and read up to 3% low, biasing the fit +# ~+0.01 high ("moving the needle and calling it done"). Convergence may take as many +# passes as calm data requires; a slower right answer beats a faster wrong one. +QUIET_TAU_S = 0.4 # smoothing of |tracking error| before its trend is judged +QUIET_ERR_RATE = 0.0002 # 1/m/s: |d|err|/dt| above this = loop dynamics, no evidence +MAX_KAPPA_RATE = 0.0015 # 1/m/s absolute floor of the admission rate bound +REL_KAPPA_RATE = 0.5 # 1/s: |dk/dt| may reach this fraction of |k| (winding roads) +STEADY_TIME_S = 0.3 # command steady this long before samples count (PSCM settle) +STEADY_DRIFT_FRAC = 0.25 # max |kappa - window start| as a fraction of |kappa| +MIN_RATIO, MAX_RATIO = 0.4, 2.5 # discard absurd ratios (measurement glitches) +MAX_LAT_ACCEL = 2.5 # m/s^2; kappa*v^2 above this is tire/comfort-limit territory, not gain error +# Near the limit cmd!=meas is physics, not gain error: evidence weight fades linearly to +# zero over the last LAT_ACCEL_SOFT_BAND m/s^2 below MAX_LAT_ACCEL. +LAT_ACCEL_SOFT_BAND = 1.0 +# Longitudinal load transfer changes the effective lateral gain — no evidence while +# braking/accelerating hard mid-curve. 1.0 rejected 8587 frames of ordinary city +# braking-into-corners on the reference drive (route 00000006); 2.0 keeps those and +# still cuts genuine hard stops (541 frames). +MAX_LONG_ACCEL = 2.0 # m/s^2 |aEgo| + +# Driver-contamination guards. Ford flips steeringPressed at STEER_DRIVER_ALLOWANCE (1.0 Nm) +# sustained — a light grip below that threshold still steers the car, and the PSCM under-delivers +# for seconds after any touch (post-override attenuation observed on the Mach-E). So: +TORQUE_GUARD_NM = 0.5 # treat half the pressed threshold as hands-on for calibration purposes +PRESS_HOLDBACK_S = 1.0 # samples are staged this long; any grip during staging cancels them +PRESS_COOLDOWN_S = 3.0 # after any grip ends, delivery is suspect this long — no samples + +# --- Disturbance / road-quality rejection ------------------------------------------------ +# All four thresholds below were tuned on the 2.7 h Mach-E reference drive (route +# 00000006--319e078ab5): tight enough to catch real disturbances, loose enough that the +# pinion-derived measurement's normal noise floor doesn't starve the estimator. +# A bump, pothole, crosswind gust or passing truck flicks the car without the command +# moving: measured curvature jumps while the command is steady. Not gain information. +SPIKE_MEAS_RATE = 0.02 # 1/m/s measured-curvature rate with a quiet command = disturbance +DISTURBANCE_BLANK_S = 1.0 # no evidence while a disturbance settles +DISTURBANCE_POISON_S = 0.3 # peak-buffer frames this far BACK from detection are suspect too +# A bump shakes the wheels before it shows in the pinion: a jump in the wheel-speed spread +# (max-min of the four wheels) corroborates and triggers/extends the blanking. +WS_SPREAD_JUMP = 0.6 # m/s frame-to-frame change of the spread +# Washboard / broken pavement: sustained high-frequency content in measured curvature. +# The residual is high-passed (curve content lives below ~1 Hz and stays in the low-pass), +# so ordinary cornering — sweepers, S-curves, apexes — can never trip this. +ROUGH_LP_TAU_S = 0.3 # low-pass defining the "curve content" of the measurement +ROUGH_RMS_TAU_S = 2.0 # window of the residual RMS +ROUGH_RMS_MAX = 0.0015 # 1/m residual RMS above this = rough stretch, no evidence + +# --- Peak (apex) evidence ---------------------------------------------------------------- +# The manual method compares the tops/bottoms of the requested vs actual curvature traces. +# A command apex is a local extremum that dominates a +-PEAK_HALF_WINDOW_S neighborhood +# with real prominence; its measured twin is the same-sign extremum within the actuator +# lag horizon. The amplitude ratio at the pair is direct gain evidence — available on +# winding roads where the steady-state gate never gets its STEADY_TIME_S. +PEAK_BUF_S = 2.5 # ring buffer length +PEAK_HALF_WINDOW_S = 1.0 # apex must dominate this neighborhood on both sides +PEAK_LAG_MAX_S = 0.7 # measured extremum searched this far after the command apex +PEAK_MIN_KAPPA = 0.0012 # 1/m minimum apex amplitude +PEAK_PROMINENCE = 0.0004 # 1/m above the window minimum — rejects ripple +PEAK_REFRACTORY_S = 1.0 # one apex per this interval +# Apex evidence carries real transient content (the plant attenuates fast transients a bit +# more than steady curves), so it supplements the steady evidence rather than dominating: +# on the reference drive w=3.0 pulled the low anchor ~0.02 above the steady-only fit, +# w=1.5 keeps the combined fit within ±0.015 of it while still covering winding roads +# where the steady gate never fires. +PEAK_WEIGHT_S = 1.5 # one clean apex counts like this many seconds of steady evidence +PEAK_MEDIAN_N = 3 # apexes commit as the median of this many — kills single outliers + +# --- Estimator robustness ---------------------------------------------------------------- +# Forgetting: slow enough that a normal drive's evidence equilibrium (commit rate x TAU) +# clears the lock threshold — the reference drive commits ~0.012-0.023 s/s per anchor, +# giving equilibria of ~90-165 s — while old drives still fade within a couple of hours +# of active collection. (An outlier gate against the running fit was tried here and +# removed: it is path-dependent — early evidence anchors the fit, then contradicting +# evidence gets rejected — and it visibly distorted the reference-drive fit. Robustness +# comes from the frame quality layer, the ratio sanity bounds, staging cancellation and +# the apex median-of-3 instead.) +TAU_EVIDENCE_S = 7200.0 # evidence forgetting time constant (seconds of active collection) +# Banked/crowned roads bias one turn direction. If the left- and right-turn estimates of an +# anchor diverge beyond LR_TOL the divergence excess inflates that anchor's effective stderr, +# blocking nudges and lock until balanced evidence arrives. +LR_TOL = 0.06 +LR_MIN_WEIGHT = 5.0 # per-direction weight before the divergence check means anything + +# --- Live nudging (the closed loop) ------------------------------------------------------ +NUDGE_PERIOD_S = 20.0 # at most one nudge per this much active collection +NUDGE_MIN_WEIGHT = 10.0 # anchor evidence before it may move its factor +NUDGE_MAX_STDERR = 0.06 # effective stderr must be at least this good +# Damped-proportional step: move a fraction of the remaining error toward the fit, quantized +# to the 0.01 menu step and capped. Big steps when far (fast convergence), single 0.01 steps +# near the target, and — because the target is invariant to the applied factor — it glides in +# without overshooting instead of crawling a fixed 0.02 or jumping past and reversing. +FACTOR_STEP = 0.01 # menu granularity (factors are 2-decimal) +NUDGE_GAIN = 0.7 # fraction of the remaining error per nudge (< 1 damps overshoot) +NUDGE_MAX_STEP = 0.05 # max factor change per nudge +_NUDGE_MAX_UNITS = round(NUDGE_MAX_STEP / FACTOR_STEP) # = 5 + +# --- Adjust-then-verify ------------------------------------------------------------------ +# 2026-07-22 design decision: the per-drive movement caps (0.10 high / 0.04 low) are GONE — +# a genuinely miscalibrated car must be allowed to walk all the way to its fit in one +# drive. What replaces them is an explicit check instead of a leash: every step is judged +# against FRESH post-step evidence before its anchor may step again. The response ratio +# (measured/commanded curvature) scales with the applied gain, so a step from g_old to +# g_new predicts the direction the ratio must move; a fast-forgetting per-anchor ratio +# tracker measures whether it actually did. Confirmed -> keep walking. Failed -> the data +# contradicted the model: hold that anchor until twice the normal evidence has spoken. +# This is the "poll a couple turns, adjust, poll some more" loop, made enforceable. +VERIFY_MIN_WEIGHT = 6.0 # fresh post-step evidence (s) before the step is judged +VERIFY_FAIL_HOLD_WEIGHT = 12.0 # a failed check demands this much evidence before stepping again +VERIFY_OK_BAND = 0.04 # |1 - ratio| inside this after a step = success outright +TAU_RECENT_S = 90.0 # recent-response forgetting (seconds of active collection) + +# --- Lock -------------------------------------------------------------------------------- +# LOCK_MIN_WEIGHT sits below the reference drive's decay equilibrium (~90 s on the weaker +# anchor) so a normally-driven car can actually reach it; the 5-minute stability window is +# the real proof that there is nothing left to adjust. +LOCK_MIN_WEIGHT = 60.0 # per-anchor evidence before locking is possible +LOCK_DEADBAND = 0.03 # applied factors this close to the estimate count as "nothing to adjust" +LOCK_STABLE_S = 300.0 # this much active collection with nothing to adjust => locked + +FACTOR_MIN, FACTOR_MAX = 0.5, 1.5 # same clamp as the settings +/- buttons + +# Rejection cause codes (order fixed: serialized as a dict of these keys). +REJ_CAUSES = ("flick", "rough", "limit", "accel", "grip") + + +def speed_alpha(v_ego: float) -> float: + """Blend position of v between the two anchors: 0 = pure low anchor, 1 = pure high.""" + if v_ego <= V_LOW: + return 0.0 + if v_ego >= V_HIGH: + return 1.0 + return (v_ego - V_LOW) / (V_HIGH - V_LOW) + + +def lat_accel_margin(kappa: float, v_ego: float) -> float: + """Evidence weight vs lateral accel: 1.0 well below MAX_LAT_ACCEL, fading linearly to 0 + over the last LAT_ACCEL_SOFT_BAND — near the limit cmd != meas is physics, not gain.""" + return min(1.0, max(0.0, (MAX_LAT_ACCEL - abs(kappa) * v_ego * v_ego) / LAT_ACCEL_SOFT_BAND)) + + +def fit_trustworthy(weight: float, stderr_eff: float, min_weight: float) -> bool: + """An anchor's fit is trustworthy enough to act on: enough evidence and a tight enough + effective stderr. Lock, nudge and the live status share this — they differ only in the + weight bar and in what they do with |target - applied| afterwards. A new eligibility + condition (e.g. a sensor-health gate) is added here once.""" + return weight >= min_weight and stderr_eff <= NUDGE_MAX_STDERR + + +def nudge_units(err: float) -> int: + """Damped, menu-quantized nudge size in whole FACTOR_STEP units, capped. Returns 0 inside + the (implicit) deadband — |NUDGE_GAIN * err| below half a step. Shared by the stepper and + the live status so 'would nudge' is defined in one place.""" + n = round(NUDGE_GAIN * err / FACTOR_STEP) + return max(-_NUDGE_MAX_UNITS, min(_NUDGE_MAX_UNITS, n)) + + +class AngleFactorEstimator: + """Weighted least-squares fit of the two gain anchors from curve samples. + + Model: the ideal gain at sample i is y_i = g_i / r_i where r_i is the measured/commanded + curvature ratio and g_i the gain that was IN FORCE when the sample was taken (recorded + per sample — the applied factors move while the nudger works, and old samples stay valid). + F_ideal(v) = (1-a)*A + a*B with a = speed_alpha(v), A = ideal low anchor + (1.30 * LOW_FACTOR), B = ideal high anchor (gain * HIGH_FACTOR). Linear in (A, B) -> + closed-form normal equations, accumulated incrementally so the onboard hook carries + O(1) state — and that state serializes to a dict for cross-drive persistence. + """ + + def __init__(self, platform_gain_high: float): + self.platform_gain_high = float(platform_gain_high) + # Normal-equation accumulators for min sum w*((1-a)A + aB - y)^2 + self.s_ll = 0.0 # sum w*(1-a)^2 + self.s_lh = 0.0 # sum w*(1-a)*a + self.s_hh = 0.0 # sum w*a^2 + self.s_ly = 0.0 # sum w*(1-a)*y + self.s_hy = 0.0 # sum w*a*y + self.s_w = 0.0 # sum w + self.s_wy2 = 0.0 # sum w*y^2 (for residual/stderr) + self.n = 0 + # Left/right split per anchor half for bank-bias detection: {(half, dir): [w, wy]} + # half: 0 = alpha < 0.5 (low anchor side), 1 = high side; dir: 0 = left, 1 = right. + self.lr = {(h, d): [0.0, 0.0] for h in (0, 1) for d in (0, 1)} + # Fast-forgetting per-anchor response ratio r = meas/cmd (TAU_RECENT_S): "what is the + # car doing RIGHT NOW under the current factors" — the adjust-then-verify check and + # the live dashboard read this, the long-memory fit above never does. + self.recent = {0: [0.0, 0.0], 1: [0.0, 0.0]} # half -> [w, sum w*r] + + def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, + applied_gain: float, weight: float = 1.0) -> bool: + """Add one curve observation taken while applied_gain was in force. + + kappa_cmd is the curvature the strategy converted to path_angle (post any clips); + kappa_meas is the pinion-derived measured curvature. Both in OP sign convention — + only same-sign, above-threshold pairs are accepted. Returns True if accepted. + """ + if abs(kappa_cmd) < MIN_KAPPA or v_ego < MIN_SPEED: + return False + if abs(kappa_cmd) * v_ego * v_ego > MAX_LAT_ACCEL: + return False # the car may physically be unable to make this turn — not gain information + if kappa_cmd * kappa_meas <= 0.0: + return False + r = kappa_meas / kappa_cmd + if not (MIN_RATIO <= r <= MAX_RATIO): + return False + y = float(applied_gain) / r + a = speed_alpha(v_ego) + w = float(weight) + la = 1.0 - a + self.s_ll += w * la * la + self.s_lh += w * la * a + self.s_hh += w * a * a + self.s_ly += w * la * y + self.s_hy += w * a * y + self.s_w += w + self.s_wy2 += w * y * y + self.n += 1 + half = 0 if a < 0.5 else 1 + acc = self.lr[(half, 0 if kappa_cmd > 0 else 1)] + acc[0] += w + acc[1] += w * y + rec = self.recent[half] + rec[0] += w + rec[1] += w * r + return True + + def decay(self, seconds: float): + """Exponential evidence forgetting: old drives fade so adaptation stays possible, + while the ~TAU saturation keeps lock thresholds reachable and stable. The recent + tracker forgets much faster (TAU_RECENT_S) — it must answer for the car as it is + now, not as it averaged over hours.""" + f = math.exp(-float(seconds) / TAU_EVIDENCE_S) + self.scale(f) + fr = math.exp(-float(seconds) / TAU_RECENT_S) + for rec in self.recent.values(): + rec[0] *= fr + rec[1] *= fr + + def scale(self, f: float): + self.s_ll *= f + self.s_lh *= f + self.s_hh *= f + self.s_ly *= f + self.s_hy *= f + self.s_w *= f + self.s_wy2 *= f + for acc in self.lr.values(): + acc[0] *= f + acc[1] *= f + for rec in self.recent.values(): + rec[0] *= f + rec[1] *= f + + def recent_response(self, half: int): + """(weight, mean ratio) of the fast tracker for one anchor half; ratio is None until + any evidence exists. Ratio 0.93 reads as 'turns 93% of requested'.""" + w, s = self.recent[half] + if w <= 1e-6: + return 0.0, None + return w, s / w + + @property + def weight_low(self) -> float: + """Effective sample weight attributed to the low anchor.""" + return self.s_ll + self.s_lh + + @property + def weight_high(self) -> float: + return self.s_hh + self.s_lh + + def lr_divergence(self, half: int) -> float: + """|mean_left - mean_right| of the implied ideal gain for one anchor half. + 0.0 until both directions carry LR_MIN_WEIGHT — one-sided evidence is not yet + proof of bias, it just hasn't been contradicted.""" + wl, yl = self.lr[(half, 0)] + wr, yr = self.lr[(half, 1)] + if wl < LR_MIN_WEIGHT or wr < LR_MIN_WEIGHT: + return 0.0 + return abs(yl / wl - yr / wr) + + def solve(self): + """Solve for the ideal anchors. Returns (low_factor, high_factor, stats) or None. + + low_factor / high_factor are the values to store in FordLowSpeedFactor_ang / + FordHighSpeedFactor_ang (already divided by the fixed anchor bases and clamped to + the same range the +/- buttons allow). stats stderr_low/high are the plain fit + errors; stderr_eff_low/high add the left/right divergence excess — the values the + nudge/lock eligibility checks use. + """ + det = self.s_ll * self.s_hh - self.s_lh * self.s_lh + if self.n < 10 or det < 1e-9: + return None + anchor_low = (self.s_ly * self.s_hh - self.s_hy * self.s_lh) / det + anchor_high = (self.s_hy * self.s_ll - self.s_ly * self.s_lh) / det + + # Residual variance -> per-anchor standard errors from the normal-equation inverse. + sse = max(0.0, self.s_wy2 + - 2.0 * (anchor_low * self.s_ly + anchor_high * self.s_hy) + + anchor_low * anchor_low * self.s_ll + + 2.0 * anchor_low * anchor_high * self.s_lh + + anchor_high * anchor_high * self.s_hh) + dof = max(1.0, self.s_w - 2.0) + var = sse / dof + stderr_low = math.sqrt(max(0.0, var * self.s_hh / det)) + stderr_high = math.sqrt(max(0.0, var * self.s_ll / det)) + div_low = self.lr_divergence(0) + div_high = self.lr_divergence(1) + + low_factor = min(FACTOR_MAX, max(FACTOR_MIN, anchor_low / LOW_ANCHOR_BASE)) + high_factor = min(FACTOR_MAX, max(FACTOR_MIN, anchor_high / self.platform_gain_high)) + stats = { + "n": self.n, + "weight_low": self.weight_low, + "weight_high": self.weight_high, + "stderr_low": stderr_low, + "stderr_high": stderr_high, + "stderr_eff_low": stderr_low + max(0.0, div_low - LR_TOL), + "stderr_eff_high": stderr_high + max(0.0, div_high - LR_TOL), + "lr_div_low": div_low, + "lr_div_high": div_high, + "anchor_low": anchor_low, + "anchor_high": anchor_high, + } + return low_factor, high_factor, stats + + def to_dict(self) -> dict: + return { + "s_ll": self.s_ll, "s_lh": self.s_lh, "s_hh": self.s_hh, + "s_ly": self.s_ly, "s_hy": self.s_hy, "s_w": self.s_w, "s_wy2": self.s_wy2, + "n": self.n, + "lr": [self.lr[(h, d)][:] for h in (0, 1) for d in (0, 1)], + "recent": [self.recent[0][:], self.recent[1][:]], + } + + def from_dict(self, d: dict): + self.s_ll = float(d["s_ll"]) + self.s_lh = float(d["s_lh"]) + self.s_hh = float(d["s_hh"]) + self.s_ly = float(d["s_ly"]) + self.s_hy = float(d["s_hy"]) + self.s_w = float(d["s_w"]) + self.s_wy2 = float(d["s_wy2"]) + self.n = int(d["n"]) + flat = d.get("lr") + if isinstance(flat, list) and len(flat) == 4: + for i, (h, dd) in enumerate(((0, 0), (0, 1), (1, 0), (1, 1))): + self.lr[(h, dd)] = [float(flat[i][0]), float(flat[i][1])] + rec = d.get("recent") # absent in pre-verify saves: tracker rebuilds in ~a minute + if isinstance(rec, list) and len(rec) == 2: + for h in (0, 1): + self.recent[h] = [float(rec[h][0]), float(rec[h][1])] + + +class QualityMonitor: + """Frame-level evidence quality: excises anomalous moments WITHIN curves without ever + rejecting the cornering itself. Detectors are curve-safe by construction — thresholds + sit above what clean cornering produces, blanking windows are short, and the rough-road + residual is high-passed so slow curve content cannot trip it.""" + + def __init__(self, dt: float = 0.05): + self.dt = dt + self.blank_s = 0.0 + self.flick_fired = False # True only on the frame a disturbance was detected + self._meas_last = None + self._cmd_last = None + self._ws_spread_last = None + self._lp = None # low-passed measurement ("curve content") + self._rms2 = 0.0 # EMA of squared high-passed residual + self.counters = {c: 0 for c in REJ_CAUSES} + + def update(self, kappa_cmd: float, kappa_meas: float, + a_ego: float = 0.0, ws_spread: float | None = None) -> bool: + """Advance one 20 Hz frame; returns True when the frame may carry evidence.""" + dt = self.dt + self.flick_fired = False + + # Transient flick: measurement jumps while the command is quiet. + if self._meas_last is not None and self._cmd_last is not None: + meas_rate = abs(kappa_meas - self._meas_last) / dt + cmd_rate = abs(kappa_cmd - self._cmd_last) / dt + if meas_rate > SPIKE_MEAS_RATE and cmd_rate <= MAX_KAPPA_RATE: + self.blank_s = DISTURBANCE_BLANK_S + self.flick_fired = True + # Corroborating bump signal: the wheels get shaken before the pinion shows it. + if ws_spread is not None and self._ws_spread_last is not None: + if abs(ws_spread - self._ws_spread_last) > WS_SPREAD_JUMP: + self.blank_s = max(self.blank_s, DISTURBANCE_BLANK_S) + self.flick_fired = True + self._meas_last = kappa_meas + self._cmd_last = kappa_cmd + self._ws_spread_last = ws_spread + + # Rough road: RMS of the high-passed measurement residual. The low-pass tracks curve + # content; what remains is surface noise. + if self._lp is None: + self._lp = kappa_meas + alpha_lp = dt / (ROUGH_LP_TAU_S + dt) + self._lp += alpha_lp * (kappa_meas - self._lp) + resid = kappa_meas - self._lp + alpha_rms = dt / (ROUGH_RMS_TAU_S + dt) + self._rms2 += alpha_rms * (resid * resid - self._rms2) + rough = math.sqrt(self._rms2) > ROUGH_RMS_MAX + + ok = True + if self.blank_s > 0.0: + self.blank_s = max(0.0, self.blank_s - dt) + self.counters["flick"] += 1 + ok = False + elif rough: + self.counters["rough"] += 1 + ok = False + if abs(a_ego) > MAX_LONG_ACCEL: + if ok: + self.counters["accel"] += 1 + ok = False + return ok + + def idle(self): + """Lateral inactive: rate baselines are meaningless across the gap.""" + self._meas_last = None + self._cmd_last = None + self._ws_spread_last = None + + +class _PeakFrame(NamedTuple): + """One ring-buffer slot of the apex matcher — named so nothing indexes it by magic number.""" + kappa_cmd: float + kappa_meas: float + v_ego: float + gain: float + clean: bool + + +class PeakMatcher: + """Apex evidence — the video method. A ring buffer of recent frames; when a command + apex (dominant, prominent local extremum with an all-clean neighborhood) scrolls to + the decision point, its measured twin is the same-sign extremum within the actuator + lag horizon, and the amplitude ratio is committed as gain evidence (median-of-N so a + single weird apex dies before reaching the estimator).""" + + def __init__(self, dt: float = 0.05): + self.dt = dt + self.n_buf = int(round(PEAK_BUF_S / dt)) # 50 + self.half_w = int(round(PEAK_HALF_WINDOW_S / dt)) # 20 + self.lag_max = int(round(PEAK_LAG_MAX_S / dt)) # 14 + self.c = self.n_buf - 1 - max(self.half_w, self.lag_max) # decision index + self.buf: list[_PeakFrame] = [] + self._refractory = 0 + self._pending: dict[int, list] = {0: [], 1: []} # anchor half -> [(r, sample), ...] + self.apexes_seen = 0 + self.apexes_committed = 0 + + def clear(self): + self.buf.clear() + self._pending = {0: [], 1: []} + self._refractory = 0 + + def poison_recent(self, seconds: float): + """A disturbance was just detected: frames shortly BEFORE detection are suspect + (the bump was already moving the car). Mark them not-ok retroactively.""" + n = int(round(seconds / self.dt)) + for i in range(max(0, len(self.buf) - n), len(self.buf)): + self.buf[i] = self.buf[i]._replace(clean=False) + + def push(self, kappa_cmd: float, kappa_meas: float, v_ego: float, + applied_gain: float, ok: bool) -> list[tuple]: + """Advance one frame. Returns samples to commit: (v, kappa_cmd, kappa_meas, + applied_gain) tuples (already median-filtered).""" + self.buf.append(_PeakFrame(kappa_cmd, kappa_meas, v_ego, applied_gain, ok)) + if len(self.buf) > self.n_buf: + self.buf.pop(0) + if self._refractory > 0: + self._refractory -= 1 + if len(self.buf) < self.n_buf or self._refractory > 0: + return [] + + c = self.c + fc = self.buf[c] + k_c, v_c, g_c = fc.kappa_cmd, fc.v_ego, fc.gain + if abs(k_c) < PEAK_MIN_KAPPA or v_c < MIN_SPEED: + return [] + if abs(k_c) * v_c * v_c > MAX_LAT_ACCEL: + return [] + window = self.buf[c - self.half_w:c + self.half_w + 1] + # Every frame around the apex must be clean — the retroactive-cancel analog for peaks. + if not all(f.clean for f in window): + return [] + mags = [abs(f.kappa_cmd) for f in window] + k_mag = abs(k_c) + # Dominant: >= everything before, strictly > everything after (fires once per plateau). + before = mags[:self.half_w + 1] + after = mags[self.half_w + 1:] + if any(m > k_mag for m in before) or any(m >= k_mag for m in after): + return [] + if k_mag - min(mags) < PEAK_PROMINENCE: + return [] + + self.apexes_seen += 1 + self._refractory = int(round(PEAK_REFRACTORY_S / self.dt)) + + # Measured twin: same-sign extremum within the lag horizon (clean frames only). + sign = 1.0 if k_c > 0 else -1.0 + m_pk = 0.0 + for f in self.buf[c:c + self.lag_max + 1]: + if not f.clean: + return [] # disturbance inside the match window: the pair is unusable + if f.kappa_meas * sign > m_pk: + m_pk = f.kappa_meas * sign + if m_pk <= 0.0: + return [] + r = (m_pk * sign) / k_c + if not (MIN_RATIO <= r <= MAX_RATIO): + return [] + + half = 0 if speed_alpha(v_c) < 0.5 else 1 + self._pending[half].append((r, (v_c, k_c, m_pk * sign, g_c))) + if len(self._pending[half]) < PEAK_MEDIAN_N: + return [] + # Median by ratio — the middle apex is committed, the outliers die here. + self._pending[half].sort(key=lambda t: t[0]) + _, sample = self._pending[half][PEAK_MEDIAN_N // 2] + self._pending[half] = [] + self.apexes_committed += 1 + return [sample] + + +class SteadyStateGate: + """Admits samples only after the command has been steady for STEADY_TIME_S. + + Both consumers drive this at the 20 Hz lateral rate with the same flags the + strategy itself computes, so offline and onboard gating are identical. + """ + + def __init__(self, dt: float = 0.05): + self.dt = dt + self.steady_s = 0.0 + self.kappa_last = None + self.kappa_window_start = None # command value when the current steady window opened + self.grip_cooldown_s = 0.0 + self.grip_this_frame = False # grip seen on the frame last passed to update() + self.frame_clear = False # per-frame admission (no grip/cooldown/limiter flags), + # computed ONCE here and shared with the apex path + + def reset(self): + """Inactive frame (disengaged / human turn / stall blip): steadiness restarts and the + last-command baseline is dropped; the grip cooldown keeps decaying in real time.""" + self.grip_cooldown_s = max(0.0, self.grip_cooldown_s - self.dt) + self.steady_s = 0.0 + self.kappa_last = None + self.kappa_window_start = None + self.grip_this_frame = False + self.frame_clear = False + + def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, + angle_rate_limited: bool, deviation_limited: bool, + saturated: bool = False, driver_torque: float = 0.0) -> bool: + # Human-turn and stall-blip frames never reach this call — the strategy early-returns + # and idles the pipeline instead — so those flags are not parameters here. + # Any grip — including light torque below the steeringPressed threshold — starts a + # cooldown: the driver was steering, and the PSCM's delivery stays suspect for a while + # after release (post-touch attenuation). + grip = steering_pressed or abs(driver_torque) > TORQUE_GUARD_NM + self.grip_this_frame = grip + if grip: + self.grip_cooldown_s = PRESS_COOLDOWN_S + else: + self.grip_cooldown_s = max(0.0, self.grip_cooldown_s - self.dt) + + # The per-frame admission predicate, computed exactly once: the pipeline's apex path + # reads it back instead of maintaining a hand-synced copy. + self.frame_clear = (not grip and self.grip_cooldown_s <= 0.0 + and not angle_rate_limited and not deviation_limited + and not saturated) + ok = lat_active and self.frame_clear and abs(kappa_cmd) >= MIN_KAPPA + if ok and self.kappa_last is not None: + # Relative rate bound with an absolute floor: lag alignment absorbs the transport + # delay, so kappa is allowed to actually MOVE (winding roads) — the bound only has + # to cap the residual delay-estimate error's effect (see the constants block). + ok = (abs(kappa_cmd - self.kappa_last) / self.dt + <= max(MAX_KAPPA_RATE, REL_KAPPA_RATE * abs(kappa_cmd))) + # Actuation-lag protection: per-frame rate alone admits slow ramps whose same-frame + # ratio is lag-biased; the window-total drift bound caps that (see STEADY_DRIFT_FRAC). + if ok and self.kappa_window_start is not None: + ok = abs(kappa_cmd - self.kappa_window_start) <= STEADY_DRIFT_FRAC * abs(kappa_cmd) + self.kappa_last = kappa_cmd if lat_active else None + if ok: + if self.kappa_window_start is None: + self.kappa_window_start = kappa_cmd + self.steady_s += self.dt + else: + self.steady_s = 0.0 + self.kappa_window_start = None + return self.steady_s >= STEADY_TIME_S + + +@dataclass +class _StagedSample: + """One holdback-staged evidence sample. Named fields (not a list-of-lists) so a + reordered field can't silently shift every value — they are all floats and nothing + would raise. age mutates as the sample waits, so this is mutable, not a NamedTuple.""" + age: float + v_ego: float + kappa_cmd: float + kappa_meas: float + gain: float + weight: float + + +class AutoCalPipeline: + """Quality layer + steady gate + apex matcher + estimator + nudger + lock, driven with + one call per 20 Hz lateral frame. Samples sit in a staging queue for PRESS_HOLDBACK_S + before they reach the estimator; a grip or disturbance while they wait cancels them. + Used identically by the onboard hook and the offline analyzer. + + The pipeline itself never writes params: recommend() returns proposed factor values and + the glue (or the analyzer's virtual car) applies them and passes the applied values back + in on subsequent update() calls — that closes the loop. + """ + + def __init__(self, platform_gain_high: float, dt: float = 0.05): + self.platform_gain_high = float(platform_gain_high) + self.est = AngleFactorEstimator(platform_gain_high) + self.gate = SteadyStateGate(dt=dt) + self.quality = QualityMonitor(dt=dt) + self.peaks = PeakMatcher(dt=dt) + self.dt = dt + self._staged: list[_StagedSample] = [] + self._meas_last = None + self._err_lp = None # smoothed |tracking error| for the quietness gate + self._decay_accum = 0.0 + # Lag alignment: ring of recent (kappa_cmd, applied_gain) so this frame's measurement + # can be ratioed against the command (and the gain in force) when it was ISSUED. + self._hist: list[tuple[float, float]] = [] + self._hist_max = int(round(LAG_MAX_S / dt)) + 2 + # Nudge / lock bookkeeping (persisted). + self.since_nudge_s = NUDGE_PERIOD_S # first nudge allowed as soon as evidence permits + self.stable_s = 0.0 + self.nudges = 0 + self.locked = False + self.lock_enabled = True # FordAngleAutoCalLock (set by the controller, not persisted): + # False = never freeze, keep adapting for the life of the toggle + # Adjust-then-verify state (persisted): each step opens a window that must be judged + # against fresh evidence before its anchor may step again. half 0 = low, 1 = high. + self.verify = {0: None, 1: None} # {"frm": factor, "to": factor, "pre_r": ratio|None} + self.verify_result = {0: "", 1: ""} # last judgment: "confirmed" / "failed" / "" + self.verify_hold = {0: 0.0, 1: 0.0} # extra fresh evidence demanded after a failure + + # -- gain model ------------------------------------------------------------------------- + def applied_gain(self, v_ego: float, low_factor: float, high_factor: float) -> float: + a = speed_alpha(v_ego) + return (1.0 - a) * (LOW_ANCHOR_BASE * low_factor) + a * (self.platform_gain_high * high_factor) + + def idle(self): + """Call on frames where lateral is inactive (disengaged / human turn / stall blip).""" + self.gate.reset() + self.quality.idle() + self.peaks.clear() + self._staged.clear() + self._meas_last = None + self._err_lp = None + self._hist.clear() # commands across a discontinuity must never be an alignment target + + def update(self, frame: Frame) -> list: + """Advance one frame. frame.low_factor/high_factor are the values currently steering + the car — each committed sample records the gain that produced it. Returns the samples + committed to the estimator this frame as (v, kappa_cmd, kappa_meas) tuples — the + offline analyzer plots them; the onboard hook ignores the return value.""" + if self.locked: + return [] + v_ego, kappa_cmd, kappa_meas = frame.v_ego, frame.kappa_cmd, frame.kappa_meas + gain_now = self.applied_gain(v_ego, frame.low_factor, frame.high_factor) + + # Lag alignment: this frame's MEASUREMENT answers the command from lateral_delay ago. + # All steadiness gating and every steady-state ratio below use that reference pair + # (cmd + the gain in force when it was issued); the apex path keeps its own explicit + # lag matching and stays on the current command. + self._hist.append((kappa_cmd, gain_now)) + if len(self._hist) > self._hist_max: + self._hist.pop(0) + lag_f = int(round(min(max(frame.lateral_delay, LAG_MIN_S), LAG_MAX_S) / self.dt)) + aligned = len(self._hist) > lag_f + kappa_ref, gain_ref = self._hist[-1 - lag_f] if aligned else (0.0, gain_now) + + if aligned: + # The gate computes grip + the per-frame admission predicate once; everything + # below reads them back instead of keeping a hand-synced copy. + eligible = self.gate.update(True, kappa_ref, frame.steering_pressed, + frame.angle_rate_limited, frame.deviation_limited, + saturated=frame.saturated, driver_torque=frame.driver_torque) + else: + # Not enough history yet (first ~lateral_delay after engaging): no steady evidence, + # and the gate restarts so its baselines never straddle the warmup. + self.gate.reset() + eligible = False + if self.gate.grip_this_frame: + self._staged.clear() + self.quality.counters["grip"] += 1 + + q_ok = self.quality.update(kappa_cmd, kappa_meas, a_ego=frame.a_ego, ws_spread=frame.ws_spread) + if self.quality.flick_fired: + # Retroactive: the bump was already moving the car before detection tripped. + self._staged.clear() + self.peaks.poison_recent(DISTURBANCE_POISON_S) + + eligible = eligible and q_ok + + # The CAR must be settled too, not just the command: during closed-loop compensation + # swings (understeer -> harder request -> convergence tail) the command can sit steady + # while the measurement is still moving toward it — those ratios are transient, not + # gain. The measurement legitimately moves as fast as the (lag-aligned) command does, + # and it is noisier — so the bound is 3x the command's own admission bound. + if eligible and self._meas_last is not None: + eligible = (abs(kappa_meas - self._meas_last) / self.dt + <= 3.0 * max(MAX_KAPPA_RATE, REL_KAPPA_RATE * abs(kappa_ref))) + self._meas_last = kappa_meas + + # Loop-quietness: only calm-tracking frames are gain evidence (see QUIET_* constants). + # A constant plant deficit keeps a FLAT error trend and passes; an excursion or the + # correction that follows it moves the trend and is refused — the loop's hunting can + # never masquerade as gain information, however long that makes a step take. + if aligned: + err_now = abs(kappa_meas - kappa_ref) + if self._err_lp is None: + self._err_lp = err_now + eligible = False # no trend established yet + else: + prev = self._err_lp + self._err_lp += (self.dt / (QUIET_TAU_S + self.dt)) * (err_now - self._err_lp) + if abs(self._err_lp - prev) / self.dt > QUIET_ERR_RATE: + eligible = False + + # Evidence near the physical limit fades to nothing: there, cmd != meas is physics. + margin_w = lat_accel_margin(kappa_cmd, v_ego) + if eligible and margin_w <= 0.0: + self.quality.counters["limit"] += 1 + eligible = False + + # Age the staging queue; entries that survived the holdback graduate to the estimator. + committed = [] + still_staged = [] + for s in self._staged: + s.age += self.dt + if s.age >= PRESS_HOLDBACK_S: + if self.est.add_sample(s.v_ego, s.kappa_cmd, s.kappa_meas, s.gain, weight=s.weight): + committed.append((s.v_ego, s.kappa_cmd, s.kappa_meas)) + else: + still_staged.append(s) + self._staged = still_staged + + if eligible: + self._staged.append(_StagedSample(0.0, v_ego, kappa_ref, kappa_meas, gain_ref, self.dt * margin_w)) + + # Apex evidence: gated by everything EXCEPT the steadiness timer (an apex is by + # definition not steady). Quality, grip, limit flags all poison the window — + # the grip/flag part is the gate's own frame_clear, computed once above. + frame_ok = q_ok and self.gate.frame_clear and margin_w > 0.0 + for (pv, pk, pm, pg) in self.peaks.push(kappa_cmd, kappa_meas, v_ego, gain_now, frame_ok): + p_margin = lat_accel_margin(pk, pv) + if self.est.add_sample(pv, pk, pm, pg, weight=PEAK_WEIGHT_S * p_margin): + committed.append((pv, pk, pm)) + + # Housekeeping clocks: forgetting, nudge cadence, lock stability. + self._decay_accum += self.dt + if self._decay_accum >= 1.0: + self.est.decay(self._decay_accum) + self._decay_accum = 0.0 + self.since_nudge_s += self.dt + self._judge_verifies() + + sol = self.est.solve() + if sol is not None: + low_t, high_t, st = sol + ready = (fit_trustworthy(st["weight_low"], st["stderr_eff_low"], LOCK_MIN_WEIGHT) + and fit_trustworthy(st["weight_high"], st["stderr_eff_high"], LOCK_MIN_WEIGHT) + and abs(low_t - frame.low_factor) <= LOCK_DEADBAND + and abs(high_t - frame.high_factor) <= LOCK_DEADBAND) + if ready: + self.stable_s += self.dt + if self.stable_s >= LOCK_STABLE_S and self.lock_enabled: + self.locked = True + else: + self.stable_s = 0.0 + + return committed + + def _judge_verifies(self): + """Judge any pending step once enough FRESH post-step evidence exists (the fast + tracker was reset to zero when the step was taken, so it holds post-step data only). + + The criterion is CLOSENESS, not direction: the response ratio tracks the applied + gain mechanically (drop the factor 2%, delivery drops ~2%, right or wrong), so + 'did it move the predicted way' would confirm every step the actuator executed. + A step is only right if it brought the car closer to doing exactly what's asked: + |1 - ratio| strictly smaller than before the step, or inside VERIFY_OK_BAND of + 1.0 outright — the step landed where calibration aims.""" + for half in (0, 1): + pend = self.verify[half] + if pend is None: + continue + w, r = self.est.recent_response(half) + if w < VERIFY_MIN_WEIGHT or r is None: + continue # keep polling — the window stays open until the data has spoken + ok = abs(1.0 - r) <= VERIFY_OK_BAND + if not ok and pend["pre_r"] is not None: + ok = abs(1.0 - r) < abs(1.0 - pend["pre_r"]) + elif not ok: + ok = True # no pre-step baseline to compare against (shouldn't happen in practice) + self.verify[half] = None + if ok: + self.verify_result[half] = "confirmed" + self.verify_hold[half] = 0.0 + else: + self.verify_result[half] = "failed" + self.verify_hold[half] = VERIFY_FAIL_HOLD_WEIGHT + + def recommend(self, low_factor: float, high_factor: float): + """The closed-loop step: propose nudged factor values, or None. + + Call once per frame with the currently applied factors; at most one nudge per + NUDGE_PERIOD_S of active collection, one bounded step at a time — but no cumulative + cap: total movement is unbounded (within FACTOR_MIN/MAX) as long as every step keeps + verifying against fresh evidence. The caller applies the returned values (params + + in-memory) — they flow back in through update()'s low_factor/high_factor and the + loop closes: an overshoot pulls the ratios past 1, the target backs off, the next + nudge reverses. + """ + if self.locked or self.since_nudge_s < NUDGE_PERIOD_S: + return None + sol = self.est.solve() + if sol is None: + return None + low_t, high_t, st = sol + + def step(half, target, applied, weight, stderr_eff): + if self.verify[half] is not None: + return None # the last step hasn't been judged against fresh evidence yet + if not fit_trustworthy(weight, stderr_eff, NUDGE_MIN_WEIGHT): + return None + if self.verify_hold[half] > 0.0: + w_rec, _ = self.est.recent_response(half) + if w_rec < self.verify_hold[half]: + return None # a failed check demands extra evidence before moving again + self.verify_hold[half] = 0.0 + units = nudge_units(target - applied) + if units == 0: + return None + new = round(max(FACTOR_MIN, min(FACTOR_MAX, applied + units * FACTOR_STEP)), 2) + return new if new != round(applied, 2) else None + + new_low = step(0, low_t, low_factor, st["weight_low"], st["stderr_eff_low"]) + new_high = step(1, high_t, high_factor, st["weight_high"], st["stderr_eff_high"]) + if new_low is None and new_high is None: + return None + # Open a verify window per moved anchor: capture the pre-step response as the + # baseline, then zero the fast tracker so the judgment sees only post-step data. + for half, applied, new in ((0, low_factor, new_low), (1, high_factor, new_high)): + if new is not None: + _, pre_r = self.est.recent_response(half) + self.verify[half] = {"frm": round(float(applied), 4), "to": new, "pre_r": pre_r} + self.verify_result[half] = "" + self.est.recent[half] = [0.0, 0.0] + out_low = new_low if new_low is not None else round(low_factor, 2) + out_high = new_high if new_high is not None else round(high_factor, 2) + self.since_nudge_s = 0.0 + self.stable_s = 0.0 + self.nudges += 1 + return out_low, out_high + + def user_edit(self): + """The driver moved a factor by hand mid-collection: their judgment is information — + adopt the value (it arrives via update()'s low/high_factor), soft-reset confidence so + the estimator re-earns it, and restart lock progress. Evidence is NOT wiped: every + sample recorded its own applied gain, so history stays valid.""" + self.est.scale(0.5) + self.stable_s = 0.0 + self.since_nudge_s = 0.0 + + # -- live UI state ---------------------------------------------------------------------- + def ui_state(self, low_factor: float, high_factor: float) -> dict: + """Per-anchor state for live dashboards (the phone /lateral page). Everything here is + ground truth from the running pipeline — never a param re-read — and rounded hard + because it travels as a ~1 Hz telemetry string. + + Per anchor: f applied factor, w/need evidence progress, r recent response ratio + ("turns 93% of requested" = 0.93), ph phase (collect / propose / verify / good), + t proposed factor (propose), to+vw+vneed step being checked (verify), vr last + judgment (confirmed / failed).""" + sol = self.est.solve() + st = sol[2] if sol is not None else None + targets = (sol[0], sol[1]) if sol is not None else (None, None) + out = {"nudges": self.nudges, "stable_s": round(self.stable_s)} + for half, name, applied in ((0, "low", low_factor), (1, "high", high_factor)): + weight = (st["weight_low"] if half == 0 else st["weight_high"]) if st is not None \ + else (self.est.weight_low if half == 0 else self.est.weight_high) + stderr = (st["stderr_eff_low"] if half == 0 else st["stderr_eff_high"]) if st is not None else None + w_rec, r_rec = self.est.recent_response(half) + target = targets[half] + d = {"f": round(float(applied), 2), "w": round(weight, 1), "need": NUDGE_MIN_WEIGHT} + if r_rec is not None and w_rec >= 2.0: + d["r"] = round(r_rec, 3) + if self.verify[half] is not None: + d["ph"] = "verify" + d["to"] = self.verify[half]["to"] + d["vw"] = round(w_rec, 1) + d["vneed"] = VERIFY_MIN_WEIGHT + elif (target is not None and stderr is not None + and fit_trustworthy(weight, stderr, NUDGE_MIN_WEIGHT)): + if nudge_units(target - applied) != 0: + d["ph"] = "propose" + d["t"] = round(target, 2) + else: + d["ph"] = "good" + else: + d["ph"] = "collect" + if self.verify_result[half]: + d["vr"] = self.verify_result[half] + out[name] = d + return out + + # -- persistence ------------------------------------------------------------------------ + def to_dict(self) -> dict: + return { + "est": self.est.to_dict(), + "stable_s": round(self.stable_s, 2), + "since_nudge_s": round(min(self.since_nudge_s, NUDGE_PERIOD_S), 2), + "nudges": self.nudges, + "locked": self.locked, + "apexes": self.peaks.apexes_committed, + "rej": dict(self.quality.counters), + "verify": [self.verify[0], self.verify[1]], + "verify_result": [self.verify_result[0], self.verify_result[1]], + "verify_hold": [self.verify_hold[0], self.verify_hold[1]], + } + + def from_dict(self, d: dict): + self.est.from_dict(d["est"]) + self.stable_s = float(d.get("stable_s", 0.0)) + self.since_nudge_s = float(d.get("since_nudge_s", NUDGE_PERIOD_S)) + self.nudges = int(d.get("nudges", 0)) + self.locked = bool(d.get("locked", False)) + self.peaks.apexes_committed = int(d.get("apexes", 0)) + rej = d.get("rej", {}) + for c in REJ_CAUSES: + self.quality.counters[c] = int(rej.get(c, 0)) + ver = d.get("verify") # absent in pre-verify saves: everything simply starts clear + if isinstance(ver, list) and len(ver) == 2: + for h in (0, 1): + self.verify[h] = dict(ver[h]) if isinstance(ver[h], dict) else None + vr = d.get("verify_result") + if isinstance(vr, list) and len(vr) == 2: + for h in (0, 1): + self.verify_result[h] = str(vr[h] or "") + vh = d.get("verify_hold") + if isinstance(vh, list) and len(vh) == 2: + for h in (0, 1): + self.verify_hold[h] = float(vh[h]) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py new file mode 100644 index 0000000000..aa8f805924 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py @@ -0,0 +1,229 @@ +"""BluePilot: lifecycle controller for the Ford angle-mode auto-calibration. + +AutoCalPipeline (angle_autocal.py) is pure math with no I/O. This controller owns +everything between that math and the car: arm/disarm from the toggle, evidence +persistence to FordAngleAutoCalState, nudge writes to the factor params, errors to +FordAngleAutoCalError, and the telemetry status string. FordLateralAngleExt calls +poll_params() at ~1 Hz, feed() per 20 Hz lateral frame, and idle() when inactive. + +Nudges are written straight to the factor params (blocking); the strategy reads them +back through poll_params, so the live steering factors have a single owner and no +in-memory adopt path is needed. +""" +import json + +from opendbc.sunnypilot.car.ford.angle_autocal import AutoCalPipeline, Frame + +SAVE_PERIOD_S = 30.0 +EDIT_TOL = 0.005 # half the menu granularity (0.01): a factor moved further than this + # without the nudger writing it is a driver hand-edit + + +def _state_locked(state: str) -> bool: + """True when the persisted state says the calibration is finished. + Legacy pre-JSON states ("done low=... high=... verified") stay honored.""" + if state.startswith("done"): + return True + if state.startswith("{"): + try: + return json.loads(state).get("phase") == "locked" + except (ValueError, AttributeError): + return False + return False + + +def _restore(pipeline, state: str): + """Load serialized evidence into a fresh pipeline; anything unparseable (legacy round + strings, garbage, empty) simply starts a fresh collection.""" + if not state.startswith("{"): + return + try: + d = json.loads(state) + pipe = d.get("pipe") + if isinstance(pipe, dict) and int(d.get("v", 0)) == 1: + pipeline.from_dict(pipe) + except (ValueError, KeyError, TypeError): + pass + + +class AutoCalController: + def __init__(self, dt: float): + self.dt = dt + self.enabled = False + self.done = True # conservative until params are read + self.pipeline = None # AutoCalPipeline while collecting + self.status = "" # live ground-truth status, published in telemetry + self._params = None + self._last_written = None # (low, high) the nudger last wrote; a different param value is a user edit + self._save_s = 0.0 + self._dirty = False + + # -- ~1 Hz: toggle, restore, user edits, status ------------------------------------------ + def poll_params(self, params, low_factor: float, high_factor: float, platform_gain_high: float): + """Arm/disarm from the toggle, restore evidence on arm, detect user hand-edits of the + factor params, refresh the status string. low/high are the currently applied values.""" + try: + if params.get_bool("FordAngleAutoCalReset"): + # Erase calibration memory: evidence, error channel, lock and factors all go + # back to neutral so the car steers stock immediately and collection restarts. + # Idempotent with the UI's own param clears; covers non-UI writers too. + params.put_bool("FordAngleAutoCalReset", False) + params.put("FordAngleAutoCalState", "") + params.put("FordAngleAutoCalError", "") + params.put("FordLowSpeedFactor_ang", 1.0) + params.put("FordHighSpeedFactor_ang", 1.0) + self.pipeline = None + self.done = False + self._last_written = (1.0, 1.0) + self._dirty = False + self._params = params + self.status = "reset" + return + enabled = bool(params.get_bool("FordAngleAutoCal")) + # Lock behavior toggle (default ON): with the lock OFF the calibration never + # freezes — and an EXISTING lock is treated as "resume from this evidence", not + # as finished, so flipping the toggle un-locks without losing anything. + lock_on = bool(params.get_bool("FordAngleAutoCalLock")) + state = params.get("FordAngleAutoCalState", return_default=True) or "" + if isinstance(state, bytes): + state = state.decode("utf-8", errors="replace") + if self.pipeline is None: + self.done = _state_locked(state) and lock_on + else: + self.pipeline.lock_enabled = lock_on + if not lock_on and self.pipeline.locked: + self.pipeline.locked = False + self.pipeline.stable_s = 0.0 + self.done = self.pipeline.locked + self.enabled = enabled and not self.done + if self.enabled and self.pipeline is None: + # Arm: build the pipeline, restore prior-drive evidence, baseline the nudger on + # the currently applied factors. + self.pipeline = AutoCalPipeline(platform_gain_high, dt=self.dt) + _restore(self.pipeline, state) + self.pipeline.lock_enabled = lock_on + if not lock_on and self.pipeline.locked: + self.pipeline.locked = False # resuming a previously locked calibration + self.pipeline.stable_s = 0.0 + self._last_written = (float(low_factor), float(high_factor)) + elif not self.enabled: + self.pipeline = None + else: + # User hand-edit: a factor param differs from what the nudger last wrote. The + # nudger's own writes are blocking (_apply_nudge), so by the time we read here + # they always match _last_written — any mismatch is the driver. Adopt their value + # (already live in the strategy) and soft-reset confidence; evidence is not wiped. + lw = self._last_written + moved = lw is not None and (abs(low_factor - lw[0]) > EDIT_TOL + or abs(high_factor - lw[1]) > EDIT_TOL) + if moved: + self.pipeline.user_edit() + self._last_written = (float(low_factor), float(high_factor)) + self._dirty = True + self._params = params + # Live status for telemetry: published from actual controller state (ground truth), + # never from a param re-read — a param/telemetry mismatch is exactly the failure + # mode that made earlier on-device issues undiagnosable. + if self.done: + self.status = "locked" + elif not self.enabled: + self.status = "off" + else: + # Armed: compact JSON so live dashboards (phone /lateral cards) can render the + # per-anchor story — evidence progress, measured response, proposed step, and + # the adjust-then-verify judgment — from the same ground truth the nudger uses. + ui = self.pipeline.ui_state(low_factor, high_factor) + ui["n"] = self.pipeline.est.n + self.status = json.dumps(ui, separators=(",", ":")) + except Exception as e: + self.enabled = False + self.status = f"tick error: {type(e).__name__}: {e}"[:200] + self._error(self.status) + + # -- 20 Hz frames ------------------------------------------------------------------------ + def idle(self): + """Frames where lateral is inactive (disengaged / human turn / stall blip).""" + if self.pipeline is not None: + self.pipeline.idle() + + def feed(self, frame: Frame, delay_estimated: bool): + """One active lateral frame. Nudges are written to the factor params (the strategy + reads them back — single reader); the lock -> disarm transition and save cadence + happen here.""" + if not self.enabled or self.pipeline is None: + return + if not delay_estimated: + # Until lagd reports 'estimated', kappa_meas (via liveParameters, same locationd + # stack) is still converging — idle so staged samples don't straddle the warmup. + self.idle() + return + committed = self.pipeline.update(frame) + if committed: + self._dirty = True + applied = (frame.low_factor, frame.high_factor) + rec = self.pipeline.recommend(frame.low_factor, frame.high_factor) + if rec is not None and self._apply_nudge(rec): + applied = rec + if self.pipeline.locked: + self._save("locked", applied) + self.done = True + self.enabled = False + self.pipeline = None + else: + self._save_s += self.dt + if self._dirty and self._save_s >= SAVE_PERIOD_S: + self._save("collecting", applied) + + # -- params I/O -------------------------------------------------------------------------- + def _apply_nudge(self, rec) -> bool: + """Write a nudged factor pair to the params, blocking so the write has landed before + the next poll reads it (that read/write ordering is what keeps a nudge from looking + like a user edit — no timing guess). Returns True on success. The factor params are + typed FLOAT; a write error is parked in FordAngleAutoCalError rather than swallowed.""" + low_new, high_new = rec + if self._params is None: + return False + try: + self._params.put("FordLowSpeedFactor_ang", float(low_new), True) + self._params.put("FordHighSpeedFactor_ang", float(high_new), True) + except Exception as e: + self._error(f"nudge write failed: {type(e).__name__}: {e}") + return False + self._last_written = (float(low_new), float(high_new)) + self._save("collecting", rec) + return True + + def _error(self, msg: str): + """Park diagnostics in their OWN param, never FordAngleAutoCalState: an error written + just before ignition-off must not be able to overwrite the serialized evidence.""" + try: + if self._params is not None: + self._params.put("FordAngleAutoCalError", f"{msg[:300]}") + except Exception: + pass + + def _save(self, phase: str, applied): + """Serialize the pipeline into FordAngleAutoCalState (JSON). Async put is fine: + a lost final write costs at most SAVE_PERIOD_S of evidence.""" + if self._params is None or self.pipeline is None: + return + d = { + "v": 1, + "phase": phase, + "pipe": self.pipeline.to_dict(), + "applied": {"low": round(applied[0], 2), "high": round(applied[1], 2)}, + } + sol = self.pipeline.est.solve() + if sol is not None: + low_t, high_t, st = sol + d["target"] = {"low": round(low_t, 2), "high": round(high_t, 2)} + d["weight"] = {"low": round(st["weight_low"], 1), "high": round(st["weight_high"], 1)} + d["stderr"] = {"low": round(st["stderr_eff_low"], 3), "high": round(st["stderr_eff_high"], 3)} + d["stable_s"] = round(self.pipeline.stable_s, 1) + try: + self._params.put("FordAngleAutoCalState", json.dumps(d, separators=(",", ":"))) + except Exception as e: + self._error(f"state save failed: {type(e).__name__}: {e}") + return + self._save_s = 0.0 + self._dirty = False 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 2b8f620338..e22dbb8235 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -21,41 +21,29 @@ back in from zero through the soft ROC below (no jump seed) -- generous at human-turn speeds, and admitted by ford.h's path_angle ROC check (2% looser) without any bypass. """ + import numpy as np from numpy import clip, interp from opendbc.car import DT_CTRL from opendbc.car.lateral import apply_std_steer_angle_limits -from opendbc.car.ford.values import CAR, CarControllerParams +from opendbc.car.ford.values import CarControllerParams +from opendbc.sunnypilot.car.ford.angle_autocal import Frame +from opendbc.sunnypilot.car.ford.angle_autocal_controller import AutoCalController from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralResult from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector -from opendbc.sunnypilot.car.ford.values_ext import BP_ANGLE_LIMITS +from opendbc.sunnypilot.car.ford.values_ext import (BP_ANGLE_LIMITS, platform_gains, + V_LOW, V_HIGH, LOW_ANCHOR_BASE) from selfdrive.modeld.constants import ModelConstants -# Hard-coded per-platform gain defaults. -# CAN vehicles (Escape MK4, Bronco Sport, Explorer, Maverick, Edge) -_GAIN_CAN = (1.00, 1.15) -# CAN-FD body-on-frame trucks (F-150, Lightning, Expedition, Ranger) -_GAIN_CANFD_BOF = (0.95, 0.95) -# CAN-FD unibody SUVs (Mustang Mach-E, Escape MK4.5) -_GAIN_CANFD_SUV = (1.00, 1.05) - -_CANFD_BOF_CARS = frozenset({ - CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, - CAR.FORD_EXPEDITION_MK4, - CAR.FORD_RANGER_MK2, -}) -_CANFD_SUV_CARS = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, - CAR.FORD_ESCAPE_MK4_5, -}) - # DBC ``LatCtlPath_An_Actl`` (rad) — panda safety uses the same in ``ford.h``; PSCM enforces in firmware. FORD_DBC_PATH_ANGLE_MIN = -0.5 FORD_DBC_PATH_ANGLE_MAX = 0.5235 +# Auto-cal state persistence cadence: losing a save costs at most this much evidence. + + # PSCM d_ref (m) vs speed (m/s) — 6 points; above ~55.6 m/s use plateau + optional cap to 5 m. _PSCM_DREF_SPEEDS_MS = (0.0, 4.17, 27.78, 41.67, 50.0, 55.56) @@ -172,17 +160,22 @@ def __init__(self, CP=None, CP_SP=None): self.stall_blip_count = 0 # pulses fired this stall episode self.angle_stall_blip_active = False self.press_timer_s = 0.0 # continuous steeringPressed time, for the hand-off blip + # BluePilot: continuous auto-calibration of the speed factors. The pure estimator lives + # in angle_autocal.py; ALL lifecycle (arm/disarm, JSON persistence, user-edit debounce, + # nudge writes, save cadence, errors, telemetry status) lives in AutoCalController — + # this class only routes frames and adopts returned nudges. + self.autocal_ctl = AutoCalController(dt=_STEER_DT) + self._autocal_param_ctr = 100 # >= threshold so the very first call reads params + # Telemetry + autocal gate: the command this frame was modified by PSCM authority + # limits or the DBC clamp — the car could not make the requested turn. + self.bp_angle_saturated = False + def update_angle_params(self, params): """Sets per-platform gain defaults and reads user angle-tuning params.""" self._ensure_lateral_curv_initialized(self.CP) fp = getattr(self.CP, 'carFingerprint', '') - if fp in _CANFD_BOF_CARS: - low, high = _GAIN_CANFD_BOF - elif fp in _CANFD_SUV_CARS: - low, high = _GAIN_CANFD_SUV - else: - low, high = _GAIN_CAN + low, high = platform_gains(fp) self.path_angle_gain_lowC_highV = low self.path_angle_gain_highC_highV = high if params is not None and hasattr(params, "get"): @@ -205,6 +198,65 @@ def update_angle_params(self, params): float(raw.decode("utf-8", errors="replace") if isinstance(raw, bytes) else raw), 0.85, 1.50)) except Exception: pass + # BluePilot: auto-calibration arm/disarm (checked ~1 Hz; this method runs at 100 Hz) + self._autocal_param_ctr += 1 + if self._autocal_param_ctr >= 100: + self._autocal_param_ctr = 0 + self.autocal_ctl.poll_params(params, self.low_speed_curv_factor, + self.high_speed_curv_factor, + self.path_angle_gain_highC_highV) + + # -- auto-cal telemetry surface (bp_card_publisher reads these off the carcontroller) ---- + @property + def autocal_enabled(self) -> bool: + return self.autocal_ctl.enabled + + @property + def bp_autocal_status(self) -> str: + return self.autocal_ctl.status + + def _feed_autocal(self, CS, kappa_cmd: float, kappa_meas: float): + """Build one evidence Frame from the car signals + this frame's limiter flags and hand + it to the controller. Frame construction (and its signal reads) happens only while the + calibrator is armed — for everyone else this is one attribute check per frame.""" + if not self.autocal_ctl.enabled: + return + ws = CS.out.wheelSpeeds + ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + self.autocal_ctl.feed( + Frame(v_ego=float(CS.out.vEgoRaw), kappa_cmd=kappa_cmd, kappa_meas=kappa_meas, + steering_pressed=bool(CS.out.steeringPressed), + angle_rate_limited=self.bp_angle_rate_limited, + deviation_limited=self.bp_curvature_deviation_limited, + saturated=self.bp_angle_saturated, + driver_torque=float(CS.out.steeringTorque), a_ego=float(CS.out.aEgo), + ws_spread=max(ws_vals) - min(ws_vals), + low_factor=self.low_speed_curv_factor, high_factor=self.high_speed_curv_factor, + lateral_delay=float(self.sm['liveDelay'].lateralDelay)), + delay_estimated=str(self.sm['liveDelay'].status) == "estimated") + + def _reset_angle_signals(self, CS): + """Clear wire/telemetry state and the calibration evidence staging. Shared by the + three branches that drop lateral to mode 0 (inactive, human-turn, stall-blip).""" + self.path_angle_last = 0.0 + self.bp_path_angle_final = 0.0 + self.apply_curvature_last = 0.0 + self.bp_angle_rate_limited = False + self.bp_curvature_rate_limited = False + self.bp_curvature_deviation_limited = False + self.sim_curvature_last = 0.0 + # Shadow tracks measured curvature while inactive: ford.h latches it from every LKA + # frame, so a stale zero would fail the deviation check on the first re-engage frame. + self.bp_kappa_cmd = self.get_current_curvature(CS) + self.precision_type = 1 + self.bp_angle_saturated = False + self.autocal_ctl.idle() # steady/staged evidence must not span a lateral discontinuity + + @staticmethod + def _inactive_result() -> LateralResult: + """All-zero mode-0 result the three inactive branches return.""" + return LateralResult(apply_curvature=0.0, curvature_rate=0.0, path_offset=0.0, + path_angle=0.0, ramp_type=0, precision_type=1, lateralUncertainty=0.0) def update_angle_strategy(self, CC, CS, actuators, CP): """ @@ -225,22 +277,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): precision = 1 if not CC.latActive: - self.path_angle_last = 0.0 - self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 - self.bp_angle_rate_limited = False - self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False - self.sim_curvature_last = 0.0 - # Publish the shadow curvature from the measured curvature while inactive. LKA keeps - # carrying angle_mode_engaged whenever angle mode is configured (independent of - # latActive), and ford.h latches the shadow from every LKA frame -- so the latched - # value must track reality here, not sit at a stale zero. Otherwise the first enabled - # LMC frame after (re-)engage races LKA's 33Hz latch against LMC's 20Hz enable bit and - # ford.h's deviation check compares a zero shadow against real measured curvature. - # (ford.h skips the check while steer_control_enabled is 0, so the value is free to - # follow the measurement during the inactive period itself.) - self.bp_kappa_cmd = self.get_current_curvature(CS) + self._reset_angle_signals(CS) self.human_turn_detector.reset() self.angle_human_turn_active = False self.stall_blip_hold_s = 0.0 @@ -249,16 +286,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.stall_blip_count = 0 self.angle_stall_blip_active = False self.press_timer_s = 0.0 - self.precision_type = 1 - return LateralResult( - apply_curvature=0.0, - curvature_rate=0.0, - path_offset=0.0, - path_angle=0.0, - ramp_type=0, - precision_type=1, - lateralUncertainty=0.0, - ) + return self._inactive_result() # Human-turn override: sustained driver press + large wheel angle → force lateral inactive # (carcontroller drops mode to 0; all signals are zero on the wire) so path_angle can't wind @@ -270,38 +298,18 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.angle_human_turn_active = self.human_turn_detector.update( True, CS.out.steeringPressed, CS.out.steeringAngleDeg) if self.angle_human_turn_active: - self.path_angle_last = 0.0 - self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 - self.bp_angle_rate_limited = False - self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False - self.sim_curvature_last = 0.0 - # Truthful shadow during the override (mirrors the inactive path -- see the comment - # there): the driver is steering, so the honest command is the car's actual curvature, - # and the panda-latched shadow stays current for the re-engage frame. - self.bp_kappa_cmd = self.get_current_curvature(CS) + self._reset_angle_signals(CS) # Keep exit detection current so resume doesn't compare against a stale pre-turn value. self._desired_curvature_last = float(actuators.curvature) - # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also - # covers the press so far: only press time accumulated AFTER the latch releases should earn - # a hand-off pulse. + # A human turn's own mode 0 does the PSCM reset job; only press time after the latch + # releases should earn a hand-off pulse, so clear the stall/press state here. self.stall_blip_hold_s = 0.0 self.stall_blip_frames_left = 0 self.stall_blip_cooldown_s = 0.0 self.stall_blip_count = 0 self.angle_stall_blip_active = False self.press_timer_s = 0.0 - self.precision_type = 1 - return LateralResult( - apply_curvature=0.0, - curvature_rate=0.0, - path_offset=0.0, - path_angle=0.0, - ramp_type=0, - precision_type=1, - lateralUncertainty=0.0, - ) + return self._inactive_result() # Proactive hand-off blip: the falling edge of a sustained press earns an immediate mode-0 # pulse (see _PRESS_BLIP_MIN_S) -- resets the PSCM's press-induced attenuation right at @@ -323,28 +331,11 @@ def update_angle_strategy(self, CC, CS, actuators, CP): if self.stall_blip_frames_left > 0: self.stall_blip_frames_left -= 1 self.angle_stall_blip_active = True - self.path_angle_last = 0.0 - self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 - self.bp_angle_rate_limited = False - self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False - self.sim_curvature_last = 0.0 - # Truthful shadow during the blip (see the inactive-path comment). - self.bp_kappa_cmd = self.get_current_curvature(CS) + self._reset_angle_signals(CS) self._desired_curvature_last = float(actuators.curvature) - self.precision_type = 1 if self.stall_blip_frames_left <= 0: self.stall_blip_cooldown_s = _STALL_COOLDOWN_S - return LateralResult( - apply_curvature=0.0, - curvature_rate=0.0, - path_offset=0.0, - path_angle=0.0, - ramp_type=0, - precision_type=1, - lateralUncertainty=0.0, - ) + return self._inactive_result() self.angle_stall_blip_active = False self.precision_type = 1 @@ -352,21 +343,33 @@ def update_angle_strategy(self, CC, CS, actuators, CP): LP = self.lp desired_curvature = float(actuators.curvature) - # Variable lookup time: t_base tracks planner pre-compensation; extra tapers on high speed and large curves. - # Cap liveDelay at 0.15s for VLT purposes. liveDelay can calibrate up to ~420ms on some runs, which inflates - # VLT to 0.6s and pushes the model lookahead 5m into the curve. At that depth the model sees full peak - # curvature, kappa_entering stays True, and the exit-biased blend is permanently disabled — causing the car - # to command max path_angle through the entire apex. 0.15s gives t_base ≤ 0.20s and VLT ≤ 0.33s, restoring - # the 2.8m lookahead that kept kappa_entering False at the apex in successful earlier runs. - _t_base = float(clip(self.sm['liveDelay'].lateralDelay, 0.1, 0.15)) + _DT_MDL + # Variable lookup time — delay compensation is SPLIT across two horizons (2026-07-23): + # - _t_entering (liveDelay capped 0.15s): the horizon for the entering/exiting DECISION + # only. The cap is load-bearing for apexes: a deeper decision horizon keeps + # kappa_entering True through the apex — the 0.42s-liveDelay era pathology where the + # exit-biased blend never engaged and the command flat-lined at max through the apex. + # A replay regression across 308 real apexes (routes 0a/0b/12/1b) showed even a + # 0.25s decision horizon regresses 7.5% of them, so this horizon stays short and the + # apex behavior stays identical by construction. + # - _t_base (liveDelay capped 0.30s): the model-prediction LEAD in the blend. The true + # actuation delay is ~0.29s (liveDelay median, confirmed by command/measurement + # cross-correlation on three drives); compensating only 0.15s of it left ~0.14s of + # known-but-ignored delay in the loop, driving a ~0.2 Hz closed-loop breathing + # (±0.5° at the wheel, in curves and straights alike — measured desired-osc 0.15, + # actual-osc 0.21 mrad/m, actual trailing desired by exactly the actuation delay). + # Exits stay protected regardless of this deeper lead: the exit-biased blend + # collapses the prediction weight to ~15% there. + _t_entering = float(clip(self.sm['liveDelay'].lateralDelay, 0.1, 0.15)) + _DT_MDL + _t_base = float(clip(self.sm['liveDelay'].lateralDelay, 0.1, 0.30)) + _DT_MDL _speed_factor = float(interp(v_ego, [_VLT_V_LOW_MS, _VLT_V_HIGH_MS], [1.0, 0.0])) # Direction-aware kappa factor: on curve ENTRY (model shows more curvature at t_base than planner now), # keep full lookahead so pre-steering begins early. On exit/apex, taper by magnitude to prevent unwind. - _kappa_at_t_base = 0.0 + _kappa_at_entering = 0.0 if self.model is not None and len(self.model.orientationRate.z) >= 17: _curvatures_ref = np.array(self.model.orientationRate.z) / max(0.01, v_ego) - _kappa_at_t_base = abs(float(interp(_t_base, ModelConstants.T_IDXS, _curvatures_ref))) - _kappa_entering = _kappa_at_t_base > abs(desired_curvature) + # Decision horizon (_t_entering, short by design) — NOT the blend lead horizon. + _kappa_at_entering = abs(float(interp(_t_entering, ModelConstants.T_IDXS, _curvatures_ref))) + _kappa_entering = _kappa_at_entering > abs(desired_curvature) if _kappa_entering: _kappa_factor = 1.0 # curve deepening ahead: full extra lookahead for gradual entry else: @@ -380,7 +383,6 @@ def update_angle_strategy(self, CC, CS, actuators, CP): predicted_curvature = float( interp(curvature_lookup_time, ModelConstants.T_IDXS, curvatures) ) - b = float(self.path_angle_blend_ratio) b = float(clip(b, 0.0, 1.0)) @@ -408,7 +410,8 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # Same bug class and fix as _PSCM_SAT_UNWIND_RATE and _soft_roc above. _desired_falling = abs(desired_curvature) < abs(self._desired_curvature_last) - 0.010 _on_exit_near_limit = not _kappa_entering and (_pscm_lim >= 1 or _in_hard_sat or _desired_falling) - b_blend = float(clip(b * 0.25, 0.0, 1.0)) if _on_exit_near_limit else b + _b_target = float(clip(b * 0.25, 0.0, 1.0)) if _on_exit_near_limit else b + b_blend = _b_target requested_curvature = predicted_curvature * b_blend + desired_curvature * (1.0 - b_blend) self._desired_curvature_last = desired_curvature @@ -456,12 +459,15 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # Speed-interpolated gain: at low speed both curves use 1.0; at high speed the params take effect. - self.low_gain_calc = interp( - v_ego, [13.5, 26.82], [1.0, (self.path_angle_gain_lowC_highV * self.user_dampening_factor)] - ) - self.high_gain_calc = interp(v_ego, [13.5, 26.82], [(1.30 * self.low_speed_curv_factor), (self.path_angle_gain_highC_highV * self.high_speed_curv_factor)]) - - # As the curve gets bigger, we will need a little boost to the signal to to not understeer + self.low_gain_calc = interp(v_ego, [V_LOW, V_HIGH], + [1.0, self.path_angle_gain_lowC_highV * self.user_dampening_factor]) + self.high_gain_calc = interp(v_ego, [V_LOW, V_HIGH], + [(LOW_ANCHOR_BASE * self.low_speed_curv_factor), + (self.path_angle_gain_highC_highV * self.high_speed_curv_factor)]) + + # As the curve grows the signal needs a boost to not understeer. + # COUPLING: the interp knee top (0.001) is auto-cal's MIN_KAPPA — the calibrator only + # samples fully inside the high branch. If this band moves, MIN_KAPPA moves with it. self.curvature_factor = interp(abs(kappa_cmd), [0.0007, 0.001], [self.low_gain_calc, self.high_gain_calc]) path_angle_calc = kappa_cmd * v_ego * self.curvature_factor @@ -487,7 +493,11 @@ def update_angle_strategy(self, CC, CS, actuators, CP): elif _pscm_lim >= 1: # LimitClose (F150/non-angle-mode only): block increases only path_angle = float(clip(path_angle, -abs(self.path_angle_last), abs(self.path_angle_last))) + _pre_dbc_clamp = path_angle path_angle = min(FORD_DBC_PATH_ANGLE_MAX, max(FORD_DBC_PATH_ANGLE_MIN, path_angle)) + # BluePilot: the car cannot make the requested turn this frame — PSCM authority limit + # active or the DBC clamp bit. Telemetry + a hard no-sample gate for the auto-calibration. + self.bp_angle_saturated = bool(_in_hard_sat or _pscm_lim >= 1 or path_angle != _pre_dbc_clamp) # Soft ROC limit — unconditional, slightly tighter than ford.h, applied before the # hardware bypass in ford.h is re-enabled. Lets us observe whether the limit would @@ -506,7 +516,6 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # BluePilot: did the soft ROC clip actually limit the path_angle we wanted to send this frame? self.bp_angle_rate_limited = bool(abs(path_angle - _path_angle_pre_roc) > 1e-9) - # c0 always zero -- no centering trim in angle mode. path_offset = 0.0 @@ -561,6 +570,10 @@ def update_angle_strategy(self, CC, CS, actuators, CP): ramp_type = 2 + # Continuous auto-calibration of the speed factors (armed-only; a no-op otherwise). + # Nudges are written to the factor params — update_angle_params reads them back, so the + # factors have a single owner here. Human-turn/stall-blip frames never reach this point. + self._feed_autocal(CS, kappa_cmd, current_curvature) return LateralResult( apply_curvature=0.0, @@ -571,3 +584,4 @@ def update_angle_strategy(self, CC, CS, actuators, CP): precision_type=self.precision_type, lateralUncertainty=lateral_uncertainty, ) + diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py new file mode 100644 index 0000000000..ef0e9eaab0 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -0,0 +1,975 @@ +"""Tests for the continuous angle-mode factor auto-calibration (angle_autocal.py).""" +import json +import math +import random + +import pytest + +from opendbc.sunnypilot.car.ford.angle_autocal import ( + Frame, + AngleFactorEstimator, AutoCalPipeline, PeakMatcher, QualityMonitor, SteadyStateGate, + speed_alpha, V_LOW, V_HIGH, LOW_ANCHOR_BASE, STEADY_TIME_S, MIN_KAPPA, REL_KAPPA_RATE, + PRESS_HOLDBACK_S, PRESS_COOLDOWN_S, MAX_LAT_ACCEL, MAX_LONG_ACCEL, + PEAK_MIN_KAPPA, PEAK_PROMINENCE, PEAK_MEDIAN_N, PEAK_WEIGHT_S, + SPIKE_MEAS_RATE, DISTURBANCE_BLANK_S, ROUGH_RMS_MAX, WS_SPREAD_JUMP, + TAU_EVIDENCE_S, LR_MIN_WEIGHT, LR_TOL, + NUDGE_PERIOD_S, NUDGE_MIN_WEIGHT, NUDGE_MAX_STEP, FACTOR_STEP, nudge_units, + VERIFY_MIN_WEIGHT, VERIFY_FAIL_HOLD_WEIGHT, + LOCK_MIN_WEIGHT, LOCK_DEADBAND, LOCK_STABLE_S, +) + +PLATFORM_GAIN_HIGH = 1.05 # Mach-E +DT = 0.05 + + +def applied_gain(v, low_factor, high_factor): + a = speed_alpha(v) + return (1.0 - a) * (LOW_ANCHOR_BASE * low_factor) + a * (PLATFORM_GAIN_HIGH * high_factor) + + +def ideal_gain(v, true_low, true_high): + return applied_gain(v, true_low, true_high) + + +def feed_plant(est, true_low, true_high, speeds, applied_low=1.0, applied_high=1.0, + kappa=0.002, n_per_speed=200, noise=0.0, seed=42): + """Feed samples from a plant whose true gain corresponds to the given ideal factors. + + The plant's response ratio r = applied_gain / ideal_gain: if the applied factors already + matched the true ones, r would be 1 everywhere. + """ + rng = random.Random(seed) + for v in speeds: + g = applied_gain(v, applied_low, applied_high) + r0 = g / ideal_gain(v, true_low, true_high) + for _ in range(n_per_speed): + r = r0 * (1.0 + (rng.uniform(-noise, noise) if noise else 0.0)) + est.add_sample(v, kappa, kappa * r, g, weight=DT) + + +class TestAngleFactorEstimator: + def test_recovers_true_factors(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + feed_plant(est, 0.92, 1.21, speeds=[10, 12, 15, 18, 21, 24, 27, 29], n_per_speed=200, noise=0.03) + low, high, _ = est.solve() + assert abs(low - 0.92) < 0.02, low + assert abs(high - 1.21) < 0.02, high + + def test_invariant_to_applied_factor_trajectory(self): + # Half the drive on one applied pair, half on another: same truth must come out. + # This is the property that keeps the nudge loop stable. + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + feed_plant(est, 0.92, 1.21, speeds=[10, 15, 20, 25, 29], applied_low=1.10, applied_high=0.90, + n_per_speed=150, noise=0.03, seed=1) + feed_plant(est, 0.92, 1.21, speeds=[10, 15, 20, 25, 29], applied_low=0.95, applied_high=1.20, + n_per_speed=150, noise=0.03, seed=2) + low, high, _ = est.solve() + assert abs(low - 0.92) < 0.02, low + assert abs(high - 1.21) < 0.02, high + + def test_rejects_bad_samples(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + g = applied_gain(20.0, 1.0, 1.0) + assert not est.add_sample(20.0, 0.0005, 0.0005, g) # below curvature threshold + assert not est.add_sample(5.0, 0.002, 0.002, g) # below speed threshold + assert not est.add_sample(20.0, 0.002, -0.002, g) # sign mismatch + assert not est.add_sample(20.0, 0.002, 0.02, g) # absurd ratio + assert not est.add_sample(29.0, 0.004, 0.004, g) # 3.4 m/s^2 lat accel + assert est.n == 0 + assert est.add_sample(29.0, 0.0025, 0.0025, g) # 2.1 m/s^2 — within tire limits + + def test_factor_clamp(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + feed_plant(est, 2.5, 0.2, speeds=[10, 20, 29], n_per_speed=100) + low, high, _ = est.solve() + assert low == 1.5 and high == 0.5 # clamped to the +/- button range + + def test_decay_halves_weight_at_tau_ln2(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + feed_plant(est, 1.0, 1.0, speeds=[10, 29], n_per_speed=100) + w0 = est.s_w + est.decay(TAU_EVIDENCE_S * math.log(2.0)) + assert abs(est.s_w - 0.5 * w0) < 1e-9 + + def test_lr_divergence_flags_bank_bias(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + n = int((LR_MIN_WEIGHT + 2) / DT) + g_hi = applied_gain(28.0, 1.0, 1.0) + for _ in range(n): # balanced clean evidence at the high anchor keeps the fit solvable + est.add_sample(28.0, 0.0015, 0.0015, g_hi, weight=DT) + est.add_sample(28.0, -0.0015, -0.0015, g_hi, weight=DT) + g = applied_gain(10.0, 1.0, 1.0) + for _ in range(n): # left turns read 10% strong, right turns 10% weak — crowned road + est.add_sample(10.0, 0.002, 0.002 * 1.10, g, weight=DT) + est.add_sample(10.0, -0.002, -0.002 * 0.90, g, weight=DT) + assert est.lr_divergence(0) > LR_TOL + _, _, st = est.solve() + assert st["stderr_eff_low"] > st["stderr_low"] # divergence inflates the effective error + + def test_serialization_round_trip(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + feed_plant(est, 0.95, 1.12, speeds=[10, 15, 20, 25, 29], n_per_speed=120, noise=0.02) + d = json.loads(json.dumps(est.to_dict())) # through real JSON, like the param + est2 = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + est2.from_dict(d) + assert est.solve() == est2.solve() + assert est2.n == est.n + + +class TestSteadyStateGate: + def test_requires_sustained_steady(self): + gate = SteadyStateGate(dt=DT) + needed = int(STEADY_TIME_S / DT) + results = [gate.update(True, MIN_KAPPA * 2, False, False, False) + for _ in range(needed + 2)] + assert not any(results[:needed - 1]) + assert results[-1] + + def test_resets_on_any_flag(self): + gate = SteadyStateGate(dt=DT) + for _ in range(int(STEADY_TIME_S / DT) + 1): + gate.update(True, MIN_KAPPA * 2, False, False, False) + assert gate.update(True, MIN_KAPPA * 2, False, False, False) + gate.update(True, MIN_KAPPA * 2, True, False, False) # pressed + assert gate.steady_s == 0.0 + + def test_ramp_within_relative_bound_admitted(self): + # Lag alignment absorbs the transport delay, so a genuinely winding road — kappa + # moving at up to REL_KAPPA_RATE of itself — IS evidence now. This ramp (25%/s + # relative) was rejected by the old frozen-command gate; that starvation discarded + # 89-100% of clean curve time on real winding-road drives (2026-07-22 analysis). + gate = SteadyStateGate(dt=DT) + k = MIN_KAPPA * 3 + admitted = False + for _ in range(int(STEADY_TIME_S / DT) * 6): + admitted |= gate.update(True, k, False, False, False) + k += 0.25 * k * DT + assert admitted + + def test_ramp_beyond_relative_bound_rejected(self): + # Twice the relative bound: the residual delay-estimate error would bias these + # ratios beyond what the stderr machinery is sized for — still rejected. + gate = SteadyStateGate(dt=DT) + k = MIN_KAPPA * 3 + admitted = False + for _ in range(int(STEADY_TIME_S / DT) * 6): + admitted |= gate.update(True, k, False, False, False) + k += 2.0 * REL_KAPPA_RATE * k * DT + assert not admitted + + def test_saturation_blocks(self): + gate = SteadyStateGate(dt=DT) + for _ in range(int(STEADY_TIME_S / DT) + 2): + assert not gate.update(True, 0.002, False, False, False, saturated=True) + + def test_light_torque_starts_cooldown(self): + gate = SteadyStateGate(dt=DT) + gate.update(True, 0.002, False, False, False, driver_torque=0.7) + assert gate.grip_cooldown_s > 0.0 + blocked = int(PRESS_COOLDOWN_S / DT) - 1 + for _ in range(blocked): + assert not gate.update(True, 0.002, False, False, False) + + +class TestQualityMonitor: + def test_clean_cornering_never_rejected(self): + q = QualityMonitor(dt=DT) + # A realistic apex sweep: command and measurement move together at plausible rates. + t = 0.0 + ok_all = True + for _ in range(400): + k = 0.002 * math.sin(2 * math.pi * t / 10.0) + ok_all &= q.update(k, k * 0.98, a_ego=0.2, ws_spread=0.05) + t += DT + assert ok_all + assert all(v == 0 for v in q.counters.values()) + + def test_flick_blanks_and_recovers(self): + q = QualityMonitor(dt=DT) + for _ in range(50): + assert q.update(0.002, 0.002) + # Bump: measurement jumps a full SPIKE step in one frame, command quiet. The return + # to baseline next frame is itself a spike (the down-edge of the same bump) and + # legitimately re-arms the blanking, so recovery takes blank + 1 frame. + assert not q.update(0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2) + assert q.flick_fired + blank_frames = int(DISTURBANCE_BLANK_S / DT) + for i in range(blank_frames): + assert not q.update(0.002, 0.002), i + assert q.update(0.002, 0.002) + assert q.counters["flick"] > 0 + + def test_command_tracking_spike_is_not_flick(self): + # The measurement racing after a moving command is control, not disturbance. + q = QualityMonitor(dt=DT) + q.update(0.002, 0.002) + q.update(0.002 + 0.001, 0.002 + SPIKE_MEAS_RATE * DT * 2) # command moved too + assert not q.flick_fired + + def test_wheel_speed_jump_corroborates(self): + q = QualityMonitor(dt=DT) + q.update(0.002, 0.002, ws_spread=0.05) + assert not q.update(0.002, 0.002, ws_spread=0.05 + WS_SPREAD_JUMP * 1.5) + assert q.flick_fired + + def test_rough_road_blocks_until_settled(self): + q = QualityMonitor(dt=DT) + rng = random.Random(7) + # Washboard: broadband measurement noise well above the RMS threshold. + rejected = 0 + for _ in range(200): + if not q.update(0.002, 0.002 + rng.uniform(-4, 4) * ROUGH_RMS_MAX): + rejected += 1 + assert rejected > 100 + assert q.counters["rough"] + q.counters["flick"] == rejected + + def test_long_accel_rejects(self): + q = QualityMonitor(dt=DT) + assert q.update(0.002, 0.002, a_ego=MAX_LONG_ACCEL * 0.5) + assert not q.update(0.002, 0.002, a_ego=MAX_LONG_ACCEL * 1.5) + assert q.counters["accel"] == 1 + + +def _sine_apex_drive(pm, v, amp, period_s, n_frames, gain_ratio=1.0, lag_frames=6, + ok=True, dt=DT): + """Drive the peak matcher with a sinusoidal command and a lagged, scaled measurement. + Returns all committed samples.""" + out = [] + hist = [] + t = 0.0 + for _ in range(n_frames): + k = amp * math.sin(2 * math.pi * t / period_s) + hist.append(k) + k_lag = hist[-1 - lag_frames] if len(hist) > lag_frames else 0.0 + out += pm.push(k, k_lag * gain_ratio, v, applied_gain(v, 1.0, 1.0), ok) + t += dt + return out + + +class TestPeakMatcher: + def test_recovers_gain_ratio_from_lagged_sine(self): + pm = PeakMatcher(dt=DT) + committed = _sine_apex_drive(pm, v=12.0, amp=0.003, period_s=8.0, n_frames=2400, + gain_ratio=0.92) + assert len(committed) >= 2 + for (_v, k_cmd, k_meas, _g) in committed: + assert abs(k_meas / k_cmd - 0.92) < 0.02 + + def test_ripple_below_prominence_never_fires(self): + pm = PeakMatcher(dt=DT) + committed = _sine_apex_drive(pm, v=12.0, amp=PEAK_PROMINENCE * 0.4 + PEAK_MIN_KAPPA, + period_s=1.6, n_frames=1200) + # Fast ripple: the +-1s dominance window contains multiple crests, so no apex is + # dominant and nothing commits. + assert committed == [] + + def test_poisoned_window_discards_apex(self): + pm = PeakMatcher(dt=DT) + n = 0 + hist = [] + t = 0.0 + for i in range(2400): + k = 0.003 * math.sin(2 * math.pi * t / 8.0) + hist.append(k) + k_lag = hist[-7] if len(hist) > 6 else 0.0 + if i == 1200: + pm.poison_recent(0.3) # a disturbance was detected mid-drive + n += len(pm.push(k, k_lag, 12.0, applied_gain(12.0, 1.0, 1.0), ok=True)) + t += DT + pm2 = PeakMatcher(dt=DT) + n_clean = len(_sine_apex_drive(pm2, v=12.0, amp=0.003, period_s=8.0, n_frames=2400)) + assert n <= n_clean # the poisoned apex (and only that region) was lost + + def test_median_of_three_kills_single_outlier(self): + pm = PeakMatcher(dt=DT) + committed = [] + hist = [] + t = 0.0 + # One apex in the middle of the drive measures wildly strong (loose gravel moment): + # the median commit must not let its ratio through. + for i in range(3600): + k = 0.003 * math.sin(2 * math.pi * t / 8.0) + hist.append(k) + k_lag = hist[-7] if len(hist) > 6 else 0.0 + ratio = 2.2 if 1180 <= i <= 1260 else 1.0 + committed += pm.push(k, k_lag * ratio, 12.0, applied_gain(12.0, 1.0, 1.0), True) + t += DT + assert len(committed) >= 2 + for (_v, k_cmd, k_meas, _g) in committed: + assert abs(k_meas / k_cmd) < 1.5 # the 2.2x apex never got committed + + +def run_pipeline(pipe, n, torque=0.0, pressed=False, saturated=False, kappa=0.002, v=20.0, + low=1.0, high=1.0): + committed = [] + for _ in range(n): + committed += pipe.update(_frame(v, kappa, kappa, pressed=pressed, + saturated=saturated, torque=torque, + low=low, high=high)) + return committed + + +class TestAutoCalPipeline: + def test_commits_after_holdback(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / DT) + 3 + _LAG_F + committed = run_pipeline(pipe, warm) + assert pipe.est.n > 0 + assert len(committed) == pipe.est.n + + def test_grip_cancels_staged_samples(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + warm = int(STEADY_TIME_S / DT) + 1 + int(PRESS_HOLDBACK_S / DT) // 2 + _LAG_F + run_pipeline(pipe, warm) + assert len(pipe._staged) > 0 and pipe.est.n == 0 + pipe.update(_frame(20.0, 0.002, 0.002, pressed=True)) # grip + assert len(pipe._staged) == 0 + assert pipe.est.n == 0 # nothing from before the grip ever reached the estimator + + def test_disturbance_cancels_staged_samples(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + warm = int(STEADY_TIME_S / DT) + 1 + int(PRESS_HOLDBACK_S / DT) // 2 + _LAG_F + run_pipeline(pipe, warm) + assert len(pipe._staged) > 0 and pipe.est.n == 0 + # Bump: measured curvature jumps while the command sits still. + pipe.update(_frame(20.0, 0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2)) + assert len(pipe._staged) == 0 and pipe.est.n == 0 + # And the blanking window keeps evidence off while the car settles. + committed = run_pipeline(pipe, int(DISTURBANCE_BLANK_S / DT) - 2) + assert committed == [] + + def test_saturated_frames_never_commit(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + committed = run_pipeline(pipe, 100, saturated=True) + assert committed == [] and pipe.est.n == 0 + + def test_idle_clears_staging(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + run_pipeline(pipe, int(STEADY_TIME_S / DT) + 5 + _LAG_F) + assert len(pipe._staged) > 0 + pipe.idle() + assert len(pipe._staged) == 0 and pipe.gate.steady_s == 0.0 + assert pipe._hist == [] # alignment must never target commands across a discontinuity + + def test_unsettled_measurement_not_staged(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / DT) + 10 + kappa_meas = 0.0010 + staged_during_sweep = 0 + for _ in range(warm): + pipe.update(_frame(20.0, 0.002, kappa_meas)) + if kappa_meas < 0.0019: + kappa_meas += 0.0002 # 0.004/s sweep, far above the settle bound + staged_during_sweep = len(pipe._staged) + pipe.est.n + assert staged_during_sweep == 0 + assert pipe.est.n > 0 + + def test_near_limit_evidence_downweighted_to_zero(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + # kappa*v^2 = 2.55 > MAX_LAT_ACCEL: hard-rejected as 'limit'. + committed = run_pipeline(pipe, 60, kappa=0.0034, v=27.4) + assert committed == [] + assert pipe.quality.counters["limit"] > 0 + + def test_pipeline_serialization_round_trip(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + run_pipeline(pipe, 200) + pipe.stable_s = 123.0 + pipe.nudges = 4 + pipe.verify_result[0] = "confirmed" + pipe.verify_hold[1] = 12.0 + d = json.loads(json.dumps(pipe.to_dict())) + pipe2 = AutoCalPipeline(PLATFORM_GAIN_HIGH) + pipe2.from_dict(d) + assert pipe2.est.solve() == pipe.est.solve() + assert pipe2.stable_s == 123.0 and pipe2.nudges == 4 and not pipe2.locked + assert pipe2.verify_result[0] == "confirmed" and pipe2.verify_hold[1] == 12.0 + + +def _evidenced_pipe(true_low=1.10, true_high=1.10, applied=(1.0, 1.0), weight_s=15.0): + """Pipeline with clean steady evidence at both anchors against a known plant.""" + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + n = int(weight_s / DT) + for v, kappa in ((10.0, 0.004), (28.0, 0.0015)): + g = applied_gain(v, *applied) + r = g / ideal_gain(v, true_low, true_high) + pipe.gate.steady_s = 0.0 + pipe._meas_last = None + pipe._hist.clear() # speed-block boundary: never align against the other block's cmd + for _ in range(n): + pipe.update(_frame(v, kappa, kappa * r, low=applied[0], high=applied[1])) + return pipe + + +def _feed_low(pipe, applied, seconds, true_low, true_high, ratio_scale=1.0, + v=10.0, kappa=0.004): + """Steady low-band frames from the plant under the given applied factors; ratio_scale + != 1 makes the car respond off-model (the adjust-then-verify failure case).""" + g = applied_gain(v, applied[0], applied[1]) + r = g / ideal_gain(v, true_low, true_high) * ratio_scale + pipe.gate.steady_s = 0.0 + pipe._meas_last = None + pipe._hist.clear() + for _ in range(int(seconds / DT)): + pipe.update(_frame(v, kappa, kappa * r, low=applied[0], high=applied[1])) + + +class TestFactorNudger: + def _evidenced_pipe(self, **kw): + return _evidenced_pipe(**kw) + + def test_nudges_toward_target_bounded(self): + # err 0.10, damped by NUDGE_GAIN then capped: a big-but-bounded step, not the whole error. + pipe = self._evidenced_pipe(true_low=1.10, true_high=1.10) + rec = pipe.recommend(1.0, 1.0) + assert rec is not None + low, high = rec + assert low == round(1.0 + NUDGE_MAX_STEP, 2) + assert high == round(1.0 + NUDGE_MAX_STEP, 2) + + def test_single_step_when_close(self): + # A target ~0.01 away moves by exactly one menu step (the point of damped stepping). + pipe = self._evidenced_pipe(true_low=1.01, true_high=1.01) + rec = pipe.recommend(1.0, 1.0) + assert rec is not None + assert rec[0] == round(1.0 + FACTOR_STEP, 2) and rec[1] == round(1.0 + FACTOR_STEP, 2) + + def test_deadband_no_nudge(self): + # Inside the implicit deadband (|gain*err| < half a step): leave it alone. + pipe = self._evidenced_pipe(true_low=1.004, true_high=1.004) + assert pipe.recommend(1.0, 1.0) is None + + def test_nudge_units_damped_and_capped(self): + cap = round(NUDGE_MAX_STEP / FACTOR_STEP) + assert nudge_units(0.0) == 0 + assert nudge_units(0.004) == 0 # implicit deadband + assert nudge_units(0.01) == 1 # single menu step when close + assert nudge_units(0.10) == cap # far: damped then capped + assert nudge_units(-0.10) == -cap # symmetric + + def test_rate_limited(self): + pipe = self._evidenced_pipe() + assert pipe.recommend(1.0, 1.0) is not None + assert pipe.recommend(1.02, 1.02) is None # inside NUDGE_PERIOD_S + # advance active time + for _ in range(int(NUDGE_PERIOD_S / DT) + 1): + pipe.update(_frame(10.0, 0.004, 0.004, low=1.02, high=1.02)) + assert pipe.recommend(1.02, 1.02) is not None + + def test_insufficient_evidence_no_nudge(self): + pipe = self._evidenced_pipe(weight_s=NUDGE_MIN_WEIGHT * 0.3) + assert pipe.recommend(1.0, 1.0) is None + + def test_no_cumulative_cap_walks_to_the_fit(self): + # 2026-07-22 design decision: no per-drive movement cap. A car that is genuinely + # 40% off must be allowed to walk all the way in one drive, as long as every step + # keeps verifying against fresh evidence (the plant here always agrees). + pipe = self._evidenced_pipe(true_low=1.40, true_high=1.40) + applied = [1.0, 1.0] + for _ in range(30): + for _f in range(int(NUDGE_PERIOD_S / DT) + 1): + g = applied_gain(10.0, *applied) + r = g / ideal_gain(10.0, 1.40, 1.40) + pipe.update(_frame(10.0, 0.004, 0.004 * r, low=applied[0], high=applied[1])) + rec = pipe.recommend(*applied) + if rec is not None: + applied = list(rec) + assert applied[0] >= 1.35, applied # far past the old 0.04/0.10 caps + assert pipe.verify_result[0] == "confirmed" # and every step was checked on the way + + def test_user_edit_soft_resets(self): + pipe = self._evidenced_pipe() + w0 = pipe.est.s_w + pipe.stable_s = 100.0 + pipe.user_edit() + assert abs(pipe.est.s_w - 0.5 * w0) < 1e-9 + assert pipe.stable_s == 0.0 + # Evidence NOT wiped: the fit is still there, just less confident. + assert pipe.est.solve() is not None + + +class TestLagAlignment: + """The 2026-07-22 evidence-starvation fix: ratios are taken against the command from + lateral_delay ago, so winding roads (a moving command) become usable evidence without + lag bias — the exact scenario the old frozen-command gate had to discard.""" + + def _ramped_pipe(self, true_gain_ratio, lag_frames=4, n=1200, rel_rate=0.25): + """Plant with a PURE transport delay: meas(t) = ratio * cmd(t - lag). The command + ramps continuously at rel_rate (within the admission bound) — under the old gate + this drive yields nothing; under alignment it must recover the ratio exactly.""" + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + hist = [] + k = 0.002 + for _ in range(n): + hist.append(k) + k_meas = true_gain_ratio * (hist[-1 - lag_frames] if len(hist) > lag_frames else 0.0) + pipe.update(_frame(10.0, k, k_meas, lat_delay=lag_frames * DT)) + k *= 1.0 + rel_rate * DT + if k > 0.004: + k = 0.002 # saw-tooth reset; the drop is a huge rate step the gate must absorb + return pipe + + def test_recovers_ratio_from_delayed_moving_command(self): + # Car delivers 90% of requested with a 0.2s transport delay, command always moving. + pipe = self._ramped_pipe(0.90) + assert pipe.est.n > 100 # the old gate got ~zero here + _w, r = pipe.est.recent_response(0) # all evidence at v=10 -> low half + assert r is not None and abs(r - 0.90) < 0.005, r # aligned ratio is exact, not lag-biased + + def test_same_frame_ratio_would_have_been_biased(self): + # Sanity for the whole design: on this plant the same-frame ratio is NOT the gain — + # the lag makes it read low on a rising ramp. Alignment is what removes that bias. + k = 0.002 + hist = [] + biased = [] + for _ in range(200): + hist.append(k) + if len(hist) > 4: + biased.append((0.90 * hist[-5]) / k) + k *= 1.0 + 0.25 * DT + assert max(biased) < 0.90 - 0.01 # every same-frame sample reads low + + def test_no_evidence_before_history_fills(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + for _ in range(_LAG_F - 2): + pipe.update(_frame(10.0, 0.003, 0.003)) + assert pipe._staged == [] and pipe.est.n == 0 + + def test_delay_clamped_to_trust_window(self): + # An absurd liveDelay value must not demand an absurd history depth. + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / DT) + 3 + int(round(0.42 / DT)) + 1 + for _ in range(warm): + pipe.update(_frame(10.0, 0.003, 0.003, lat_delay=5.0)) + assert pipe.est.n > 0 # clamped to LAG_MAX_S and evidence still flows + + +class TestQuietGate: + """Loop hunting must never become gain evidence — the user's criterion, 2026-07-23: + taking 1-4 passes per step is fine; moving the needle on mid-dynamics data is not.""" + + def test_hunting_yields_almost_no_evidence(self): + # Same duration, same command: a calm constant-deficit plant vs a hunting plant + # whose error swings on a ~3s loop cycle (all swings INSIDE the rate bounds that + # used to admit them). The hunting run must yield a small fraction of the weight. + quiet = AutoCalPipeline(PLATFORM_GAIN_HIGH) + hunt = AutoCalPipeline(PLATFORM_GAIN_HIGH) + for i in range(1200): + quiet.update(_frame(10.0, 0.003, 0.003 * 0.90)) + swing = 0.0006 * math.sin(2 * math.pi * i * DT / 3.0) + hunt.update(_frame(10.0, 0.003, 0.003 * 0.90 + swing)) + assert quiet.est.s_w > 0 + assert hunt.est.s_w < 0.35 * quiet.est.s_w, (hunt.est.s_w, quiet.est.s_w) + + def test_constant_deficit_is_calm_and_admitted(self): + # A steady plant deficit keeps a FLAT error trend: exactly the signal we want, + # and the quiet gate must not confuse it with dynamics. + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + _feed_low(pipe, (1.0, 1.0), 10.0, 1.10, 1.10) + assert pipe.est.s_w > 0 + _w, r = pipe.est.recent_response(0) + assert r is not None and abs(r - 1.0 / 1.10) < 0.01 + + +class TestAdjustVerify: + """Every step is judged against FRESH post-step evidence before its anchor may step + again — the no-cap regime's runaway protection ('poll a couple turns, adjust, poll + some more', made enforceable).""" + + def test_step_opens_verify_window(self): + pipe = _evidenced_pipe() + rec = pipe.recommend(1.0, 1.0) + assert rec is not None + assert pipe.verify[0] is not None and pipe.verify[0]["to"] == rec[0] + assert pipe.est.recent[0] == [0.0, 0.0] # the judgment sees only post-step data + + def test_no_second_step_until_fresh_evidence(self): + pipe = _evidenced_pipe(true_low=1.40, true_high=1.40) + rec = pipe.recommend(1.0, 1.0) + assert rec is not None + # Advance the nudge clock with frames that carry NO evidence (below MIN_KAPPA): + # plenty of time passes, but the step has not been answered by data. + for _ in range(int(NUDGE_PERIOD_S / DT) + 1): + pipe.update(_frame(10.0, 0.0005, 0.0005, low=rec[0], high=rec[1])) + assert pipe.recommend(*rec) is None # window still open + # Fresh agreeing evidence arrives: the step is judged and the walk continues. + _feed_low(pipe, rec, VERIFY_MIN_WEIGHT + 3.0, 1.40, 1.40) + assert pipe.verify[0] is None + assert pipe.verify_result[0] == "confirmed" + assert pipe.recommend(*rec) is not None + + def test_failed_verify_holds_anchor(self): + pipe = _evidenced_pipe(true_low=1.10, true_high=1.10) + rec = pipe.recommend(1.0, 1.0) + assert rec is not None and rec[0] > 1.0 # stepped UP toward the fit + # Contrarian car: after the step up, the measured response DROPS — the data + # contradicts the model, so the step must not be trusted. + _feed_low(pipe, rec, VERIFY_MIN_WEIGHT + 3.0, 1.10, 1.10, ratio_scale=0.85) + assert pipe.verify_result[0] == "failed" + assert pipe.verify_hold[0] == VERIFY_FAIL_HOLD_WEIGHT + # Kill more clock without evidence: still held (fresh weight < the fail demand). + for _ in range(int(NUDGE_PERIOD_S / DT) + 1): + pipe.update(_frame(10.0, 0.0005, 0.0005, low=rec[0], high=rec[1])) + assert pipe.recommend(*rec) is None + # Twice the evidence arrives and keeps asking for movement: the hold releases. + _feed_low(pipe, rec, VERIFY_FAIL_HOLD_WEIGHT + 4.0, 1.10, 1.10, ratio_scale=0.85) + assert pipe.recommend(*rec) is not None + + def test_lock_disabled_never_freezes(self): + # FordAngleAutoCalLock off: stability may accumulate forever, the pipeline must not + # lock — continuous adaptation for the life of the toggle. Lock-eligible evidence + # (weights past LOCK_MIN_WEIGHT, target == applied) is earned for real so the + # 'ready' predicate holds and only the lock_enabled check stands between + # stable_s and the freeze. + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + feed_plant(pipe.est, 1.0, 1.0, speeds=[10, 28], n_per_speed=1400) + pipe.lock_enabled = False + pipe.stable_s = LOCK_STABLE_S - 0.1 + _feed_low(pipe, (1.0, 1.0), 3.0, 1.0, 1.0) + assert not pipe.locked + assert pipe.stable_s > LOCK_STABLE_S # kept counting straight past the threshold + + def test_verify_state_survives_serialization(self): + pipe = _evidenced_pipe() + rec = pipe.recommend(1.0, 1.0) + assert rec is not None + d = json.loads(json.dumps(pipe.to_dict())) + pipe2 = AutoCalPipeline(PLATFORM_GAIN_HIGH) + pipe2.from_dict(d) + assert pipe2.verify[0] == pipe.verify[0] + assert pipe2.est.recent == pipe.est.recent + + +class TestRecentResponse: + def test_tracks_current_ratio(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + g = applied_gain(10.0, 1.0, 1.0) + for _ in range(100): + est.add_sample(10.0, 0.002, 0.002 * 0.93, g, weight=DT) + w, r = est.recent_response(0) + assert abs(r - 0.93) < 1e-9 and w > 4.0 # "turns 93% of requested" + assert est.recent_response(1)[1] is None # no high-band evidence yet + + +class TestUiState: + def test_propose_then_verify_phases(self): + pipe = _evidenced_pipe(true_low=1.10, true_high=1.10) + ui = pipe.ui_state(1.0, 1.0) + assert ui["low"]["ph"] == "propose" and ui["low"]["t"] > 1.0 + assert abs(ui["low"]["r"] - 1.0 / 1.10) < 0.02 + json.dumps(ui) # must survive the telemetry string + rec = pipe.recommend(1.0, 1.0) + ui = pipe.ui_state(*rec) + assert ui["low"]["ph"] == "verify" and ui["low"]["to"] == rec[0] + + def test_collect_phase_before_evidence(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + ui = pipe.ui_state(1.0, 1.0) + assert ui["low"]["ph"] == "collect" and ui["high"]["ph"] == "collect" + json.dumps(ui) + + def test_good_phase_when_matched(self): + pipe = _evidenced_pipe(true_low=1.0, true_high=1.0) + ui = pipe.ui_state(1.0, 1.0) + assert ui["low"]["ph"] == "good" and ui["high"]["ph"] == "good" + + +class TestClosedLoopConvergence: + """The whole point: a synthetic car with true factors 1.02/1.15 driven from 1.00/1.00 + must be nudged into the lock deadband and eventually lock, across simulated drives, + with bumps and grips injected along the way.""" + + TRUE_LOW, TRUE_HIGH = 1.02, 1.15 + + def _drive(self, pipe, applied, seconds, v, kappa_amp, rng): + """Alternating-direction steady arcs with brief transitions; occasional bumps and + grips. Plant: first-order lag toward gain-scaled command. Nudges applied live.""" + lag_tau = 0.35 + k_meas = 0.0 + frames = int(seconds / DT) + seg_frames = int(20.0 / DT) + nudge_log = [] + for i in range(frames): + seg, pos = divmod(i, seg_frames) + direction = 1.0 if seg % 2 == 0 else -1.0 + # 1.5s ramp between arcs (clearly non-steady), then constant curvature. + ramp = min(1.0, pos / int(1.5 / DT)) + k_cmd = direction * kappa_amp * ramp + g = applied_gain(v, *applied) + k_target = k_cmd * g / ideal_gain(v, self.TRUE_LOW, self.TRUE_HIGH) + k_meas += (k_target - k_meas) * DT / (lag_tau + DT) + bump = rng.random() < 0.001 # ~one flick per 50 s + meas = k_meas + (SPIKE_MEAS_RATE * DT * 3 if bump else 0.0) + grip = 1.2 if rng.random() < 0.0005 else 0.0 + pipe.update(_frame(v, k_cmd, meas, torque=grip, a_ego=0.1, + low=applied[0], high=applied[1])) + rec = pipe.recommend(*applied) + if rec is not None: + nudge_log.append(rec) + applied[0], applied[1] = rec + if pipe.locked: + break + return applied, nudge_log + + def test_converges_and_locks(self): + rng = random.Random(11) + applied = [1.00, 1.00] + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) + all_nudges = [] + # Simulated multi-drive: each "drive" serializes and restores like an ignition cycle. + for _drive_i in range(8): + if pipe.locked: + break + # Half the drive at the low anchor, half at the high anchor. + applied, n1 = self._drive(pipe, applied, 240.0, v=11.0, kappa_amp=0.004, rng=rng) + applied, n2 = self._drive(pipe, applied, 240.0, v=28.0, kappa_amp=0.0015, rng=rng) + all_nudges += n1 + n2 + d = json.loads(json.dumps(pipe.to_dict())) + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH) # new card process + pipe.from_dict(d) + pipe.idle() + + assert abs(applied[0] - self.TRUE_LOW) <= LOCK_DEADBAND, (applied, len(all_nudges)) + assert abs(applied[1] - self.TRUE_HIGH) <= LOCK_DEADBAND, (applied, len(all_nudges)) + assert pipe.locked, (applied, pipe.stable_s, pipe.est.weight_low, pipe.est.weight_high) + # No oscillation: once inside the deadband the nudger must not bounce in and out. + lows = [r[0] for r in all_nudges] + assert all(l2 >= l1 - NUDGE_MAX_STEP - 1e-9 for l1, l2 in zip(lows, lows[1:])), lows + + +def _frame(v, kc, km, pressed=False, rate=False, dev=False, saturated=False, + torque=0.0, a_ego=0.0, ws=None, low=1.0, high=1.0, lat_delay=0.2) -> Frame: + """Test scaffolding: Frame with benign defaults (the production dataclass has none).""" + return Frame(v_ego=v, kappa_cmd=kc, kappa_meas=km, steering_pressed=pressed, + angle_rate_limited=rate, deviation_limited=dev, saturated=saturated, + driver_torque=torque, a_ego=a_ego, ws_spread=ws, + low_factor=low, high_factor=high, lateral_delay=lat_delay) + + +# Alignment warmup at the default test delay (0.2 s): the pipeline needs this many frames +# of command history before any steady evidence can exist. +_LAG_F = int(round(0.2 / DT)) + 1 + + +class _MockParams: + """Duck-typed openpilot Params: just enough for update_angle_params. put() lands + immediately (readable on the next get), like a completed async write. + + TYPE-CHECKED like the real fork's Params (params_pyx python2cpp): writing the wrong + python type raises TypeError. The real system silently ate a str-into-FLOAT nudge + write on-device because the old mock accepted anything — never again.""" + + _TYPES = { + "FordLowSpeedFactor_ang": float, + "FordHighSpeedFactor_ang": float, + "FordAngleAutoCal": bool, + "FordAngleAutoCalState": str, + "FordAngleAutoCalError": str, + "FordAngleAutoCalReset": bool, + "FordAngleAutoCalLock": bool, + "lane_change_factor_high_ang": float, + } + + # Params whose declared default (params_keys.h) is true — the real get_bool returns + # the default for unwritten keys, so the mock must too. + _BOOL_DEFAULTS = {"FordAngleAutoCalLock": True} + + def __init__(self, values): + self.values = values + self.written = {} + + def get(self, key, return_default=False): + return self.values.get(key) + + def get_bool(self, key): + if key not in self.values: + return self._BOOL_DEFAULTS.get(key, False) + return bool(self.values.get(key)) + + def put(self, key, value, block=False): + # The real Params.put lands immediately when block=True; this mock always lands + # immediately, so both paths behave the same here (readable on the next get). + expected = self._TYPES.get(key) + if expected is not None and not isinstance(value, expected): + raise TypeError(f"Type mismatch while writing param {key}: got {type(value)}, expected {expected}") + self.values[key] = value + self.written[key] = value + + def put_bool(self, key, value): + self.put(key, bool(value)) + + +class TestOnboardGlue: + """Exercise the REAL LateralAngleExt param/arming glue — the seam unit tests of the + pipeline cannot see. This is the class of test that caught the on-device card + crash-loop (stale attribute) that component tests missed.""" + + def _ext(self): + pytest.importorskip("cereal.messaging") # linux-only + from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt + + class _Harness(LateralAngleExt): + def _ensure_lateral_curv_initialized(self, CP): + pass + + ext = _Harness() + class _CP: + carFingerprint = "FORD_MUSTANG_MACH_E_MK1" + ext.CP = _CP() + return ext + + def _tick(self, ext, p, n=1): + for _ in range(100 * n): + ext.update_angle_params(p) + + def test_param_glue_runs_without_error(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 0, "FordAngleAutoCalState": ""}) + self._tick(ext, p, n=2) + assert ext.autocal_ctl.pipeline is None and not ext.autocal_enabled + + def test_arming_builds_pipeline_with_baseline(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.10", "FordHighSpeedFactor_ang": "0.95"}) + ext.update_angle_params(p) + assert ext.autocal_enabled and ext.autocal_ctl.pipeline is not None + assert ext.autocal_ctl._last_written == (1.10, 0.95) + + def test_arming_restores_serialized_evidence(self): + donor = AutoCalPipeline(PLATFORM_GAIN_HIGH) + feed_plant(donor.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + state = json.dumps({"v": 1, "phase": "collecting", "pipe": donor.to_dict()}) + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": state, + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + assert ext.autocal_ctl.pipeline is not None + assert ext.autocal_ctl.pipeline.est.n == donor.est.n + assert ext.autocal_ctl.pipeline.est.solve() == donor.est.solve() + + def test_locked_json_never_arms(self): + ext = self._ext() + state = json.dumps({"v": 1, "phase": "locked", "pipe": {}}) + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": state}) + ext.update_angle_params(p) + assert ext.autocal_ctl.pipeline is None and ext.autocal_ctl.done and not ext.autocal_enabled + + def test_legacy_done_state_never_arms(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "done low=1.02 high=1.15"}) + ext.update_angle_params(p) + assert ext.autocal_ctl.pipeline is None and ext.autocal_ctl.done and not ext.autocal_enabled + + def test_garbage_state_starts_fresh(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "round 3 collecting; applied"}) + ext.update_angle_params(p) + assert ext.autocal_ctl.pipeline is not None and ext.autocal_ctl.pipeline.est.n == 0 + + def test_nudge_writes_params_and_state(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + assert ext.autocal_ctl._apply_nudge((1.02, 1.15)) + # The fork's params are typed FLOAT — a string write raises and the nudge dies. + assert p.written["FordLowSpeedFactor_ang"] == 1.02 and isinstance(p.written["FordLowSpeedFactor_ang"], float) + assert p.written["FordHighSpeedFactor_ang"] == 1.15 and isinstance(p.written["FordHighSpeedFactor_ang"], float) + st = json.loads(p.written["FordAngleAutoCalState"]) + assert st["phase"] == "collecting" and st["applied"] == {"low": 1.02, "high": 1.15} + # The blocking write landed, so it must NOT read back as a user edit. Point the strategy + # at the written values (the single reader) and tick: no soft-reset, evidence untouched. + p.values["FordLowSpeedFactor_ang"] = 1.02 + p.values["FordHighSpeedFactor_ang"] = 1.15 + n0 = ext.autocal_ctl.pipeline.est.n + self._tick(ext, p, n=2) + assert ext.autocal_ctl.pipeline is not None + assert ext.autocal_ctl.pipeline.est.n == n0 # user_edit() not triggered + + def test_user_edit_adopted_single_tick(self): + # Blocking nudge writes mean any param/last_written mismatch is a real driver edit — + # detected and adopted on ONE tick, no async-lag debounce. + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + feed_plant(ext.autocal_ctl.pipeline.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + w0 = ext.autocal_ctl.pipeline.est.s_w + p.values["FordLowSpeedFactor_ang"] = "1.08" # driver taps + in the menu + self._tick(ext, p, n=1) + assert abs(ext.autocal_ctl.pipeline.est.s_w - 0.5 * w0) < 1e-9 # soft reset, not a wipe + assert ext.autocal_ctl._last_written == (1.08, 1.00) + + def test_save_restore_round_trip_through_param(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + feed_plant(ext.autocal_ctl.pipeline.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + sol = ext.autocal_ctl.pipeline.est.solve() + ext.autocal_ctl._save("collecting", (1.00, 1.00)) + # New process, same params: evidence must come back. + ext2 = self._ext() + ext2.update_angle_params(p) + assert ext2.autocal_ctl.pipeline is not None + assert ext2.autocal_ctl.pipeline.est.solve() == sol + + def test_lock_off_resumes_a_locked_calibration(self): + # A finished (locked) calibration + FordAngleAutoCalLock=0: the lock is treated as + # "resume from this evidence" — the controller arms, restores, and un-locks. + donor = AutoCalPipeline(PLATFORM_GAIN_HIGH) + feed_plant(donor.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + donor.locked = True + state = json.dumps({"v": 1, "phase": "locked", "pipe": donor.to_dict()}) + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": state, + "FordAngleAutoCalLock": 0, + "FordLowSpeedFactor_ang": "1.05", "FordHighSpeedFactor_ang": "1.05"}) + ext.update_angle_params(p) + ctl = ext.autocal_ctl + assert ctl.enabled and ctl.pipeline is not None and not ctl.done + assert not ctl.pipeline.locked and not ctl.pipeline.lock_enabled + assert ctl.pipeline.est.n == donor.est.n # evidence carried over, nothing lost + # Flipping the lock back ON mid-run re-enables freezing (but doesn't instantly lock). + p.values["FordAngleAutoCalLock"] = 1 + self._tick(ext, p, n=1) + assert ctl.pipeline is not None and ctl.pipeline.lock_enabled and not ctl.pipeline.locked + + def test_reset_param_erases_everything(self): + # The "erase calibration memory" button: evidence, error log, the LOCK, and the + # factors themselves all go back to neutral — a finished calibration can be retried. + ext = self._ext() + state = json.dumps({"v": 1, "phase": "locked", "pipe": {}}) + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": state, + "FordAngleAutoCalReset": 1, + "FordLowSpeedFactor_ang": "1.12", "FordHighSpeedFactor_ang": "1.20"}) + ext.update_angle_params(p) # first ~1 Hz tick consumes the reset + assert p.values["FordAngleAutoCalReset"] is False + assert p.values["FordAngleAutoCalState"] == "" and p.values["FordAngleAutoCalError"] == "" + assert p.written["FordLowSpeedFactor_ang"] == 1.0 and p.written["FordHighSpeedFactor_ang"] == 1.0 + assert ext.autocal_ctl.status == "reset" and not ext.autocal_ctl.done + # Next tick arms a FRESH collection despite the previously locked state. + self._tick(ext, p, n=1) + assert ext.autocal_ctl.pipeline is not None and ext.autocal_ctl.pipeline.est.n == 0 + assert ext.autocal_ctl._last_written == (1.0, 1.0) # the wipe is not a "user edit" + assert ext.low_speed_curv_factor == 1.0 and ext.high_speed_curv_factor == 1.0 + + def test_status_is_json_when_armed(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + self._tick(ext, p, n=1) + st = json.loads(ext.autocal_ctl.status) # dashboards parse this + assert st["low"]["ph"] == "collect" and st["high"]["f"] == 1.0 + assert st["low"]["need"] == NUDGE_MIN_WEIGHT + + def test_toggle_off_disarms(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + assert ext.autocal_ctl.pipeline is not None + p.values["FordAngleAutoCal"] = 0 + self._tick(ext, p, n=1) + assert ext.autocal_ctl.pipeline is None and not ext.autocal_enabled diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py index a748a6f97c..917b13de9f 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py @@ -123,3 +123,42 @@ def apply_bp_device_mount(car_docs, CP): else: car_docs.car_parts = CarParts([Device.threex, harness]) + + +# --------------------------------------------------------------------------------------- +# Angle-mode gain model — owned by the strategy (lateral_angle_ext), consumed by the +# auto-calibrator (angle_autocal) and the offline analyzer. Single source of truth here +# so the strategy never imports its own gain model back out of the calibrator. +# +# Hard-coded per-platform gain defaults (not user-tunable): +GAIN_CAN = (1.00, 1.15) # CAN vehicles (Escape MK4, Bronco Sport, Explorer, Maverick, Edge) +GAIN_CANFD_BOF = (0.95, 0.95) # CAN-FD body-on-frame trucks (F-150, Lightning, Expedition, Ranger) +GAIN_CANFD_SUV = (1.00, 1.05) # CAN-FD unibody SUVs (Mustang Mach-E, Escape MK4.5) + +CANFD_BOF_CARS = frozenset({ + CAR.FORD_F_150_MK14, + CAR.FORD_F_150_LIGHTNING_MK1, + CAR.FORD_EXPEDITION_MK4, + CAR.FORD_RANGER_MK2, +}) +CANFD_SUV_CARS = frozenset({ + CAR.FORD_MUSTANG_MACH_E_MK1, + CAR.FORD_ESCAPE_MK4_5, +}) + + +def platform_gains(fingerprint: str) -> tuple[float, float]: + """(lowC_highV, highC_highV) platform gain pair for a car fingerprint.""" + if fingerprint in CANFD_BOF_CARS: + return GAIN_CANFD_BOF + if fingerprint in CANFD_SUV_CARS: + return GAIN_CANFD_SUV + return GAIN_CAN + + +# Speed anchors of the strategy's gain interpolation (m/s: ~30 mph and ~60 mph), and the +# fixed multiplier on the low-speed anchor. The auto-calibrator's fit is expressed +# against these — if the strategy's interp changes, they must move together. +V_LOW = 13.5 +V_HIGH = 26.82 +LOW_ANCHOR_BASE = 1.30 diff --git a/selfdrive/ui/bp/layouts/settings/bluepilot.py b/selfdrive/ui/bp/layouts/settings/bluepilot.py index 03f1e2ecfc..79d4015aaa 100644 --- a/selfdrive/ui/bp/layouts/settings/bluepilot.py +++ b/selfdrive/ui/bp/layouts/settings/bluepilot.py @@ -78,6 +78,8 @@ def __init__(self): self._refresh_toggles = ( ("send_hands_free_cluster_msg", self._show_hands_free_ui), ("FordPrefSteerAngleCurvature", self._steer_angle_curvature), + ("FordAngleAutoCal", self._angle_autocal), + ("FordAngleAutoCalLock", self._angle_autocal_lock), ("BPDisableLaneLineStatusColor", self._disable_lane_line_status_color), ("BPHideCameraView", self._hide_camera_view), ("BPRadRacerTheme", self._rad_racer_theme), @@ -532,6 +534,38 @@ def _initialize_items(self): step=0.01, icon="chffr_wheel.png" ) + # BluePilot: one-time auto-calibration of the two factors above. Compares requested vs + # actual turn in steady engaged curves and writes the corrected factors once, then locks. + self._angle_autocal = toggle_item( + lambda: tr("Auto-Calibrate Adjustment Factors"), + lambda: tr("Learns the low/high speed factors automatically by comparing requested and actual " + "turn in steady engaged curves, then locks them (one-time, per car). Drive normally " + "with lateral engaged; curves at city and highway speeds both needed. Toggle off and " + "back on to recalibrate."), + initial_state=self._safe_get_bool(self._params, "FordAngleAutoCal"), + callback=self._toggle_angle_autocal, + icon="chffr_wheel.png" + ) + # BluePilot: lock behavior for the calibration above. Off = never freeze, keep + # adapting; flipping it off on a locked car resumes from the saved evidence. + self._angle_autocal_lock = toggle_item( + lambda: tr("Calibration Lock"), + lambda: tr("On (default): auto-calibration freezes once the factors have been stable for " + "5 minutes of driving. Off: it never locks and keeps adapting continuously — " + "turning this off on an already-locked car resumes calibration from its saved " + "evidence without losing anything."), + initial_state=self._safe_get_bool(self._params, "FordAngleAutoCalLock", default=True), + callback=lambda state: self._toggle_callback(state, "FordAngleAutoCalLock"), + icon="chffr_wheel.png" + ) + # BluePilot: full calibration do-over — evidence, error log and the factors themselves. + self._angle_autocal_erase = button_item( + lambda: tr("Erase Calibration Memory"), + lambda: tr("ERASE"), + lambda: tr("Wipes all collected calibration evidence, clears any lock, and puts both " + "adjustment factors back to 1.00 for a clean retry. Works offroad or mid-drive."), + callback=self._erase_angle_autocal + ) self._high_speed_dampening = float_control_item( lambda: tr("High Speed Low Curve Adjustment Factor"), lambda: tr("Tune adjustment factor for low curve straightaways (highways) at high speeds. If oversteering, reduce. If understeering, increase"), @@ -624,6 +658,9 @@ def _section(title: str, items: list) -> list: self._low_speed_curv_factor, self._high_speed_curv_factor, self._high_speed_dampening, + self._angle_autocal, + self._angle_autocal_lock, + self._angle_autocal_erase, self._lane_change_factor_high_ang, ] angle_header = CollapsibleSectionHeader(tr("Angle Tuning")) @@ -872,6 +909,9 @@ def _update_toggles(self, just_toggled: dict | None = None): self._low_speed_curv_factor.action_item.set_enabled(is_angle) self._high_speed_curv_factor.action_item.set_enabled(is_angle) self._high_speed_dampening.action_item.set_enabled(is_angle) + self._angle_autocal.action_item.set_enabled(is_angle) + self._angle_autocal_lock.action_item.set_enabled(is_angle) + self._angle_autocal_erase.action_item.set_enabled(is_angle) self._lane_change_factor_high_ang.action_item.set_enabled(is_angle) # Curvature-mode items: always visible (Curvature Tuning section), greyed out when angle mode is active self._lane_change_factor_high_curv.action_item.set_enabled(is_curv) @@ -1015,6 +1055,29 @@ def _set_overlay_size(self, button_index: int): """Handle overlay size button selection.""" self._params.put("FordPrefRadarOverlaySize", button_index) + def _toggle_angle_autocal(self, state: bool): + """Arm/disarm the one-time factor auto-calibration; disarming clears a finished + calibration's lock so re-enabling starts a fresh collection.""" + self._toggle_callback(state, "FordAngleAutoCal") + if not state: + try: + self._params.put("FordAngleAutoCalState", "") + except UnknownKeyName: + pass + + def _erase_angle_autocal(self): + """Erase calibration memory: evidence, error log, any lock, and the factors back to + 1.00. The params are cleared here for immediate offroad visibility; the onroad + controller consumes FordAngleAutoCalReset so a mid-drive erase lands within a second.""" + try: + self._params.put_bool("FordAngleAutoCalReset", True) + self._params.put("FordAngleAutoCalState", "") + self._params.put("FordAngleAutoCalError", "") + self._params.put("FordLowSpeedFactor_ang", 1.0) + self._params.put("FordHighSpeedFactor_ang", 1.0) + except UnknownKeyName: + pass + def _set_wheel_icon_style(self, button_index: int): """Handle wheel icon style: 0 = comma 4, 1 = comma 3X.""" self._params.put("BPSteeringWheelIconStyle", button_index) diff --git a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py index de74296508..c6b0b15901 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py +++ b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py @@ -2,13 +2,32 @@ from collections.abc import Callable -from openpilot.selfdrive.ui.bp.mici.widgets.button_bp import BigParamControlBP +from openpilot.selfdrive.ui.bp.mici.widgets.button_bp import BigButtonBP, BigParamControlBP from openpilot.selfdrive.ui.bp.mici.widgets.floatbutton import BigParamFloatControl, BigParamIntControl from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.widgets.scroller import NavScroller from opendbc.sunnypilot.car.ford.lateral_curv_ext import PrimaryLateralControl +class _EraseAutoCalButton(BigButtonBP): + """One-tap 'erase calibration memory': evidence, the error log and the factors + themselves go back to neutral so a calibration run can simply be retried. The params + are cleared here for immediate offroad visibility (the factor steppers show 1.00 on + next refresh); the onroad controller consumes FordAngleAutoCalReset to drop its + in-memory pipeline too, so a mid-drive erase takes effect within a second.""" + + def __init__(self): + super().__init__("Erase Calibration Memory") + + def _handle_mouse_release(self, mouse_pos): + super()._handle_mouse_release(mouse_pos) + ui_state.params.put_bool("FordAngleAutoCalReset", True) + ui_state.params.put("FordAngleAutoCalState", "") + ui_state.params.put("FordAngleAutoCalError", "") + ui_state.params.put("FordLowSpeedFactor_ang", 1.0) + ui_state.params.put("FordHighSpeedFactor_ang", 1.0) + + class LateralLayoutMici(NavScroller): def __init__(self, back_callback: Callable[[], None] | None = None): super().__init__() @@ -25,6 +44,20 @@ def __init__(self, back_callback: Callable[[], None] | None = None): self.high_speed_dampening = BigParamFloatControl( "High Speed Low Curve Adjustment Factor", "FordHighSpeedDampening_ang", min=0.75, max=1.25, step=0.01, ) + # One-time auto-calibration of the two factors above; toggling off clears the lock + # so re-enabling starts a fresh collection. + self.angle_autocal = BigParamControlBP( + "Auto-Calibrate Factors", "FordAngleAutoCal", + toggle_callback=self._on_autocal_toggled, + ) + # Full retry: wipes evidence AND puts both factors back to 1.00 (the toggle above + # only clears the lock; it leaves the factors wherever the calibrator walked them). + self.angle_autocal_erase = _EraseAutoCalButton() + # On (default): calibration freezes once stable. Off: never locks — keeps adapting; + # turning it off on an already-locked car resumes from the saved evidence. + self.angle_autocal_lock = BigParamControlBP( + "Calibration Lock", "FordAngleAutoCalLock", + ) self.lane_change_factor_high_ang = BigParamFloatControl( "Lane Change Factor High", "lane_change_factor_high_ang", min=0.85, max=1.50, ) @@ -73,6 +106,9 @@ def __init__(self, back_callback: Callable[[], None] | None = None): self.low_speed_factor, self.high_speed_factor, self.high_speed_dampening, + self.angle_autocal, + self.angle_autocal_lock, + self.angle_autocal_erase, self.lane_change_factor_high_ang, self.disable_lane_change_under_speed, self.blinker_min_speed, @@ -90,6 +126,8 @@ def __init__(self, back_callback: Callable[[], None] | None = None): ]) self._refresh_toggles = ( + ("FordAngleAutoCal", self.angle_autocal), + ("FordAngleAutoCalLock", self.angle_autocal_lock), ("disable_BP_lat_UI", self.disable_BP_lat), ("BlinkerPauseLaneChange", self.disable_lane_change_under_speed), ("enable_human_turn_detection_curv", self.enable_human_turn_detection), @@ -101,6 +139,11 @@ def __init__(self, back_callback: Callable[[], None] | None = None): ui_state.add_offroad_transition_callback(self._update_toggles) + def _on_autocal_toggled(self, state: bool): + """Disarming clears a finished calibration's lock so re-enabling starts fresh.""" + if not state: + ui_state.params.put("FordAngleAutoCalState", "") + def show_event(self): super().show_event() self._update_toggles() @@ -115,6 +158,9 @@ def _update_toggles(self): self.low_speed_factor.set_visible(is_angle) self.high_speed_factor.set_visible(is_angle) self.high_speed_dampening.set_visible(is_angle) + self.angle_autocal.set_visible(is_angle) + self.angle_autocal_lock.set_visible(is_angle) + self.angle_autocal_erase.set_visible(is_angle) self.lane_change_factor_high_ang.set_visible(is_angle) self.blinker_min_speed.set_enabled(ui_state.params.get_bool("BlinkerPauseLaneChange")) for item in ( diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index d3a25d7887..a4867f67cc 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -2582,6 +2582,19 @@ } ] }, + { + "key": "FordAngleAutoCal", + "widget": "toggle", + "title": "[Lateral Tuning] Auto-Calibrate Adjustment Factors", + "description": "Learns the low/high speed factors by comparing requested and actual turn, nudging them live until locked. Manual +/- always wins. Toggle off and back on to recalibrate.", + "visibility": [ + { + "type": "param", + "key": "FordPrefLateralControl", + "equals": 1 + } + ] + }, { "key": "lane_change_factor_high_ang", "widget": "option", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 1c0134c41e..c40151547b 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -319,6 +319,14 @@ sections: - type: param key: FordPrefLateralControl equals: 1 + - key: FordAngleAutoCal + widget: toggle + title: '[Lateral Tuning] Auto-Calibrate Adjustment Factors' + description: Learns the low/high speed factors by comparing requested and actual turn, nudging them live until locked. Manual +/- always wins. Toggle off and back on to recalibrate. + visibility: + - type: param + key: FordPrefLateralControl + equals: 1 - key: lane_change_factor_high_ang widget: option title: '[Lateral Tuning] Lane Change Factor High (Angle)'