diff --git a/bluepilot/selfdrive/car/bp_card_publisher.py b/bluepilot/selfdrive/car/bp_card_publisher.py index cde85839d9..7c135062bb 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: settings snapshot -- refreshed at most every _SETTINGS_INTERVAL s so Params # reads don't add latency to every card.py tick. @@ -145,6 +149,17 @@ def publish_controller_state_bp(CI, pm): for field, value in _settings_cache.items(): setattr(cs_bp, field, value) + # BluePilot: auto-cal fields are GROUND TRUTH from the live controller, not the param + # snapshot — a device once had params armed while the controller ran disarmed, and the + # param-sourced telemetry made that undiagnosable from logs. bp_autocal_status carries + # the controller's own view (armed/evidence/nudges, "off", "locked", or an error). + cc = CI.CC + if hasattr(cc, "autocal_enabled"): + cs_bp.bmsAngleAutoCalibrate = bool(cc.autocal_enabled) + status = getattr(cc, "bp_autocal_status", "") + if status: + cs_bp.bmsAngleAutoCalState = str(status) + # BluePilot: fingerprint info -- plain attribute reads on CarParams, no Params round-trip # needed, so no caching required (fingerprint never changes after startup). CP = getattr(CI, "CP", None) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index d02bcbedf3..489c657cde 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -548,6 +548,9 @@ struct ControllerStateBP @0xcd96dafb67a082d0 { # --- Fingerprint (not a menu item, but requested alongside the settings snapshot) --- bmsFingerprintForced @52 :Bool; # true when CarParams.fingerprintSource == fixed (CarPlatformBundle / FINGERPRINT env) bmsFingerprint @53 :Text; # CarParams.carFingerprint + bmsAngleAutoCalibrate @54 :Bool; # FordAngleAutoCal (one-time speed-factor auto-calibration) + bmsAngleAutoCalState @55 :Text; # FordAngleAutoCalState ("" collecting, "done ..." locked) + angleSaturated @56 :Bool; # angle mode: PSCM authority limit or DBC clamp modified this frame's command } struct CarStateBP @0xb057204d7deadf3f { diff --git a/common/params_keys.h b/common/params_keys.h index e183f1180d..7f9f3b150b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -309,6 +309,11 @@ inline static std::unordered_map keys = { {"LC_PID_gain_UI_curv", {PERSISTENT | BACKUP, FLOAT, "3.0"}}, {"FordLowSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, {"FordHighSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, + {"FordAngleAutoCal", {PERSISTENT | BACKUP, BOOL, "0"}}, // one-time auto-calibration of the angle speed factors + {"FordAngleAutoCalState", {PERSISTENT | BACKUP, STRING, ""}}, // "" = collecting; JSON = evidence; "locked"/"done ..." = finished + {"FordAngleAutoCalError", {PERSISTENT, STRING, ""}}, // diagnostics only — separate channel so an error can never clobber evidence + {"FordAngleSmoothing", {PERSISTENT | BACKUP, BOOL, "1"}}, // anti-weave smoothing of the angle command path + {"FordAngleSmoothStrength", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // 0.0=minimal .. 1.0=tuned default .. 1.5=strong {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/docs/ford-angle-autocal.md b/docs/ford-angle-autocal.md new file mode 100644 index 0000000000..1cc2532996 --- /dev/null +++ b/docs/ford-angle-autocal.md @@ -0,0 +1,130 @@ +# Ford Angle-Mode Auto-Calibration — User Guide + +BluePilot can tune your car's two angle-mode adjustment factors for you, while you +drive, using exactly the comparison you'd do by hand — and stop when it's done. + +--- + +## What it does (and why you'd want it) + +On Fords running **angle mode**, BluePilot sends the car a target steering angle and the +car's power-steering computer (the PSCM) turns the wheel. That conversion isn't perfectly +1:1, and it drifts from car to car with tires, alignment, and platform. Two menu values +correct for it: + +- **Low Speed Adjustment Factor** (`FordLowSpeedFactor_ang`) +- **High Speed Adjustment Factor** (`FordHighSpeedFactor_ang`) + +The manual tuning method is: drive, plot requested vs. actual turn, compare the tops and +bottoms of the two curves, tap +/- until the peaks line up, repeat. It works, but it's +per-car, slow, and easy to get subtly wrong. + +**Auto-calibration automates that exact loop.** It watches requested vs. actual curvature +in real time, collects evidence only from clean cornering, and nudges the same two menu +values you would have tapped — in small steps, with statistical error bars instead of an +eyeball. When there's nothing left to adjust, it **locks** and stops touching anything. + +## Requirements + +- A Ford running BluePilot with **Lateral Control set to Angle** (the toggle is greyed + out in curvature mode). +- Nothing else. It's **off by default** and changes nothing until you turn it on. + +## Turning it on + +**comma 3X:** Settings → BluePilot → Lateral Tuning → **Auto-Calibrate Adjustment Factors** + +**comma four:** Lateral menu → **Auto-Calibrate Factors** + +**Sunnylink:** [Lateral Tuning] Auto-Calibrate Adjustment Factors + +Then just drive normally with lateral engaged. + +## What you'll see + +Open the Lateral Tuning menu during or after a drive: the low/high factor values **move on +their own**, at most 0.02 at a time. That's it working. There's no ceremony — the +calibrator uses the same values the +/- buttons use, so the menu is always the truth. + +What it's doing underneath: + +- Evidence comes from **steady engaged curves** and from **curve apexes** (the "tops and + bottoms of the graphs"), so winding roads count even when nothing is steady. +- Everything suspicious is thrown away: potholes and bump-flicks, rough washboard + surfaces, hard braking/accelerating, tire-limit cornering, any moment your hands are on + the wheel (plus a cooldown after), and crowned/banked roads that push all the evidence + to one side. +- Evidence **survives ignition cycles** — progress is saved every 30 seconds and picked + up on the next drive. + +## How long does it take? + +Honest answer from real drives: **roughly an hour of mixed driving**, but it depends +heavily on the roads. + +- **Highway curves calibrate the high factor quickly** — sweeping interstate curves are + ideal evidence and pile up fast. +- **The low factor is slower on purpose.** City cornering is exactly where hands, bumps, + and sharp maneuvers contaminate the data, so most of it is rejected. Gentle 25–45 mph + curvy roads with hands off are what it wants. +- The first few minutes of every drive contribute nothing — the calibrator waits for the + car's own sensor-calibration stack to warm up before trusting any measurement. + +You don't have to do anything special. It gets there on normal driving; special trips +just get there sooner. + +## Your +/- buttons still win + +Tap +/- any time, calibrating or not. Your value is adopted immediately and the +calibrator treats it as a strong hint — it softens its accumulated evidence rather than +fighting you. It will only move the value again if fresh driving data genuinely disagrees. + +## Locking + +When both factors have solid evidence behind them and the applied values have sat within +0.03 of the statistical target for 5 minutes of driving, the calibration **locks**: + +- The factors stop changing. Permanently, for this car. +- The toggle stays on but does nothing further. + +**To recalibrate** (new tires, alignment work, seasonal tire swap, or you just want a +fresh pass): toggle it **off and back on**. That clears everything and starts a clean +collection. + +## What it will never do + +- It never moves a factor more than **0.02 per step**, **0.10 per drive** for the high + factor and **0.04 per drive** for the low factor — one drive can't transform how your + car steers. +- It never acts on thin data: each factor needs sustained clean evidence and a tight + error bar before its first nudge. +- It never runs in curvature mode, never runs while locked, and never runs before the + measurement stack is warmed up. +- Turning the toggle off stops it instantly and clears its state. + +## Troubleshooting + +| Symptom | Likely reason | +|---|---| +| Factors never move | Normal for the first drives — evidence takes time, and city-heavy driving is mostly rejected by design. Check you're in angle mode and the toggle is on. Highway curves speed things up. | +| Factors moved, then stopped | It probably **locked** — that's success. Toggle off/on if you want a re-run. | +| Low factor barely changes while high converged | Expected — see "How long does it take?". Gentle mid-speed curves with hands off are the low anchor's food. | +| A value looks wrong after calibration | Tap +/- to your preferred value; the calibrator adopts it. If it drifts back, the data disagrees with you — try a re-run after checking tire pressures/alignment. | +| Suspected fault | The calibrator writes any internal error to the `FordAngleAutoCalError` param (visible in logs) instead of failing silently — include it when reporting. | + +## For the curious + +The estimator is pure math shared byte-for-byte with an offline analyzer. If you upload +your drives, anyone can replay exactly what the car's calibrator saw — every accepted +sample, every rejection and its reason, and the nudge-by-nudge timeline: + +``` +python bp/angle_autocal_analyze.py +``` + +(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/docs/ford-angle-smoothing.md b/docs/ford-angle-smoothing.md new file mode 100644 index 0000000000..63f3c9c98c --- /dev/null +++ b/docs/ford-angle-smoothing.md @@ -0,0 +1,111 @@ +# Ford Angle-Mode Anti-Weave Smoothing — User Guide + +BluePilot can damp the slow left-right "rhythmic centering" weave some Fords show in +angle mode on straight roads — with a strength dial you control, and a guarantee that +the neutral setting is exactly stock steering. + +--- + +## What it does (and why you'd want it) + +Some angle-mode Fords develop a gentle, rhythmic side-to-side motion on straights — the +car works the wheel every few seconds even though the lane is dead straight. Log analysis +traced it to a feedback loop: tiny curvature noise crosses internal thresholds, those +crossings modulate the steering command, and the car's power-steering computer integrates +the result into motion you can feel. + +Smoothing breaks that loop at its sources: it filters the noise that feeds the thresholds +and holds the steering command steady when changes are smaller than what the car can even +represent on the wire. It does **not** slow the steering down — all shaping is on the +input side, and curve entry is deliberately left fast at every strength. + +Measured on the same car, straight-road driving, with and without: + +| | Without | With (menu 1.8) | +|---|---|---| +| Slow lane sway (the weave) | ±0.46 m | **±0.26 m (−43%)** | +| Fast wheel-working dither | ±0.033 m | ±0.022 m (−33%) | + +Curve entry and exit were checked specifically: entries are unaffected by design, and +exits measured *cleaner* with smoothing on. + +## The strength dial + +**Settings → BluePilot → Lateral Tuning → Smooth Steering (Anti-Weave)** — a master +toggle plus a **Smoothing Strength** stepper. Also on the comma four lateral menu and in +Sunnylink. + +The scale is deliberately simple: + +- **1.0 — stock.** Not "a little smoothing": *bit-for-bit identical* to the feature not + existing. This is verified by an automated test on every change. +- **1.1 – 1.9** — increasing damping. +- **2.0 — the tuned setting.** Chosen on logged drives; this is where the numbers in the + table above come from (measured at 1.8, tuned default 2.0). +- **2.5 — maximum.** More damping, with a measurable cost (below). For cars that still + weave at 2.0. + +The toggle defaults ON with strength 1.0 — which means **stock behavior until you step +the strength up**. Damping is always your explicit choice. + +## What it costs + +Nothing is free in a control loop. The closed-loop simulator puts numbers on it: + +| Setting | Lane-keeping tightness (std) | +|---|---| +| 1.0 / off | 0.039 m | +| 2.0 | 0.042 m (+3 mm) | +| 2.5 | 0.047 m (+8 mm) | + +At the tuned setting you trade **three millimeters** of station-keeping for roughly half +the weave. At 2.5 the cost triples for diminishing extra damping — try 2.0 first. + +## How to find your setting + +1. Leave strength at 1.0 for a drive or two so you know your baseline. +2. If you feel the slow weave on straights, step to **2.0** and drive the same roads. +3. Still feel it? Step toward 2.5 one notch at a time. +4. If the car ever feels *lazier* than you like, step back down — every value between + 1.0 and your current setting is a valid operating point. + +Changes take effect within a second (no reboot), and stepping strength mid-drive is safe: +the filters are built to pick up from the live steering state, never from stale values. + +## What it will never do + +- **1.0 is stock, provably.** The passthrough is tested for bit-identity, not "close". +- It never adds lag on top of the steering output — that specific design was tested in a + closed-loop simulator, measured to *hurt* lane-keeping 2.5x, and rejected. Input-side + shaping only. +- Curve entry speed is independent of strength: the entry filter is fixed-fast, and an + automated test pins it. +- All of it disengages instantly with lateral control, and every filter resets across + takeovers — no state survives a disengagement, a driver override, or a steering pause. + +## Troubleshooting + +| Symptom | Likely reason | +|---|---| +| No difference at 1.0 | Correct — 1.0 *is* stock. Step up to feel the feature. | +| Still weaving at 2.0 | Step toward 2.5. If maxed and still weaving, report it with a route ID — your car may need the underlying factors calibrated first (see the auto-calibration guide). | +| Feels slow into curves | Not a smoothing effect at any strength (entry is fixed-fast) — check your speed-factor calibration instead. | +| Wandering within the lane | Distinguish: the weave is *rhythmic* (a steady few-second cycle); random wander is usually crosswind, crown, or camera calibration. Smoothing targets the rhythm. | + +## Relationship to auto-calibration + +They're complementary and independent. Auto-calibration fixes the *average* correction +(how much turn you get per command); smoothing fixes the *oscillation* around it. A car +with badly-off factors can weave for that reason alone — calibrate first, then judge how +much smoothing you still want. + +## For the curious + +The smoothing math lives in one pure, unit-tested module +(`opendbc/sunnypilot/car/ford/angle_smoothing.py`) with five elements: a hysteresis on +the curve-entry decision, a low-pass on the model's predicted curvature, a slew on the +exit blend, an asymmetric filter on the gain schedule (the primary fix — fast attack, +strength-scaled release), and a one-LSB hold on the outgoing wire value. The measurement +tooling — spectral weave analysis of any logged drive, and the closed-loop simulator used +to bound the costs above — lives in [bp-tools](https://github.com/ghbarker/bp-tools) +(`bp/angle_weave_analyze.py`, `sim/closed_loop_weave.py`). diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 1094e52741..fddfcf6f5f 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..8b63dcc64a --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -0,0 +1,794 @@ +"""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 + +# 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 +MAX_KAPPA_RATE = 0.0015 # 1/m/s; quasi-steady curvature only +STEADY_TIME_S = 0.6 # command must be steady this long before samples count (PSCM lag) +# The measurement lags the command by the actuation delay (liveDelay: ~0.15s typical, up to +# ~0.42s observed), so on a slow ramp a same-frame ratio compares meas(t) ~ cmd(t - tau) +# against cmd(t). The per-frame rate bound alone admits ramps whose lag error reaches +# tau*MAX_KAPPA_RATE/kappa — tens of percent at the MIN_KAPPA floor. Bounding the TOTAL +# drift across the steady window caps that error at DRIFT_FRAC * (tau / STEADY_TIME_S) +# regardless of the actual delay: <= ~7% instantaneous even at the 0.42s extreme, sign- +# symmetric over entries/exits, well inside the stderr machinery. +STEADY_DRIFT_FRAC = 0.10 # 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 +NUDGE_DEADBAND = 0.015 # |target - applied| below this: leave it alone +NUDGE_STEP = 0.02 # max factor change per nudge (menu granularity is 0.01) +MAX_DRIVE_DELTA = 0.10 # high-factor cumulative cap per drive (card process lifetime) +# The low anchor accumulates evidence far slower than the high one (city curve frames are +# mostly grip/accel-rejected), so each sample moves it more. On-road 2026-07-22 the low +# factor round-tripped 0.98->1.06->1.00 inside one drive on ~70s of evidence and the +# +-6% curve-branch gain swing (x1.30 branch) was felt as turn-in overshoot. Half the cap +# bounds any single drive's wander to two steps while still allowing full convergence +# (0.96->1.00 fit within it). +MAX_DRIVE_DELTA_LOW = 0.04 # low-factor cumulative cap per drive + +# --- 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) + + +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)} + + 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 + acc = self.lr[(0 if a < 0.5 else 1, 0 if kappa_cmd > 0 else 1)] + acc[0] += w + acc[1] += w * y + 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.""" + f = math.exp(-float(seconds) / TAU_EVIDENCE_S) + self.scale(f) + + 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 + + @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)], + } + + 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])] + + +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: + ok = abs(kappa_cmd - self.kappa_last) / self.dt <= MAX_KAPPA_RATE + # 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 + + +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[list] = [] # [age_s, v, kappa_cmd, kappa_meas, applied_gain, weight] + self._meas_last = None + self._decay_accum = 0.0 + # 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.drive_delta_low = 0.0 # per-drive (process lifetime) — not persisted + self.drive_delta_high = 0.0 + self.locked = False + + # -- 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 + + 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 + + # 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_cmd, frame.steering_pressed, + frame.angle_rate_limited, frame.deviation_limited, + saturated=frame.saturated, driver_torque=frame.driver_torque) + 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. Measured curvature is noisier than the command, so the bound is 3x looser. + if eligible and self._meas_last is not None: + eligible = abs(kappa_meas - self._meas_last) / self.dt <= 3.0 * MAX_KAPPA_RATE + self._meas_last = kappa_meas + + # Evidence near the physical limit fades to nothing: there, cmd != meas is physics. + lat_accel = abs(kappa_cmd) * v_ego * v_ego + margin_w = min(1.0, max(0.0, (MAX_LAT_ACCEL - lat_accel) / LAT_ACCEL_SOFT_BAND)) + if eligible and margin_w <= 0.0: + self.quality.counters["limit"] += 1 + eligible = False + + gain_now = self.applied_gain(v_ego, frame.low_factor, frame.high_factor) + + # Age the staging queue; entries that survived the holdback graduate to the estimator. + committed = [] + still_staged = [] + for entry in self._staged: + entry[0] += self.dt + if entry[0] >= PRESS_HOLDBACK_S: + if self.est.add_sample(entry[1], entry[2], entry[3], entry[4], weight=entry[5]): + committed.append((entry[1], entry[2], entry[3])) + else: + still_staged.append(entry) + self._staged = still_staged + + if eligible: + self._staged.append([0.0, v_ego, kappa_cmd, kappa_meas, gain_now, 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 = min(1.0, max(0.0, (MAX_LAT_ACCEL - abs(pk) * pv * pv) / LAT_ACCEL_SOFT_BAND)) + 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 + + sol = self.est.solve() + if sol is not None: + low_t, high_t, st = sol + ready = (st["weight_low"] >= LOCK_MIN_WEIGHT and st["weight_high"] >= LOCK_MIN_WEIGHT + and st["stderr_eff_low"] <= NUDGE_MAX_STDERR and st["stderr_eff_high"] <= NUDGE_MAX_STDERR + 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: + self.locked = True + else: + self.stable_s = 0.0 + + return committed + + 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, bounded steps, per-drive cap. 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(target, applied, weight, stderr_eff, drive_delta, drive_cap): + if weight < NUDGE_MIN_WEIGHT or stderr_eff > NUDGE_MAX_STDERR: + return None + err = target - applied + if abs(err) <= NUDGE_DEADBAND: + return None + s = max(-NUDGE_STEP, min(NUDGE_STEP, err)) + if abs(drive_delta + s) > drive_cap: + return None # enough movement for one drive — pick it up next drive + new = round(max(FACTOR_MIN, min(FACTOR_MAX, applied + s)), 2) + return new if abs(new - applied) >= 0.005 else None + + new_low = step(low_t, low_factor, st["weight_low"], st["stderr_eff_low"], + self.drive_delta_low, MAX_DRIVE_DELTA_LOW) + new_high = step(high_t, high_factor, st["weight_high"], st["stderr_eff_high"], + self.drive_delta_high, MAX_DRIVE_DELTA) + if new_low is None and new_high is None: + return None + 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) + if new_low is not None: + self.drive_delta_low += out_low - low_factor + if new_high is not None: + self.drive_delta_high += out_high - high_factor + 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 + + # -- 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), + } + + 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)) 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..0fed735e3c --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py @@ -0,0 +1,220 @@ +"""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: the params handle, arm/disarm from the +FordAngleAutoCal toggle, evidence (de)serialization to FordAngleAutoCalState with +its save cadence, user-edit debounce, nudge application to the factor params, +error reporting to FordAngleAutoCalError, and the ground-truth telemetry status +string. FordLateralAngleExt holds one instance and calls exactly three things: +poll_params() at its ~1 Hz param cadence, feed() once per 20 Hz lateral frame, +and idle() on frames where lateral is inactive. + +The controller never touches the strategy's in-memory factors — feed() returns a +nudged (low, high) pair for the strategy to adopt, keeping the write path to the +live steering values in exactly one place (the caller). +""" +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) floats the nudger last wrote; edits differ + self._edit_pending = False # user edit needs 2 consecutive ticks (async put lag) + 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: + enabled = bool(params.get_bool("FordAngleAutoCal")) + 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) + else: + self.done = self.pipeline.locked + self.enabled = enabled and not self.done + if self.enabled and self.pipeline is None: + # Arm: build the pipeline and restore serialized evidence from a prior drive. + # The currently applied factors are the nudge baseline. + self.pipeline = AutoCalPipeline(platform_gain_high, dt=self.dt) + _restore(self.pipeline, state) + self._last_written = (float(low_factor), float(high_factor)) + self._edit_pending = False + elif not self.enabled: + self.pipeline = None + else: + # User-edit detection: the factor params moved without the nudger writing them. + # Confirmed on two consecutive ticks — an async put of our own nudge may not be + # readable yet on the first tick after it. The driver's judgment is adopted + # (values already live in the strategy); evidence is soft-reset, 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: + if self._edit_pending: + self.pipeline.user_edit() + self._last_written = (float(low_factor), float(high_factor)) + self._edit_pending = False + self._dirty = True + else: + self._edit_pending = True + else: + self._edit_pending = False + 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: + est = self.pipeline.est + self.status = (f"armed n={est.n} w={est.weight_low:.0f}/{est.weight_high:.0f}" + f" applied={low_factor:.2f}/{high_factor:.2f}" + f" nudges={self.pipeline.nudges}") + 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. Returns a nudged (low, high) pair the strategy should + adopt, or None. Save cadence and the lock -> disarm transition happen here.""" + if not self.enabled or self.pipeline is None: + return None + if not delay_estimated: + # Measurement-chain warmup: kappa_meas flows through liveParameters from the same + # locationd stack that estimates the actuation delay — until lagd reports + # 'estimated' those inputs are defaults/converging. Idle (not pause): staged + # samples and peak windows must not straddle the unestimated period. + self.idle() + return None + committed = self.pipeline.update(frame) + if committed: + self._dirty = True + applied = (frame.low_factor, frame.high_factor) + out = None + rec = self.pipeline.recommend(frame.low_factor, frame.high_factor) + if rec is not None and self._apply_nudge(rec, applied): + applied = rec + out = 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) + return out + + # -- params I/O -------------------------------------------------------------------------- + def _apply_nudge(self, rec, applied) -> bool: + """Write a nudged factor pair to the params (the lateral tuning menu shows them move). + Returns True when the write landed — only then does the caller adopt the values. + + The params are TYPED (FLOAT) in this fork: writes must be python floats — a string + raises TypeError. That failure mode was invisible once (swallowed except -> nudges + silently never landed); now any write error is recorded in FordAngleAutoCalError so + it shows up in the next drive's logs instead of vanishing.""" + low_new, high_new = rec + if self._params is None: + return False + try: + self._params.put("FordLowSpeedFactor_ang", float(low_new)) + self._params.put("FordHighSpeedFactor_ang", float(high_new)) + 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._edit_pending = False + self._save("collecting", rec) + return True + + def _error(self, msg: str): + """Self-reporting diagnostics: park the error in its OWN param so it is visible in + qlogs/initData without ever touching FordAngleAutoCalState — an error written just + before ignition-off must not be able to replace (and thereby erase) the serialized + evidence from the last good save. Never raises.""" + 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/angle_smoothing.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_smoothing.py new file mode 100644 index 0000000000..688cebc5c7 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_smoothing.py @@ -0,0 +1,154 @@ +"""BluePilot: anti-weave smoothing for the Ford angle-mode command path. + +Pure math, no I/O — the same pattern as angle_autocal.py, so every element is +unit-testable without cereal. FordLateralAngleExt owns one AngleSmoother, feeds it +the toggle/strength from params, and calls one method per smoothing element at the +exact point in the command path where that element lives: + + entering() hysteresis on the curve-entering decision (VLT direction gate) + prediction() low-pass on the model predicted curvature (pre-blend) + blend() slew on the exit-blend ratio (no 4x steps from boolean chatter) + kappa_schedule() asymmetric filter on |kappa_cmd| feeding the gain interp — the + PRIMARY fix: the 0.0007-0.001 interp band sits in straight-road + noise, so unfiltered |kappa| lets the weave modulate its own loop + gain every cycle (0.23 Hz limit cycle measured on route 00000006) + wire() 1-LSB hold on the outgoing path_angle (kills LSB dither) + +Semantics: menu 1.0 = stock (strength 0.0 internally) — every method is an exact +passthrough, bit-identical to the toggle being off. Strength scales the release +time constant, the prediction RC, and the wire-hold band; curve ENTRY behavior is +strength-independent by design (fast-rise RC is fixed). + +Never add output-side lag here: the closed-loop rig measured a curvature-scheduled +output low-pass degrading station-keeping 2.5x (in-loop lag). Input-side shaping + +the wire hold is the whole design. Validate control changes in the closed-loop rig +(bp-tools/sim/closed_loop_weave.py), not just open-loop replay. +""" + +_STEER_DT = 0.05 # 20 Hz lateral cadence (mirrors lateral_angle_ext._STEER_DT) + +GAIN_RC_UP = 0.10 # s — gain-schedule filter, rising |kappa| (preserves curve entry) +GAIN_RC_DOWN = 0.60 # s — falling side (kills the 0.2-0.27 Hz gain modulation) +PRED_RC = 0.12 # s — model predicted-curvature low-pass (inside VLT slack) +ENTER_HYST = 0.0003 # 1/m — hysteresis on the curve-entering decision (above model noise) +BLEND_SLEW = 0.0375 # blend-ratio step per 20 Hz call = 0.75/s +WIRE_HOLD = 0.0005 # rad = 1 LSB of LatCtlPath_An_Actl at strength 1.0 + +MENU_MIN = 1.0 # menu 1.0 = stock, no smoothing (bit-identical to toggle off) +MENU_MAX = 2.5 # strongest damping; internal strength = menu - 1.0 (0..1.5) + + +class AngleSmoother: + """State container + per-element filters. Every method returns its input unchanged + (and keeps its internal state seeded for a clean future enable) whenever smoothing + is inactive — so toggling or stepping strength mid-drive can never produce a + transient from stale state.""" + + def __init__(self, dt: float = _STEER_DT): + self.dt = dt + self.enabled = True # master toggle (param re-read ~1 Hz by the owner) + self.strength = 0.0 # EFFECTIVE scale (menu - 1.0); 0 = stock passthrough + self.reset() + + def configure(self, enabled: bool, menu_value: float): + """From the params poll: menu value is clamped to [MENU_MIN, MENU_MAX].""" + self.enabled = bool(enabled) + menu = min(MENU_MAX, max(MENU_MIN, float(menu_value))) + self.strength = menu - 1.0 + + @property + def active(self) -> bool: + return self.enabled and self.strength > 1e-6 + + def reset(self): + """Command-path discontinuity (disengage / human turn / stall blip): every filter + re-seeds on its next active frame instead of averaging across the gap.""" + self._sched = 0.0 + self._sched_init = False + self._pred = 0.0 + self._pred_init = False + self._b_blend = None + self._entering = False + self._wire = 0.0 + + # -- elements, in command-path order ------------------------------------------------------ + def entering(self, d_enter: float, raw_entering: bool) -> bool: + """Hysteresis on the curve-entering boolean so noise straddling the boundary can't + flip it (and the exit-blend gate with it) frame to frame near zero curvature.""" + if not self.active: + self._entering = raw_entering + return raw_entering + if d_enter > ENTER_HYST: + self._entering = True + elif d_enter < -ENTER_HYST: + self._entering = False + # inside the band: hold the previous decision + return self._entering + + def prediction(self, predicted_curvature: float) -> float: + """Low-pass the model prediction (~50% of the straight-road command) to strip + frame-to-frame model jitter. Equivalent to a ~PRED_RC earlier lookahead — inside + the VLT's own slack, so no curve-entry cost.""" + if not self.active: + self._pred_init = False # a future enable re-seeds from the live value + return predicted_curvature + rc = PRED_RC * self.strength + if not self._pred_init: + self._pred = predicted_curvature + self._pred_init = True + elif rc > 1e-6: + a = self.dt / (rc + self.dt) + self._pred += a * (predicted_curvature - self._pred) + else: + self._pred = predicted_curvature + return float(self._pred) + + def blend(self, b_target: float) -> float: + """Slew the exit-blend ratio instead of stepping it — boolean chatter then produces + bounded 0.75/s ramps in the command mix rather than 4x discontinuities.""" + if not self.active: + self._b_blend = None + return b_target + if self._b_blend is None: + self._b_blend = float(b_target) + else: + step = b_target - self._b_blend + step = max(-BLEND_SLEW, min(BLEND_SLEW, step)) + self._b_blend = float(self._b_blend + step) + return self._b_blend + + def kappa_schedule(self, k_abs: float) -> float: + """Asymmetric filter on |kappa_cmd| feeding the curvature-gain interp — the PRIMARY + anti-weave fix (see module docstring). Fast rise keeps curve-entry gain arrival + within ~0.2 s at any strength; the slow strength-scaled fall removes the gain + modulation and ratchets toward the stable higher gain under oscillation. + + Seeds at the CURRENT |kappa| on its first active frame — enabling mid-curve must + not start the schedule from zero and momentarily read 'straight road'.""" + if not self.active: + self._sched_init = False # a future enable re-seeds from the live value + return k_abs + if not self._sched_init: + self._sched = k_abs + self._sched_init = True + return k_abs + rc_down = max(GAIN_RC_UP, GAIN_RC_DOWN * self.strength) + rc = GAIN_RC_UP if k_abs > self._sched else rc_down + a = self.dt / (rc + self.dt) + self._sched += a * (k_abs - self._sched) + return float(self._sched) + + def wire(self, path_angle: float) -> float: + """Hold the outgoing wire value inside a 1-LSB band (scaled by strength). + LatCtlPath_An_Actl's LSB is 0.0005 rad; near-zero commands crossed a code boundary + 962/min on the baseline route and the PSCM integrates that square wave into the + felt dither. Worst steady-state bias is under the yaw-measurement quantum; a held + frame is a zero-ROC frame and the release step is far inside the tightest soft + ROC, so panda's path_angle checks cannot be tripped by this.""" + if not self.active: + self._wire = path_angle + return path_angle + if abs(path_angle - self._wire) < WIRE_HOLD * self.strength: + return self._wire + self._wire = path_angle + return path_angle 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 42aa95ed59..b8f8e23a5a 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,33 @@ 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.angle_smoothing import AngleSmoother 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 (not user-tunable). -# 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. + +# --- Anti-weave smoothing (FordAngleSmoothing) ------------------------------------------- +# All constants, semantics, and filter math live in angle_smoothing.AngleSmoother (pure, +# unit-tested). Menu 1.0 = stock/no smoothing, bit-identical to the toggle being off. + # 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) @@ -171,17 +163,41 @@ 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 (also gates smoothing reads) + # BluePilot: anti-weave smoothing (FordAngleSmoothing; see angle_smoothing.py). + self.smoother = AngleSmoother(dt=_STEER_DT) + # 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 + + + # -- smoothing compat surface (offline replay tooling sets these directly) --------------- + @property + def smoothing_enabled(self) -> bool: + return self.smoother.enabled + + @smoothing_enabled.setter + def smoothing_enabled(self, v: bool): + self.smoother.enabled = bool(v) + + @property + def smoothing_strength(self) -> float: + return self.smoother.strength + + @smoothing_strength.setter + def smoothing_strength(self, v: float): + self.smoother.strength = float(v) def update_angle_params(self, params): """Sets per-platform gain defaults and reads user feel-factor 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"): @@ -201,6 +217,32 @@ 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 + try: + _sm_enabled = bool(params.get_bool("FordAngleSmoothing")) + raw_strength = params.get("FordAngleSmoothStrength", return_default=True) + _menu = 1.0 + self.smoother.strength # keep current on unreadable/empty + if raw_strength is not None and raw_strength != b"": + _menu = float( + raw_strength.decode("utf-8", errors="replace") if isinstance(raw_strength, bytes) else raw_strength) + self.smoother.configure(_sm_enabled, _menu) + except Exception: + pass # keep the previous values; defaults are enabled / 1.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 update_angle_strategy(self, CC, CS, actuators, CP): """ @@ -246,6 +288,9 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.angle_stall_blip_active = False self.press_timer_s = 0.0 self.precision_type = 1 + self.bp_angle_saturated = False + self.autocal_ctl.idle() # steady-state timer and staged samples must not span disengagements + self.smoother.reset() return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -289,6 +334,9 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.angle_stall_blip_active = False self.press_timer_s = 0.0 self.precision_type = 1 + self.bp_angle_saturated = False + self.autocal_ctl.idle() # steady-state timer and staged samples must not span disengagements + self.smoother.reset() return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -330,6 +378,12 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_kappa_cmd = self.get_current_curvature(CS) self._desired_curvature_last = float(actuators.curvature) self.precision_type = 1 + self.bp_angle_saturated = False # published flag must not stay stale across the blip + # Same discontinuity handling as the disengage/human-turn branches: the blip breaks + # the steady-state baseline and straddles the apex buffer, so staged evidence and + # peak windows must not survive it. + self.autocal_ctl.idle() + self.smoother.reset() # path ramps back from zero; filters must too if self.stall_blip_frames_left <= 0: self.stall_blip_cooldown_s = _STALL_COOLDOWN_S return LateralResult( @@ -362,7 +416,10 @@ def update_angle_strategy(self, CC, CS, actuators, CP): 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) + # Anti-weave: hysteresis so noise straddling the entering/exiting boundary can't flip + # this boolean (and with it the exit-blend gate) frame to frame near zero curvature. + _kappa_entering = self.smoother.entering(_kappa_at_t_base - abs(desired_curvature), + _kappa_at_t_base > abs(desired_curvature)) if _kappa_entering: _kappa_factor = 1.0 # curve deepening ahead: full extra lookahead for gradual entry else: @@ -376,6 +433,9 @@ def update_angle_strategy(self, CC, CS, actuators, CP): predicted_curvature = float( interp(curvature_lookup_time, ModelConstants.T_IDXS, curvatures) ) + # Anti-weave: low-pass the model prediction to strip frame-to-frame jitter (details + # in angle_smoothing.prediction — inside the VLT's slack, so no curve-entry cost). + predicted_curvature = self.smoother.prediction(predicted_curvature) b = float(self.path_angle_blend_ratio) b = float(clip(b, 0.0, 1.0)) @@ -404,7 +464,9 @@ 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 + # Anti-weave: slew instead of stepping the exit blend (bounded ramps, no 4x steps). + b_blend = self.smoother.blend(_b_target) requested_curvature = predicted_curvature * b_blend + desired_curvature * (1.0 - b_blend) self._desired_curvature_last = desired_curvature @@ -452,11 +514,17 @@ 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.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.curvature_factor = interp(abs(kappa_cmd), [0.0007, 0.001], [self.low_gain_calc, self.high_gain_calc]) + self.low_gain_calc = interp(v_ego, [V_LOW, V_HIGH], [1.0, self.path_angle_gain_lowC_highV]) + 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. The smoother's + # asymmetric filter on |kappa| is the PRIMARY anti-weave fix (see angle_smoothing.py). + # 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. + _kappa_for_gain = self.smoother.kappa_schedule(abs(kappa_cmd)) + self.curvature_factor = interp(_kappa_for_gain, [0.0007, 0.001], [self.low_gain_calc, self.high_gain_calc]) path_angle_calc = kappa_cmd * v_ego * self.curvature_factor path_angle = path_angle_calc @@ -481,7 +549,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 @@ -500,6 +572,10 @@ 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) + # Anti-weave: 1-LSB wire hold on the outgoing path_angle (kills LSB dither; held + # frames are zero-ROC and cannot trip panda — details in angle_smoothing.wire). + path_angle = self.smoother.wire(path_angle) + # c0 always zero -- no centering trim in angle mode. path_offset = 0.0 @@ -555,6 +631,37 @@ def update_angle_strategy(self, CC, CS, actuators, CP): ramp_type = 2 + # BluePilot: continuous auto-calibration — the automated version of the manual method: + # compare requested vs actual turn (steady segments AND curve apexes), nudge the lateral + # tuning menu factors toward the fit in small bounded steps, back off on overshoot, and + # persist the evidence so it spans drives. kappa_cmd here is the exact post-clip + # curvature path_angle was derived from; current_curvature is the same measured value + # the strategy itself steers against. The pipeline stages samples for 1s (a grip or + # road-disturbance cancels them retroactively), holds a 3s post-grip cooldown, and + # rejects any frame where cmd != meas has an explanation other than gain error (bump + # flick, rough surface, tire-limit lat accel, longitudinal load transfer, saturation). + # Human-turn and stall-blip frames never reach here (their branches early-return after + # idling the pipeline). The controller owns the liveDelay warmup gate, nudge writes, + # save cadence, and the lock -> disarm transition; a returned pair is adopted as the + # live factors so this very frame steers with the new gain. Frame construction (and + # its signal reads) only happens while the calibrator is armed — for everyone else + # this whole block is one attribute check per frame. + if self.autocal_ctl.enabled: + ws = CS.out.wheelSpeeds + ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + nudged = self.autocal_ctl.feed( + Frame(v_ego=v_ego, kappa_cmd=kappa_cmd, kappa_meas=current_curvature, + 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), + delay_estimated=str(self.sm['liveDelay'].status) == "estimated") + if nudged is not None: + self.low_speed_curv_factor = float(nudged[0]) + self.high_speed_curv_factor = float(nudged[1]) return LateralResult( apply_curvature=0.0, @@ -565,3 +672,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..5f1e98f29a --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -0,0 +1,668 @@ +"""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, + 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_DEADBAND, NUDGE_STEP, MAX_DRIVE_DELTA, + 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_slow_ramp_bounded_by_window_drift(self): + # A ramp inside the per-frame rate bound but drifting through the window must not + # pass: the same-frame ratio would be actuation-lag-biased (liveDelay up to ~0.42s). + gate = SteadyStateGate(dt=DT) + k = MIN_KAPPA * 2 + admitted = False + for _ in range(int(STEADY_TIME_S / DT) * 4): + admitted |= gate.update(True, k, False, False, False) + k += 0.5 * 0.0015 * DT # half the per-frame rate limit, sustained + assert not admitted + # A truly flat command still passes — one extra frame for the drift reset that + # closes the ramp's stale window, then a full fresh steady period. + for _ in range(int(STEADY_TIME_S / DT) + 3): + ok = gate.update(True, k, False, False, False) + assert ok + + 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 + 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 + 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 + 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) + assert len(pipe._staged) > 0 + pipe.idle() + assert len(pipe._staged) == 0 and pipe.gate.steady_s == 0.0 + + 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 + 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 + + +class TestFactorNudger: + def _evidenced_pipe(self, 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 + for _ in range(n): + pipe.update(_frame(v, kappa, kappa * r, low=applied[0], high=applied[1])) + return pipe + + def test_nudges_toward_target_bounded(self): + 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_STEP, 2) # full step, not the whole error + assert high == round(1.0 + NUDGE_STEP, 2) + + def test_deadband_no_nudge(self): + pipe = self._evidenced_pipe(true_low=1.01, true_high=1.01) + assert pipe.recommend(1.0, 1.0) is None # |err| ~ 0.01 < deadband + + 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_per_drive_cap(self): + pipe = self._evidenced_pipe(true_low=1.40, true_high=1.40) + applied = [1.0, 1.0] + moved = 0.0 + for _ in range(30): # far more opportunities than the cap allows + 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: + moved += abs(rec[0] - applied[0]) + applied = list(rec) + assert moved <= MAX_DRIVE_DELTA + 1e-9 + assert pipe.drive_delta_low <= MAX_DRIVE_DELTA + 1e-9 + + 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 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_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) -> 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) + + +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, + "lane_change_factor_high_ang": float, + } + + 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): + return bool(self.values.get(key)) + + def put(self, key, value): + 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 + + +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), (1.00, 1.00)) + # 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} + # Our own write must NOT read back as a user edit. + self._tick(ext, p, n=2) + assert ext.autocal_ctl.pipeline is not None and not ext.autocal_ctl._edit_pending + + def test_user_edit_adopted_after_two_ticks(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) + 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) # first tick: pending + assert ext.autocal_ctl._edit_pending and abs(ext.autocal_ctl.pipeline.est.s_w - w0) < 1e-9 + self._tick(ext, p, n=1) # second tick: confirmed + assert not ext.autocal_ctl._edit_pending + 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_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/tests/test_angle_smoothing.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_smoothing.py new file mode 100644 index 0000000000..7f3af130e2 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_smoothing.py @@ -0,0 +1,158 @@ +"""Anti-weave smoothing: unit tests for the pure AngleSmoother. + +The load-bearing guarantees, in order of importance: + 1. menu 1.0 (or toggle off) is a bit-identical passthrough — the stock contract; + 2. curve-entry behavior is strength-independent and fast; + 3. mid-drive enable/strength changes can never act on stale state; + 4. the wire hold cannot chatter and releases on any real step. +""" +import math +import random + +from opendbc.sunnypilot.car.ford.angle_smoothing import ( + AngleSmoother, GAIN_RC_UP, GAIN_RC_DOWN, PRED_RC, ENTER_HYST, BLEND_SLEW, + WIRE_HOLD, MENU_MIN, MENU_MAX, +) + +DT = 0.05 + + +def _smoother(menu=2.0, enabled=True): + s = AngleSmoother(dt=DT) + s.configure(enabled, menu) + return s + + +class TestStockContract: + def test_menu_one_is_bit_identical_passthrough(self): + s = _smoother(menu=1.0) + rng = random.Random(7) + for _ in range(500): + pred = rng.uniform(-0.01, 0.01) + b = rng.uniform(0.0, 0.6) + k = rng.uniform(0.0, 0.02) + pa = rng.uniform(-0.5, 0.5) + raw_ent = rng.random() < 0.5 + assert s.prediction(pred) == pred + assert s.blend(b) == b + assert s.kappa_schedule(k) == k + assert s.wire(pa) == pa + assert s.entering(rng.uniform(-0.001, 0.001), raw_ent) == raw_ent + + def test_toggle_off_is_bit_identical_passthrough(self): + s = _smoother(menu=2.5, enabled=False) + for i in range(100): + v = math.sin(i * 0.3) * 0.01 + assert s.prediction(v) == v + assert s.kappa_schedule(abs(v)) == abs(v) + assert s.wire(v) == v + + def test_configure_clamps_menu(self): + s = AngleSmoother(dt=DT) + s.configure(True, 0.2) + assert s.strength == 0.0 and not s.active # below MENU_MIN -> stock + s.configure(True, 99.0) + assert abs(s.strength - (MENU_MAX - 1.0)) < 1e-12 + + +class TestEntryGuarantee: + def test_gain_schedule_rise_is_fast_at_any_strength(self): + # Discrete one-pole with RC=GAIN_RC_UP at 20 Hz (a = dt/(rc+dt) = 1/3 per frame): + # 80% of a step at 0.2 s, 90% at 0.3 s — and the rise must NOT slow down as + # strength increases (entry behavior is strength-independent by design). + for menu in (1.5, 2.0, MENU_MAX): + s = _smoother(menu=menu) + s.kappa_schedule(0.0) # seed at zero (straight road) + out = 0.0 + for i in range(round(0.3 / DT)): + out = s.kappa_schedule(0.004) + if i == round(0.2 / DT) - 1: + assert out >= 0.80 * 0.004, (menu, out) + assert out >= 0.90 * 0.004, (menu, out) + + def test_release_slows_with_strength(self): + outs = {} + for menu in (1.5, 2.5): + s = _smoother(menu=menu) + s.kappa_schedule(0.004) # seed high + for _ in range(round(0.4 / DT)): + out = s.kappa_schedule(0.0) + outs[menu] = out + assert outs[2.5] > outs[1.5] > 0.0 # stronger damping decays slower + + +class TestTransitionSafety: + def test_enable_mid_curve_seeds_at_current_kappa(self): + # Regression: the schedule used to start from 0, momentarily reading a curve + # as a straight (gain dip on enable). First active frame must pass through. + s = _smoother(menu=1.0) # stock + for _ in range(50): + s.kappa_schedule(0.005) # driving a curve, passthrough + s.configure(True, 2.0) # driver steps strength mid-curve + assert s.kappa_schedule(0.005) == 0.005 + + def test_reenable_reseeds_prediction(self): + # Regression: pred filter kept a stale value across a disable/enable cycle. + s = _smoother(menu=2.0) + s.prediction(0.009) # seeds at 0.009 + s.configure(True, 1.0) # stock: passthrough, must clear seeding + assert s.prediction(0.0) == 0.0 + s.configure(True, 2.0) # re-enable minutes later + assert s.prediction(0.002) == 0.002 # seeds fresh at the live value + + def test_reset_reseeds_everything(self): + s = _smoother(menu=2.0) + s.kappa_schedule(0.004) + s.prediction(0.004) + s.blend(0.6) + s.wire(0.3) + s.reset() + assert s.kappa_schedule(0.001) == 0.001 # re-seeded, not averaged across the gap + assert s.prediction(0.001) == 0.001 + assert s.blend(0.15) == 0.15 + # wire re-seeds via its normal release (any step from 0 exceeds the band) + assert s.wire(0.2) == 0.2 + + +class TestElements: + def test_entering_hysteresis_holds_in_band(self): + s = _smoother(menu=2.0) + assert s.entering(2 * ENTER_HYST, False) is True # crossed up + assert s.entering(0.0, False) is True # in band: holds + assert s.entering(-0.5 * ENTER_HYST, False) is True # still in band + assert s.entering(-2 * ENTER_HYST, True) is False # crossed down + + def test_blend_slew_is_bounded(self): + s = _smoother(menu=2.0) + s.blend(0.6) + out = s.blend(0.15) # 4x exit step requested + assert abs(out - 0.6) <= BLEND_SLEW + 1e-12 + steps = 0 + while abs(out - 0.15) > 1e-9 and steps < 100: + out = s.blend(0.15) + steps += 1 + assert steps <= round(0.5 / DT) + 1 # full 0.60 -> 0.15 within ~0.5 s + + def test_prediction_rc_scales_with_strength(self): + outs = {} + for menu in (1.5, 2.5): + s = _smoother(menu=menu) + s.prediction(0.0) # seed at 0 + outs[menu] = s.prediction(0.01) # one step toward 0.01 + assert outs[1.5] > outs[2.5] # stronger -> heavier filtering + + def test_wire_hold_no_chatter_and_clean_release(self): + s = _smoother(menu=2.0) # band = WIRE_HOLD * 1.0 + band = WIRE_HOLD * s.strength + s.wire(0.10) # establish the held value + rng = random.Random(3) + for _ in range(200): # dither strictly inside the band + out = s.wire(0.10 + rng.uniform(-0.9, 0.9) * band) + assert out == 0.10 # wire never moves + assert s.wire(0.10 + 1.5 * band) == 0.10 + 1.5 * band # real step releases + + def test_wire_tracks_when_stock(self): + s = _smoother(menu=1.0) + for i in range(20): + v = 0.0001 * i + assert s.wire(v) == v diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py index a6650d9072..049e7a3863 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py @@ -202,5 +202,212 @@ def test_safety_param_stays_a_plain_int(self): self.assertIs(type(CP_SP.safetyParam), int) +class _SmModel: + """Minimal modelV2 stand-in: constant curvature along the horizon.""" + class _OR: + def __init__(self, z): + self.z = z + + class _Meta: + laneChangeState = 0 + laneChangeDirection = 0 + + def __init__(self, kappa, v): + self.orientationRate = self._OR([kappa * v] * 33) + self.meta = self._Meta() + + +class _SmParams: + """Typed-enough mock params for the smoothing toggle glue.""" + def __init__(self, values): + self.values = values + + def get(self, key, return_default=False): + return self.values.get(key) + + def get_bool(self, key): + return bool(self.values.get(key)) + + def put(self, key, value): + self.values[key] = value + + +class TestAngleSmoothing(unittest.TestCase): + """Anti-weave smoothing (FordAngleSmoothing). The OFF path must behave exactly like the + unsmoothed math; the ON path must remove dither injectors without softening curve entry.""" + + V = 20.0 + + def _ext(self, smoothing): + cp = _explorer_cp() + ext = _Harness(cp) + ext.CP = cp # update_angle_params reads self.CP (set by carcontroller in the real stack) + ext.human_turn_detector = _ForcedDetector(False) + ext.smoothing_enabled = smoothing + # Effective scale (menu - 1.0): tests exercise the tuned package (menu 2.0). + ext.smoothing_strength = 1.0 if smoothing else 0.0 + return ext + + def _cs(self, desired=0.0): + # yaw tracks desired so the deviation clip never binds and measured == desired. + return _CS(vEgoRaw=self.V, vEgo=self.V, yawRate=-desired * self.V) + + def _drive(self, ext, desired_seq, model_kappa=None): + out = [] + for d in desired_seq: + if model_kappa is not None: + ext.model = _SmModel(model_kappa, self.V) + out.append(ext.update_angle_strategy(_CC(), self._cs(d), _Actuators(curvature=d), _explorer_cp()).path_angle) + return out + + def test_off_gain_schedule_uses_raw_kappa(self): + from numpy import interp as np_interp + ext = self._ext(False) + for d in [0.0006, 0.0011, 0.0006, 0.0011] * 10: + ext.update_angle_strategy(_CC(), self._cs(d), _Actuators(curvature=d), _explorer_cp()) + expected = float(np_interp(abs(ext.bp_kappa_cmd), [0.0007, 0.001], + [ext.low_gain_calc, ext.high_gain_calc])) + self.assertAlmostEqual(ext.curvature_factor, expected, places=12) + # OFF path must leave the smoothing filters untouched at their reset values. + self.assertEqual(ext.smoother._sched, 0.0) + self.assertIsNone(ext.smoother._b_blend) + + def test_on_gain_schedule_filters_oscillation(self): + ext = self._ext(True) + factors = [] + for d in [0.0006, 0.0011] * 40: # square wave straddling the interp band + ext.update_angle_strategy(_CC(), self._cs(d), _Actuators(curvature=d), _explorer_cp()) + factors.append(ext.curvature_factor) + tail = factors[-20:] + # Raw input would swing the factor across the whole low<->high range every frame; + # the filtered schedule input must pin it nearly constant once settled. + self.assertLess(max(tail) - min(tail), 0.05) + + def test_gain_filter_asymmetry(self): + from opendbc.sunnypilot.car.ford.angle_smoothing import GAIN_RC_UP as _SM_GAIN_RC_UP, GAIN_RC_DOWN as _SM_GAIN_RC_DOWN + ext = self._ext(True) + rise_frames = int(2.3 * _SM_GAIN_RC_UP / 0.05) + 2 + self._drive(ext, [0.002] * rise_frames, model_kappa=0.002) + self.assertGreater(ext.smoother._sched, 0.9 * 0.002) + fall_frames = int(_SM_GAIN_RC_DOWN / 0.05) + self._drive(ext, [0.0] * fall_frames, model_kappa=0.0) + self.assertGreater(ext.smoother._sched, 0.3 * 0.002) + + def test_wire_hold_stops_sub_lsb_dither(self): + from opendbc.sunnypilot.car.ford.angle_smoothing import WIRE_HOLD as _SM_WIRE_HOLD + ext = self._ext(True) + self._drive(ext, [0.0015] * 60) # settle onto a working point + held = ext.path_angle_last + eps = _SM_WIRE_HOLD / (self.V * 3.0) + out = self._drive(ext, [0.0015 + (eps if i % 2 else -eps) for i in range(40)]) + self.assertTrue(all(abs(pa - held) < 1e-12 for pa in out[5:])) + out = self._drive(ext, [0.0030] * 30) # a genuine move releases the hold + self.assertNotAlmostEqual(out[-1], held, places=6) + + def test_blend_slew_bounded(self): + from opendbc.sunnypilot.car.ford.angle_smoothing import BLEND_SLEW as _SM_BLEND_SLEW + ext = self._ext(True) + ext.model = _SmModel(0.002, self.V) + prev = None + for d in [0.002] * 20 + [0.015, 0.002] * 20: # >0.010 drops toggle _desired_falling + ext.update_angle_strategy(_CC(), self._cs(d), _Actuators(curvature=d), _explorer_cp()) + if prev is not None and ext.smoother._b_blend is not None: + self.assertLessEqual(abs(ext.smoother._b_blend - prev), _SM_BLEND_SLEW + 1e-9) + prev = ext.smoother._b_blend + + def test_kappa_entering_hysteresis(self): + ext = self._ext(True) + flips = 0 + last = None + for i in range(60): + mk = 0.0005 + (0.0001 if i % 2 else -0.0001) # dither inside the +-0.0003 band + ext.model = _SmModel(mk, self.V) + ext.update_angle_strategy(_CC(), self._cs(0.0005), _Actuators(curvature=0.0005), _explorer_cp()) + if last is not None and ext.smoother._entering != last: + flips += 1 + last = ext.smoother._entering + self.assertEqual(flips, 0) + + def test_curve_entry_not_softened(self): + ramp = [min(0.003, 0.0002 * i) for i in range(60)] + off = self._drive(self._ext(False), ramp) + on = self._drive(self._ext(True), ramp) + target = 0.9 * off[-1] + t_off = next(i for i, x in enumerate(off) if x >= target) + t_on = next(i for i, x in enumerate(on) if x >= target) + self.assertLessEqual(t_on - t_off, 2) # <=0.1 s later at 20 Hz + self.assertAlmostEqual(on[-1], off[-1], delta=abs(off[-1]) * 0.02 + 1e-9) + + def test_roc_property_holds_with_smoothing(self): + import random + rng = random.Random(3) + ext = self._ext(True) + prev = ext.path_angle_last + for _ in range(300): + d = rng.uniform(-0.004, 0.004) + ext.update_angle_strategy(_CC(), self._cs(d), _Actuators(curvature=d), _explorer_cp()) + self.assertLessEqual(abs(ext.path_angle_last - prev), 0.055 + 1e-9) # loosest soft ROC + prev = ext.path_angle_last + + def test_resets_on_override_paths(self): + ext = self._ext(True) + self._drive(ext, [0.002] * 40) + self.assertGreater(ext.smoother._sched, 0.0) + ext.human_turn_detector = _ForcedDetector(True) # forces the override early-return + ext.update_angle_strategy(_CC(), self._cs(0.002), _Actuators(curvature=0.002), _explorer_cp()) + self.assertEqual(ext.smoother._sched, 0.0) + self.assertEqual(ext.smoother._wire, 0.0) + self.assertIsNone(ext.smoother._b_blend) + + def test_menu_one_is_bit_identical_stock(self): + # Menu 1.0 (effective 0) must equal the toggle-off path EXACTLY, frame by frame. + import random + rng = random.Random(7) + seq = [rng.uniform(-0.003, 0.003) for _ in range(200)] + off = self._drive(self._ext(False), seq) + neutral = self._ext(True) + neutral.smoothing_strength = 0.0 # menu 1.0 + on = self._drive(neutral, seq) + self.assertEqual(off, on) + + def test_strength_max_entry_still_fast(self): + ramp = [min(0.003, 0.0002 * i) for i in range(60)] + off = self._drive(self._ext(False), ramp) + strong = self._ext(True) + strong.smoothing_strength = 1.5 + on = self._drive(strong, ramp) + target = 0.9 * off[-1] + t_off = next(i for i, x in enumerate(off) if x >= target) + t_on = next(i for i, x in enumerate(on) if x >= target) + self.assertLessEqual(t_on - t_off, 2) # entry guarantee is strength-independent + + def test_param_glue_reads_strength(self): + ext = self._ext(True) + p = _SmParams({"FordAngleSmoothing": True, "FordAngleSmoothStrength": 1.5, + "FordAngleAutoCal": 0, "FordAngleAutoCalState": ""}) + for _ in range(101): + ext.update_angle_params(p) + self.assertAlmostEqual(ext.smoothing_strength, 0.5) # menu 1.5 -> effective 0.5 + p.values["FordAngleSmoothStrength"] = 9.0 # clamped to the menu max (2.5) + for _ in range(101): + ext.update_angle_params(p) + self.assertAlmostEqual(ext.smoothing_strength, 1.5) + p.values["FordAngleSmoothStrength"] = 0.2 # below stock clamps to menu 1.0 = neutral + for _ in range(101): + ext.update_angle_params(p) + self.assertAlmostEqual(ext.smoothing_strength, 0.0) + + def test_param_glue_reads_toggle(self): + ext = self._ext(True) + p = _SmParams({"FordAngleSmoothing": False, "FordAngleAutoCal": 0, "FordAngleAutoCalState": ""}) + for _ in range(101): + ext.update_angle_params(p) + self.assertFalse(ext.smoothing_enabled) + p.values["FordAngleSmoothing"] = True + for _ in range(101): + ext.update_angle_params(p) + self.assertTrue(ext.smoothing_enabled) + + if __name__ == '__main__': unittest.main() 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 a6bcc449eb..e3a75bccc7 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), + ("FordAngleSmoothing", self._angle_smoothing), ("BPDisableLaneLineStatusColor", self._disable_lane_line_status_color), ("BPHideCameraView", self._hide_camera_view), ("BPRadRacerTheme", self._rad_racer_theme), @@ -532,6 +534,41 @@ 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: anti-weave smoothing of the angle command path (gain-schedule filter, + # wire-quantization hold, blend slew — see lateral_angle_ext.py _SM_* constants). + self._angle_smoothing = toggle_item( + lambda: tr("Smooth Steering (Anti-Weave)"), + lambda: tr("Removes the rhythmic left-right centering motion in angle mode by filtering " + "the sources of steering dither on straight roads. No effect in curves. " + "Turn off to compare against the unsmoothed behavior."), + initial_state=self._safe_get_bool(self._params, "FordAngleSmoothing", default=True), + callback=lambda state: self._toggle_callback(state, "FordAngleSmoothing"), + icon="chffr_wheel.png" + ) + # Manual strength for the smoothing above: 0 = minimal, 1.0 = tuned default, 1.5 = strong. + self._angle_smoothing_strength = float_control_item( + lambda: tr("Smoothing Strength"), + lambda: tr("1.0 = stock steering (no smoothing). Step up for more damping of the " + "straight-road weave; 2.0 = the log-tuned setting, 2.5 = strongest. " + "Curve response is unaffected at any strength."), + param="FordAngleSmoothStrength", + min_value=1.0, + max_value=2.5, + step=0.1, + icon="chffr_wheel.png" + ) # Disable BP lateral control toggle self._disable_BP_lat = toggle_item( lambda: tr("Disable BP Lateral Control"), @@ -614,6 +651,9 @@ def _section(title: str, items: list) -> list: angle_items = [ self._low_speed_curv_factor, self._high_speed_curv_factor, + self._angle_autocal, + self._angle_smoothing, + self._angle_smoothing_strength, self._lane_change_factor_high_ang, ] angle_header = CollapsibleSectionHeader(tr("Angle Tuning")) @@ -861,6 +901,9 @@ def _update_toggles(self, just_toggled: dict | None = None): # Angle-mode items: always visible (Angle Tuning section), greyed out when curvature mode is active self._low_speed_curv_factor.action_item.set_enabled(is_angle) self._high_speed_curv_factor.action_item.set_enabled(is_angle) + self._angle_autocal.action_item.set_enabled(is_angle) + self._angle_smoothing.action_item.set_enabled(is_angle) + self._angle_smoothing_strength.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) @@ -1004,6 +1047,16 @@ 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 _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 789ea7cfad..44446b0403 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py +++ b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py @@ -22,6 +22,19 @@ def __init__(self, back_callback: Callable[[], None] | None = None): self.high_speed_factor = BigParamFloatControl( "High Speed Adjustment Factor", "FordHighSpeedFactor_ang", min=0.5, max=1.5, 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, + ) + # Anti-weave smoothing of the angle command path (see lateral_angle_ext.py _SM_*). + self.angle_smoothing = BigParamControlBP( + "Smooth Steering (Anti-Weave)", "FordAngleSmoothing", + ) + self.angle_smoothing_strength = BigParamFloatControl( + "Smoothing Strength", "FordAngleSmoothStrength", min=1.0, max=2.5, step=0.1, + ) self.lane_change_factor_high_ang = BigParamFloatControl( "Lane Change Factor High", "lane_change_factor_high_ang", min=0.85, max=1.50, ) @@ -69,6 +82,9 @@ def __init__(self, back_callback: Callable[[], None] | None = None): self._scroller.add_widgets([ self.low_speed_factor, self.high_speed_factor, + self.angle_autocal, + self.angle_smoothing, + self.angle_smoothing_strength, self.lane_change_factor_high_ang, self.disable_lane_change_under_speed, self.blinker_min_speed, @@ -86,6 +102,8 @@ def __init__(self, back_callback: Callable[[], None] | None = None): ]) self._refresh_toggles = ( + ("FordAngleAutoCal", self.angle_autocal), + ("FordAngleSmoothing", self.angle_smoothing), ("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), @@ -97,6 +115,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() @@ -110,6 +133,9 @@ def _update_toggles(self): is_curv = not is_angle self.low_speed_factor.set_visible(is_angle) self.high_speed_factor.set_visible(is_angle) + self.angle_autocal.set_visible(is_angle) + self.angle_smoothing.set_visible(is_angle) + self.angle_smoothing_strength.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 20c4782734..e119c6c76d 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -2566,6 +2566,48 @@ } ] }, + { + "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": "FordAngleSmoothing", + "widget": "toggle", + "title": "[Lateral Tuning] Smooth Steering (Anti-Weave)", + "description": "Master enable for the anti-weave smoothing of the angle command path. Strength below sets how much; 1.0 strength = stock either way.", + "visibility": [ + { + "type": "param", + "key": "FordPrefLateralControl", + "equals": 1 + } + ] + }, + { + "key": "FordAngleSmoothStrength", + "widget": "option", + "title": "[Lateral Tuning] Smoothing Strength", + "description": "1.0 = stock steering (no smoothing). Step up for more damping of straight-road weave; 2.0 = log-tuned, 2.5 = strongest. Curves unaffected.", + "min": 1.0, + "max": 2.5, + "step": 0.1, + "visibility": [ + { + "type": "param", + "key": "FordPrefLateralControl", + "equals": 1 + } + ] + }, { "key": "lane_change_factor_high_ang", "widget": "option",