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)'