Ford: use full geometric prediction within existing command limits

Remove the hand-chosen 0.15 m / 25% cap on the prediction adjustment.
Retain the available-horizon bound, nonfinite fallback, total field limits,
yaw damping, slew, input gates, two states and zero C2/C3.

Cover full predictions and geometric countersteering in regression tests.
Validate 374 tests plus 26 subtests, four-route replay and 817,346
Float32/CAN round trips. Document command changes without inferring
physical tracking performance from PSCM limits or fixed-input replay.

Assisted-by: OpenAI Codex

.gitattributes

@@ -9,6 +9,7 @@
 *.ttf filter=lfs diff=lfs merge=lfs -text
 *.otf filter=lfs diff=lfs merge=lfs -text
 *.wav filter=lfs diff=lfs merge=lfs -text
+openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
 
 openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
 openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text

docs/ford_model_action_candidate.md

@@ -0,0 +1,153 @@
+# Offline Ford selected-action candidate
+
+This document and `ford_model_action_validation.json` record the offline
+stage committed as `7ca3c6e3b`. The candidate is now available behind a
+separate default-off Sunnylink toggle; see
+[drive-test setup and validation](ford_model_action_drive_test.md).
+The counts, source hashes and selector status below describe that earlier
+stage, not the subsequent wiring change.
+
+The decision is `C0 = current model y(7 m)`,
+`C1 = max(7 m, speed × 1 s) × selected upstream-limited desiredCurvature`,
+with C2=C3=0. The 7 m station and one-second scale are engineering choices,
+not identified PSCM gains. `calibration_approved=false`.
+
+`openpilot/selfdrive/controls/lib/ford_model_action.py` contains the core
+and a separate adapter compatible with the existing controlsd call.
+At that stage, the production selector, v8 implementation, settings, opendbc
+submodule and Panda safety remained unchanged. Tests injected the adapter
+offline; there was no production setting. No hardware or CAN transmission
+occurs in the lab tools.
+
+## Construction and integration
+
+Only the unquantized C0 and C1 slew positions persist in the core.
+Each field is clipped independently (±5.11 m / ±0.5 rad), slewed independently
+(4 m/s / 0.5 rad/s), then packed using the existing Float32/sign-negation
+rounding contract (0.01 m / 0.0005 rad). Heading overflow is not transferred
+to C0. No yaw integral, blend, additional curvature contribution, reference
+filter, turn modes, or 10 m C1 cap is introduced.
+
+The selected standalone implementation from worktree 3548 is the provenance
+for this law. Its two-state packer has been moved into the library core so
+the controller does not depend on experimental lab code. Invalid numeric
+types, overflowing arc geometry and malformed paths reset the core instead
+of throwing or retaining a command.
+
+Arc stations use cumulative model x/y distance, not forward x. As in the
+reviewed standalone core, a path ending before 7 m holds its available
+endpoint instead of extrapolating. This matters: route95 contains 44 active
+cycles with 5.45–6.94 m of path at 2.78–3.46 m/s. A tested strict 7 m
+coverage gate would have introduced disengagements and was removed. There
+is no speed-dependent C0 horizon beyond this existing endpoint behavior.
+
+The adapter retains the existing input age allowance (−5 to +150 ms),
+speed domain (0.3–55 m/s), yaw sanity bound (±3 rad/s), selected curvature
+sanity bound (±1/m), and control interval (2–100 ms). It rejects backward
+model/measurement timestamps and invalid services. Repeated timestamps may
+continue slew, but geometry is validated again on each tick. Disengagement,
+invalid inputs and timing faults clear all command and adapter timing state.
+The first valid tick after reset uses 10 ms, as v8 does.
+
+controlsd still owns reference selection, upstream curvature limiting,
+service health and engagement. Tests execute its actual source-selection
+and limiter code, its Ford call, Float32 publication in ControlsExt, conversion
+to CarControlSP, and the pinned Ford CarController's in-memory CAN builder.
+Both model-action and maneuver-planner selection are covered, including
+disabling latActive after invalid output. Only the test chooses the adapter.
+
+Yaw is not an input to the control law. The adapter checks it solely for the
+inherited invalid-input policy. Driver override and optional PSCM status
+do not modify the candidate base; existing engagement and downstream driver
+arbitration remain responsible for authorization, as with v8's base request.
+
+## Offline evidence
+
+The checked-in `ford_model_action_validation.json` records the completed
+checks and source hashes. Full arrays and detailed reports are generated
+locally under `.cache/ford_model_action/`; original route files are read-only.
+
+Completed validation: **264 Ford tests and 150 subtests pass**, including
+120 new core/adapter/replay-validator cases. The candidate module has 100%
+statement and branch coverage (78 statements, 24 branches). Ruff and Ty pass.
+The 200,000-cycle numerical stress test also checks 200,000 mirrored core
+updates and 18,138 field-boundary cases. Across route and stress runs,
+485,238 Float32/CAN round trips pass. Eight deliberately injected faults
+(heading gain/cap, erased C0, wrong C0 slew, retained invalid state, stale
+model acceptance, model clock rollback and reversed C0 sign) are all caught
+by the tests. Mutation runs replace code only inside isolated Python
+processes; production source files are never modified by those probes.
+
+Independent Standards and Spec reviews reported zero findings. The full
+suite's Params setting test uses an existing local native library from
+worktree 3548 after checking relevant source files are byte-identical;
+its hash and provenance are in the manifest. That library is an ignored
+test dependency, not part of this change. This is the full relevant Ford
+suite, not the hardware-dependent test suite for every openpilot subsystem.
+
+The replay has two separate passes:
+
+* Core compatibility uses the archived eligibility mask and requires exact
+  equality with the independently implemented `action_heading` commands.
+* Adapter reconstruction derives eligibility from recorded service streams
+  independently of the archived output mask. It retains original timestamps,
+  gaps and consumed model frames. Controls publication time proxies the
+  unlogged computation clock, and complete SubMaster health is unavailable.
+
+All 54,738 route95 and 78,812 route90 core cycles match exactly, including
+37,614 and 73,055 active cycles. The adapter preserves those active counts.
+Its 59 / 19 changed commands arise solely from the fresh 10 ms engagement
+tick instead of the archived harness's preceding publication interval;
+the replay checks that attribution on every cycle. Maximum differences are
+0.01 m / 0.001 rad (95) and 0.02 m / 0.002 rad (90).
+
+Every core and adapter replay output is round-tripped through Float32 and
+the real CAN packer/parser, including zero C2/C3, signs, mode and counter.
+Continuous field slew and quantization allowance are checked separately
+from immediate invalid-command resets. The original driver-clean cohorts,
+speed strata and command RMS are reproduced without redoing the encoder search.
+
+The numerical stress harness uses analytic rotated paths, scalar slew
+arithmetic, mirrored requests, irregular intervals and invalid-input resets.
+It also sweeps every representable host field value and the Float32 values
+immediately below, at and above every half-quantum boundary. Direct CAN
+packing of the continuous state must agree with the host's quantized output.
+The unit tests cover releases, reversals, clipping, service freshness,
+clock resets, malformed inputs, endpoint fallback and actual integration.
+
+## Limits of the result
+
+On turns at ≥15 m/s, candidate C0 RMS is 79%/81% below v8 on routes95/90,
+while C1 is 33%/41% higher. Those are command changes, not evidence of
+equivalent steering authority. The PSCM's independent C0/C1 response remains
+unknown. Replay cannot establish physical model following, strong turns,
+centering, overshoot, oscillation or closed-loop stability.
+
+The release probe is intentionally explicit: a model bend can increase
+while selected curvature decreases. At 20 m/s, one synthetic probe changes
+C0/C1 from 0.24 m / 0.10 rad to 0.49 m / 0.08 rad. Zero selected curvature
+sets the C1 target to zero but does not erase a nonzero current model C0.
+Removing a yaw-integral tail does not prove that physical overshoot is solved.
+No additional release policy or unsupported plant model is added to hide
+that uncertainty.
+
+## Reproduce
+
+From this worktree, use the logged construction dependency explicitly:
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo
+PY=/Users/ibpersonal/dev/sunnypilot/.venv/bin/python
+EVIDENCE=/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904
+$PY -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py
+$PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route95" --output .cache/ford_model_action/route95
+$PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route90" --output .cache/ford_model_action/route90
+$PY -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --output .cache/ford_model_action/stress.json
+```
+
+The route replay refuses an opendbc revision other than
+`72a775d35e54c21ff5c5798acef22016eedcc0a7`. Stress defaults to this pin and
+also accepts an explicitly required commit with `--opendbc-revision` for
+deployment checks. A mismatch still fails. This historical pin reproduces
+logged construction; it does not change the merge's submodule pointer.

docs/ford_model_action_damping.md

@@ -0,0 +1,88 @@
+# Experimental Ford offset damping, v2
+
+This document and its validation counts describe the archived v2 source. The
+[current v4 experiment](ford_model_action_full_prediction.md) uses full path prediction.
+
+Segment 10 of the supplied route9b recording shows measured turning persisting
+as requested right curvature falls. At about 643.0 s, before strong driver
+intervention, device-gyro curvature is approximately 0.01786/m against a
+0.01172/m request. Around 643.9 s, heading demand has reversed slightly but
+C0 still requests approximately +0.12 m into the turn. Strong column input
+starts around 643.852 s; later motion cannot establish autonomous recovery.
+Earlier light driver input also exists.
+
+The outgoing CAN commands match preceding publications. All 70,937 decoded
+frames have zero C2/C3 and valid checksums. Focused exit diagnostics have fresh
+model/carState inputs and targets within normal quantization of the outputs.
+This supports trying less residual C0; it does not identify PSCM dynamics or
+prove C0 alone caused the physical oversteer.
+
+## Change
+
+C0 starts from the clipped current model offset at 7 m. When C0 and measured
+host yaw point in the same direction, compute:
+
+```
+requested_yaw = max(0, sign(C0) * speed * desiredCurvature)
+excess = max(0, sign(C0) * yaw - requested_yaw - 0.02 rad/s)
+reduction = 7 m * 0.2 s * excess
+target = sign(C0) * max(0, abs(C0) - reduction)
+```
+
+Opposing centering demand is unchanged. The correction cannot increase the
+target's magnitude or reverse its sign. Opposite-direction planned curvature
+cannot amplify a small yaw bias into a correction. Existing 4 m/s C0 slew still
+applies; this target bound is not a claim that every stateful output is smaller than a
+separate v1 controller after arbitrary direction reversals. C1 construction,
+clipping and slew are unchanged; C2=C3=0. Only C0/C1 slew states persist.
+There is no integral, model-history filter, turn state machine or fitted plant.
+
+Host yaw is `-carState.yawRate`, as in the existing Ford call path. The
+0.02 rad/s deadband exceeds the approximately 0.008 rad/s offset measured
+against the device gyro on quiet straights. The 0.2 s scale is an initial
+engineering choice, not an identified delay or gain. Both remain physically
+unvalidated. Large biased or noisy yaw within the existing sanity gate can
+still attenuate useful centering; fixed-input replay cannot establish stability.
+
+## Offline evidence
+
+The complete 12-rlog route is replayed at original controls publication times,
+with exact consumed model geometry, causal carState, and carControl matched
+within 5 ms. These times proxy computation; full SubMaster health is unavailable.
+V1 reconstruction is within one field quantum of all 64,701 paired active
+publications. V2 has identical eligibility and exactly identical C1.
+
+On 381.78 seconds of driver-clean low requests above 8 m/s, only 3 of 37,937 cycles
+change C0, each by one 0.01 m quantum. Across the 642.7–643.852 s exit window,
+C0 changes on all 115 cycles, averaging 0.080 m reduction. Entry/peak C0
+maximum stays 2.80 m; some entry-window samples decrease by up to 0.05 m.
+These are command comparisons on recorded inputs, not predicted tracking.
+Low request means requested lateral acceleration below 0.15 m/s²; it is a
+proxy for straight driving and does not establish a physically straight path.
+
+The original routes90/95 also run through v2. Their zero-yaw baseline pass
+checks archived v1 compatibility; the measured-yaw adapter pass checks current
+construction, eligibility, field limits and packing. It is not an exact match
+to v1 or v8. Randomized testing checks the damping against an independent
+piecewise oracle, mirror symmetry, resets and slew, with real Float32/CAN
+round trips. See `ford_model_action_damping_validation.json` for counts and hashes.
+
+Final validation passes 325 tests and 26 subtests, with 100% controller
+statement/branch coverage, 204,946 original route cycles and 628,030 CAN round
+trips. The module is 166 total lines, including 107 code lines excluding
+comments, blanks and docstrings. Standards and Spec reviews have no remaining findings.
+
+## Reproduce
+
+Use the dependency setup and suite command in the [drive-test guide](ford_model_action_drive_test.md).
+The new route replay requires the deployment opendbc pin recorded there:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/complete/rlogs --baseline v1 --candidate v2 --window segment10_entry_peak 637 640 --window segment10_exit_before_strong_input 642.7 643.852 --output /path/to/separate/results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+At the v2 revision, the same default-off Sunnylink toggle selects v2; no additional setting is
+introduced. Updating an installation with the toggle already enabled selects
+v2 at the next controlsd startup. `calibration_approved=false` remains explicit.
+No physical fix, hardware build or device boot is established by these checks.

docs/ford_model_action_damping_validation.json

@@ -0,0 +1,164 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c0-c1-yaw-damping-v2",
+  "scope": "Experimental bounded offset damping; fixed-input offline evidence only.",
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 166,
+    "code_lines_excluding_blanks_comments_docstrings": 107,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "325 passed, 26 subtests passed; no skips",
+    "suite_log_sha256": "69cb8ab40e93e00a9ff7b6ea1933e4c3554336746860efb53dad3b29f94fc9d3",
+    "coverage": {
+      "statements": 98,
+      "branches": 28,
+      "percent": 100.0
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "review_resolutions": [
+      "Prevent opposite-direction planned curvature amplifying small yaw bias; eight new cases failed before the fix and passed after it.",
+      "Relabel requested-acceleration cohort as low request; it does not establish physically straight driving."
+    ],
+    "mutation_probe": "Disabling damping fails all four mirrored recorded-exit cases."
+  },
+  "stress": {
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "bounded_excess_yaw_damping_checked": true,
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "routes": {
+    "route90": {
+      "cycles": 78812,
+      "core_exact_archived_match": true,
+      "adapter_active_cycles": 73055,
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 2280,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.1900000000000004,
+        0.0020000000000000018
+      ],
+      "float32_can_round_trips": 157624,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+    },
+    "route95": {
+      "cycles": 54738,
+      "core_exact_archived_match": true,
+      "adapter_active_cycles": 37614,
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 2051,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.22999999999999998,
+        0.0010000000000000009
+      ],
+      "float32_can_round_trips": 109476,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+    },
+    "route9b": {
+      "cycles": 71396,
+      "eligible_cycles": 64701,
+      "same_validity": true,
+      "c1_exactly_unchanged": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 142792,
+      "v1_reconstruction_vs_recorded": {
+        "paired_cycles": 64701,
+        "within_one_quantum_cycles": 64701,
+        "maximum_absolute_error_c0_c1": [
+          0.010000114440917862,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline": "Current adapter with zero yaw retains v1 targets and actual-yaw sanity gate.",
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "cohorts": {
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 37937,
+          "seconds": 381.77672216900055,
+          "changed_c0_cycles": 3,
+          "mean_absolute_c0_change_m": 8.419774473786704e-07,
+          "max_absolute_c0_change_m": 0.009999999999999787,
+          "v1_peak_absolute_c0_m": 0.08999999999999986,
+          "v2_peak_absolute_c0_m": 0.08999999999999986
+        },
+        "segment10_entry_peak": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 100,
+          "mean_absolute_c0_change_m": 0.007564164795694466,
+          "max_absolute_c0_change_m": 0.04999999999999982,
+          "v1_peak_absolute_c0_m": 2.8000000000000003,
+          "v2_peak_absolute_c0_m": 2.8000000000000003
+        },
+        "segment10_exit_before_strong_input": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 115,
+          "mean_absolute_c0_change_m": 0.0799313220721162,
+          "max_absolute_c0_change_m": 0.1200000000000001,
+          "v1_peak_absolute_c0_m": 0.75,
+          "v2_peak_absolute_c0_m": 0.7000000000000002
+        }
+      },
+      "exit_c0_strictly_lower_on_all_115_cycles": true,
+      "rlog_sha256_by_segment": {
+        "0": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "1": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "2": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "3": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "4": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "5": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "6": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "7": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "8": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "9": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "10": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "11": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45"
+      }
+    }
+  },
+  "total_original_route_cycles": 204946,
+  "total_float32_can_round_trips": 628030,
+  "source_sha256": {
+    "docs/ford_model_action_damping.md": "1aca8ca8e78d953beeda5b0c9803161a1d7c58556b1966040c71f809c3960bf8",
+    "docs/ford_model_action_drive_test.md": "3ce8bf6cb511a4461fa7abf194f47020af7ef5a6c90fcae7ed09571d1f750846",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "318742bd707ae526d0f5181bf7f081660c2c55de4bdf28518ccdf50d60e88080",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "de6f8524347f7c4a339941bc8565ccaa131cb93aa0418c75006ce08ab7edeb99",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "76466997ab4fef435f44339a6cb2d06303d5f0ab8717d487f27655297bd429d1",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "dbb78c98f57eef532f0dff0cb0b38396442882876e6d115f0d3c36159f64baf5",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "b92b55e23467e74227988fb39fff13a4ebba3c960fb90e34a6b341411699fc95",
+    "tools/ford_pscm_lab/damping_replay.py": "2f52fef12116ce87c0a1f465cf13d6af5e9fd76ce05d4318d72e34b70bc6a11f",
+    "tools/ford_pscm_lab/model_action_replay.py": "05a658dfcaf81693bf0d92184c0edff0172802f351a61a9e866a7967e74ae46d",
+    "tools/ford_pscm_lab/stress_model_action.py": "3e308733f4af0101ad0c414fbd724f6a99f56269ed8e791f0e15526d8bfd8f17"
+  },
+  "artifact_sha256": {
+    "route9b/report.json": "1b9317850c1724f433269c6a58349d0a0ee4eb6c9a03d6ec5858fa17713796c5",
+    "route9b/commands.npz": "d9f553a5384c84416a27c52b5dda0e751dda25edc5d6620fc311977f34a7a946",
+    "route90/report.json": "e37f71dc032e375b1c9b0beb4d6e0c915257bf72e59b0785ac0ca49c2472d2c2",
+    "route95/report.json": "d42d5a080fef8a1f0b3c7ae2cabcad20c88ce01d91be07ab33770ff5948c06b6",
+    "stress.json": "06e69a23340e3f5ed174e8e0b2e5791b320686dce7df963233d50a9982dca17b",
+    "coverage.json": "8f7915b9bd884abedfdbc2e0c18ef4474737e27225a714414384242535cc396f",
+    "mutation.txt": "67a76549fd7bb71e7092a155d4d0c3459be7b04dc2ca36eef9b54fbb574ad83d",
+    "bias_regression_red.txt": "8903c5a967e8c376050db85f7cf5f73abf71ee972f6025b25eb874479f94c62c",
+    "segment10_damping.png": "7d60cf9aabdcca9000fcf49bd14bd6130418aa6ab1b57af2678b171498ee9505"
+  }
+}

docs/ford_model_action_drive_test.md

@@ -0,0 +1,84 @@
+# Ford selected-action drive-test branch
+
+The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind
+its own persistent, default-off Sunnylink toggle. Version 4 uses
+[full geometric path prediction](ford_model_action_full_prediction.md) while retaining
+[excess-yaw offset damping](ford_model_action_damping.md). Input gates are unchanged.
+`calibration_approved=false`: offline checks do not establish physical tracking,
+turn-exit behavior or closed-loop stability.
+
+## Select and restore
+
+1. Install branch `hiimisaac-dev` from
+   `sunnypilot/sunnypilot` on the device using your normal branch-switch process.
+   Allow its build to finish before changing the setting.
+2. While offroad, open Sunnylink device settings → Vehicle → Ford and enable
+   **Selected-Action Path Tracking (Experimental)** (`FordModelActionController`).
+3. Complete a real offroad-to-onroad cycle. Selection occurs when `controlsd`
+   starts; changing a stored toggle or disengaging alone cannot swap an active
+   controller. Initial physical evaluation remains controlled testing.
+
+The startup log event `Ford path controller selected` should report
+`FordModelActionController`. Periodic `Ford C2-free path tracking` events
+identify `hypothesis=model-action-c0-c1-prediction-v4` and report host yaw and the command tuple.
+
+Turning the new toggle off and completing another offroad-to-onroad cycle
+restores **PSCM Coefficient Observer** if selected, otherwise the original
+Ford path controller. The stored observer selection is preserved. The candidate
+takes priority on the supported vehicle, independently of EPS firmware query
+results. Other vehicles retain their existing selection.
+
+The v8 implementation, its Sunnylink toggle and its dedicated tests are removed.
+A leftover `FordVirtualAngleController=1` file cannot enable the new controller.
+The shared Float32/CAN rounding helper now lives in `ford_model_action.py`;
+unused v8 PSCM-feedback plumbing is removed. Historical v8 route evidence remains
+in Git history and the archived validation documents.
+
+## Wiring and validation
+
+`Controls.__init__` selects the candidate once at startup. It shares the
+existing Ford call path, selected upstream-limited curvature, service gates,
+invalid-output disengagement, Float32 publication and downstream CAN builder.
+C2 and C3 stay zero. No opendbc pointer or Panda safety change is included.
+
+Sunnylink publishes the toggle through its generated settings schema and
+writes the registered Boolean through the existing parameter endpoint. The
+offroad UI rule and `needs_onroad_cycle` metadata describe when it can be
+changed and when it takes effect. An onroad backend write changes storage
+only; the controller continues using its startup selection.
+
+Native validation also exposed a pre-existing `params_keys_by_flag` bug:
+every returned buffer referenced the same reusable string. Sunnylink backup
+key enumeration could therefore return corrupted names. The bridge now
+returns separate strings owned by the parameter handle. Regression tests
+check distinct registered keys across flags, and toggle tests check its
+persistence and backup registration using the rebuilt native library.
+
+The current validation record is `ford_model_action_full_prediction_validation.json`;
+the [full-prediction notes](ford_model_action_full_prediction.md) explain cap removal
+and remaining physical uncertainty. `ford_model_action_prediction_validation.json`
+archives the capped v3 evaluation. `ford_model_action_damping_validation.json`
+archives the preceding v2 checks at their recorded source hashes.
+`ford_model_action_drive_test_validation.json` archives v1 wiring validation
+at the recorded source hashes, including 284 tests and 26 subtests. Its counts
+and 145-line controller size describe v1. The 469-line v8 module remains removed.
+
+The original 133,550-cycle route reconstruction, 485,238 packing round trips
+and mutation probes remain recorded separately in
+`ford_model_action_validation.json` at the offline-stage source hashes.
+
+## Reproduce deployment checks
+
+Initialize the branch's exact opendbc submodule (`c21a9013700734dd20b09e05aa68329ad8cc20f9`)
+and build the native Params library from this branch before testing.
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:opendbc_repo
+python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_model_action_drive_test/stress.json
+```
+
+The full hardware build and device boot are not performed by these offline
+tests. Installing the branch and enabling the toggle are separate actions;
+pushing the branch does not change a device's selected software or settings.

docs/ford_model_action_drive_test_validation.json

@@ -0,0 +1,145 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "7ca3c6e3b3e659c6f446039501c5826bbd14092e",
+  "branch": "codex/ford-model-action-drive-test",
+  "scope": "Default-off Sunnylink selection and v8 retirement; offline validation only. No device installation or physical performance validation.",
+  "calibration_approved": false,
+  "production_selector_changed": true,
+  "toggle": "FordModelActionController",
+  "default_enabled": false,
+  "v8_removed": true,
+  "panda_safety_changed": false,
+  "opendbc_submodule_changed": false,
+  "deployment_opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "controller_size": {
+    "total_lines": 145,
+    "code_lines_excluding_blanks_comments_docstrings": 95,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3,
+    "removed_v8_module_lines": 469
+  },
+  "tests": {
+    "combined_ford_params_sunnylink_suite": "284 passed, 26 subtests passed in 2.63s",
+    "suite_log_sha256": "2e223a507f0630481cf6f83b9f8893d226f3f4273a79a09fc35905aa875b1d2c",
+    "coverage": {
+      "covered_lines": 87,
+      "num_statements": 87,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 26,
+      "num_partial_branches": 0,
+      "covered_branches": 26,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "resolved_review_finding": "Updated YAML authoring source and regenerated settings JSON before final compiler/schema suite."
+  },
+  "routes": {
+    "route95": {
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 59,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.010000000000000675,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "report_sha256": "72fab710dc81c8c7d9b75d371b97fec32402d3b01fa05517dfa814d8daff3134"
+    },
+    "route90": {
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 19,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.020000000000000462,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "report_sha256": "cc2224bae597a341697a7681560a077cb209d77e4d060c0850997691c57d32fb"
+    }
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000000147,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "total_float32_can_round_trips": 485238,
+  "native_params": {
+    "source": "Rebuilt locally from this branch with clang++ and generated Capnp headers; ignored test dependency, not committed binary.",
+    "library_sha256": "270bf43241cf7c02cc432cf78ec9411a62d7653ca445695efe785ae82241aa09",
+    "sources_sha256": {
+      "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
+      "openpilot/common/params.cc": "a5adacb1d47cb3bf6e0d87d44ce158b41982d7eaf2e8114e32c48d3a6631304c",
+      "openpilot/common/params.h": "ed03d137e126ecd6f1608016020af18c0339fb987e27d0a2aa6830bba396970c",
+      "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
+      "openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
+      "openpilot/common/swaglog.cc": "9c2f88a2f1c3c4253b73defb264cc367a13ade23e02928e1d469b5c5833df176"
+    }
+  },
+  "test_dependency_notes": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "pyyaml": "6.0.3 from local uv cache",
+    "jsonschema": "Local cached package appended after venv to run schema validator without skips",
+    "hardware_build_and_device_boot": "not performed"
+  },
+  "source_sha256": {
+    "docs/ford_model_action_candidate.md": "c968132348d20891a9396f6e69db1315d570505796e19e09ffbb2da748e6e687",
+    "docs/ford_virtual_angle_experiment.md": "da6322f3c3d2d81463e44c50cc6cad1a962f97008ff9425c314da333ebe47a87",
+    "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
+    "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
+    "openpilot/common/tests/test_params.py": "557a1f616af5fd9f5e623fbee0fd6f44cb059a30c290c68ce2b57e9bcceed081",
+    "openpilot/selfdrive/controls/controlsd.py": "102b383e5beff43b8dd7c219178bef62e8a4b54443ebe682694606862bcd4e7f",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "1c9448d88d8021e5d34a5dccd14a17c6c1bc64b5531342bfc6d15574d9d3e710",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "2c5b14f814e84e59d749f61a43ef1dcfe06253f6e185443fa123c37525ac8466",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "9974df3ac4cc58ae78d47848cd18ef4aca1bcbb00edb257f28b4220d92890528",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "7a3fa18e562d5a03a5b85c72ee3f3ebeb836c497285dcd9aaad24f8fb4dd6942",
+    "openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py": "3db566612381fd87d3655a3ccff470be7da998c4ea7f6365f53e58cdb9c0ffb7",
+    "tools/ford_pscm_lab/model_action_replay.py": "827a6dc488d554bdf6e87438c6a2a985b3195bf6d01ab09002bfd6049d22a868",
+    "tools/ford_pscm_lab/stress_model_action.py": "2d5c72cc4b8ae214f2f5a19a050fe138f5c2ab0294d92d24e2481af5f8185613",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "ebf6bcd9260745100311521f8e11e85b7aebdd5561ab0876bfc2e802429d6896",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "docs/ford_model_action_drive_test.md": "d825b177cd099efd797fe89b7041695d6d41e4b9e8bba6bcaeb32aece164262b"
+  },
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev",
+    "validated_code_commit": "ea1ed70c718d32539ef6b9a89b89c0e297c92e06"
+  }
+}

docs/ford_model_action_full_prediction.md

@@ -0,0 +1,83 @@
+# Experimental Ford full path prediction, v4
+
+V4 removes the extra 15 cm / 25% limit on the geometric prediction introduced
+in [v3](ford_model_action_prediction.md). Those numbers were hand-chosen tuning
+bounds, not identified Ford response limits. The current user request is to
+remove that restriction; the existing default-off Sunnylink toggle remains.
+
+The controller now uses the full predicted offset from the same model path,
+assuming 150 ms of motion along the selected, upstream-limited curvature. The
+150 ms horizon remains an engineering assumption. Available model geometry
+still limits the prediction distance, with endpoint hold for short paths and
+fallback to the valid base offset if prediction arithmetic is nonfinite.
+
+The existing total command limits (C0 ±5.11 m, C1 ±0.5 rad), independent slew
+rates (4 m/s, 0.5 rad/s), quantization, yaw damping, input/service gates and
+zero C2/C3 remain unchanged. Only two control states persist. No integrator,
+model history, extra toggle or PSCM feedback loop is added.
+
+Removing the adjustment cap also permits the predicted C0 to oppose the
+original offset or become nonzero from a zero original offset. For example,
+a straight path with a nonzero selected turn request can have an opposing
+future-frame offset. Tests cover that behavior, mirrored turn releases,
+return to zero, and unchanged slew; sign preservation of the original C0 is
+no longer claimed. The yaw damper still cannot reverse its input target.
+
+## Evidence and interpretation
+
+The PSCM reports a generic `LimitReached` state. It does not tell us whether
+an incoming target is geometrically correct or well timed. Its internal
+limits cannot establish the tracking performance of this predictor. Removal
+is an experiment supported by command comparisons, not by an assumption that
+the PSCM will correct an excessive or mistimed request.
+
+Compared with capped v3 on identical recorded inputs:
+
+| Interval | Effect of removing the extra cap |
+| --- | --- |
+| Latest tight-left entry, 173–175.4 s | Mean C0 magnitude +0.032 m, maximum change 0.07 m |
+| Earlier right entry, 637–640 s | Mean magnitude +0.028 m, maximum change 0.10 m |
+| Earlier right exit, 642.7–643.852 s | All 115 commands unchanged |
+| Driver-clean low requests above 8 m/s, routes9b/9e | Mean absolute change 0.0018 / 0.0014 m; maximum 0.02 m |
+| All eligible samples on either recent route | Maximum absolute command change 0.13 m |
+
+C1 and command eligibility are exactly identical to v3 on both recent routes.
+Some command signs change near zero: this is an intended consequence of using
+the full transform, not proof those corrections improve driving. Entry windows
+include driver input, reported in the validation record. The magnitude changes
+above describe controller C0, not measured lateral vehicle displacement.
+
+The earlier v1 archive is also reproduced exactly on routes90/95, separately
+from the current controller pass. All replay uses original timestamps;
+publication times proxy computation, exact consumed model frames and causal
+carState are retained, and complete SubMaster health is unavailable. The
+recorded model and vehicle motion remain fixed. There is no measured physical
+improvement, stability result or new desired-versus-actual steering trajectory.
+
+Final validation passes 374 tests and 26 subtests with 100% controller statement
+and branch coverage, 299,604 original route cycles and 817,346 Float32/CAN
+round trips. The controller is 190 total lines / 122 code lines excluding
+blanks, comments and docstrings; two control states persist.
+
+See `ford_model_action_full_prediction_validation.json` for final test counts,
+coverage, dependency pins, source hashes and route/packing results. The initial
+uncapped variant was evaluated in a separate lab file before editing production;
+final route checks execute the production v4 source.
+
+## Reproduce and select
+
+Use the dependencies and combined suite command in the
+[drive-test guide](ford_model_action_drive_test.md). For the recent routes:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v3 --candidate current --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --window reversal 728 734 --output /path/to/separate/route9e-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v3 --candidate current --window right_entry 637 640 --window right_exit 642.7 643.852 --output /path/to/separate/route9b-results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+The same **Selected-Action Path Tracking (Experimental)** toggle selects v4
+on the CAN FD F-150 Lightning. An installation with the toggle already enabled
+selects v4 after updating and restarting controlsd. Diagnostics identify
+`model-action-c0-c1-prediction-v4`; `calibration_approved=false` remains explicit.
+Deployment branch: `sunnypilot/sunnypilot`, `hiimisaac-dev`. This work does not
+install software on the device or change its settings.

docs/ford_model_action_full_prediction_validation.json

@@ -0,0 +1,799 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+  "hypothesis": "model-action-c0-c1-prediction-v4",
+  "scope": "Removal of only the extra prediction adjustment cap; no physical tracking or stability claim.",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 190,
+    "code_lines_excluding_blanks_comments_docstrings": 122,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "374 passed, 26 subtests passed; no skips",
+    "coverage": {
+      "covered_lines": 113,
+      "num_statements": 113,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 32,
+      "num_partial_branches": 0,
+      "covered_branches": 32,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "cap_removal_red_probe": "11 tests fail with the v3 cap present; all 40 prediction tests pass after removal.",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "independent_review_verification": "Each reviewer passed 172 focused tests and verified source/artifact hashes and route/packing totals."
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "bounded_excess_yaw_damping_checked": true,
+    "full_geometric_prediction_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.400000000000329,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "66adef6cba120a1a3d8e9730ea987ef49f8fe283023362dbb99608dee0e3229f",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19"
+    }
+  },
+  "routes": {
+    "route90": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 42170,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.33999999999999986,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 475.24161910000026,
+          "core_c0_c1_rms": [
+            0.06125184365338734,
+            0.0057692154738982855
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.057970374224682646,
+            0.007060795713496332
+          ],
+          "adapter_eligible_seconds": 475.24161910000026,
+          "adapter_c0_c1_rms": [
+            0.0666350702799088,
+            0.0057692154738982855
+          ]
+        },
+        "turn": {
+          "seconds": 47.85871389900012,
+          "core_c0_c1_rms": [
+            0.18725251016049418,
+            0.046449510236699354
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4773534845264775,
+            0.04315935101392594
+          ],
+          "adapter_eligible_seconds": 47.85871389900012,
+          "adapter_c0_c1_rms": [
+            0.21255148222786066,
+            0.046449510236699354
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 38.696717235999785,
+          "core_c0_c1_rms": [
+            0.209264275431127,
+            0.014019080484620104
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.1593558816527209,
+            0.01823849401244768
+          ],
+          "adapter_eligible_seconds": 38.696717235999785,
+          "adapter_c0_c1_rms": [
+            0.22422047076469706,
+            0.014019080484620104
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 0.30159887100000304,
+          "core_c0_c1_rms": [
+            2.090789090165622,
+            0.21878942570759013
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.155728831937131,
+            0.38375302254248855
+          ],
+          "adapter_eligible_seconds": 0.30159887100000304,
+          "adapter_c0_c1_rms": [
+            2.2977717861671105,
+            0.21878942570759013
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 79.2599167590001,
+          "core_c0_c1_rms": [
+            0.02250905681378238,
+            0.005843904113223639
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03477379324171293,
+            0.006545440153133947
+          ],
+          "adapter_eligible_seconds": 79.2599167590001,
+          "adapter_c0_c1_rms": [
+            0.027461818062006878,
+            0.005843904113223639
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 0.022264622000001566,
+          "core_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.25,
+            0.04050000011920929
+          ],
+          "adapter_eligible_seconds": 0.022264622000001566,
+          "adapter_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 357.2849851050004,
+          "core_c0_c1_rms": [
+            0.011622097035118838,
+            0.003925571829874466
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03809717934501699,
+            0.0045587590980714285
+          ],
+          "adapter_eligible_seconds": 357.2849851050004,
+          "adapter_c0_c1_rms": [
+            0.017138026451004092,
+            0.003925571829874466
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 47.53485040600012,
+          "core_c0_c1_rms": [
+            0.08686835454709245,
+            0.043216347944464766
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4471065308273131,
+            0.030663943784303503
+          ],
+          "adapter_eligible_seconds": 47.53485040600012,
+          "adapter_c0_c1_rms": [
+            0.10939067791228956,
+            0.043216347944464766
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 427.99642020700026,
+          "core_c0_c1_rms": [
+            0.01261133542366393,
+            0.00422389167351435
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03724956081568198,
+            0.0046880581807907966
+          ],
+          "adapter_eligible_seconds": 427.99642020700026,
+          "adapter_c0_c1_rms": [
+            0.017590935519277658,
+            0.00422389167351435
+          ]
+        }
+      },
+      "workspace_head": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/metadata.json": "742afec55ed629155b22d387f376f878f7b2765221fe94f515a829052bdd916f",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.json": "64e1cc4be84394ac7ec408d383b99b48ada9b3fd129cdef2fd846e5fac620d66",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/pose_candidate/pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      }
+    },
+    "route95": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 25912,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.2999999999999998,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 160.93427628100014,
+          "core_c0_c1_rms": [
+            0.027283284164810895,
+            0.006054573235641941
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.04189890174428264,
+            0.0075342351227490395
+          ],
+          "adapter_eligible_seconds": 160.93427628100014,
+          "adapter_c0_c1_rms": [
+            0.03232621215993653,
+            0.006054573235641941
+          ]
+        },
+        "turn": {
+          "seconds": 58.39837079799993,
+          "core_c0_c1_rms": [
+            0.5425346915921071,
+            0.09102985199238507
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.63700479987207,
+            0.09691236120507696
+          ],
+          "adapter_eligible_seconds": 58.39837079799993,
+          "adapter_c0_c1_rms": [
+            0.5674989495695087,
+            0.09102985199238507
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 8.083969085000064,
+          "core_c0_c1_rms": [
+            0.09941192804175551,
+            0.016572084086884566
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.07597383189748476,
+            0.016403469943482765
+          ],
+          "adapter_eligible_seconds": 8.083969085000064,
+          "adapter_c0_c1_rms": [
+            0.11170387210996009,
+            0.016572084086884566
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 2.9056519089999995,
+          "core_c0_c1_rms": [
+            2.286598281178436,
+            0.3420570411209625
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.4371443775668484,
+            0.37360610108051345
+          ],
+          "adapter_eligible_seconds": 2.9056519089999995,
+          "adapter_c0_c1_rms": [
+            2.3669481787447944,
+            0.3420570411209625
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 32.021085868,
+          "core_c0_c1_rms": [
+            0.028387860150467858,
+            0.0067602044219821
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.036941169841952046,
+            0.007349671742584858
+          ],
+          "adapter_eligible_seconds": 32.021085868,
+          "adapter_c0_c1_rms": [
+            0.033317964465740625,
+            0.0067602044219821
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 22.160645306000006,
+          "core_c0_c1_rms": [
+            0.28668316323581683,
+            0.0671915746138579
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3391022553367454,
+            0.07297678510653233
+          ],
+          "adapter_eligible_seconds": 22.160645306000006,
+          "adapter_c0_c1_rms": [
+            0.32040564199313326,
+            0.0671915746138579
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 120.82922132800007,
+          "core_c0_c1_rms": [
+            0.010802222799973712,
+            0.004282512931613862
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03987958989535526,
+            0.0065793876512313535
+          ],
+          "adapter_eligible_seconds": 120.82922132800007,
+          "adapter_c0_c1_rms": [
+            0.016212091011218406,
+            0.004282512931613862
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 33.332073582999925,
+          "core_c0_c1_rms": [
+            0.07257996778378971,
+            0.03628926246224237
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3416081588451539,
+            0.027322786376822172
+          ],
+          "adapter_eligible_seconds": 33.332073582999925,
+          "adapter_c0_c1_rms": [
+            0.08724830487921002,
+            0.03628926246224237
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 145.24136911700015,
+          "core_c0_c1_rms": [
+            0.012566204405760934,
+            0.004614194153565963
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03855743841849719,
+            0.006621274926345545
+          ],
+          "adapter_eligible_seconds": 145.24136911700015,
+          "adapter_c0_c1_rms": [
+            0.01710948709543248,
+            0.004614194153565963
+          ]
+        }
+      },
+      "workspace_head": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/metadata.json": "f7c95a102e0e084bbcc9938399219080c1d3b26274b4ea3c3a9514ff7dc8ea8d",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.json": "1ab24a796d14817b8672ea6dfd39711d65cb06744eaaef7dcc02a840c392c4d3",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/pose_candidate/pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      }
+    },
+    "route9b": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 82386,
+      "eligible_cycles": 70703,
+      "status_counts": {
+        "inactive": 11683,
+        "active": 70703
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 164772,
+      "cohorts": {
+        "eligible": {
+          "cycles": 70703,
+          "seconds": 711.571703289,
+          "changed_c0_cycles": 10475,
+          "mean_absolute_c0_change_m": 0.00219777369954635,
+          "max_absolute_c0_change_m": 0.13000000000000078,
+          "increased_absolute_c0_cycles": 9822,
+          "decreased_absolute_c0_cycles": 633,
+          "driver_input_percent": 3.0297623863274845,
+          "baseline_peak_absolute_c0_m": 3.96,
+          "candidate_peak_absolute_c0_m": 3.99
+        },
+        "driver_clean": {
+          "cycles": 63267,
+          "seconds": 636.7469998759999,
+          "changed_c0_cycles": 8901,
+          "mean_absolute_c0_change_m": 0.0014713899370902674,
+          "max_absolute_c0_change_m": 0.05999999999999961,
+          "increased_absolute_c0_cycles": 8314,
+          "decreased_absolute_c0_cycles": 568,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.24,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 40656,
+          "seconds": 409.13067679800054,
+          "changed_c0_cycles": 7132,
+          "mean_absolute_c0_change_m": 0.0017587822497730527,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 6888,
+          "decreased_absolute_c0_cycles": 244,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.11000000000000032,
+          "candidate_peak_absolute_c0_m": 0.11000000000000032
+        },
+        "turn": {
+          "cycles": 5459,
+          "seconds": 54.939139939000256,
+          "changed_c0_cycles": 119,
+          "mean_absolute_c0_change_m": 0.00021719119817404867,
+          "max_absolute_c0_change_m": 0.010000000000000675,
+          "increased_absolute_c0_cycles": 119,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.24,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "right_entry": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 216,
+          "mean_absolute_c0_change_m": 0.028050765542768245,
+          "max_absolute_c0_change_m": 0.10000000000000009,
+          "increased_absolute_c0_cycles": 216,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 20.395198628296086,
+          "baseline_peak_absolute_c0_m": 2.95,
+          "candidate_peak_absolute_c0_m": 3.0300000000000002
+        },
+        "right_exit": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.7199999999999998,
+          "candidate_peak_absolute_c0_m": 0.7199999999999998
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 70703,
+        "within_one_quantum_cycles": 65890,
+        "maximum_absolute_error_c0_c1": [
+          0.25000004768371564,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v3",
+      "baseline_revision": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "right_entry",
+          "637",
+          "640"
+        ],
+        [
+          "right_exit",
+          "642.7",
+          "643.852"
+        ]
+      ],
+      "baseline_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "candidate_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45",
+        "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4",
+        "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21"
+      }
+    },
+    "route9e": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 83668,
+      "eligible_cycles": 74669,
+      "status_counts": {
+        "inactive": 8999,
+        "active": 74669
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 167336,
+      "cohorts": {
+        "eligible": {
+          "cycles": 74669,
+          "seconds": 751.5479497329998,
+          "changed_c0_cycles": 8108,
+          "mean_absolute_c0_change_m": 0.00147612892002983,
+          "max_absolute_c0_change_m": 0.13000000000000034,
+          "increased_absolute_c0_cycles": 7665,
+          "decreased_absolute_c0_cycles": 423,
+          "driver_input_percent": 2.7324748055386094,
+          "baseline_peak_absolute_c0_m": 4.38,
+          "candidate_peak_absolute_c0_m": 4.38
+        },
+        "driver_clean": {
+          "cycles": 66321,
+          "seconds": 667.5291971840106,
+          "changed_c0_cycles": 7257,
+          "mean_absolute_c0_change_m": 0.0012096637080993352,
+          "max_absolute_c0_change_m": 0.08000000000000007,
+          "increased_absolute_c0_cycles": 6883,
+          "decreased_absolute_c0_cycles": 374,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.4699999999999998,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 39635,
+          "seconds": 398.9613925449994,
+          "changed_c0_cycles": 5427,
+          "mean_absolute_c0_change_m": 0.0013748276784907216,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5341,
+          "decreased_absolute_c0_cycles": 86,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.07000000000000028,
+          "candidate_peak_absolute_c0_m": 0.07000000000000028
+        },
+        "turn": {
+          "cycles": 6219,
+          "seconds": 62.59491144700405,
+          "changed_c0_cycles": 244,
+          "mean_absolute_c0_change_m": 0.000391833417652352,
+          "max_absolute_c0_change_m": 0.010000000000000675,
+          "increased_absolute_c0_cycles": 244,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.4699999999999998,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "left_entry": {
+          "cycles": 238,
+          "seconds": 2.4048367620016506,
+          "changed_c0_cycles": 204,
+          "mean_absolute_c0_change_m": 0.03167584138917884,
+          "max_absolute_c0_change_m": 0.07000000000000028,
+          "increased_absolute_c0_cycles": 204,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 2.403816920672437,
+          "baseline_peak_absolute_c0_m": 2.6100000000000003,
+          "candidate_peak_absolute_c0_m": 2.6399999999999997
+        },
+        "left_peak": {
+          "cycles": 288,
+          "seconds": 2.891819394000777,
+          "changed_c0_cycles": 6,
+          "mean_absolute_c0_change_m": 0.00022373297976799967,
+          "max_absolute_c0_change_m": 0.019999999999999574,
+          "increased_absolute_c0_cycles": 6,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 6.41034524444729,
+          "baseline_peak_absolute_c0_m": 2.9400000000000004,
+          "candidate_peak_absolute_c0_m": 2.95
+        },
+        "left_exit": {
+          "cycles": 219,
+          "seconds": 2.203320298998733,
+          "changed_c0_cycles": 11,
+          "mean_absolute_c0_change_m": 0.0008062973780191794,
+          "max_absolute_c0_change_m": 0.019999999999999574,
+          "increased_absolute_c0_cycles": 11,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 9.936619705158035,
+          "baseline_peak_absolute_c0_m": 2.51,
+          "candidate_peak_absolute_c0_m": 2.51
+        },
+        "reversal": {
+          "cycles": 554,
+          "seconds": 5.570295188001182,
+          "changed_c0_cycles": 54,
+          "mean_absolute_c0_change_m": 0.002577813237060219,
+          "max_absolute_c0_change_m": 0.04999999999999982,
+          "increased_absolute_c0_cycles": 29,
+          "decreased_absolute_c0_cycles": 25,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.21999999999999975,
+          "candidate_peak_absolute_c0_m": 0.21999999999999975
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 74669,
+        "within_one_quantum_cycles": 69690,
+        "maximum_absolute_error_c0_c1": [
+          0.369999988079071,
+          0.0010000016689301061
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v3",
+      "baseline_revision": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "left_entry",
+          "173",
+          "175.4"
+        ],
+        [
+          "left_peak",
+          "175.4",
+          "178.3"
+        ],
+        [
+          "left_exit",
+          "178.3",
+          "180.5"
+        ],
+        [
+          "reversal",
+          "728",
+          "734"
+        ]
+      ],
+      "baseline_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "candidate_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7",
+        "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b",
+        "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2",
+        "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc",
+        "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223",
+        "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540",
+        "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d",
+        "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538",
+        "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940",
+        "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf",
+        "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb",
+        "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2",
+        "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1",
+        "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754",
+        "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21"
+      }
+    }
+  },
+  "ablation_comparison": {
+    "route9b": {
+      "lab_variant_commands_exactly_match_production": true,
+      "opposed_nonzero_c0_commands": 24,
+      "max_absolute_difference_on_opposed_commands_m": 0.05999999999999961
+    },
+    "route9e": {
+      "lab_variant_commands_exactly_match_production": true,
+      "opposed_nonzero_c0_commands": 30,
+      "max_absolute_difference_on_opposed_commands_m": 0.02999999999999936
+    }
+  },
+  "total_original_route_cycles": 299604,
+  "total_float32_can_round_trips": 817346,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "30cfffb86fcd3830320e9b2ec1f65cdfd219684a55d0bc0bb13f77966cc18b53",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "8ab5dd52958f13a2426e07f8713b7d8d56c52b8a05d173ffba87ffccc0087796",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "a7062d2c3c46b3abcd4c3facd97970644ce609fb55285daa0359681950948250",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "eb50e1d6acdb5e7332fadc3dcfcfa1b3809545e607c47c54b0ba3de80b2df208",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "175a1969ac83b14b88798bbde37371f740cd05cf3f8f6e5d21bbf9afe857bb57",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+    "tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+    "tools/ford_pscm_lab/stress_model_action.py": "66adef6cba120a1a3d8e9730ea987ef49f8fe283023362dbb99608dee0e3229f",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "1f7d90c06ec09b405571565632653dc7d984b106b955877916d4a0836c11e3b5",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "41ac31726b941e470f5be34e2523dc1fc2b25fb6a12705ab267a57ed4f3c27c8",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "c44c75e02042a0485b5bd7896e0b794761d768bbe513b6dc9c2c986ddd22b44c"
+  },
+  "artifact_sha256": {
+    ".cache/ford_prediction_uncapped/uncapped_red.txt": "09160dbc36320be886561cfdbdd21eef0c8bc420a11bae9bb1a8dd43f7b93a3c",
+    ".cache/ford_prediction_uncapped/suite_run.txt": "99b3bd2af256c9a4a083cb015a51a458a26ac02a8cad73c28c47470f9a5eb910",
+    ".cache/ford_prediction_uncapped/coverage.json": "50a282c90fa86147ed6925fdf37918471f8e5204a4c1e1ef9adb39335e220f42",
+    ".cache/ford_prediction_uncapped/stress.json": "e4226f403a3a914b9f3a7868b14a2b763215ec1f77e7d45ac06604859acc9fb6",
+    ".cache/ford_prediction_uncapped/route90/report.json": "c5cee3c56f9f7f292c8069be22dd1ffb1649055fccaf829b28d96bdec6558541",
+    ".cache/ford_prediction_uncapped/route95/report.json": "6fcc8a3bd7abb5b513888cb64426670e7e05f8b2100b1876be196a800cec2c90",
+    ".cache/ford_prediction_uncapped/final9b/report.json": "79faca3ff89e2015dad4823d4d988ea26ead55c7e51e4a594258102d32701b07",
+    ".cache/ford_prediction_uncapped/final9e/report.json": "9b003ce97225e65e03e351c5e133f3fed08dc79e026f73308098d51a98dd67ae",
+    ".cache/ford_prediction_uncapped/route90/commands.npz": "8787aabbe7007bcd7c68976c272085761aec6a4f360fa6cc8c8553bd1421c92b",
+    ".cache/ford_prediction_uncapped/route95/commands.npz": "349b716bd910cd030716b9ef519ee24f620fae5280776ffa871bc43318939400",
+    ".cache/ford_prediction_uncapped/final9b/commands.npz": "ff4a14fc74b144800f459c250be961ebb05a46a20e6ad1fc0394a659c9854394",
+    ".cache/ford_prediction_uncapped/final9e/commands.npz": "59f6046acb10b00ef00e7b9feb5b8547139a67a2f8767d7cbfe3603a59e6c77d"
+  }
+}

docs/ford_model_action_prediction.md

@@ -0,0 +1,126 @@
+# Experimental Ford path prediction, v3
+
+This document and its counts describe archived v3. The
+[current v4 experiment](ford_model_action_full_prediction.md) removes the extra
+15 cm / 25% prediction adjustment cap.
+
+The latest driven route9e used v1 (`5fc16abc7`), before the v2 yaw damping.
+It often follows the requested steering angle closely, but some tight turns
+fall behind after a reasonable initial turn-in. The requested angle is replanned
+from the car's changing position; a large late request may partly be a recovery
+request after arriving wide. It is not proof that the original turn required
+that much steering. The generic PSCM limit flag does not identify a torque,
+rate or mechanical limit, and the miss starts before our C1 cap in the clearest
+left turn.
+
+Short measured-motion integrations against earlier frozen model paths are
+consistent with a growing miss, but model uncertainty, reference timing and
+some driver input prevent a conclusive causal attribution. V3 tests a bounded
+change to initial path demand. No counterfactual physical tracking score is
+claimed from replaying fixed logs.
+
+## Change and bounds
+
+Start with the current model's lateral offset at 7 m of path arc length, as in
+v1/v2. Advance the reference pose by speed × 0.15 s along the **selected,
+upstream-limited curvature**, and read the same model path 7 m beyond that
+advance, expressed in the predicted ego frame. Bound the change from the
+original offset to both ±0.15 m and ±25% of its magnitude. Prediction cannot
+reverse that target or create C0 from a zero offset.
+
+For advance `d`, selected curvature `k`, rotation `theta = k*d`, and model
+point `(x, y)` at arc station `7+d`, the predicted lateral coordinate is:
+
+```
+y_predicted = cos(theta)*y - sin(theta)*x + (1-cos(theta))/k
+```
+
+The code evaluates the last term continuously at zero curvature without
+cancellation. Available path horizon limits `d`; prediction tapers to zero as
+the horizon approaches 7 m. Nonfinite prediction falls back to the validated
+current offset. The existing endpoint hold remains for paths shorter than 7 m.
+
+A matched constant-radius path retains essentially the same C0, subject to
+sample interpolation. Developing and flattening bends can move the target
+earlier. This is a geometric hypothesis assuming motion along selected
+curvature, not an identified 150 ms actuator delay or a calibrated plant model.
+Errors in that assumption can increase or reduce useful steering demand.
+
+The predicted offset passes through the existing ±5.11 m clip, v2 excess-yaw
+damping, independent 4 m/s slew and 0.01 m quantization. C1 construction, clip,
+slew and quantization are unchanged. C2=C3=0. Input sanity, freshness, service
+health and startup selection gates are unchanged. There are still only two
+control states (C0 and C1 slew positions), plus three adapter timestamps.
+The module is 193 total lines, including 125 code lines excluding blanks,
+comments and docstrings (18 more code lines than v2). No model history, integral
+or turn state machine is added. The bounds above
+apply to the prediction target, not arbitrary differences between separately
+slewed controllers after different histories.
+
+## Offline results and tradeoff
+
+All four supplied routes run at their original controls timestamps. Routes90/95
+also reproduce the archived v1 command construction exactly. The newer routes
+compare immutable v2 code with v3 using identical measured yaw, selected
+curvature, exact consumed model and causal carState. Publication times proxy
+computation time; complete SubMaster health is unavailable. Neither v2 nor v3
+was driven on these recordings. V2-versus-recorded error is therefore not a
+reconstruction accuracy measurement.
+
+| Recorded interval | Command change versus v2 |
+| --- | --- |
+| route9e left entry, 173–175.4 s | Mean C0 magnitude +0.143 m; same 2.0 m level reached 0.203 s earlier |
+| route9e left peak, 175.4–178.3 s | Mean magnitude +0.095 m; prediction also increases some late demand |
+| route9e reversal, 728–734 s | Peak C0 magnitude 0.24 → 0.22 m |
+| route9b right exit, 642.7–643.852 s | Mean C0 +0.024 m, partially offsetting v2 damping |
+| Driver-clean low requests above 8 m/s, routes9b/9e | Mean absolute C0 change ≈0.0015 m; maximum 0.02 m |
+
+The entry C0 crossings at 0.5, 1.0, 1.5, 2.0 and 2.4 m move earlier by 61, 64,
+367, 203 and 90 ms respectively. These are command-level crossing times,
+not measured improvements in wheel response. Several entry/peak windows
+contain driver input, quantified in the validation record.
+
+On all 115 earlier right-exit cycles before strong intervention, v3 remains
+below the driven v1 reconstruction: mean C0 is 0.370 m for v1, 0.290 m for v2,
+and 0.314 m for v3. This tradeoff is retained explicitly; v3 does not improve
+every exit command relative to v2. There is no evidence yet that it reduces
+the late model request or the physical miss.
+
+Route9b now includes full rlogs 12/13, added after the archived v2 evaluation.
+Its 14-rlog totals therefore differ from the historical 12-rlog report. The
+focused segment-10 comparison uses identical timestamps and data.
+
+Validation passes 372 tests and 26 subtests, including the real extracted
+controlsd selection/limiter/publication path and downstream CAN builder, with
+100% controller statement and branch coverage. Four routes cover 299,604
+original cycles. Randomized testing adds 200,000 core updates plus their
+mirrors against an independent analytic geometry/damping/slew oracle; boundary
+and route checks total 817,346 Float32/CAN round trips. These checks establish
+command construction and retained gates, not closed-loop vehicle behavior.
+See `ford_model_action_prediction_validation.json` for provenance and counts.
+
+## Reproduce and select
+
+To reproduce the archived suite and stress results, use v3 commit
+`01f8d51c82b3e863f1012d383b5994813ef01b81` with the native dependencies and
+suite command in the [drive-test guide](ford_model_action_drive_test.md).
+Current replay tooling can select the immutable v3 source explicitly. New-route comparisons use
+the deployment opendbc pin `c21a9013700734dd20b09e05aa68329ad8cc20f9`:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v2 --candidate v3 --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --window reversal 728 734 --window final_entry 822 825.5 --output /path/to/separate/route9e-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v2 --candidate v3 --window right_entry 637 640 --window right_exit_before_strong_input 642.7 643.852 --output /path/to/separate/route9b-results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+Historical replay loads trusted controller source from immutable local Git
+commits; those objects must exist in the checkout. The original replay tool
+still requires its explicit historical opendbc pin. Neither tool downloads
+code or drives the car.
+
+The existing default-off Sunnylink **Selected-Action Path Tracking
+(Experimental)** toggle selects v3 on the CAN FD F-150 Lightning. Updating
+with that toggle already enabled selects v3 at the next controlsd startup.
+Diagnostics identify `model-action-c0-c1-prediction-v3` and keep
+`calibration_approved=false`. No new device installation, hardware build,
+physical calibration or device boot is part of this offline validation.

docs/ford_model_action_prediction_validation.json

@@ -0,0 +1,849 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "744a97d9bc08d8743b250eceff7c88585b5480de",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c0-c1-prediction-v3",
+  "scope": "Bounded geometric prediction; fixed-input offline evidence only.",
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 193,
+    "code_lines_excluding_blanks_comments_docstrings": 125,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "372 passed, 26 subtests passed; no skips",
+    "suite_log_sha256": "b9546bd4cac8349ab9a699f175ba569ed41fc8a9e85e79a0018c79d3af24c11d",
+    "coverage": {
+      "covered_lines": 116,
+      "num_statements": 116,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 32,
+      "num_partial_branches": 0,
+      "covered_branches": 32,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "prediction_red_probe": "Disabling prediction fails all four mirrored developing/flattening-bend cases (4 failed, 34 passed); all 38 pass with prediction.",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "review_scope": "Tracked and untracked v3 changes since 744a97d9bc08d8743b250eceff7c88585b5480de; final loader unit test checked separately.",
+    "test_portability": "Archive loader unit test mocks Git source retrieval; full offline route runs use the actual immutable commit sources."
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "bounded_excess_yaw_damping_checked": true,
+    "bounded_geometric_prediction_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000015523,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "be77689e5327a13b7b4d0e18a3fc1863b57cab76be714339efc609124400489f",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6"
+    }
+  },
+  "routes": {
+    "route90": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 27264,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.16000000000000014,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 475.24161910000026,
+          "core_c0_c1_rms": [
+            0.06125184365338734,
+            0.0057692154738982855
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.057970374224682646,
+            0.007060795713496332
+          ],
+          "adapter_eligible_seconds": 475.24161910000026,
+          "adapter_c0_c1_rms": [
+            0.06384544973437024,
+            0.0057692154738982855
+          ]
+        },
+        "turn": {
+          "seconds": 47.85871389900012,
+          "core_c0_c1_rms": [
+            0.18725251016049418,
+            0.046449510236699354
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4773534845264775,
+            0.04315935101392594
+          ],
+          "adapter_eligible_seconds": 47.85871389900012,
+          "adapter_c0_c1_rms": [
+            0.20375259142277982,
+            0.046449510236699354
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 38.696717235999785,
+          "core_c0_c1_rms": [
+            0.209264275431127,
+            0.014019080484620104
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.1593558816527209,
+            0.01823849401244768
+          ],
+          "adapter_eligible_seconds": 38.696717235999785,
+          "adapter_c0_c1_rms": [
+            0.21664870054721863,
+            0.014019080484620104
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 0.30159887100000304,
+          "core_c0_c1_rms": [
+            2.090789090165622,
+            0.21878942570759013
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.155728831937131,
+            0.38375302254248855
+          ],
+          "adapter_eligible_seconds": 0.30159887100000304,
+          "adapter_c0_c1_rms": [
+            2.2043400076988204,
+            0.21878942570759013
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 79.2599167590001,
+          "core_c0_c1_rms": [
+            0.02250905681378238,
+            0.005843904113223639
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03477379324171293,
+            0.006545440153133947
+          ],
+          "adapter_eligible_seconds": 79.2599167590001,
+          "adapter_c0_c1_rms": [
+            0.026080258136122546,
+            0.005843904113223639
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 0.022264622000001566,
+          "core_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.25,
+            0.04050000011920929
+          ],
+          "adapter_eligible_seconds": 0.022264622000001566,
+          "adapter_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 357.2849851050004,
+          "core_c0_c1_rms": [
+            0.011622097035118838,
+            0.003925571829874466
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03809717934501699,
+            0.0045587590980714285
+          ],
+          "adapter_eligible_seconds": 357.2849851050004,
+          "adapter_c0_c1_rms": [
+            0.013693372023947319,
+            0.003925571829874466
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 47.53485040600012,
+          "core_c0_c1_rms": [
+            0.08686835454709245,
+            0.043216347944464766
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4471065308273131,
+            0.030663943784303503
+          ],
+          "adapter_eligible_seconds": 47.53485040600012,
+          "adapter_c0_c1_rms": [
+            0.10462899220936993,
+            0.043216347944464766
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 427.99642020700026,
+          "core_c0_c1_rms": [
+            0.01261133542366393,
+            0.00422389167351435
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03724956081568198,
+            0.0046880581807907966
+          ],
+          "adapter_eligible_seconds": 427.99642020700026,
+          "adapter_c0_c1_rms": [
+            0.014639447114479471,
+            0.00422389167351435
+          ]
+        }
+      },
+      "workspace_head": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/metadata.json": "742afec55ed629155b22d387f376f878f7b2765221fe94f515a829052bdd916f",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.json": "64e1cc4be84394ac7ec408d383b99b48ada9b3fd129cdef2fd846e5fac620d66",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/pose_candidate/pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      }
+    },
+    "route95": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 20110,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.16999999999999993,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 160.93427628100014,
+          "core_c0_c1_rms": [
+            0.027283284164810895,
+            0.006054573235641941
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.04189890174428264,
+            0.0075342351227490395
+          ],
+          "adapter_eligible_seconds": 160.93427628100014,
+          "adapter_c0_c1_rms": [
+            0.02970169836480558,
+            0.006054573235641941
+          ]
+        },
+        "turn": {
+          "seconds": 58.39837079799993,
+          "core_c0_c1_rms": [
+            0.5425346915921071,
+            0.09102985199238507
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.63700479987207,
+            0.09691236120507696
+          ],
+          "adapter_eligible_seconds": 58.39837079799993,
+          "adapter_c0_c1_rms": [
+            0.5622074838217578,
+            0.09102985199238507
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 8.083969085000064,
+          "core_c0_c1_rms": [
+            0.09941192804175551,
+            0.016572084086884566
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.07597383189748476,
+            0.016403469943482765
+          ],
+          "adapter_eligible_seconds": 8.083969085000064,
+          "adapter_c0_c1_rms": [
+            0.1039594708778471,
+            0.016572084086884566
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 2.9056519089999995,
+          "core_c0_c1_rms": [
+            2.286598281178436,
+            0.3420570411209625
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.4371443775668484,
+            0.37360610108051345
+          ],
+          "adapter_eligible_seconds": 2.9056519089999995,
+          "adapter_c0_c1_rms": [
+            2.342145227147664,
+            0.3420570411209625
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 32.021085868,
+          "core_c0_c1_rms": [
+            0.028387860150467858,
+            0.0067602044219821
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.036941169841952046,
+            0.007349671742584858
+          ],
+          "adapter_eligible_seconds": 32.021085868,
+          "adapter_c0_c1_rms": [
+            0.032473217223426755,
+            0.0067602044219821
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 22.160645306000006,
+          "core_c0_c1_rms": [
+            0.28668316323581683,
+            0.0671915746138579
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3391022553367454,
+            0.07297678510653233
+          ],
+          "adapter_eligible_seconds": 22.160645306000006,
+          "adapter_c0_c1_rms": [
+            0.32040564199313326,
+            0.0671915746138579
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 120.82922132800007,
+          "core_c0_c1_rms": [
+            0.010802222799973712,
+            0.004282512931613862
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03987958989535526,
+            0.0065793876512313535
+          ],
+          "adapter_eligible_seconds": 120.82922132800007,
+          "adapter_c0_c1_rms": [
+            0.013133007539405142,
+            0.004282512931613862
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 33.332073582999925,
+          "core_c0_c1_rms": [
+            0.07257996778378971,
+            0.03628926246224237
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3416081588451539,
+            0.027322786376822172
+          ],
+          "adapter_eligible_seconds": 33.332073582999925,
+          "adapter_c0_c1_rms": [
+            0.08556161843867907,
+            0.03628926246224237
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 145.24136911700015,
+          "core_c0_c1_rms": [
+            0.012566204405760934,
+            0.004614194153565963
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03855743841849719,
+            0.006621274926345545
+          ],
+          "adapter_eligible_seconds": 145.24136911700015,
+          "adapter_c0_c1_rms": [
+            0.014652655970248427,
+            0.004614194153565963
+          ]
+        }
+      },
+      "workspace_head": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/metadata.json": "f7c95a102e0e084bbcc9938399219080c1d3b26274b4ea3c3a9514ff7dc8ea8d",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.json": "1ab24a796d14817b8672ea6dfd39711d65cb06744eaaef7dcc02a840c392c4d3",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/pose_candidate/pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      }
+    },
+    "route9b": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 82386,
+      "eligible_cycles": 70703,
+      "status_counts": {
+        "inactive": 11683,
+        "active": 70703
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 164772,
+      "cohorts": {
+        "eligible": {
+          "cycles": 70703,
+          "seconds": 711.571703289,
+          "changed_c0_cycles": 20858,
+          "mean_absolute_c0_change_m": 0.006916798541216091,
+          "max_absolute_c0_change_m": 0.15000000000000124,
+          "increased_absolute_c0_cycles": 17700,
+          "decreased_absolute_c0_cycles": 3157,
+          "driver_input_percent": 3.0297623863274845,
+          "baseline_peak_absolute_c0_m": 3.89,
+          "candidate_peak_absolute_c0_m": 3.96
+        },
+        "driver_clean": {
+          "cycles": 63267,
+          "seconds": 636.7469998759999,
+          "changed_c0_cycles": 16024,
+          "mean_absolute_c0_change_m": 0.002923399405403562,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 13587,
+          "decreased_absolute_c0_cycles": 2437,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.16,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 40656,
+          "seconds": 409.13067679800054,
+          "changed_c0_cycles": 6007,
+          "mean_absolute_c0_change_m": 0.0014813245503935623,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5099,
+          "decreased_absolute_c0_cycles": 908,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.08999999999999986,
+          "candidate_peak_absolute_c0_m": 0.11000000000000032
+        },
+        "turn": {
+          "cycles": 5459,
+          "seconds": 54.939139939000256,
+          "changed_c0_cycles": 3156,
+          "mean_absolute_c0_change_m": 0.007655026845650452,
+          "max_absolute_c0_change_m": 0.08999999999999986,
+          "increased_absolute_c0_cycles": 2938,
+          "decreased_absolute_c0_cycles": 218,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.16,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "right_entry": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 298,
+          "mean_absolute_c0_change_m": 0.14871908015683724,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 298,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 20.395198628296086,
+          "baseline_peak_absolute_c0_m": 2.8000000000000003,
+          "candidate_peak_absolute_c0_m": 2.95
+        },
+        "right_exit_before_strong_input": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 115,
+          "mean_absolute_c0_change_m": 0.02362929150320746,
+          "max_absolute_c0_change_m": 0.040000000000000036,
+          "increased_absolute_c0_cycles": 115,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.7000000000000002,
+          "candidate_peak_absolute_c0_m": 0.7199999999999998
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 70703,
+        "within_one_quantum_cycles": 69862,
+        "maximum_absolute_error_c0_c1": [
+          0.20999998569488554,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v2",
+      "baseline_revision": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "right_entry",
+          "637",
+          "640"
+        ],
+        [
+          "right_exit_before_strong_input",
+          "642.7",
+          "643.852"
+        ]
+      ],
+      "baseline_source_sha256": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+      "candidate_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45",
+        "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4",
+        "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0"
+      }
+    },
+    "route9e": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 83668,
+      "eligible_cycles": 74669,
+      "status_counts": {
+        "inactive": 8999,
+        "active": 74669
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 167336,
+      "cohorts": {
+        "eligible": {
+          "cycles": 74669,
+          "seconds": 751.5479497329998,
+          "changed_c0_cycles": 23282,
+          "mean_absolute_c0_change_m": 0.00617904439256301,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 19880,
+          "decreased_absolute_c0_cycles": 3402,
+          "driver_input_percent": 2.7324748055386094,
+          "baseline_peak_absolute_c0_m": 4.33,
+          "candidate_peak_absolute_c0_m": 4.38
+        },
+        "driver_clean": {
+          "cycles": 66321,
+          "seconds": 667.5291971840106,
+          "changed_c0_cycles": 18123,
+          "mean_absolute_c0_change_m": 0.003409158953346422,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 16136,
+          "decreased_absolute_c0_cycles": 1987,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.3600000000000003,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 39635,
+          "seconds": 398.9613925449994,
+          "changed_c0_cycles": 5866,
+          "mean_absolute_c0_change_m": 0.0014778774629256156,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5377,
+          "decreased_absolute_c0_cycles": 489,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.05999999999999961,
+          "candidate_peak_absolute_c0_m": 0.07000000000000028
+        },
+        "turn": {
+          "cycles": 6219,
+          "seconds": 62.59491144700405,
+          "changed_c0_cycles": 5129,
+          "mean_absolute_c0_change_m": 0.011069748090899225,
+          "max_absolute_c0_change_m": 0.10999999999999943,
+          "increased_absolute_c0_cycles": 5098,
+          "decreased_absolute_c0_cycles": 31,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.3600000000000003,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "left_entry": {
+          "cycles": 238,
+          "seconds": 2.4048367620016506,
+          "changed_c0_cycles": 238,
+          "mean_absolute_c0_change_m": 0.14261645650107144,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 238,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 2.403816920672437,
+          "baseline_peak_absolute_c0_m": 2.46,
+          "candidate_peak_absolute_c0_m": 2.6100000000000003
+        },
+        "left_peak": {
+          "cycles": 288,
+          "seconds": 2.891819394000777,
+          "changed_c0_cycles": 288,
+          "mean_absolute_c0_change_m": 0.095420590363382,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 288,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 6.41034524444729,
+          "baseline_peak_absolute_c0_m": 2.79,
+          "candidate_peak_absolute_c0_m": 2.9400000000000004
+        },
+        "left_exit": {
+          "cycles": 219,
+          "seconds": 2.203320298998733,
+          "changed_c0_cycles": 185,
+          "mean_absolute_c0_change_m": 0.0422943841629376,
+          "max_absolute_c0_change_m": 0.09000000000000075,
+          "increased_absolute_c0_cycles": 56,
+          "decreased_absolute_c0_cycles": 129,
+          "driver_input_percent": 9.936619705158035,
+          "baseline_peak_absolute_c0_m": 2.4299999999999997,
+          "candidate_peak_absolute_c0_m": 2.51
+        },
+        "reversal": {
+          "cycles": 554,
+          "seconds": 5.570295188001182,
+          "changed_c0_cycles": 167,
+          "mean_absolute_c0_change_m": 0.007060409634059627,
+          "max_absolute_c0_change_m": 0.040000000000000036,
+          "increased_absolute_c0_cycles": 20,
+          "decreased_absolute_c0_cycles": 147,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.2400000000000002,
+          "candidate_peak_absolute_c0_m": 0.21999999999999975
+        },
+        "final_entry": {
+          "cycles": 348,
+          "seconds": 3.4958847329980927,
+          "changed_c0_cycles": 348,
+          "mean_absolute_c0_change_m": 0.11950442609746584,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 348,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 3.556656282920533,
+          "baseline_peak_absolute_c0_m": 3.31,
+          "candidate_peak_absolute_c0_m": 3.4400000000000004
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 74669,
+        "within_one_quantum_cycles": 74156,
+        "maximum_absolute_error_c0_c1": [
+          0.29000000834465034,
+          0.0010000016689301061
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v2",
+      "baseline_revision": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "left_entry",
+          "173",
+          "175.4"
+        ],
+        [
+          "left_peak",
+          "175.4",
+          "178.3"
+        ],
+        [
+          "left_exit",
+          "178.3",
+          "180.5"
+        ],
+        [
+          "reversal",
+          "728",
+          "734"
+        ],
+        [
+          "final_entry",
+          "822",
+          "825.5"
+        ]
+      ],
+      "baseline_source_sha256": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+      "candidate_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7",
+        "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b",
+        "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2",
+        "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc",
+        "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223",
+        "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540",
+        "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d",
+        "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538",
+        "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940",
+        "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf",
+        "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb",
+        "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2",
+        "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1",
+        "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754",
+        "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0"
+      }
+    }
+  },
+  "focus_comparison": {
+    "scope": "Same recorded inputs; crossing a C0 level earlier does not identify vehicle response time.",
+    "left_entry_crossings": [
+      {
+        "c0_magnitude_m": 0.5,
+        "v2_time_s": 173.04409075799958,
+        "v3_time_s": 172.98298211199835,
+        "earlier_ms": 61.10864600123023
+      },
+      {
+        "c0_magnitude_m": 1.0,
+        "v2_time_s": 173.41054691999852,
+        "v3_time_s": 173.3469171409979,
+        "earlier_ms": 63.629779000621056
+      },
+      {
+        "c0_magnitude_m": 1.5,
+        "v2_time_s": 174.09016323599826,
+        "v3_time_s": 173.7234842099988,
+        "earlier_ms": 366.6790259994741
+      },
+      {
+        "c0_magnitude_m": 2.0,
+        "v2_time_s": 174.69427409499804,
+        "v3_time_s": 174.49167145299907,
+        "earlier_ms": 202.60264199896483
+      },
+      {
+        "c0_magnitude_m": 2.4,
+        "v2_time_s": 175.08411706099832,
+        "v3_time_s": 174.99415714599854,
+        "earlier_ms": 89.95991499978118
+      }
+    ],
+    "right_exit": {
+      "mean_c0_v1_m": 0.3704272817752263,
+      "mean_c0_v2_m": 0.29049595970311604,
+      "mean_c0_v3_m": 0.31412525120632345,
+      "v3_below_driven_v1_every_cycle": true,
+      "cycles": 115
+    }
+  },
+  "total_original_route_cycles": 299604,
+  "total_float32_can_round_trips": 817346,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "8851104f811757f6a31b5e23ec5933c7bfc18c17012af9a0a78f851217eca525",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "04daed84093330c05ae1669c155ed9c197f95ebb02fd4e7ca1c7a10ce70d03da",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "a7062d2c3c46b3abcd4c3facd97970644ce609fb55285daa0359681950948250",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "c05ae173c809228a9f4782af32a174b1b928f506a76c454aa89a51ef0ed17bdd",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "fd4095bc4f1363fd014c43a9e0776a2245105e8fb2703d87da367d0f33dc22dc",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+    "tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+    "tools/ford_pscm_lab/stress_model_action.py": "be77689e5327a13b7b4d0e18a3fc1863b57cab76be714339efc609124400489f",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "d2a3602e145db4f969ecf1402fe66684c3b1f884e3ed429e95f5fb5dcaddefb3",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "b5951a90cb01bfcd79b82705600ad2d165b3cc5f7e33c7404536eb989bc3ce39",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "6badb0080d8a999d63d063430c0a320f1a58884dfe0c390774f90a39cbf0e0e2"
+  },
+  "artifact_sha256": {
+    ".cache/ford_model_action_prediction/prediction_red.txt": "81e765edd1fc73783855ef30aa3c4aa8f111c001d7e9e0951d5791e18a860f4e",
+    ".cache/ford_model_action_prediction/stress.json": "e358a7caafe457a83637857cee7f0b1b2b60e0e4020f64b46630b8462e5edff5",
+    ".cache/ford_model_action_prediction/coverage.json": "333ed4414632adbb12a8a1f3ef2f9dc7a62dd9fde08bbc6b8a3d372cf9a6869b",
+    ".cache/ford_model_action_prediction/focus_comparison.json": "b0af320c0eb7e535420702ed78df30c43134f8f9e2a977e5f88f765ba0626c69",
+    ".cache/ford_model_action_prediction/route90/report.json": "b58f738269d9fe176672670f5bf4116f3226d319f43b5b407ac221c288aba174",
+    ".cache/ford_model_action_prediction/route95/report.json": "379a6f5bc9c98d8d1bccf687d61d46c54eda439284508e321d49b5312c1ca9f2",
+    ".cache/ford_model_action_prediction/route9b/report.json": "650ce4056b663124e2f72fae94cd1b66423ff74de42611a99501271c2c846bc2",
+    ".cache/ford_model_action_prediction/route9e/report.json": "196b3b242e3e056ed531cd6e657a235cc9bf8ed4357ce696b1bec1e5f0a49a39",
+    ".cache/ford_model_action_prediction/route9b/commands.npz": "5d82318935253d4fa8a1b8c30cc7f2d465c151ae1cb9109fa09a622d5be49871",
+    ".cache/ford_model_action_prediction/route9e/commands.npz": "2917bbe801a78711340fa4eae63d5bf1cb4a4464c45d8538c9a46a77b5511e06",
+    ".cache/ford_model_action_prediction/plot_commands.py": "6a5bd97fedeb9bd951925dddc656e508082f6d457dbc1a6c9d4db769afcdd921",
+    "/Users/ibpersonal/.codex/visualizations/2026/09/07/01a07c33-1f6c-7292-8030-ad4b01923034/ford-route9e/prediction_command_comparison.png": "86e68a0497490352065997f73cd5a798ff93dcabc93152028669ccfb36a4fefe",
+    "/Users/ibpersonal/.codex/visualizations/2026/09/07/01a07c33-1f6c-7292-8030-ad4b01923034/ford-route9e/prediction_command_comparison.svg": "2310b2978b60b97619886c2e10ecca88c73b5fde3477452347e6db0a5bbcd5b3"
+  }
+}

docs/ford_model_action_validation.json

@@ -0,0 +1,220 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "c4b3c55c826fca1ce09618e418e95f0a24478d96",
+  "calibration_approved": false,
+  "production_selector_changed": false,
+  "vehicle_settings_changed": false,
+  "panda_safety_changed": false,
+  "scope": "Offline command construction, adapter integration, numerical fault probes and Ford regression tests. Physical response remains unvalidated.",
+  "controller_size": {
+    "total_lines": 131,
+    "code_lines_excluding_blanks_comments_docstrings": 86,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+  "tests": {
+    "ford_suite": "264 passed, 150 subtests passed in 14.29s",
+    "new_core_adapter_tests": 107,
+    "new_replay_validator_tests": 13,
+    "controller_coverage": {
+      "statements": 78,
+      "missing_statements": 0,
+      "branches": 24,
+      "partial_branches": 0,
+      "percent": 100
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass"
+  },
+  "routes": {
+    "route95": {
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 59,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.010000000000000675,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "turn_speed_15_55": {
+        "seconds": 33.332073582999925,
+        "core_c0_c1_rms": [
+          0.07257996778378971,
+          0.03628926246224237
+        ],
+        "recorded_v8_c0_c1_rms": [
+          0.3416081588451539,
+          0.027322786376822172
+        ],
+        "adapter_eligible_seconds": 33.332073582999925,
+        "adapter_c0_c1_rms": [
+          0.07257996778378971,
+          0.03628926246224237
+        ]
+      },
+      "input_sha256": {
+        "route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      },
+      "report_sha256": "cf4e3804f2ebdb0e50f3c49636bf60a30e3c1180411b582826a115970ab972fc"
+    },
+    "route90": {
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 19,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.020000000000000462,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "turn_speed_15_55": {
+        "seconds": 47.53485040600012,
+        "core_c0_c1_rms": [
+          0.08686835454709245,
+          0.043216347944464766
+        ],
+        "recorded_v8_c0_c1_rms": [
+          0.4471065308273131,
+          0.030663943784303503
+        ],
+        "adapter_eligible_seconds": 47.53485040600012,
+        "adapter_c0_c1_rms": [
+          0.08686835454709245,
+          0.043216347944464766
+        ]
+      },
+      "input_sha256": {
+        "route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      },
+      "report_sha256": "b95247bdf6bbec16e5dc4781eaf7a678aca787418251cd161f6438e3173ad490"
+    }
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000000147,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+  },
+  "mutation_checks": {
+    "mutations": [
+      {
+        "mutation": "halve_heading",
+        "detected_by_tests": true,
+        "failed_tests": 9
+      },
+      {
+        "mutation": "cap_heading_preview",
+        "detected_by_tests": true,
+        "failed_tests": 9
+      },
+      {
+        "mutation": "erase_centering",
+        "detected_by_tests": true,
+        "failed_tests": 15
+      },
+      {
+        "mutation": "slow_c0_to_c1_rate",
+        "detected_by_tests": true,
+        "failed_tests": 11
+      },
+      {
+        "mutation": "retain_invalid_state",
+        "detected_by_tests": true,
+        "failed_tests": 18
+      },
+      {
+        "mutation": "ignore_model_freshness",
+        "detected_by_tests": true,
+        "failed_tests": 2
+      },
+      {
+        "mutation": "ignore_model_clock_rollback",
+        "detected_by_tests": true,
+        "failed_tests": 1
+      },
+      {
+        "mutation": "reverse_c0_wire_sign",
+        "detected_by_tests": true,
+        "failed_tests": 11
+      }
+    ],
+    "all_detected": true
+  },
+  "native_test_dependency": {
+    "scope": "Native dependency for inherited Params selection test only; copied existing local build, not rebuilt.",
+    "source_library": "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/openpilot/common/libparams_c.dylib",
+    "sha256": "ddde738108eab18b75f085c865c43aa197fd79a0384b390b1515ff90116e20e0",
+    "byte_identical_source_files": [
+      "openpilot/common/params.cc",
+      "openpilot/common/params.h",
+      "openpilot/common/params_c.cc",
+      "openpilot/common/params.py",
+      "openpilot/common/params_keys.h",
+      "openpilot/common/queue.h",
+      "openpilot/common/util.cc",
+      "openpilot/common/util.h",
+      "openpilot/common/hardware/hw.h"
+    ]
+  },
+  "review": {
+    "standards_findings": 0,
+    "spec_findings": 0,
+    "method": "Independent parallel read-only reviews; 120 focused tests independently passed."
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "cb6353f00f2f5c84df4e606c6b7e20650f8c1e908b9fd71890f72aa4a5e42592",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "c3b971622cc4041575aeec1826d45b76cebab9a2f77b2b9525c85d4184961295",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "5aaa29c1f11080b7df0165fd0202a29b065053809e5f9c4b9ed8abfcce47e41b",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/model_action_replay.py": "c114c479bd22e4fc61a3e8d3ee7fae5d71d1d80ec4b952faad8f8f3692fb1508",
+    "tools/ford_pscm_lab/stress_model_action.py": "a78a50eed1f801f3b096d694ab8c2fd70804b6c250465b4152f38d83770a982b",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "09d024c59d44b83ec081d6416d0f946a7719a73f22213af1f4ddc03dc4e6f4ac"
+  },
+  "artifacts": {
+    "directory": ".cache/ford_model_action",
+    "route_reports": [
+      "route95/report.json",
+      "route90/report.json"
+    ],
+    "stress_report": "stress.json",
+    "mutation_report": "mutations/report.json",
+    "test_log": "ford_suite.txt"
+  }
+}

docs/ford_virtual_angle_experiment.md

@@ -0,0 +1,327 @@
+# Ford C2-free model-pose tracking with measured feedback
+
+This experiment is retired. Its implementation, setting and dedicated tests
+were removed from the selected-action drive-test branch. For current setup,
+see [Ford selected-action drive testing](ford_model_action_drive_test.md).
+The material below is historical; it does not describe an available toggle.
+
+Hypothesis `model-pose-c0-c1-feedback-v8` retains the model-pose C0/C1 base
+and adds two guarded release policies. When measured turning exceeds both
+current and delayed requests, a separate output guard prevents same-direction
+C0/C1 growth, including while feedback history rebuilds after driver input.
+When turning instead falls below both requests and is no longer increasing,
+bounded C1 tracking can use remaining release-entry command headroom.
+Existing opposing-bias recovery still stops at zero bias. Geometry, blending,
+feedback gain, slew rates and field limits are unchanged; C2/C3 remain zero.
+
+This is an experimental outer controller around the multivariable PSCM.
+Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle
+servo. V8 has offline validation only. Command replay cannot establish the
+truck's response, closed-loop stability, or an overshoot improvement.
+
+## Evidence and scope
+
+Route80 ran v3 and contains both sustained under-response and over-response.
+Representative eligible windows had median CAN response/request ratios of
+0.78, 1.77 and 0.69 with a declared 0.2-second comparison interval. These
+are descriptive tracking ratios, not identified controller gains.
+
+V4 replaced separate model-heading C1 with selected-curvature C1 and reduced
+heading demand in several large maneuvers. The user subsequently reported
+weak turning and steering repeatedly stopping near 85 degrees. Older logs
+contain larger wheel angles; the inspected host code has no fixed 85-degree
+wheel stop, although upstream curvature limits depend on speed.
+
+Route83 had the Sunnylink toggle on, but omitted EPS firmware responses.
+The former firmware gate selected the default `FordPathController`; replay
+reproduced its recorded C0/C1/C2 requests. Its favorable turns are evidence
+for the existing model-pose construction, not validation of v5 or v6.
+V6 reuses that construction while replacing its remaining C2 request with
+C0/C1 geometry. Removing C2 changes the request received by the PSCM, so
+matching large C0/C1 commands does not guarantee matching vehicle motion.
+
+Route8a ran v6 and was reported as the best drive. Route8e ran v7 throughout
+with the experiment enabled; it includes entry lag and excessive turning
+while requests release. Fixed-input v6/v7 replay produced identical commands
+in the main reversal and over-response examples, so the v7 recovery change
+does not directly explain their command behavior. In the over-response
+example, model C0/C1 grew while selected curvature fell and driver resets
+repeatedly removed feedback history. Another exit remained deficient after
+opposing bias reached zero. These observations motivate the v8 guards; they
+do not isolate an EPS transfer function or demonstrate the proposed response.
+
+## Base request
+
+controlsd selects valid `lateralManeuverPlan.desiredCurvature`, otherwise
+`modelV2.action.desiredCurvature`, after the existing curvature limiter.
+This action already includes upstream delay handling; it receives no extra
+response advance here.
+
+The model contribution uses the existing allocator's raw forward pose and
+bounded short-pose correction. `_model_pose` advances 0.1 seconds, retains
+the model's remaining forward geometry, and separately corrects the short
+pose using measured curvature and its recent change. Its offset preview is
+up to 7 m and its heading preview is up to max(7 m, speed × 1 s), bounded by
+available path length. This raw pose is not passed through a second model
+filter. The filtered, ego-aligned reference remains available for comparison
+and the existing geometry-validity checks.
+
+```text
+share(k) = clip((k - 0.006/m) / (0.012/m - 0.006/m), 0, 1)
+aligned = desired_curvature × model_forward_heading > 0
+model_share = min(share(abs(desired_curvature)), share(model_curvature_demand))
+              if aligned, otherwise 0
+model_pair = existing_pose_encoder(model_pose, model_share, C2=0)
+
+remaining_curvature = desired_curvature × (1 - model_share)
+L0 = max(8 m, speed × 1 s)
+L1 = max(7 m, speed × 1 s)
+curvature_C0 = 0.5 × remaining_curvature × L0²
+curvature_C1 = remaining_curvature × L1
+C0_base = clip(model_pair.C0 + curvature_C0, ±5.11 m)
+C1_base = clip(model_pair.C1 + curvature_C1, ±0.5 rad)
+```
+
+`model_curvature_demand` is the larger absolute curvature implied by the
+forward offset and heading previews. The share uses the existing allocator's
+0.006–0.012/m thresholds. Both model and action must request a substantial
+turn in the same direction before model pose supplies the full base.
+Small, flat, opposed or zero requests use the curvature contribution; zero
+action produces a zero base. Partial shares combine both contributions.
+The existing pose encoder retains its quantization and field-allocation rules.
+The residual-curvature lift is geometric, not a claim of EPS equivalence to C2.
+
+The inherited pose encoder allocates heading overflow using its asymmetric
+limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad
+heading bound. On clipped tails, this can leave mirrored C0 requests differing
+by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie
+below that heading cap; full model-base odd symmetry is not claimed.
+
+## Measured feedback and limits
+
+```text
+past_request = selected curvature held at or before (measurement_time - delay)
+yaw_error = measured_speed × past_request - measured_yaw_rate
+bias_trial = released_bias + feedback_gain × yaw_error × measurement_dt
+C1_unconstrained = clip(C1_base + accepted_bias, ±0.5 rad)
+C1_target = temporary_backoff_ceiling(C1_unconstrained) if backoff_active
+            otherwise C1_unconstrained
+```
+
+Measured yaw is negated Ford CAN yaw, matching the control sign convention.
+The historical request uses zero-order hold; it never interpolates toward a
+future publication. Nominal comparison delay is `CP.steerActuatorDelay`
+(0.2 seconds on the source vehicle). Feedback compares against selected
+curvature, not curvature inferred from the model-pose coefficients.
+
+| Quantity | Value |
+|---|---:|
+| C0 / C1 final bounds | ±5.11 m / ±0.5 rad |
+| Independent C0 / C1 slew | 4 m/s / 0.5 rad/s |
+| Feedback integration scale | 1.0 |
+| Feedback minimum speed | 2 m/s |
+| Maximum PSCM/core input age | 150 ms |
+| Allowed timestamp lead | 5 ms |
+| Release comparison tolerance | one C1 wire quantum, 0.0005 rad |
+
+The integration scale, preview distances and blend thresholds are effective
+gains; none establishes stability. No wheel-response gain is fitted.
+Zero yaw error retains acquired bias while an eligible turn continues.
+Host anti-windup admits reachable correction within the combined C1 field
+and slew limits. Feedback overflow is not transferred into C0.
+
+The release logic scales bias as the bounded base decreases and resets on
+zero/reversal. When delayed curvature still represents a stronger or opposing
+request, or PSCM reports LimitReached, new integration is normally frozen.
+One exception permits measured-error backoff: measured turning must exceed
+both the delayed and current selected yaw requests in the base's direction,
+and total heading must still have the base's sign. Exceeding only an older,
+smaller request during turn-in does not qualify. The accepted increment may
+only reduce that existing total toward zero; it cannot grow the request or
+carry it through zero. Existing host field and slew limits still apply.
+
+The existing release-recovery exception requires fresh valid PSCM status with
+limit below 2, retained bias opposing the base, and both current and delayed
+requests aligned with that base. Measured turning must be below both requests
+in their direction. It then uses the current yaw deficit × the existing
+feedback gain × measurement interval to unwind only the opposing bias toward
+zero. The increment is clipped so recovery cannot cross zero bias or create
+demand beyond the existing base. Common host anti-windup still limits what
+can be accepted. A separate release-tracking exception is described below;
+other constrained cases remain frozen. PSCM limit 2 never permits either
+request-increasing exception.
+The no-new-bias restriction applies to `release_recovery`. It does not apply
+to the separate bounded `release_tracking` branch. Once release ends,
+ordinary eligible integration can add correction beyond the base as before;
+its existing limits and guards are unchanged.
+
+`release_recovery` and `feedback_recovery_active=true` indicate that the
+recovery branch actually changed bias on that update. If host anti-windup
+blocks the entire increment, the status remains `host_limit` and the flag is
+false. Recovery is evaluated only on fresh measurements; the flag is false
+on repeated-measurement updates and after reset.
+
+Diagnostics distinguish `release_backoff` and `pscm_backoff`; a release takes
+precedence when both conditions apply. While `feedback_backoff_active` is
+true, total C1 is also capped at the preceding continuous heading request in
+the current request direction and at zero in the opposite direction. This
+ceiling affects the output only: it is not stored or projected into bias.
+The measured-error increment can still update bias under the normal limits,
+but a changing model base does not create persistent integral suppression.
+The ceiling persists between repeated measurements; C1 cannot grow or reverse
+while it applies. The next fresh measurement clears it unless backoff is
+again warranted. It does not cap C0, and normal feedback has its own rules
+outside backoff. Independent slew remains 0.5 rad/s for C1 and 4 m/s for C0.
+Backoff still compares against the delayed reference, so response lag remains.
+Reducing a request does not demonstrate that physical overshoot is resolved.
+
+## V8 release guard and tracking
+
+`ReleaseGuard` retains selected-request history independently of feedback
+bias history. Driver-related feedback resets do not erase that reference,
+but the guard still requires current fresh valid PSCM status, no current
+driver override, and the existing input and speed eligibility. Invalid core
+input or disengagement resets its history with the controller.
+
+During release, measured yaw must exceed both the current and delay-matched
+requests in the requested turn direction. Only then does the guard cap
+same-direction C0/C1 growth at each preceding continuous request. Terms
+already reducing the turn, including an opposing C0 centering offset, remain
+available. The guard follows base allocation and C1 feedback, so changing
+model geometry cannot bypass it. Its ceilings affect outputs, never stored
+bias. No scalar-curvature cap replaces strong model geometry during turn-in
+or undertracking. Existing independent slew and field limits still apply.
+
+`release_tracking` addresses an eligible release deficit once bias is zero
+or already in the base's direction. Both current and delayed requests must
+align with that base, measured turning must be below both, and measured
+curvature must not be rising in the turn direction across the response
+interval by more than one C1 wire quantum after scaling by heading preview.
+Fresh valid PSCM status with limit below 2 is required. The current yaw deficit
+uses the existing integration gain and measurement interval;
+new C1 tracking increments are limited by command headroom captured at
+release entry, tapered with remaining desired curvature. The allowance is
+`max(0, entry_command_magnitude - abs(base)) × min(1, abs(desired) / entry_reference)`
+above the current base; any existing same-direction bias consumes it first.
+This limits new tracking integration, not the existing model base or bias.
+Only that additional allowance is tapered; strong model geometry remains
+available. A brief pause does not reacquire a higher entry
+ceiling; a full response interval without release ends the retained episode.
+Common host anti-windup, field and slew bounds still apply. Opposing bias
+continues through `release_recovery`, which stops at zero, before any separate
+tracking exception can be considered.
+
+Neither exception relaxes the PSCM LimitReached growth restriction. The
+reference delay and finite response time remain; these output policies are
+command-construction changes, not evidence of improved physical tracking.
+
+## PSCM status and driver handling
+
+card publishes `Lane_Assist_Data3_FD1` in `carStateSP.fordPscmStatus`, retaining
+the original CAN receipt timestamp. Republishing carStateSP or receiving
+unrelated frames cannot refresh it. The opendbc submodule is unchanged.
+
+Feedback requires valid fresh status, InProgress lateral state (2), capability
+LimitedModeAvailable or ExtendedModeAvailable (1 or 2), and no denial.
+Missing, malformed, stale, backward-timestamped, denied or unavailable status
+clears feedback bias/history and disables the separate release guard,
+leaving the base subject to its core validity gates.
+LimitReached (2) permits only the bounded request-reducing backoff described
+above and otherwise freezes integration. LimitWithDriverActive (3) clears
+feedback. Backoff still requires fresh, valid, InProgress status with an
+available capability and no denial. These generic PSCM reports do not identify
+a specific torque or rate limit.
+
+`steeringPressed`, raw torque above the existing Ford driver allowance, or
+nonfinite torque clear feedback. Below 2 m/s feedback also clears. A fresh
+feedback reference interval is required after override; the independent
+release guard can use retained valid request history once its current gates
+are satisfied. Base requests retain normal
+PSCM driver arbitration while lateral control remains authorized; an unset
+override flag cannot rule out subthreshold driver influence.
+
+## Gates and Sunnylink selection
+
+Core model/action/car-state freshness, finite-value, clock and speed checks
+remain in place. Invalid core inputs reset both commands and clear latActive.
+Raw model geometry is validated on every update, including repeated model
+timestamps; an invalid raw path cannot reuse the cached valid reference.
+Missing PSCM status disables feedback, not an otherwise valid base request.
+
+Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the
+`FordVirtualAngleController` key, default-off setting and offroad/onroad cycle
+requirement. Enabled selects v8 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
+regardless of missing or different EPS firmware-query results. Other platforms
+retain their existing controller. V8 takes priority over PSCM Coefficient
+Observer while selected; disabling and cycling offroad/onroad restores the
+previous selection. Controller selection does not force lateral engagement.
+
+The analyzed firmware is `RL38-14D003-AA`; removing the eligibility check
+is not validation of other firmware. No live device setting is changed.
+
+## Diagnostics and verification
+
+The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v8.
+`model_offset_base` / `model_heading_base` report the already weighted and
+encoded model contribution; `curvature_offset_base` / `curvature_heading_base`
+report the residual-curvature contribution. `model_share` and `base_guard`
+identify model-pose, blended, curvature-only, opposed-model and zero-request
+cases. `heading_base` is the bounded pre-feedback C1. `offset_target` and
+`heading_target` are the final targets after the independent release guard;
+`offset_target_unguarded` and `heading_target_unguarded` retain the inputs to
+that guard. The latter C1 already includes its normal feedback/backoff policy.
+
+The event retains source timestamps, measured curvature/yaw, final commands,
+slew scales, feedback bias/status/history, raw torque and PSCM status/age.
+`feedback_backoff_active` records the persistent heading ceiling, including
+cycles whose feedback status is `no_new_measurement`.
+`release_guard_active` and `release_guard_reference_curvature` expose the
+independent C0/C1 guard and its retained delayed reference.
+`feedback_release_tracking_active`, `feedback_release_ceiling` and
+`feedback_curvature_delta` identify accepted release
+tracking, the total-heading threshold used to admit new bias, and the
+measured-curvature change across the response interval (1/m). The tracking
+flag is true only when the branch accepts a bias change on a new measurement;
+it is false on repeated measurements. The ceiling/trend fields can describe
+an evaluated condition even when no increment is accepted.
+`feedback_recovery_active` records an accepted recovery increment on this
+update only; it does not persist between measurements.
+`feedback_yaw_error` retains its delayed-reference meaning. Recovery instead
+uses current error, reconstructed from logged `desired_curvature`,
+synchronized car-state speed and `yaw_rate`; those two errors can differ.
+During backoff or the independent release guard, `heading_target` can be lower in the request direction than
+the bounded sum of `heading_base` and `heading_bias`, because the temporary
+ceiling is not part of the stored bias.
+`model_heading_target` remains a filtered comparison reference; it is not the
+weighted model contribution. `angleState.saturated` is not an EPS-limit signal.
+
+Validation must cover large recorded maneuvers, flat-model centering, both
+turn directions, model/action disagreement, share transitions, release and
+reversal, release/limit backoff without growth or zero crossing, status/driver
+resets, reference causality, bounds, slew and CAN packing with C2/C3 zero.
+Recovery checks cover both directions, stopping at zero bias, repeated
+measurements, current-and-delayed agreement, and rejection at PSCM limit 2.
+Old v3/v4 command-equality expectations do not define
+v8 success. Guard checks also cover driver reset/history rebuilding,
+same-direction growth, opposing coefficients, repeated measurements,
+undertracking and invalid-status inhibition. Tracking checks cover delayed
+curvature trends and tapered release-entry headroom. Historical v5–v7 replay
+results remain historical observations.
+
+The v8 recorded-input fixture contains 15,273 cycles with 4,879 selected
+evidence samples. Base allocation and output eligibility match v7. In the
+clean deficient exit, median absolute C1 changes from 0.0665 to 0.0845 rad
+while C0 stays unchanged. The growth guard also acts while feedback history
+rebuilds; the largest over-growth witness includes nearby driver input and
+is excluded from the strict autonomous tracking score. Both good comparison
+curves in that fixture retain their median requests, and the older large-turn
+fixtures retain their required command scale.
+
+On the earlier good drive, one comparison curve retains extra C1 after
+eligible release tracking: median magnitude changes from 0.121 to 0.128 rad.
+In its 103–110 s interval, tracking increments occur only while measured
+turning falls short, with a median current response/request ratio of 0.895.
+Acquired bias can persist after matching, as with ordinary integral feedback.
+This collateral command change remains a reason to compare new vehicle logs.
+Replay fixes recorded motion and planner outputs, so enabled vehicle logs
+are still required to assess tracking error, oscillation and interventions.

openpilot/cereal/custom.capnp

@@ -383,6 +383,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  fordLateralPath @5 :FordLateralPath;
 
   struct Param {
     key @0 :Text;
@@ -403,6 +404,14 @@ struct CarControlSP @0xa5cd762cd951a455 {
   }
 }
 
+struct FordLateralPath {
+  pathOffset @0 :Float32;     # c0 [m]
+  pathAngle @1 :Float32;      # c1 [rad]
+  curvature @2 :Float32;      # c2 [1/m]
+  curvatureRate @3 :Float32;  # c3 [1/m^2]
+  valid @4 :Bool;
+}
+
 struct BackupManagerSP @0xf98d843bfd7004a3 {
   backupStatus @0 :Status;
   restoreStatus @1 :Status;
@@ -447,6 +456,16 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 
 struct CarStateSP @0xb86e6369214c01c8 {
   speedLimit @0 :Float32;
+  fordPscmStatus @1 :FordPscmStatus;
+
+  struct FordPscmStatus {
+    valid @0 :Bool;
+    canMonoTime @1 :UInt64;  # Last accepted Lane_Assist_Data3_FD1 CAN receipt, not carStateSP publication time.
+    lateralState @2 :UInt8;  # LatCtlSte_D_Stat
+    limit @3 :UInt8;  # LatCtlLim_D_Stat: generic lateral limit, not a torque/rate diagnosis.
+    capability @4 :UInt8;  # LatCtlCpblty_D_Stat
+    denied @5 :Bool;  # LaActDeny_B_Actl
+  }
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {
@@ -470,7 +489,30 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
+  enabled @0 :Bool;
+  madsDistanceMeters @1 :Float64;
+  fullAssistDistanceMeters @2 :Float64;
+  event @3 :Event;
+
+  struct Event {
+    id @0 :UInt64;
+    category @1 :Category;
+    distanceMeters @2 :Float64;
+    previousDistanceMeters @3 :Float64;
+    unit @4 :Unit;
+  }
+
+  enum Category {
+    none @0;
+    mads @1;
+    fullAssist @2;
+  }
+
+  enum Unit {
+    imperial @0;
+    metric @1;
+  }
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

openpilot/cereal/log.capnp

@@ -2642,7 +2642,7 @@ struct Event {
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
     modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;
     customReserved13 @139 :Custom.CustomReserved13;

openpilot/cereal/services.py

@@ -90,6 +90,7 @@ _services: dict[str, tuple] = {
   "carParamsSP": (True, 0.02, 1),
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
+  "assistedDrivingMilestoneState": (True, 10., 1),
   "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20., None, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),

openpilot/common/params.cc

@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
 }
 
 Params::~Params() {
+  flushNonBlockingWrites();
+}
+
+void Params::flushNonBlockingWrites() {
   if (future.valid()) {
     future.wait();
   }

openpilot/common/params.h

@@ -75,6 +75,7 @@ public:
     return put(key.c_str(), val ? "1" : "0", 1);
   }
   void putNonBlocking(const std::string &key, const std::string &val);
+  void flushNonBlockingWrites();
   inline void putBoolNonBlocking(const std::string &key, bool val) {
     putNonBlocking(key, val ? "1" : "0");
   }

openpilot/common/params.py

@@ -73,6 +73,7 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
 params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
 params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
 params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
+params_flush = _bind("params_flush", [ParamsHandle])
 params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
 params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
 params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
@@ -178,6 +179,10 @@ class Params:
   def put_bool(self, key, val, block=False):
     params_put_bool(self.p, self.check_key(key), val, block)
 
+  def flush(self):
+    """Wait for all prior nonblocking writes from this Params instance."""
+    params_flush(self.p)
+
   def remove(self, key):
     params_remove(self.p, self.check_key(key))
 

openpilot/common/params_c.cc

@@ -133,6 +133,12 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
   });
 }
 
+void params_flush(ParamsHandle *handle) noexcept {
+  translate_exceptions([&]() {
+    handle->params.flushNonBlockingWrites();
+  });
+}
+
 int params_remove(ParamsHandle *handle, const char *key) noexcept {
   return translate_exceptions(-1, [&]() {
     return handle->params.remove(key);
@@ -162,12 +168,15 @@ ParamsBuffer params_key_at(ParamsHandle *handle, size_t index) noexcept {
 
 size_t params_keys_by_flag(ParamsHandle *handle, uint32_t flag, ParamsBuffer *out, size_t out_size) noexcept {
   return translate_exceptions(size_t{0}, [&]() {
-    auto filtered = handle->params.allKeys(static_cast<ParamKeyFlag>(flag));
+    size_t count = 0;
-    size_t count = std::min(filtered.size(), out_size);
+    for (const auto &key : handle->keys) {
-    for (size_t i = 0; i < count; i++) {
+      if (flag == ALL || (handle->params.getKeyFlag(key) & flag)) {
-      out[i] = return_string(filtered[i]);
+        // Each buffer borrows a different string, stable for the handle's lifetime.
+        if (count < out_size) out[count] = {key.data(), key.size()};
+        ++count;
       }
-    return filtered.size();
+    }
+    return count;
   });
 }
 

openpilot/common/params_keys.h

@@ -143,6 +143,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // --- sunnypilot params --- //
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
+    {"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
+    {"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
     {"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}},  // seconds
@@ -163,6 +165,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
     {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -172,7 +175,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"LastGPSPositionLLK", {PERSISTENT, STRING}},
+    {"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
     {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
     {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
     {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
     {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
@@ -232,6 +237,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"BackupManager_RestoreVersion", {PERSISTENT, STRING}},
 
     // sunnypilot car specific params
+    {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
     {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},

openpilot/common/tests/test_params.py

@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
     assert q.get("CarParams") is None
     assert q.get("CarParams", True) == b"1"
 
+  def test_flush_non_blocking_writes(self):
+    self.params.put("DongleId", "first")
+    self.params.put("DongleId", "last")
+    self.params.flush()
+
+    assert self.params.get("DongleId") == "last"
+
   def test_params_all_keys(self):
     keys = Params().all_keys()
 
@@ -126,6 +133,16 @@ class TestParams(OpenpilotTestCase):
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 
+  def test_filtered_keys_are_distinct_registered_strings(self):
+    registered = set(self.params.all_keys())
+    for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP, ParamKeyFlag.CLEAR_ON_MANAGER_START):
+      filtered = self.params.all_keys(flag)
+      assert len(filtered) > 1
+      assert len(filtered) == len(set(filtered))
+      assert set(filtered) <= registered
+      assert all(key.decode('utf-8') for key in filtered)
+      assert self.params.all_keys(flag) == filtered
+
   def test_params_get_type(self):
     # json
     self.params.put("ApiCache_FirehoseStats", {"a": 0}, block=True)

openpilot/selfdrive/car/card.py

@@ -21,6 +21,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp
+from openpilot.selfdrive.car.ford_pscm_status import populate_ford_pscm_status
 
 from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
@@ -198,6 +199,7 @@ class Car:
     # Update carState from CAN
     CS, CS_SP = self.CI.update(can_list)
     CS_SP = convert_to_capnp(CS_SP)
+    populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid)
 
     # Update radar tracks from CAN
     RD: structs.RadarDataT | None = self.RI.update(can_list)

openpilot/selfdrive/car/ford_pscm_status.py

@@ -0,0 +1,36 @@
+"""Publish the Ford PSCM's actual CAN status without changing opendbc structs."""
+import math
+
+from opendbc.car import Bus
+from opendbc.car.ford.values import FordFlags
+
+
+MESSAGE = 'Lane_Assist_Data3_FD1'
+SIGNALS = ('LatCtlSte_D_Stat', 'LatCtlLim_D_Stat', 'LatCtlCpblty_D_Stat', 'LaActDeny_B_Actl')
+
+
+def populate_ford_pscm_status(CP, can_parsers, CS_SP, can_valid):
+  if CP.brand != 'ford' or not CP.flags & FordFlags.CANFD:
+    return
+  status = CS_SP.init('fordPscmStatus')
+  parser = can_parsers.get(Bus.pt)
+  if parser is None:
+    return
+  values = parser.vl.get(MESSAGE, {})
+  timestamps = parser.ts_nanos.get(MESSAGE, {})
+  if any(signal not in values or signal not in timestamps for signal in SIGNALS):
+    return
+  received = timestamps[SIGNALS[0]]
+  if received <= 0 or any(timestamps[signal] != received for signal in SIGNALS):
+    return
+  decoded = [values[signal] for signal in SIGNALS]
+  if any(not math.isfinite(value) or int(value) != value or not 0 <= value <= maximum
+         for value, maximum in zip(decoded, (7, 3, 3, 1), strict=True)):
+    return
+  status.canMonoTime = received
+  status.lateralState, status.limit, status.capability = map(int, decoded[:3])
+  status.denied = bool(decoded[3])
+  # CI.update already checked all parser validity. Reading can_valid again here
+  # would advance the parser's invalid-message counter a second time per tick.
+  # Age is evaluated by the feedback consumer using this original CAN timestamp.
+  status.valid = bool(can_valid)

openpilot/selfdrive/car/helpers.py

@@ -63,5 +63,6 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
     **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
   )
+  struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
 
   return struct_dataclass

openpilot/selfdrive/car/tests/test_ford_pscm_status.py

@@ -0,0 +1,109 @@
+import ast
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.ford_pscm_status import MESSAGE, SIGNALS, populate_ford_pscm_status
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from opendbc.can import CANPacker, CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.ford.values import FordFlags
+
+
+class TestFordPscmStatus(unittest.TestCase):
+  def setUp(self):
+    self.cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+    self.packer = CANPacker('ford_lincoln_base_pt')
+    self.parser = CANParser('ford_lincoln_base_pt', [(MESSAGE, 33), ('Yaw_Data_FD1', 100)], 0)
+
+  def update_status(self, timestamp, *, lateral_state=2, limit=0, capability=2, denied=False):
+    status = self.packer.make_can_msg(MESSAGE, 0, dict(zip(SIGNALS, (lateral_state, limit, capability, denied), strict=True)))
+    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': 0.1})
+    self.parser.update([(timestamp, [status, yaw])])
+
+  def publish(self, *, can_valid=True):
+    state_sp = convert_to_capnp(structs.CarStateSP(speedLimit=13.5))
+    populate_ford_pscm_status(self.cp, {Bus.pt: self.parser}, state_sp, can_valid)
+    return state_sp
+
+  def test_decodes_status_and_preserves_receipt_time_across_other_can_messages(self):
+    self.update_status(1_000_000_000, limit=2, capability=1, denied=True)
+    original = self.publish()
+    self.assertEqual(original.speedLimit, 13.5)
+    status = original.fordPscmStatus
+    self.assertTrue(status.valid)
+    self.assertEqual(status.canMonoTime, 1_000_000_000)
+    self.assertEqual((status.lateralState, status.limit, status.capability, status.denied), (2, 2, 1, True))
+
+    # carStateSP may publish at 100 Hz while this 33 Hz message is absent. New
+    # unrelated CAN must not freshen the timestamp of an old PSCM status.
+    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': .2})
+    self.parser.update([(1_080_000_000, [yaw])])
+    copied = self.publish().fordPscmStatus
+    self.assertEqual(copied.canMonoTime, 1_000_000_000)
+    self.assertEqual((copied.limit, copied.capability, copied.denied), (2, 1, True))
+
+    self.update_status(1_090_000_000, lateral_state=3, limit=3, capability=2)
+    next_state = self.publish()
+    with custom.CarStateSP.from_bytes(next_state.to_bytes()) as decoded:
+      latest = decoded.fordPscmStatus
+      self.assertTrue(latest.valid)
+      self.assertEqual(latest.canMonoTime, 1_090_000_000)
+      self.assertEqual((latest.lateralState, latest.limit, latest.capability, latest.denied), (3, 3, 2, False))
+
+  def test_absent_parser_unseen_message_and_invalid_can_do_not_claim_valid_status(self):
+    state = custom.CarStateSP.new_message()
+    populate_ford_pscm_status(self.cp, {}, state, True)
+    self.assertFalse(state.fordPscmStatus.valid)
+    self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
+    self.assertFalse(self.publish().fordPscmStatus.valid)
+    self.update_status(1_000_000_000)
+    invalid = self.publish(can_valid=False).fordPscmStatus
+    self.assertFalse(invalid.valid)
+    self.assertEqual(invalid.canMonoTime, 1_000_000_000)
+
+  def test_mixed_timestamps_or_malformed_status_cannot_enable_feedback(self):
+    self.update_status(1_000_000_000)
+    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 990_000_000
+    self.assertFalse(self.publish().fordPscmStatus.valid)
+    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 1_000_000_000
+    for value in (float('nan'), -1, 1.5, 4):
+      self.parser.vl[MESSAGE]['LatCtlLim_D_Stat'] = value
+      self.assertFalse(self.publish().fordPscmStatus.valid)
+
+  def test_other_vehicles_and_legacy_messages_default_to_unavailable(self):
+    for cp in (SimpleNamespace(brand='toyota'), SimpleNamespace(brand='ford', flags=0)):
+      state = custom.CarStateSP.new_message(speedLimit=10.)
+      populate_ford_pscm_status(cp, {}, state, True)
+      self.assertFalse(state.fordPscmStatus.valid)
+      self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
+      self.assertEqual(state.speedLimit, 10.)
+    # Old recordings/readers have no appended status pointer; defaults must
+    # remain unavailable rather than interpreting zeroed enums as fresh data.
+    self.assertFalse(custom.CarStateSP.new_message().fordPscmStatus.valid)
+
+  def test_actual_card_update_populates_status_after_dataclass_conversion(self):
+    self.update_status(1_000_000_000, limit=1)
+    source_path = Path(__file__).resolve().parents[1] / 'card.py'
+    source = ast.parse(source_path.read_text())
+    car_class = next(n for n in source.body if isinstance(n, ast.ClassDef) and n.name == 'Car')
+    method = next(n for n in car_class.body if isinstance(n, ast.FunctionDef) and n.name == 'state_update')
+    statements = method.body
+    first = next(i for i, n in enumerate(statements) if isinstance(n, ast.Assign) and ast.unparse(n.value) == 'self.CI.update(can_list)')
+    last = next(i for i, n in enumerate(statements) if isinstance(n, ast.Expr) and isinstance(n.value, ast.Call)
+                and isinstance(n.value.func, ast.Name) and n.value.func.id == 'populate_ford_pscm_status')
+    self.assertGreater(last, first)
+    code = compile(ast.Module(body=statements[first:last + 1], type_ignores=[]), str(source_path), 'exec')
+    ci = SimpleNamespace(update=lambda _: (SimpleNamespace(canValid=True), structs.CarStateSP(speedLimit=11.)),
+                         can_parsers={Bus.pt: self.parser})
+    environment = {'self': SimpleNamespace(CP=self.cp, CI=ci), 'can_list': [], 'convert_to_capnp': convert_to_capnp,
+                   'populate_ford_pscm_status': populate_ford_pscm_status}
+    exec(code, environment)
+    self.assertTrue(environment['CS_SP'].fordPscmStatus.valid)
+    self.assertEqual(environment['CS_SP'].fordPscmStatus.canMonoTime, 1_000_000_000)
+    self.assertEqual(environment['CS_SP'].fordPscmStatus.limit, 1)
+
+
+if __name__ == '__main__':
+  unittest.main()

openpilot/selfdrive/controls/controlsd.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import math
+import time
 from numbers import Number
 
 from openpilot.cereal import log
@@ -11,8 +12,11 @@ from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority
 from openpilot.common.swaglog import cloudlog
 
 from opendbc.car.car_helpers import interfaces
+from opendbc.car.ford.values import FordFlags
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -44,7 +48,7 @@ class Controls(ControlsExt):
     self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
 
     self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carStateSP', 'carOutput',
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
                                   poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
@@ -52,6 +56,15 @@ class Controls(ControlsExt):
     self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
+    self.ford_pscm_observer = (self.CP.brand == "ford" and self.CP.flags & FordFlags.CANFD and
+                               self.params.get_bool("FordPscmObserver"))
+    self.ford_path_controller = FordPscmObserverPathController() if self.ford_pscm_observer else FordPathController()
+    self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
+                                                              self.ford_path_controller)
+    self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
+    if self.CP.brand == "ford":
+      cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__)
+    self.ford_path = FordPath()
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -155,6 +168,34 @@ class Controls(ControlsExt):
       actuators.curvature = float(lateral_output)
     else:
       actuators.steeringAngleDeg = float(lateral_output)
+    if self.CP.brand == "ford":
+      ford_model = model_v2 if self.sm.valid['modelV2'] else None
+      if self.ford_model_action:
+        reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
+        self.ford_path = self.ford_path_controller.update(
+          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
+          measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
+          model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
+          reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
+          active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
+        )
+        if not self.ford_path.valid:
+          CC.latActive = False
+        if self.sm.frame % 20 == 0:
+          cloudlog.event("Ford C2-free path tracking", model_mono_time=self.sm.logMonoTime['modelV2'],
+                         measurement_mono_time=self.sm.logMonoTime['carState'],
+                         reference_service=reference_service, reference_mono_time=self.sm.logMonoTime[reference_service],
+                         measured_curvature=self.curvature,
+                         **self.ford_path_controller.diagnostics)
+      elif self.ford_pscm_observer:
+        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
+                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
+                                                          v_ego_raw=CS.vEgoRaw, active=CC.latActive)
+      else:
+        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
+                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
+                                                          active=CC.latActive)
+      actuators.curvature = float(self.ford_path.curvature)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -0,0 +1,190 @@
+"""Experimental Ford C2-free controller: nearby offset and selected-action heading.
+
+Selected only by its explicit toggle. The 7 m station and one-second scale are
+engineering choices, not identified PSCM gains or physical calibration.
+"""
+import math
+import struct
+
+import numpy as np
+
+from opendbc.car.ford.values import FordFlags
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
+
+
+OFFSET_STATION_M = 7.0
+HEADING_TIME_S = 1.0
+EXCESS_YAW_DEADBAND = .02  # rad/s; above the observed approximately .008 rad/s Ford yaw offset
+EXCESS_YAW_LOOKAHEAD_S = .2  # engineering choice, not an identified PSCM delay
+CALIBRATION_APPROVED = False
+PREDICTION_TIME_S = .15  # geometric preview, not an identified actuator delay
+
+
+def _predict_offset(path, c0, desired_curvature, speed):
+  """Read the same path from a predicted pose along the selected curvature.
+
+  A matched constant-radius path retains its offset. Developing/flattening
+  bends can move the target earlier. The core retains field limits and slew.
+  Use only available geometry; shortened horizons taper prediction to zero.
+  """
+  station, longitudinal, lateral, _ = path
+  distance = min(speed*PREDICTION_TIME_S, max(0., station[-1]-OFFSET_STATION_M))
+  if distance == 0.:
+    return c0
+  x = float(np.interp(OFFSET_STATION_M+distance, station, longitudinal))
+  y = float(np.interp(OFFSET_STATION_M+distance, station, lateral))
+  rotation = desired_curvature*distance
+  # (1-cos(rotation))/curvature, evaluated without cancellation or division by zero.
+  translation = distance*math.sin(rotation/2)*float(np.sinc(rotation/(2*math.pi)))
+  predicted = math.cos(rotation)*y-math.sin(rotation)*x+translation
+  if not _finite(predicted):
+    return c0
+  return predicted
+
+
+def _packed(value, resolution, offset):
+  """Mirror Float32 carControlSP and sign-reversed CANPacker rounding."""
+  value = struct.unpack("f", struct.pack("f", value))[0]
+  return -(math.floor((-value - offset) / resolution + 0.5) * resolution + offset)
+
+
+def _finite(*values):
+  try:
+    return all(math.isfinite(value) for value in values)
+  except (TypeError, ValueError, OverflowError):
+    return False
+
+
+def encode_model_action(model, desired_curvature, speed):
+  """Encode predicted y(7) and max(7, v*1s)*selected limited curvature.
+
+  Preserve the reviewed core's endpoint hold when the path ends before 7 m.
+  This samples the available geometry; it does not extrapolate an unseen path.
+  """
+  if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
+    return FordPath()
+  try:
+    path = _model_path(model)
+  except OverflowError:
+    return FordPath()
+  if path is None or not all(_finite(*values) for values in path):
+    return FordPath()
+  station, _, lateral, _ = path
+  c0 = float(np.interp(min(OFFSET_STATION_M, station[-1]), station, lateral))
+  c0 = _predict_offset(path, c0, desired_curvature, speed)
+  c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
+  return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
+
+
+def damp_offset(c0, desired_curvature, speed, yaw_rate):
+  """Attenuate same-direction C0 demand when yaw exceeds the requested turn.
+
+  Inputs are finite and range-checked by the caller. The deadband avoids
+  chasing small yaw offsets. Opposing centering demand is left intact.
+  """
+  if c0*yaw_rate <= 0.:
+    return c0
+  direction = math.copysign(1., c0)
+  # An opposed plan must not amplify near-zero yaw bias into a large correction.
+  requested_yaw = max(0., direction*speed*desired_curvature)
+  excess = max(0., direction*yaw_rate-requested_yaw-EXCESS_YAW_DEADBAND)
+  reduction = OFFSET_STATION_M*EXCESS_YAW_LOOKAHEAD_S*excess
+  return direction*max(0., abs(c0)-reduction)
+
+
+class ModelActionController:
+  """Only two control states: unquantized, independently slewed C0 and C1.
+
+  Freshness and engagement belong to the caller. Excess yaw attenuates the
+  offset target without model history, an integral or release modes.
+  """
+  __slots__ = ('c0', 'c1')
+
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.c0 = self.c1 = 0.
+
+  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True):
+    # The production adapter always supplies validated measured yaw.
+    if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3:
+      self.reset()
+      return FordPath()
+    target = encode_model_action(model, desired_curvature, speed)
+    if not target.valid:
+      self.reset()
+      return FordPath()
+    c0 = float(np.clip(target.path_offset, -5.11, 5.11))
+    c0 = damp_offset(c0, desired_curvature, speed, yaw_rate)
+    c1 = float(np.clip(target.path_angle, -.5, .5))
+    self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
+    self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt))
+    return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.)
+
+
+class FordModelActionController:
+  """Input adapter for the opt-in selected-action controller.
+
+  controlsd owns upstream selection/limiting and service health. This adapter
+  checks ages and clock order, then supplies elapsed time to the two-state
+  core. Its timestamps and diagnostics never affect the targets. Raw model
+  geometry is checked on every cycle, even at a repeated model timestamp.
+
+  Validated host-coordinate yaw supplies stateless offset damping. Engagement
+  and downstream driver arbitration still apply. PSCM status and driver torque
+  are not control-law inputs.
+  """
+  def __init__(self):
+    self.core = ModelActionController()
+    self.reset()
+
+  def reset(self, status='inactive'):
+    self.core.reset()
+    self.last_time = self.last_measurement_time = self.last_model_time = None
+    self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-prediction-v4',
+                        'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
+
+  def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
+             active, valid=True):
+    reason = None
+    if not active:
+      reason = 'inactive'
+    elif not valid:
+      reason = 'invalid_service'
+    elif not _finite(desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
+      reason = 'nonfinite'
+    elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)):
+      reason = 'stale_input'
+    elif not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1:
+      reason = 'input_range'
+    if reason is not None:
+      self.reset(reason)
+      return FordPath()
+
+    dt = .01 if self.last_time is None else now - self.last_time
+    if not .002 <= dt <= .1 or (self.last_measurement_time is not None and measurement_time < self.last_measurement_time) or (
+      self.last_model_time is not None and model_time < self.last_model_time
+    ):
+      self.reset('timing_reset')
+      return FordPath()
+    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate)
+    if not command.valid:
+      self.reset('invalid_path')
+      return command
+    self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-prediction-v4',
+                        'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
+                        'yaw_rate': yaw_rate,
+                        'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
+                        'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
+                        'command': (command.path_offset, command.path_angle, 0., 0.)}
+    return command
+
+
+def select_model_action_controller(CP, enabled, previous_controller):
+  """The separate default-off toggle takes priority on the CAN FD Lightning."""
+  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD and CP.carFingerprint == 'FORD_F_150_LIGHTNING_MK1'
+  if enabled and compatible:
+    return FordModelActionController()
+  return previous_controller

openpilot/selfdrive/controls/lib/ford_path.py

@@ -0,0 +1,368 @@
+from collections import deque
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+from opendbc.car.ford.values import CarControllerParams
+
+
+DBC_OFFSET = (-5.12, 5.11)
+DBC_ANGLE = (-0.5, 0.5235)
+DBC_CURVATURE = (-0.02, 0.02)
+DBC_CURVATURE_RATE = (-0.001024, 0.001023)
+
+DBC_OFFSET_RESOLUTION = 0.01
+DBC_ANGLE_RESOLUTION = 0.0005
+DBC_CURVATURE_RESOLUTION = 0.00002
+DBC_CURVATURE_RATE_RESOLUTION = 0.000001
+_PATH_MIN_LOOKAHEAD = 7.0
+_POSE_PREDICTION_TIME = 0.1
+_POSE_BLEND_CURVATURE = (0.006, 0.012)
+_PATH_OFFSET_RATE = 4.0
+_PATH_ANGLE_RATE = 1.0
+
+_PSCM_DT = 0.004
+_PSCM_C0_RATE = 1.5
+_PSCM_C1_RATE = 0.100006103515625
+_PSCM_C2_RATE = 0.0030059814453125
+_PSCM_SPEED_KPH = (0.0, 15.0, 40.0, 70.0, 100.0, 150.0, 200.0, 250.0)
+_PSCM_SPEED_GAIN = (32.0, 32.0, 32.0, 30.0, 30.0, 24.0, 12.0, 0.0)
+_PSCM_C0_EFFECTIVE_LIMIT = 1.0
+_PSCM_C1_EFFECTIVE_LIMIT = 0.349609375 / 10.0
+
+
+@dataclass(frozen=True)
+class FordPath:
+  valid: bool = False
+  path_offset: float = 0.0
+  path_angle: float = 0.0
+  curvature: float = 0.0
+  curvature_rate: float = 0.0
+
+
+@dataclass(frozen=True)
+class FordPscmState:
+  path_offset: float = 0.0
+  path_angle: float = 0.0
+  curvature: float = 0.0
+
+
+@dataclass(frozen=True)
+class FordModelPose:
+  path_offset: float
+  path_angle: float
+  offset_horizon: float
+  curvature_demand: float
+  forward_angle: float
+
+
+def _finite(value: float) -> float:
+  return float(value) if math.isfinite(value) else 0.0
+
+
+def _sample(distance: float, distances: list[float], values: list[float]) -> float:
+  return float(np.interp(distance, distances, values))
+
+
+def _blend_share(demand: float) -> float:
+  lower, upper = _POSE_BLEND_CURVATURE
+  return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
+
+
+def _model_path(model) -> tuple[list[float], list[float], list[float], list[float]] | None:
+  try:
+    x = [float(value) for value in model.position.x]
+    y = [float(value) for value in model.position.y]
+    heading = [float(value) for value in model.orientation.z]
+  except (AttributeError, TypeError, ValueError):
+    return None
+  if len(x) < 2 or len(x) != len(y) or len(x) != len(heading):
+    return None
+  if not all(math.isfinite(value) for values in (x, y, heading) for value in values):
+    return None
+
+  distance = [0.0]
+  for i in range(1, len(x)):
+    distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]))
+  if distance[-1] <= 0.0:
+    return None
+
+  unwrapped_heading = [heading[0]]
+  for value in heading[1:]:
+    delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi
+    unwrapped_heading.append(unwrapped_heading[-1] + delta)
+  return distance, x, y, unwrapped_heading
+
+
+def _predicted_pose(distance: float, current_curvature: float,
+                    curvature_delta: float) -> tuple[float, float, float]:
+  curvature = current_curvature + 0.5 * curvature_delta
+  heading = curvature * distance
+  if abs(curvature) < 1e-9:
+    return distance, 0.0, 0.0
+  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
+
+
+def _relative_pose(target_distance: float, path: tuple[list[float], list[float], list[float], list[float]],
+                   vehicle_pose: tuple[float, float, float]) -> tuple[float, float]:
+  distance, x, y, heading = path
+  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
+  dx = _sample(target_distance, distance, x) - vehicle_x
+  dy = _sample(target_distance, distance, y) - vehicle_y
+  cosine = math.cos(vehicle_heading)
+  sine = math.sin(vehicle_heading)
+  offset = -sine * dx + cosine * dy
+  angle = math.atan2(math.sin(_sample(target_distance, distance, heading) - vehicle_heading),
+                     math.cos(_sample(target_distance, distance, heading) - vehicle_heading))
+  return offset, angle
+
+
+def _path_pose(target_distance: float,
+               path: tuple[list[float], list[float], list[float], list[float]]) -> tuple[float, float, float]:
+  distance, x, y, heading = path
+  return (_sample(target_distance, distance, x), _sample(target_distance, distance, y),
+          _sample(target_distance, distance, heading))
+
+
+def _bounded_feedback(feedforward: float, feedback: float, resolution: float, zero_path_limit: float) -> float:
+  quantization_threshold = 0.5 * resolution
+  limit = max(abs(feedforward) - resolution, 0.0) if abs(feedforward) >= quantization_threshold else zero_path_limit
+  return float(np.clip(feedback, -limit, limit))
+
+
+def _model_pose(path: tuple[list[float], list[float], list[float], list[float]],
+                current_curvature: float, curvature_delta: float, v_ego: float) -> FordModelPose:
+  distance, _, _, _ = path
+  advance = min(v_ego * _POSE_PREDICTION_TIME, distance[-1])
+  offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1] - advance)
+  angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1] - advance)
+
+  # Keep the model's remaining path as feedforward. Measured vehicle motion is
+  # a separate, short delay-aligned correction, so catching the requested
+  # curvature cannot erase a turn that is still present in the model path.
+  model_pose = _path_pose(advance, path)
+  model_offset, _ = _relative_pose(advance + offset_horizon, path, model_pose)
+  _, model_angle = _relative_pose(advance + angle_horizon, path, model_pose)
+  vehicle_pose = _predicted_pose(advance, current_curvature, curvature_delta)
+  feedback_offset, feedback_angle = _relative_pose(advance, path, vehicle_pose)
+  gentle_curvature = _POSE_BLEND_CURVATURE[0]
+  feedback_offset = _bounded_feedback(model_offset, feedback_offset, DBC_OFFSET_RESOLUTION,
+                                       0.5 * gentle_curvature * advance ** 2)
+  feedback_angle = _bounded_feedback(model_angle, feedback_angle, DBC_ANGLE_RESOLUTION,
+                                      gentle_curvature * advance)
+
+  offset_curvature = 2.0 * model_offset / max(offset_horizon, 1e-3) ** 2
+  angle_curvature = model_angle / max(angle_horizon, 1e-3)
+  return FordModelPose(model_offset + feedback_offset, model_angle + feedback_angle, offset_horizon,
+                       max(abs(offset_curvature), abs(angle_curvature)), model_angle)
+
+
+def _encode_pose(pose: FordModelPose, pose_share: float, curvature: float) -> FordPath:
+  path_offset = pose_share * pose.path_offset
+  path_angle = pose_share * pose.path_angle
+  if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION:
+    path_offset = 0.0
+  if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION:
+    path_angle = 0.0
+  limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
+  path_offset += (path_angle - limited_path_angle) * pose.offset_horizon
+  return FordPath(
+    valid=True,
+    path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
+    path_angle=limited_path_angle,
+    curvature=float(np.clip(curvature, *DBC_CURVATURE)),
+    curvature_rate=0.0,
+  )
+
+
+def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float,
+                 current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath:
+  pose = _model_pose(path, current_curvature, curvature_delta, v_ego)
+  pose_share = _blend_share(max(pose.curvature_demand, abs(desired_curvature)))
+
+  # Match upstream's C2-only normal driving, then continuously transfer the
+  # command to the model pose for larger maneuvers. An opposing/finished model
+  # path must unload sticky C2 and retain the fast pose needed to unwind it.
+  c2_opposes_path = desired_curvature != 0.0 and desired_curvature * pose.forward_angle <= 0.0
+  if c2_opposes_path:
+    pose_share = 1.0
+    curvature = 0.0
+  else:
+    curvature = desired_curvature * (1.0 - pose_share)
+
+  return _encode_pose(pose, pose_share, curvature)
+
+
+class FordPathController:
+  """Blend normal C2 following into the model's forward C0/C1 pose."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
+
+  def _limit(self, target: FordPath) -> FordPath:
+    offset_delta = target.path_offset - self._last_path.path_offset
+    angle_delta = target.path_angle - self._last_path.path_angle
+    scale = min(
+      1.0,
+      _PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
+      _PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
+    )
+    self._last_path = FordPath(
+      True,
+      self._last_path.path_offset + scale * offset_delta,
+      self._last_path.path_angle + scale * angle_delta,
+      self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
+      0.0,
+    )
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
+             v_ego: float = 0.0, active: bool = True) -> FordPath:
+    if not active:
+      self._last_path = FordPath(valid=True)
+      self._curvature_history.clear()
+      return FordPath()
+    current_curvature = _finite(current_curvature)
+    self._curvature_history.append(current_curvature)
+    curvature_delta = (current_curvature - self._curvature_history[0]
+                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
+    path = _model_path(model) if model is not None else None
+    if path is None:
+      return self._limit(FordPath(valid=True))
+    return self._limit(_encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
+                                    max(_finite(v_ego), 0.0)))
+
+
+def _pscm_slew(value: float, target: float, rate: float, ticks: int) -> float:
+  step = rate * _PSCM_DT * ticks
+  return float(np.clip(target, value - step, value + step))
+
+
+def _pscm_speed_gain(v_ego: float) -> float:
+  return float(np.interp(max(v_ego, 0.0) * 3.6, _PSCM_SPEED_KPH, _PSCM_SPEED_GAIN))
+
+
+def _wire_path(path: FordPath) -> FordPath:
+  return FordPath(
+    valid=path.valid,
+    path_offset=round(path.path_offset / DBC_OFFSET_RESOLUTION) * DBC_OFFSET_RESOLUTION,
+    path_angle=round(path.path_angle / DBC_ANGLE_RESOLUTION) * DBC_ANGLE_RESOLUTION,
+    curvature=round(path.curvature / DBC_CURVATURE_RESOLUTION) * DBC_CURVATURE_RESOLUTION,
+    curvature_rate=round(path.curvature_rate / DBC_CURVATURE_RATE_RESOLUTION) * DBC_CURVATURE_RATE_RESOLUTION,
+  )
+
+
+def _pscm_contributions(state: FordPscmState, v_ego: float) -> tuple[float, float, float]:
+  gain = _pscm_speed_gain(v_ego)
+  return (
+    float(np.clip(0.5 * gain * state.path_offset, -0.5 * gain, 0.5 * gain)),
+    float(np.clip(10.0 * gain * state.path_angle, -0.349609375 * gain, 0.349609375 * gain)),
+    float(np.clip(0.30078125 * gain * state.curvature * v_ego ** 2, -0.5 * gain, 0.5 * gain)),
+  )
+
+
+class FordPscmObserver:
+  """Mirror the firmware's held-command coefficient states at its 250 Hz step."""
+
+  def __init__(self):
+    self.state = FordPscmState()
+    self.command = FordPath(valid=True)
+    self._phase = 0.0
+
+  def reset(self) -> None:
+    self.state = FordPscmState()
+    self.command = FordPath(valid=True)
+    self._phase = 0.0
+
+  def advance(self, elapsed: float) -> None:
+    self._phase += max(elapsed, 0.0)
+    ticks = int((self._phase + 1e-12) / _PSCM_DT)
+    self._phase -= ticks * _PSCM_DT
+    if ticks == 0:
+      return
+    self.state = FordPscmState(
+      _pscm_slew(self.state.path_offset, self.command.path_offset, _PSCM_C0_RATE, ticks),
+      _pscm_slew(self.state.path_angle, self.command.path_angle, _PSCM_C1_RATE, ticks),
+      _pscm_slew(self.state.curvature, self.command.curvature + 10.0 * self.command.curvature_rate,
+                  _PSCM_C2_RATE, ticks),
+    )
+
+  def set_command(self, command: FordPath) -> None:
+    self.command = _wire_path(command)
+
+
+class FordPscmObserverPathController:
+  """Compensate model-path commands for the PSCM coefficient state it still carries."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
+    self.observer = FordPscmObserver()
+    self._sent_c2 = 0.0
+
+  def _reset(self) -> None:
+    self._last_path = FordPath(valid=True)
+    self._curvature_history.clear()
+    self.observer.reset()
+    self._sent_c2 = 0.0
+
+  def _command_for_state(self, target: FordPath, v_ego: float) -> FordPath:
+    # The target describes the desired fully-settled PSCM contribution. C0 keeps
+    # the remaining C1-saturated residual. C1 supplies the primary contribution
+    # that the known slow C2 state does not yet provide, without a guessed gain.
+    target_state = FordPscmState(target.path_offset, target.path_angle, target.curvature)
+    target_contribution = sum(_pscm_contributions(target_state, v_ego))
+    _, _, observed_c2 = _pscm_contributions(self.observer.state, v_ego)
+    gain = _pscm_speed_gain(v_ego)
+    required_fast = target_contribution - observed_c2
+    c1_contribution = float(np.clip(required_fast, -0.349609375 * gain, 0.349609375 * gain))
+    c0_contribution = required_fast - c1_contribution
+    path_offset = c0_contribution / (0.5 * gain) if gain > 0.0 else 0.0
+    path_angle = c1_contribution / (10.0 * gain) if gain > 0.0 else 0.0
+    return FordPath(
+      valid=True,
+      path_offset=float(np.clip(path_offset, -_PSCM_C0_EFFECTIVE_LIMIT, _PSCM_C0_EFFECTIVE_LIMIT)),
+      path_angle=float(np.clip(path_angle, -_PSCM_C1_EFFECTIVE_LIMIT, _PSCM_C1_EFFECTIVE_LIMIT)),
+      curvature=target.curvature,
+      curvature_rate=target.curvature_rate,
+    )
+
+  def _limit(self, target: FordPath, v_ego_raw: float) -> FordPath:
+    path_offset = float(np.clip(target.path_offset,
+                                self._last_path.path_offset - _PATH_OFFSET_RATE * self.dt,
+                                self._last_path.path_offset + _PATH_OFFSET_RATE * self.dt))
+    path_angle = float(np.clip(target.path_angle,
+                               self._last_path.path_angle - _PATH_ANGLE_RATE * self.dt,
+                               self._last_path.path_angle + _PATH_ANGLE_RATE * self.dt))
+    curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(
+      target.curvature, self._sent_c2, v_ego_raw, 0.0, True, CarControllerParams.LMC2_STEP,
+    )
+    self._sent_c2 = curvature
+    self._last_path = FordPath(True, path_offset, path_angle, curvature, target.curvature_rate)
+    self.observer.set_command(self._last_path)
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
+             v_ego: float = 0.0, v_ego_raw: float = 0.0, active: bool = True) -> FordPath:
+    if not active:
+      self._reset()
+      return FordPath()
+
+    self.observer.advance(self.dt)
+    current_curvature = _finite(current_curvature)
+    self._curvature_history.append(current_curvature)
+    curvature_delta = (current_curvature - self._curvature_history[0]
+                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
+    path = _model_path(model) if model is not None else None
+    if path is None:
+      target = FordPath(valid=True)
+    else:
+      target = _encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
+                            max(_finite(v_ego), 0.0))
+    v_ego_raw = max(_finite(v_ego_raw), 0.0)
+    command = self._command_for_state(target, v_ego_raw)
+    return self._limit(command, v_ego_raw)

openpilot/selfdrive/controls/tests/fixtures/ford_c2_free_path_routes.json

@@ -0,0 +1,229 @@
+{
+  "description": "Curvature-driven C0 and full-heading C1 command regression; does not predict counterfactual wheel response. Contains geometry and control signals only, no GPS.",
+  "fixture_sha256": "12782ac1b0d0637945f729a46ad03af16cd58188872b6a65f104e32c4db70e9b",
+  "episodes": [
+    {
+      "name": "left_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        809.5,
+        815.0
+      ],
+      "evidence_seconds": [
+        812.1,
+        814.0
+      ],
+      "samples": 532
+    },
+    {
+      "name": "right_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        866.5,
+        872.0
+      ],
+      "evidence_seconds": [
+        869.5,
+        871.0
+      ],
+      "samples": 547
+    },
+    {
+      "name": "left_very_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        880.0,
+        884.0
+      ],
+      "evidence_seconds": [
+        882.9,
+        883.32
+      ],
+      "samples": 397
+    },
+    {
+      "name": "right_plateau",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        964.0,
+        970.0
+      ],
+      "evidence_seconds": [
+        967.2,
+        968.93
+      ],
+      "samples": 583
+    },
+    {
+      "name": "oscillation",
+      "route": "84865544361f55cb_00000078--349f5b8695",
+      "range_seconds": [
+        29.0,
+        37.0
+      ],
+      "evidence_seconds": [
+        32.5,
+        36.1
+      ],
+      "samples": 795
+    },
+    {
+      "name": "weak_first",
+      "route": "84865544361f55cb_0000007a--5a95fc717e",
+      "range_seconds": [
+        90.5,
+        95.2
+      ],
+      "evidence_seconds": [
+        93.5,
+        95.08
+      ],
+      "samples": 467
+    },
+    {
+      "name": "weak_second",
+      "route": "84865544361f55cb_0000007a--5a95fc717e",
+      "range_seconds": [
+        119.5,
+        125.3
+      ],
+      "evidence_seconds": [
+        122.5,
+        125.2
+      ],
+      "samples": 576
+    }
+  ],
+  "sources": {
+    "84865544361f55cb_00000077--4b55791ce6": [
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--0--rlog.zst",
+        "bytes": 10342855,
+        "sha256": "2f072eff3076f4d32dbc1a077f24fc85818abeafd13a4b970b1ba558a0d2ca52"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--1--rlog.zst",
+        "bytes": 11688902,
+        "sha256": "bbbbf7fc79b1c7133b83678a5202ac41cd8b3dcdd0bb358843d43fdb38f25e1e"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--2--rlog.zst",
+        "bytes": 11875525,
+        "sha256": "18d7e0224a61d068aeeef0e176da071f5e67d77fcb7ebaa7b5854756d4438add"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--3--rlog.zst",
+        "bytes": 12489313,
+        "sha256": "7869f95c87849018df07c680e0584145a31c407608f96f2fc77f35b729f02d2c"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--4--rlog.zst",
+        "bytes": 12301118,
+        "sha256": "2c2125fb2320b9bc6620fc586cedb6558b33e8475d0cce3fb361db4eea6a1c28"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--5--rlog.zst",
+        "bytes": 12976655,
+        "sha256": "b18c448786daf46cfa05bf0352396ce5851a09603d34ee5b989e096da5ee5180"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--6--rlog.zst",
+        "bytes": 13223857,
+        "sha256": "8c08b8d47ebca38c70aa94a1cdda25443ed3be94727bb487406b518471c88dd1"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--7--rlog.zst",
+        "bytes": 13043701,
+        "sha256": "638948d7e5853046773f82df8a531c518c42883c77f060e61ff683ccb59d52af"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--8--rlog.zst",
+        "bytes": 12569024,
+        "sha256": "41e5c83d2205964889579cf24967712dd0340da5f30f213409f2b1e04e6eb78a"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--9--rlog.zst",
+        "bytes": 11926213,
+        "sha256": "6034a90e817424c02755c2c0d9bdbad088c4286edfb887d9f2acbd60d7818da7"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--10--rlog.zst",
+        "bytes": 13010961,
+        "sha256": "23958a1ac8277977952c73e889fbfd9245bc2cf82b3af58233c245dbf1e76545"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--11--rlog.zst",
+        "bytes": 13204208,
+        "sha256": "f78d65b7b5927a8570f52d765f9af45431f8ab72260987f718321ad1b03c70af"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--12--rlog.zst",
+        "bytes": 12562994,
+        "sha256": "485621d1cf71605fccc8c679cb146b1f0954078849db7c74b1b65b166d741025"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--13--rlog.zst",
+        "bytes": 12610836,
+        "sha256": "0b4b0c01caae39a7dc4ff2219ab168029ea12c43b1966b8c204972743627c24b"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--14--rlog.zst",
+        "bytes": 13114068,
+        "sha256": "f18769800bd08c7614280916b50ec0274eca9b4765ad26d6549b72912321dac0"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--15--rlog.zst",
+        "bytes": 13113162,
+        "sha256": "abf0adc702db6f5fdd78a145709504c7061255bc9b9a61329a575707d5bd2f74"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--16--rlog.zst",
+        "bytes": 13029294,
+        "sha256": "a76ed74b889bda60e2d929123186df83653591d1e058fbd1497551fbbf23941e"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--17--rlog.zst",
+        "bytes": 12429846,
+        "sha256": "51d3aff2afa1a7ce3c5372a499b14394f22ac730e952273a69496c65ef726ed2"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--18--rlog.zst",
+        "bytes": 9241845,
+        "sha256": "2a1646227f9cdb1ab7eaf79fa653b7f4443a629a703577ff6c38a715dbced3a4"
+      }
+    ],
+    "84865544361f55cb_00000078--349f5b8695": [
+      {
+        "name": "84865544361f55cb_00000078--349f5b8695--0--rlog.zst",
+        "bytes": 10919702,
+        "sha256": "2d35f6c9ac9b09f8b86b3fe2fbe864e4af0d50c411e3dd56572b5aa113bf1973"
+      },
+      {
+        "name": "84865544361f55cb_00000078--349f5b8695--1--rlog.zst",
+        "bytes": 10600112,
+        "sha256": "6cabb48ea0caeb2fddd58be35b5c7e7c42faf87fc01991c4603573bffe33eaeb"
+      }
+    ],
+    "84865544361f55cb_0000007a--5a95fc717e": [
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--0--rlog.zst",
+        "bytes": 10841486,
+        "sha256": "bfc9e3308e5241e76cba57f2441043710fe8b18ade314221ee5e423f640c211a"
+      },
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--1--rlog.zst",
+        "bytes": 12567837,
+        "sha256": "ab9eb05c5805e286a6ec639bbbbc1cf086bfcf1b440801ad000713db95dfa7fc"
+      },
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--2--rlog.zst",
+        "bytes": 11015857,
+        "sha256": "af4f87f39be37f0a5b23b58de50c2ed8801bdbda6544cd6cd292525fc3bd4fb3"
+      }
+    ]
+  },
+  "pairing": "controlsState cycle time; causal carState speed/yaw/pressed; exact consumed model timestamp and geometry; nearest same-cycle carControl and carControlSP within 5ms.",
+  "yaw_rate": "Negative carState.yawRate, matching the model/control curvature coordinate sign; no wheel-to-curvature conversion.",
+  "desired_curvature": "Exact controlsState.desiredCurvature from the matching controlsState cycle. This is the post-selection, post-limiting request consumed by controlsd; it is not a wheel-angle-to-curvature fit.",
+  "reference_time": "Exact consumed modelV2 publication time, in the same relative seconds as each episode. The extraction cache does not retain consumed lateralManeuverPlan timestamps or validity; model time is an explicit replay assumption and cannot verify alternate-reference freshness."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_curvature_heading_route80.json

@@ -0,0 +1,67 @@
+{
+  "description": "Real route80 turn-command regressions. Signal-only fixture; no GPS. Counterfactual commands do not predict physical vehicle response.",
+  "route": "84865544361f55cb_00000080--1643deea7e",
+  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
+  "frozen_v3_controller_sha256": "576f4ec6f2dbc93f7e6c93a69839f69447eb5a0c2f834bd48b24f84a163dc2eb",
+  "fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
+  "episodes": [
+    {
+      "name": "under_333_339",
+      "range_seconds": [
+        331.5,
+        339.0
+      ],
+      "evidence_seconds": [
+        333.0,
+        339.0
+      ],
+      "samples": 745
+    },
+    {
+      "name": "over_417_420",
+      "range_seconds": [
+        415.5,
+        420.0
+      ],
+      "evidence_seconds": [
+        417.0,
+        420.0
+      ],
+      "samples": 447
+    },
+    {
+      "name": "under_430_435",
+      "range_seconds": [
+        428.5,
+        435.0
+      ],
+      "evidence_seconds": [
+        430.0,
+        435.0
+      ],
+      "samples": 646
+    }
+  ],
+  "sources": [
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--5--rlog.zst",
+      "bytes": 12531711,
+      "sha256": "059482830794cb0eabe6069b75a9610b900bf2a93d7a6624f53c575cef997157"
+    },
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--6--rlog.zst",
+      "bytes": 12560505,
+      "sha256": "147276789f5b14913adc4cd16db18f3d4bd27ce8497c9ff96fdf0315c219339f"
+    },
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--7--rlog.zst",
+      "bytes": 12660797,
+      "sha256": "b311b6ace75819db52b9618154d68c7d12e2751d5046b6d174adb89ef87a223c"
+    }
+  ],
+  "pairing": "Exact controlsState desiredCurvature and consumed model publication timestamp; causal carState speed, negative CAN yaw, and steeringPressed; nearest same-cycle carControl/carControlSP within 5 ms.",
+  "reference_time": "Consumed modelV2 publication time. Controller audit confirms route80 used modelV2 as reference throughout.",
+  "preroll": "Each episode starts from reset 1.5 s before evidence; v3_replay stores those exact cold-start commands and gates, while recorded stores original live path fields.",
+  "benchmark_clean": "Existing route80 benchmark mask: whole interval request minus 0.5 s through response (0.2 s) plus 0.25 s active, unpressed, valid, fresh, and speed >= 2 m/s.",
+  "expected_common_c1": "Independent shadow: clip(desiredCurvature * max(7 m, vEgo * 1 s), +/-0.5 rad), independently slewed at 0.5 rad/s and packed to Float32/sign-reversed CAN semantics. No subtraction of measured curvature."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_heading_feedback_route80_status.json

@@ -0,0 +1,13 @@
+{
+  "description": "PSCM status and raw driver-torque overlay for the existing three route80 request windows. No GPS. No counterfactual vehicle response.",
+  "fixture_sha256": "a9defdc5abdf26724358d606beb16becbdf30faa972974d49b179a9e004d7629",
+  "base_fixture": "ford_curvature_heading_route80.npz",
+  "base_fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
+  "source_route": "84865544361f55cb_00000080--1643deea7e",
+  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
+  "samples": 1838,
+  "source_cache_sha256": "1cd3e0c00805869ace1c5954dc682644f36f5eddb71785b69e4cb9da40f7f04f",
+  "pairing": "Latest actual bus-0 EPS 972 frame at or before each controlsState cycle; raw steering torque from the exact causal carState used by the base fixture.",
+  "timestamp_policy": "Actual CAN event logMonoTime in route-relative seconds, not the benchmark response-shifted status. The old route predates the new carStateSP status telemetry; source CAN timestamps are an explicit replay approximation.",
+  "validity": "Replay validity uses the paired carState valid and canValid values; enum validity, availability and age are checked by the production feedback controller."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json

@@ -0,0 +1,40 @@
+{
+  "description": "Signal-only v6 turn-exit recovery regression; no location, device identity, or predicted new vehicle response.",
+  "recorded_controller_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "baseline_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "response_delay": 0.20000000298023224,
+  "samples": 9134,
+  "models": 1843,
+  "fixture_sha256": "41d5e3efcee03a9e02fcaf7bf456c050c6a671a5b7f7fc9706ddf4d27bad71b8",
+  "windows": [
+    {
+      "name": "overturn_then_underturn",
+      "range_s": [
+        19.99615067150053,
+        29.48070058550053
+      ],
+      "samples": 521
+    },
+    {
+      "name": "well_tracked_curve_a",
+      "range_s": [
+        56.19474309950053,
+        63.68808603150053
+      ],
+      "samples": 729
+    },
+    {
+      "name": "well_tracked_curve_b",
+      "range_s": [
+        101.19780691350051,
+        116.33885445450052
+      ],
+      "samples": 810
+    }
+  ],
+  "selection": "One previously identified overturn-then-underturn event and two previously reported well-tracked curves; selected before recovery implementation.",
+  "mask": "Whole t-0.5 through t+0.65 interval active, valid, fresh, unpressed, raw driver torque magnitude <=1 Nm; requested |curvature|*speed\u00b2 >=.5 m/s\u00b2.",
+  "timing": "Exact consumed model publication; causal CAN/PSCM at estimated control computation time. Subtract observed median computation-to-publication delay; unsampled tick timing remains approximate.",
+  "context": "At least 20 seconds prior context or the available start, extended before the latest observed reset. Overlapping episodes are merged.",
+  "coordinates": "Times are local elapsed seconds; models contain only relative position.x/y and orientation.z arrays."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.json

@@ -0,0 +1,247 @@
+{
+  "description": "Signal-only historical fallback evidence and frozen-v5 comparison; no GPS or inferred counterfactual vehicle response.",
+  "route": "route83",
+  "recorded_commit": "79a4caa1f6b71488949108aee9ae6ae6566347b1",
+  "fixture_sha256": "d00312c430ace47000c05b8284ee8d56df56ec24bb17a9ea8f4dce83133527c3",
+  "samples": 11744,
+  "model_count": 2367,
+  "source_cache_sha256": "53d786aff2e0b6338e1991320145305fda3101f7b76e50bd2929adbcaea95b28",
+  "response_delay": 0.20000000298023224,
+  "episodes": [
+    [
+      1861.2933736250002,
+      1874.756970279
+    ],
+    [
+      1878.07372132,
+      1892.07372132
+    ],
+    [
+      1950.874232409,
+      1964.874232409
+    ],
+    [
+      2440.9020600930003,
+      2456.964158177
+    ],
+    [
+      2580.722658577,
+      2611.364366768
+    ],
+    [
+      2734.478264791,
+      2764.574374172
+    ]
+  ],
+  "windows": [
+    {
+      "name": "successful_large_early",
+      "role": "authority_target",
+      "range_s": [
+        1866.722720383,
+        1874.756970279
+      ],
+      "samples": 426,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0233371460413845,
+      "published_median_abs_c0_c1": [
+        1.6002928018569946,
+        0.2796146124601364
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        1.6002928018569946,
+        0.2796146124601364
+      ],
+      "phase_samples": {
+        "phase_turn_in": 15,
+        "phase_held": 122,
+        "phase_release": 402,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "centering_reversal_positive_to_negative",
+      "role": "reversal",
+      "range_s": [
+        1888.07372132,
+        1892.07372132
+      ],
+      "samples": 396,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 283,
+        "phase_held": 48,
+        "phase_release": 104,
+        "phase_reversal": 21
+      }
+    },
+    {
+      "name": "centering_reversal_negative_to_positive",
+      "role": "reversal",
+      "range_s": [
+        1960.874232409,
+        1964.874232409
+      ],
+      "samples": 397,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 154,
+        "phase_held": 0,
+        "phase_release": 183,
+        "phase_reversal": 21
+      }
+    },
+    {
+      "name": "clean_release",
+      "role": "release",
+      "range_s": [
+        2453.714158177,
+        2456.964158177
+      ],
+      "samples": 323,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 4,
+        "phase_held": 0,
+        "phase_release": 305,
+        "phase_reversal": 17
+      }
+    },
+    {
+      "name": "successful_smaller_positive",
+      "role": "sign_coverage_only",
+      "range_s": [
+        2590.722658577,
+        2600.918740146
+      ],
+      "samples": 175,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0960646334373787,
+      "published_median_abs_c0_c1": [
+        0.42173025012016296,
+        0.1222948431968689
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.42173025012016296,
+        0.1222948431968689
+      ],
+      "phase_samples": {
+        "phase_turn_in": 170,
+        "phase_held": 61,
+        "phase_release": 0,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "large_under_response",
+      "role": "under_response_challenge",
+      "range_s": [
+        2604.2254721,
+        2611.364366768
+      ],
+      "samples": 128,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 0.7322859508492778,
+      "published_median_abs_c0_c1": [
+        2.4204851388931274,
+        0.42145511507987976
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        2.4204851388931274,
+        0.42145511507987976
+      ],
+      "phase_samples": {
+        "phase_turn_in": 68,
+        "phase_held": 96,
+        "phase_release": 56,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "successful_large_181deg",
+      "role": "authority_target",
+      "range_s": [
+        2744.478264791,
+        2750.573209708
+      ],
+      "samples": 207,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0087938914780248,
+      "published_median_abs_c0_c1": [
+        2.1044259071350098,
+        0.3815947473049164
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        2.1044259071350098,
+        0.3815947473049164
+      ],
+      "phase_samples": {
+        "phase_turn_in": 137,
+        "phase_held": 94,
+        "phase_release": 64,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "large_over_response_290deg",
+      "role": "over_response_challenge_not_target",
+      "range_s": [
+        2760.493612962,
+        2764.574374172
+      ],
+      "samples": 181,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.2515789463064766,
+      "published_median_abs_c0_c1": [
+        4.737145900726318,
+        0.5235000252723694
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        4.737145900726318,
+        0.5
+      ],
+      "phase_samples": {
+        "phase_turn_in": 139,
+        "phase_held": 90,
+        "phase_release": 41,
+        "phase_reversal": 0
+      }
+    }
+  ],
+  "selection": "Authority targets require automatic turn windows with >=1 second strict torque eligibility, eligible |wheel|>=150 degrees, and whole-window CAN response ratio median 0.90..1.10 at fixed 0.2 s. No positive-request large turn qualifies.",
+  "non_targets": "Positive smaller turn supplies sign coverage only. Under/over response and release/reversal windows are regression challenges, not authority targets.",
+  "context": "At least 10 s pre-roll or available route start, extended to include the preceding feedback reset/sign reversal. Overlapping intervals are merged. First episode begins at the partial route boundary with unobserved earlier history.",
+  "phase_policy": "Held means request curvature range over +/-0.25 s times speed squared <0.15 m/s2 at demand>=0.5. Turn-in/release compare current absolute curvature with the historical held request at measurement_time-delay, scaled by max(7,speed), using +/-0.0005 rad. These masks can overlap held; reversal means opposing delayed/current signs.",
+  "wire_policy": "Published coefficients preserve Float32 values. Send-clamped copy caps C0 to +/-5.11 and C1 to +/-0.5 before packing. Actual decoded wire is normalized to controller sign, nearest within 15 ms; wire_time/fresh/mode expose timing approximation.",
+  "model_schema": "models[model_index] contains position.x, position.y, orientation.z; Float32 conversion preserves the original model payload precision.",
+  "v5_reference": "Frozen full sequential replay from command_replay.npz, whose source hash and limitations are recorded in command_replay.json.",
+  "frozen_v5_revision": "09acf8ec2f327769f00ee53563ad2dd9225e37a7",
+  "preroll_validation": "Compact reset replay exactly matches full sequential frozen-v5 C0/C1, gates and bias on all 2233 evidence samples."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_turn_exit_requests.json

@@ -0,0 +1,156 @@
+{
+  "description": "Anonymous recorded-input turn-exit regression fixture; command construction only, not simulated vehicle response.",
+  "baseline_revision": "dfcfddb91ce2409511f5b2dbce25d06d5056b3d6",
+  "baseline_hypothesis": "model-pose-c0-c1-feedback-v7",
+  "baseline_source_hashes": {
+    "controller_sha256": "4951a6352d89fcd66277bbfe682bd22e935a31b5a4db33e617ad21189b6705fd",
+    "allocator_sha256": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a"
+  },
+  "fixture_sha256": "87a030c309061b7dc218715d05440c2077e465a8138079b46e8e8cee94201e54",
+  "source_fixture_sha256": "d476110b83dc628ffbd094220e464d6d3114b709bda2977813c3217964d41086",
+  "response_delay": 0.20000000298023224,
+  "publication_latency_estimate_s": 0.0015483515003040793,
+  "samples": 15273,
+  "model_count": 3078,
+  "evidence_samples": 4879,
+  "context_policy": "At least twenty seconds prior context, extended before the last observed reset. Overlapping intervals are merged.",
+  "provenance": "Selected from a recorded drive running the pinned baseline; request, model, driver and PSCM observations stay fixed during replay.",
+  "baseline_policy": "Stored commands, validity and bias exactly match the complete baseline replay on evidence samples. Context outside evidence initializes state and is not an exact-output target.",
+  "compact_full_baseline_evidence_parity": {
+    "commands": {
+      "exact": true,
+      "max_difference": 0.0
+    },
+    "valid": {
+      "exact": true,
+      "max_difference": 0.0
+    },
+    "heading_bias": {
+      "exact": true,
+      "max_difference": 0.0
+    }
+  },
+  "measurement_policy": "Controller computation time is estimated from publication time using the recorded median latency; exact vehicle motion under changed commands is unknown.",
+  "clean_policy": "Every sample from request time minus 0.5 s through plus 0.65 s is active, valid, fresh, unpressed and within 1 Nm raw driver torque. Demand is absolute desired curvature times current speed squared; substantial means at least 0.5 m/s2.",
+  "driver_policy": "All replay inputs retain driver interference; only comparison metrics use the clean mask. History-reset failures intentionally retain nearby driver context.",
+  "coordinates": "Elapsed seconds shifted to the first fixture control cycle; model x/y/heading are vehicle-relative, not global position.",
+  "retained_fields": [
+    "t",
+    "episode",
+    "model_index",
+    "models",
+    "desired_curvature",
+    "yaw_rate",
+    "speed",
+    "measurement_time",
+    "model_time",
+    "reference_time",
+    "active",
+    "valid",
+    "pressed",
+    "steering_torque",
+    "pscm_timestamp",
+    "pscm_valid",
+    "pscm_lateral_state",
+    "pscm_limit",
+    "pscm_capability",
+    "pscm_denied",
+    "clean_rawtorque",
+    "demand",
+    "window_masks",
+    "evidence",
+    "baseline_commands",
+    "baseline_valid",
+    "baseline_heading_base",
+    "baseline_heading_target",
+    "baseline_heading_bias",
+    "baseline_feedback_yaw_error",
+    "baseline_feedback_reference_curvature",
+    "baseline_status",
+    "baseline_offset_target"
+  ],
+  "omitted_data": "No route/device identifiers, VIN, GPS, private paths, raw wheel angle, wheel rate, EPS torque, or absolute clock origins.",
+  "baseline_status_meaning": "feedback_status from the pinned baseline",
+  "windows": [
+    {
+      "name": "good_curve_a",
+      "role": "comparison",
+      "range_s": [
+        20.0002130975003,
+        25.0002130975003
+      ],
+      "samples": 496,
+      "clean_substantial_samples": 259
+    },
+    {
+      "name": "first_reversal",
+      "role": "reversal",
+      "range_s": [
+        83.0002130975003,
+        92.7002130975003
+      ],
+      "samples": 964,
+      "clean_substantial_samples": 167
+    },
+    {
+      "name": "good_curve_b",
+      "role": "comparison",
+      "range_s": [
+        121.0002130975003,
+        128.0002130975003
+      ],
+      "samples": 695,
+      "clean_substantial_samples": 308
+    },
+    {
+      "name": "second_reversal",
+      "role": "reversal",
+      "range_s": [
+        133.5002130975003,
+        138.9002130975003
+      ],
+      "samples": 537,
+      "clean_substantial_samples": 191
+    },
+    {
+      "name": "large_turn_driver_context_a",
+      "role": "driver_context",
+      "range_s": [
+        150.0002130975003,
+        157.0002130975003
+      ],
+      "samples": 695,
+      "clean_substantial_samples": 0
+    },
+    {
+      "name": "over_growth",
+      "role": "over_response",
+      "range_s": [
+        182.0002130975003,
+        191.0002130975003
+      ],
+      "samples": 897,
+      "clean_substantial_samples": 66
+    },
+    {
+      "name": "large_turn_driver_context_b",
+      "role": "driver_context",
+      "range_s": [
+        199.0002130975003,
+        205.0002130975003
+      ],
+      "samples": 595,
+      "clean_substantial_samples": 281
+    },
+    {
+      "name": "zero_bias_release",
+      "role": "under_response",
+      "range_s": [
+        202.0002130975003,
+        205.0002130975003
+      ],
+      "samples": 297,
+      "clean_substantial_samples": 279
+    }
+  ]
+}

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -0,0 +1,59 @@
+import ast
+import io
+import json
+import logging
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+from openpilot.common.logging_extra import SwagFormatter, SwagLogger
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPathController, FordPscmObserverPathController
+from openpilot.selfdrive.controls.tests.test_ford_model_action import circle
+
+
+class TestFordControlsLogging(unittest.TestCase):
+  def emit_controls_event(self, event, controls):
+    # Execute the actual controlsd call with the real logger and formatter,
+    # without launching hardware-dependent Controls or opening logging IPC.
+    source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
+    source = ast.parse(source_path.read_text())
+    calls = [node for node in ast.walk(source) if isinstance(node, ast.Call)
+             and isinstance(node.func, ast.Attribute) and isinstance(node.func.value, ast.Name)
+             and node.func.value.id == 'cloudlog' and node.args
+             and isinstance(node.args[0], ast.Constant) and node.args[0].value == event]
+    self.assertEqual(len(calls), 1)
+    logger = SwagLogger()
+    logger.setLevel(logging.INFO)  # disabled INFO logging would hide this crash
+    stream = io.StringIO()
+    handler = logging.StreamHandler(stream)
+    handler.setFormatter(SwagFormatter(logger))
+    logger.addHandler(handler)
+    try:
+      expression = ast.Expression(body=calls[0])
+      eval(compile(expression, str(source_path), 'eval'), {'cloudlog': logger, 'self': controls, 'reference_service': 'modelV2'})
+      record = json.loads(stream.getvalue())
+    finally:
+      handler.close()
+    self.assertEqual(record['level'], 'INFO')
+    self.assertEqual(record['msg']['event'], event)
+    return record['msg']
+
+  def test_startup_logs_selected_controller_without_crashing(self):
+    for controller in (FordPathController(), FordPscmObserverPathController(), FordModelActionController()):
+      with self.subTest(controller=type(controller).__name__):
+        record = self.emit_controls_event('Ford path controller selected', SimpleNamespace(ford_path_controller=controller))
+        self.assertEqual(record['controller'], type(controller).__name__)
+
+  def test_candidate_diagnostics_identify_the_experiment_and_do_not_claim_calibration(self):
+    controller = FordModelActionController()
+    for active, valid in ((False, True), (True, True), (True, False)):
+      controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1.,
+                        measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid)
+      controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
+                                 sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
+      record = self.emit_controls_event('Ford C2-free path tracking', controls)
+      self.assertEqual(record['hypothesis'], 'model-action-c0-c1-prediction-v4')
+      self.assertIs(record['calibration_approved'], False)
+      self.assertEqual(record['command'][2:], [0., 0.])
+      self.assertEqual(record['status'], controller.diagnostics['status'])

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -0,0 +1,199 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from opendbc.can import CANPacker, CANParser
+from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
+from openpilot.cereal import custom
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
+
+
+def make_model(x, y, heading):
+  return SimpleNamespace(position=SimpleNamespace(x=x, y=y), orientation=SimpleNamespace(z=heading))
+
+
+def circle(curvature):
+  s = np.linspace(0., 60., 601)
+  return make_model(np.sin(curvature*s)/curvature, (1-np.cos(curvature*s))/curvature, curvature*s)
+
+
+def straight(offset=0.):
+  x = np.linspace(0., 60., 121)
+  return make_model(x, np.full_like(x, offset), np.zeros_like(x))
+
+
+def test_selected_action_controls_heading_even_when_model_previews_another_turn():
+  model = circle(.02)
+  assert encode_model_action(model, 0., 20.).path_angle == 0.
+  assert encode_model_action(model, -.004, 20.).path_angle == pytest.approx(-.08)
+  assert encode_model_action(model, 0., 20.).path_offset > 0.
+
+
+def test_straight_centering_and_matched_curves_keep_base_gain_across_speed():
+  for speed in (2., 7., 20., 35.):
+    target = encode_model_action(straight(.4), 0., speed)
+    assert target == FordPath(True, .4, 0., 0., 0.)
+  for sign in (-1, 1):
+    target = encode_model_action(circle(sign*.01), sign*.01, 20.)
+    assert target.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=1e-6)
+    assert target.path_angle == pytest.approx(sign*.2)  # No 10 m cap at highway speed.
+
+
+def test_two_actuator_positions_are_sufficient_for_every_next_output():
+  controller = ModelActionController()
+  assert not hasattr(controller, '__dict__')
+  for i in range(300):
+    copied = ModelActionController()
+    copied.c0, copied.c1 = controller.c0, controller.c1
+    model = straight(.2*math.sin(i*.1))
+    kwargs = {'speed': 20., 'dt': .01}
+    desired = .005*math.cos(i*.03)
+    assert controller.update(model, desired, **kwargs) == copied.update(model, desired, **kwargs)
+
+
+def test_held_turn_releases_with_geometric_countersteering_and_no_retained_bias():
+  for sign in (-1., 1.):
+    controller = ModelActionController()
+    for _ in range(400):
+      out = controller.update(circle(sign*.01), sign*.01, speed=20., dt=.01)
+    assert out.path_angle == pytest.approx(sign*.2)
+    previous = np.array([controller.c0, controller.c1])
+    opposed = False
+    for desired in sign*np.linspace(.01, 0., 101):
+      out = controller.update(straight(), desired, speed=20., dt=.01)
+      rotation = desired*3.
+      predicted = (1.-math.cos(rotation))/desired-10.*math.sin(rotation) if desired else 0.
+      expected = previous+np.clip([predicted, 20.*desired]-previous, [-.04, -.005], [.04, .005])
+      values = np.array([controller.c0, controller.c1])
+      np.testing.assert_allclose(values, expected, atol=1e-10)
+      opposed |= sign*out.path_offset < 0.
+      previous = values
+    assert opposed
+    assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_current_model_replacement_leaves_only_independent_actuator_slew():
+  controller = ModelActionController()
+  for _ in range(150):
+    controller.update(straight(1.), .04, speed=20., dt=.01)
+  for _ in range(25):
+    out = controller.update(straight(), 0., speed=20., dt=.01)
+  assert out.path_offset == pytest.approx(0.)
+  assert out.path_angle > 0.  # C1 cannot hold C0 during its longer release.
+  for _ in range(75):
+    out = controller.update(straight(), 0., speed=20., dt=.01)
+  assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+@pytest.mark.parametrize('overrides', [{'active': False}, {'valid': False}, {'dt': .2}, {'speed': math.nan}])
+def test_invalid_or_inactive_input_clears_state_before_reengagement(overrides):
+  controller = ModelActionController()
+  for _ in range(100):
+    controller.update(straight(.5), .01, speed=20., dt=.01)
+  kwargs = {'speed': 20., 'dt': .01, 'active': True, 'valid': True}
+  kwargs.update(overrides)
+  assert controller.update(straight(), 0., **kwargs) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+  assert controller.update(straight(), 0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_malformed_geometry_and_nonfinite_action_never_create_an_active_command():
+  for model, desired in ((None, 0.), (straight(), math.nan), (straight(), math.inf)):
+    assert not encode_model_action(model, desired, 20.).valid
+
+
+def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2():
+  controller = ModelActionController()
+  packer = CANPacker('ford_lincoln_base_pt')
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+  bus = CanBus(fingerprint={0: {}})
+  previous = np.zeros(2)
+  for i in range(600):
+    sign = 1. if i < 300 else -1.
+    out = controller.update(straight(sign*8.), sign*.1, speed=30., dt=.01)
+    fields = np.array([out.path_offset, out.path_angle])
+    assert (abs(fields) <= [5.1100001, .5000001]).all()
+    assert (abs(fields-previous) <= [.0500001, .0055001]).all()
+    previous = fields
+    message = custom.CarControlSP.new_message()
+    message.fordLateralPath.pathOffset = out.path_offset
+    message.fordLateralPath.pathAngle = out.path_angle
+    packet = create_lat_ctl2_msg(packer, bus, 2, -message.fordLateralPath.pathOffset,
+                                -message.fordLateralPath.pathAngle, out.curvature, out.curvature_rate, i % 16)
+    parser.update([i*10_000_000, [packet]])
+    decoded = parser.vl['LateralMotionControl2']
+    assert decoded['LatCtlPathOffst_L_Actl'] == pytest.approx(-out.path_offset)
+    assert decoded['LatCtlPath_An_Actl'] == pytest.approx(-out.path_angle)
+    assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
+
+
+def test_short_path_holds_available_endpoint_without_extrapolation():
+  model = make_model([0., 1.], [0., .1], [0., 0.])
+  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, .2, 0., 0.)
+
+
+def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
+  model = make_model([0., 1e308, -1e308], [0., 0., 0.], [0., 0., 0.])
+  controller = ModelActionController()
+  controller.update(straight(.4), .01, speed=20., dt=.01)
+  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('value', [None, 'bad', 10**400])
+@pytest.mark.parametrize('field', ['dt', 'speed', 'desired_curvature'])
+def test_malformed_numeric_input_resets_without_throwing(field, value):
+  controller = ModelActionController()
+  kwargs = {'speed': 20., 'dt': .01, 'desired_curvature': .01}
+  controller.update(straight(.4), **kwargs)
+  kwargs[field] = value
+  assert controller.update(straight(.4), **kwargs) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('model', [
+  make_model([], [], []), make_model([0.], [0.], [0.]),
+  make_model([0., 10.], [0.], [0., 0.]), make_model([0., 10.], [0., 0.], [0.]),
+  make_model([0., 0.], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., math.nan], [0., 0.]), make_model([0., math.inf], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., 0.], [0., math.inf]),
+  make_model([0., 10**400], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., 0.], [1e308, -1e308]),
+])
+def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model):
+  controller = ModelActionController()
+  controller.update(straight(.4), .01, speed=20., dt=.01)
+  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('field,value,valid', [
+  ('speed', .2999, False), ('speed', .3, True), ('speed', 55., True), ('speed', 55.0001, False),
+  ('desired_curvature', -1., True), ('desired_curvature', 1., True), ('desired_curvature', -1.0001, False),
+  ('dt', .001999, False), ('dt', .002, True), ('dt', .1, True), ('dt', .100001, False), ('dt', 0., False),
+])
+def test_domain_and_elapsed_time_boundaries(field, value, valid):
+  kwargs = {'speed': 20., 'desired_curvature': .01, 'dt': .01}
+  kwargs[field] = value
+  assert ModelActionController().update(straight(.4), **kwargs).valid == valid
+
+
+def test_arc_station_not_forward_x_or_model_heading_determines_offset():
+  x = np.array([0., 6., 12.])
+  y = .4+x*.75
+  target = encode_model_action(make_model(x, y, [2., -2., 1.]), -.01, 20.)
+  # Arc length is 1.25*x. At station 10, x=8 and y=6.4; use the full predicted pose.
+  expected = math.cos(-.03)*6.4-math.sin(-.03)*8.+(1.-math.cos(-.03))/-.01
+  assert target.path_offset == pytest.approx(expected)
+  assert target.path_angle == pytest.approx(-.2)
+
+
+def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
+  model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
+  assert encode_model_action(model, 0., 20.) == FordPath(True, .4, 0., 0., 0.)
+  out = ModelActionController().update(model, .01, speed=20., dt=.002)
+  assert out.path_offset == pytest.approx(.01)
+  assert out.path_angle == pytest.approx(.001)

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -0,0 +1,229 @@
+"""Exercise the candidate through existing selection, publication and CAN code.
+
+Tests enable the candidate through controlsd's real startup selection.
+No hardware, IPC or CAN transmission is involved.
+"""
+import ast
+from collections import defaultdict
+import json
+import math
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.can import CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.values import FordFlags
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
+from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
+
+
+def update(controller, now=1., **overrides):
+  kwargs = {'model': straight(.4), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now,
+                'model_time': now, 'measurement_time': now, 'reference_time': now, 'active': True}
+  kwargs.update(overrides)
+  return controller.update(**kwargs)
+
+
+@pytest.mark.parametrize('field', ['model_time', 'measurement_time', 'reference_time'])
+@pytest.mark.parametrize('age', [.151, -.006])
+def test_stale_or_future_service_clears_commands_and_reengages_from_zero(field, age):
+  controller = FordModelActionController()
+  update(controller)
+  assert update(controller, 1.01, **{field: 1.01-age}) == FordPath()
+  assert controller.diagnostics['status'] == 'stale_input'
+  assert update(controller, 1.02).path_offset == pytest.approx(.04)
+
+
+@pytest.mark.parametrize('change,reason', [
+  ({'now': 1.}, 'timing_reset'),
+  ({'now': .99}, 'timing_reset'),
+  ({'now': 1.001}, 'timing_reset'),
+  ({'now': 1.101}, 'timing_reset'),
+  ({'model_time': .999}, 'timing_reset'),
+  ({'measurement_time': .999}, 'timing_reset'),
+  ({'active': False}, 'inactive'),
+  ({'valid': False}, 'invalid_service'),
+  ({'model': None}, 'invalid_path'),
+  ({'yaw_rate': math.nan}, 'nonfinite'),
+  ({'yaw_rate': 3.01}, 'input_range'),
+  ({'speed': 55.01}, 'input_range'),
+  ({'desired_curvature': 1.01}, 'input_range'),
+])
+def test_invalid_cycle_never_keeps_a_previous_active_request(change, reason):
+  controller = FordModelActionController()
+  update(controller)
+  now = change.get('now', 1.01)
+  assert update(controller, **dict(change, now=now)) == FordPath()
+  assert controller.diagnostics['status'] == reason
+  assert (controller.core.c0, controller.core.c1) == (0., 0.)
+  assert update(controller, now+1.).path_angle == pytest.approx(.005)
+
+
+@pytest.mark.parametrize('field', ['now', 'measurement_time', 'model_time', 'reference_time', 'speed', 'yaw_rate', 'desired_curvature'])
+@pytest.mark.parametrize('value', [math.nan, math.inf, -math.inf, None])
+def test_nonfinite_input_never_raises_or_leaks_into_diagnostics(field, value):
+  controller = FordModelActionController()
+  update(controller)
+  assert update(controller, **{field: value}) == FordPath()
+  assert controller.diagnostics['status'] == 'nonfinite'
+  json.dumps(controller.diagnostics, allow_nan=False)
+
+
+def test_repeated_measurements_do_not_freeze_slew_or_cache_invalid_model_geometry():
+  controller = FordModelActionController()
+  for i in range(10):
+    result = update(controller, 1.+i*.01, measurement_time=1., model_time=1., reference_time=1.)
+  assert result.path_offset == pytest.approx(.14)  # Full preview accounts for selected turning toward the offset path.
+  assert result.path_angle == pytest.approx(.05)
+  broken = straight(.4)
+  broken.position.y[5] = math.nan
+  assert update(controller, 1.1, model=broken, model_time=1., measurement_time=1.) == FordPath()
+  assert controller.diagnostics['status'] == 'invalid_path'
+
+
+def test_yaw_offset_does_not_change_the_base():
+  controllers = [FordModelActionController() for _ in range(3)]
+  variants = [{}, {'yaw_rate': .0072}, {'yaw_rate': -.0072}]
+  for i in range(100):
+    outputs = [update(c, 1.+i*.01, **kwargs) for c, kwargs in zip(controllers, variants, strict=True)]
+    assert all(out == outputs[0] for out in outputs)
+  assert outputs[0].path_angle == pytest.approx(.2)
+
+
+def test_reference_source_can_change_to_an_older_but_fresh_publication():
+  controller = FordModelActionController()
+  update(controller, reference_time=.99)
+  assert update(controller, 1.01, reference_time=.98).valid
+
+
+def test_release_keeps_current_geometry_and_may_grow_c0_while_c1_decreases():
+  for sign in (-1., 1.):
+    controller = FordModelActionController()
+    for i in range(100):
+      before = update(controller, 1.+i*.01, model=circle(sign*.01), desired_curvature=sign*.005)
+    for i in range(100):
+      after = update(controller, 2.+i*.01, model=circle(sign*.02), desired_curvature=sign*.004)
+    assert abs(after.path_offset) > abs(before.path_offset)
+    assert abs(after.path_angle) < abs(before.path_angle)
+    for i in range(100):
+      released = update(controller, 3.+i*.01, model=circle(sign*.02), desired_curvature=0.)
+    assert abs(released.path_offset) > abs(after.path_offset)  # Less expected turning raises the future-frame offset.
+    assert released.path_angle == pytest.approx(0.)
+
+
+def _method(filename, class_name, method):
+  tree = ast.parse(filename.read_text())
+  cls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == class_name)
+  return next(node for node in cls.body if isinstance(node, ast.FunctionDef) and node.name == method)
+
+
+@pytest.fixture
+def pipeline():
+  root = Path(__file__).resolve().parents[3]
+  controls_file = root/'selfdrive/controls/controlsd.py'
+  body = _method(controls_file, 'Controls', 'state_control').body
+  # Execute the actual source choice, upstream limiter and Ford integration.
+  selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']")
+  limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and
+                 isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature')
+  branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
+  call = compile(ast.Module(body=[selection, limiter, branch], type_ignores=[]), str(controls_file), 'exec')
+  publication_file = root/'sunnypilot/selfdrive/controls/controlsd_ext.py'
+  body = _method(publication_file, 'ControlsExt', 'state_control_ext').body
+  publish = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'ford_path') or
+             (isinstance(n, ast.If) and ast.unparse(n.test) == 'ford_path is not None')]
+  assert len(publish) == 2
+  publication = compile(ast.Module(body=publish, type_ignores=[]), str(publication_file), 'exec')
+  return call, publication
+
+
+class Subscriptions:
+  frame = 1
+
+  def __init__(self, maneuver):
+    self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True}
+    self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000}
+    self.failed = set()
+    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)}
+
+  def __getitem__(self, service):
+    return self.messages[service]
+
+  def all_checks(self, services):
+    return not self.failed.intersection(services) and all(self.valid.get(s, True) for s in services)
+
+
+@pytest.mark.parametrize('maneuver', [False, True])
+@pytest.mark.parametrize('host_yaw', [.0072, .3])
+@pytest.mark.parametrize('initial_curvature', [0., .005])
+def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature):
+  call, publication = pipeline
+  sm = Subscriptions(maneuver)
+  controls = startup()
+  controller = controls.ford_path_controller
+  initial_curvature *= -1 if maneuver else 1
+  controls.sm, controls.desired_curvature, controls.curvature = sm, initial_curvature, 0.
+  if initial_curvature:
+    # Start at the old target so startup slew cannot hide prediction on the real call path.
+    controller.core.c0, controller.core.c1 = .4, 20.*initial_curvature
+  model = straight(.4)
+  model.action = SimpleNamespace(desiredCurvature=.1)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=-host_yaw, canValid=True, steeringPressed=False, steeringTorque=0.)
+  environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+                     'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
+  exec(call, environment)
+  expected_curvature = initial_curvature+(-1 if maneuver else 1)*.000125
+  assert controls.desired_curvature == pytest.approx(expected_curvature)
+  assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
+  expected_offset = .04 if host_yaw < .02 else .01
+  if initial_curvature:
+    expected_offset = .44 if maneuver and host_yaw < .02 else .36
+  assert controls.ford_path.path_offset == pytest.approx(expected_offset)
+  assert controller.diagnostics['yaw_rate'] == host_yaw
+  assert cc.latActive and cc.actuators.curvature == 0.
+  assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
+
+  cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
+  downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
+  vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
+                            lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
+  for i, fail in enumerate((False, True)):
+    if fail:
+      sm.failed.add('modelV2')
+      exec(call, environment)
+      assert not cc.latActive and controls.ford_path == FordPath()
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, (i+1)*10_000_000)
+    parser.update([(i+1)*10_000_000, packets])
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert wire['LatCtl_D2_Rq'] == (0 if fail else 2)
+
+
+@pytest.mark.parametrize('maneuver', [False, True])
+@pytest.mark.parametrize('failed', ['carState', 'modelV2', 'vehicleParameters', 'lateralManeuverPlan'])
+def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
+  sm = Subscriptions(maneuver)
+  sm.failed.add(failed)
+  controls = startup()
+  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  model = straight()
+  model.action = SimpleNamespace(desiredCurvature=.1)
+  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+                         'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
+  assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)

openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py

@@ -0,0 +1,75 @@
+"""Bounded excess-yaw damping: remove offset demand without integral or modes."""
+import math
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, damp_offset
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_recorded_turn_exit_reduces_same_direction_offset_before_driver_intervention(sign):
+  # Route9b, segment10, about643.0s; fresh Ford yaw in host coordinates.
+  c0, desired, speed, yaw = sign*.5, sign*.0117238564, 9.71, sign*.180
+  reduced = damp_offset(c0, desired, speed, yaw)
+  assert reduced == pytest.approx(sign*(.5-1.4*(.180-9.71*.0117238564-.02)))
+  assert 0. < sign*reduced < .45
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_recorded_late_exit_removes_remaining_offset_without_creating_countersteer(sign):
+  # About643.9s. C1 is already slightly opposite; C0 still points into the turn.
+  assert damp_offset(sign*.12, sign*-.0004078, 10.77, sign*.1002) == pytest.approx(sign*.00772)
+  assert damp_offset(sign*.12, 0., 10.77, sign*.2) == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013])
+def test_matched_turn_and_straight_bias_cannot_reduce_offset(sign, bias):
+  for desired in (0., sign*.01, sign*.05):
+    assert damp_offset(sign*.4, desired, 10., 10.*desired+bias) == sign*.4
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013, .02])
+def test_opposed_plan_cannot_amplify_small_yaw_bias(sign, bias):
+  for desired in (-sign*.01, -sign*.1):
+    assert damp_offset(sign*.4, desired, 20., sign*bias) == sign*.4
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_damping_begins_continuously_above_the_yaw_deadband(sign):
+  assert damp_offset(sign*.4, -sign*.1, 20., sign*.020001) == pytest.approx(sign*(.4-1.4e-6))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_entry_deficit_opposing_centering_and_zero_offset_are_preserved(sign):
+  assert damp_offset(sign*2.75, sign*.053889, 5.283, sign*.272) == sign*2.75
+  assert damp_offset(sign*-.25, sign*.001, 10., sign*.2) == sign*-.25
+  assert damp_offset(0., sign*.001, 10., sign*.2) == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_core_keeps_heading_unchanged_and_slews_offset_independently(sign):
+  baseline, damped = ModelActionController(), ModelActionController()
+  for i in range(300):
+    desired = sign*(.02 if i < 100 else .001)
+    a = baseline.update(straight(sign*.4), desired, speed=10., dt=.01)
+    b = damped.update(straight(sign*.4), desired, speed=10., dt=.01, yaw_rate=sign*.2)
+    assert a.path_angle == b.path_angle
+    assert a.curvature == b.curvature == a.curvature_rate == b.curvature_rate == 0.
+  assert a.path_offset == pytest.approx(sign*.39)  # Geometric prediction slightly reduces the .4m offset.
+  assert b.path_offset == pytest.approx(sign*.15)
+  # No damping memory: a fresh copied pair of actuator states behaves identically.
+  copied = ModelActionController()
+  copied.c0, copied.c1 = damped.c0, damped.c1
+  assert copied.update(straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.) == damped.update(
+    straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.)
+
+
+@pytest.mark.parametrize('yaw', [math.nan, math.inf, -math.inf, None, 'bad', 3.001, -3.001])
+def test_invalid_yaw_resets_core(yaw):
+  c = ModelActionController()
+  c.update(straight(.4), .01, speed=10., dt=.01)
+  assert c.update(straight(.4), .01, speed=10., dt=.01, yaw_rate=yaw) == FordPath()
+  assert c.c0 == c.c1 == 0.

openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py

@@ -0,0 +1,89 @@
+"""Geometric prediction checks; these do not simulate the Ford steering plant."""
+import math
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import encode_model_action
+from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, make_model, straight
+
+
+def geometric_offset(model):
+  station = np.r_[0., np.cumsum(np.hypot(np.diff(model.position.x), np.diff(model.position.y)))]
+  return float(np.interp(7., station, model.position.y))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_developing_bend_uses_full_prediction_beyond_former_cap(sign):
+  x = np.linspace(0., 30., 3001)
+  model = make_model(x, sign*.001*x**3, np.zeros_like(x))
+  base = geometric_offset(model)
+  target = encode_model_action(model, 0., 10.)
+  assert sign*target.path_offset > sign*base+.05
+  station = np.r_[0., np.cumsum(np.hypot(np.diff(x), np.diff(model.position.y)))]
+  assert target.path_offset == pytest.approx(np.interp(8.5, station, model.position.y))
+  assert abs(target.path_offset-base) > .25*abs(base)
+  assert target.path_angle == target.curvature == target.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_flattening_bend_reduces_offset_before_the_near_path_disappears(sign):
+  station = np.linspace(0., 30., 3001)
+  heading = sign*.01*np.minimum(station, 5.)
+  ds = station[1]-station[0]
+  x = np.r_[0., np.cumsum(np.cos((heading[:-1]+heading[1:])/2)*ds)]
+  y = np.r_[0., np.cumsum(np.sin((heading[:-1]+heading[1:])/2)*ds)]
+  model = make_model(x, y, heading)
+  base = geometric_offset(model)
+  target = encode_model_action(model, sign*.01, 20.)
+  assert 0. < sign*target.path_offset < sign*base-.02
+  assert target.path_angle == pytest.approx(sign*.2)
+
+
+@pytest.mark.parametrize('speed', [.3, 5., 20., 55.])
+@pytest.mark.parametrize('curvature', [-.05, -.01, .01, .05])
+def test_matched_constant_circle_is_not_given_a_blanket_gain_increase(speed, curvature):
+  model = circle(curvature)
+  assert encode_model_action(model, curvature, speed).path_offset == pytest.approx(geometric_offset(model), abs=1e-4)
+
+
+@pytest.mark.parametrize('offset', [-.4, 0., .4])
+@pytest.mark.parametrize('curvature', [-1e-300, 0., 1e-300])
+def test_straight_centering_and_near_zero_curvature_remain_well_conditioned(offset, curvature):
+  for speed in (.3, 20., 55.):
+    assert encode_model_action(straight(offset), curvature, speed).path_offset == pytest.approx(offset, abs=1e-12)
+
+
+@pytest.mark.parametrize('offset', [-4., -.4, -.001, 0., .001, .4, 4.])
+def test_full_rotated_path_prediction_is_independent_of_base_offset_magnitude(offset):
+  station = np.linspace(0., 30., 301)
+  for heading in (-.3, .3):
+    model = make_model(station*np.cos(heading), offset+station*np.sin(heading), np.full_like(station, heading))
+    value = encode_model_action(model, 0., 20.).path_offset
+    assert value == pytest.approx(offset+10.*math.sin(heading))
+    assert abs(value-geometric_offset(model)) > .15
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_zero_near_offset_can_request_opposing_centering_from_the_predicted_pose(sign):
+  curvature = sign*.01
+  target = encode_model_action(straight(), curvature, 20.)
+  expected = (1.-math.cos(curvature*3.))/curvature-10.*math.sin(curvature*3.)
+  assert target.path_offset == pytest.approx(expected)
+  assert sign*target.path_offset < -.25
+  assert target.path_angle == pytest.approx(sign*.2)
+
+
+def test_short_horizon_holds_endpoint_and_available_prediction_tapers_to_zero():
+  for length in (1., 6.99, 7.):
+    assert encode_model_action(make_model([0., length], [.4, .4], [0., 0.]), .01, 20.).path_offset == .4
+  near = encode_model_action(make_model([0., 7.000001], [.4, .4], [0., 0.]), .01, 20.).path_offset
+  assert abs(near-.4) < 1e-6
+
+
+def test_unrepresentable_predicted_geometry_keeps_the_valid_current_offset():
+  large = 1.79e308
+  model = make_model([-large, np.nextafter(-large, 0.)], [large, large], [0., 0.])
+  target = encode_model_action(model, .3, 20.)
+  assert target.valid and math.isfinite(target.path_offset)
+  assert target.path_offset == large

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -0,0 +1,100 @@
+"""Exercise real startup selection and Sunnylink writes without starting hardware."""
+import ast
+import base64
+import itertools
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.car.ford.values import FordFlags
+from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
+
+
+def car_params(**overrides):
+  return SimpleNamespace(**({'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1',
+                             'carFw': []} | overrides))
+
+
+def startup(cp=None, params=None):
+  filename = Path(__file__).resolve().parents[1]/'controlsd.py'
+  tree = ast.parse(filename.read_text())
+  cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Controls')
+  body = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == '__init__').body
+  start = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_pscm_observer')
+  end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path')
+  if params is None:
+    params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController')
+  controls = SimpleNamespace(CP=cp or car_params(), params=params)
+  environment = {'self': controls, 'FordFlags': FordFlags, 'FordPath': FordPath,
+                 'FordPathController': FordPathController, 'FordPscmObserverPathController': FordPscmObserverPathController,
+                 'FordModelActionController': FordModelActionController,
+                 'select_model_action_controller': select_model_action_controller,
+                 'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
+  exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment)
+  return controls
+
+
+@pytest.mark.parametrize('candidate,observer', list(itertools.product((False, True), repeat=2)))
+def test_actual_startup_priority(candidate, observer):
+  settings = {'FordModelActionController': candidate, 'FordPscmObserver': observer}
+  selected = startup(params=SimpleNamespace(get_bool=settings.__getitem__))
+  previous = FordPscmObserverPathController if observer else FordPathController
+  expected = FordModelActionController if candidate else previous
+  assert type(selected.ford_path_controller) is expected
+  assert selected.ford_model_action == candidate
+  assert selected.ford_path == FordPath()
+
+
+@pytest.mark.parametrize('overrides', [{'brand': 'tesla'}, {'flags': 0}, {'carFingerprint': 'FORD_F_150_MK14'}])
+@pytest.mark.parametrize('observer', [False, True])
+def test_other_vehicles_keep_their_previous_selection(overrides, observer):
+  settings = {'FordModelActionController': False, 'FordPscmObserver': observer}
+  params = SimpleNamespace(get_bool=settings.__getitem__)
+  before = startup(car_params(**overrides), params)
+  settings['FordModelActionController'] = True
+  after = startup(car_params(**overrides), params)
+  assert type(after.ford_path_controller) is type(before.ford_path_controller)
+  assert not after.ford_model_action
+
+
+@pytest.mark.parametrize('firmware', [[], [SimpleNamespace(ecu='eps', fwVersion=b'other')]])
+def test_candidate_does_not_depend_on_eps_firmware_query(firmware):
+  assert isinstance(startup(car_params(carFw=firmware)).ford_path_controller, FordModelActionController)
+
+
+@pytest.mark.parametrize('observer', [False, True])
+def test_sunnylink_write_takes_effect_on_restart_and_restores_stored_selection(tmp_path, monkeypatch, observer):
+  from openpilot.sunnypilot.sunnylink import utils
+
+  params = Params(str(tmp_path))
+  monkeypatch.setattr(utils, 'Params', lambda: params)
+  assert params.get_default_value('FordModelActionController') is False
+  assert params.get_type('FordModelActionController') == ParamKeyType.BOOL
+  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.PERSISTENT)
+  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.BACKUP)
+  params.put_bool('FordPscmObserver', observer, block=True)
+  old = startup(params=params)
+  assert not isinstance(old.ford_path_controller, FordModelActionController)
+  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'true').decode())
+  enabled = startup(params=params)
+  assert isinstance(enabled.ford_path_controller, FordModelActionController)
+  assert not isinstance(old.ford_path_controller, FordModelActionController)
+  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'false').decode())
+  assert isinstance(enabled.ford_path_controller, FordModelActionController)
+  assert type(startup(params=params).ford_path_controller) is type(old.ford_path_controller)
+  assert params.get_bool('FordPscmObserver') == observer
+
+
+def test_stored_retired_toggle_cannot_enable_the_candidate(tmp_path):
+  params = Params(str(tmp_path))
+  Path(params.get_param_path('FordVirtualAngleController')).write_text('1')
+  assert b'FordVirtualAngleController' not in params.all_keys()
+  assert params.get_bool('FordModelActionController') is False
+  assert type(startup(params=params).ford_path_controller) is FordPathController
+  params.put_bool('FordModelActionController', True, block=True)
+  params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
+  assert not Path(params.get_param_path('FordVirtualAngleController')).exists()
+  assert params.get_bool('FordModelActionController') is True

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,422 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.ford_path import (DBC_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPath, FordPathController,
+                                                        FordPscmObserver, FordPscmObserverPathController, FordPscmState,
+                                                        _bounded_feedback, _encode_path, _model_path, _predicted_pose,
+                                                        _pscm_contributions, _relative_pose)
+
+
+def _path(curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  heading = curvature * distance
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
+  heading = np.zeros_like(distance)
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
+  return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
+
+
+def _equivalent_curvature(command) -> float:
+  return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
+
+
+def test_gentle_path_uses_only_c2():
+  command = _command(_path(0.004, speed=20.0), 0.004, current_curvature=0.004, v_ego=20.0)
+  assert command.valid
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.004, atol=1e-6)
+  assert command.curvature_rate == 0.0
+
+
+def test_gentle_path_uses_only_c2_when_model_and_action_disagree():
+  command = _command(_path(0.005), 0.002, current_curvature=0.005)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.002, atol=1e-6)
+
+
+def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
+  controller = FordPathController(dt=1.0)
+  command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature < 0.012
+  assert command.curvature_rate == 0.0
+
+
+def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
+  constant = _command(_path(0.012), 0.012)
+  growing = _command(_changing_path(0.0, 0.04), 0.012)
+  assert _equivalent_curvature(growing) > _equivalent_curvature(constant)
+  assert constant.curvature_rate == 0.0
+  assert growing.curvature_rate == 0.0
+
+
+def test_local_tracking_error_corrects_without_replacing_forward_pose():
+  model = _changing_path(0.0, 0.04)
+  local_curvature = 0.5 * 0.04 * 2.0 / 7.0
+  aligned = _command(model, 0.012, current_curvature=local_curvature)
+  under = _command(model, 0.012, current_curvature=0.0)
+  assert aligned.path_offset > 0.0
+  assert aligned.path_angle > 0.0
+  assert under.path_offset > aligned.path_offset
+  assert under.path_angle > aligned.path_angle
+
+
+def test_large_maneuver_uses_fast_pose_and_zeros_c2():
+  command = _command(_path(0.04), 0.04)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+  assert command.curvature_rate == 0.0
+
+
+def test_model_pose_can_trigger_maneuver_when_action_is_late():
+  command = _command(_path(0.04), 0.002)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+
+
+def test_gentle_model_pose_does_not_replace_a_collapsed_action():
+  command = _command(_path(0.005), 0.0, current_curvature=0.005)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert command.curvature == 0.0
+
+
+def test_changing_gentle_curve_keeps_upstream_strength_c2():
+  command = _command(_changing_path(0.0, 0.008), 0.004, current_curvature=0.0)
+  assert np.isclose(command.curvature, 0.004)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+
+
+def test_action_only_maneuver_cannot_invent_large_model_pose():
+  command = _command(_path(0.002), 0.04)
+  assert 0.0 < command.path_offset < 0.1
+  assert 0.0 < command.path_angle < 0.03
+  assert command.curvature == 0.0
+
+
+def test_nearby_demands_blend_continuously_without_a_mode_threshold():
+  low = _command(_path(0.0119), 0.0119)
+  high = _command(_path(0.0121), 0.0121)
+  assert abs(high.path_offset - low.path_offset) < 0.05
+  assert abs(high.path_angle - low.path_angle) < 0.03
+  assert abs(high.curvature - low.curvature) < 0.001
+
+
+def test_leaving_c2_normal_band_does_not_drop_total_authority():
+  normal = _command(_path(0.006), 0.006)
+  transition = _command(_path(0.0061), 0.0061)
+  assert transition.curvature <= normal.curvature
+  assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
+
+
+def test_low_speed_still_uses_available_model_pose():
+  command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_higher_speed_advances_predicted_pose_and_extends_heading_horizon():
+  model = _changing_path(0.0, 0.015, speed=20.0)
+  slow = _command(model, 0.012, v_ego=7.0)
+  fast = _command(model, 0.012, v_ego=20.0)
+  assert fast.path_offset > slow.path_offset
+  assert fast.path_angle > slow.path_angle
+
+
+def test_short_model_uses_available_endpoint():
+  model = _path(0.04, speed=1.0)
+  command = _command(model, 0.04, v_ego=1.0)
+  assert command.valid
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
+  outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset > 0.0
+  assert outputs[0].path_angle > 0.0
+  assert outputs[-1].curvature == 0.0
+
+
+def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
+  outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
+  assert outputs[-1].path_offset == 0.0
+  assert outputs[-1].path_angle == 0.0
+
+
+def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
+  controller = FordPathController(dt=0.01)
+  normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
+  entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
+  assert np.all(np.diff(entry_authority) >= -1e-9)
+  assert entry_authority[0] >= _equivalent_curvature(normal)
+
+  exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
+  assert np.all(np.diff(exit_authority) <= 1e-9)
+  assert np.all(exit_authority >= 0.004 - 1e-9)
+
+
+def test_measured_tracking_error_closes_bidirectionally_without_abandoning_the_turn():
+  model = _path(0.04)
+  under = _command(model, 0.04, current_curvature=0.005)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.05)
+  assert under.path_offset > on_target.path_offset
+  assert under.path_angle > on_target.path_angle
+  assert 0.0 < over.path_offset < on_target.path_offset
+  assert 0.0 < over.path_angle < on_target.path_angle
+
+
+def test_gentle_curve_does_not_add_fast_tracking_trim():
+  model = _path(0.004)
+  under = _command(model, 0.004, current_curvature=0.002)
+  on_target = _command(model, 0.004, current_curvature=0.004)
+  over = _command(model, 0.004, current_curvature=0.006)
+  assert under.path_offset == on_target.path_offset == over.path_offset == 0.0
+  assert under.path_angle == on_target.path_angle == over.path_angle == 0.0
+  assert np.allclose([under.curvature, on_target.curvature, over.curvature], 0.004, atol=2e-6)
+
+
+def test_overshoot_trim_cannot_erase_a_modeled_turn():
+  model = _path(0.04)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.06)
+  assert over.path_offset > 0.95 * on_target.path_offset
+  assert over.path_angle > 0.9 * on_target.path_angle
+
+
+def test_corrupt_measured_curvature_cannot_reverse_a_modeled_turn():
+  command = _command(_path(0.04), 0.04, current_curvature=0.5)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature == 0.0
+
+
+def test_feedback_preserves_half_lsb_feedforward_direction():
+  for feedforward, resolution in ((0.006, 0.01), (0.0004, 0.0005)):
+    result = feedforward + _bounded_feedback(feedforward, -1.0, resolution, 1.0)
+    assert result >= 0.5 * resolution
+
+
+def test_recent_curvature_trend_advances_vehicle_pose_without_a_response_gain():
+  model = _model_path(_path(0.04))
+  assert model is not None
+  constant = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.0, v_ego=8.0)
+  rising = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.01, v_ego=8.0)
+  assert 0.0 < rising.path_offset < constant.path_offset
+  assert 0.0 < rising.path_angle < constant.path_angle
+
+
+def test_model_path_exit_zeros_lingering_c2_and_countersteers():
+  command = _command(_path(0.0), 0.004, current_curvature=0.006)
+  assert command.path_offset <= 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_model_path_reversal_zeros_opposing_lingering_c2():
+  command = _command(_path(-0.004), 0.004, current_curvature=0.002)
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_s_turn_reverses_model_pose_without_slow_c2():
+  controller = FordPathController(dt=0.05)
+  for _ in range(10):
+    controller.update(_path(0.04), 0.04, v_ego=8.0)
+  outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)]
+  assert all(command.curvature == 0.0 for command in outputs)
+  assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0)
+  assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0)
+  assert outputs[-1].path_offset < 0.0
+  assert outputs[-1].path_angle < 0.0
+
+
+def test_output_limits_and_rates_are_bounded():
+  controller = FordPathController()
+  outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)]
+  assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs)
+  assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs)
+  assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs)
+  assert np.max(np.abs(np.diff([command.path_offset for command in outputs]))) <= 0.04 + 1e-9
+  assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
+
+
+def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
+  horizon = 7.0
+  for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
+    model = _path(curvature)
+    command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
+    path = _model_path(model)
+    assert path is not None
+    advance = 0.1 * horizon
+    model_offset, model_angle = _relative_pose(advance + horizon, path,
+                                                _predicted_pose(advance, curvature, 0.0))
+
+    assert command.path_angle == angle_limit
+    assert np.isclose(command.path_offset + horizon * command.path_angle,
+                      model_offset + horizon * model_angle)
+
+
+def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    active = controller.update(_path(0.04), 0.04, v_ego=8.0)
+  invalid = controller.update(None, 0.0, v_ego=8.0)
+  assert invalid.valid
+  assert abs(invalid.path_offset) < abs(active.path_offset)
+  assert abs(invalid.path_angle) < abs(active.path_angle)
+  assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid
+
+
+def test_sunnypilot_path_message_round_trip():
+  message = custom.CarControlSP.new_message()
+  message.fordLateralPath.pathOffset = 0.3
+  message.fordLateralPath.pathAngle = -0.2
+  message.fordLateralPath.curvature = 0.008
+  message.fordLateralPath.curvatureRate = -0.0004
+  message.fordLateralPath.valid = True
+  path = convert_carControlSP(message.as_reader()).fordLateralPath
+  assert np.isclose(path.pathOffset, 0.3)
+  assert np.isclose(path.pathAngle, -0.2)
+  assert np.isclose(path.curvature, 0.008)
+  assert np.isclose(path.curvatureRate, -0.0004)
+  assert path.valid
+
+
+def test_pscm_observer_mirrors_exact_250hz_slew_and_c3_target():
+  observer = FordPscmObserver()
+  observer.set_command(FordPath(True, 1.0, 0.5, 0.0, 0.001))
+  observer.advance(1.0)
+  assert np.isclose(observer.state.path_offset, 1.0)
+  assert np.isclose(observer.state.path_angle, 0.100006103515625)
+  assert np.isclose(observer.state.curvature, 0.0030059814453125)
+
+
+def test_pscm_observer_tracks_wire_quantized_commands():
+  observer = FordPscmObserver()
+  observer.set_command(FordPath(True, 0.006, 0.0004, 0.000011, 0.0))
+  assert observer.command.path_offset == 0.01
+  assert observer.command.path_angle == 0.0005
+  assert observer.command.curvature == 0.00002
+
+
+def test_pscm_c2_contribution_is_speed_scheduled():
+  state = FordPscmObserver().state
+  state = type(state)(curvature=0.004)
+  low = _pscm_contributions(state, 5.0)[2]
+  high = _pscm_contributions(state, 20.0)[2]
+  assert high > low * 10.0
+
+
+def test_pscm_observer_fills_missing_gentle_c2_with_fast_fields():
+  controller = FordPscmObserverPathController(dt=0.01)
+  command = controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                              v_ego=20.0, v_ego_raw=20.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature > 0.0
+
+
+def test_pscm_observer_uses_c0_only_after_c1_reaches_its_effective_limit():
+  controller = FordPscmObserverPathController(dt=0.01)
+  small = controller._command_for_state(FordPath(True, 0.2, 0.0, 0.0, 0.0), 8.0)
+  large = controller._command_for_state(FordPath(True, 1.0, 0.5, 0.0, 0.0), 8.0)
+  assert small.path_offset == 0.0
+  assert small.path_angle > 0.0
+  assert large.path_offset > 0.0
+  assert large.path_angle == 0.349609375 / 10.0
+
+
+def test_pscm_observer_preserves_c2_residual_across_c0_c1_headroom():
+  controller = FordPscmObserverPathController(dt=0.01)
+  target = FordPath(True, 0.0, 0.0, 0.004, 0.0)
+  command = controller._command_for_state(target, 20.0)
+  target_contribution = sum(_pscm_contributions(FordPscmState(curvature=target.curvature), 20.0))
+  command_contributions = _pscm_contributions(FordPscmState(command.path_offset, command.path_angle), 20.0)
+  assert np.isclose(sum(command_contributions), target_contribution)
+
+  controller.observer.state = FordPscmState(curvature=0.004)
+  unwind = controller._command_for_state(FordPath(valid=True), 20.0)
+  unwind_contributions = _pscm_contributions(FordPscmState(unwind.path_offset, unwind.path_angle), 20.0)
+  lingering_c2 = _pscm_contributions(controller.observer.state, 20.0)[2]
+  assert np.isclose(sum(unwind_contributions) + lingering_c2, 0.0)
+
+
+def test_pscm_observer_unloads_fast_residual_as_c2_loads():
+  controller = FordPscmObserverPathController(dt=0.01)
+  outputs = [controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                               v_ego=20.0, v_ego_raw=20.0) for _ in range(200)]
+  assert outputs[0].path_angle > outputs[-1].path_angle >= 0.0
+  assert controller.observer.state.curvature > 0.003
+
+
+def test_pscm_observer_counters_lingering_c2_during_model_exit():
+  controller = FordPscmObserverPathController(dt=0.01)
+  for _ in range(200):
+    controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                      v_ego=20.0, v_ego_raw=20.0)
+  command = controller.update(_path(0.0, speed=20.0), 0.0, current_curvature=0.004,
+                              v_ego=20.0, v_ego_raw=20.0)
+  assert command.path_angle < 0.0
+  assert command.curvature < controller.observer.state.curvature
+
+
+def test_pscm_observer_avoids_ineffective_c0_c1_windup():
+  controller = FordPscmObserverPathController(dt=1.0)
+  command = controller.update(_path(0.2), 0.2, v_ego=8.0, v_ego_raw=8.0)
+  assert abs(command.path_offset) <= 1.0
+  assert abs(command.path_angle) <= 0.349609375 / 10.0

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -32,7 +32,14 @@ from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
 from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
 from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
+from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
+  AssistCategory,
+  AssistedDrivingMilestones,
+  MilestoneEvent,
+  MilestoneStore,
+)
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.system.statsd import statlog
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -88,7 +95,8 @@ class SelfdriveD(CruiseHelper):
     self.big_model_ready_t = 0.
 
     # Setup sockets
-    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
+                                  ['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
 
     self.gps_location_service = get_gps_location_service(self.params)
     self.gps_packets = [self.gps_location_service]
@@ -127,6 +135,7 @@ class SelfdriveD(CruiseHelper):
       self.params.remove("ExperimentalMode")
 
     self.CS_prev = car.CarState.new_message()
+    self.car_state_log_mono_time = 0
     self.AM = AlertManager()
     self.events = Events()
 
@@ -137,6 +146,11 @@ class SelfdriveD(CruiseHelper):
     self.cruise_mismatch_counter = 0
     self.last_steering_pressed_frame = 0
     self.distance_traveled = 0
+    self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
+    self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
+    self.assisted_driving_milestone_drive_id = ""
+    self._milestone_event: MilestoneEvent | None = None
+    self._milestone_event_expires_ns = 0
     self.last_functional_fan_frame = 0
     self.events_prev = []
     self.logged_comm_issue = None
@@ -528,6 +542,8 @@ class SelfdriveD(CruiseHelper):
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)
     CS = _car_state.carState if _car_state else self.CS_prev
+    if _car_state is not None:
+      self.car_state_log_mono_time = _car_state.logMonoTime
 
     self.sm.update(0)
 
@@ -646,6 +662,31 @@ class SelfdriveD(CruiseHelper):
       self.pm.send('onroadEventsSP', ce_send_sp)
     self.events_sp_prev = self.events_sp.names.copy()
 
+  def publish_assisted_driving_milestones(self, now_ns: int, event: MilestoneEvent | None) -> None:
+    if event is not None:
+      self._milestone_event = event
+      self._milestone_event_expires_ns = now_ns + 1_000_000_000
+    elif now_ns >= self._milestone_event_expires_ns:
+      self._milestone_event = None
+
+    if event is None and self.sm.frame % 10 != 0:
+      return
+
+    snapshot = self.assisted_driving_milestones.snapshot()
+    msg = messaging.new_message("assistedDrivingMilestoneState")
+    msg.valid = True
+    state = msg.assistedDrivingMilestoneState
+    state.enabled = self.assisted_driving_milestones_enabled
+    state.madsDistanceMeters = snapshot.distances_meters[AssistCategory.MADS]
+    state.fullAssistDistanceMeters = snapshot.distances_meters[AssistCategory.FULL_ASSIST]
+    if self._milestone_event is not None:
+      state.event.id = self._milestone_event.event_id
+      state.event.category = self._milestone_event.category.value
+      state.event.distanceMeters = self._milestone_event.distance_meters
+      state.event.previousDistanceMeters = self._milestone_event.previous_distance_meters
+      state.event.unit = self._milestone_event.unit.value
+    self.pm.send("assistedDrivingMilestoneState", msg)
+
   def step(self):
     CS = self.data_sample()
     self.update_events(CS)
@@ -655,6 +696,28 @@ class SelfdriveD(CruiseHelper):
       self.mads.update(CS)
     self.update_alerts(CS)
 
+    now_ns = time.monotonic_ns()
+    if not self.assisted_driving_milestone_drive_id:
+      self.assisted_driving_milestone_drive_id = self.params.get("CurrentRoute") or ""
+      self.assisted_driving_milestones.set_drive_id(self.assisted_driving_milestone_drive_id)
+    car_control = self.sm['carControl']
+    milestone_event = self.assisted_driving_milestones.update(
+      self.car_state_log_mono_time,
+      CS.vEgo,
+      lat_active=car_control.latActive,
+      long_active=car_control.longActive,
+      is_metric=self.is_metric,
+      enabled=self.assisted_driving_milestones_enabled,
+    )
+    if milestone_event is not None:
+      cloudlog.event("assisted_driving_milestone_reached",
+                     event_id=milestone_event.event_id,
+                     category=milestone_event.category.value,
+                     distance_meters=milestone_event.distance_meters)
+      statlog.gauge(f"assisted_driving_milestone.{milestone_event.category.value}.meters",
+                    milestone_event.distance_meters)
+    self.publish_assisted_driving_milestones(now_ns, milestone_event)
+
     self.button_state_tracker.update(CS)
     self.publish_selfdriveState(CS)
 
@@ -667,6 +730,7 @@ class SelfdriveD(CruiseHelper):
       self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
       self.personality = self.params.get("LongitudinalPersonality", return_default=True)
+      self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
 
       self.mads.read_params()
       time.sleep(0.1)
@@ -680,6 +744,7 @@ class SelfdriveD(CruiseHelper):
         self.step()
         self.rk.monitor_time()
     finally:
+      self.assisted_driving_milestones.close()
       e.set()
       t.join()
 

openpilot/selfdrive/ui/mici/layouts/main.py

@@ -1,5 +1,8 @@
+import os
+
 import pyray as rl
 import openpilot.cereal.messaging as messaging
+from openpilot.common.hardware import PC
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
@@ -61,7 +64,8 @@ class MiciMainLayout(Scroller):
 
     # Start onboarding if terms or training not completed, make sure to push after self
     self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
-    if not self._onboarding_window.completed:
+    skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
+    if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
       gui_app.push_widget(self._onboarding_window)
 
     # initialize correct onroad layout
@@ -119,6 +123,8 @@ class MiciMainLayout(Scroller):
         self._onroad_time_delay = rl.get_time()
       else:
         self._scroll_to(self._home_layout)
+        if hasattr(self._home_layout, "request_drive_summary"):
+          self._home_layout.request_drive_summary()
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -47,6 +47,7 @@ class TogglesLayoutMici(NavScroller):
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
     always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
+    milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
     record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
     record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
     enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
@@ -57,6 +58,7 @@ class TogglesLayoutMici(NavScroller):
       is_metric_toggle,
       ldw_toggle,
       always_on_dm_toggle,
+      milestone_celebrations_toggle,
       record_front,
       record_mic,
       enable_openpilot,
@@ -68,6 +70,7 @@ class TogglesLayoutMici(NavScroller):
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
+      ("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
       ("RecordFront", record_front),
       ("RecordAudio", record_mic),
       ("OpenpilotEnabledToggle", enable_openpilot),

openpilot/selfdrive/ui/mici/onroad/alert_renderer.py

@@ -20,6 +20,7 @@ AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus
 
 ALERT_MARGIN = 18
+ALERT_BACKGROUND_OPACITY = 0.90
 
 ALERT_FONT_SMALL = 66 - 50
 ALERT_FONT_BIG = 88 - 40
@@ -279,7 +280,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
   def _draw_background(self, alert: Alert) -> None:
     # draw top gradient for alert text at top
     color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
-    color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
+    color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
     translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
 
     small_alert_height = round(self._rect.height * 0.583) # 140px at mici height

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -19,10 +19,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
 from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum
 
+MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
+
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
   from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
 
+  if MILESTONE_CELEBRATION_ENABLED:
+    from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
+
 OpState = log.SelfdriveState.OpenpilotState
 CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
 NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
@@ -156,6 +161,7 @@ class AugmentedRoadView(CameraView):
     self._alert_renderer = AlertRenderer()
     self._driver_state_renderer = DriverStateRenderer()
     self._confidence_ball = ConfidenceBall()
+    self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
     self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                        text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
                                        alignment=TextAlignment.CENTER,
@@ -223,6 +229,12 @@ class AugmentedRoadView(CameraView):
 
     alert_to_render, not_animating_out = self._alert_renderer.will_render()
 
+    if self._milestone_celebration is not None:
+      if alert_to_render is not None:
+        self._milestone_celebration.cancel_for_alert()
+      else:
+        self._milestone_celebration.render(self._content_rect)
+
     # Hide DMoji when disengaged unless AlwaysOnDM is enabled
     should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
                          (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
@@ -247,7 +259,6 @@ class AugmentedRoadView(CameraView):
     self._confidence_ball.render(self.rect)
 
     self._bookmark_icon.render(self.rect)
-
   def _switch_stream_if_needed(self, sm):
     if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
       v_ego = sm['carState'].vEgo
@@ -355,10 +366,12 @@ class AugmentedRoadView(CameraView):
     return self._cached_matrix
 
   def show_event(self):
+    super().show_event()
     if gui_app.sunnypilot_ui():
       ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
 
   def hide_event(self):
+    super().hide_event()
     if gui_app.sunnypilot_ui():
       ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
 

openpilot/selfdrive/ui/soundd.py

@@ -24,14 +24,8 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
 SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
 FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)
 
-AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
 DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
 
-VOLUME_BASE = 20
-if HARDWARE.get_device_type() == "tizi":
-  AMBIENT_DB = 30
-  VOLUME_BASE = 10
-
 AudibleAlert = log.SelfdriveState.AudibleAlert
 AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
 
@@ -53,6 +47,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
   AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),
 
   AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
+  AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),
 
   AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
   AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
@@ -60,6 +55,14 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
   **sound_list_sp,
 }
 
+
+def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
+  ambient_db = 30 if device_type in ("mici", "tizi") else 26
+  volume_base = 10 if device_type in ("mici", "tizi") else 20
+  volume_boost = 1.5 if device_type == "mici" else 1.0
+  volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
+  return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
+
 def check_selfdrive_timeout_alert(sm):
   ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
 
@@ -74,6 +77,7 @@ class Soundd(QuietMode):
   def __init__(self):
     super().__init__()
 
+    self.device_type = HARDWARE.get_device_type()
     self.load_sounds()
 
     self.current_alert = AudibleAlert.none
@@ -85,6 +89,7 @@ class Soundd(QuietMode):
 
     self.selfdrive_timeout_alert = False
     self.pending_stop = False
+    self.last_milestone_event_id = 0
 
     self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
 
@@ -164,9 +169,19 @@ class Soundd(QuietMode):
       self.update_alert(AudibleAlert.none)
       self.selfdrive_timeout_alert = False
 
+  def update_milestone_alert(self, sm):
+    if not sm.updated['assistedDrivingMilestoneState']:
+      return
+    milestone_state = sm['assistedDrivingMilestoneState']
+    event_id = milestone_state.event.id
+    if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
+      return
+    self.last_milestone_event_id = event_id
+    if self.current_alert == AudibleAlert.none and not self.enabled:
+      self.update_alert(AudibleAlert.complete)
+
   def calculate_volume(self, weighted_db):
-    volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
+    return calculate_volume_for_device(weighted_db, self.device_type)
-    return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
 
   @retry(attempts=10, delay=3)
   def get_stream(self, sd):
@@ -180,7 +195,7 @@ class Soundd(QuietMode):
     import sounddevice as sd
     micd.patch_sounddevice(sd)
 
-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)
@@ -198,6 +213,7 @@ class Soundd(QuietMode):
             self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
 
         self.get_audible_alert(sm)
+        self.update_milestone_alert(sm)
 
         # Ramp up immediate warning sound over 4s
         if self.current_alert == AudibleAlert.warningImmediate:

openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py

@@ -5,19 +5,79 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import math
+import time
 
 import pyray as rl
 
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
-from openpilot.system.ui.lib.application import FontWeight
+from openpilot.system.ui.lib.application import FontWeight, TextAlignment
-from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
+
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+SUMMARY_DURATION_SECONDS = 10.0
+SUMMARY_WAIT_SECONDS = 3.0
+
+
+def _nonnegative_float(value) -> float:
+  try:
+    return max(0.0, float(value))
+  except (TypeError, ValueError):
+    return 0.0
 
 
 class MiciHomeLayoutSP(MiciHomeLayout):
   def __init__(self):
     super().__init__()
     self._openpilot_label = UnifiedLabel("sunnypilot", font_size=88, font_weight=FontWeight.AUDIOWIDE, max_width=480, wrap_text=False)
+    initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+    self._last_summary_id = initial_summary.get("id", 0)
+    self._summary_wait_until = 0.0
+    self._summary_visible_until = 0.0
+    self._drive_summary = {}
+
+  def request_drive_summary(self) -> None:
+    self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
+
+  def _render(self, _: rl.Rectangle) -> None:
+    super()._render(_)
+    now = time.monotonic()
+    if now < self._summary_wait_until:
+      summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+      summary_id = summary.get("id", 0)
+      if summary_id and summary_id != self._last_summary_id:
+        self._last_summary_id = summary_id
+        distances = summary.get("distancesMeters", {})
+        enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
+        if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
+          self._drive_summary = summary
+          self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
+        self._summary_wait_until = 0.0
+
+    if now < self._summary_visible_until:
+      self._draw_drive_summary(_)
+
+  def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
+    distances = self._drive_summary.get("distancesMeters", {})
+    metric = self._drive_summary.get("unit") == "metric"
+    meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
+    unit = "KM" if metric else "MI"
+    mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
+    full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
+
+    rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
+    gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
+              font_weight=FontWeight.SEMI_BOLD, alignment=TextAlignment.CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=TextAlignment.CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=TextAlignment.CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=TextAlignment.CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=TextAlignment.CENTER)
 
   def _set_chestnut_visibility(self):
     usb_connected = ui_state.usb_connected

openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py

@@ -0,0 +1,228 @@
+"""Render assisted-driving milestone celebrations over the on-road view."""
+
+import math
+import random
+import time
+from collections import deque
+from dataclasses import dataclass
+
+import pyray as rl
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
+from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+
+CELEBRATION_DURATION = 4.5
+PARTICLE_COUNT = 150
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+
+CONFETTI_COLORS = (
+  rl.Color(255, 55, 95, 255),
+  rl.Color(255, 183, 3, 255),
+  rl.Color(48, 209, 88, 255),
+  rl.Color(36, 179, 255, 255),
+  rl.Color(112, 72, 232, 255),
+  rl.Color(255, 45, 196, 255),
+)
+
+
+@dataclass(frozen=True)
+class ConfettiParticle:
+  x: float
+  y: float
+  width: float
+  height: float
+  speed: float
+  drift: float
+  angle: float
+  spin: float
+  phase: float
+  color: rl.Color
+
+
+@dataclass(frozen=True)
+class CelebrationMilestone:
+  event_id: int
+  full_assist: bool
+  distance_meters: float
+  previous_distance_meters: float
+  metric: bool
+
+
+class MilestoneCelebration(Widget):
+  """Pure renderer for typed assisted-driving milestone events."""
+
+  def __init__(self):
+    super().__init__()
+    self._drive_started_time = -1.0
+    self._celebration_started_time: float | None = None
+    self._current_milestone: CelebrationMilestone | None = None
+    self._pending_milestones: deque[CelebrationMilestone] = deque()
+    self._last_event_id = 0
+    self._particles = self._make_particles()
+
+  @staticmethod
+  def _make_particles() -> list[ConfettiParticle]:
+    rng = random.Random(20260828)
+    return [
+      ConfettiParticle(
+        x=rng.random(),
+        y=rng.uniform(-0.25, 0.95),
+        width=rng.uniform(10, 24),
+        height=rng.uniform(24, 58),
+        speed=rng.uniform(0.12, 0.34),
+        drift=rng.uniform(-0.035, 0.035),
+        angle=rng.uniform(0, 360),
+        spin=rng.uniform(-150, 150),
+        phase=rng.uniform(0, math.tau),
+        color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
+      )
+      for _ in range(PARTICLE_COUNT)
+    ]
+
+  def _render(self, rect: rl.Rectangle, /) -> None:
+    now = time.monotonic()
+    if ui_state.started_time != self._drive_started_time:
+      self._drive_started_time = ui_state.started_time
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+
+    self._consume_event(suppress=False)
+
+    if self._current_milestone is None and self._pending_milestones:
+      self._current_milestone = self._pending_milestones.popleft()
+      self._celebration_started_time = now
+
+    if self._celebration_started_time is None or self._current_milestone is None:
+      return
+
+    elapsed = now - self._celebration_started_time
+    if elapsed >= CELEBRATION_DURATION:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      return
+
+    alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
+    self._draw_background_scrim(rect, alpha)
+    self._draw_confetti(rect, elapsed, alpha)
+    self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
+
+  def cancel_for_alert(self) -> None:
+    self._consume_event(suppress=True)
+    self._celebration_started_time = None
+    self._current_milestone = None
+    self._pending_milestones.clear()
+
+  def _consume_event(self, suppress: bool) -> None:
+    if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
+      return
+    state = ui_state.sm["assistedDrivingMilestoneState"]
+    event = state.event
+    if not state.enabled:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+      return
+    if event.id == 0 or event.id == self._last_event_id:
+      return
+    self._last_event_id = event.id
+    if suppress:
+      return
+    self._pending_milestones.append(CelebrationMilestone(
+      event_id=event.id,
+      full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
+      distance_meters=event.distanceMeters,
+      previous_distance_meters=event.previousDistanceMeters,
+      metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
+    ))
+
+  def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
+    travel_height = rect.height * 1.45
+    compact = rect.height <= 300
+    particle_scale = rect.height / 1080.0
+    particles = self._particles[:100] if compact else self._particles
+    for particle in particles:
+      x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
+      y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
+      flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
+      particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
+      origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
+      color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
+      rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
+
+  @staticmethod
+  def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
+    # Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
+    scale = rect.height / 240.0
+    pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
+    number_size = int(FONT_SIZES.set_speed * scale * pulse)
+    milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
+    category_size = int(22 * scale * pulse)
+    unit_size = category_size
+
+    display_font = gui_app.font(FontWeight.DISPLAY)
+    semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
+    tween_progress = min(elapsed / 0.85, 1.0)
+    tween_progress = 1.0 - (1.0 - tween_progress) ** 3
+    meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
+    previous_distance = milestone.previous_distance_meters / meters_per_unit
+    milestone_distance = milestone.distance_meters / meters_per_unit
+    displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
+    if tween_progress >= 1.0:
+      number = f"{round(milestone_distance):,}"
+    else:
+      number = f"{displayed_distance:,.1f}"
+    unit = tr("KM") if milestone.metric else tr("MI")
+    category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
+    milestone_label = tr("MILESTONE")
+
+    unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
+    number_bounds = measure_text_cached(display_font, number, number_size)
+    max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
+    if number_bounds.x > max_number_width:
+      number_size = max(1, int(number_size * max_number_width / number_bounds.x))
+      number_bounds = measure_text_cached(display_font, number, number_size)
+    category_bounds = measure_text_cached(semibold_font, category, category_size)
+    milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
+
+    center_x = rect.x + rect.width / 2
+    center_y = rect.y + rect.height / 2
+    text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
+    secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
+    number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
+    number_x = center_x - number_line_width / 2
+    number_y = center_y - 76 * scale
+    unit_y = center_y + 14 * scale
+    category_y = center_y - 91 * scale
+    milestone_y = center_y + 50 * scale
+
+    rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
+                    category_size, 0, secondary_color)
+    rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
+    rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
+                    unit_size, 0, secondary_color)
+    rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
+                    milestone_size, 0, text_color)
+
+  @staticmethod
+  def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
+    # Match the alert background: a mostly opaque black core fading to transparent.
+    fade_height = round(rect.height * 0.25)
+    solid_height = round(rect.height * 0.50)
+    solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
+    transparent = rl.Color(0, 0, 0, 0)
+    x = int(rect.x)
+    y = int(rect.y)
+    width = int(rect.width)
+
+    rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
+    rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
+    rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -35,7 +35,8 @@ class UIStateSP:
     self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
+      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
+      "assistedDrivingMilestoneState",
     ]
 
     self.sunnylink_state = SunnylinkState()

openpilot/selfdrive/ui/tests/generate_milestone_sound.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+"""Generate the assisted-driving milestone celebration chime."""
+
+import math
+import wave
+from array import array
+from pathlib import Path
+
+
+SAMPLE_RATE = 48_000
+DURATION_SECONDS = 0.82
+NOTES = (
+  (0.00, 523.25),
+  (0.11, 659.25),
+  (0.22, 783.99),
+)
+
+
+def note_sample(age: float, frequency: float) -> float:
+  if not 0 <= age <= 0.58:
+    return 0.0
+  attack = min(age / 0.008, 1.0)
+  release = min((0.58 - age) / 0.15, 1.0)
+  envelope = attack * release * math.exp(-3.8 * age)
+  tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
+  return envelope * tone
+
+
+def main() -> None:
+  output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
+  samples = array('h')
+  for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
+    t = frame / SAMPLE_RATE
+    value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
+    samples.append(round(max(-1.0, min(1.0, value)) * 32767))
+
+  with wave.open(str(output), "wb") as wav:
+    wav.setnchannels(1)
+    wav.setsampwidth(2)
+    wav.setframerate(SAMPLE_RATE)
+    wav.writeframes(samples.tobytes())
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/ui/tests/milestone_preview.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+"""Publish deterministic milestone events for the local comma-four UI preview."""
+
+import itertools
+import time
+
+from openpilot.cereal import messaging
+
+
+def main() -> None:
+  pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
+  milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
+  event_id = 0
+  milestone, previous_milestone, category = 0, 0, "mads"
+  next_event_time = time.monotonic() + 1.0
+
+  while True:
+    now = time.monotonic()
+    if now >= next_event_time:
+      event_id += 1
+      milestone, previous_milestone, category = next(milestones)
+      next_event_time = now + 6.0
+
+    msg = messaging.new_message("assistedDrivingMilestoneState")
+    state = msg.assistedDrivingMilestoneState
+    state.enabled = True
+    if event_id:
+      state.event.id = event_id
+      state.event.category = category
+      state.event.distanceMeters = milestone * 1609.344
+      state.event.previousDistanceMeters = previous_milestone * 1609.344
+      state.event.unit = "imperial"
+    pm.send("assistedDrivingMilestoneState", msg)
+    time.sleep(0.1)
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/ui/tests/run_milestone_celebration.sh

@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+set -e
+
+repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
+replay_pid=""
+preview_pid=""
+
+cleanup() {
+  for pid in "$preview_pid" "$replay_pid"; do
+    if [[ -n "$pid" ]]; then
+      kill "$pid" 2>/dev/null || true
+      wait "$pid" 2>/dev/null || true
+    fi
+  done
+}
+trap cleanup EXIT INT TERM
+
+export PATH="$repo_root/.venv/bin:$PATH"
+export SP_MILESTONE_PREVIEW=1
+playback="${SP_MILESTONE_PLAYBACK:-1}"
+
+"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
+replay_pid=$!
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
+preview_pid=$!
+
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"

openpilot/selfdrive/ui/tests/test_soundd.py

@@ -4,12 +4,63 @@ import time
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import log, messaging
 from openpilot.cereal.messaging import SubMaster, PubMaster
-from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
+from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
 
 AudibleAlert = log.SelfdriveState.AudibleAlert
 
 
 class TestSoundd(OpenpilotTestCase):
+  @staticmethod
+  def milestone_submaster(event_id=42):
+    class SubMasterStub:
+      def __init__(self):
+        self.updated = {'assistedDrivingMilestoneState': True}
+        msg = messaging.new_message('assistedDrivingMilestoneState')
+        msg.assistedDrivingMilestoneState.enabled = True
+        msg.assistedDrivingMilestoneState.event.id = event_id
+        self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
+
+      def __getitem__(self, service):
+        return self.data[service]
+
+    return SubMasterStub()
+
+  def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
+    for weighted_db in (20.0, 30.0, 40.0, 50.0):
+      with self.subTest(weighted_db=weighted_db):
+        comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
+        comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
+        assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
+
+  def test_milestone_chime_uses_typed_milestone_event_once(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.complete
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_safety_alert_consumes_milestone_without_replaying_it(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.current_alert = AudibleAlert.warningImmediate
+
+    soundd.update_milestone_alert(sm)
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_quiet_mode_consumes_milestone_without_playing_it(self):
+    soundd = Soundd()
+    soundd.enabled = True
+
+    soundd.update_milestone_alert(self.milestone_submaster())
+
+    assert soundd.current_alert == AudibleAlert.none
+
   def test_check_selfdrive_timeout_alert(self, mocker):
     sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
     pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -104,6 +104,14 @@ class ControlsExt(ModelStateBase):
     CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
     CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
 
+    ford_path = getattr(self, 'ford_path', None)
+    if ford_path is not None:
+      CC_SP.fordLateralPath.valid = ford_path.valid
+      CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
+      CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
+      CC_SP.fordLateralPath.curvature = ford_path.curvature
+      CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
+
     return CC_SP
 
   @staticmethod

openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py

@@ -0,0 +1,260 @@
+"""Authoritative assisted-driving distance and milestone tracking."""
+
+import math
+from collections.abc import Mapping
+from dataclasses import dataclass
+from enum import StrEnum
+
+from openpilot.common.params import Params
+
+
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+MAX_SAMPLE_INTERVAL_SECONDS = 0.5
+PERSIST_INTERVAL_NS = 10_000_000_000
+STATE_VERSION = 1
+STATE_PARAM = "AssistedDrivingMilestoneState"
+LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
+
+
+class AssistCategory(StrEnum):
+  MADS = "mads"
+  FULL_ASSIST = "fullAssist"
+
+
+class MilestoneUnit(StrEnum):
+  IMPERIAL = "imperial"
+  METRIC = "metric"
+
+
+@dataclass(frozen=True)
+class MilestoneEvent:
+  event_id: int
+  category: AssistCategory
+  distance_meters: float
+  previous_distance_meters: float
+  unit: MilestoneUnit
+
+
+@dataclass(frozen=True)
+class MilestoneSnapshot:
+  distances_meters: dict[AssistCategory, float]
+  drive_start_distances_meters: dict[AssistCategory, float]
+  next_event_id: int
+  next_summary_id: int
+  unit: MilestoneUnit
+  active_drive_id: str
+
+
+def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
+  if not lat_active:
+    return None
+  return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
+
+
+def _meters_per_unit(unit: MilestoneUnit) -> float:
+  return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
+
+
+def _next_ladder_value(value: float) -> float:
+  value = max(0.0, value)
+  magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
+  for multiplier in (1.0, 2.0, 5.0):
+    candidate = multiplier * magnitude
+    if candidate > value + 1e-9:
+      return candidate
+  return 10.0 * magnitude
+
+
+def _previous_ladder_value(value: float) -> float:
+  if value <= 1.0:
+    return 0.0
+  magnitude = 10.0 ** math.floor(math.log10(value))
+  normalized = value / magnitude
+  if normalized <= 1.0 + 1e-9:
+    return 5.0 * magnitude / 10.0
+  if normalized <= 2.0 + 1e-9:
+    return magnitude
+  return 2.0 * magnitude
+
+
+def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
+  meters_per_unit = _meters_per_unit(unit)
+  return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
+
+
+class MilestoneStore:
+  def __init__(self, params: Params | None = None):
+    self._params = params or Params()
+
+  def load(self) -> MilestoneSnapshot:
+    raw = self._params.get(STATE_PARAM, return_default=True)
+    raw = raw if isinstance(raw, dict) else {}
+    raw_distances = raw.get("distancesMeters", {})
+    raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
+    try:
+      unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
+    except ValueError:
+      unit = MilestoneUnit.IMPERIAL
+
+    def distance(category: AssistCategory) -> float:
+      try:
+        return max(0.0, float(raw_distances.get(category.value, 0.0)))
+      except (TypeError, ValueError):
+        return 0.0
+
+    distances = {category: distance(category) for category in AssistCategory}
+    raw_drive_start = raw.get("driveStartDistancesMeters", {})
+    raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
+
+    def drive_start_distance(category: AssistCategory) -> float:
+      try:
+        return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
+      except (TypeError, ValueError):
+        return distances[category]
+
+    try:
+      next_event_id = max(1, int(raw.get("nextEventId", 1)))
+    except (TypeError, ValueError):
+      next_event_id = 1
+    try:
+      next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
+    except (TypeError, ValueError):
+      next_summary_id = 1
+
+    return MilestoneSnapshot(
+      distances_meters=distances,
+      drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
+      next_event_id=next_event_id,
+      next_summary_id=next_summary_id,
+      unit=unit,
+      active_drive_id=str(raw.get("activeDriveId", "")),
+    )
+
+  def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
+    if block:
+      self._params.flush()
+    self._params.put(STATE_PARAM, {
+      "version": STATE_VERSION,
+      "distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
+      "driveStartDistancesMeters": {
+        category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
+      },
+      "nextEventId": max(1, snapshot.next_event_id),
+      "nextSummaryId": max(1, snapshot.next_summary_id),
+      "unit": snapshot.unit.value,
+      "activeDriveId": snapshot.active_drive_id,
+    }, block=block)
+
+  def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
+    self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
+      "version": STATE_VERSION,
+      "id": summary_id,
+      "distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
+      "unit": unit.value,
+    }, block=True)
+
+
+class AssistedDrivingMilestones:
+  """Tracks, persists, and emits milestones through one small interface."""
+
+  def __init__(self, store: MilestoneStore | None = None):
+    self._store = store or MilestoneStore()
+    snapshot = self._store.load()
+    self._distances_meters = snapshot.distances_meters
+    self._drive_start_distances_meters = snapshot.drive_start_distances_meters
+    self._next_event_id = snapshot.next_event_id
+    self._next_summary_id = snapshot.next_summary_id
+    self._unit = snapshot.unit
+    self._active_drive_id = snapshot.active_drive_id
+    self._next_milestone_meters = {
+      category: next_milestone_meters(distance, self._unit)
+      for category, distance in self._distances_meters.items()
+    }
+    self._last_timestamp_ns: int | None = None
+    self._last_persist_timestamp_ns: int | None = None
+    self._last_speed_mps = 0.0
+    self._last_category: AssistCategory | None = None
+    self._enabled = False
+    self._closed = False
+
+  def snapshot(self) -> MilestoneSnapshot:
+    return MilestoneSnapshot(
+      self._distances_meters.copy(),
+      self._drive_start_distances_meters.copy(),
+      self._next_event_id,
+      self._next_summary_id,
+      self._unit,
+      self._active_drive_id,
+    )
+
+  def set_drive_id(self, drive_id: str) -> None:
+    if not drive_id or drive_id == self._active_drive_id:
+      return
+    self._active_drive_id = drive_id
+    self._drive_start_distances_meters = self._distances_meters.copy()
+    self._persist()
+
+  def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
+             is_metric: bool, enabled: bool) -> MilestoneEvent | None:
+    self._enabled = enabled
+    unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
+    if unit != self._unit:
+      self._unit = unit
+      self._next_milestone_meters = {
+        category: next_milestone_meters(distance, unit)
+        for category, distance in self._distances_meters.items()
+      }
+
+    speed_mps = max(0.0, speed_mps)
+    category = assist_category(lat_active, long_active) if enabled else None
+    event = None
+
+    if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
+      dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
+      if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
+        active_category = self._last_category
+        self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
+        threshold_meters = self._next_milestone_meters[active_category]
+        if self._distances_meters[active_category] >= threshold_meters:
+          meters_per_unit = _meters_per_unit(self._unit)
+          threshold_units = threshold_meters / meters_per_unit
+          event = MilestoneEvent(
+            event_id=self._next_event_id,
+            category=active_category,
+            distance_meters=threshold_meters,
+            previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
+            unit=self._unit,
+          )
+          self._next_event_id += 1
+          self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
+          self._persist(timestamp_ns=timestamp_ns)
+
+    self._last_timestamp_ns = timestamp_ns
+    self._last_speed_mps = speed_mps
+    self._last_category = category
+
+    if self._last_persist_timestamp_ns is None:
+      self._last_persist_timestamp_ns = timestamp_ns
+    elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
+      self._persist(timestamp_ns=timestamp_ns)
+
+    return event
+
+  def close(self) -> None:
+    if self._closed:
+      return
+    self._closed = True
+    drive_distances = {
+      category: self._distances_meters[category] - self._drive_start_distances_meters[category]
+      for category in AssistCategory
+    }
+    summary_id = self._next_summary_id
+    self._next_summary_id += 1
+    self._persist(block=True)
+    if self._enabled:
+      self._store.save_drive_summary(summary_id, drive_distances, self._unit)
+
+  def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
+    self._store.save(self.snapshot(), block=block)
+    self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns

openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py

@@ -0,0 +1,123 @@
+import unittest
+
+from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
+  METERS_PER_MILE,
+  AssistCategory,
+  AssistedDrivingMilestones,
+  MilestoneStore,
+  MilestoneUnit,
+)
+
+
+class ParamsStub:
+  def __init__(self, state=None):
+    self.values = {"AssistedDrivingMilestoneState": state or {}}
+    self.writes = []
+
+  def get(self, key, return_default=False):
+    return self.values.get(key, {} if return_default else None)
+
+  def put(self, key, value, block=False):
+    self.values[key] = value
+    self.writes.append((key, value, block))
+
+  def flush(self):
+    pass
+
+
+class TestAssistedDrivingMilestones(unittest.TestCase):
+  def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
+    params = ParamsStub({
+      "version": 1,
+      "distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
+      "nextEventId": 7,
+      "unit": "imperial",
+    })
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
+    event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+
+    self.assertIsNotNone(event)
+    assert event is not None
+    self.assertEqual(event.event_id, 7)
+    self.assertEqual(event.category, AssistCategory.MADS)
+    self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
+    self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
+    self.assertFalse(params.writes[-1][2])
+
+  def test_switching_units_schedules_only_a_future_milestone(self):
+    params = ParamsStub({
+      "version": 1,
+      "distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
+      "nextEventId": 2,
+      "unit": "imperial",
+    })
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
+    event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
+
+    self.assertIsNotNone(event)
+    assert event is not None
+    self.assertEqual(event.unit, MilestoneUnit.METRIC)
+    self.assertAlmostEqual(event.distance_meters, 10_000.0)
+
+  def test_ignores_disabled_reverse_and_timestamp_gaps(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+    milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+    milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+
+    self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
+
+  def test_close_persists_totals_and_last_drive_summary(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
+    milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
+
+    milestones.close()
+
+    summary = params.values["LastDriveAssistedDrivingSummary"]
+    self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
+    self.assertTrue(params.writes[-1][2])
+
+    write_count = len(params.writes)
+    milestones.close()
+    self.assertEqual(len(params.writes), write_count)
+
+  def test_process_restart_preserves_the_current_drive_start(self):
+    params = ParamsStub()
+    first_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    first_process.set_drive_id("route-1")
+    first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    first_process.close()
+
+    second_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    second_process.set_drive_id("route-1")
+    second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    second_process.close()
+
+    summary = params.values["LastDriveAssistedDrivingSummary"]
+    self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
+
+  def test_disabled_feature_does_not_publish_drive_summary(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+
+    milestones.close()
+
+    self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
+
+
+if __name__ == "__main__":
+  unittest.main()

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -1383,6 +1383,12 @@
               "title": "Steering Arc",
               "description": "Display steering arc on the driving screen when lateral control is enabled."
             },
+            {
+              "key": "AssistedDrivingMilestonesEnabled",
+              "widget": "toggle",
+              "title": "Assisted Driving Milestones",
+              "description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
+            },
             {
               "key": "ShowTurnSignals",
               "widget": "toggle",
@@ -2168,6 +2174,43 @@
     }
   ],
   "vehicle_settings": {
+    "ford": {
+      "title": "Ford Settings",
+      "description": "",
+      "items": [
+        {
+          "key": "FordModelActionController",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Selected-Action Path Tracking (Experimental)",
+          "description": "Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.",
+          "details": "Uses a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Reduces same-direction offset demand when measured turning exceeds the requested turn. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "enablement": [
+            {
+              "type": "offroad_only"
+            }
+          ]
+        },
+        {
+          "key": "FordPscmObserver",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "PSCM Coefficient Observer (Experimental)",
+          "description": "Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.",
+          "details": "This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when Selected-Action Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.",
+          "enablement": [
+            {
+              "type": "offroad_only"
+            },
+            {
+              "type": "param",
+              "key": "FordModelActionController",
+              "equals": false
+            }
+          ]
+        }
+      ]
+    },
     "hyundai": {
       "title": "Hyundai / Kia / Genesis Settings",
       "description": "",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -6,6 +6,29 @@ icon: vehicle
 order: 99
 kind: vehicle
 sections:
+- id: ford
+  title: Ford Settings
+  description: ''
+  items:
+  - key: FordModelActionController
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Selected-Action Path Tracking (Experimental)
+    description: Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.
+    details: Uses a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Reduces same-direction offset demand when measured turning exceeds the requested turn. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    enablement:
+    - $ref: '#/macros/offroad'
+  - key: FordPscmObserver
+    widget: toggle
+    needs_onroad_cycle: true
+    title: PSCM Coefficient Observer (Experimental)
+    description: Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.
+    details: This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when Selected-Action Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.
+    enablement:
+    - $ref: '#/macros/offroad'
+    - type: param
+      key: FordModelActionController
+      equals: false
 - id: hyundai
   title: Hyundai / Kia / Genesis Settings
   description: ''

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml

@@ -20,6 +20,10 @@ sections:
     widget: toggle
     title: Steering Arc
     description: Display steering arc on the driving screen when lateral control is enabled.
+  - key: AssistedDrivingMilestonesEnabled
+    widget: toggle
+    title: Assisted Driving Milestones
+    description: Celebrate cumulative MADS and full-assist distance milestones while driving.
   - key: ShowTurnSignals
     widget: toggle
     title: Display Turn Signals

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -5,6 +5,7 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import json
+import tempfile
 
 from openpilot.common.params import Params
 from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import (
@@ -278,6 +279,34 @@ class TestKnownPanels(OpenpilotTestCase):
 
 
 class TestKnownVehicleSettings(OpenpilotTestCase):
+  def test_ford_model_action_is_separate_default_off_cycle_only_toggle(self, schema):
+    items = _brand_items(schema["vehicle_settings"].get("ford"))
+    candidate = next(item for item in items if item["key"] == "FordModelActionController")
+    assert candidate["title"] == "Selected-Action Path Tracking (Experimental)"
+    assert candidate["widget"] == "toggle"
+    assert candidate["needs_onroad_cycle"] is True
+    assert candidate["enablement"] == [{"type": "offroad_only"}]
+    assert "takes priority over PSCM Coefficient Observer" in candidate["details"]
+    assert "Turning it off restores PSCM Coefficient Observer if selected" in candidate["details"]
+    assert "not road-validated" in candidate["details"]
+    with tempfile.TemporaryDirectory() as path:
+      assert Params(path).get_default_value("FordModelActionController") is False
+
+  def test_retired_ford_toggles_are_not_exposed(self, schema):
+    items = _brand_items(schema["vehicle_settings"].get("ford"))
+    keys = {item["key"] for item in items}
+    assert "FordVirtualAngleController" not in keys
+    assert "FordSharedPathController" not in keys
+
+  def test_ford_has_pscm_observer(self, schema):
+    items = _brand_items(schema["vehicle_settings"].get("ford"))
+    observer = next(item for item in items if item["key"] == "FordPscmObserver")
+    assert observer["needs_onroad_cycle"] is True
+    assert observer["enablement"] == [
+      {"type": "offroad_only"},
+      {"type": "param", "key": "FordModelActionController", "equals": False},
+    ]
+
   def test_hyundai_has_longitudinal_tuning(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
     assert "HyundaiLongitudinalTuning" in keys

openpilot/sunnypilot/system/params_migration.py

@@ -103,6 +103,32 @@ def _migrate_model_bundle_slots(_params):
     cloudlog.exception(f"Error migrating model bundle slots: {e}")
 
 
+def _migrate_assisted_driving_milestones(_params):
+  try:
+    state = _params.get("AssistedDrivingMilestoneState", return_default=True)
+    if isinstance(state, dict) and state.get("version") == 1:
+      return
+
+    _params.put("AssistedDrivingMilestoneState", {
+      "version": 1,
+      "distancesMeters": {
+        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
+        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
+      },
+      "driveStartDistancesMeters": {
+        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
+        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
+      },
+      "nextEventId": 1,
+      "nextSummaryId": 1,
+      "unit": "metric" if _params.get_bool("IsMetric") else "imperial",
+      "activeDriveId": "",
+    }, block=True)
+    cloudlog.info("params_migration: migrated assisted-driving milestone state")
+  except Exception as e:
+    cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
+
+
 def run_migration(_params):
   # migrate OnroadScreenOffBrightness
   if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -142,3 +168,5 @@ def run_migration(_params):
 
   # seed the chestnut model slot from the pre-split single slot
   _migrate_model_bundle_slots(_params)
+
+  _migrate_assisted_driving_milestones(_params)

openpilot/sunnypilot/system/tests/test_params_migration.py

@@ -7,7 +7,44 @@ See the LICENSE.md file in the root directory for more details.
 
 from openpilot.common.params import Params
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
+from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
+
+
+class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
+  def test_preserves_prototype_distances_once(self):
+    class ParamsStub:
+      def __init__(self):
+        self.values = {
+          "MadsDrivenDistanceMeters": 123.0,
+          "FullAssistDrivenDistanceMeters": 456.0,
+          "OnroadScreenOffBrightness": 0,
+          "OnroadScreenOffTimer": 15,
+          "AssistedDrivingMilestoneState": {},
+          "IsMetric": False,
+        }
+
+      def get(self, key, return_default=False):
+        return self.values.get(key)
+
+      def put(self, key, value, block=False):
+        self.values[key] = value
+
+      def get_bool(self, key):
+        return bool(self.values.get(key, False))
+
+    params = ParamsStub()
+
+    run_migration(params)
+
+    state = params.get("AssistedDrivingMilestoneState")
+    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
+
+    params.put("MadsDrivenDistanceMeters", 12.0, block=True)
+    params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
+    run_migration(params)
+
+    state = params.get("AssistedDrivingMilestoneState")
+    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
 
 
 class TestModelBundleSlotMigration(OpenpilotTestCase):

tools/ford_native_path.py

@@ -0,0 +1,498 @@
+#!/usr/bin/env python3
+"""Offline evaluation of Ford's native four-field path polynomial.
+
+The experiment deliberately does not alter the live controller. It rebases the
+model path into the vehicle pose expected at actuation time, fits one cubic over
+the remaining short path, and converts the cubic into the LMC2 C0/C1/C2/C3
+signals. A first-order C2 response envelope is included to expose commands that
+would look good only if the PSCM curvature channel were instantaneous.
+"""
+
+import argparse
+from collections import defaultdict
+from dataclasses import dataclass
+import glob
+import math
+from pathlib import Path
+
+import numpy as np
+
+from openpilot.tools.lib.logreader import LogReader
+
+
+DBC_OFFSET = (-5.12, 5.11)
+DBC_ANGLE = (-0.5, 0.5235)
+DBC_CURVATURE = (-0.02, 0.02)
+DBC_CURVATURE_RATE = (-0.001024, 0.001023)
+MAX_LATERAL_ACCEL = 3.0 + 9.81 * 0.06
+MAX_LATERAL_JERK = 3.0 + 9.81 * 0.06
+@dataclass(frozen=True)
+class ModelPath:
+  x: np.ndarray
+  y: np.ndarray
+  heading: np.ndarray
+  distance: np.ndarray
+
+
+@dataclass(frozen=True)
+class Sample:
+  route: str
+  time: float
+  speed: float
+  curvature: float
+  steering_pressed: bool
+  path: ModelPath
+  sent_c0: float
+  sent_c1: float
+  sent_c2: float
+  sent_c3: float
+@dataclass(frozen=True)
+class NativePath:
+  c0: float
+  c1: float
+  c2: float
+  c3: float
+  fit_rmse: float
+  path_rms: float
+
+
+def _model_path(model) -> ModelPath | None:
+  try:
+    x = np.asarray(model.position.x, dtype=float)
+    y = np.asarray(model.position.y, dtype=float)
+    heading = np.unwrap(np.asarray(model.orientation.z, dtype=float))
+  except (AttributeError, TypeError, ValueError):
+    return None
+  if len(x) < 4 or len(x) != len(y) or len(x) != len(heading):
+    return None
+  if not np.isfinite(np.concatenate((x, y, heading))).all():
+    return None
+  distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
+  unique_distance, unique = np.unique(distance, return_index=True)
+  if len(unique_distance) < 4 or unique_distance[-1] <= 0.0:
+    return None
+  return ModelPath(x[unique], y[unique], heading[unique], unique_distance)
+
+
+def _arc_pose(distance: float, curvature: float) -> tuple[float, float, float]:
+  heading = curvature * distance
+  if abs(curvature) < 1e-9:
+    return distance, 0.0, 0.0
+  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
+
+
+def _relative_points(path: ModelPath, vehicle_pose: tuple[float, float, float], start: float,
+                     horizon: float, count: int = 25) -> tuple[np.ndarray, np.ndarray]:
+  sample_distance = np.linspace(start, min(start + horizon, path.distance[-1]), count)
+  desired_x = np.interp(sample_distance, path.distance, path.x)
+  desired_y = np.interp(sample_distance, path.distance, path.y)
+  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
+  dx = desired_x - vehicle_x
+  dy = desired_y - vehicle_y
+  cosine = math.cos(vehicle_heading)
+  sine = math.sin(vehicle_heading)
+  return cosine * dx + sine * dy, -sine * dx + cosine * dy
+
+
+def _fit_points(path: ModelPath, speed: float, current_curvature: float, delay: float,
+                horizon: float) -> tuple[np.ndarray, np.ndarray] | None:
+  advance = min(max(speed, 0.0) * delay, path.distance[-1])
+  available = min(horizon, path.distance[-1] - advance)
+  if available <= 0.25:
+    return None
+  x, y = _relative_points(path, _arc_pose(advance, current_curvature), advance, available)
+  forward = (x >= -0.25) & (x <= horizon)
+  x = x[forward]
+  y = y[forward]
+  if len(x) < 4 or np.ptp(x) <= 0.25:
+    return None
+  return x, y
+
+
+def _wire_coefficients(c0: float, c1: float, c2: float, c3: float) -> tuple[float, float, float, float]:
+  slope = math.tan(c1)
+  slope_norm = 1.0 + slope ** 2
+  a2 = 0.5 * c2 * slope_norm ** 1.5
+  a3 = (c3 + 12.0 * slope * a2 ** 2 / slope_norm ** 3) * slope_norm ** 2 / 6.0
+  return c0, slope, a2, a3
+
+
+def _wire_rmse(command: tuple[float, float, float, float], x: np.ndarray, y: np.ndarray) -> float:
+  a0, a1, a2, a3 = _wire_coefficients(*command)
+  reconstructed = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3
+  return float(np.sqrt(np.mean((reconstructed - y) ** 2)))
+
+
+def fit_native_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
+                    horizon: float) -> NativePath:
+  """Fit the delay-aligned path and return physical LMC2 fields.
+
+  C2 and C3 are curvature and curvature rate at the vehicle-frame origin, not
+  the raw quadratic and cubic polynomial coefficients.
+  """
+  points = _fit_points(path, speed, current_curvature, delay, horizon)
+  if points is None:
+    return NativePath(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+  x, y = points
+
+  # Scaling x before the least-squares solve keeps tight-turn fits well
+  # conditioned while preserving an ordinary cubic in vehicle coordinates.
+  scale = max(float(np.max(np.abs(x))), 1.0)
+  normalized_x = x / scale
+  design = np.column_stack((np.ones(len(x)), normalized_x, normalized_x ** 2, normalized_x ** 3))
+  scaled, *_ = np.linalg.lstsq(design, y, rcond=None)
+  a0, a1, a2, a3 = (float(scaled[index] / scale ** index) for index in range(4))
+  slope = a1
+  slope_norm = 1.0 + slope ** 2
+  curvature = 2.0 * a2 / slope_norm ** 1.5
+  curvature_rate = 6.0 * a3 / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
+  command = (float(np.clip(a0, *DBC_OFFSET)),
+             float(np.clip(math.atan(slope), *DBC_ANGLE)),
+             float(np.clip(curvature, *DBC_CURVATURE)),
+             float(np.clip(curvature_rate, *DBC_CURVATURE_RATE)))
+  return NativePath(
+    *command,
+    _wire_rmse(command, x, y),
+    float(np.sqrt(np.mean(y ** 2))),
+  )
+
+
+def fit_c2_aware_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
+                      horizon: float, target_c2: float, effective_c2: float,
+                      use_c3: bool = True) -> NativePath:
+  """Fit fast fields around the C2 curvature the PSCM is expected to realize."""
+  points = _fit_points(path, speed, current_curvature, delay, horizon)
+  if points is None:
+    return NativePath(0.0, 0.0, target_c2, 0.0, 0.0, 0.0)
+  x, y = points
+
+  slope = 0.0
+  a0 = a1 = a3 = 0.0
+  for _ in range(3):
+    a2 = 0.5 * effective_c2 * (1.0 + slope ** 2) ** 1.5
+    design = np.column_stack((np.ones(len(x)), x, x ** 3))
+    (a0, a1, a3), *_ = np.linalg.lstsq(design, y - a2 * x ** 2, rcond=None)
+    slope = float(a1)
+
+  slope_norm = 1.0 + slope ** 2
+  c3 = 6.0 * float(a3) / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
+  c3 = float(np.clip(c3, *DBC_CURVATURE_RATE)) if use_c3 else 0.0
+  # Once C2 and C3 are fixed to what the hardware can realize, refit C0/C1 so
+  # their fast feedback preserves as much of the same path as possible.
+  _, _, fixed_a2, fixed_a3 = _wire_coefficients(0.0, math.atan(slope), effective_c2, c3)
+  (a0, a1), *_ = np.linalg.lstsq(np.column_stack((np.ones(len(x)), x)),
+                                  y - fixed_a2 * x ** 2 - fixed_a3 * x ** 3, rcond=None)
+  c0 = float(np.clip(a0, *DBC_OFFSET))
+  c1 = float(np.clip(math.atan(float(a1)), *DBC_ANGLE))
+  effective_command = (c0, c1, effective_c2, c3)
+  return NativePath(c0, c1, target_c2, c3, _wire_rmse(effective_command, x, y),
+                    float(np.sqrt(np.mean(y ** 2))))
+
+
+def _route(path: str) -> str:
+  return Path(path).name.split("--", 1)[0]
+
+
+def load_samples(paths: list[str], stride: int = 2) -> list[Sample]:
+  grouped: dict[str, list[str]] = defaultdict(list)
+  for path in paths:
+    grouped[_route(path)].append(path)
+
+  samples = []
+  for route, route_paths in sorted(grouped.items()):
+    events = []
+    for path in sorted(route_paths):
+      events.extend(LogReader(path))
+    events.sort(key=lambda event: event.logMonoTime)
+    if not events:
+      continue
+    start_time = events[0].logMonoTime
+    model_path = None
+    curvature = 0.0
+    lat_active = path_valid = False
+    sent = (0.0, 0.0, 0.0, 0.0)
+    car_state_count = 0
+    for event in events:
+      which = event.which()
+      if which == "modelV2":
+        model_path = _model_path(event.modelV2)
+      elif which == "controlsState":
+        curvature = float(event.controlsState.curvature)
+      elif which == "carControl":
+        lat_active = bool(event.carControl.latActive)
+      elif which == "carControlSP":
+        command = event.carControlSP.fordLateralPath
+        path_valid = bool(command.valid)
+        sent = (float(command.pathOffset), float(command.pathAngle),
+                float(command.curvature), float(command.curvatureRate))
+      elif which == "carState" and lat_active and path_valid and model_path is not None:
+        car_state_count += 1
+        if car_state_count % stride:
+          continue
+        samples.append(Sample(
+          route, (event.logMonoTime - start_time) * 1e-9, float(event.carState.vEgo), curvature,
+          bool(event.carState.steeringPressed), model_path, *sent,
+        ))
+  return samples
+
+
+def _percentile(values: np.ndarray, percentile: float, mask: np.ndarray | None = None) -> float:
+  selected = values if mask is None else values[mask]
+  return float(np.percentile(np.abs(selected), percentile)) if len(selected) else math.nan
+
+
+def _route_rate(samples: list[Sample], values: np.ndarray) -> np.ndarray:
+  rate = np.zeros(len(values))
+  for index in range(1, len(values)):
+    dt = samples[index].time - samples[index - 1].time
+    if samples[index].route == samples[index - 1].route and 0.005 <= dt <= 0.2:
+      rate[index] = (values[index] - values[index - 1]) / dt
+  return rate
+
+
+def _c2_response(samples: list[Sample], target: np.ndarray, tau_load: float,
+                 tau_unload: float) -> np.ndarray:
+  effective = np.zeros(len(target))
+  previous_route = None
+  previous_time = 0.0
+  state = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      state = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    loading = target[index] * state >= 0.0 and abs(target[index]) > abs(state)
+    tau = tau_load if loading else tau_unload
+    state += (1.0 - math.exp(-dt / tau)) * (target[index] - state)
+    effective[index] = state
+    previous_route, previous_time = sample.route, sample.time
+  return effective
+
+
+def _limit_c2_command(samples: list[Sample], target: np.ndarray) -> np.ndarray:
+  """Mirror the CAN-FD Ford curvature acceleration/jerk limiter."""
+  limited = np.zeros(len(target))
+  previous_route = None
+  previous_time = 0.0
+  previous = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      previous = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    speed = max(sample.speed, 1.0)
+    value = float(np.clip(target[index], -MAX_LATERAL_ACCEL / speed ** 2,
+                          MAX_LATERAL_ACCEL / speed ** 2))
+    step = MAX_LATERAL_JERK / speed ** 2 * dt
+    value = float(np.clip(value, previous - step, previous + step))
+    limited[index] = float(np.clip(value, *DBC_CURVATURE))
+    previous = limited[index]
+    previous_route, previous_time = sample.route, sample.time
+  return limited
+
+
+def _limit_fast_fields(samples: list[Sample], c0_target: np.ndarray,
+                       c1_target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+  c0 = np.zeros(len(samples))
+  c1 = np.zeros(len(samples))
+  previous_route = None
+  previous_time = 0.0
+  previous_c0 = previous_c1 = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      previous_c0 = previous_c1 = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    c0[index] = np.clip(c0_target[index], previous_c0 - 4.0 * dt, previous_c0 + 4.0 * dt)
+    c1[index] = np.clip(c1_target[index], previous_c1 - 1.0 * dt, previous_c1 + 1.0 * dt)
+    previous_c0, previous_c1 = c0[index], c1[index]
+    previous_route, previous_time = sample.route, sample.time
+  return c0, c1
+
+
+def evaluate(samples: list[Sample], *, delay: float, horizon: float,
+             tau_load: float, tau_unload: float, horizon_time: float = 0.0,
+             assumed_tau_load: float | None = None, assumed_tau_unload: float | None = None,
+             use_c3: bool = True, c2_limit: float = DBC_CURVATURE[1]) -> dict[str, float]:
+  horizons = np.asarray([float(np.clip(sample.speed * horizon_time, 1.0, horizon))
+                         if horizon_time > 0.0 else horizon for sample in samples])
+  commands = [fit_native_path(sample.path, sample.speed, sample.curvature,
+                              delay=delay, horizon=sample_horizon)
+              for sample, sample_horizon in zip(samples, horizons, strict=True)]
+  c0 = np.asarray([command.c0 for command in commands])
+  c1 = np.asarray([command.c1 for command in commands])
+  raw_c2 = np.asarray([command.c2 for command in commands])
+  c2 = np.clip(raw_c2, -c2_limit, c2_limit)
+  c3 = np.asarray([command.c3 for command in commands])
+  fit_rmse = np.asarray([command.fit_rmse for command in commands])
+  path_rms = np.asarray([command.path_rms for command in commands])
+  transmitted_c2 = _limit_c2_command(samples, c2)
+  effective_c2 = _c2_response(samples, transmitted_c2, tau_load, tau_unload)
+  estimated_c2 = _c2_response(samples, transmitted_c2,
+                              tau_load if assumed_tau_load is None else assumed_tau_load,
+                              tau_unload if assumed_tau_unload is None else assumed_tau_unload)
+  compensated = [fit_c2_aware_path(sample.path, sample.speed, sample.curvature,
+                                   delay=delay, horizon=sample_horizon, target_c2=target,
+                                   effective_c2=estimated, use_c3=use_c3)
+                 for sample, sample_horizon, target, estimated in
+                 zip(samples, horizons, c2, estimated_c2, strict=True)]
+  compensated_c0 = np.asarray([command.c0 for command in compensated])
+  compensated_c1 = np.asarray([command.c1 for command in compensated])
+  compensated_c3 = np.asarray([command.c3 for command in compensated])
+  limited_c0, limited_c1 = _limit_fast_fields(samples, compensated_c0, compensated_c1)
+  estimated_compensated_rmse = np.asarray([command.fit_rmse for command in compensated])
+  compensated_rmse = []
+  for sample, sample_horizon, command, effective in zip(samples, horizons, compensated, effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    compensated_rmse.append(0.0 if points is None else _wire_rmse(
+      (command.c0, command.c1, effective, command.c3), *points))
+  compensated_rmse = np.asarray(compensated_rmse)
+  limited_compensated_rmse = []
+  for sample, sample_horizon, c0_value, c1_value, c3_value, effective in \
+      zip(samples, horizons, limited_c0, limited_c1, compensated_c3, effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    limited_compensated_rmse.append(0.0 if points is None else _wire_rmse(
+      (c0_value, c1_value, effective, c3_value), *points))
+  limited_compensated_rmse = np.asarray(limited_compensated_rmse)
+  missing_c2 = transmitted_c2 - effective_c2
+  # Compare channels by their lateral contribution at the fit horizon. This
+  # includes C3: treating it as zero would incorrectly blame C0/C1 for a
+  # curvature transition the native polynomial assigns to curvature rate.
+  fast = (2.0 * compensated_c0 / horizons ** 2 +
+          2.0 * np.tan(compensated_c1) / horizons +
+          compensated_c3 * horizons / 3.0)
+  lagging = np.abs(missing_c2) > 0.0005
+  unloading = lagging & (np.abs(c2) < 0.75 * np.abs(effective_c2))
+  pressed = np.asarray([sample.steering_pressed for sample in samples])
+  speed = np.asarray([sample.speed for sample in samples])
+  sent_c2 = np.asarray([sample.sent_c2 for sample in samples])
+  sent_transmitted_c2 = _limit_c2_command(samples, sent_c2)
+  sent_effective_c2 = _c2_response(samples, sent_transmitted_c2, tau_load, tau_unload)
+  sent_lpf_rmse = []
+  for sample, sample_horizon, effective in zip(samples, horizons, sent_effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    sent_lpf_rmse.append(0.0 if points is None else _wire_rmse(
+      (sample.sent_c0, sample.sent_c1, effective, sample.sent_c3), *points))
+  sent_lpf_rmse = np.asarray(sent_lpf_rmse)
+  raw_c2_rate = _route_rate(samples, c2)
+  c2_rate = _route_rate(samples, transmitted_c2)
+  sent_c2_rate = _route_rate(samples, sent_c2)
+  compensated_c0_rate = _route_rate(samples, compensated_c0)
+  compensated_c1_rate = _route_rate(samples, compensated_c1)
+  compensated_c3_rate = _route_rate(samples, compensated_c3)
+  normalized_fit = np.divide(fit_rmse, path_rms, out=np.zeros_like(fit_rmse), where=path_rms > 1e-4)
+  return {
+    "samples": float(len(samples)),
+    "delay": delay,
+    "horizon": horizon,
+    "horizon_time": horizon_time,
+    "assumed_tau_load": tau_load if assumed_tau_load is None else assumed_tau_load,
+    "assumed_tau_unload": tau_unload if assumed_tau_unload is None else assumed_tau_unload,
+    "use_c3": float(use_c3),
+    "c2_limit": c2_limit,
+    "actual_horizon_p50": _percentile(horizons, 50),
+    "actual_horizon_p95": _percentile(horizons, 95),
+    "fit_rmse_p50": _percentile(fit_rmse, 50),
+    "fit_rmse_p95": _percentile(fit_rmse, 95),
+    "normalized_fit_p95": _percentile(normalized_fit, 95),
+    "c2_aware_rmse_p50": _percentile(compensated_rmse, 50),
+    "c2_aware_rmse_p95": _percentile(compensated_rmse, 95),
+    "c2_aware_estimated_rmse_p95": _percentile(estimated_compensated_rmse, 95),
+    "c2_aware_limited_rmse_p95": _percentile(limited_compensated_rmse, 95),
+    "sent_lpf_rmse_p50": _percentile(sent_lpf_rmse, 50),
+    "sent_lpf_rmse_p95": _percentile(sent_lpf_rmse, 95),
+    "c0_p95": _percentile(c0, 95),
+    "c1_p95": _percentile(c1, 95),
+    "c2_p95": _percentile(c2, 95),
+    "c3_p95": _percentile(c3, 95),
+    "c0_clip_rate": float(np.mean((c0 <= DBC_OFFSET[0]) | (c0 >= DBC_OFFSET[1]))),
+    "c1_clip_rate": float(np.mean((c1 <= DBC_ANGLE[0]) | (c1 >= DBC_ANGLE[1]))),
+    "c2_clip_rate": float(np.mean((c2 <= DBC_CURVATURE[0]) | (c2 >= DBC_CURVATURE[1]))),
+    "c3_clip_rate": float(np.mean((c3 <= DBC_CURVATURE_RATE[0]) | (c3 >= DBC_CURVATURE_RATE[1]))),
+    "c2_aware_c0_p95": _percentile(compensated_c0, 95),
+    "c2_aware_c1_p95": _percentile(compensated_c1, 95),
+    "c2_aware_c3_p95": _percentile(compensated_c3, 95),
+    "c2_aware_c0_rate_p95": _percentile(compensated_c0_rate, 95),
+    "c2_aware_c1_rate_p95": _percentile(compensated_c1_rate, 95),
+    "c2_aware_c0_rate_limit_rate": float(np.mean(np.abs(compensated_c0_rate) > 4.0)),
+    "c2_aware_c1_rate_limit_rate": float(np.mean(np.abs(compensated_c1_rate) > 1.0)),
+    "c2_aware_c3_rate_p95": _percentile(compensated_c3_rate, 95),
+    "raw_c2_rate_p95": _percentile(raw_c2_rate, 95),
+    "c2_rate_p95": _percentile(c2_rate, 95),
+    "sent_c2_rate_p95": _percentile(sent_c2_rate, 95),
+    "c2_lag_p95": _percentile(missing_c2, 95),
+    "lag_samples": float(np.count_nonzero(lagging)),
+    "lag_fast_support_rate": float(np.mean(fast[lagging] * missing_c2[lagging] > 0.0)) if np.any(lagging) else math.nan,
+    "lag_fast_coverage_p50": _percentile(np.divide(fast, missing_c2, out=np.zeros_like(fast),
+                                                    where=np.abs(missing_c2) > 1e-6), 50, lagging),
+    "unload_samples": float(np.count_nonzero(unloading)),
+    "unload_fast_counter_rate": float(np.mean(fast[unloading] * effective_c2[unloading] < 0.0)) if np.any(unloading) else math.nan,
+    "unload_residual_c2_p95": _percentile(effective_c2 - c2, 95, unloading),
+    "pressed_c0_p95": _percentile(c0, 95, pressed),
+    "pressed_c1_p95": _percentile(c1, 95, pressed),
+    "low_speed_fit_p95": _percentile(fit_rmse, 95, speed < 5.0),
+    "road_speed_fit_p95": _percentile(fit_rmse, 95, speed >= 15.0),
+  }
+
+
+def _expand(patterns: list[str]) -> list[str]:
+  return sorted({path for pattern in patterns for path in glob.glob(pattern)})
+
+
+def _self_test() -> None:
+  distance = np.linspace(0.0, 20.0, 81)
+  coefficients = (0.2, 0.03, 0.004, -0.00005)
+  y = sum(coefficient * distance ** power for power, coefficient in enumerate(coefficients))
+  slope = coefficients[1] + 2.0 * coefficients[2] * distance + 3.0 * coefficients[3] * distance ** 2
+  heading = np.arctan(slope)
+  path = ModelPath(distance, y, heading, np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(distance), np.diff(y))))))
+  command = fit_native_path(path, 0.0, 0.0, delay=0.1, horizon=7.0)
+  assert abs(command.c0 - coefficients[0]) < 2e-3
+  assert abs(command.c1 - math.atan(coefficients[1])) < 2e-3
+  expected_c2 = 2.0 * coefficients[2] / (1.0 + coefficients[1] ** 2) ** 1.5
+  assert abs(command.c2 - expected_c2) < 2e-4
+  assert command.fit_rmse < 1e-4
+
+
+def main() -> int:
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument("--logs", action="append", help="rlog glob", default=[])
+  parser.add_argument("--delay", type=float, default=0.1)
+  parser.add_argument("--horizon", type=float, action="append")
+  parser.add_argument("--time-horizon", type=float, default=0.0,
+                      help="if nonzero, use clamp(speed * seconds, 1 m, --horizon)")
+  parser.add_argument("--tau-load", type=float, default=0.75)
+  parser.add_argument("--tau-unload", type=float, default=1.3)
+  parser.add_argument("--assumed-tau-load", type=float)
+  parser.add_argument("--assumed-tau-unload", type=float)
+  parser.add_argument("--zero-c3", action="store_true")
+  parser.add_argument("--c2-limit", type=float, action="append",
+                      help="C2 cap to test; defaults to gentle 0.006 and full 0.02")
+  parser.add_argument("--self-test", action="store_true")
+  args = parser.parse_args()
+  if args.self_test:
+    _self_test()
+  paths = _expand(args.logs)
+  if not paths:
+    if args.self_test:
+      return 0
+    parser.error("at least one usable --logs glob is required")
+  samples = load_samples(paths)
+  if not samples:
+    parser.error("logs contain no active Ford path samples")
+  print(f"loaded_logs={len(paths)} samples={len(samples)} tau_load={args.tau_load} tau_unload={args.tau_unload}")
+  for horizon in args.horizon or [3.5, 5.0, 7.0, 10.0]:
+    for c2_limit in args.c2_limit or [0.006, DBC_CURVATURE[1]]:
+      result = evaluate(samples, delay=args.delay, horizon=horizon,
+                        tau_load=args.tau_load, tau_unload=args.tau_unload,
+                        horizon_time=args.time_horizon,
+                        assumed_tau_load=args.assumed_tau_load,
+                        assumed_tau_unload=args.assumed_tau_unload,
+                        use_c3=not args.zero_c3,
+                        c2_limit=c2_limit)
+      print(" ".join(f"{key}={value:.8g}" for key, value in result.items()))
+  return 0
+
+
+if __name__ == "__main__":
+  raise SystemExit(main())

tools/ford_pscm_lab/__init__.py

@@ -0,0 +1 @@
+"""Offline Ford candidate verification. No hardware or CAN transmission."""

tools/ford_pscm_lab/damping_replay.py

@@ -0,0 +1,169 @@
+"""Compare pinned selected-action controllers or current code on complete rlogs.
+
+Original controls publication times proxy computation time. Consumed model
+timestamps are exact; carState is causal and carControl is matched within 5 ms.
+This compares commands on a fixed recording, never counterfactual vehicle motion.
+"""
+import argparse
+from collections import Counter
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+import zstandard
+
+from openpilot.cereal import log
+from openpilot.selfdrive.controls.lib import ford_model_action
+from tools.ford_pscm_lab.model_action_replay import V1_REVISION, V2_REVISION, V3_REVISION, WireCheck, field_checks, load_controller, sample, verify_dependency
+
+
+DEPLOYMENT_OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9'
+
+
+def extract(directory):
+  columns = {'cs': 't valid can_valid speed yaw torque pressed', 'controls': 't valid desired model_ns',
+             'cc': 't valid active', 'params': 't valid', 'path': 't valid active c0 c1', 'model': 't valid ns'}
+  rows = {name: [] for name in columns}
+  models, sources = [], {}
+  t0 = None
+  files = sorted(directory.glob('*--rlog.zst'), key=lambda p: int(p.name.split('--')[-2]))
+  if not files:
+    raise ValueError('No complete rlogs found')
+  for file in files:
+    compressed = file.read_bytes()
+    sources[file.name] = hashlib.sha256(compressed).hexdigest()
+    data = zstandard.ZstdDecompressor().stream_reader(compressed).read()
+    for event in log.Event.read_multiple_bytes(data):
+      kind, t, valid = event.which(), event.logMonoTime*1e-9, event.valid
+      if t0 is None:
+        t0 = t
+      if kind == 'carState':
+        cs = event.carState
+        rows['cs'].append((t, valid, cs.canValid, cs.vEgo, -cs.yawRate, cs.steeringTorque, cs.steeringPressed))
+      elif kind == 'controlsState':
+        cs = event.controlsState
+        rows['controls'].append((t, valid, cs.desiredCurvature, cs.lateralPlanMonoTime))
+      elif kind == 'carControl':
+        rows['cc'].append((t, valid, event.carControl.latActive))
+      elif kind == 'vehicleParameters':
+        rows['params'].append((t, valid))
+      elif kind == 'carControlSP':
+        path = event.carControlSP.fordLateralPath
+        rows['path'].append((t, valid, path.valid, path.pathOffset, path.pathAngle))
+      elif kind == 'modelV2':
+        model = event.modelV2
+        models.append(SimpleNamespace(position=SimpleNamespace(x=list(model.position.x), y=list(model.position.y)),
+                                      orientation=SimpleNamespace(z=list(model.orientation.z))))
+        rows['model'].append((t, valid, event.logMonoTime))
+      elif kind == 'lateralManeuverPlan':
+        raise ValueError('This replay requires routes without a separate maneuver reference')
+  streams = {}
+  for name, fields in columns.items():
+    values = np.array(rows[name])
+    if not len(values) or np.any(np.diff(values[:, 0]) < 0.):
+      raise ValueError(f'Missing or backward {name} stream')
+    streams[name] = dict(zip(fields.split(), values.T, strict=True))
+  return streams, models, sources, t0
+
+
+def run(directory, output, baseline_version='v1', candidate_version='v2', windows=()):
+  directory, output = directory.resolve(), output.resolve()
+  if output == directory or directory in output.parents:
+    raise ValueError('Output must be outside the source route directory')
+  verify_dependency(DEPLOYMENT_OPENDBC)
+  revisions = {'v1': V1_REVISION, 'v2': V2_REVISION, 'v3': V3_REVISION}
+  baseline_source = load_controller(revisions[baseline_version])
+  candidate_source = ford_model_action if candidate_version == 'current' else load_controller(revisions[candidate_version])
+  streams, models, sources, t0 = extract(directory)
+  controls, model = streams['controls'], streams['model']
+  t = controls['t']
+  cs, params = (sample(streams[name], t) for name in ('cs', 'params'))
+  cc, recorded = (sample(streams[name], t, nearest=True) for name in ('cc', 'path'))
+  mi = np.clip(np.searchsorted(model['ns'], controls['model_ns']), 0, len(models)-1)
+  exact = model['ns'][mi] == controls['model_ns']
+  services = ((controls['valid'] == 1) & (cc['valid'] == 1) & (abs(cc['t']-t) < .005) &
+              (cs['valid'] == 1) & (cs['can_valid'] == 1) & (params['valid'] == 1) &
+              (t-params['t'] >= 0.) & (t-params['t'] <= .15) & exact & (model['valid'][mi] == 1))
+  baseline, candidate, wire = baseline_source.FordModelActionController(), candidate_source.FordModelActionController(), WireCheck()
+  before, after = np.zeros((len(t), 4)), np.zeros((len(t), 4))
+  eligible = np.zeros(len(t), bool)
+  reasons = Counter()
+  for i, now in enumerate(t):
+    kwargs = {'speed': cs['speed'][i], 'now': now, 'measurement_time': cs['t'][i], 'model_time': model['t'][mi[i]],
+              'reference_time': model['t'][mi[i]], 'active': bool(cc['active'][i]), 'valid': bool(services[i])}
+    geometry = models[mi[i]] if exact[i] else None
+    a = baseline.update(geometry, controls['desired'][i], yaw_rate=cs['yaw'][i], **kwargs)
+    b = candidate.update(geometry, controls['desired'][i], yaw_rate=cs['yaw'][i], **kwargs)
+    assert a.valid == b.valid and a.path_angle == b.path_angle
+    before[i] = a.path_offset, a.path_angle, a.curvature, a.curvature_rate
+    after[i] = b.path_offset, b.path_angle, b.curvature, b.curvature_rate
+    eligible[i] = b.valid
+    reasons[candidate.diagnostics['status']] += 1
+    wire.check(a)
+    wire.check(b)
+  field_checks(before, eligible, t)
+  field_checks(after, eligible, t)
+  clean = eligible & (cs['pressed'] == 0) & (abs(cs['torque']) <= 1.)
+  # Erode driver eligibility by one second in each direction on original time.
+  bad = np.r_[0, np.cumsum(~clean)]
+  left, right = np.searchsorted(t, t-1.), np.searchsorted(t, t+1., side='right')
+  clean &= (bad[right] == bad[left]) & (t >= t[0]+1.) & (t <= t[-1]-1.)
+  relative = t-t0
+  masks = {'eligible': eligible, 'driver_clean': clean,
+           'driver_clean_low_request_above_8mps': clean & (cs['speed'] >= 8.) & (abs(controls['desired'])*cs['speed']**2 < .15),
+           'turn': clean & (abs(controls['desired'])*cs['speed']**2 >= .5)}
+  for label, start, end in windows:
+    start, end = float(start), float(end)
+    if not np.isfinite([start, end]).all() or start >= end or label in masks:
+      raise ValueError('Focus windows need unique labels and finite increasing bounds')
+    masks[label] = eligible & (relative >= start) & (relative < end)
+  weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
+  difference = abs(before[:, 0]-after[:, 0])
+  cohorts = {}
+  for name, mask in masks.items():
+    if mask.any():
+      cohorts[name] = {'cycles': int(mask.sum()), 'seconds': float(weight[mask].sum()),
+                       'changed_c0_cycles': int((difference[mask] > 1e-9).sum()),
+                       'mean_absolute_c0_change_m': float(np.average(difference[mask], weights=weight[mask])),
+                       'max_absolute_c0_change_m': float(difference[mask].max()),
+                       'increased_absolute_c0_cycles': int((abs(after[mask, 0])-abs(before[mask, 0]) > 1e-9).sum()),
+                       'decreased_absolute_c0_cycles': int((abs(before[mask, 0])-abs(after[mask, 0]) > 1e-9).sum()),
+                       'driver_input_percent': float(100*np.average((cs['pressed'][mask] == 1) | (abs(cs['torque'][mask]) > 1.), weights=weight[mask])),
+                       'baseline_peak_absolute_c0_m': float(abs(before[mask, 0]).max()),
+                       'candidate_peak_absolute_c0_m': float(abs(after[mask, 0]).max())}
+  paired = eligible & (recorded['valid'] == 1) & (recorded['active'] == 1) & (abs(recorded['t']-t) < .005)
+  actual = np.column_stack((recorded['c0'], recorded['c1']))
+  error = abs(before[:, :2]-actual)
+  report = {'scope': 'Fixed-input command replay only; no physical improvement or stability claim.', 'calibration_approved': False,
+            'cycles': len(t), 'eligible_cycles': int(eligible.sum()), 'status_counts': dict(reasons),
+            'c1_exactly_unchanged': True, 'same_validity': True, 'field_slew_zero_c2_c3_pass': True,
+            'float32_can_round_trips': wire.count, 'cohorts': cohorts,
+            'baseline_commands_vs_recorded_publications': {'paired_cycles': int(paired.sum()),
+              'within_one_quantum_cycles': int(np.all(error[paired] <= [.010001, .0005001], axis=1).sum()),
+              'maximum_absolute_error_c0_c1': np.max(error[paired], axis=0).tolist()},
+            'timing': 'Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.',
+            'baseline_version': baseline_version, 'baseline_revision': revisions[baseline_version],
+            'candidate_version': candidate_version, 'candidate_revision': revisions.get(candidate_version, 'working_tree'),
+            'focus_windows': windows, 'baseline_source_sha256': baseline_source.source_sha256,
+            'candidate_source_sha256': (hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()
+                                        if candidate_version == 'current' else candidate_source.source_sha256),
+            'source_rlog_sha256': sources, 'opendbc_head': DEPLOYMENT_OPENDBC,
+            'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in (Path(__file__), Path(ford_model_action.__file__))}}
+  output.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(output/'commands.npz', t=relative, before=before, after=after, eligible=eligible,
+                      speed=cs['speed'], yaw=cs['yaw'], desired=controls['desired'], torque=cs['torque'])
+  (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({k: v for k, v in report.items() if k not in ('source_rlog_sha256', 'source_sha256')}, indent=2))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('rlog_directory', type=Path)
+  parser.add_argument('--output', type=Path, required=True)
+  parser.add_argument('--baseline', choices=['v1', 'v2', 'v3'], default='v1')
+  parser.add_argument('--candidate', choices=['v2', 'v3', 'current'], default='v2')
+  parser.add_argument('--window', action='append', nargs=3, metavar=('LABEL', 'START_SECONDS', 'END_SECONDS'), default=[])
+  args = parser.parse_args()
+  run(args.rlog_directory, args.output, args.baseline, args.candidate, args.window)

tools/ford_pscm_lab/model_action_replay.py

@@ -0,0 +1,236 @@
+"""Replay the selected core and its adapter on route90/95 original-time extracts.
+
+The historical pass uses pinned v1 source and the archived eligibility mask to check
+command compatibility. The separate current adapter pass reconstructs input eligibility
+from service records, never from candidate/baseline output validity. Neither
+pass scores counterfactual motion. Source extracts and archived reports are
+read-only; --output selects a separate destination.
+"""
+import argparse
+from collections import Counter
+from functools import lru_cache
+import hashlib
+import json
+from pathlib import Path
+import subprocess
+from types import ModuleType, SimpleNamespace
+
+import numpy as np
+import opendbc
+from opendbc.can import CANPacker, CANParser
+from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
+from openpilot.cereal import custom
+from openpilot.selfdrive.controls.lib import ford_model_action
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import _model_path
+
+
+PINNED_OPENDBC = '72a775d35e54c21ff5c5798acef22016eedcc0a7'
+V1_REVISION = '5fc16abc7662020706e29f57d31a6d5e2bc1293a'
+V2_REVISION = '744a97d9bc08d8743b250eceff7c88585b5480de'
+V3_REVISION = '01f8d51c82b3e863f1012d383b5994813ef01b81'
+
+
+@lru_cache(maxsize=3)
+def load_controller(commit):
+  """Load exact archived Python source for offline comparisons, never production."""
+  if len(commit) != 40 or any(c not in '0123456789abcdef' for c in commit):
+    raise ValueError('A full immutable commit hash is required')
+  filename = 'openpilot/selfdrive/controls/lib/ford_model_action.py'
+  root = Path(__file__).resolve().parents[2]
+  source = subprocess.check_output(['git', '-C', str(root), 'show', f'{commit}:{filename}'])
+  module = ModuleType(f'ford_model_action_{commit}')
+  exec(compile(source, f'{commit}:{filename}', 'exec'), module.__dict__)
+  module.source_sha256 = hashlib.sha256(source).hexdigest()
+  return module
+
+
+def revision(directory):
+  return subprocess.check_output(['git', '-C', str(directory), 'rev-parse', 'HEAD'], text=True).strip()
+
+
+def verify_dependency(expected=PINNED_OPENDBC):
+  directory = Path(opendbc.__file__).resolve().parent.parent
+  actual = revision(directory)
+  if actual != expected:
+    raise ValueError(f'Expected opendbc {expected}; imported {directory} at {actual}')
+  return directory
+
+
+def table(raw, name):
+  return dict(zip(raw[name+'_names'], raw[name].T, strict=True))
+
+
+def sample(stream, query, *, nearest=False):
+  if len(stream['t']) == 0 or np.any(np.diff(stream['t']) < 0):
+    raise ValueError('Replay requires nonempty streams in original timestamp order')
+  if nearest:
+    right = np.clip(np.searchsorted(stream['t'], query), 0, len(stream['t'])-1)
+    left = np.maximum(right-1, 0)
+    index = np.where(abs(stream['t'][right]-query) < abs(stream['t'][left]-query), right, left)
+  else:
+    index = np.clip(np.searchsorted(stream['t'], query, side='right')-1, 0, len(stream['t'])-1)
+  return {key: values[index] for key, values in stream.items()}
+
+
+def field_checks(command, valid, t):
+  assert np.isfinite(command).all()
+  assert (abs(command[:, :2]) <= [5.1100001, .5000001]).all()
+  assert (command[:, 2:] == 0.).all()
+  assert (command[~valid] == 0.).all()
+  dt = np.r_[.01, np.diff(t)]
+  consecutive = valid[1:] & valid[:-1]
+  assert (abs(np.diff(command[:, :2], axis=0))[consecutive] <=
+          (dt[1:, None]*[4., .5]+[.0100001, .0005001])[consecutive]).all()
+  return True
+
+
+class WireCheck:
+  """Real Float32 publication and in-memory CAN round trips on every cycle."""
+  def __init__(self):
+    self.packer = CANPacker('ford_lincoln_base_pt')
+    self.parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+    self.bus = CanBus(fingerprint={0: {}})
+    self.count = 0
+
+  def check(self, path):
+    msg = custom.CarControlSP.new_message()
+    msg.fordLateralPath.valid = path.valid
+    msg.fordLateralPath.pathOffset = path.path_offset
+    msg.fordLateralPath.pathAngle = path.path_angle
+    msg.fordLateralPath.curvature = path.curvature
+    msg.fordLateralPath.curvatureRate = path.curvature_rate
+    p = msg.fordLateralPath
+    counter = self.count % 16
+    packet = create_lat_ctl2_msg(self.packer, self.bus, 2 if path.valid else 0,
+                                -p.pathOffset, -p.pathAngle, -p.curvature, -p.curvatureRate, counter)
+    self.count += 1
+    # Synthetic parser clock only; input times and gaps are never resampled.
+    self.parser.update([self.count*10_000_000, [packet]])
+    decoded = self.parser.vl['LateralMotionControl2']
+    assert abs(decoded['LatCtlPathOffst_L_Actl']+path.path_offset) < 1e-9
+    assert abs(decoded['LatCtlPath_An_Actl']+path.path_angle) < 1e-9
+    assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert decoded['LatCtl_D2_Rq'] == (2 if path.valid else 0)
+    assert decoded['LatCtlPath_No_Cnt'] == counter
+
+
+def run(directory, output):
+  directory, output = directory.resolve(), output.resolve()
+  if output == directory or directory in output.parents:
+    raise ValueError('Output must be outside the source route directory')
+  dependency = verify_dependency()
+  with np.load(directory/'route.npz', allow_pickle=False) as raw:
+    streams = {name: table(raw, name) for name in ('controls', 'cs', 'cc', 'model', 'params', 'path')}
+    if len(raw['maneuver']):
+      raise ValueError('This extract cannot identify the selected maneuver service per cycle; use the integration tests for that source')
+    models = [SimpleNamespace(position=SimpleNamespace(x=p[1], y=p[2]), orientation=SimpleNamespace(z=p[3])) for p in raw['model_paths']]
+  with np.load(directory/'encoder_comparison.npz', allow_pickle=False) as archive:
+    baseline = {key: archive[key] for key in ('t', 'valid', 'action_heading')}
+  with np.load(directory/'pose_candidate/pose_replay.npz', allow_pickle=False) as pose:
+    np.testing.assert_array_equal(pose['t'], baseline['t'])
+    clean = pose['clean']
+  controls = streams['controls']
+  t = controls['t']
+  np.testing.assert_array_equal(t, baseline['t'])
+  cs, params = (sample(streams[name], t) for name in ('cs', 'params'))
+  cc = sample(streams['cc'], t, nearest=True)  # same-cycle publication match, never a motion sample
+  mi = np.clip(np.searchsorted(streams['model']['ns'], controls['model_ns']), 0, len(models)-1)
+  model = {key: values[mi] for key, values in streams['model'].items()}
+  exact = model['ns'] == controls['model_ns']
+  services_valid = ((controls['valid'] == 1) & (cc['valid'] == 1) & (abs(cc['t']-t) < .005) &
+                    (cs['valid'] == 1) & (cs['can_valid'] == 1) & (params['valid'] == 1) &
+                    (t-params['t'] >= 0.) & (t-params['t'] <= .15) & exact & (model['valid'] == 1))
+  dt = np.r_[.01, np.diff(t)]
+  archived = load_controller(V1_REVISION)
+  core, entry_clock_core, adapter, wire = archived.ModelActionController(), ModelActionController(), FordModelActionController(), WireCheck()
+  commands = np.zeros((len(t), 4))
+  adapted = np.zeros_like(commands)
+  valid = np.zeros(len(t), bool)
+  adapter_valid = np.zeros_like(valid)
+  reasons = Counter()
+  for i, now in enumerate(t):
+    selected_model = models[mi[i]] if exact[i] else None
+    old_gate = core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=dt[i], active=bool(baseline['valid'][i]))
+    commands[i] = old_gate.path_offset, old_gate.path_angle, old_gate.curvature, old_gate.curvature_rate
+    valid[i] = old_gate.valid
+    wire.check(old_gate)
+    new_gate = adapter.update(selected_model, controls['desired'][i], speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now,
+                              measurement_time=cs['t'][i], model_time=model['t'][i], reference_time=model['t'][i],
+                              active=bool(cc['active'][i]), valid=bool(services_valid[i]))
+    adapted[i] = new_gate.path_offset, new_gate.path_angle, new_gate.curvature, new_gate.curvature_rate
+    adapter_valid[i] = new_gate.valid
+    reasons[adapter.diagnostics['status']] += 1
+    wire.check(new_gate)
+    # Current core receives actual yaw and a fresh 10 ms engagement tick.
+    entry_dt = dt[i] if i > 0 and baseline['valid'][i-1] else .01
+    expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=entry_dt,
+                                               yaw_rate=cs['yaw'][i], active=bool(baseline['valid'][i]))
+    assert new_gate == expected_adapter, f'Unexplained adapter difference at cycle {i}'
+  np.testing.assert_array_equal(valid, baseline['valid'])
+  np.testing.assert_array_equal(commands[:, :2], baseline['action_heading'])
+  field_checks(commands, valid, t)
+  field_checks(adapted, adapter_valid, t)
+
+  # Recompute the archived cohorts from actual commands, retaining the original
+  # interval-clean driver mask and time weights. No recorded yaw performance score.
+  v = cs['speed']
+  demand = abs(controls['desired'])*v**2
+  masks = {'small_request': clean & (demand < .15), 'turn': clean & (demand >= .5)}
+  for low, high in ((2, 8), (8, 15), (15, 55)):
+    for name in ('small', 'turn'):
+      masks[f'{name}_speed_{low}_{high}'] = masks['small_request' if name == 'small' else name] & (v >= low) & (v < high)
+  paths = [_model_path(m) for m in models]
+  y10 = np.array([np.interp(10., p[0], p[2]) if p is not None else np.nan for p in paths])
+  h10 = np.array([np.interp(10., p[0], p[3]) if p is not None else np.nan for p in paths])
+  coverage = np.array([p[0][-1] if p is not None else 0. for p in paths])[mi]
+  masks['pose_quiet'] = masks['small_request'] & (v >= 8) & (abs(y10[mi]) <= .1) & (abs(h10[mi]) <= np.radians(.5))
+  recorded = sample(streams['path'], t, nearest=True)
+  recorded = np.column_stack((recorded['c0'], recorded['c1']))
+  weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
+  cohorts = {}
+  previous_report = json.loads((directory/'encoder_comparison.json').read_text())
+  for name, mask in masks.items():
+    def rms(values, selected=mask):
+      return np.sqrt(np.average(values[selected, :2]**2, weights=weight[selected], axis=0)).tolist() if selected.any() else None
+    cohorts[name] = {'seconds': float(weight[mask].sum()), 'core_c0_c1_rms': rms(commands), 'recorded_v8_c0_c1_rms': rms(recorded),
+                     'adapter_eligible_seconds': float(weight[mask & adapter_valid].sum()),
+                     'adapter_c0_c1_rms': rms(adapted, mask & adapter_valid)}
+    expected = previous_report['cohorts'][name]
+    np.testing.assert_allclose(cohorts[name]['seconds'], expected['seconds'], rtol=0., atol=1e-8)
+    np.testing.assert_allclose(cohorts[name]['core_c0_c1_rms'], expected['candidates']['action_heading']['c0_c1_rms'], rtol=0., atol=1e-12)
+    np.testing.assert_allclose(cohorts[name]['recorded_v8_c0_c1_rms'], expected['v8_c0_c1_rms'], rtol=0., atol=1e-12)
+  root = Path(__file__).resolve().parents[2]
+  sources = [Path(__file__), Path(ford_model_action.__file__),
+             root/'openpilot/selfdrive/controls/lib/ford_path.py', directory/'route.npz', directory/'metadata.json',
+             directory/'encoder_comparison.npz', directory/'encoder_comparison.json', directory/'pose_candidate/pose_replay.npz']
+  report = {'scope': 'Command construction and adapter reconstruction only; no counterfactual closed-loop score.',
+            'calibration_approved': False, 'executes_live_selector': False, 'cycles': len(t),
+            'core_active_cycles': int(valid.sum()), 'core_exact_archived_match': True, 'cohorts_reproduced': True,
+            'adapter_active_cycles': int(adapter_valid.sum()), 'adapter_status_counts': dict(reasons),
+            'adapter_matches_current_core_with_yaw_and_fresh_engagement_dt': True,
+            'core_active_path_shorter_than_7m_cycles': int(np.sum(valid & (coverage < 7.))),
+            'adapter_validity_differs_from_archive_cycles': int(np.sum(adapter_valid != valid)),
+            'adapter_command_differs_from_archive_cycles': int(np.any(abs(adapted-commands) > 1e-9, axis=1).sum()),
+            'adapter_max_absolute_command_difference_c0_c1': np.max(abs(adapted[:, :2]-commands[:, :2]), axis=0).tolist(),
+            'field_slew_zero_c2_c3_pass': True, 'float32_can_round_trips': wire.count,
+            'timing': 'Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.',
+            'eligibility': 'Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.',
+            'reference': 'Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.',
+            'host_yaw': 'Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.',
+            'archived_core_revision': V1_REVISION, 'archived_core_source_sha256': archived.source_sha256,
+            'cohorts': cohorts, 'workspace_head': revision(root), 'opendbc_import_head': revision(dependency),
+            'opendbc_import_path': str(dependency),
+            'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}}
+  output.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(output/'commands.npz', t=t, core=commands, core_valid=valid, adapter=adapted, adapter_valid=adapter_valid)
+  (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({k: v for k, v in report.items() if k not in ('cohorts', 'source_sha256')}, indent=2))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('route_directory', type=Path)
+  parser.add_argument('--output', type=Path, required=True)
+  args = parser.parse_args()
+  run(args.route_directory, args.output)

tools/ford_pscm_lab/stress_model_action.py

@@ -0,0 +1,145 @@
+"""Deterministic numerical stress and exhaustive field-boundary CAN checks.
+
+Analytic straight/rotated paths and an independent rigid-motion transform
+check geometric prediction. The reference slew uses scalar arithmetic. Packing is checked against direct
+Float32/CAN packing of the continuous state, independently of host _packed.
+No synthetic plant is fitted or used to claim vehicle tracking performance.
+"""
+import argparse
+import hashlib
+import json
+import math
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.controls.lib import ford_model_action
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
+from opendbc.car.ford.fordcan import create_lat_ctl2_msg
+from tools.ford_pscm_lab.model_action_replay import PINNED_OPENDBC, WireCheck, verify_dependency, revision
+
+
+def line(offset, heading=0.):
+  s = np.linspace(0., 30., 33)
+  return SimpleNamespace(position=SimpleNamespace(x=s*math.cos(heading), y=offset+s*math.sin(heading)),
+                         orientation=SimpleNamespace(z=np.full_like(s, heading)))
+
+
+def check_raw_packing(wire, controller, path):
+  # Set a raw Float32 publication independently of the host quantization helper.
+  msg = custom.CarControlSP.new_message()
+  msg.fordLateralPath.pathOffset = controller.c0
+  msg.fordLateralPath.pathAngle = controller.c1
+  raw = create_lat_ctl2_msg(wire.packer, wire.bus, 2 if path.valid else 0, -msg.fordLateralPath.pathOffset,
+                            -msg.fordLateralPath.pathAngle, 0., 0., wire.count % 16)
+  wire.check(path)
+  wire.parser.update([wire.count*10_000_000, [raw]])
+  decoded = wire.parser.vl['LateralMotionControl2']
+  assert abs(decoded['LatCtlPathOffst_L_Actl']+path.path_offset) < 1e-9
+  assert abs(decoded['LatCtlPath_An_Actl']+path.path_angle) < 1e-9
+
+
+def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
+  dependency = verify_dependency(opendbc_revision)
+  if cycles < 1:
+    raise ValueError('cycles must be positive')
+  rng = np.random.default_rng(seed)
+  controller, mirrored, wire = ModelActionController(), ModelActionController(), WireCheck()
+  c0 = c1 = 0.
+  resets = 0
+  rates = (4., .5)
+  dt_values = (.002, .003, .01, .013, .05, .1)
+  max_continuous_step = np.zeros(2)
+  for i in range(cycles):
+    offset, heading = float(rng.uniform(-8., 8.)), float(rng.uniform(-1.2, 1.2))
+    speed = float(rng.uniform(.3, 55.))
+    desired = float(rng.uniform(-.15, .15))
+    yaw = float(rng.uniform(-3., 3.))
+    dt = dt_values[i % len(dt_values)]
+    active = i % 137 != 0
+    valid = i % 211 != 0
+    if i % 307 == 0:
+      dt = .101
+    model, mirror = line(offset, heading), line(-offset, -heading)
+    if i % 401 == 0:
+      model.position.y[4] = mirror.position.y[4] = math.nan
+    out = controller.update(model, desired, speed=speed, dt=dt, yaw_rate=yaw, active=active, valid=valid)
+    other = mirrored.update(mirror, -desired, speed=speed, dt=dt, yaw_rate=-yaw, active=active, valid=valid)
+    expected_valid = active and valid and dt <= .1 and i % 401 != 0
+    assert out.valid == other.valid == expected_valid
+    previous = np.array([c0, c1])
+    if expected_valid:
+      distance, rotation = speed*.15, desired*speed*.15
+      # Independent full pose transform, including stable small-angle series.
+      if abs(rotation) < 1e-5:
+        ego_x = distance*(1.-rotation**2/6.+rotation**4/120.)
+        ego_y = distance*(rotation/2.-rotation**3/24.+rotation**5/720.)
+      else:
+        ego_x, ego_y = math.sin(rotation)/desired, (1.-math.cos(rotation))/desired
+      future_x, future_y = (7.+distance)*math.cos(heading), offset+(7.+distance)*math.sin(heading)
+      predicted = math.cos(rotation)*(future_y-ego_y)-math.sin(rotation)*(future_x-ego_x)
+      target = (max(-5.11, min(5.11, predicted)), max(-.5, min(.5, max(7., speed)*desired)))
+      # Independent piecewise scalar oracle; do not call the production helper.
+      offset_target = target[0]
+      if offset_target > 0. and yaw > 0.:
+        offset_target = max(0., offset_target-1.4*max(0., yaw-max(0., speed*desired)-.02))
+      elif offset_target < 0. and yaw < 0.:
+        offset_target = min(0., offset_target+1.4*max(0., -yaw-max(0., -speed*desired)-.02))
+      assert abs(offset_target) <= abs(target[0]) and offset_target*target[0] >= 0.
+      c0 += max(-4.*dt, min(4.*dt, offset_target-c0))
+      c1 += max(-.5*dt, min(.5*dt, target[1]-c1))
+      step = abs(np.array([controller.c0, controller.c1])-previous)
+      assert (step <= np.array(rates)*dt+1e-10).all()
+      max_continuous_step = np.maximum(max_continuous_step, step)
+    else:
+      c0 = c1 = 0.
+      resets += 1
+      assert out.path_offset == out.path_angle == 0.
+    assert abs(controller.c0-c0) < 1e-10 and abs(controller.c1-c1) < 1e-10
+    assert abs(controller.c0+mirrored.c0) < 1e-10 and abs(controller.c1+mirrored.c1) < 1e-10
+    assert abs(out.path_offset+other.path_offset) <= .0100001
+    assert abs(out.path_angle+other.path_angle) <= .0005001
+    assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0.
+    check_raw_packing(wire, controller, out)
+
+  # Every representable host C0/C1 value, plus the float32 immediately below,
+  # at, and above each half-quantum transition. Seed the slew positions only
+  # here to isolate packing from slew; the sequence above checks actual slew.
+  boundary_cases = 0
+  for field, resolution, low, high in ((0, .01, -5.11, 5.11), (1, .0005, -.5, .5)):
+    grid = np.arange(round(low/resolution), round(high/resolution)+1)*resolution
+    for value in np.r_[grid, (grid[:-1]+grid[1:])/2.]:
+      raw = np.float32(value)
+      for scalar in (np.nextafter(raw, np.float32(-np.inf)), raw, np.nextafter(raw, np.float32(np.inf))):
+        selected = float(np.clip(scalar, low, high))
+        offset, heading = (selected, 0.) if field == 0 else (0., selected)
+        controller.c0, controller.c1 = offset, heading
+        out = controller.update(line(offset), heading/20., speed=20., dt=.01)
+        check_raw_packing(wire, controller, out)
+        boundary_cases += 1
+  report = {'seed': seed, 'random_cycles': cycles, 'mirrored_core_updates': cycles,
+            'invalid_or_inactive_resets': resets, 'field_boundary_cases': boundary_cases,
+            'float32_can_round_trips': wire.count, 'analytic_targets_scalar_slew_and_mirror_checks_pass': True,
+            'bounded_excess_yaw_damping_checked': True,
+            'full_geometric_prediction_checked': True,
+            'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(),
+            'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.',
+            'opendbc_import_head': revision(dependency),
+            'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in
+                              (Path(__file__), Path(ford_model_action.__file__), Path(__file__).with_name('model_action_replay.py'))}}
+  output.parent.mkdir(parents=True, exist_ok=True)
+  output.write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({k: v for k, v in report.items() if k != 'source_sha256'}, indent=2))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--cycles', type=int, default=200_000)
+  parser.add_argument('--seed', type=int, default=20260907)
+  parser.add_argument('--output', type=Path, required=True)
+  parser.add_argument('--opendbc-revision', default=PINNED_OPENDBC,
+                      help='Exact required dependency commit; defaults to the historical replay pin.')
+  args = parser.parse_args()
+  run(args.cycles, args.seed, args.output, args.opendbc_revision)

tools/ford_pscm_lab/test_model_action_replay.py

@@ -0,0 +1,93 @@
+import hashlib
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from tools.ford_pscm_lab import model_action_replay as replay
+
+
+def test_service_sampling_keeps_original_gaps_and_never_pulls_future_inputs():
+  stream = {'t': np.array([1., 1.01, 2.]), 'value': np.array([3., 4., 5.])}
+  sampled = replay.sample(stream, np.array([1.005, 1.5, 2.]))
+  np.testing.assert_array_equal(sampled['t'], [1., 1.01, 2.])
+  np.testing.assert_array_equal(sampled['value'], [3., 4., 5.])
+  assert 1.5-sampled['t'][1] > .15  # The adapter sees the gap, not a resampled fresh input.
+
+
+@pytest.mark.parametrize('times', [[], [1., .9]])
+def test_replay_does_not_sort_away_backward_time_or_invent_missing_streams(times):
+  with pytest.raises(ValueError):
+    replay.sample({'t': np.array(times)}, np.array([1.]))
+
+
+def test_dependency_mismatch_fails_before_replaying(monkeypatch):
+  monkeypatch.setattr(replay, 'revision', lambda _: 'wrong_revision')
+  with pytest.raises(ValueError, match='Expected opendbc'):
+    replay.verify_dependency()
+
+
+@pytest.mark.parametrize('revision', ['HEAD', '744a97d9b', '--help', 'z'*40])
+def test_archived_controller_requires_an_immutable_commit(revision):
+  with pytest.raises(ValueError, match='immutable'):
+    replay.load_controller(revision)
+
+
+def test_archived_loader_uses_exact_source_and_records_its_hash(monkeypatch):
+  # Unit tests must also work in shallow checkouts. Full replays verify real archived code.
+  calls = []
+  source = b'archived_value = 42\n'
+
+  def read_source(command):
+    calls.append(command)
+    return source
+
+  monkeypatch.setattr(replay.subprocess, 'check_output', read_source)
+  replay.load_controller.cache_clear()
+  try:
+    for commit in (replay.V1_REVISION, replay.V2_REVISION, replay.V3_REVISION):
+      module = replay.load_controller(commit)
+      assert module.archived_value == 42
+      assert module.source_sha256 == hashlib.sha256(source).hexdigest()
+      assert calls[-1][-2:] == ['show', f'{commit}:openpilot/selfdrive/controls/lib/ford_model_action.py']
+      assert replay.load_controller(commit) is module
+    assert len(calls) == 3
+  finally:
+    replay.load_controller.cache_clear()
+
+
+def test_explicit_stress_dependency_still_requires_an_exact_match(monkeypatch):
+  monkeypatch.setattr(replay, 'revision', lambda _: 'deployment_commit')
+  replay.verify_dependency('deployment_commit')
+  with pytest.raises(ValueError, match='Expected opendbc'):
+    replay.verify_dependency('different_commit')
+  with pytest.raises(ValueError, match='Expected opendbc'):
+    replay.verify_dependency()  # Historical replay never silently follows the local checkout.
+
+
+def test_source_route_directory_cannot_be_overwritten(tmp_path):
+  with pytest.raises(ValueError, match='outside the source'):
+    replay.run(tmp_path, tmp_path/'selected_controller')
+
+
+@pytest.mark.parametrize('field,value', [(0, 5.12), (1, .501), (2, .00002), (3, .000001), (0, np.nan)])
+def test_field_validation_catches_range_and_zero_c2_c3_violations(field, value):
+  command = np.zeros((2, 4))
+  command[0, field] = value
+  with pytest.raises(AssertionError):
+    replay.field_checks(command, np.array([True, True]), np.array([1., 1.01]))
+
+
+def test_invalid_cycles_cannot_publish_a_retained_command():
+  with pytest.raises(AssertionError):
+    replay.field_checks(np.array([[.01, 0., 0., 0.]]), np.array([False]), np.array([1.]))
+
+
+def test_field_validation_catches_excessive_slew_with_original_dt():
+  with pytest.raises(AssertionError):
+    replay.field_checks(np.array([[0., 0., 0., 0.], [.08, 0., 0., 0.]]), np.array([True, True]), np.array([1., 1.01]))
+
+
+def test_wire_validator_checks_real_curvature_fields_instead_of_filling_them_with_zero():
+  with pytest.raises(AssertionError):
+    replay.WireCheck().check(FordPath(True, 0., 0., .001, 0.))