From d3099c8ae5716574f51b8b7d70e875839e3dab85 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:09:37 -0400 Subject: [PATCH 01/17] ford autocal: params, capnp telemetry fields, card publisher FordAngleAutoCal / FordAngleAutoCalState params (persistent), ControllerStateBP fields @54-56, publisher wiring. Co-Authored-By: Claude Fable 5 --- bluepilot/selfdrive/car/bp_card_publisher.py | 3 +++ cereal/custom.capnp | 3 +++ common/params_keys.h | 2 ++ 3 files changed, 8 insertions(+) diff --git a/bluepilot/selfdrive/car/bp_card_publisher.py b/bluepilot/selfdrive/car/bp_card_publisher.py index cde85839d9..1ee4b8224f 100644 --- a/bluepilot/selfdrive/car/bp_card_publisher.py +++ b/bluepilot/selfdrive/car/bp_card_publisher.py @@ -104,6 +104,8 @@ def _refresh_settings_cache() -> dict: "bmsMinimumSpeedToPauseLaneChange": _get_int(p, "BlinkerMinLateralControlSpeed", 20), "bmsShowLateralControlMode": _get_bool(p, "BpShowLateralControl"), # --- Angle Tuning --- + "bmsAngleAutoCalibrate": _get_bool(p, "FordAngleAutoCal"), + "bmsAngleAutoCalState": _get_str(p, "FordAngleAutoCalState"), "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 +133,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. 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..a3eeac1931 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -309,6 +309,8 @@ 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; "done ..." = calibrated and locked {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, From a5baff96c90461b60084551255160d8ff6bc3bbc Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:09:49 -0400 Subject: [PATCH 02/17] ford: angle-mode speed-factor auto-calibration (core + carcontroller glue + tests) Two-anchor gain estimator with steady-state gating, driven from the angle lateral strategy in the card process; factors applied live, state persisted. Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/angle_autocal.py | 319 ++++++++++++++++++ .../sunnypilot/car/ford/lateral_angle_ext.py | 135 +++++++- .../car/ford/tests/test_angle_autocal.py | 241 +++++++++++++ 3 files changed, 677 insertions(+), 18 deletions(-) create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py 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..e99b83264f --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -0,0 +1,319 @@ +"""BluePilot: one-time 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 +/- buttons by comparing requested turn +to actual turn. This module automates exactly that comparison: in steady engaged curves +the ratio r = kappa_measured / kappa_commanded is the plant gain error, so the factor +that would have made actual == requested is factor_applied / r. Samples are collected +across speeds and solved as a weighted least-squares fit of the same two-anchor interp +model, yielding the corrected anchors directly. + +The estimator is pure math with no I/O so the exact same code runs in two places: + - offline, in tools/bp/angle_autocal_analyze.py, replaying logged drives so the + result (and every accepted sample) can be inspected before anything touches the car + - onboard, fed from lateral_angle_ext during normal driving until convergence, after + which the factors are written once and the calibration locks (per-car, not per-drive) +""" +import math + +from opendbc.car.ford.values import CAR + +# Hard-coded per-platform gain defaults (moved here from lateral_angle_ext so this module +# and the offline analyzer share one source; lateral_angle_ext imports them back). +# 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, +}) + + +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 gain interpolation in lateral_angle_ext (m/s: ~30 mph and ~60 mph). +V_LOW = 13.5 +V_HIGH = 26.82 +# Fixed multiplier on the low-speed anchor in lateral_angle_ext. +LOW_ANCHOR_BASE = 1.30 + +# 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) +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 + +# 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 + +# Convergence: effective weight is accumulated seconds of valid steady cornering. +CONVERGE_MIN_WEIGHT = 30.0 # per anchor (~30 s of steady curves near each anchor) +# Real-road residual scatter (crown, wind, surface) floors around 2-2.5% even with hours of +# clean samples (measured: 2.7 h Mach-E drive -> stderr 0.022/0.026), so 2% was unreachably +# strict. 3% matches VERIFY_TOL: a fit good to ~3% is exactly what a verification round can +# confirm or refute, and rounds are the real lock guard. +CONVERGE_MAX_STDERR = 0.03 # fit standard error per anchor + +FACTOR_MIN, FACTOR_MAX = 0.5, 1.5 # same clamp as the settings +/- buttons + +# Multi-round verification: after a converged fit the factors are applied and collection +# restarts against them; the calibration only locks when a subsequent round's recommendation +# is a no-change within VERIFY_TOL (proof the correction actually landed on the real car — +# the ratio model is first-order, so a large correction deserves a confirmation pass). +VERIFY_TOL = 0.03 # per-factor |new - applied| considered "no further change" +MAX_ROUNDS = 4 # safety bound; lock after this many applications regardless + + +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 steady-curve samples. + + Model: the ideal gain at sample i is y_i = F_applied(v_i) / r_i where r_i is the + measured/commanded curvature ratio. 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. + """ + + def __init__(self, platform_gain_high: float, low_factor_applied: float, high_factor_applied: float): + self.platform_gain_high = float(platform_gain_high) + self.low_factor_applied = float(low_factor_applied) + self.high_factor_applied = float(high_factor_applied) + # 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 + + def applied_gain(self, v_ego: float) -> float: + a = speed_alpha(v_ego) + low = LOW_ANCHOR_BASE * self.low_factor_applied + high = self.platform_gain_high * self.high_factor_applied + return (1.0 - a) * low + a * high + + def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, weight: float = 1.0) -> bool: + """Add one steady-curve observation. Returns True if accepted. + + 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. + """ + 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 = self.applied_gain(v_ego) / 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 + return True + + @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 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). + """ + 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)) + + 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, + "anchor_low": anchor_low, + "anchor_high": anchor_high, + } + return low_factor, high_factor, stats + + def converged(self) -> bool: + result = self.solve() + if result is None: + return False + _, _, stats = result + return (stats["weight_low"] >= CONVERGE_MIN_WEIGHT + and stats["weight_high"] >= CONVERGE_MIN_WEIGHT + and stats["stderr_low"] <= CONVERGE_MAX_STDERR + and stats["stderr_high"] <= CONVERGE_MAX_STDERR) + + +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.grip_cooldown_s = 0.0 + + def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, + angle_rate_limited: bool, deviation_limited: bool, + human_turn: bool, stall_blip: bool, + saturated: bool = False, driver_torque: float = 0.0) -> bool: + # 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 human_turn or abs(driver_torque) > TORQUE_GUARD_NM + if grip: + self.grip_cooldown_s = PRESS_COOLDOWN_S + else: + self.grip_cooldown_s = max(0.0, self.grip_cooldown_s - self.dt) + + ok = (lat_active and not grip and self.grip_cooldown_s <= 0.0 + and not angle_rate_limited and not deviation_limited + and not stall_blip and not saturated + 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 + self.kappa_last = kappa_cmd if lat_active else None + self.steady_s = self.steady_s + self.dt if ok else 0.0 + return self.steady_s >= STEADY_TIME_S + + +class AutoCalPipeline: + """Gate + holdback staging + estimator, driven with one call per 20 Hz lateral frame. + + Samples sit in a staging queue for PRESS_HOLDBACK_S before they reach the estimator; + if the driver grips the wheel while they wait, they are cancelled — the grip was + likely already influencing the car before detection tripped. Used identically by the + onboard hook and the offline analyzer. + """ + + def __init__(self, platform_gain_high: float, low_factor_applied: float, + high_factor_applied: float, dt: float = 0.05): + self.est = AngleFactorEstimator(platform_gain_high, low_factor_applied, high_factor_applied) + self.gate = SteadyStateGate(dt=dt) + self.dt = dt + self._staged: list[list] = [] # [age_s, v, kappa_cmd, kappa_meas] + self._meas_last = None + + def idle(self): + """Call on frames where lateral is inactive (disengaged / human-turn override).""" + self.gate.update(False, 0.0, False, False, False, False, False) + self._staged.clear() + self._meas_last = None + + def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, + steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, + human_turn: bool, stall_blip: bool, + saturated: bool = False, driver_torque: float = 0.0) -> list: + """Advance one frame. 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.""" + grip = steering_pressed or human_turn or abs(driver_torque) > TORQUE_GUARD_NM + if grip: + self._staged.clear() + + eligible = self.gate.update(True, kappa_cmd, steering_pressed, + angle_rate_limited, deviation_limited, + human_turn, stall_blip, + saturated=saturated, driver_torque=driver_torque) + + # 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 + + # 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], weight=self.dt): + 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]) + return committed 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..f8555e781f 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -21,35 +21,29 @@ back in from zero through the soft ROC below (no jump seed) -- generous at human-turn speeds, and admitted by ford.h's path_angle ROC check (2% looser) without any bypass. """ +import re + 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.sunnypilot.car.ford.angle_autocal import AutoCalPipeline, VERIFY_TOL, MAX_ROUNDS 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 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, -}) +# Hard-coded per-platform gain defaults (not user-tunable). Single source lives in +# angle_autocal.py so the offline analyzer and the auto-calibrator share them. +from opendbc.sunnypilot.car.ford.angle_autocal import ( # noqa: E402 + GAIN_CAN as _GAIN_CAN, + GAIN_CANFD_BOF as _GAIN_CANFD_BOF, + GAIN_CANFD_SUV as _GAIN_CANFD_SUV, + CANFD_BOF_CARS as _CANFD_BOF_CARS, + CANFD_SUV_CARS as _CANFD_SUV_CARS, +) # DBC ``LatCtlPath_An_Actl`` (rad) — panda safety uses the same in ``ford.h``; PSCM enforces in firmware. @@ -171,6 +165,19 @@ 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: one-time auto-calibration of the speed factors (see angle_autocal.py). + # Enabled by FordAngleAutoCal; FordAngleAutoCalState empty = collecting, "done ..." = locked. + # The values are per-car constants, so once converged the factors are written and the + # calibrator never runs again (toggling the setting off clears the state to allow a re-run). + self.autocal_enabled = False + self.autocal_done = True # conservative until params are read + self.autocal = None # AutoCalPipeline while collecting + self._autocal_round = 1 + self._autocal_param_ctr = 100 # >= threshold so the very first call reads params + self._autocal_params_handle = None + # Telemetry + autocal gate: the command this frame was modified by PSCM authority + # limits or the DBC clamp — the car could not make the requested turn. + self.bp_angle_saturated = False def update_angle_params(self, params): """Sets per-platform gain defaults and reads user feel-factor params.""" @@ -201,6 +208,35 @@ 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: + 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") + self.autocal_done = state.startswith("done") + self.autocal_enabled = enabled and not self.autocal_done + # Verification round number: 1 on a fresh start, parsed back from the state string + # after an "applied" round so rounds survive restarts and drives. + _m = re.search(r"round (\d+)", state) + self._autocal_round = int(_m.group(1)) if _m else 1 + # (Re)build the pipeline when arming, and restart it if the user hand-changes a + # factor mid-collection — samples are relative to the factors in force when taken. + factors_changed = (self.autocal is not None and + (abs(self.autocal.est.low_factor_applied - self.low_speed_curv_factor) > 1e-6 or + abs(self.autocal.est.high_factor_applied - self.high_speed_curv_factor) > 1e-6)) + if self.autocal_enabled and (self.autocal is None or factors_changed): + self.autocal = AutoCalPipeline(self.path_angle_gain_highC_highV, + self.low_speed_curv_factor, + self.high_speed_curv_factor, dt=_STEER_DT) + elif not self.autocal_enabled: + self.autocal = None + self._autocal_params_handle = params + except Exception: + self.autocal_enabled = False def update_angle_strategy(self, CC, CS, actuators, CP): """ @@ -246,6 +282,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 + if self.autocal is not None: # steady-state timer and staged samples must not span disengagements + self.autocal.idle() return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -289,6 +328,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 + if self.autocal is not None: # steady-state timer and staged samples must not span disengagements + self.autocal.idle() return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -481,7 +523,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 @@ -555,6 +601,21 @@ def update_angle_strategy(self, CC, CS, actuators, CP): ramp_type = 2 + # BluePilot: one-time auto-calibration — feed steady engaged curves to the estimator and, + # on convergence, write the corrected factors once and lock. 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 + # cancels them retroactively), holds a 3s post-grip cooldown, and drops any frame where + # the car couldn't make the turn (PSCM authority / DBC saturation, tire-limit lat accel). + if self.autocal_enabled and self.autocal is not None: + self.autocal.update(v_ego, kappa_cmd, current_curvature, + CS.out.steeringPressed, + self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, + self.angle_human_turn_active, self.angle_stall_blip_active, + saturated=self.bp_angle_saturated, + driver_torque=float(CS.out.steeringTorque)) + if self.autocal.est.n > 0 and self.autocal.est.n % 20 == 0 and self.autocal.est.converged(): + self._autocal_finish() return LateralResult( apply_curvature=0.0, @@ -565,3 +626,41 @@ def update_angle_strategy(self, CC, CS, actuators, CP): precision_type=self.precision_type, lateralUncertainty=lateral_uncertainty, ) + + def _autocal_finish(self): + """A round converged. Apply the factors; lock only when a verification round confirms. + + Round semantics: the ratio model is first-order, so a large correction is applied and + then re-measured with the new factors in force. When a round's recommendation is a + no-change within VERIFY_TOL (or MAX_ROUNDS is hit), the calibration locks for good. + """ + result = self.autocal.est.solve() + if result is None or self._autocal_params_handle is None: + return + low_new, high_new, stats = result + delta_low = abs(low_new - self.autocal.est.low_factor_applied) + delta_high = abs(high_new - self.autocal.est.high_factor_applied) + verified = delta_low <= VERIFY_TOL and delta_high <= VERIFY_TOL + final = verified or self._autocal_round >= MAX_ROUNDS + if final: + state = (f"done low={low_new:.2f} high={high_new:.2f} rounds={self._autocal_round} " + f"{'verified' if verified else 'round-limit'} n={stats['n']} " + f"se={stats['stderr_low']:.3f}/{stats['stderr_high']:.3f}") + else: + state = (f"round {self._autocal_round + 1} collecting; applied low={low_new:.2f} " + f"high={high_new:.2f} (moved {delta_low:.2f}/{delta_high:.2f})") + try: + self._autocal_params_handle.put("FordLowSpeedFactor_ang", f"{low_new:.2f}") + self._autocal_params_handle.put("FordHighSpeedFactor_ang", f"{high_new:.2f}") + self._autocal_params_handle.put("FordAngleAutoCalState", state) + except Exception: + return + # Apply immediately so this drive benefits without waiting for the param round-trip. + self.low_speed_curv_factor = float(low_new) + self.high_speed_curv_factor = float(high_new) + self.autocal = None # verification rounds rebuild against the new factors on the next param tick + if final: + self.autocal_done = True + self.autocal_enabled = False + else: + self._autocal_round += 1 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..21fb1662ce --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -0,0 +1,241 @@ +"""Tests for the one-time angle-mode factor auto-calibration (angle_autocal.py).""" +import random + +import pytest + +from opendbc.sunnypilot.car.ford.angle_autocal import ( + AngleFactorEstimator, AutoCalPipeline, SteadyStateGate, speed_alpha, + V_LOW, V_HIGH, LOW_ANCHOR_BASE, STEADY_TIME_S, MIN_KAPPA, + PRESS_HOLDBACK_S, PRESS_COOLDOWN_S, MAX_LAT_ACCEL, +) + +PLATFORM_GAIN_HIGH = 1.05 # Mach-E + + +def simulate_plant(est, true_low_factor, true_high_factor, speeds, kappa=0.002, n_per_speed=200, noise=0.0): + """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(42) + for v in speeds: + a = speed_alpha(v) + ideal = (1.0 - a) * (LOW_ANCHOR_BASE * true_low_factor) + a * (PLATFORM_GAIN_HIGH * true_high_factor) + for _ in range(n_per_speed): + r = est.applied_gain(v) / ideal + r *= 1.0 + (rng.uniform(-noise, noise) if noise else 0.0) + est.add_sample(v, kappa, kappa * r, weight=0.05) + + +class TestAngleFactorEstimator: + def test_recovers_true_factors(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) + simulate_plant(est, true_low_factor=0.92, true_high_factor=1.21, + speeds=[10, 12, 15, 18, 21, 24, 27, 29], n_per_speed=200, noise=0.03) + low, high, stats = est.solve() + assert abs(low - 0.92) < 0.02, low + assert abs(high - 1.21) < 0.02, high + assert est.converged() + + def test_accounts_for_applied_factors(self): + # A drive made with non-default factors must still recover the same truth. + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.10, 0.90) + simulate_plant(est, true_low_factor=0.92, true_high_factor=1.21, + speeds=[10, 15, 20, 25, 29], n_per_speed=300, 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_not_converged_with_one_sided_speeds(self): + # Only low-speed driving: high anchor must not report convergence. + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) + simulate_plant(est, 0.95, 1.10, speeds=[10, 11, 12], n_per_speed=400, noise=0.02) + assert not est.converged() + + def test_rejects_bad_samples(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) + assert not est.add_sample(20.0, 0.0005, 0.0005) # below curvature threshold + assert not est.add_sample(5.0, 0.002, 0.002) # below speed threshold + assert not est.add_sample(20.0, 0.002, -0.002) # sign mismatch + assert not est.add_sample(20.0, 0.002, 0.02) # absurd ratio + assert not est.add_sample(29.0, 0.004, 0.004) # 3.4 m/s^2 lat accel — car may not make this turn + assert est.n == 0 + assert est.add_sample(29.0, 0.0025, 0.0025) # 2.1 m/s^2 — comfortably within tire limits + + def test_factor_clamp(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) + simulate_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 + + +class TestSteadyStateGate: + def test_requires_sustained_steady(self): + gate = SteadyStateGate(dt=0.05) + needed = int(STEADY_TIME_S / 0.05) + results = [gate.update(True, MIN_KAPPA * 2, False, False, 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=0.05) + for _ in range(int(STEADY_TIME_S / 0.05) + 1): + gate.update(True, MIN_KAPPA * 2, False, False, False, False, False) + assert gate.update(True, MIN_KAPPA * 2, False, False, False, False, False) + gate.update(True, MIN_KAPPA * 2, True, False, False, False, False) # pressed + assert gate.steady_s == 0.0 + + def test_resets_on_curvature_jump(self): + gate = SteadyStateGate(dt=0.05) + for _ in range(int(STEADY_TIME_S / 0.05) + 1): + gate.update(True, 0.002, False, False, False, False, False) + assert gate.update(True, 0.002, False, False, False, False, False) + assert not gate.update(True, 0.004, False, False, False, False, False) # jump + assert gate.steady_s == 0.0 + + def test_saturation_blocks(self): + gate = SteadyStateGate(dt=0.05) + for _ in range(int(STEADY_TIME_S / 0.05) + 2): + assert not gate.update(True, 0.002, False, False, False, False, False, saturated=True) + + def test_light_torque_starts_cooldown(self): + gate = SteadyStateGate(dt=0.05) + # Torque below the steeringPressed threshold but above the guard: hands are on. + gate.update(True, 0.002, False, False, False, False, False, driver_torque=0.7) + assert gate.grip_cooldown_s > 0.0 + # Cooldown blocks sampling for PRESS_COOLDOWN_S after the grip ends. + blocked = int(PRESS_COOLDOWN_S / 0.05) - 1 + for _ in range(blocked): + assert not gate.update(True, 0.002, False, False, False, False, False) + + +def run_pipeline(pipe, n, torque=0.0, pressed=False, saturated=False, kappa=0.002, v=20.0): + committed = [] + for _ in range(n): + committed += pipe.update(v, kappa, kappa, pressed, False, False, False, False, + saturated=saturated, driver_torque=torque) + return committed + + +class TestAutoCalPipeline: + def test_commits_after_holdback(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) + warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / 0.05) + 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, 1.0, 1.0) + # Reach eligibility, stage a few samples, but grip before the holdback elapses: + warm = int(STEADY_TIME_S / 0.05) + 1 + int(PRESS_HOLDBACK_S / 0.05) // 2 + run_pipeline(pipe, warm) + assert len(pipe._staged) > 0 and pipe.est.n == 0 + pipe.update(20.0, 0.002, 0.002, True, False, False, False, False) # grip + assert len(pipe._staged) == 0 + assert pipe.est.n == 0 # nothing from before the grip ever reached the estimator + + def test_light_torque_also_cancels(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) + warm = int(STEADY_TIME_S / 0.05) + 5 + run_pipeline(pipe, warm) + assert len(pipe._staged) > 0 + pipe.update(20.0, 0.002, 0.002, False, False, False, False, False, driver_torque=0.7) + assert len(pipe._staged) == 0 and pipe.est.n == 0 + + def test_saturated_frames_never_commit(self): + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) + 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, 1.0, 1.0) + run_pipeline(pipe, int(STEADY_TIME_S / 0.05) + 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): + # Command steady but the car still converging toward it (closed-loop compensation + # tail): those frames must not be sampled even though the command gate passes. + pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) + warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / 0.05) + 10 + kappa_meas = 0.0010 # far from the 0.002 command, sweeping up fast + staged_during_sweep = 0 + for _ in range(warm): + pipe.update(20.0, 0.002, kappa_meas, False, False, False, False, False) + 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 # nothing accepted while the car was still turning in + assert pipe.est.n > 0 # but samples flow once the measurement settles + + +class _MockParams: + """Duck-typed openpilot Params: just enough for update_angle_params.""" + 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): + 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 test that would have caught the autocal_gate + AttributeError that crashed card on-device while every component test passed.""" + + def _ext(self): + messaging = pytest.importorskip("cereal.messaging") # noqa: F841 (linux-only) + from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt + + class _Harness(LateralAngleExt): + # In the real CarController this comes from the LateralCurvExt mixin, where it is + # a no-op compatibility shim (state is initialized eagerly). Same no-op here. + def _ensure_lateral_curv_initialized(self, CP): + pass + + ext = _Harness() + class _CP: + carFingerprint = "FORD_MUSTANG_MACH_E_MK1" + ext.CP = _CP() + return ext + + def test_param_glue_runs_without_error(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 0, "FordAngleAutoCalState": ""}) + for _ in range(205): # spans two 100-call read cycles including the first-call read + ext.update_angle_params(p) + assert ext.autocal is None and not ext.autocal_enabled + + def test_arming_builds_pipeline(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 is not None + assert abs(ext.autocal.est.low_factor_applied - 1.10) < 1e-6 + assert abs(ext.autocal.est.high_factor_applied - 0.95) < 1e-6 + assert ext._autocal_round == 1 + + def test_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 is None and ext.autocal_done and not ext.autocal_enabled + + def test_round_number_survives_restart(self): + ext = self._ext() + p = _MockParams({"FordAngleAutoCal": 1, + "FordAngleAutoCalState": "round 3 collecting; applied low=1.05 high=1.10"}) + ext.update_angle_params(p) + assert ext._autocal_round == 3 and ext.autocal_enabled From 212c1640875bd3b2dd8b95d04862bb46441c499d Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:10:04 -0400 Subject: [PATCH 03/17] ui: FordAngleAutoCal toggle in lateral tuning menus (TICI + MICI) Co-Authored-By: Claude Fable 5 --- selfdrive/ui/bp/layouts/settings/bluepilot.py | 25 +++++++++++++++++++ .../bp/mici/layouts/settings/lateral_mici.py | 14 +++++++++++ 2 files changed, 39 insertions(+) diff --git a/selfdrive/ui/bp/layouts/settings/bluepilot.py b/selfdrive/ui/bp/layouts/settings/bluepilot.py index a6bcc449eb..7a03847eca 100644 --- a/selfdrive/ui/bp/layouts/settings/bluepilot.py +++ b/selfdrive/ui/bp/layouts/settings/bluepilot.py @@ -78,6 +78,7 @@ 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), ("BPDisableLaneLineStatusColor", self._disable_lane_line_status_color), ("BPHideCameraView", self._hide_camera_view), ("BPRadRacerTheme", self._rad_racer_theme), @@ -532,6 +533,18 @@ 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" + ) # Disable BP lateral control toggle self._disable_BP_lat = toggle_item( lambda: tr("Disable BP Lateral Control"), @@ -614,6 +627,7 @@ def _section(title: str, items: list) -> list: angle_items = [ self._low_speed_curv_factor, self._high_speed_curv_factor, + self._angle_autocal, self._lane_change_factor_high_ang, ] angle_header = CollapsibleSectionHeader(tr("Angle Tuning")) @@ -861,6 +875,7 @@ 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._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 +1019,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..a2f9ecc8bf 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py +++ b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py @@ -22,6 +22,12 @@ 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, + ) self.lane_change_factor_high_ang = BigParamFloatControl( "Lane Change Factor High", "lane_change_factor_high_ang", min=0.85, max=1.50, ) @@ -69,6 +75,7 @@ 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.lane_change_factor_high_ang, self.disable_lane_change_under_speed, self.blinker_min_speed, @@ -86,6 +93,7 @@ def __init__(self, back_callback: Callable[[], None] | None = None): ]) self._refresh_toggles = ( + ("FordAngleAutoCal", self.angle_autocal), ("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 +105,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 +123,7 @@ 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.lane_change_factor_high_ang.set_visible(is_angle) self.blinker_min_speed.set_enabled(ui_state.params.get_bool("BlinkerPauseLaneChange")) for item in ( From 7f351aab9414933fefe6c1069a4b19448495d26f Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:22:52 -0400 Subject: [PATCH 04/17] ford autocal: continuous closed-loop calibration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replaces the one-shot converge/verify-rounds design (whose evidence died at ignition-off — a normal drive rarely satisfied both anchors, so nothing ever visibly happened) with the automated version of the manual tuning method: - apex evidence: command/measured curvature extrema matched across the actuator lag, amplitude ratio = gain (the 'tops and bottoms of the graphs'), median-of-3 committed; steady-state evidence kept alongside - quality layer with per-cause rejection counters: bump-flick blanking (with wheel-speed corroboration), rough-road RMS on the high-passed residual, lat-accel margin down-weighting, longitudinal-accel gate, left/right divergence guard for crowned/banked roads - live nudging: factors step toward the fit (0.02 max, 20s apart, 0.10/drive cap) and are written to the params — the lateral tuning menu shows them move; per-sample applied-gain bookkeeping keeps the estimate invariant to the nudge trajectory, so the loop backs off on overshoot - evidence persists across drives: estimator sufficient statistics serialize into FordAngleAutoCalState (JSON) every 30s, restored on arming; legacy 'done' states stay honored as locked - lock when there is nothing left to adjust for 5 min of driving; user edits win instantly (soft evidence reset); toggle-off still clears everything Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/angle_autocal.py | 570 ++++++++++++++--- .../sunnypilot/car/ford/lateral_angle_ext.py | 202 ++++-- .../car/ford/tests/test_angle_autocal.py | 593 +++++++++++++++--- 3 files changed, 1138 insertions(+), 227 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py index e99b83264f..c3c47749fc 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -1,4 +1,4 @@ -"""BluePilot: one-time auto-calibration of the angle-mode speed adjustment factors. +"""BluePilot: continuous auto-calibration of the angle-mode speed adjustment factors. The angle strategy (lateral_angle_ext.py) computes, for the high-curvature branch: @@ -6,18 +6,29 @@ 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 +/- buttons by comparing requested turn -to actual turn. This module automates exactly that comparison: in steady engaged curves -the ratio r = kappa_measured / kappa_commanded is the plant gain error, so the factor -that would have made actual == requested is factor_applied / r. Samples are collected -across speeds and solved as a weighted least-squares fit of the same two-anchor interp -model, yielding the corrected anchors directly. +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 tools/bp/angle_autocal_analyze.py, replaying logged drives so the - result (and every accepted sample) can be inspected before anything touches the car - - onboard, fed from lateral_angle_ext during normal driving until convergence, after - which the factors are written once and the calibration locks (per-car, not per-drive) + - 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 @@ -66,6 +77,12 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: STEADY_TIME_S = 0.6 # command must be steady this long before samples count (PSCM lag) 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. +MAX_LONG_ACCEL = 1.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 @@ -74,22 +91,64 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: 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 -# Convergence: effective weight is accumulated seconds of valid steady cornering. -CONVERGE_MIN_WEIGHT = 30.0 # per anchor (~30 s of steady curves near each anchor) -# Real-road residual scatter (crown, wind, surface) floors around 2-2.5% even with hours of -# clean samples (measured: 2.7 h Mach-E drive -> stderr 0.022/0.026), so 2% was unreachably -# strict. 3% matches VERIFY_TOL: a fit good to ~3% is exactly what a verification round can -# confirm or refute, and rounds are the real lock guard. -CONVERGE_MAX_STDERR = 0.03 # fit standard error per anchor +# --- Disturbance / road-quality rejection ------------------------------------------------ +# 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.008 # 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.3 # 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.0006 # 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 +PEAK_WEIGHT_S = 3.0 # 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 ---------------------------------------------------------------- +TAU_EVIDENCE_S = 1800.0 # evidence forgetting time constant (seconds of active collection) +OUTLIER_MIN_WEIGHT = 20.0 # anchor weight before the outlier gate arms +OUTLIER_GATE = 0.15 # reject samples whose implied gain is this far from the fit +# 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 # per-factor cumulative cap per drive (card process lifetime) + +# --- Lock -------------------------------------------------------------------------------- +LOCK_MIN_WEIGHT = 120.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 -# Multi-round verification: after a converged fit the factors are applied and collection -# restarts against them; the calibration only locks when a subsequent round's recommendation -# is a no-change within VERIFY_TOL (proof the correction actually landed on the real car — -# the ratio model is first-order, so a large correction deserves a confirmation pass). -VERIFY_TOL = 0.03 # per-factor |new - applied| considered "no further change" -MAX_ROUNDS = 4 # safety bound; lock after this many applications regardless +# 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: @@ -102,19 +161,19 @@ def speed_alpha(v_ego: float) -> float: class AngleFactorEstimator: - """Weighted least-squares fit of the two gain anchors from steady-curve samples. - - Model: the ideal gain at sample i is y_i = F_applied(v_i) / r_i where r_i is the - measured/commanded curvature ratio. 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. + """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, low_factor_applied: float, high_factor_applied: float): + def __init__(self, platform_gain_high: float): self.platform_gain_high = float(platform_gain_high) - self.low_factor_applied = float(low_factor_applied) - self.high_factor_applied = float(high_factor_applied) # 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 @@ -124,19 +183,17 @@ def __init__(self, platform_gain_high: float, low_factor_applied: float, high_fa 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 applied_gain(self, v_ego: float) -> float: - a = speed_alpha(v_ego) - low = LOW_ANCHOR_BASE * self.low_factor_applied - high = self.platform_gain_high * self.high_factor_applied - return (1.0 - a) * low + a * high - - def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, weight: float = 1.0) -> bool: - """Add one steady-curve observation. Returns True if accepted. + 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. + 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 @@ -147,8 +204,18 @@ def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, weight: r = kappa_meas / kappa_cmd if not (MIN_RATIO <= r <= MAX_RATIO): return False - y = self.applied_gain(v_ego) / r + y = float(applied_gain) / r a = speed_alpha(v_ego) + # Robustness backstop behind the frame-level quality layer: once an anchor has real + # evidence, a sample that disagrees wildly with the fit is a glitch, not information. + sol = self.solve() + if sol is not None: + _, _, st = sol + w_anchor = st["weight_low"] if a < 0.5 else st["weight_high"] + if w_anchor >= OUTLIER_MIN_WEIGHT: + y_pred = (1.0 - a) * st["anchor_low"] + a * st["anchor_high"] + if abs(y - y_pred) > OUTLIER_GATE: + return False w = float(weight) la = 1.0 - a self.s_ll += w * la * la @@ -159,8 +226,29 @@ def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, weight: 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.""" @@ -170,12 +258,24 @@ def weight_low(self) -> float: 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). + 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: @@ -193,6 +293,8 @@ def solve(self): 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)) @@ -202,20 +304,200 @@ def solve(self): "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 converged(self) -> bool: - result = self.solve() - if result is None: - return False - _, _, stats = result - return (stats["weight_low"] >= CONVERGE_MIN_WEIGHT - and stats["weight_high"] >= CONVERGE_MIN_WEIGHT - and stats["stderr_low"] <= CONVERGE_MAX_STDERR - and stats["stderr_high"] <= CONVERGE_MAX_STDERR) + 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 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[tuple] = [] # (kappa_cmd, kappa_meas, v, applied_gain, ok) + 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)): + k, m, v, g, _ = self.buf[i] + self.buf[i] = (k, m, v, g, 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((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 + k_c, _, v_c, g_c, _ = self.buf[c] + 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[4] for f in window): + return [] + mags = [abs(f[0]) 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[4]: + return [] # disturbance inside the match window: the pair is unusable + if f[1] * sign > m_pk: + m_pk = f[1] * 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: @@ -256,43 +538,76 @@ def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, class AutoCalPipeline: - """Gate + holdback staging + estimator, driven with one call per 20 Hz lateral frame. - - Samples sit in a staging queue for PRESS_HOLDBACK_S before they reach the estimator; - if the driver grips the wheel while they wait, they are cancelled — the grip was - likely already influencing the car before detection tripped. Used identically by the - onboard hook and the offline analyzer. + """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, low_factor_applied: float, - high_factor_applied: float, dt: float = 0.05): - self.est = AngleFactorEstimator(platform_gain_high, low_factor_applied, high_factor_applied) + 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] + 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 override).""" self.gate.update(False, 0.0, False, False, False, False, False) + self.quality.idle() + self.peaks.clear() self._staged.clear() self._meas_last = None def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, human_turn: bool, stall_blip: bool, - saturated: bool = False, driver_torque: float = 0.0) -> list: - """Advance one frame. 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.""" + saturated: bool = False, driver_torque: float = 0.0, + a_ego: float = 0.0, ws_spread: float | None = None, + low_factor: float = 1.0, high_factor: float = 1.0) -> list: + """Advance one 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 [] + grip = steering_pressed or human_turn or abs(driver_torque) > TORQUE_GUARD_NM if grip: self._staged.clear() + self.quality.counters["grip"] += 1 + + q_ok = self.quality.update(kappa_cmd, kappa_meas, a_ego=a_ego, ws_spread=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 = self.gate.update(True, kappa_cmd, steering_pressed, angle_rate_limited, deviation_limited, human_turn, stall_blip, saturated=saturated, driver_torque=driver_torque) + 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 @@ -302,18 +617,133 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, 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, low_factor, 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], weight=self.dt): + 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]) + 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. + frame_ok = (q_ok and not grip and self.gate.grip_cooldown_s <= 0.0 + and not angle_rate_limited and not deviation_limited + and not stall_blip and not saturated 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 - low_factor) <= LOCK_DEADBAND and abs(high_t - 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): + 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) > MAX_DRIVE_DELTA: + 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) + new_high = step(high_t, high_factor, st["weight_high"], st["stderr_eff_high"], self.drive_delta_high) + 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/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index f8555e781f..7263439f18 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -21,7 +21,7 @@ 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 re +import json import numpy as np from numpy import clip, interp @@ -29,7 +29,7 @@ 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.sunnypilot.car.ford.angle_autocal import AutoCalPipeline, VERIFY_TOL, MAX_ROUNDS +from opendbc.sunnypilot.car.ford.angle_autocal import AutoCalPipeline 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 @@ -50,6 +50,36 @@ 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. +_AUTOCAL_SAVE_PERIOD_S = 30.0 + + +def _autocal_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 _autocal_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 + # 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) @@ -165,16 +195,20 @@ 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: one-time auto-calibration of the speed factors (see angle_autocal.py). - # Enabled by FordAngleAutoCal; FordAngleAutoCalState empty = collecting, "done ..." = locked. - # The values are per-car constants, so once converged the factors are written and the - # calibrator never runs again (toggling the setting off clears the state to allow a re-run). + # BluePilot: continuous auto-calibration of the speed factors (see angle_autocal.py). + # Enabled by FordAngleAutoCal. FordAngleAutoCalState carries the estimator's serialized + # evidence as JSON (phase "collecting") so collection spans drives and ignition cycles; + # phase "locked" (or a legacy "done ..." string) means calibrated — the calibrator stays + # off until the user toggles the setting off (clearing the state) and on again. self.autocal_enabled = False self.autocal_done = True # conservative until params are read self.autocal = None # AutoCalPipeline while collecting - self._autocal_round = 1 self._autocal_param_ctr = 100 # >= threshold so the very first call reads params self._autocal_params_handle = None + self._autocal_last_written = None # ("x.xx", "x.xx") the nudger last wrote; edits differ + self._autocal_edit_pending = False # user edit needs 2 consecutive ticks (async put lag) + self._autocal_save_s = 0.0 + self._autocal_dirty = False # 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 @@ -217,23 +251,37 @@ def update_angle_params(self, params): state = params.get("FordAngleAutoCalState", return_default=True) or "" if isinstance(state, bytes): state = state.decode("utf-8", errors="replace") - self.autocal_done = state.startswith("done") + if self.autocal is None: + self.autocal_done = _autocal_state_locked(state) + else: + self.autocal_done = self.autocal.locked self.autocal_enabled = enabled and not self.autocal_done - # Verification round number: 1 on a fresh start, parsed back from the state string - # after an "applied" round so rounds survive restarts and drives. - _m = re.search(r"round (\d+)", state) - self._autocal_round = int(_m.group(1)) if _m else 1 - # (Re)build the pipeline when arming, and restart it if the user hand-changes a - # factor mid-collection — samples are relative to the factors in force when taken. - factors_changed = (self.autocal is not None and - (abs(self.autocal.est.low_factor_applied - self.low_speed_curv_factor) > 1e-6 or - abs(self.autocal.est.high_factor_applied - self.high_speed_curv_factor) > 1e-6)) - if self.autocal_enabled and (self.autocal is None or factors_changed): - self.autocal = AutoCalPipeline(self.path_angle_gain_highC_highV, - self.low_speed_curv_factor, - self.high_speed_curv_factor, dt=_STEER_DT) + if self.autocal_enabled and self.autocal is None: + # Arm: build the pipeline and restore serialized evidence from a prior drive. + # The currently applied factors (just read above) are the nudge baseline. + self.autocal = AutoCalPipeline(self.path_angle_gain_highC_highV, dt=_STEER_DT) + _autocal_restore(self.autocal, state) + self._autocal_last_written = (f"{self.low_speed_curv_factor:.2f}", + f"{self.high_speed_curv_factor:.2f}") + self._autocal_edit_pending = False elif not self.autocal_enabled: self.autocal = 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 read into the live factors); evidence is soft-reset, not wiped. + cur = (f"{self.low_speed_curv_factor:.2f}", f"{self.high_speed_curv_factor:.2f}") + if self._autocal_last_written is not None and cur != self._autocal_last_written: + if self._autocal_edit_pending: + self.autocal.user_edit() + self._autocal_last_written = cur + self._autocal_edit_pending = False + self._autocal_dirty = True + else: + self._autocal_edit_pending = True + else: + self._autocal_edit_pending = False self._autocal_params_handle = params except Exception: self.autocal_enabled = False @@ -601,21 +649,42 @@ def update_angle_strategy(self, CC, CS, actuators, CP): ramp_type = 2 - # BluePilot: one-time auto-calibration — feed steady engaged curves to the estimator and, - # on convergence, write the corrected factors once and lock. 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 - # cancels them retroactively), holds a 3s post-grip cooldown, and drops any frame where - # the car couldn't make the turn (PSCM authority / DBC saturation, tire-limit lat accel). + # 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). if self.autocal_enabled and self.autocal is not None: - self.autocal.update(v_ego, kappa_cmd, current_curvature, - CS.out.steeringPressed, - self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, - self.angle_human_turn_active, self.angle_stall_blip_active, - saturated=self.bp_angle_saturated, - driver_torque=float(CS.out.steeringTorque)) - if self.autocal.est.n > 0 and self.autocal.est.n % 20 == 0 and self.autocal.est.converged(): - self._autocal_finish() + ws = CS.out.wheelSpeeds + ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + committed = self.autocal.update(v_ego, kappa_cmd, current_curvature, + CS.out.steeringPressed, + self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, + self.angle_human_turn_active, self.angle_stall_blip_active, + 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) + if committed: + self._autocal_dirty = True + rec = self.autocal.recommend(self.low_speed_curv_factor, self.high_speed_curv_factor) + if rec is not None: + self._autocal_apply_nudge(rec) + if self.autocal.locked: + self._autocal_save("locked") + self.autocal_done = True + self.autocal_enabled = False + self.autocal = None + else: + self._autocal_save_s += _STEER_DT + if self._autocal_dirty and self._autocal_save_s >= _AUTOCAL_SAVE_PERIOD_S: + self._autocal_save("collecting") return LateralResult( apply_curvature=0.0, @@ -627,40 +696,45 @@ def update_angle_strategy(self, CC, CS, actuators, CP): lateralUncertainty=lateral_uncertainty, ) - def _autocal_finish(self): - """A round converged. Apply the factors; lock only when a verification round confirms. - - Round semantics: the ratio model is first-order, so a large correction is applied and - then re-measured with the new factors in force. When a round's recommendation is a - no-change within VERIFY_TOL (or MAX_ROUNDS is hit), the calibration locks for good. - """ - result = self.autocal.est.solve() - if result is None or self._autocal_params_handle is None: + def _autocal_apply_nudge(self, rec): + """Write a nudged factor pair to the params (the lateral tuning menu shows them move) + and apply in-memory immediately so this very frame steers with the new gain.""" + low_new, high_new = rec + if self._autocal_params_handle is None: return - low_new, high_new, stats = result - delta_low = abs(low_new - self.autocal.est.low_factor_applied) - delta_high = abs(high_new - self.autocal.est.high_factor_applied) - verified = delta_low <= VERIFY_TOL and delta_high <= VERIFY_TOL - final = verified or self._autocal_round >= MAX_ROUNDS - if final: - state = (f"done low={low_new:.2f} high={high_new:.2f} rounds={self._autocal_round} " - f"{'verified' if verified else 'round-limit'} n={stats['n']} " - f"se={stats['stderr_low']:.3f}/{stats['stderr_high']:.3f}") - else: - state = (f"round {self._autocal_round + 1} collecting; applied low={low_new:.2f} " - f"high={high_new:.2f} (moved {delta_low:.2f}/{delta_high:.2f})") try: self._autocal_params_handle.put("FordLowSpeedFactor_ang", f"{low_new:.2f}") self._autocal_params_handle.put("FordHighSpeedFactor_ang", f"{high_new:.2f}") - self._autocal_params_handle.put("FordAngleAutoCalState", state) except Exception: return - # Apply immediately so this drive benefits without waiting for the param round-trip. self.low_speed_curv_factor = float(low_new) self.high_speed_curv_factor = float(high_new) - self.autocal = None # verification rounds rebuild against the new factors on the next param tick - if final: - self.autocal_done = True - self.autocal_enabled = False - else: - self._autocal_round += 1 + self._autocal_last_written = (f"{low_new:.2f}", f"{high_new:.2f}") + self._autocal_edit_pending = False + self._autocal_save("collecting") + + def _autocal_save(self, phase: str): + """Serialize the pipeline into FordAngleAutoCalState (JSON). Async put is fine: + a lost final write costs at most _AUTOCAL_SAVE_PERIOD_S of evidence.""" + if self._autocal_params_handle is None or self.autocal is None: + return + d = { + "v": 1, + "phase": phase, + "pipe": self.autocal.to_dict(), + "applied": {"low": round(self.low_speed_curv_factor, 2), + "high": round(self.high_speed_curv_factor, 2)}, + } + sol = self.autocal.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.autocal.stable_s, 1) + try: + self._autocal_params_handle.put("FordAngleAutoCalState", json.dumps(d, separators=(",", ":"))) + except Exception: + return + self._autocal_save_s = 0.0 + self._autocal_dirty = False 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 index 21fb1662ce..28f1086d72 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -1,180 +1,515 @@ -"""Tests for the one-time angle-mode factor auto-calibration (angle_autocal.py).""" +"""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 ( - AngleFactorEstimator, AutoCalPipeline, SteadyStateGate, speed_alpha, - V_LOW, V_HIGH, LOW_ANCHOR_BASE, STEADY_TIME_S, MIN_KAPPA, - PRESS_HOLDBACK_S, PRESS_COOLDOWN_S, MAX_LAT_ACCEL, + 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, OUTLIER_MIN_WEIGHT, OUTLIER_GATE, 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 simulate_plant(est, true_low_factor, true_high_factor, speeds, kappa=0.002, n_per_speed=200, noise=0.0): +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(42) + rng = random.Random(seed) for v in speeds: - a = speed_alpha(v) - ideal = (1.0 - a) * (LOW_ANCHOR_BASE * true_low_factor) + a * (PLATFORM_GAIN_HIGH * true_high_factor) + g = applied_gain(v, applied_low, applied_high) + r0 = g / ideal_gain(v, true_low, true_high) for _ in range(n_per_speed): - r = est.applied_gain(v) / ideal - r *= 1.0 + (rng.uniform(-noise, noise) if noise else 0.0) - est.add_sample(v, kappa, kappa * r, weight=0.05) + 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, 1.0, 1.0) - simulate_plant(est, true_low_factor=0.92, true_high_factor=1.21, - speeds=[10, 12, 15, 18, 21, 24, 27, 29], n_per_speed=200, noise=0.03) - low, high, stats = est.solve() + 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 - assert est.converged() - def test_accounts_for_applied_factors(self): - # A drive made with non-default factors must still recover the same truth. - est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.10, 0.90) - simulate_plant(est, true_low_factor=0.92, true_high_factor=1.21, - speeds=[10, 15, 20, 25, 29], n_per_speed=300, noise=0.03) + 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_not_converged_with_one_sided_speeds(self): - # Only low-speed driving: high anchor must not report convergence. - est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) - simulate_plant(est, 0.95, 1.10, speeds=[10, 11, 12], n_per_speed=400, noise=0.02) - assert not est.converged() - def test_rejects_bad_samples(self): - est = AngleFactorEstimator(PLATFORM_GAIN_HIGH, 1.0, 1.0) - assert not est.add_sample(20.0, 0.0005, 0.0005) # below curvature threshold - assert not est.add_sample(5.0, 0.002, 0.002) # below speed threshold - assert not est.add_sample(20.0, 0.002, -0.002) # sign mismatch - assert not est.add_sample(20.0, 0.002, 0.02) # absurd ratio - assert not est.add_sample(29.0, 0.004, 0.004) # 3.4 m/s^2 lat accel — car may not make this turn + 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) # 2.1 m/s^2 — comfortably within tire limits + 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, 1.0, 1.0) - simulate_plant(est, 2.5, 0.2, speeds=[10, 20, 29], n_per_speed=100) + 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_outlier_gate_arms_with_weight(self): + est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) + # Solid clean evidence at truth (factors 1.0 -> r = 1) at BOTH anchors — a single + # anchor leaves the two-parameter fit singular and the gate unarmed. + n = int(OUTLIER_MIN_WEIGHT / DT) + 50 + for v, kappa in ((10.0, 0.002), (28.0, 0.0015)): + g = applied_gain(v, 1.0, 1.0) + for _ in range(n): + est.add_sample(v, kappa, kappa, g, weight=DT) + assert est.weight_low >= OUTLIER_MIN_WEIGHT + g = applied_gain(10.0, 1.0, 1.0) + # A sample implying a gain 1.5x the gate outside the fit is rejected... + r_bad = g / (LOW_ANCHOR_BASE + 1.5 * OUTLIER_GATE) + assert not est.add_sample(10.0, 0.002, 0.002 * r_bad, g, weight=DT) + # ...while a mildly-off sample still lands. + r_mild = g / (LOW_ANCHOR_BASE + 0.3 * OUTLIER_GATE) + assert est.add_sample(10.0, 0.002, 0.002 * r_mild, g, weight=DT) + + 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=0.05) - needed = int(STEADY_TIME_S / 0.05) + gate = SteadyStateGate(dt=DT) + needed = int(STEADY_TIME_S / DT) results = [gate.update(True, MIN_KAPPA * 2, False, False, 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=0.05) - for _ in range(int(STEADY_TIME_S / 0.05) + 1): + gate = SteadyStateGate(dt=DT) + for _ in range(int(STEADY_TIME_S / DT) + 1): gate.update(True, MIN_KAPPA * 2, False, False, False, False, False) assert gate.update(True, MIN_KAPPA * 2, False, False, False, False, False) gate.update(True, MIN_KAPPA * 2, True, False, False, False, False) # pressed assert gate.steady_s == 0.0 - def test_resets_on_curvature_jump(self): - gate = SteadyStateGate(dt=0.05) - for _ in range(int(STEADY_TIME_S / 0.05) + 1): - gate.update(True, 0.002, False, False, False, False, False) - assert gate.update(True, 0.002, False, False, False, False, False) - assert not gate.update(True, 0.004, False, False, False, False, False) # jump - assert gate.steady_s == 0.0 - def test_saturation_blocks(self): - gate = SteadyStateGate(dt=0.05) - for _ in range(int(STEADY_TIME_S / 0.05) + 2): + gate = SteadyStateGate(dt=DT) + for _ in range(int(STEADY_TIME_S / DT) + 2): assert not gate.update(True, 0.002, False, False, False, False, False, saturated=True) def test_light_torque_starts_cooldown(self): - gate = SteadyStateGate(dt=0.05) - # Torque below the steeringPressed threshold but above the guard: hands are on. + gate = SteadyStateGate(dt=DT) gate.update(True, 0.002, False, False, False, False, False, driver_torque=0.7) assert gate.grip_cooldown_s > 0.0 - # Cooldown blocks sampling for PRESS_COOLDOWN_S after the grip ends. - blocked = int(PRESS_COOLDOWN_S / 0.05) - 1 + blocked = int(PRESS_COOLDOWN_S / DT) - 1 for _ in range(blocked): assert not gate.update(True, 0.002, False, False, False, False, False) -def run_pipeline(pipe, n, torque=0.0, pressed=False, saturated=False, kappa=0.002, v=20.0): +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(v, kappa, kappa, pressed, False, False, False, False, - saturated=saturated, driver_torque=torque) + saturated=saturated, driver_torque=torque, + low_factor=low, high_factor=high) return committed class TestAutoCalPipeline: def test_commits_after_holdback(self): - pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) - warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / 0.05) + 3 + 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, 1.0, 1.0) - # Reach eligibility, stage a few samples, but grip before the holdback elapses: - warm = int(STEADY_TIME_S / 0.05) + 1 + int(PRESS_HOLDBACK_S / 0.05) // 2 + 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(20.0, 0.002, 0.002, True, False, False, False, False) # grip assert len(pipe._staged) == 0 assert pipe.est.n == 0 # nothing from before the grip ever reached the estimator - def test_light_torque_also_cancels(self): - pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) - warm = int(STEADY_TIME_S / 0.05) + 5 + 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 - pipe.update(20.0, 0.002, 0.002, False, False, False, False, False, driver_torque=0.7) + assert len(pipe._staged) > 0 and pipe.est.n == 0 + # Bump: measured curvature jumps while the command sits still. + pipe.update(20.0, 0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2, False, False, False, False, False) 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, 1.0, 1.0) + 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, 1.0, 1.0) - run_pipeline(pipe, int(STEADY_TIME_S / 0.05) + 5) + 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): - # Command steady but the car still converging toward it (closed-loop compensation - # tail): those frames must not be sampled even though the command gate passes. - pipe = AutoCalPipeline(PLATFORM_GAIN_HIGH, 1.0, 1.0) - warm = int((STEADY_TIME_S + PRESS_HOLDBACK_S) / 0.05) + 10 - kappa_meas = 0.0010 # far from the 0.002 command, sweeping up fast + 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(20.0, 0.002, kappa_meas, False, False, False, False, False) 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 # nothing accepted while the car was still turning in - assert pipe.est.n > 0 # but samples flow once the measurement settles + 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(v, kappa, kappa * r, False, False, False, False, False, + low_factor=applied[0], high_factor=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(10.0, 0.004, 0.004, False, False, False, False, False, + low_factor=1.02, high_factor=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(10.0, 0.004, 0.004 * r, False, False, False, False, False, + low_factor=applied[0], high_factor=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(v, k_cmd, meas, False, False, False, False, False, + driver_torque=grip, a_ego=0.1, + low_factor=applied[0], high_factor=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 class _MockParams: - """Duck-typed openpilot Params: just enough for update_angle_params.""" + """Duck-typed openpilot Params: just enough for update_angle_params. put() lands + immediately (readable on the next get), like a completed async write.""" def __init__(self, values): self.values = values self.written = {} @@ -186,21 +521,20 @@ def get_bool(self, key): return bool(self.values.get(key)) def put(self, key, value): + 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 test that would have caught the autocal_gate - AttributeError that crashed card on-device while every component test passed.""" + 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): - messaging = pytest.importorskip("cereal.messaging") # noqa: F841 (linux-only) + pytest.importorskip("cereal.messaging") # linux-only from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt class _Harness(LateralAngleExt): - # In the real CarController this comes from the LateralCurvExt mixin, where it is - # a no-op compatibility shim (state is initialized eagerly). Same no-op here. def _ensure_lateral_curv_initialized(self, CP): pass @@ -210,32 +544,105 @@ class _CP: 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": ""}) - for _ in range(205): # spans two 100-call read cycles including the first-call read - ext.update_angle_params(p) + self._tick(ext, p, n=2) assert ext.autocal is None and not ext.autocal_enabled - def test_arming_builds_pipeline(self): + 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 is not None - assert abs(ext.autocal.est.low_factor_applied - 1.10) < 1e-6 - assert abs(ext.autocal.est.high_factor_applied - 0.95) < 1e-6 - assert ext._autocal_round == 1 + assert ext._autocal_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 is not None + assert ext.autocal.est.n == donor.est.n + assert ext.autocal.est.solve() == donor.est.solve() - def test_done_state_never_arms(self): + 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 is None and ext.autocal_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 is None and ext.autocal_done and not ext.autocal_enabled - def test_round_number_survives_restart(self): + 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 is not None and ext.autocal.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) + ext._autocal_apply_nudge((1.02, 1.15)) + assert p.written["FordLowSpeedFactor_ang"] == "1.02" + assert p.written["FordHighSpeedFactor_ang"] == "1.15" + st = json.loads(p.written["FordAngleAutoCalState"]) + assert st["phase"] == "collecting" and st["applied"] == {"low": 1.02, "high": 1.15} + assert ext.low_speed_curv_factor == 1.02 and ext.high_speed_curv_factor == 1.15 + # Our own write must NOT read back as a user edit. + self._tick(ext, p, n=2) + assert ext.autocal is not None and not ext._autocal_edit_pending + + def test_user_edit_adopted_after_two_ticks(self): ext = self._ext() - p = _MockParams({"FordAngleAutoCal": 1, - "FordAngleAutoCalState": "round 3 collecting; applied low=1.05 high=1.10"}) + p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": "", + "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) + ext.update_angle_params(p) + feed_plant(ext.autocal.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + w0 = ext.autocal.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_edit_pending and abs(ext.autocal.est.s_w - w0) < 1e-9 + self._tick(ext, p, n=1) # second tick: confirmed + assert not ext._autocal_edit_pending + assert abs(ext.autocal.est.s_w - 0.5 * w0) < 1e-9 # soft reset, not a wipe + assert ext._autocal_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) - assert ext._autocal_round == 3 and ext.autocal_enabled + feed_plant(ext.autocal.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) + sol = ext.autocal.est.solve() + ext._autocal_save("collecting") + # New process, same params: evidence must come back. + ext2 = self._ext() + ext2.update_angle_params(p) + assert ext2.autocal is not None + assert ext2.autocal.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 is not None + p.values["FordAngleAutoCal"] = 0 + self._tick(ext, p, n=1) + assert ext.autocal is None and not ext.autocal_enabled From f998c8930f65e916a43b088671bfd60af74bffee Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:33:09 -0400 Subject: [PATCH 05/17] ford autocal: tune quality thresholds on the reference drive; drop outlier gate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replayed against route 00000006 (2.7h Mach-E, known-good result 1.02/1.15): - outlier-vs-fit gate removed: path-dependent (early evidence anchors the fit, later contradicting evidence gets rejected) — it alone distorted the fit to 0.95/1.22. Robustness stays with the quality layer, ratio bounds, staging cancellation and apex median-of-3. - accel 1.0->2.0 m/s^2 (was rejecting 8587 frames of ordinary braking into corners), spike 0.008->0.02 (pinion noise floor), rough RMS 0.0006->0.0015, ws jump 0.3->0.6 - decay TAU 30min->2h and lock weight 120->60s so a normally-driven car's evidence equilibrium (~90-165s/anchor) can actually reach lock - peak weight 3.0->1.5s: apex evidence supplements steady evidence instead of dominating (transient undershoot pulled the low anchor ~0.02 high) Virtual closed-loop replay: drive 1 ends applied 1.04/1.08; drive 2 (state persisted) converges to 1.02/1.14, target 1.02/1.15 = the reference, and locks. Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/angle_autocal.py | 49 +++++++++++-------- .../car/ford/tests/test_angle_autocal.py | 20 +------- 2 files changed, 30 insertions(+), 39 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py index c3c47749fc..4937263ea0 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -81,8 +81,10 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: # 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. -MAX_LONG_ACCEL = 1.0 # m/s^2 |aEgo| +# 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 @@ -92,20 +94,23 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: 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.008 # 1/m/s measured-curvature rate with a quiet command = disturbance +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.3 # m/s frame-to-frame change of the spread +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.0006 # 1/m residual RMS above this = rough stretch, no evidence +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. @@ -119,13 +124,24 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: 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 -PEAK_WEIGHT_S = 3.0 # one clean apex counts like this many seconds of steady evidence +# 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 ---------------------------------------------------------------- -TAU_EVIDENCE_S = 1800.0 # evidence forgetting time constant (seconds of active collection) -OUTLIER_MIN_WEIGHT = 20.0 # anchor weight before the outlier gate arms -OUTLIER_GATE = 0.15 # reject samples whose implied gain is this far from the fit +# 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. @@ -141,7 +157,10 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: MAX_DRIVE_DELTA = 0.10 # per-factor cumulative cap per drive (card process lifetime) # --- Lock -------------------------------------------------------------------------------- -LOCK_MIN_WEIGHT = 120.0 # per-anchor evidence before locking is possible +# 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 @@ -206,16 +225,6 @@ def add_sample(self, v_ego: float, kappa_cmd: float, kappa_meas: float, return False y = float(applied_gain) / r a = speed_alpha(v_ego) - # Robustness backstop behind the frame-level quality layer: once an anchor has real - # evidence, a sample that disagrees wildly with the fit is a glitch, not information. - sol = self.solve() - if sol is not None: - _, _, st = sol - w_anchor = st["weight_low"] if a < 0.5 else st["weight_high"] - if w_anchor >= OUTLIER_MIN_WEIGHT: - y_pred = (1.0 - a) * st["anchor_low"] + a * st["anchor_high"] - if abs(y - y_pred) > OUTLIER_GATE: - return False w = float(weight) la = 1.0 - a self.s_ll += w * la * la 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 index 28f1086d72..4d5f97fa28 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -11,7 +11,7 @@ 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, OUTLIER_MIN_WEIGHT, OUTLIER_GATE, LR_MIN_WEIGHT, LR_TOL, + 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, ) @@ -89,24 +89,6 @@ def test_decay_halves_weight_at_tau_ln2(self): est.decay(TAU_EVIDENCE_S * math.log(2.0)) assert abs(est.s_w - 0.5 * w0) < 1e-9 - def test_outlier_gate_arms_with_weight(self): - est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) - # Solid clean evidence at truth (factors 1.0 -> r = 1) at BOTH anchors — a single - # anchor leaves the two-parameter fit singular and the gate unarmed. - n = int(OUTLIER_MIN_WEIGHT / DT) + 50 - for v, kappa in ((10.0, 0.002), (28.0, 0.0015)): - g = applied_gain(v, 1.0, 1.0) - for _ in range(n): - est.add_sample(v, kappa, kappa, g, weight=DT) - assert est.weight_low >= OUTLIER_MIN_WEIGHT - g = applied_gain(10.0, 1.0, 1.0) - # A sample implying a gain 1.5x the gate outside the fit is rejected... - r_bad = g / (LOW_ANCHOR_BASE + 1.5 * OUTLIER_GATE) - assert not est.add_sample(10.0, 0.002, 0.002 * r_bad, g, weight=DT) - # ...while a mildly-off sample still lands. - r_mild = g / (LOW_ANCHOR_BASE + 0.3 * OUTLIER_GATE) - assert est.add_sample(10.0, 0.002, 0.002 * r_mild, g, weight=DT) - def test_lr_divergence_flags_bank_bias(self): est = AngleFactorEstimator(PLATFORM_GAIN_HIGH) n = int((LR_MIN_WEIGHT + 2) / DT) From f59d2c8e06738490c2e7251a4a4e793bf8cd1fc6 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Tue, 21 Jul 2026 22:16:58 -0400 Subject: [PATCH 06/17] ford autocal: fix typed-param nudge writes; ground-truth telemetry; self-reporting errors MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Field forensics from the first bp7 install (routes 00000002/00000003 on 2026-07-21): params showed FordAngleAutoCal=1 + saved evidence at boot while every logged drive published disarmed — and the logs couldn't say why, because telemetry re-read the params instead of reporting the controller, and every failure path was a silent except. - nudge writes sent f'{x:.2f}' STRINGS to FLOAT-typed params: TypeError, swallowed, nudges silently never landed. Now writes python floats; the mock Params in tests type-checks like the real params_pyx so this class of bug fails in CI. - bmsAngleAutoCalibrate/bmsAngleAutoCalState now publish the controller's live state (armed/evidence/nudges, off, locked, or an error string), never a param re-read. - every previously-silent failure (param tick, nudge write, state save) now parks a short 'error: ...' breadcrumb in FordAngleAutoCalState, visible in qlogs/initData. Co-Authored-By: Claude Fable 5 --- bluepilot/selfdrive/car/bp_card_publisher.py | 11 +++++ .../sunnypilot/car/ford/lateral_angle_ext.py | 45 ++++++++++++++++--- .../car/ford/tests/test_angle_autocal.py | 23 ++++++++-- 3 files changed, 70 insertions(+), 9 deletions(-) diff --git a/bluepilot/selfdrive/car/bp_card_publisher.py b/bluepilot/selfdrive/car/bp_card_publisher.py index 1ee4b8224f..d2efe8531a 100644 --- a/bluepilot/selfdrive/car/bp_card_publisher.py +++ b/bluepilot/selfdrive/car/bp_card_publisher.py @@ -148,6 +148,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/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 7263439f18..3f8914e8ac 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -209,6 +209,7 @@ def __init__(self, CP=None, CP_SP=None): self._autocal_edit_pending = False # user edit needs 2 consecutive ticks (async put lag) self._autocal_save_s = 0.0 self._autocal_dirty = False + self.bp_autocal_status = "" # live ground-truth status, published in telemetry # 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 @@ -283,8 +284,22 @@ def update_angle_params(self, params): else: self._autocal_edit_pending = False self._autocal_params_handle = params - except Exception: + # Live status for telemetry: published from the EXT's actual 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.autocal_done: + self.bp_autocal_status = "locked" + elif not self.autocal_enabled: + self.bp_autocal_status = "off" + else: + est = self.autocal.est + self.bp_autocal_status = (f"armed n={est.n} w={est.weight_low:.0f}/{est.weight_high:.0f}" + f" applied={self.low_speed_curv_factor:.2f}/{self.high_speed_curv_factor:.2f}" + f" nudges={self.autocal.nudges}") + except Exception as e: self.autocal_enabled = False + self.bp_autocal_status = f"tick error: {type(e).__name__}: {e}"[:200] + self._autocal_error(self.bp_autocal_status) def update_angle_strategy(self, CC, CS, actuators, CP): """ @@ -698,14 +713,20 @@ def update_angle_strategy(self, CC, CS, actuators, CP): def _autocal_apply_nudge(self, rec): """Write a nudged factor pair to the params (the lateral tuning menu shows them move) - and apply in-memory immediately so this very frame steers with the new gain.""" + and apply in-memory immediately so this very frame steers with the new gain. + + 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 the state param so it + shows up in the next drive's logs instead of vanishing.""" low_new, high_new = rec if self._autocal_params_handle is None: return try: - self._autocal_params_handle.put("FordLowSpeedFactor_ang", f"{low_new:.2f}") - self._autocal_params_handle.put("FordHighSpeedFactor_ang", f"{high_new:.2f}") - except Exception: + self._autocal_params_handle.put("FordLowSpeedFactor_ang", float(low_new)) + self._autocal_params_handle.put("FordHighSpeedFactor_ang", float(high_new)) + except Exception as e: + self._autocal_error(f"nudge write failed: {type(e).__name__}: {e}") return self.low_speed_curv_factor = float(low_new) self.high_speed_curv_factor = float(high_new) @@ -713,6 +734,17 @@ def _autocal_apply_nudge(self, rec): self._autocal_edit_pending = False self._autocal_save("collecting") + def _autocal_error(self, msg: str): + """Self-reporting diagnostics: park the error in the state param (STRING) so it is + visible in qlogs/initData and to the offline analyzer. Evidence in the estimator is + NOT touched; the error string replaces the serialized state only until the next + successful save. Never raises.""" + try: + if self._autocal_params_handle is not None: + self._autocal_params_handle.put("FordAngleAutoCalState", f"error: {msg[:300]}") + except Exception: + pass + def _autocal_save(self, phase: str): """Serialize the pipeline into FordAngleAutoCalState (JSON). Async put is fine: a lost final write costs at most _AUTOCAL_SAVE_PERIOD_S of evidence.""" @@ -734,7 +766,8 @@ def _autocal_save(self, phase: str): d["stable_s"] = round(self.autocal.stable_s, 1) try: self._autocal_params_handle.put("FordAngleAutoCalState", json.dumps(d, separators=(",", ":"))) - except Exception: + except Exception as e: + self._autocal_error(f"state save failed: {type(e).__name__}: {e}") return self._autocal_save_s = 0.0 self._autocal_dirty = False 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 index 4d5f97fa28..ff91d5d6bc 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -491,7 +491,20 @@ def test_converges_and_locks(self): 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.""" + 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 = {} @@ -503,6 +516,9 @@ 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 @@ -581,8 +597,9 @@ def test_nudge_writes_params_and_state(self): "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) ext.update_angle_params(p) ext._autocal_apply_nudge((1.02, 1.15)) - assert p.written["FordLowSpeedFactor_ang"] == "1.02" - assert p.written["FordHighSpeedFactor_ang"] == "1.15" + # The fork's params are typed FLOAT — a string write raises and the nudge dies. + assert p.written["FordLowSpeedFactor_ang"] == 1.02 and isinstance(p.written["FordLowSpeedFactor_ang"], float) + assert p.written["FordHighSpeedFactor_ang"] == 1.15 and isinstance(p.written["FordHighSpeedFactor_ang"], float) st = json.loads(p.written["FordAngleAutoCalState"]) assert st["phase"] == "collecting" and st["applied"] == {"low": 1.02, "high": 1.15} assert ext.low_speed_curv_factor == 1.02 and ext.high_speed_curv_factor == 1.15 From 44e682f27451c8e4f10f3465aa26517d95344e8b Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 00:36:20 -0400 Subject: [PATCH 07/17] ford autocal: declare telemetry fields in the ControllerStateBP struct MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Root cause of the muted auto-cal telemetry on EVERY build to date: the publisher set bmsAngleAutoCalibrate/bmsAngleAutoCalState/angleSaturated on the python struct, but convert_to_capnp only carries DECLARED dataclass fields — undeclared setattr is silently dropped and the capnp defaults (False/'') got logged. That is why the device always appeared disarmed in logs while params (and, tonight, tracking physics at 1.000/0.993 median ratio with the nudged 1.04/1.16 factors) proved the calibrator was running the whole time. angleSaturated was equally affected: logged always-False since the field's introduction, so offline analysis never actually excluded saturated frames. Co-Authored-By: Claude Fable 5 --- opendbc_repo/opendbc/car/structs.py | 6 ++++++ 1 file changed, 6 insertions(+) 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 From a8df68eb1430d375d0b62617f4c26d657f882814 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 15:57:05 -0400 Subject: [PATCH 08/17] ford angle auto-cal: close review blockers 1-3 + bound low-anchor wander MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit P1: human_turn/stall_blip parameters removed from SteadyStateGate.update and AutoCalPipeline.update — provably dead False at the onboard call site (both branches early-return after idling the pipeline). Gate gains reset() replacing the seven-positional-False idle call. Offline analyzer now idles on those frames, matching onboard semantics exactly. P2: the stall-blip branch now clears bp_angle_saturated and idles the pipeline before returning, like the disengage and human-turn branches — a blip can no longer leak a stale gate baseline, straddled apex windows, or surviving staged samples into the evidence stream. P3: errors write to their own FordAngleAutoCalError param; a transient write failure before ignition-off can no longer replace (and erase) the serialized evidence in FordAngleAutoCalState. Guard from route 0000000b (2026-07-22): the low anchor holds ~5x less evidence than the high one and round-tripped 0.98->1.06->1.00 in one drive — felt as turn-in overshoot via the 1.30x curve branch. MAX_DRIVE_DELTA_LOW = 0.04 bounds one drive's low-side movement to two steps; replaying the same drive now walks 0.96->0.98->1.00 monotonically to the identical lock (1.00/1.14). 36 tests updated and passing. Co-Authored-By: Claude Fable 5 --- common/params_keys.h | 3 +- .../sunnypilot/car/ford/angle_autocal.py | 43 +++++++++++++------ .../sunnypilot/car/ford/lateral_angle_ext.py | 23 ++++++---- .../car/ford/tests/test_angle_autocal.py | 30 ++++++------- 4 files changed, 61 insertions(+), 38 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index a3eeac1931..ebfcdfa8ea 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -310,7 +310,8 @@ inline static std::unordered_map keys = { {"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; "done ..." = calibrated and locked + {"FordAngleAutoCalState", {PERSISTENT | BACKUP, STRING, ""}}, // "" = collecting; JSON = evidence; "locked"/"done ..." = finished + {"FordAngleAutoCalError", {PERSISTENT, STRING, ""}}, // diagnostics only — separate channel so an error can never clobber evidence {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py index 4937263ea0..3d74aa656a 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -154,7 +154,14 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: 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 # per-factor cumulative cap per drive (card process lifetime) +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 @@ -522,14 +529,22 @@ def __init__(self, dt: float = 0.05): self.kappa_last = None self.grip_cooldown_s = 0.0 + 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 + def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, - human_turn: bool, stall_blip: 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 human_turn or abs(driver_torque) > TORQUE_GUARD_NM + grip = steering_pressed or abs(driver_torque) > TORQUE_GUARD_NM if grip: self.grip_cooldown_s = PRESS_COOLDOWN_S else: @@ -537,7 +552,7 @@ def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, ok = (lat_active and not grip and self.grip_cooldown_s <= 0.0 and not angle_rate_limited and not deviation_limited - and not stall_blip and not saturated + and not saturated 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 @@ -581,8 +596,8 @@ def applied_gain(self, v_ego: float, low_factor: float, high_factor: float) -> f 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 override).""" - self.gate.update(False, 0.0, False, False, False, False, False) + """Call on frames where lateral is inactive (disengaged / human turn / stall blip).""" + self.gate.reset() self.quality.idle() self.peaks.clear() self._staged.clear() @@ -590,7 +605,6 @@ def idle(self): def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, - human_turn: bool, stall_blip: bool, saturated: bool = False, driver_torque: float = 0.0, a_ego: float = 0.0, ws_spread: float | None = None, low_factor: float = 1.0, high_factor: float = 1.0) -> list: @@ -601,7 +615,7 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, if self.locked: return [] - grip = steering_pressed or human_turn or abs(driver_torque) > TORQUE_GUARD_NM + grip = steering_pressed or abs(driver_torque) > TORQUE_GUARD_NM if grip: self._staged.clear() self.quality.counters["grip"] += 1 @@ -614,7 +628,6 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, eligible = self.gate.update(True, kappa_cmd, steering_pressed, angle_rate_limited, deviation_limited, - human_turn, stall_blip, saturated=saturated, driver_torque=driver_torque) eligible = eligible and q_ok @@ -654,7 +667,7 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, # definition not steady). Quality, grip, limit flags all poison the window. frame_ok = (q_ok and not grip and self.gate.grip_cooldown_s <= 0.0 and not angle_rate_limited and not deviation_limited - and not stall_blip and not saturated and margin_w > 0.0) + and not saturated 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): @@ -698,20 +711,22 @@ def recommend(self, low_factor: float, high_factor: float): return None low_t, high_t, st = sol - def step(target, applied, weight, stderr_eff, drive_delta): + 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) > MAX_DRIVE_DELTA: + 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) - new_high = step(high_t, high_factor, st["weight_high"], st["stderr_eff_high"], self.drive_delta_high) + 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) 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 3f8914e8ac..7796dd77e4 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -435,6 +435,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. + if self.autocal is not None: + self.autocal.idle() if self.stall_blip_frames_left <= 0: self.stall_blip_cooldown_s = _STALL_COOLDOWN_S return LateralResult( @@ -676,10 +682,11 @@ def update_angle_strategy(self, CC, CS, actuators, CP): if self.autocal_enabled and self.autocal is not None: ws = CS.out.wheelSpeeds ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + # Human-turn and stall-blip frames never reach here (their branches early-return + # after idling the pipeline), so those flags are not passed — they'd be dead False. committed = self.autocal.update(v_ego, kappa_cmd, current_curvature, CS.out.steeringPressed, self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, - self.angle_human_turn_active, self.angle_stall_blip_active, saturated=self.bp_angle_saturated, driver_torque=float(CS.out.steeringTorque), a_ego=float(CS.out.aEgo), @@ -717,8 +724,8 @@ def _autocal_apply_nudge(self, rec): 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 the state param so it - shows up in the next drive's logs instead of vanishing.""" + 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._autocal_params_handle is None: return @@ -735,13 +742,13 @@ def _autocal_apply_nudge(self, rec): self._autocal_save("collecting") def _autocal_error(self, msg: str): - """Self-reporting diagnostics: park the error in the state param (STRING) so it is - visible in qlogs/initData and to the offline analyzer. Evidence in the estimator is - NOT touched; the error string replaces the serialized state only until the next - successful save. Never raises.""" + """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._autocal_params_handle is not None: - self._autocal_params_handle.put("FordAngleAutoCalState", f"error: {msg[:300]}") + self._autocal_params_handle.put("FordAngleAutoCalError", f"{msg[:300]}") except Exception: pass 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 index ff91d5d6bc..32a092cffd 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -118,7 +118,7 @@ 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, False, False) + results = [gate.update(True, MIN_KAPPA * 2, False, False, False) for _ in range(needed + 2)] assert not any(results[:needed - 1]) assert results[-1] @@ -126,23 +126,23 @@ def test_requires_sustained_steady(self): 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, False, False) - assert gate.update(True, MIN_KAPPA * 2, False, False, False, False, False) - gate.update(True, MIN_KAPPA * 2, True, False, False, False, False) # pressed + 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_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, False, False, saturated=True) + 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, False, False, driver_torque=0.7) + 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, False, False) + assert not gate.update(True, 0.002, False, False, False) class TestQualityMonitor: @@ -277,7 +277,7 @@ def run_pipeline(pipe, n, torque=0.0, pressed=False, saturated=False, kappa=0.00 low=1.0, high=1.0): committed = [] for _ in range(n): - committed += pipe.update(v, kappa, kappa, pressed, False, False, False, False, + committed += pipe.update(v, kappa, kappa, pressed, False, False, saturated=saturated, driver_torque=torque, low_factor=low, high_factor=high) return committed @@ -296,7 +296,7 @@ def test_grip_cancels_staged_samples(self): 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(20.0, 0.002, 0.002, True, False, False, False, False) # grip + pipe.update(20.0, 0.002, 0.002, True, False, False) # grip assert len(pipe._staged) == 0 assert pipe.est.n == 0 # nothing from before the grip ever reached the estimator @@ -306,7 +306,7 @@ def test_disturbance_cancels_staged_samples(self): 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(20.0, 0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2, False, False, False, False, False) + pipe.update(20.0, 0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2, False, False, False) 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) @@ -330,7 +330,7 @@ def test_unsettled_measurement_not_staged(self): kappa_meas = 0.0010 staged_during_sweep = 0 for _ in range(warm): - pipe.update(20.0, 0.002, kappa_meas, False, False, False, False, False) + pipe.update(20.0, 0.002, kappa_meas, False, False, False) 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 @@ -367,7 +367,7 @@ def _evidenced_pipe(self, true_low=1.10, true_high=1.10, applied=(1.0, 1.0), wei pipe.gate.steady_s = 0.0 pipe._meas_last = None for _ in range(n): - pipe.update(v, kappa, kappa * r, False, False, False, False, False, + pipe.update(v, kappa, kappa * r, False, False, False, low_factor=applied[0], high_factor=applied[1]) return pipe @@ -389,7 +389,7 @@ def test_rate_limited(self): 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(10.0, 0.004, 0.004, False, False, False, False, False, + pipe.update(10.0, 0.004, 0.004, False, False, False, low_factor=1.02, high_factor=1.02) assert pipe.recommend(1.02, 1.02) is not None @@ -405,7 +405,7 @@ def test_per_drive_cap(self): 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(10.0, 0.004, 0.004 * r, False, False, False, False, False, + pipe.update(10.0, 0.004, 0.004 * r, False, False, False, low_factor=applied[0], high_factor=applied[1]) rec = pipe.recommend(*applied) if rec is not None: @@ -452,7 +452,7 @@ def _drive(self, pipe, applied, seconds, v, kappa_amp, rng): 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(v, k_cmd, meas, False, False, False, False, False, + pipe.update(v, k_cmd, meas, False, False, False, driver_torque=grip, a_ego=0.1, low_factor=applied[0], high_factor=applied[1]) rec = pipe.recommend(*applied) From 32a195d26e84e3b1d2dd37e1047fbb8f6b065811 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 16:16:30 -0400 Subject: [PATCH 09/17] =?UTF-8?q?ford=20angle=20auto-cal:=20actuation-dela?= =?UTF-8?q?y=20protections=20=E2=80=94=20liveDelay=20warmup=20gate=20+=20s?= =?UTF-8?q?teady-window=20drift=20bound?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The evidence design was delay-insensitive by construction (steady windows and apex amplitude ratios), but two exposures remained: 1) No measurement-chain warmup gate: kappa_meas flows through the vehicle model with liveParameters offsets from the same locationd stack that estimates the actuation delay. Evidence now collects only while liveDelay.status is 'estimated'; before that the pipeline idles (staged samples and peak windows must not straddle the unestimated period). Offline analyzer mirrors the gate from the logged liveDelay stream. 2) Slow ramps inside the per-frame rate bound carry a lag bias: meas(t) answers cmd(t - tau), so a drifting command biases the same-frame ratio by up to tau*rate/kappa — tens of percent at the MIN_KAPPA floor at the ~0.42s delays liveDelay has measured. New STEADY_DRIFT_FRAC=0.10 bounds total command drift across the steady window, capping the error at a few percent worst case regardless of the actual delay, sign-symmetric over entries/exits. Route 0000000b replay with both gates: same qualitative outcome (low walks 0.96->0.98 monotonically, high untouched at 1.14, locks), with ~10-15% less evidence — the warmup minutes and ramp samples it was designed to drop. 37 tests passing. Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/angle_autocal.py | 22 ++++++++++- .../sunnypilot/car/ford/lateral_angle_ext.py | 37 ++++++++++++------- .../car/ford/tests/test_angle_autocal.py | 16 ++++++++ 3 files changed, 61 insertions(+), 14 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py index 3d74aa656a..50601b67cd 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -75,6 +75,14 @@ def platform_gains(fingerprint: str) -> tuple[float, float]: 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 @@ -527,6 +535,7 @@ 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 def reset(self): @@ -535,6 +544,7 @@ def reset(self): 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 def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, @@ -556,8 +566,18 @@ def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, 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 - self.steady_s = self.steady_s + self.dt if ok else 0.0 + 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 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 7796dd77e4..5914e8fbd3 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -680,19 +680,30 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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). if self.autocal_enabled and self.autocal is not None: - ws = CS.out.wheelSpeeds - ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) - # Human-turn and stall-blip frames never reach here (their branches early-return - # after idling the pipeline), so those flags are not passed — they'd be dead False. - committed = self.autocal.update(v_ego, kappa_cmd, current_curvature, - CS.out.steeringPressed, - self.bp_angle_rate_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) + # Measurement-chain warmup gate: kappa_meas comes through the vehicle model with + # liveParameters' angle offset / steer ratio, and the same locationd stack that + # feeds them also estimates the actuation delay. Until lagd reports 'estimated', + # those inputs are defaults/converging — evidence collected then is not comparing + # the command against a trustworthy measurement. Idle (not pause): staged samples + # and peak windows must not straddle the unestimated period. Status is effectively + # monotonic within a drive, so this costs only the warmup minutes. + if str(self.sm['liveDelay'].status) != "estimated": + self.autocal.idle() + committed = [] + else: + ws = CS.out.wheelSpeeds + ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + # Human-turn and stall-blip frames never reach here (their branches early-return + # after idling the pipeline), so those flags are not passed — they'd be dead False. + committed = self.autocal.update(v_ego, kappa_cmd, current_curvature, + CS.out.steeringPressed, + self.bp_angle_rate_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) if committed: self._autocal_dirty = True rec = self.autocal.recommend(self.low_speed_curv_factor, self.high_speed_curv_factor) 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 index 32a092cffd..6c1f735fc5 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -131,6 +131,22 @@ def test_resets_on_any_flag(self): 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): From 771d8277447d27267b5632745f4dad642047c13c Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 16:22:45 -0400 Subject: [PATCH 10/17] =?UTF-8?q?ford=20angle=20mode:=20gain=20model=20mov?= =?UTF-8?q?es=20to=20values=5Fext=20=E2=80=94=20strategy=20owns=20it,=20ca?= =?UTF-8?q?librator=20consumes=20(review=205.2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit GAIN_CAN / GAIN_CANFD_BOF / GAIN_CANFD_SUV, the per-platform CAR sets, platform_gains(), and the V_LOW / V_HIGH / LOW_ANCHOR_BASE anchors now live in values_ext.py. Kills the inverted dependency (strategy re-importing its own gain model out of the calibrator through a noqa'd second import block) and the inline duplicate of platform_gains() in update_angle_params. The strategy's gain interp now uses the named anchors instead of hardcoded 13.5/26.82/1.30 — one source for the numbers the calibrator's fit is expressed against. Pure refactor: 37 tests pass, route 0000000b analyzer replay byte-identical. Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/angle_autocal.py | 39 +------------------ .../sunnypilot/car/ford/lateral_angle_ext.py | 28 ++++--------- .../opendbc/sunnypilot/car/ford/values_ext.py | 39 +++++++++++++++++++ 3 files changed, 49 insertions(+), 57 deletions(-) diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py index 50601b67cd..7e0e366a73 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -32,43 +32,8 @@ """ import math -from opendbc.car.ford.values import CAR - -# Hard-coded per-platform gain defaults (moved here from lateral_angle_ext so this module -# and the offline analyzer share one source; lateral_angle_ext imports them back). -# 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, -}) - - -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 gain interpolation in lateral_angle_ext (m/s: ~30 mph and ~60 mph). -V_LOW = 13.5 -V_HIGH = 26.82 -# Fixed multiplier on the low-speed anchor in lateral_angle_ext. -LOW_ANCHOR_BASE = 1.30 +# 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 # 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 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 5914e8fbd3..3350e8885c 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -28,23 +28,14 @@ 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 AutoCalPipeline 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). Single source lives in -# angle_autocal.py so the offline analyzer and the auto-calibrator share them. -from opendbc.sunnypilot.car.ford.angle_autocal import ( # noqa: E402 - GAIN_CAN as _GAIN_CAN, - GAIN_CANFD_BOF as _GAIN_CANFD_BOF, - GAIN_CANFD_SUV as _GAIN_CANFD_SUV, - CANFD_BOF_CARS as _CANFD_BOF_CARS, - CANFD_SUV_CARS as _CANFD_SUV_CARS, -) - # DBC ``LatCtlPath_An_Actl`` (rad) — panda safety uses the same in ``ford.h``; PSCM enforces in firmware. FORD_DBC_PATH_ANGLE_MIN = -0.5 @@ -218,12 +209,7 @@ 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"): @@ -563,8 +549,10 @@ 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)]) + 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 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]) 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 From cadc34499728ab05132207ffaf8583af49159872 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 16:32:13 -0400 Subject: [PATCH 11/17] =?UTF-8?q?ford=20angle=20auto-cal:=20extract=20Auto?= =?UTF-8?q?CalController=20=E2=80=94=20lifecycle=20out=20of=20the=20strate?= =?UTF-8?q?gy=20(review=20P4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New angle_autocal_controller.py owns everything between the pure estimator and the car: params handle, arm/disarm from the toggle, JSON evidence persistence + save cadence, user-edit debounce, nudge writes, the liveDelay warmup gate, error reporting, and the ground-truth telemetry status. FordLateralAngleExt shrinks to three calls — poll_params() at its 1 Hz cadence, feed() per lateral frame, idle() on inactive branches — plus two read-only properties (autocal_enabled, bp_autocal_status) preserving the bp_card_publisher surface. feed() returns a nudged factor pair and the strategy adopts it, so the write path to the live steering values stays in exactly one place. Removes from the strategy: 9 instance attributes, 3 _autocal_* methods, 2 module-level helpers, the module json dependency, and both inline lifecycle blocks. The estimator (angle_autocal.py) stays pure math, untouched. Glue tests target the controller surface. 37 tests pass on Windows; the TestOnboardGlue suite runs in the WSL gate. Route 0000000b analyzer replay byte-identical. Co-Authored-By: Claude Fable 5 --- .../car/ford/angle_autocal_controller.py | 223 ++++++++++++++++ .../sunnypilot/car/ford/lateral_angle_ext.py | 247 ++++-------------- .../car/ford/tests/test_angle_autocal.py | 49 ++-- 3 files changed, 292 insertions(+), 227 deletions(-) create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py 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..a2997f91af --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py @@ -0,0 +1,223 @@ +"""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 + +SAVE_PERIOD_S = 30.0 + + +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 # ("x.xx", "x.xx") 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 = (f"{low_factor:.2f}", f"{high_factor:.2f}") + 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. + cur = (f"{low_factor:.2f}", f"{high_factor:.2f}") + if self._last_written is not None and cur != self._last_written: + if self._edit_pending: + self.pipeline.user_edit() + self._last_written = cur + 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, 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, + low_factor: float, high_factor: float, 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(v_ego, kappa_cmd, kappa_meas, + steering_pressed, angle_rate_limited, deviation_limited, + saturated=saturated, driver_torque=driver_torque, + a_ego=a_ego, ws_spread=ws_spread, + low_factor=low_factor, high_factor=high_factor) + if committed: + self._dirty = True + applied = (low_factor, high_factor) + out = None + rec = self.pipeline.recommend(low_factor, 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 = (f"{low_new:.2f}", f"{high_new:.2f}") + 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/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 3350e8885c..f960919acc 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -21,7 +21,6 @@ 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 json import numpy as np from numpy import clip, interp @@ -29,7 +28,7 @@ from opendbc.car import DT_CTRL from opendbc.car.lateral import apply_std_steer_angle_limits from opendbc.car.ford.values import CarControllerParams -from opendbc.sunnypilot.car.ford.angle_autocal import AutoCalPipeline +from opendbc.sunnypilot.car.ford.angle_autocal_controller import AutoCalController from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralResult from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector from opendbc.sunnypilot.car.ford.values_ext import (BP_ANGLE_LIMITS, platform_gains, @@ -42,34 +41,6 @@ FORD_DBC_PATH_ANGLE_MAX = 0.5235 # Auto-cal state persistence cadence: losing a save costs at most this much evidence. -_AUTOCAL_SAVE_PERIOD_S = 30.0 - - -def _autocal_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 _autocal_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 # PSCM d_ref (m) vs speed (m/s) — 6 points; above ~55.6 m/s use plateau + optional cap to 5 m. @@ -186,21 +157,11 @@ 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 (see angle_autocal.py). - # Enabled by FordAngleAutoCal. FordAngleAutoCalState carries the estimator's serialized - # evidence as JSON (phase "collecting") so collection spans drives and ignition cycles; - # phase "locked" (or a legacy "done ..." string) means calibrated — the calibrator stays - # off until the user toggles the setting off (clearing the state) and on again. - self.autocal_enabled = False - self.autocal_done = True # conservative until params are read - self.autocal = None # AutoCalPipeline while collecting - self._autocal_param_ctr = 100 # >= threshold so the very first call reads params - self._autocal_params_handle = None - self._autocal_last_written = None # ("x.xx", "x.xx") the nudger last wrote; edits differ - self._autocal_edit_pending = False # user edit needs 2 consecutive ticks (async put lag) - self._autocal_save_s = 0.0 - self._autocal_dirty = False - self.bp_autocal_status = "" # live ground-truth status, published in telemetry + # 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) # 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 @@ -234,58 +195,25 @@ def update_angle_params(self, params): if self._autocal_param_ctr >= 100: self._autocal_param_ctr = 0 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.autocal is None: - self.autocal_done = _autocal_state_locked(state) - else: - self.autocal_done = self.autocal.locked - self.autocal_enabled = enabled and not self.autocal_done - if self.autocal_enabled and self.autocal is None: - # Arm: build the pipeline and restore serialized evidence from a prior drive. - # The currently applied factors (just read above) are the nudge baseline. - self.autocal = AutoCalPipeline(self.path_angle_gain_highC_highV, dt=_STEER_DT) - _autocal_restore(self.autocal, state) - self._autocal_last_written = (f"{self.low_speed_curv_factor:.2f}", - f"{self.high_speed_curv_factor:.2f}") - self._autocal_edit_pending = False - elif not self.autocal_enabled: - self.autocal = 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 read into the live factors); evidence is soft-reset, not wiped. - cur = (f"{self.low_speed_curv_factor:.2f}", f"{self.high_speed_curv_factor:.2f}") - if self._autocal_last_written is not None and cur != self._autocal_last_written: - if self._autocal_edit_pending: - self.autocal.user_edit() - self._autocal_last_written = cur - self._autocal_edit_pending = False - self._autocal_dirty = True - else: - self._autocal_edit_pending = True - else: - self._autocal_edit_pending = False - self._autocal_params_handle = params - # Live status for telemetry: published from the EXT's actual 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.autocal_done: - self.bp_autocal_status = "locked" - elif not self.autocal_enabled: - self.bp_autocal_status = "off" - else: - est = self.autocal.est - self.bp_autocal_status = (f"armed n={est.n} w={est.weight_low:.0f}/{est.weight_high:.0f}" - f" applied={self.low_speed_curv_factor:.2f}/{self.high_speed_curv_factor:.2f}" - f" nudges={self.autocal.nudges}") - except Exception as e: - self.autocal_enabled = False - self.bp_autocal_status = f"tick error: {type(e).__name__}: {e}"[:200] - self._autocal_error(self.bp_autocal_status) + raw_strength = params.get("FordAngleSmoothStrength", return_default=True) + if raw_strength is not None and raw_strength != b"": + _menu = float(clip(float( + raw_strength.decode("utf-8", errors="replace") if isinstance(raw_strength, bytes) else raw_strength), + _SM_MENU_MIN, _SM_MENU_MAX)) + 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): """ @@ -332,8 +260,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.press_timer_s = 0.0 self.precision_type = 1 self.bp_angle_saturated = False - if self.autocal is not None: # steady-state timer and staged samples must not span disengagements - self.autocal.idle() + self.autocal_ctl.idle() # steady-state timer and staged samples must not span disengagements return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -378,8 +305,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.press_timer_s = 0.0 self.precision_type = 1 self.bp_angle_saturated = False - if self.autocal is not None: # steady-state timer and staged samples must not span disengagements - self.autocal.idle() + self.autocal_ctl.idle() # steady-state timer and staged samples must not span disengagements return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -425,8 +351,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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. - if self.autocal is not None: - self.autocal.idle() + self.autocal_ctl.idle() if self.stall_blip_frames_left <= 0: self.stall_blip_cooldown_s = _STALL_COOLDOWN_S return LateralResult( @@ -667,45 +592,23 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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). - if self.autocal_enabled and self.autocal is not None: - # Measurement-chain warmup gate: kappa_meas comes through the vehicle model with - # liveParameters' angle offset / steer ratio, and the same locationd stack that - # feeds them also estimates the actuation delay. Until lagd reports 'estimated', - # those inputs are defaults/converging — evidence collected then is not comparing - # the command against a trustworthy measurement. Idle (not pause): staged samples - # and peak windows must not straddle the unestimated period. Status is effectively - # monotonic within a drive, so this costs only the warmup minutes. - if str(self.sm['liveDelay'].status) != "estimated": - self.autocal.idle() - committed = [] - else: - ws = CS.out.wheelSpeeds - ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) - # Human-turn and stall-blip frames never reach here (their branches early-return - # after idling the pipeline), so those flags are not passed — they'd be dead False. - committed = self.autocal.update(v_ego, kappa_cmd, current_curvature, - CS.out.steeringPressed, - self.bp_angle_rate_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) - if committed: - self._autocal_dirty = True - rec = self.autocal.recommend(self.low_speed_curv_factor, self.high_speed_curv_factor) - if rec is not None: - self._autocal_apply_nudge(rec) - if self.autocal.locked: - self._autocal_save("locked") - self.autocal_done = True - self.autocal_enabled = False - self.autocal = None - else: - self._autocal_save_s += _STEER_DT - if self._autocal_dirty and self._autocal_save_s >= _AUTOCAL_SAVE_PERIOD_S: - self._autocal_save("collecting") + ws = CS.out.wheelSpeeds + ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) + # 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. + nudged = self.autocal_ctl.feed(v_ego, kappa_cmd, current_curvature, + CS.out.steeringPressed, + self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, + self.bp_angle_saturated, + float(CS.out.steeringTorque), float(CS.out.aEgo), + max(ws_vals) - min(ws_vals), + self.low_speed_curv_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, @@ -717,63 +620,3 @@ def update_angle_strategy(self, CC, CS, actuators, CP): lateralUncertainty=lateral_uncertainty, ) - def _autocal_apply_nudge(self, rec): - """Write a nudged factor pair to the params (the lateral tuning menu shows them move) - and apply in-memory immediately so this very frame steers with the new gain. - - 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._autocal_params_handle is None: - return - try: - self._autocal_params_handle.put("FordLowSpeedFactor_ang", float(low_new)) - self._autocal_params_handle.put("FordHighSpeedFactor_ang", float(high_new)) - except Exception as e: - self._autocal_error(f"nudge write failed: {type(e).__name__}: {e}") - return - self.low_speed_curv_factor = float(low_new) - self.high_speed_curv_factor = float(high_new) - self._autocal_last_written = (f"{low_new:.2f}", f"{high_new:.2f}") - self._autocal_edit_pending = False - self._autocal_save("collecting") - - def _autocal_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._autocal_params_handle is not None: - self._autocal_params_handle.put("FordAngleAutoCalError", f"{msg[:300]}") - except Exception: - pass - - def _autocal_save(self, phase: str): - """Serialize the pipeline into FordAngleAutoCalState (JSON). Async put is fine: - a lost final write costs at most _AUTOCAL_SAVE_PERIOD_S of evidence.""" - if self._autocal_params_handle is None or self.autocal is None: - return - d = { - "v": 1, - "phase": phase, - "pipe": self.autocal.to_dict(), - "applied": {"low": round(self.low_speed_curv_factor, 2), - "high": round(self.high_speed_curv_factor, 2)}, - } - sol = self.autocal.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.autocal.stable_s, 1) - try: - self._autocal_params_handle.put("FordAngleAutoCalState", json.dumps(d, separators=(",", ":"))) - except Exception as e: - self._autocal_error(f"state save failed: {type(e).__name__}: {e}") - return - self._autocal_save_s = 0.0 - self._autocal_dirty = False 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 index 6c1f735fc5..7beeb0ffeb 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -566,15 +566,15 @@ 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 is None and not ext.autocal_enabled + 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 is not None - assert ext._autocal_last_written == ("1.10", "0.95") + 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) @@ -584,80 +584,79 @@ def test_arming_restores_serialized_evidence(self): p = _MockParams({"FordAngleAutoCal": 1, "FordAngleAutoCalState": state, "FordLowSpeedFactor_ang": "1.00", "FordHighSpeedFactor_ang": "1.00"}) ext.update_angle_params(p) - assert ext.autocal is not None - assert ext.autocal.est.n == donor.est.n - assert ext.autocal.est.solve() == donor.est.solve() + 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 is None and ext.autocal_done and not ext.autocal_enabled + 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 is None and ext.autocal_done and not ext.autocal_enabled + 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 is not None and ext.autocal.est.n == 0 + 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) - ext._autocal_apply_nudge((1.02, 1.15)) + 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} - assert ext.low_speed_curv_factor == 1.02 and ext.high_speed_curv_factor == 1.15 # Our own write must NOT read back as a user edit. self._tick(ext, p, n=2) - assert ext.autocal is not None and not ext._autocal_edit_pending + 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.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) - w0 = ext.autocal.est.s_w + 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_edit_pending and abs(ext.autocal.est.s_w - w0) < 1e-9 + 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_edit_pending - assert abs(ext.autocal.est.s_w - 0.5 * w0) < 1e-9 # soft reset, not a wipe - assert ext._autocal_last_written == ("1.08", "1.00") + 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.est, 1.05, 1.05, speeds=[10, 28], n_per_speed=200) - sol = ext.autocal.est.solve() - ext._autocal_save("collecting") + 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 is not None - assert ext2.autocal.est.solve() == sol + 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 is not None + assert ext.autocal_ctl.pipeline is not None p.values["FordAngleAutoCal"] = 0 self._tick(ext, p, n=1) - assert ext.autocal is None and not ext.autocal_enabled + assert ext.autocal_ctl.pipeline is None and not ext.autocal_enabled From f7acbda7f67151368920eadb6eb5948a93f2a1ef Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 16:42:49 -0400 Subject: [PATCH 12/17] ford angle auto-cal: review 5.3 should-fix sweep MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Frame dataclass (frozen, no defaults): AutoCalPipeline.update() and AutoCalController.feed() take one Frame instead of a 12-parameter clump the onboard hook and offline analyzer had to keep in positional agreement; defaults on physical signals (a_ego, saturated) were a silent-wrong-answer risk and are gone from the production API. - Admission predicate computed once: SteadyStateGate exposes frame_clear (and grip_this_frame); the pipeline's apex-path frame_ok reads it back instead of maintaining a hand-synced copy of the grip/cooldown/flag logic. - PeakMatcher ring buffer entries are a NamedTuple (_PeakFrame) — no more magic f[0]/f[1]/f[4] indices or unpack-repack in poison_recent(). - User-edit detection compares floats with EDIT_TOL (half the 0.01 menu granularity) instead of round-tripping through %.2f strings; the two-tick debounce stays (independently justified by async put lag). - bp_card_publisher: the param-snapshot copies of bmsAngleAutoCalibrate/State are gone — they were dead (overwritten from the live controller every publish) and a second source of truth by construction. - safety.c debug accessors: the reported idx <= COUNT off-by-one is a FALSE POSITIVE — the table is sized COUNT+1 with slot 0 as the sentinel and real rows 1..COUNT (index rides bits 1-4 of the safety param; ford.h bounds-checks index == 0 || index > COUNT the same way). Verified by tightening the check and watching test_ford's geometry test fail on the last row. Documented at the accessors so it doesn't get 'fixed' again. (gate.reset() from this list already shipped with the P1 fix.) 37 tests pass; route 0000000b analyzer replay byte-identical. Co-Authored-By: Claude Fable 5 --- bluepilot/selfdrive/car/bp_card_publisher.py | 5 +- .../opendbc/safety/tests/libsafety/safety.c | 4 + .../sunnypilot/car/ford/angle_autocal.py | 104 ++++++++++++------ .../car/ford/angle_autocal_controller.py | 33 +++--- .../sunnypilot/car/ford/lateral_angle_ext.py | 28 ++--- .../car/ford/tests/test_angle_autocal.py | 40 ++++--- 6 files changed, 127 insertions(+), 87 deletions(-) diff --git a/bluepilot/selfdrive/car/bp_card_publisher.py b/bluepilot/selfdrive/car/bp_card_publisher.py index d2efe8531a..7c135062bb 100644 --- a/bluepilot/selfdrive/car/bp_card_publisher.py +++ b/bluepilot/selfdrive/car/bp_card_publisher.py @@ -104,8 +104,9 @@ def _refresh_settings_cache() -> dict: "bmsMinimumSpeedToPauseLaneChange": _get_int(p, "BlinkerMinLateralControlSpeed", 20), "bmsShowLateralControlMode": _get_bool(p, "BpShowLateralControl"), # --- Angle Tuning --- - "bmsAngleAutoCalibrate": _get_bool(p, "FordAngleAutoCal"), - "bmsAngleAutoCalState": _get_str(p, "FordAngleAutoCalState"), + # 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), 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 index 7e0e366a73..8b63dcc64a 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal.py @@ -31,10 +31,32 @@ - 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 @@ -396,6 +418,15 @@ def idle(self): 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 @@ -409,7 +440,7 @@ def __init__(self, dt: float = 0.05): 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[tuple] = [] # (kappa_cmd, kappa_meas, v, applied_gain, ok) + self.buf: list[_PeakFrame] = [] self._refractory = 0 self._pending: dict[int, list] = {0: [], 1: []} # anchor half -> [(r, sample), ...] self.apexes_seen = 0 @@ -425,14 +456,13 @@ def poison_recent(self, seconds: float): (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)): - k, m, v, g, _ = self.buf[i] - self.buf[i] = (k, m, v, g, False) + 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((kappa_cmd, kappa_meas, v_ego, applied_gain, ok)) + 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: @@ -441,16 +471,17 @@ def push(self, kappa_cmd: float, kappa_meas: float, v_ego: float, return [] c = self.c - k_c, _, v_c, g_c, _ = self.buf[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[4] for f in window): + if not all(f.clean for f in window): return [] - mags = [abs(f[0]) for f in window] + 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] @@ -467,10 +498,10 @@ def push(self, kappa_cmd: float, kappa_meas: float, v_ego: float, 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[4]: + if not f.clean: return [] # disturbance inside the match window: the pair is unusable - if f[1] * sign > m_pk: - m_pk = f[1] * sign + 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 @@ -502,6 +533,9 @@ def __init__(self, dt: float = 0.05): 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 @@ -510,6 +544,8 @@ def reset(self): 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, @@ -520,15 +556,18 @@ def update(self, lat_active: bool, kappa_cmd: float, steering_pressed: bool, # 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) - ok = (lat_active and not grip and self.grip_cooldown_s <= 0.0 - and not angle_rate_limited and not deviation_limited - and not saturated - and abs(kappa_cmd) >= MIN_KAPPA) + # 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 @@ -588,32 +627,30 @@ def idle(self): self._staged.clear() self._meas_last = None - def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, - steering_pressed: bool, angle_rate_limited: bool, deviation_limited: bool, - saturated: bool = False, driver_torque: float = 0.0, - a_ego: float = 0.0, ws_spread: float | None = None, - low_factor: float = 1.0, high_factor: float = 1.0) -> list: - """Advance one frame. low_factor/high_factor are the values currently steering the - car — each committed sample records the gain that produced it. Returns the samples + 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 [] - - grip = steering_pressed or abs(driver_torque) > TORQUE_GUARD_NM - if grip: + 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=a_ego, ws_spread=ws_spread) + 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 = self.gate.update(True, kappa_cmd, steering_pressed, - angle_rate_limited, deviation_limited, - saturated=saturated, driver_torque=driver_torque) eligible = eligible and q_ok # The CAR must be settled too, not just the command: during closed-loop compensation @@ -631,7 +668,7 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, self.quality.counters["limit"] += 1 eligible = False - gain_now = self.applied_gain(v_ego, low_factor, high_factor) + 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 = [] @@ -649,10 +686,9 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, 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. - frame_ok = (q_ok and not grip and self.gate.grip_cooldown_s <= 0.0 - and not angle_rate_limited and not deviation_limited - and not saturated and margin_w > 0.0) + # 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): @@ -670,7 +706,7 @@ def update(self, v_ego: float, kappa_cmd: float, kappa_meas: float, 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 - low_factor) <= LOCK_DEADBAND and abs(high_t - high_factor) <= LOCK_DEADBAND) + 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: diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py index a2997f91af..0fed735e3c 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/angle_autocal_controller.py @@ -15,9 +15,11 @@ """ import json -from opendbc.sunnypilot.car.ford.angle_autocal import AutoCalPipeline +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: @@ -55,7 +57,7 @@ def __init__(self, dt: float): self.pipeline = None # AutoCalPipeline while collecting self.status = "" # live ground-truth status, published in telemetry self._params = None - self._last_written = None # ("x.xx", "x.xx") the nudger last wrote; edits differ + 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 @@ -79,7 +81,7 @@ def poll_params(self, params, low_factor: float, high_factor: float, platform_ga # The currently applied factors are the nudge baseline. self.pipeline = AutoCalPipeline(platform_gain_high, dt=self.dt) _restore(self.pipeline, state) - self._last_written = (f"{low_factor:.2f}", f"{high_factor:.2f}") + self._last_written = (float(low_factor), float(high_factor)) self._edit_pending = False elif not self.enabled: self.pipeline = None @@ -88,11 +90,13 @@ def poll_params(self, params, low_factor: float, high_factor: float, platform_ga # 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. - cur = (f"{low_factor:.2f}", f"{high_factor:.2f}") - if self._last_written is not None and cur != self._last_written: + 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 = cur + self._last_written = (float(low_factor), float(high_factor)) self._edit_pending = False self._dirty = True else: @@ -123,10 +127,7 @@ def idle(self): if self.pipeline is not None: self.pipeline.idle() - def feed(self, 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, - low_factor: float, high_factor: float, delay_estimated: bool): + 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: @@ -138,16 +139,12 @@ def feed(self, v_ego: float, kappa_cmd: float, kappa_meas: float, # samples and peak windows must not straddle the unestimated period. self.idle() return None - committed = self.pipeline.update(v_ego, kappa_cmd, kappa_meas, - steering_pressed, angle_rate_limited, deviation_limited, - saturated=saturated, driver_torque=driver_torque, - a_ego=a_ego, ws_spread=ws_spread, - low_factor=low_factor, high_factor=high_factor) + committed = self.pipeline.update(frame) if committed: self._dirty = True - applied = (low_factor, high_factor) + applied = (frame.low_factor, frame.high_factor) out = None - rec = self.pipeline.recommend(low_factor, high_factor) + 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 @@ -180,7 +177,7 @@ def _apply_nudge(self, rec, applied) -> bool: except Exception as e: self._error(f"nudge write failed: {type(e).__name__}: {e}") return False - self._last_written = (f"{low_new:.2f}", f"{high_new:.2f}") + self._last_written = (float(low_new), float(high_new)) self._edit_pending = False self._save("collecting", rec) return True 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 f960919acc..c1f9a0eff4 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -28,6 +28,7 @@ from opendbc.car import DT_CTRL from opendbc.car.lateral import apply_std_steer_angle_limits from opendbc.car.ford.values import CarControllerParams +from opendbc.sunnypilot.car.ford.angle_autocal import Frame from opendbc.sunnypilot.car.ford.angle_autocal_controller import AutoCalController from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralResult from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector @@ -162,6 +163,7 @@ def __init__(self, CP=None, CP_SP=None): # nudge writes, save cadence, errors, telemetry status) lives in AutoCalController — # this class only routes frames and adopts returned nudges. self.autocal_ctl = AutoCalController(dt=_STEER_DT) + self._autocal_param_ctr = 100 # >= threshold so the very first call reads params # Telemetry + autocal gate: the command this frame was modified by PSCM authority # limits or the DBC clamp — the car could not make the requested turn. self.bp_angle_saturated = False @@ -194,14 +196,6 @@ def update_angle_params(self, params): self._autocal_param_ctr += 1 if self._autocal_param_ctr >= 100: self._autocal_param_ctr = 0 - try: - raw_strength = params.get("FordAngleSmoothStrength", return_default=True) - if raw_strength is not None and raw_strength != b"": - _menu = float(clip(float( - raw_strength.decode("utf-8", errors="replace") if isinstance(raw_strength, bytes) else raw_strength), - _SM_MENU_MIN, _SM_MENU_MAX)) - 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) @@ -598,14 +592,16 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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. - nudged = self.autocal_ctl.feed(v_ego, kappa_cmd, current_curvature, - CS.out.steeringPressed, - self.bp_angle_rate_limited, self.bp_curvature_deviation_limited, - self.bp_angle_saturated, - float(CS.out.steeringTorque), float(CS.out.aEgo), - max(ws_vals) - min(ws_vals), - self.low_speed_curv_factor, self.high_speed_curv_factor, - delay_estimated=str(self.sm['liveDelay'].status) == "estimated") + 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]) 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 index 7beeb0ffeb..5f1e98f29a 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_autocal.py @@ -6,6 +6,7 @@ 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, @@ -293,9 +294,9 @@ def run_pipeline(pipe, n, torque=0.0, pressed=False, saturated=False, kappa=0.00 low=1.0, high=1.0): committed = [] for _ in range(n): - committed += pipe.update(v, kappa, kappa, pressed, False, False, - saturated=saturated, driver_torque=torque, - low_factor=low, high_factor=high) + committed += pipe.update(_frame(v, kappa, kappa, pressed=pressed, + saturated=saturated, torque=torque, + low=low, high=high)) return committed @@ -312,7 +313,7 @@ def test_grip_cancels_staged_samples(self): 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(20.0, 0.002, 0.002, True, False, False) # grip + 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 @@ -322,7 +323,7 @@ def test_disturbance_cancels_staged_samples(self): 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(20.0, 0.002, 0.002 + SPIKE_MEAS_RATE * DT * 2, False, False, False) + 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) @@ -346,7 +347,7 @@ def test_unsettled_measurement_not_staged(self): kappa_meas = 0.0010 staged_during_sweep = 0 for _ in range(warm): - pipe.update(20.0, 0.002, kappa_meas, False, False, False) + 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 @@ -383,8 +384,7 @@ def _evidenced_pipe(self, true_low=1.10, true_high=1.10, applied=(1.0, 1.0), wei pipe.gate.steady_s = 0.0 pipe._meas_last = None for _ in range(n): - pipe.update(v, kappa, kappa * r, False, False, False, - low_factor=applied[0], high_factor=applied[1]) + pipe.update(_frame(v, kappa, kappa * r, low=applied[0], high=applied[1])) return pipe def test_nudges_toward_target_bounded(self): @@ -405,8 +405,7 @@ def test_rate_limited(self): 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(10.0, 0.004, 0.004, False, False, False, - low_factor=1.02, high_factor=1.02) + 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): @@ -421,8 +420,7 @@ def test_per_drive_cap(self): 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(10.0, 0.004, 0.004 * r, False, False, False, - low_factor=applied[0], high_factor=applied[1]) + 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]) @@ -468,9 +466,8 @@ def _drive(self, pipe, applied, seconds, v, kappa_amp, rng): 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(v, k_cmd, meas, False, False, False, - driver_torque=grip, a_ego=0.1, - low_factor=applied[0], high_factor=applied[1]) + 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) @@ -505,6 +502,15 @@ def test_converges_and_locks(self): 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. @@ -574,7 +580,7 @@ def test_arming_builds_pipeline_with_baseline(self): "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") + assert ext.autocal_ctl._last_written == (1.10, 0.95) def test_arming_restores_serialized_evidence(self): donor = AutoCalPipeline(PLATFORM_GAIN_HIGH) @@ -635,7 +641,7 @@ def test_user_edit_adopted_after_two_ticks(self): 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") + assert ext.autocal_ctl._last_written == (1.08, 1.00) def test_save_restore_round_trip_through_param(self): ext = self._ext() From cddc5886919debc6d10b9f6281dd20cbac4d1652 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 17:05:23 -0400 Subject: [PATCH 13/17] docs: Ford angle-mode auto-calibration user guide Plain-language guide for drivers: what the calibrator does, enabling it on both device layouts + sunnylink, what to expect and how long it takes (honest numbers from the on-road lock drive), manual +/- interaction, locking and recalibration, hard behavior bounds, troubleshooting, and the offline analyzer for the curious. Co-Authored-By: Claude Fable 5 --- docs/ford-angle-autocal.md | 130 +++++++++++++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 docs/ford-angle-autocal.md 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. From 942fc8670831838fbf116b1b8ce5c27bf46f0dd9 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 17:32:37 -0400 Subject: [PATCH 14/17] ford angle auto-cal: only build the evidence Frame while armed MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The P4 port left Frame construction (and its wheel-speed/torque/accel reads) running unconditionally every lateral frame — including for the majority of drivers who never enable the calibrator, and controller.feed() then discarded it. Caught by the closed-loop smoothing rig downstream, whose minimal CarState mock had no reason to carry wheelSpeeds for a disabled feature. Gated on autocal_ctl.enabled: disabled cost is one attribute check. Co-Authored-By: Claude Fable 5 --- .../sunnypilot/car/ford/lateral_angle_ext.py | 35 ++++++++++--------- 1 file changed, 19 insertions(+), 16 deletions(-) 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 c1f9a0eff4..8b55b77d26 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -586,25 +586,28 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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). - ws = CS.out.wheelSpeeds - ws_vals = (float(ws.fl), float(ws.fr), float(ws.rl), float(ws.rr)) # 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. - 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]) + # 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, From 46b68abbf12ab3347b3fbbea533be51b73de970b Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 17:35:11 -0400 Subject: [PATCH 15/17] sunnylink: expose the auto-cal toggle Same title/description/angle-mode gating as the on-device toggles. Co-Authored-By: Claude Fable 5 --- sunnypilot/sunnylink/settings_ui.json | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 20c4782734..bf1dc8a1f3 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -2566,6 +2566,19 @@ } ] }, + { + "key": "FordAngleAutoCal", + "widget": "toggle", + "title": "[Lateral Tuning] Auto-Calibrate Adjustment Factors", + "description": "Learns the low/high speed factors by comparing requested and actual turn, nudging them live until locked. Manual +/- always wins. Toggle off and back on to recalibrate.", + "visibility": [ + { + "type": "param", + "key": "FordPrefLateralControl", + "equals": 1 + } + ] + }, { "key": "lane_change_factor_high_ang", "widget": "option", From c20343393be8c83f530d12280524b2d6cc494238 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 17:36:33 -0400 Subject: [PATCH 16/17] ford angle mode: anti-weave smoothing of the command path (FordAngleSmoothing + strength) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Removes the rhythmic left-right centering motion some angle-mode Fords show on straight roads. Log analysis traced it to a closed loop: curvature noise crosses the gain-schedule interp's 0.0003-wide transition band, the crossings modulate loop gain 1.0<->1.2 every cycle (0.23 Hz limit cycle measured), and the PSCM integrates near-zero LSB toggling (962/min) into felt dither. Five input-side elements, all in one pure unit-tested module (angle_smoothing.AngleSmoother): - asymmetric filter on |kappa| feeding the gain schedule — fast fixed attack (entry speed strength-independent), slow strength-scaled release (the primary fix: removes the self-modulation) - low-pass on the model predicted curvature (inside the VLT's slack) - slew on the exit-blend ratio (bounded ramps, no 4x steps from boolean chatter) - hysteresis on the curve-entering decision - 1-LSB hold on the outgoing wire value Strength is a menu dial with factor semantics: 1.0 = stock (bit-identical to the feature not existing — fuzz-tested), 2.0 = tuned, 2.5 = max. Defaults ship neutral: toggle ON at strength 1.0 means stock until the driver steps up. Output-side lag was explicitly tried and rejected: the closed-loop rig measured it degrading station-keeping 2.5x. Every filter resets across disengage/human-turn/stall-blip discontinuities, and each element re-seeds from the live value when inactive — toggling or stepping strength mid-drive can never act on stale state. On-road A/B (same car): slow lane sway ±0.46 -> ±0.26 m (−43%), dither sway −33%; curve exits measured faster and cleaner with smoothing on. Closed-loop cost at tuned strength: +3 mm station-keeping std. Params, both settings surfaces (angle-mode gated), sunnylink entries, 13 pure unit tests + closed-loop ext tests. Co-Authored-By: Claude Fable 5 --- common/params_keys.h | 2 + .../sunnypilot/car/ford/angle_smoothing.py | 154 +++++++++++++ .../sunnypilot/car/ford/lateral_angle_ext.py | 64 +++++- .../car/ford/tests/test_angle_smoothing.py | 158 +++++++++++++ .../car/ford/tests/test_lateral_angle_ext.py | 207 ++++++++++++++++++ selfdrive/ui/bp/layouts/settings/bluepilot.py | 28 +++ .../bp/mici/layouts/settings/lateral_mici.py | 12 + sunnypilot/sunnylink/settings_ui.json | 29 +++ 8 files changed, 649 insertions(+), 5 deletions(-) create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/angle_smoothing.py create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_angle_smoothing.py diff --git a/common/params_keys.h b/common/params_keys.h index ebfcdfa8ea..7f9f3b150b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -312,6 +312,8 @@ inline static std::unordered_map keys = { {"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/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 8b55b77d26..b8f8e23a5a 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -30,6 +30,7 @@ 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, platform_gains, @@ -43,6 +44,10 @@ # 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) @@ -163,11 +168,31 @@ def __init__(self, CP=None, CP_SP=None): # 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 + 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) @@ -196,6 +221,16 @@ def update_angle_params(self, params): 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) @@ -255,6 +290,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): 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, @@ -300,6 +336,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): 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, @@ -346,6 +383,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # 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( @@ -378,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: @@ -392,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)) @@ -420,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 @@ -473,8 +519,12 @@ def update_angle_strategy(self, CC, CS, actuators, CP): [(LOW_ANCHOR_BASE * 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]) + # 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 @@ -522,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 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/selfdrive/ui/bp/layouts/settings/bluepilot.py b/selfdrive/ui/bp/layouts/settings/bluepilot.py index 7a03847eca..e3a75bccc7 100644 --- a/selfdrive/ui/bp/layouts/settings/bluepilot.py +++ b/selfdrive/ui/bp/layouts/settings/bluepilot.py @@ -79,6 +79,7 @@ def __init__(self): ("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), @@ -545,6 +546,29 @@ def _initialize_items(self): 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"), @@ -628,6 +652,8 @@ def _section(title: str, items: list) -> list: 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")) @@ -876,6 +902,8 @@ def _update_toggles(self, just_toggled: dict | None = None): self._low_speed_curv_factor.action_item.set_enabled(is_angle) self._high_speed_curv_factor.action_item.set_enabled(is_angle) self._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) diff --git a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py index a2f9ecc8bf..44446b0403 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py +++ b/selfdrive/ui/bp/mici/layouts/settings/lateral_mici.py @@ -28,6 +28,13 @@ def __init__(self, back_callback: Callable[[], None] | None = None): "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, ) @@ -76,6 +83,8 @@ def __init__(self, back_callback: Callable[[], None] | None = None): 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, @@ -94,6 +103,7 @@ 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), @@ -124,6 +134,8 @@ def _update_toggles(self): 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 bf1dc8a1f3..e119c6c76d 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -2579,6 +2579,35 @@ } ] }, + { + "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", From efc7b2fafffd8ad226ef1ac88222f352ec9247d2 Mon Sep 17 00:00:00 2001 From: ghbarker <117389362+ghbarker@users.noreply.github.com> Date: Wed, 22 Jul 2026 17:30:50 -0400 Subject: [PATCH 17/17] docs: Ford angle-mode anti-weave smoothing user guide Driver-facing guide matching the auto-calibration one: the measured before/after numbers, the strength dial semantics (1.0 = provably stock), the honest cost table from the closed-loop rig, how to find a setting, hard guarantees, and troubleshooting including the calibrate-first relationship to auto-cal. Co-Authored-By: Claude Fable 5 --- docs/ford-angle-smoothing.md | 111 +++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 docs/ford-angle-smoothing.md 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`).