Merge sunnypilot master into hiimisaac-dev

Routine sync: bring hiimisaac-dev up to date with master while preserving Ford-related work on this branch.

.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_c1_carryover.md

@@ -0,0 +1,102 @@
+# Ford C1 correction carryover experiment
+
+The feedback controller at `5fbb583e5` can retain a correction from an earlier
+turn that outweighs the new base C1. The measured curvature can already be
+opposite the desired curvature, yet total C1 continues to request the old
+direction while the integral works back toward zero.
+
+This experiment keeps the existing 1:1 feedback strength and adds a conditional
+reset of that correction. It is a command-policy experiment, not a demonstrated
+improvement in physical steering response.
+
+## Release rule
+
+All of the following must be true on a valid, active cycle:
+
+- Feedback is enabled and a fresh steering publication advances measurement time.
+- Base C1 is nonzero by at least one DBC step (0.0005 rad).
+- Both target C0 and the slewed C0 request agree with base C1's direction,
+  by at least one DBC step (0.01 m).
+- Measured steering-derived curvature points opposite the desired curvature.
+- The accumulated correction prevents total C1 from requesting the base direction:
+  the sum of base C1 and correction is zero or opposite base C1.
+
+The stored correction is then set to zero before the usual feedback increment.
+The final C1 command still passes through its existing ±0.5 rad amplitude and
+0.5 rad/s slew limits. The reset cannot directly jump the transmitted command.
+The DBC steps reject requests smaller than one representable step; they are
+not new strength multipliers. This reset policy is itself an engineering choice.
+
+There is no reset simply because steering error crosses zero, or because C1
+and its correction have opposite signs. Matched curvature, neutral/conflicting
+C0, a correction that does not outweigh base C1, and repeated measurements all
+preserve normal integration. The condition can apply to small steering
+corrections as well as large turns; it has no turn-size or speed threshold.
+
+No previous-turn direction or timer is stored. Agreement between current path
+requests and disagreement with measured curvature are the confirmation. This
+does not establish which part of the combined C0/C1 request a PSCM physically
+needs. In particular, when C0 still points into the previous turn, this rule
+deliberately leaves the integral alone.
+
+## Preserved behavior and diagnostics
+
+C0's 7 m mapping, its limits, the base C1 mapping, upstream curvature limiting,
+the original integral strength, driver/PSCM arbitration, C2=C3=0 and the 100 Hz
+sender are unchanged. No fitted PSCM model, proportional term or gain schedule
+is added. There are still three values used by the command law: C0, C1 and
+the correction. A diagnostic-only `carryover_release_count` is added and resets
+with the controller. It is included in the existing periodic diagnostic event.
+
+The same default-off Sunnylink toggle selects this version. Its diagnostic
+identity is `model-action-c1-feedback-v2`. See the [drive-test instructions](ford_model_action_drive_test.md).
+
+## Offline evidence
+
+The two mirrored command-regression tests failed before the change. After
+building correction through actual feedback, the old controller still requested
+the old C1 direction 0.4 s into a reversal. Both tests now pass with the original
+output slew. Additional tests cover holding a steady curve, small error
+crossings, neutral and conflicting C0, representable command boundaries,
+freshness, driver override and PSCM limits. Integration tests execute the actual
+controlsd selection and upstream limiter, Float32 publication and Ford CAN
+builder, using both model and maneuver-plan requests and both turn directions.
+
+The combined suite passes **567 tests and 9,146 subtests**, with the same 178
+inherited/unsupported safety-test skips as the original feedback validation.
+The randomized checks include mirrored inputs, zero-error compatibility, and
+comparison against the exact previous controller from cloned pre-update states.
+
+Frozen b8 replay triggers 11 releases; b9 triggers 14. Activation and C0 match
+the previous controller exactly on every reconstructed cycle. In b9, most
+releases concern small corrections; one follows the large turn around 13:28.
+The C0/C1 disagreement at 14:36 is preserved. Numerical details and source
+hashes are in `ford_c1_carryover_validation.json`.
+
+At the release around 13:28, the candidate C1 crosses into the requested
+direction 0.255 s earlier than the previous controller on identical frozen
+inputs. This is a command zero-crossing comparison, not a measured improvement
+in the truck's steering response. The lab checks total 669,343 Float32/CAN
+round trips, in addition to the integration tests.
+
+Replay preserves recorded model requests and measured motion. A difference
+between candidate and baseline commands can persist because the recorded
+steering does not respond to the changed command. Replay cannot predict wheel
+angles, centering, oscillation, or how much earlier the vehicle would unwind.
+No device build, boot, installation or physical validation was performed.
+
+## Reproduction
+
+Use the branch's native dependencies and pinned opendbc revision
+`c21a9013700734dd20b09e05aa68329ad8cc20f9`. The route commands require the existing
+full-rlog b8/b9 extracts and the baseline Git revision. Run:
+
+```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 opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_c1_carryover/stress.json
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_c1_carryover/zero_error_stress.json
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb8 --baseline 5fbb583e592d30de266f8160a5d6b9c620c97f56 --output .cache/ford_c1_carryover/routeb8
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb9 --baseline 5fbb583e592d30de266f8160a5d6b9c620c97f56 --output .cache/ford_c1_carryover/routeb9
+```

docs/ford_c1_carryover_validation.json

@@ -0,0 +1,177 @@
+{
+  "created_at_utc": "2026-09-10T14:00:37.853762+00:00",
+  "scope": "Conditional release of accumulated C1 correction; offline command behavior only, no predicted vehicle response.",
+  "baseline_commit": "5fbb583e592d30de266f8160a5d6b9c620c97f56",
+  "baseline_source_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c1-feedback-v2",
+  "calibration_approved": false,
+  "toggle": {
+    "key": "FordModelActionController",
+    "default_enabled": false,
+    "activation": "Existing controlsd startup selection"
+  },
+  "release_rule": "Fresh enabled feedback; target and slewed C0 agree with base C1 by >= one DBC step; measured curvature is opposite; stored correction makes total C1 zero or opposite base. Clear correction, then apply original integration and output slew.",
+  "engineering_choices": "Conditional reset policy, using existing DBC steps (0.01 m, 0.0005 rad) to confirm nonzero commands. Original 1:1 integral strength is unchanged.",
+  "preserved": [
+    "C0 mapping and limits",
+    "Base C1 mapping",
+    "Original integral strength",
+    "Final C1 amplitude and slew limits",
+    "Driver and PSCM arbitration",
+    "Upstream selection and limiting",
+    "100 Hz sender",
+    "C2=C3=0"
+  ],
+  "panda_safety_changed": false,
+  "opendbc_submodule_changed": false,
+  "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "controller_size": {
+    "total_lines": 194,
+    "code_lines_excluding_blanks_comments_docstrings": 131,
+    "core_command_state_values": 3,
+    "core_diagnostic_counters": 1
+  },
+  "tests": {
+    "combined_suite": "567 passed, 178 skipped, 9146 subtests passed in 6.45s",
+    "safety_skips": "Same 178 inherited or unsupported variants recorded in ford_c1_feedback_validation.json.",
+    "regression": "Two mirrored carryover command tests fail on the exact baseline class and pass in the candidate suite.",
+    "ruff_changed_python": "pass",
+    "ty_controller": "pass",
+    "settings_compiler_check": "pass",
+    "carryover_controlsd_to_can_frames": 1120,
+    "existing_feedback_controlsd_to_can_frames": 1010,
+    "integration_scope": "Actual source selection, upstream limiting, controller, Float32 publication, Ford sender, both plan sources and signs, all counters and checksums."
+  },
+  "routes": {
+    "b8": {
+      "cycles": 160431,
+      "active_cycles": 68217,
+      "validity_and_c0_match_baseline_exactly": true,
+      "c1_changed_cycles": 6065,
+      "max_abs_c1_change_rad": 0.09250000000000003,
+      "can_round_trips": 160431,
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; b8 and b9 have no maneuver-plan messages.",
+      "carryover_release_count": 11,
+      "input_sha256": {
+        "route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+        "model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+        "metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c"
+      }
+    },
+    "b9": {
+      "cycles": 90774,
+      "active_cycles": 86474,
+      "validity_and_c0_match_baseline_exactly": true,
+      "c1_changed_cycles": 15208,
+      "max_abs_c1_change_rad": 0.10400000000000004,
+      "can_round_trips": 90774,
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; b8 and b9 have no maneuver-plan messages.",
+      "carryover_release_count": 14,
+      "input_sha256": {
+        "route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee"
+      }
+    }
+  },
+  "command_timing_example": {
+    "event": {
+      "time_s": 808.286646083,
+      "correction_before_rad": -0.13089810321135922,
+      "correction_after_rad": 0.0,
+      "base_c1_rad": 0.06412824021622576,
+      "desired_angle_deg": -24.17155647277832,
+      "actual_angle_deg": -0.30000001192092896,
+      "speed_m_s": 11.804088592529297,
+      "baseline_c0_c1": [
+        0.15000000000000036,
+        -0.06600000000000006
+      ],
+      "candidate_c0_c1": [
+        0.15000000000000036,
+        -0.062000000000000055
+      ]
+    },
+    "scope": "Command zero crossing on identical frozen recorded inputs; not wheel response.",
+    "baseline_c1_rightward_at_s": 808.67241324,
+    "candidate_c1_rightward_at_s": 808.4169884780001,
+    "command_crossing_advance_s": 0.25542476199984776
+  },
+  "feedback_stress": {
+    "cycles": 200000,
+    "mirrored_updates": 200000,
+    "can_round_trips": 200000,
+    "carryover_release_count": 946,
+    "baseline_revision": "5fbb583e592d30de266f8160a5d6b9c620c97f56",
+    "baseline_source_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34",
+    "exact_unchanged_state_and_commands_without_release": 199054,
+    "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, carryover direction/confirmation, integration, PSCM limits, CAN.",
+    "scope": "Numerical software invariants only; no model of vehicle motion.",
+    "calibration_approved": false,
+    "controller_sha256": "6f40a05977253987a2c96e74c8c18d912367ed1e55630558ed7b28d52576e552"
+  },
+  "zero_error_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": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "total_lab_float32_can_round_trips": 669343,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "6f40a05977253987a2c96e74c8c18d912367ed1e55630558ed7b28d52576e552",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py": "04935fb941a795cb243870a4c03f7073c68147b01da3cedf2872476aa5fb798e",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "e2e98d3a0a531235abd032fc4d3564796613ad51ff6ba22a230c48e36f6f6848",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "90fb42ce580f0085e349467086a2eef28c2512c671755d0771f6331d30b19035",
+    "tools/ford_pscm_lab/feedback_replay.py": "9bf145fbff6ed685aec2c0e5d0584e2dc7ff831021f15110f939e8a94c93280b",
+    "tools/ford_pscm_lab/stress_model_action.py": "0b25188edf2b248ebe741173ce02ce75bd59f1f39fd5bd909d41a3dca2294aa8",
+    "tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f",
+    "docs/ford_c1_carryover.md": "e8d08375963efc6d1ae6ce503bb580ba00cfa90adb71c941d56fe6e3701d5cbb",
+    "docs/ford_c1_feedback.md": "1b440a03082e5cec264a1d6693833ed0a7e07b6c6e2122f8e4455c5971121b57",
+    "docs/ford_model_action_drive_test.md": "7ac5ca0faf9690a23e7058d09b55f23e21e2ba74666001cacb56b67e8d8b4376",
+    "openpilot/selfdrive/controls/controlsd.py": "2b7e246f00bccce3a2bb9f6f44009ca77690cadb8527cd2bdfe855e9ad72ad1e",
+    "opendbc_repo/opendbc/car/ford/carcontroller.py": "b2d327a1833fb1f0d09ee17f54c9c8d45517fa29beb04a4543cfbf1b43f1a65e",
+    "opendbc_repo/opendbc/safety/modes/ford.h": "1d9d996292d6697ab4f02d55fae348d6aca1df94a07f7bdae48b68971b91afe7",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "7d38f315a7c5ce6d46d01a06f7eaddd4933f85639e5325ff71fdce22866ef401"
+  },
+  "artifact_sha256": {
+    ".cache/ford_c1_carryover/tests.txt": "cd65146df93632e4a2c1e086781e1da4673db7d038c7e673124f227efefbb567",
+    ".cache/ford_c1_carryover/baseline_regression.txt": "4469873f95ccf45a376a27fed05be3bdad5f808af7ceca472c6e2cb9d973eb7f",
+    ".cache/ford_c1_carryover/stress.json": "7a8d20d7abd7cf444f55b7316e8bedbed0fbe8e9587e09f90d5b1946c3c2e98c",
+    ".cache/ford_c1_carryover/zero_error_stress.json": "09e64eaac35df4ec324b41fabdc8baf91931106ac98b89c1ca71f4c8bf8796a4",
+    ".cache/ford_c1_carryover/timing.json": "0d18ed4164803adedbbd660fe024f4c28de8eb7921caa60659346ff386d3847f",
+    ".cache/ford_c1_carryover/routeb8/report.json": "d2c6f767cef29a74e292b6a16263d2da13b8c302e4653e419b0e232e1aaf762e",
+    ".cache/ford_c1_carryover/routeb8/commands.npz": "89b5c3474940b61afce060111c27fd9bad9e24d703c59fca61adf4ce10473df3",
+    ".cache/ford_c1_carryover/routeb9/report.json": "e3ff7bfa70e770eca763b125c283fd8a1d509ef1b6e7f26a81c398aca89a87da",
+    ".cache/ford_c1_carryover/routeb9/commands.npz": "e4f5f341146e2897a479baf222d678fd16352c8da931876a2471c3719faf9edf"
+  },
+  "test_environment": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps",
+    "PYTHONDONTWRITEBYTECODE": "1",
+    "LOG_ROOT": "/private/tmp/ford-carryover-logs",
+    "PARAMS_ROOT": "/private/tmp/ford-carryover-params"
+  },
+  "limitations": [
+    "Frozen replay preserves recorded requests and measured motion; changed commands do not establish changed wheel angles, centering or stability.",
+    "C0/C1 agreement is a reset-policy choice, not an identified relationship between PSCM input and wheel angle.",
+    "The rule can release small corrections and does not promise unchanged centering during transients.",
+    "No device build, boot, installation or physical validation was performed."
+  ]
+}

docs/ford_c1_feedback.md

@@ -0,0 +1,129 @@
+# Ford C1 feedback experiment
+
+This document records the original feedback change at `5fbb583e5`. The current
+version retains its feedback law and adds [conditional carryover release](ford_c1_carryover.md).
+The validation counts below describe the original change; current results are
+recorded in `ford_c1_carryover_validation.json`.
+
+The restored original v1 can leave a steering error while C0 and C1 still have
+room. Its command law does not directly correct measured steering error. This
+experiment keeps that mapping and adds one accumulated C1 correction:
+
+```text
+error = selected_limited_desired_curvature - measured_curvature
+correction += error * speed * elapsed_measurement_time
+C1_target = original_model_C1 + correction
+```
+
+Curvature (1/m) multiplied by traveled distance (m) gives heading mismatch in
+radians. Applying that mismatch to C1 at **1:1 is an explicit feedback-strength
+choice**. Dimensional consistency does not prove that every PSCM responds
+correctly to that strength. There is no fitted PSCM response model or new
+tunable multiplier.
+
+For example, at 20 m/s, a constant curvature shortfall of 0.001/m adds 0.02 rad
+to C1 over one second when the output can accept it. When measured curvature
+matches the request, the correction holds. If the vehicle turns more than
+requested, the correction moves in the unwind direction. Changing the model
+request still changes the base immediately, subject to the existing slew.
+
+## Preserved mapping and limits
+
+- C0 is the current model path's lateral offset at 7 m of arc distance, holding
+  the available endpoint for shorter paths; its limits remain ±5.11 m and 4 m/s.
+- Base C1 is `max(7 m, speed × 1 s) × selected_limited_desired_curvature`, clipped
+  to ±0.5 rad. Final C1 uses the same ±0.5 rad and 0.5 rad/s limits as v1.
+- C2 and C3 are zero. Sign conversion, Float32/CAN rounding, upstream curvature
+  limiting and the 100 Hz sender retain their existing behavior.
+
+The core holds three values: unquantized C0, unquantized C1 and the correction.
+Zero error from a reset leaves the correction at zero and preserves the old
+command arithmetic exactly. There is no separate percentage or distance cap
+on the correction.
+
+## Feedback measurement, timing and limits
+
+The measurement is `controlsd.curvature`, computed from measured steering
+angle with the existing live vehicle parameters. It matches the curvature
+used for the desired-versus-actual steering comparison. It is not an independent
+measurement of tire slip or the vehicle's actual ground path. CAN yaw remains
+an input-health gate and does not drive this feedback.
+
+The adapter integrates only elapsed time between fresh `carState` publications.
+The first publication after reset integrates zero time. Duplicate timestamps
+integrate zero; a fresh timestamp accounts for the elapsed measurement interval.
+Output slew continues on valid control cycles. Existing service-age, speed,
+model-geometry and clock-order gates remain, with the same finite/range check
+also applied to measured curvature. Disengagement or invalid input clears all
+three core states.
+
+The correction cannot accumulate farther into an unavailable C1 amplitude or
+slew request. Increments that move back toward the available output remain
+allowed. Moving the base request does not itself rewrite the correction.
+
+Fresh PSCM status means a valid message whose original CAN receipt timestamp
+is within the existing −5 to +150 ms age allowance. Reached-limit status (2)
+prevents extra accumulation in the measured turn direction. An old correction
+opposing that direction can return to zero; it cannot be trapped below the
+base request by the limit flag. Unwind and base model changes remain available.
+Close-to-limit status (1) does not block feedback. Missing or stale status
+does not gate it; local amplitude and slew anti-windup still apply.
+
+Driver steering-pressed, torque above the existing 1 Nm allowance, nonfinite
+torque, or fresh driver-limit status (3) clears the correction. Fresh denied
+or inactive PSCM status also clears it. The base model request continues
+through existing engagement and driver arbitration; clearing the correction
+does not bypass the final output slew.
+
+## Offline evidence and reproduction
+
+`ford_c1_feedback_validation.json` records the source hashes and completed
+checks. Tests exercise build, hold, unwind, saturation, limit flags, immediate
+driver input, stale and repeated measurements, invalid inputs and both signs.
+Integration tests execute actual controlsd selection and limiting, Float32
+publication, CarControlSP conversion and the Ford CarController CAN builder.
+Randomized runs check feedback invariants separately from zero-error
+compatibility with the original independent scalar oracle.
+
+The combined suite passes **511 tests and 9,146 subtests**. Its 178 skips are
+in inherited safety base classes or unsupported safety-test variants. Ruff,
+the controller's Ty check and settings compilation pass. Feedback stress,
+zero-error stress and the b8 replay total **578,569 Float32/CAN round trips**;
+the integration test separately verifies 1,010 transmitted packet constructions,
+including every counter and checksum. No packets are sent to hardware.
+
+The b8 replay retains recorded desired/measured curvature, model publications,
+driver input and PSCM flags. It compares candidate commands with the restored
+v1 at `a7d70e2b0890184636827351e4789d866f2a7c97`. All 160,431 reconstructed
+activation decisions and C0 commands match. C1 changes on 58,106 cycles.
+At 4:12.493, for example, reconstructed host C1 changes from −0.1625 to
+−0.2035 rad; at 3:56.250 it changes from −0.1280 to −0.1080 rad. These are
+changes to commands on frozen measurements, not predicted wheel angles.
+
+Controls publication time proxies the unlogged computation clock, and the
+full SubMaster health state cannot be reconstructed. This route uses the
+consumed model publication as its reference and has no maneuver-plan messages.
+Replay cannot show whether this feedback fixes weak turns, hanging turns or
+oscillation. A new drive is needed to measure those outcomes.
+
+Use the branch's native dependencies and pinned opendbc revision
+`c21a9013700734dd20b09e05aa68329ad8cc20f9`:
+
+```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 opendbc_repo/opendbc/safety/tests/test_ford.py
+python openpilot/sunnypilot/sunnylink/tools/compile_settings_ui.py --check
+python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_c1_feedback/feedback_stress.json
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_c1_feedback/zero_error_stress.json
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb8 --output .cache/ford_c1_feedback/routeb8
+```
+
+The last command requires the existing full-rlog b8 extract (`route.npz`,
+`model_paths.npz`, `metadata.json`), identified by hashes in the validation
+record. The historical route90/95 replay deliberately sets measured curvature
+equal to requested curvature to check zero-error compatibility; it does not
+exercise recorded steering feedback.
+
+Enable using the [existing Sunnylink toggle](ford_model_action_drive_test.md).
+The diagnostic identity is `model-action-c1-feedback-v1`.

docs/ford_c1_feedback_validation.json

@@ -0,0 +1,258 @@
+{
+  "created_at_utc": "2026-09-09T14:45:33.853345+00:00",
+  "baseline_commit": "a7d70e2b0890184636827351e4789d866f2a7c97",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "scope": "C1 measured-curvature feedback on restored original v1. Offline software validation only; no predicted or measured physical improvement.",
+  "calibration_approved": false,
+  "toggle": {
+    "key": "FordModelActionController",
+    "default_enabled": false,
+    "activation": "Existing startup selection after offroad-to-onroad cycle"
+  },
+  "feedback_law": "correction += (desired_curvature - measured_curvature) * speed * elapsed_measurement_time, subject to output and PSCM anti-windup",
+  "feedback_strength": "Explicit 1:1 heading-error-to-C1 choice; no fitted PSCM plant or new tunable multiplier",
+  "preserved": [
+    "C0 mapping and limits",
+    "C2=C3=0",
+    "C1 final amplitude and slew limits",
+    "100 Hz sender",
+    "upstream selection and limiting"
+  ],
+  "panda_safety_changed": false,
+  "opendbc_submodule_changed": false,
+  "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "controller_size": {
+    "total_lines": 181,
+    "code_lines_excluding_blanks_comments_docstrings": 123,
+    "core_persistent_values": 3,
+    "adapter_timestamps": 3
+  },
+  "tests": {
+    "combined_suite": "511 passed, 178 skipped, 9146 subtests passed in 5.14s",
+    "suite_log_sha256": "001ef6633b22513317593dd8debc160a0ca8aaf78ea53418c5f7a50c370cc818",
+    "ruff_changed_python": "pass",
+    "ty_controller": "pass",
+    "settings_compiler_check": "pass",
+    "safety_skip_reasons": [
+      "SKIPPED [145] ../../../../dev/sunnypilot/.venv/lib/python3.12/site-packages/_pytest/unittest.py:523: Skipped",
+      "SKIPPED [9] opendbc_repo/opendbc/safety/tests/common.py:64: Safety mode implements no _user_regen_msg",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:51: Skipping test because MADS button is not supported",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:254: Skipping test because MADS button is not supported",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:67: Skipping test because _acc_state_msg is not implemented for this car",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:165: Skipping test because MADS button is not supported",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:165: Skipping test because ACC main is not supported",
+      "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:411: MADS button not supported",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:378: CAN FD only",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:361: CAN FD only",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:351: CAN FD only",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:327: CAN FD only",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:341: CAN FD only",
+      "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:320: CAN FD only"
+    ],
+    "safety_native_build": "Pinned safety C source is compiled locally by libsafety_py before testing.",
+    "controlsd_to_can_feedback_integration_frames": 1010,
+    "integration_checks": "Both signs: build, hold, unwind, rebuild, immediate driver override; actual 100 Hz sender, counter, checksum, fields and publication. Separate integration tests validate PSCM service forwarding.",
+    "regression_test_evidence": [
+      "Nonzero-error integration failed with zero correction before implementing feedback.",
+      "Both sign tests failed when a reached limit trapped an old opposing correction; they pass after allowing return to zero."
+    ]
+  },
+  "route_b8": {
+    "baseline_revision": "a7d70e2b0890184636827351e4789d866f2a7c97",
+    "baseline_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+    "cycles": 160431,
+    "active_cycles": 68217,
+    "validity_and_c0_match_original_v1_exactly": true,
+    "status_counts": {
+      "inactive": 92214,
+      "active": 68217
+    },
+    "feedback_enabled_seconds": 598.256888772994,
+    "pscm_limit_2_seconds": 12.139079590997426,
+    "c1_changed_cycles": 58106,
+    "max_abs_c1_change_rad": 0.29800000000000004,
+    "max_abs_correction_rad": 0.29816844327770786,
+    "can_round_trips": 160431,
+    "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+    "reference_limit": "Uses exact consumed model publication as reference; the b8 route has no maneuver-plan messages.",
+    "example_points": [
+      {
+        "time_s": 130.9368894940053,
+        "old_c0_c1": [
+          -0.7400000000000002,
+          -0.18700000000000006
+        ],
+        "candidate_c0_c1": [
+          -0.7400000000000002,
+          -0.22899999999999998
+        ],
+        "correction_rad": -0.042171663052515254,
+        "feedback_enabled": true,
+        "pscm_limited": false
+      },
+      {
+        "time_s": 235.3960996990063,
+        "old_c0_c1": [
+          -2.04,
+          -0.40449999999999997
+        ],
+        "candidate_c0_c1": [
+          -2.04,
+          -0.4145
+        ],
+        "correction_rad": -0.00989648519895422,
+        "feedback_enabled": true,
+        "pscm_limited": true
+      },
+      {
+        "time_s": 236.25034470800165,
+        "old_c0_c1": [
+          -1.46,
+          -0.128
+        ],
+        "candidate_c0_c1": [
+          -1.46,
+          -0.10799999999999998
+        ],
+        "correction_rad": 0.01990758350705991,
+        "feedback_enabled": true,
+        "pscm_limited": false
+      },
+      {
+        "time_s": 252.49320156300382,
+        "old_c0_c1": [
+          -0.6699999999999999,
+          -0.16249999999999998
+        ],
+        "candidate_c0_c1": [
+          -0.6699999999999999,
+          -0.20350000000000001
+        ],
+        "correction_rad": -0.040907632902654506,
+        "feedback_enabled": true,
+        "pscm_limited": false
+      },
+      {
+        "time_s": 674.430371745002,
+        "old_c0_c1": [
+          0.4299999999999997,
+          0.128
+        ],
+        "candidate_c0_c1": [
+          0.4299999999999997,
+          0.1345
+        ],
+        "correction_rad": 0.00639271291315417,
+        "feedback_enabled": true,
+        "pscm_limited": false
+      },
+      {
+        "time_s": 1534.5190040400048,
+        "old_c0_c1": [
+          2.62,
+          0.5
+        ],
+        "candidate_c0_c1": [
+          2.62,
+          0.5
+        ],
+        "correction_rad": 0.0,
+        "feedback_enabled": true,
+        "pscm_limited": true
+      },
+      {
+        "time_s": 1562.5074677500015,
+        "old_c0_c1": [
+          -0.1200000000000001,
+          -0.051000000000000045
+        ],
+        "candidate_c0_c1": [
+          -0.1200000000000001,
+          -0.046499999999999986
+        ],
+        "correction_rad": 0.004453988923883501,
+        "feedback_enabled": true,
+        "pscm_limited": false
+      }
+    ]
+  },
+  "route_input_sha256": {
+    "route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+    "model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+    "metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c"
+  },
+  "feedback_stress": {
+    "cycles": 200000,
+    "mirrored_updates": 200000,
+    "can_round_trips": 200000,
+    "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, integration direction/size, PSCM anti-windup, CAN fields.",
+    "scope": "Numerical software invariants only; no model of vehicle motion.",
+    "calibration_approved": false,
+    "controller_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34"
+  },
+  "zero_error_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": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "total_lab_float32_can_round_trips": 578569,
+  "artifact_sha256": {
+    ".cache/ford_c1_feedback/routeb8/report.json": "648887eebb76260a60f7c0f0d9aaac83d0f1c06b443e28bc6fdf296bad4526c0",
+    ".cache/ford_c1_feedback/routeb8/commands.npz": "1aef98365572b3fb3cb8ba2a93cf30041ff3be133718d469145b710f2c94dc32",
+    ".cache/ford_c1_feedback/feedback_stress.json": "abf4e7bccc1e460008cc7450fcd92e9b2a6108bd71e53a01bdc24e31e5b5ad32",
+    ".cache/ford_c1_feedback/zero_error_stress.json": "2a3f284e10e5054205a788cce59bcf57bd837e13afff327457244141ba5522f0",
+    ".cache/ford_c1_feedback/safety_skip_reasons.txt": "5384c82b07b7cc20c6b22b8e94246cb104d53f8866af02b28fda7d4138cf377f"
+  },
+  "native_params": {
+    "library_sha256": "270bf43241cf7c02cc432cf78ec9411a62d7653ca445695efe785ae82241aa09",
+    "sources_match_original_rebuild_record": true,
+    "provenance": "Same locally rebuilt native library and source hashes recorded in ford_model_action_drive_test_validation.json; verified for this run."
+  },
+  "test_environment": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps",
+    "LOG_ROOT": "/private/tmp/ford-feedback-logs",
+    "PARAMS_ROOT": "/private/tmp/ford-feedback-params",
+    "PYTHONDONTWRITEBYTECODE": "1"
+  },
+  "source_sha256": {
+    "docs/ford_c1_feedback.md": "c1bc7f24c5ebe28679b4a04d09085d7b937926e63a38d43a3abfac93dfcfa0f9",
+    "docs/ford_model_action_candidate.md": "20cd8d10008cd796cc8719f5795ee80f50d8133d6e7684fb057a78fb05323fbe",
+    "docs/ford_model_action_drive_test.md": "7860ae26a61682aff86743ba302eb23c8f271d5700a2e616da1b6b38d438b57d",
+    "opendbc_repo/opendbc/car/vehicle_model.py": "ddc2a93d9c2b2ef6c9a913a5aef4c51e2bc387db1f7640473657e5ade4e50fac",
+    "openpilot/selfdrive/controls/controlsd.py": "2b7e246f00bccce3a2bb9f6f44009ca77690cadb8527cd2bdfe855e9ad72ad1e",
+    "openpilot/selfdrive/controls/lib/drive_helpers.py": "916bcd83c2a909a89795da58c7c43d7b168c9b82e1a6d281484bae45c667c01e",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34",
+    "openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "84113b1b7800c868117af0034278f53a1a6153c7bb5fadc1ea45958e62c4f0d0",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "cbe1b2aa1961deba3a42e1d82f5f75ae0c3d7a219428dea5f50cb70e1b27fd11",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "c7f5ffd650e804e6e02fa12d435e0867b56b13a49c3d9fa511993188d5cb625a",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py": "f7a956c082a246d9506e21adbf348cbdc7f94d5342d832841058c71f7e264eeb",
+    "openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py": "7a13dc5ce49b40e27e05e62cdb9ef1bb764de8ed8167f7e982d54a4dffe97ed4",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "7d38f315a7c5ce6d46d01a06f7eaddd4933f85639e5325ff71fdce22866ef401",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "410e306958ece12e49fc114707741c57a2dd927c6ba3410e160834e52a759ea9",
+    "tools/ford_pscm_lab/feedback_replay.py": "ca552217953f3cce35da0b1666252fd43b6f8ab6c067e5c9102da8ea8d97c2f3",
+    "tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f",
+    "tools/ford_pscm_lab/stress_model_action.py": "0b25188edf2b248ebe741173ce02ce75bd59f1f39fd5bd909d41a3dca2294aa8"
+  },
+  "limitations": [
+    "Frozen route replay changes commands only; it cannot establish tracking, unwind response or closed-loop stability.",
+    "Measured curvature uses the existing steering-angle vehicle model; it is not an independent ground-path measurement.",
+    "No full device build, device boot, installation or road validation was performed."
+  ]
+}

docs/ford_c1_minimal_pi.md

@@ -0,0 +1,82 @@
+# Ford continuous PI drive trial (v7)
+
+The selected controller follows the current, upstream-limited desired curvature
+with P=0.50 and I=0.25. It replaces v6's request-change, C0-confirmation and
+unwind-catchup release rules. C0 mapping, heading overflow allocation, input
+health gates, driver/PSCM arbitration and final output limits remain unchanged.
+Only C0, C1 and I carry control history. C2/C3 stay zero.
+
+```text
+D = max(7 m, speed × 1 s)
+error = selected desired curvature − measured steering-derived curvature
+base = clip(D × desired curvature, −0.5, +0.5)
+P = 0.50 × D × error
+increment = 0.25 × speed × error × fresh steering measurement interval
+C1 target = clip(base + P + I, −0.5, +0.5)
+```
+
+After PSCM outward-accumulation arbitration, the increment first cancels up to
+its own magnitude of opposing I. It cannot cross zero in that step. Any remainder
+is bounded by the combined command's amplitude/slew headroom. C1 output still
+slews at 0.5 rad/s. This allows old I to unwind even when the output is already
+slewing. It adds no timer, request history, turn detection or position threshold.
+
+P doubles relative to v6, and I builds at one quarter the previous rate for an
+identical error and fresh measurement interval. Lower I also unwinds more slowly
+for an identical existing state and error; its replay benefit comes mainly from
+storing less correction. Zero error removes P and holds I. Persistent tracking
+bias may need that holding correction. There is no claim that all I is unwanted.
+
+## Evidence and limitations
+
+Six offline settings were compared across fourteen routes and 1,578,250 source
+cycles before selecting this candidate. The production selector and adapter now
+exactly reproduce the selected P=0.50/I=0.25 archived commands, validity, P, I,
+feedforward, C0 overflow and feedback/PSCM gates on every cycle of all fourteen
+routes. These comprise Lightning 112–117, a0, a2, a5, a9, b8, b9, ca and Raptor 02.
+The integration replay performs 1,578,250 actual Float32/CAN round trips.
+
+A further 20,000 randomized cycles with mirrored and independent C0 paths perform
+60,000 CAN checks on production, checking independent scalar arithmetic,
+C0-independent feedback, symmetry, reset, amplitude and slew behavior.
+The Ford, Sunnylink, params, sender and safety suite passes 679 tests and
+9,145 subtests; 178 are skipped by the platform test suite. Removed maneuver
+heuristic tests are replaced with continuous-error, cancellation, freshness,
+three-state reproduction and actual controlsd-to-CAN entry/exit checks.
+Historical untracked offline experiment tests are outside this deployment suite.
+
+At a previously reviewed route-115 exit (133.595 s), the original small PI
+controller requested +0.1090 rad C1; this trial requests +0.0175. That sample
+includes driver context. Reviewed large entries remain similar, but commands
+are not identical everywhere. In a previously well-tracked route-116 bend
+(112 s), C1 falls from +0.1530 to +0.1355 rad. Lower I could weaken a persistent
+bend, while higher P can increase response to measurement fluctuations.
+
+Recorded wheel motion remains fixed in replay. These checks establish software
+behavior and exact integration of the candidate; they do not establish improved
+physical tracking or stability. Gains remain experimental, not an identified
+universal PSCM calibration. No device build, boot or new drive is claimed.
+
+## Selection and reproduction
+
+Use the existing default-off Sunnylink **Selected-Action Path Tracking
+(Experimental)** toggle on any Ford CAN FD, followed by a real offroad-to-onroad
+cycle. Logs identify `model-action-c1-pi-v7`, `proportional_gain=0.5`,
+`integral_gain=0.25`. Toggle-off selects upstream Ford control. See the
+[drive instructions](ford_model_action_drive_test.md).
+
+With the built cereal/opendbc environment and archived local extracts:
+
+```sh
+export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps
+export PYTHONDONTWRITEBYTECODE=1
+export PARAMS_ROOT=/tmp/ford-v7-params
+export LOG_ROOT=/tmp/ford-v7-logs
+python -m tools.ford_pscm_lab.minimal_pi_validate --output .cache/ford_minimal_tuning/production --workers 4
+python -m tools.ford_pscm_lab.minimal_pi_production_stress --cycles 20000 --output .cache/ford_minimal_tuning/production_stress.json
+```
+
+The validation JSON records route and source hashes. The archived six-setting
+sweep is local evidence, not a checked-in dataset. Historical v5/v6 lab tools
+load their pinned controller revisions so their baseline comparisons retain
+their original meaning after production changes.

docs/ford_c1_minimal_pi_validation.json

@@ -0,0 +1,332 @@
+{
+  "scope": "Production integration of the selected continuous PI drive trial; fixed recorded motion, not a physical tracking prediction.",
+  "parent_revision": "5e6993aab",
+  "previous_controller_revision": "bf00bc691def830e1beb15363d05416714c1dc42",
+  "hypothesis": "model-action-c1-pi-v7",
+  "kp": 0.5,
+  "ki": 0.25,
+  "control_history": [
+    "c0",
+    "c1",
+    "correction"
+  ],
+  "physical_lines": {
+    "module": 201,
+    "core_class": 58,
+    "core_update": 42
+  },
+  "production_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+  "route_cycles": 1578250,
+  "production_can_round_trips": 1638250,
+  "tests": {
+    "passed": 679,
+    "skipped": 178,
+    "subtests_passed": 9145,
+    "log_sha256": "2bc3dfd7ba08d4985d3dea98f235eb1fc99b23e5bbb3927e823a2439c8e9a481",
+    "scope": "Ford controls, tracked PSCM lab tests, PSCM status, Sunnylink, params, Ford car and safety suites; historical untracked experiments excluded."
+  },
+  "ruff": "pass",
+  "ty_production": "pass",
+  "routes": [
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "112",
+      "cycles": 108971,
+      "can_round_trips": 108971,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/112/report.json": "d8cbcf8b676a67a90310390611da31783b4e025a0a965239ef4672caa47aae56",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/112/commands.npz": "8d91f449c7f8741ce465ec4326949a9d9527ed6b5de7a526ad7d30212f16c469",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "113",
+      "cycles": 49614,
+      "can_round_trips": 49614,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/113/report.json": "e2dfeba5d3fe8a1147f82bce9ddf2ad3f14271bda35fac1639d383f2a0189228",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/113/commands.npz": "3890c802072dd2d38b4cded5f9f80310d1f86eb8a8f3598138235a0e2cc16460",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "114",
+      "cycles": 61027,
+      "can_round_trips": 61027,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/114/report.json": "eb630bb5f35a43c8902e15a97b53d83db3dc3cfa843a14d09cb8a1e6dab89289",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/114/commands.npz": "899033d0c9eeffe7929aa1289bfbc0e7da5608833b03989ce10a87d975344d3d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "115",
+      "cycles": 40037,
+      "can_round_trips": 40037,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/115/report.json": "58cee499da3191808aedbd79a9497d9c1553f49007f24543c44bc2b58ff971fa",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/115/commands.npz": "09c2da33dabbc0072c61578224cd5ab27ed44690394315350c1c33708b0a67bc",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "116",
+      "cycles": 68793,
+      "can_round_trips": 68793,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/report.json": "01d2e608a53f9fd805baee853d18acc63b5506319e0f0ceeeb3e59a92bae0385",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/commands.npz": "2186a1a1fea2ea98390fb90c9df1288d321cd165f70e3aef0c268cbbd2fd8c95",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "117",
+      "cycles": 27301,
+      "can_round_trips": 27301,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/report.json": "80efcf48f018d3a887e8b9fdb36f67d9c5d9a57b09f117c8fdecd4d5556ff80b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/commands.npz": "c2056b5d9faf765cb4713f73e9c4937bb24c9eeb1d4dac0d21e141322d52609e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a0",
+      "cycles": 114582,
+      "can_round_trips": 114582,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/report.json": "5f3ca84b700e019cc72201d9e4ff85c42fbd15e9a3850434caf1bac688b332d8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/commands.npz": "5c52d587ba45446117677686aabc6af81d5d2eacab5c04b02b53fe6fb95e36d0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a2",
+      "cycles": 71111,
+      "can_round_trips": 71111,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/report.json": "de61021f0c13af16e98688b17aadae9d61c99d20be82329b7cc235836defa41a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/commands.npz": "fd263ab667b97905d840751577179ad75472421104cf20663e3ee51bf32dd2ee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a5",
+      "cycles": 38961,
+      "can_round_trips": 38961,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/report.json": "dc6c05043c5caef6bf12b12eff69ef53d213fe8606f59865859d2c475e0aa3ee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/commands.npz": "45f551b32a354a65bff585b81a89091abc5edad406b6ca100a8dcabc50c5dbe3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a9",
+      "cycles": 286319,
+      "can_round_trips": 286319,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/report.json": "17c41415b9fb9fcd8d72835347a205113c32d32d955c0de68850a9764968b3e9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/commands.npz": "27af5f6fb8f6e08f31d54e24c90430d30c72d31b1448bc1fbcb31f35804afe8a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "b8",
+      "cycles": 160431,
+      "can_round_trips": 160431,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/report.json": "5c6b52222abbb95147dc756a34e62ada97c42aa550b952fd6ae7b26d8d9639fa",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/commands.npz": "9c158eff98009bcb039a4e7c29b97b0d3a9c67d61160a4e4766a176174de52d7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "b9",
+      "cycles": 90774,
+      "can_round_trips": 90774,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b9/report.json": "9eeda0c24ede7a55ef8a924e0ebe0bcb0dea7a9612457af0c5aff898a828c195",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b9/commands.npz": "2dcc0865591c988d8a08671311a9631f3c4aa3ded4e90ee6f76d28b1654809ad",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "ca",
+      "cycles": 327448,
+      "can_round_trips": 327448,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/ca/report.json": "025f2f49f68ec2164a10474a4c6ec0eb1b6022990e135fc38134a052a8710dff",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/ca/commands.npz": "cbe9f63955a409495f57466dc5811d3eed65af2cf096ee060d924437488c3f76",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "raptor02",
+      "cycles": 132881,
+      "can_round_trips": 132881,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-c1-pi-v7",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/report.json": "2c2dda2a5e4e8a764a10852cca3e5caf319b62e3d1d841ef9e1b8c11be77ebf6",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/commands.npz": "e0440d0a4035b975a9c4a47d85335b5b0f0903fcd00dd7e7c858856deffd1249",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+      }
+    }
+  ],
+  "production_stress": {
+    "cycles": 20000,
+    "kp": 0.5,
+    "ki": 0.25,
+    "controller_updates": 60000,
+    "can_round_trips": 60000,
+    "seed": 20260913,
+    "independent_c0_comparisons": 20000,
+    "checks": "Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_production_stress.py": "fa9058826512498be1938797efba25666df832080fb7ad6ff18788ee4bda333d",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8"
+    }
+  },
+  "limits": [
+    "No hardware build, device boot or new physical drive performed.",
+    "Matched offline commands do not establish improved tracking or closed-loop stability.",
+    "Higher P can amplify measurement fluctuations; lower I can take longer to correct persistent error.",
+    "Toggle defaults off and off selects upstream Ford control; on applies to any Ford CAN FD."
+  ]
+}

docs/ford_c1_overflow.md

@@ -0,0 +1,131 @@
+# Ford base-heading overflow experiment
+
+This records the overflow implementation and validation at `b81c00f5b`.
+The later [toggle-off restoration](ford_upstream_fallback.md) updates selection
+and the opendbc sender while preserving the enabled experiment's command law.
+
+The Lightning ca route recorded controller `959ae3d6e`. Its large turns included
+flat C1 requests at ±0.5 rad while C0 still had available range. Those were
+nonzero, active commands, but increasing base heading above the C1 limit was
+discarded. Other apparent pauses followed reductions in the selected model
+request; this change continues to follow those reductions.
+
+The new experiment allocates clipped-away **base heading** to C0 using the
+existing 7 m reference. It does not allocate the accumulated feedback correction.
+This is a hypothesis about command allocation, not a measured improvement in
+PSCM response or a claim that C0 and C1 are physically interchangeable.
+
+## Command rule
+
+Using the selected, upstream-limited desired curvature:
+
+```text
+raw_base_c1 = max(7 m, speed × 1 s) × desired_curvature
+base_c1 = clip(raw_base_c1, -0.5 rad, +0.5 rad)
+extra_c0 = 7 m × (raw_base_c1 - base_c1)
+c0_target = clip(model_y_at_7m + extra_c0, -5.11 m, +5.11 m)
+```
+
+The combined C0 target still passes through the existing 4 m/s slew limit.
+C1 retains its existing feedback, ±0.5 rad amplitude and 0.5 rad/s slew limits.
+C2 and C3 remain zero. Short model paths retain their existing endpoint hold.
+
+Before amplitude/slew limits, the allocation preserves the linear reference
+`C0 + 7*C1` for the base request. This is a single-reference identity; it does
+not preserve the entire path or predict steering torque. The 7 m reference is
+an existing engineering choice. No fitted plant, new tunable strength multiplier,
+timer or stored overflow is added. The existing 1:1 feedback strength remains.
+
+Extra C0 falls with raw base heading and its target becomes zero at the C1 cap.
+The output can take longer to return because of its existing slew state. There
+is no guarantee that increasing C0 makes every PSCM turn better or release sooner.
+
+## Preserved integration
+
+The conditional correction release still requires **original model C0** and
+applied C0 to confirm base C1's direction. Added overflow cannot itself substitute
+for model confirmation. Changed applied C0 can nevertheless affect release
+timing in some histories. Driver/PSCM arbitration, service freshness, resets,
+upstream curvature limiting, Float32 publication and the 100 Hz sender remain.
+No opendbc dependency or Panda safety change is made.
+
+The existing default-off Sunnylink toggle selects this version on any Ford
+CAN FD vehicle. Diagnostic identity is `model-action-c1-feedback-v3`.
+`offset_overflow` records extra target meters before C0 amplitude and slew;
+`offset_request` continues to record the actual continuous C0 state.
+See the [drive-test instructions](ford_model_action_drive_test.md).
+
+## Offline evidence
+
+The focused overflow regressions initially produced 28 failures and 18 passes
+against the prior controller. They now pass. They cover both signs, several
+speeds, the heading threshold, combined C0 clipping, short paths, release,
+feedback-only saturation, driver/PSCM feedback gates and independent model
+confirmation. Twelve integration cases send 4,800 frames through actual
+controlsd selection/limiting, Float32 publication and Ford CAN packing on all
+six listed Ford CAN FD platforms, checking counters and checksums.
+
+The combined suite passes **670 tests and 9,146 subtests**, with 178 inherited
+or unsupported safety-test skips. Both 200,000-cycle randomized runs pass,
+including mirrored inputs, independent scalar target/slew checks, feedback
+invariants, comparison with the exact prior controller from cloned states,
+and 18,138 exhaustive field/Float32 boundary cases. These checks and the ca
+replay total **745,586 Float32/CAN round trips**, in addition to integration tests.
+
+Frozen ca replay covers 327,448 control cycles across all 55 extracted segments.
+Activation is identical; C1, C2 and C3 are identical on every cycle. C0 differs
+for 855 cycles (8.607 s), concentrated in the large turns and their slew tails.
+The extra target is present for 7.359 s. Before the first overflow, every command
+matches the prior controller. All disabled cycles have zero commands.
+
+At the same recorded peak-request timestamps, absolute packed C0 changes as follows:
+
+| Segment | Previous C0 | Candidate C0 | C1 magnitude, both |
+| --- | ---: | ---: | ---: |
+| 10 | 3.90 m | 5.11 m | 0.50 rad |
+| 31 | 2.85 m | 3.08 m | 0.50 rad |
+| 35 | 3.67 m | 5.11 m | 0.50 rad |
+| 52 | 3.11 m | 3.42 m | 0.50 rad |
+
+The candidate reaches the existing C0 cap for 2.054 s. These are reconstructed
+commands on original inputs, not newly transmitted commands or predicted wheel
+angles. Segment 52's output is still slewing at the selected timestamp.
+
+Every overflow episode returns to the previous C0 output without a reset or
+another overflow interrupting the comparison. After overflow first becomes
+zero, the longest output tails are **0.475 s in segment 10** and **0.712 s in
+segment 35**. This is the added slew tail relative to the prior command, not
+the truck's physical release delay. It is a material behavior to inspect during
+controlled evaluation: more pull through capped turns may also add hanging
+on exit. Ordinary requests below the cap retain the original target mapping.
+
+The recorded model, vehicle motion, driver input and PSCM flags remain fixed.
+Replay cannot establish resulting tracking, centering, torque or stability.
+The route has no maneuver-plan messages; the replay uses the consumed model
+reference and recorded selected curvature. Computation time is approximated
+by control publication time, and full SubMaster health checks are unavailable.
+No device build, boot or installation is performed offline.
+
+## Reproduction
+
+Numeric results and source hashes are in `ford_c1_overflow_validation.json`.
+Use native project dependencies and pinned opendbc
+`c21a9013700734dd20b09e05aa68329ad8cc20f9`. The ca replay requires the existing
+full-rlog extract (`route.npz`, `model_paths.npz`, `metadata.json`).
+The following commands apply to this version; earlier validation documents
+record their named historical controllers.
+
+```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 opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_c1_overflow/stress.json
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260910 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_c1_overflow/zero_error_stress.json
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeca --baseline 959ae3d6e76c479f48e081c060b0f3569a6f15f4 --output .cache/ford_c1_overflow/routeca
+python openpilot/sunnypilot/sunnylink/tools/compile_settings_ui.py --check
+```
+
+For slew-tail analysis, in the replay's `commands.npz` find each nonzero run of
+`offset_overflow`. From its first zero sample, measure until packed candidate
+and baseline C0 agree within 1e-8 m, stopping separately at another overflow or
+inactive cycle. Sum sample durations capped at 30 ms for weighted time totals.

docs/ford_c1_overflow_validation.json

@@ -0,0 +1,307 @@
+{
+  "created_at_utc": "2026-09-10T22:04:22.589047+00:00",
+  "scope": "Base heading overflow allocated to C0; frozen-input command verification only, no vehicle response prediction.",
+  "baseline_commit": "959ae3d6e76c479f48e081c060b0f3569a6f15f4",
+  "baseline_source_sha256": "47fff1fd1bd7e65ca6d6b437fa9d2e8a864d421622d9efdfe0fe04b927c6d972",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c1-feedback-v3",
+  "calibration_approved": false,
+  "toggle": {
+    "key": "FordModelActionController",
+    "default_enabled": false,
+    "eligibility": "Any Ford CAN FD",
+    "activation": "Existing controlsd startup selection"
+  },
+  "command_rule": "extra C0 = 7 m * (raw base C1 - clip(raw base C1, -0.5, 0.5)); add to original model C0, then existing C0 amplitude/slew limits.",
+  "engineering_choices": "Single-reference linear allocation at existing 7 m. No new tuning parameter or stored overflow. Does not establish physical C0/C1 interchangeability. Existing 1:1 integral feedback strength remains.",
+  "output_limits": {
+    "c0_m": [
+      -5.11,
+      5.11
+    ],
+    "c1_rad": [
+      -0.5,
+      0.5
+    ],
+    "c0_slew_m_s": 4.0,
+    "c1_slew_rad_s": 0.5,
+    "c2": 0.0,
+    "c3": 0.0
+  },
+  "preserved": [
+    "Upstream reference selection/limiting",
+    "Driver and PSCM arbitration",
+    "Freshness/reset gates",
+    "C1 feedback law",
+    "Original model C0 required for carryover confirmation",
+    "100 Hz CAN sender",
+    "Float32 publication"
+  ],
+  "controller_command_state_values": 3,
+  "controller_diagnostic_counters": 1,
+  "controller_total_lines": 200,
+  "opendbc_submodule_changed": false,
+  "panda_safety_changed": false,
+  "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "tests": {
+    "combined_suite": "670 passed, 178 skipped, 9146 subtests passed in 5.77s",
+    "safety_skips": "Inherited or unsupported variants; unchanged from prior feedback validations.",
+    "new_core_regressions": "Before implementation: 28 failed, 18 passed; after: all 46 pass.",
+    "overflow_controlsd_to_can_cases": 12,
+    "overflow_controlsd_to_can_frames": 4800,
+    "integration_scope": "All six listed CAN FD platforms; both signs; selected upstream-limited request, release, limits, Float32 publication, decoded commands, zero C2/C3, active mode, counters, checksums.",
+    "ruff_changed_python": "pass",
+    "ty_controller": "pass",
+    "git_diff_check": "pass",
+    "settings_compiler_check": "pass"
+  },
+  "stress": {
+    "cycles": 200000,
+    "mirrored_updates": 200000,
+    "can_round_trips": 200000,
+    "carryover_release_count": 864,
+    "baseline_revision": "959ae3d6e76c479f48e081c060b0f3569a6f15f4",
+    "baseline_source_sha256": "47fff1fd1bd7e65ca6d6b437fa9d2e8a864d421622d9efdfe0fe04b927c6d972",
+    "exact_unchanged_state_and_commands_without_overflow": 56061,
+    "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, carryover direction/confirmation, integration, PSCM limits, CAN.",
+    "scope": "Numerical software invariants only; no model of vehicle motion.",
+    "calibration_approved": false
+  },
+  "zero_error_stress": {
+    "seed": 20260910,
+    "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.4000000000000019,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "route_ca": {
+    "baseline_revision": "959ae3d6e76c479f48e081c060b0f3569a6f15f4",
+    "baseline_source_sha256": "47fff1fd1bd7e65ca6d6b437fa9d2e8a864d421622d9efdfe0fe04b927c6d972",
+    "calibration_approved": false,
+    "cycles": 327448,
+    "active_cycles": 118756,
+    "validity_matches_baseline_exactly": true,
+    "status_counts": {
+      "inactive": 208692,
+      "active": 118756
+    },
+    "c0_matches_baseline_exactly": false,
+    "c0_changed_cycles": 855,
+    "max_abs_c0_change_m": 2.2,
+    "offset_overflow_seconds": 7.359375754000212,
+    "max_abs_offset_overflow_target_m": 2.867466852068901,
+    "feedback_enabled_seconds": 1128.448330694985,
+    "pscm_limit_2_seconds": 1.5229074359986043,
+    "c1_changed_cycles": 0,
+    "max_abs_c1_change_rad": 0.0,
+    "max_abs_correction_rad": 0.13238253764709199,
+    "can_round_trips": 327448,
+    "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+    "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+    "carryover_release_count": 32
+  },
+  "route_ca_release": {
+    "scope": "Describe candidate command tails on frozen ca measurements, not wheel response.",
+    "changed_c0_seconds": 8.606610047996583,
+    "overflow_target_seconds": 7.359375754000212,
+    "changed_c0_without_current_overflow_seconds": 1.4386431469993113,
+    "candidate_c0_at_cap_seconds": 2.054423716999736,
+    "baseline_c0_at_cap_seconds": 0.0,
+    "all_c1_c2_c3_match_baseline_exactly": true,
+    "all_pre_overflow_commands_match_baseline_exactly": true,
+    "all_inactive_commands_zero": true,
+    "windows": [
+      {
+        "start_s": 645.4515506280004,
+        "last_overflow_s": 647.9591778859995,
+        "duration_s": 2.5175176129996544,
+        "extra_target_peak_m": 2.867466852068901,
+        "c0_change_peak_m": 2.2,
+        "post_overflow_tail_s": 0.4753179760009516,
+        "tail_end_s": 648.444386217001,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 1896.7594062080007,
+        "last_overflow_s": 1897.0382758169999,
+        "duration_s": 0.29005605599923,
+        "extra_target_peak_m": 0.16229432076215744,
+        "c0_change_peak_m": 0.16999999999999993,
+        "post_overflow_tail_s": 0.0,
+        "tail_end_s": 1897.0494622639999,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 1897.3739526980007,
+        "last_overflow_s": 1897.4413651220002,
+        "duration_s": 0.0802515539999149,
+        "extra_target_peak_m": 0.049945808947086334,
+        "c0_change_peak_m": 0.04999999999999982,
+        "post_overflow_tail_s": 0.0,
+        "tail_end_s": 1897.4542042520006,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 1897.492552009,
+        "last_overflow_s": 1897.681751143,
+        "duration_s": 0.19981637000091723,
+        "extra_target_peak_m": 0.10685679316520691,
+        "c0_change_peak_m": 0.11000000000000032,
+        "post_overflow_tail_s": 0.01193945599879953,
+        "tail_end_s": 1897.7043078349998,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 1897.743499225,
+        "last_overflow_s": 1897.7842587169998,
+        "duration_s": 0.05174380599964934,
+        "extra_target_peak_m": 0.04502199590206146,
+        "c0_change_peak_m": 0.040000000000000036,
+        "post_overflow_tail_s": 0.011911696001334349,
+        "tail_end_s": 1897.807154727001,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 1897.9437203819998,
+        "last_overflow_s": 1899.2943476260007,
+        "duration_s": 1.361525778000214,
+        "extra_target_peak_m": 0.22848158329725266,
+        "c0_change_peak_m": 0.22999999999999954,
+        "post_overflow_tail_s": 0.021877274999496876,
+        "tail_end_s": 1899.3271234349995,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 2146.154050938001,
+        "last_overflow_s": 2146.185627021001,
+        "duration_s": 0.04039318899958744,
+        "extra_target_peak_m": 0.045987628400325775,
+        "c0_change_peak_m": 0.050000000000000266,
+        "post_overflow_tail_s": 0.034061254000334884,
+        "tail_end_s": 2146.228505381001,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 2146.2950925490004,
+        "last_overflow_s": 2146.5387542179997,
+        "duration_s": 0.25293986199903884,
+        "extra_target_peak_m": 0.21167446672916412,
+        "c0_change_peak_m": 0.17999999999999972,
+        "post_overflow_tail_s": 0.040063559001282556,
+        "tail_end_s": 2146.5880959700007,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 2146.6577708509994,
+        "last_overflow_s": 2148.3543101739997,
+        "duration_s": 1.7074812410010054,
+        "extra_target_peak_m": 2.0555079206824303,
+        "c0_change_peak_m": 1.58,
+        "post_overflow_tail_s": 0.7118495689992415,
+        "tail_end_s": 2149.0771016609997,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 3121.067818462001,
+        "last_overflow_s": 3121.8665919270006,
+        "duration_s": 0.8071899679998751,
+        "extra_target_peak_m": 0.5380096957087517,
+        "c0_change_peak_m": 0.5099999999999998,
+        "post_overflow_tail_s": 0.06879738699899463,
+        "tail_end_s": 3121.943805817,
+        "tail_ended_by": "matches baseline"
+      },
+      {
+        "start_s": 3122.0664172689994,
+        "last_overflow_s": 3122.105900957,
+        "duration_s": 0.05046031700112508,
+        "extra_target_peak_m": 0.008371405303478241,
+        "c0_change_peak_m": 0.010000000000000231,
+        "post_overflow_tail_s": 0.02107719199921121,
+        "tail_end_s": 3122.1379547779998,
+        "tail_ended_by": "matches baseline"
+      }
+    ],
+    "example_points": [
+      {
+        "segment": 10,
+        "time_s": 646.7503591629993,
+        "baseline_c0_m": 3.9000000000000004,
+        "candidate_c0_m": 5.11,
+        "extra_c0_target_m": 2.867466852068901,
+        "baseline_c1_rad": 0.5,
+        "candidate_c1_rad": 0.5
+      },
+      {
+        "segment": 31,
+        "time_s": 1898.0837712450011,
+        "baseline_c0_m": -2.8499999999999996,
+        "candidate_c0_m": -3.079999999999999,
+        "extra_c0_target_m": -0.22848158329725266,
+        "baseline_c1_rad": -0.5,
+        "candidate_c1_rad": -0.5
+      },
+      {
+        "segment": 35,
+        "time_s": 2147.4098699660008,
+        "baseline_c0_m": 3.67,
+        "candidate_c0_m": 5.11,
+        "extra_c0_target_m": 2.0555079206824303,
+        "baseline_c1_rad": 0.5,
+        "candidate_c1_rad": 0.5
+      },
+      {
+        "segment": 52,
+        "time_s": 3121.239466367,
+        "baseline_c0_m": 3.11,
+        "candidate_c0_m": 3.42,
+        "extra_c0_target_m": 0.5380096957087517,
+        "baseline_c1_rad": 0.5,
+        "candidate_c1_rad": 0.5
+      }
+    ]
+  },
+  "route_ca_input_sha256": {
+    "route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+    "model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+    "metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525"
+  },
+  "lab_can_round_trips_excluding_integration": 745586,
+  "validation_environment": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps",
+    "PYTHONDONTWRITEBYTECODE": "1",
+    "LOG_ROOT": "/private/tmp/ford-overflow-logs",
+    "PARAMS_ROOT": "/private/tmp/ford-overflow-params"
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "90269d748d55558bf2495d3b4afcfd7429f373108df1f9259e154d1b92184262",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py": "9086434d2b76ab51f69ef08c4f0033c4eaa1950083cc9cce4b34279eb17c5b1b",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "dc552f3088b7e437a928f349989b74c4e7772d2a88b14a93e933ea8b8344d23b",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "4e82101463c5d83f6b1f72ba4918c2b37731bc2db6ec7e29e6b402ea67276db2",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "6b73e70b120527275a9e4e3dff07dd7d19648da517402187d57a90ba09b017eb",
+    "tools/ford_pscm_lab/feedback_replay.py": "7c65515d37aac900eaaef8451a7d643b72c566a2366651f843de2ffca5f24b8d",
+    "tools/ford_pscm_lab/stress_model_action.py": "cec2619285dd41274562ac035ee8ea0a389269a0c4ef1b62efa6252ad1a714aa",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "3ae6b16a8c267ab181e00f4b59be8b65134480bec26c8c039bcbae88cb745feb",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "ba803819751b304e82936a95902afae63ff88e16e64988622715a7b713f3b982"
+  },
+  "limitations": [
+    "Recorded vehicle motion does not react to changed commands.",
+    "Computation time proxies and service-check reconstruction limits apply.",
+    "C0 slew can leave extra command after overflow stops; maximum observed tail 0.712 s.",
+    "No device build, boot, installation, road tracking or physical stability validation."
+  ]
+}

docs/ford_c1_p75_trial.md

@@ -0,0 +1,98 @@
+# Ford C1 proportional trial: P=0.75
+
+Route 149 contains large steering shortfalls before driver intervention while
+the selected curvature matches the model and neither C1's bound nor the PSCM
+reached-limit flag explains the shortfall. This trial raises the immediate C1
+error correction from P=0.50 to P=0.75. I remains 0.25. Runtime changes are the
+gain constant and the diagnostic version, `model-action-curvature-c0-distance-pi-v13`.
+
+The intended effect is more correction while behind and more release correction
+when measured steering exceeds the request. Increasing P does not establish a
+faster physical response: it can also amplify measurement fluctuations and
+produce oscillation. This is a trial coefficient, not a learned calibration.
+
+The selected model action, +0.40 s low-speed preview, C0 distance setting and
+formula, integral arithmetic, PSCM arbitration, field bounds, and 100 Hz sender
+remain unchanged. C2/C3 stay zero. The existing default-off Sunnylink toggle
+still selects the experiment on Ford CAN FD; toggle-off selects upstream Ford.
+
+## Paired production replay
+
+Compared explicit P=0.50 and P=0.75 production adapters with I=0.25 and fixed-7 m
+C0 across 21 route extracts: 112–117, 119, 11a, 120, 124, 125, 146, 149, a0, a2,
+a5, a9, b8, b9, ca, and Raptor 02. The passes cover 2,275,248 source cycles and
+4,550,496 real Float32-to-CAN encode/decode round trips.
+
+Both passes use the same recorded selected curvature, measured motion, model
+geometry, input timestamps, and driver/PSCM flags. Older routes retain their
+original model requests; their neural inference is not rerun with the new delay.
+This compares commands, not predicted wheel motion or tracking accuracy.
+
+Checks passed on every cycle: identical C0, eligibility, feedforward, overflow,
+and feedback/driver/PSCM gates; finite and bounded output; inactive zero output;
+zero C2/C3; exact decoded fields, mode and counter; and the expected 1.5 ratio
+between proportional terms. Integration tests separately check the selected
+defaults, downstream checksums, reference selection, reversals, driver override,
+reached-limit behavior, duplicate measurements, and toggle-off upstream behavior.
+
+On route 149, the P=0.50 replay agrees with the recorded path commands over the
+clean scoring cohort to Float32 precision: maximum C0 difference 5.8e-8 m and C1
+difference 1.5e-8 rad. Full SubMaster health and exact control execution clocks
+are not in the extract; publication timestamps approximate them. Historical
+versions used different command laws, so their recorded commands are not
+expected to match this baseline.
+
+Clean scoring excludes driver steering, unavailable feedback, inactive/invalid
+control, the following second, and speed below 3 mph. It contains 11,128.80 s.
+Durations use original timestamps, clipping gaps to 30 ms. Percentiles are
+sample-based. Request-angle categories do not identify road geometry.
+
+| Recorded request magnitude | Scored seconds | Mean absolute C1 change | P95 C1 change |
+| --- | ---: | ---: | ---: |
+| Under 10 degrees | 9,051.37 | 0.00088 rad | 0.00250 rad |
+| 10–45 degrees | 1,631.68 | 0.00250 rad | 0.00900 rad |
+| At least 45 degrees | 445.75 | 0.00829 rad | 0.03100 rad |
+
+C1 bound exposure increases from 21.30 to 22.67 s over the clean cohort. Mean
+absolute stored I changes from 0.005987 to 0.005984 rad; a larger P term changes
+the remaining accumulation headroom even though I's gain is unchanged.
+
+The largest small-request C1 difference is 0.093 rad on route 117, where the
+recorded request is +4.6 degrees and the wheel is still at -210 degrees. This is
+a large release error, not ordinary centering. Restricting both requested and
+actual wheel angle to within 10 degrees leaves 8,788.38 s: mean absolute C1
+change 0.00073 rad, P95 0.00200 rad, maximum 0.01250 rad. These measurements do not
+establish preserved centering or closed-loop stability.
+
+In route 149, the candidate increases the C1 request at the reviewed entry
+misses and reduces the remaining turn command during the clean segment-14
+release. Its clean C1 bound exposure rises from 0.23 to 0.61 s. The paired I
+traces remain nearly identical. The local report includes five entry, reversal,
+and exit comparisons with the recorded wheel trace clearly distinguished from
+replayed command traces.
+
+## Validation and reproduction
+
+455 tests and 25 subtests pass, including Ford controller/adapter/selection,
+C0 distance settings, diagnostic logging, delay helpers, and Ford CAN tests.
+The actual controlsd-to-CAN integration test failed at the old proportional
+output before changing the default, then passed at the new setting. Ruff and
+`git diff --check` pass. A device build, installation, and physical evaluation
+are not part of these offline checks.
+
+The compact evidence record is [ford_c1_p75_validation.json](ford_c1_p75_validation.json).
+Full command arrays and per-route reports are in `.cache/ford_p75_trial` locally.
+Reproduce one route using the built cereal/opendbc environment:
+
+```sh
+PYTHONPATH=.:opendbc_repo PYTHONDONTWRITEBYTECODE=1 python \
+  tools/ford_pscm_lab/proportional_replay.py \
+  --routes 149=.cache/ford_route149/full \
+  --output .cache/ford_p75_recheck --workers 1
+```
+
+Additional `label=extract-directory` pairs replay independently. Each directory
+must contain `route.npz`, `model_paths.npz`, and `metadata.json`; injection routes
+are rejected. The input hashes are recorded in each result. The new test is
+worth evaluating as a bounded change, but improved entry and preserved smooth
+release still require measured vehicle response.

docs/ford_c1_p75_validation.json

@@ -0,0 +1,707 @@
+{
+  "routes": 21,
+  "cycles": 2275248,
+  "can_round_trips": 4550496,
+  "scope": "Fixed-motion production controller and CAN replay. No predicted wheel motion or stability claim.",
+  "cohorts": {
+    "all": {
+      "seconds": 11128.798286258982,
+      "mean_abs_c1_change": 0.0014142689534451223,
+      "p95_abs_c1_change": 0.004500000000000004,
+      "max_abs_c1_change": 0.09299999999999997,
+      "c1_bound_seconds": [
+        21.296853938993763,
+        22.66821589999995
+      ],
+      "mean_abs_integral": [
+        0.00598675347437431,
+        0.005984439210041702
+      ]
+    },
+    "small": {
+      "seconds": 9051.37332302302,
+      "mean_abs_c1_change": 0.0008801267467942443,
+      "p95_abs_c1_change": 0.0025000000000000022,
+      "max_abs_c1_change": 0.09299999999999997,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "mean_abs_integral": [
+        0.005708567029833155,
+        0.005708567029833155
+      ]
+    },
+    "bend": {
+      "seconds": 1631.677856559975,
+      "mean_abs_c1_change": 0.0024991973101179256,
+      "p95_abs_c1_change": 0.008999999999999897,
+      "max_abs_c1_change": 0.08899999999999997,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "mean_abs_integral": [
+        0.006396022651557497,
+        0.006396128425459091
+      ]
+    },
+    "turn": {
+      "seconds": 445.74710667598833,
+      "mean_abs_c1_change": 0.008289169314090508,
+      "p95_abs_c1_change": 0.031000000000000028,
+      "max_abs_c1_change": 0.08000000000000002,
+      "c1_bound_seconds": [
+        21.296853938993763,
+        22.66821589999995
+      ],
+      "mean_abs_integral": [
+        0.01013747903479613,
+        0.010079312488152544
+      ]
+    },
+    "releasing": {
+      "seconds": 106.96494071107061,
+      "mean_abs_c1_change": 0.005058356779798081,
+      "p95_abs_c1_change": 0.025500000000000023,
+      "max_abs_c1_change": 0.08899999999999997,
+      "c1_bound_seconds": [
+        0.009879735000026812,
+        0.0
+      ],
+      "mean_abs_integral": [
+        0.006354435594257913,
+        0.006350676023352573
+      ]
+    }
+  },
+  "baseline_commit": "b720e9f1bb805c69e5560f13dd86034d4bf50260",
+  "gains": [
+    0.5,
+    0.75
+  ],
+  "integral_gain": 0.25,
+  "c0_time_based": false,
+  "hypothesis": "model-action-curvature-c0-distance-pi-v13",
+  "tests": {
+    "passed": 455,
+    "subtests_passed": 25,
+    "ruff": "passed",
+    "diff_check": "passed"
+  },
+  "replay_command_invariants_passed": true,
+  "physical_tracking_or_stability_validated": false,
+  "route_results": [
+    {
+      "route": "112",
+      "cycles": 108971,
+      "can_round_trips": 217942,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.0679107408662434,
+          "p95": 0.26000000476837126,
+          "max": 3.119999904632568
+        },
+        "c1": {
+          "mean": 0.005686226432883118,
+          "p95": 0.014999999850988377,
+          "max": 0.18949999547004703
+        }
+      },
+      "clean_seconds": 732.0411244650002,
+      "c1_bound_seconds": [
+        0.0,
+        0.021201738000058867
+      ],
+      "source_sha256": {
+        ".cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        ".cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "113",
+      "cycles": 49614,
+      "can_round_trips": 99228,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.1281940820409913,
+          "p95": 0.897999979019158,
+          "max": 3.260000095367432
+        },
+        "c1": {
+          "mean": 0.006052000019791209,
+          "p95": 0.033500000965595265,
+          "max": 0.13200000381469723
+        }
+      },
+      "clean_seconds": 212.64164745499875,
+      "c1_bound_seconds": [
+        1.2653679959998954,
+        1.2653679959998954
+      ],
+      "source_sha256": {
+        ".cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        ".cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        ".cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "114",
+      "cycles": 61027,
+      "can_round_trips": 122054,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.11075714097529091,
+          "p95": 0.3999999904632565,
+          "max": 3.500000133514404
+        },
+        "c1": {
+          "mean": 0.006464089004288734,
+          "p95": 0.01899999927878381,
+          "max": 0.17200000238418578
+        }
+      },
+      "clean_seconds": 336.37317285400025,
+      "c1_bound_seconds": [
+        0.26486993000000325,
+        0.26486993000000325
+      ],
+      "source_sha256": {
+        ".cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        ".cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        ".cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "115",
+      "cycles": 40037,
+      "can_round_trips": 80074,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.08961574629129629,
+          "p95": 0.3500000071525573,
+          "max": 2.7699999141693112
+        },
+        "c1": {
+          "mean": 0.006259665157786696,
+          "p95": 0.0225000038146973,
+          "max": 0.10550000000000004
+        }
+      },
+      "clean_seconds": 213.9659020539998,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        ".cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        ".cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "116",
+      "cycles": 68793,
+      "can_round_trips": 137586,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.1229211861673407,
+          "p95": 0.40999998092651335,
+          "max": 3.539999895095825
+        },
+        "c1": {
+          "mean": 0.00492455679344663,
+          "p95": 0.01600000357627862,
+          "max": 0.12550000327825545
+        }
+      },
+      "clean_seconds": 286.72210779700015,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
+        ".cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
+        ".cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "117",
+      "cycles": 27301,
+      "can_round_trips": 54602,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.1466742382755,
+          "p95": 0.6379999847412033,
+          "max": 4.270000057220459
+        },
+        "c1": {
+          "mean": 0.00582792986837905,
+          "p95": 0.024499997889995594,
+          "max": 0.05100000405311583
+        }
+      },
+      "clean_seconds": 97.48647941900026,
+      "c1_bound_seconds": [
+        0.9928339700004472,
+        1.080442829000276
+      ],
+      "source_sha256": {
+        ".cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
+        ".cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
+        ".cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "119",
+      "cycles": 171423,
+      "can_round_trips": 342846,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.06276063585086782,
+          "p95": 0.22999999165535012,
+          "max": 4.129999866485596
+        },
+        "c1": {
+          "mean": 0.00042448693421451755,
+          "p95": 0.0024999996423721305,
+          "max": 0.035999999776482605
+        }
+      },
+      "clean_seconds": 616.4493080259994,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_route119/route.npz": "415a099935fef152123b7422870442d6cfe6302bab9b8983ce3b3ffc71a7702b",
+        ".cache/ford_route119/model_paths.npz": "dfd41383bea486ccd0a476e94e612921a44a099a0c1213ff132ab81df3aa94d7",
+        ".cache/ford_route119/metadata.json": "d61bd3351912f2f1a3c51f6061beffe89b430ebc50178c35abff8d1ecc1fc04d",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "11a",
+      "cycles": 235771,
+      "can_round_trips": 471542,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.060649023224496834,
+          "p95": 0.17000000953674288,
+          "max": 3.7
+        },
+        "c1": {
+          "mean": 0.00018421321567653392,
+          "p95": 0.0005000000949949013,
+          "max": 0.045000003814697265
+        }
+      },
+      "clean_seconds": 1085.65972405699,
+      "c1_bound_seconds": [
+        0.0811529149996204,
+        0.0811529149996204
+      ],
+      "source_sha256": {
+        ".cache/ford_route11a/route.npz": "428c203d090731386f06bf9fdeefe608f1999c43c52ff39cab7ee86ec0ac804f",
+        ".cache/ford_route11a/model_paths.npz": "e2b827fd7c3dfbf66d7872a56b57eaec13c600a9a48e12dae59be81c31b0978f",
+        ".cache/ford_route11a/metadata.json": "b7b10ccb1c0fa740779a1cf977faa4ae6714c9f9d7a0ffc39f3684680c16ae2a",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "120",
+      "cycles": 56704,
+      "can_round_trips": 113408,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.07882976760905562,
+          "p95": 0.3099999999999996,
+          "max": 3.3499999046325684
+        },
+        "c1": {
+          "mean": 6.576444779324904e-10,
+          "p95": 3.218650845848714e-09,
+          "max": 1.478195188475695e-08
+        }
+      },
+      "clean_seconds": 325.62925950599976,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_route120/route.npz": "ca40b52257af313ed0abd1109a33115a0a3227bd3438367faf282467e807a34a",
+        ".cache/ford_route120/model_paths.npz": "9b4a21357de4076c555e8776ed36b1d5728da1307d739268a3f84e84593e12b4",
+        ".cache/ford_route120/metadata.json": "110fe2016561722743064f2619e32f1b69164cc86015ef01c0a9fd2dd7f24bba",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "124",
+      "cycles": 5820,
+      "can_round_trips": 11640,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 2.909556870453723e-10,
+          "p95": 6.70552502413102e-10,
+          "max": 4.76837147544984e-09
+        },
+        "c1": {
+          "mean": 2.2964119221745972e-10,
+          "p95": 8.344650526126429e-10,
+          "max": 3.3974647539736225e-09
+        }
+      },
+      "clean_seconds": 22.732139372000063,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_c0_response_124_125/route124/route.npz": "26f8cf284e54c5f80bc7414c1302c429d4c10d54147e137fc5b8a348249e4fe5",
+        ".cache/ford_c0_response_124_125/route124/model_paths.npz": "898d75cc80f29fc50a818c29614961ca8fdf56d1e8908993662c6e92e54ebf6f",
+        ".cache/ford_c0_response_124_125/route124/metadata.json": "8f93b186178c89a95e56177dda7f0aba740bc57dfb832cde6d5c3fd3f43a8126",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "125",
+      "cycles": 41717,
+      "can_round_trips": 83434,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 3.143796501787139e-10,
+          "p95": 1.3411041166477844e-09,
+          "max": 4.76837147544984e-09
+        },
+        "c1": {
+          "mean": 0.001590841778388488,
+          "p95": 0.004000000216066835,
+          "max": 0.0040000033974647575
+        }
+      },
+      "clean_seconds": 400.6387703520005,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_c0_response_124_125/route125/route.npz": "8dcf304871bf5c6bf1ce206c788d7034bf5f056522faded0aa7948ff1cccbb75",
+        ".cache/ford_c0_response_124_125/route125/model_paths.npz": "4be9c7809dabbcb52296ba93b4743146dc941e49a32aca07f008b75757703e49",
+        ".cache/ford_c0_response_124_125/route125/metadata.json": "1f2b812d3c2cc8e5a2246098756940d6f8989a2591daf4d74b1ef4c8555e2d57",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "146",
+      "cycles": 53229,
+      "can_round_trips": 106458,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 3.47655752720815e-10,
+          "p95": 1.3411041166477844e-09,
+          "max": 2.8610229740877458e-08
+        },
+        "c1": {
+          "mean": 2.782228297198912e-10,
+          "p95": 1.19209286886246e-09,
+          "max": 1.3351440397713077e-08
+        }
+      },
+      "clean_seconds": 388.4780471659999,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_model_replay_146/full/route.npz": "b01db5bdd1c3b9fb83418a298571c98e60b5eb0509800d86cdaf07bafcdc6d63",
+        ".cache/ford_model_replay_146/full/model_paths.npz": "11e0e9c8ce2959aac72d2f8a9cc4f6a025480034d5cd8b59b68a41b2ae3cf4a8",
+        ".cache/ford_model_replay_146/full/metadata.json": "2be1254dd1eee422f3b2c2011faa91a21ab431751998525105381c2886df15ca",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "149",
+      "cycles": 132334,
+      "can_round_trips": 264668,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 1.604709350643841e-09,
+          "p95": 7.152557657263969e-09,
+          "max": 5.7220459037665705e-08
+        },
+        "c1": {
+          "mean": 6.774543859790247e-10,
+          "p95": 3.0994415922691587e-09,
+          "max": 1.478195188475695e-08
+        }
+      },
+      "clean_seconds": 868.9707798709996,
+      "c1_bound_seconds": [
+        0.2315784860001031,
+        0.6131428530001415
+      ],
+      "source_sha256": {
+        ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
+        ".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
+        ".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "a0",
+      "cycles": 114582,
+      "can_round_trips": 229164,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.02570244338167474,
+          "p95": 0.08000000059604684,
+          "max": 1.4500001144409183
+        },
+        "c1": {
+          "mean": 0.01759150615156611,
+          "p95": 0.0444999985605478,
+          "max": 0.1095000026226044
+        }
+      },
+      "clean_seconds": 926.827608547,
+      "c1_bound_seconds": [
+        7.083908395000037,
+        7.184738287000073
+      ],
+      "source_sha256": {
+        ".cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        ".cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
+        ".cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "a2",
+      "cycles": 71111,
+      "can_round_trips": 142222,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.04191847719972587,
+          "p95": 0.11000001072883592,
+          "max": 2.5599999904632567
+        },
+        "c1": {
+          "mean": 0.018344418527189366,
+          "p95": 0.06350000252127648,
+          "max": 0.10950000333786014
+        }
+      },
+      "clean_seconds": 362.6477764930001,
+      "c1_bound_seconds": [
+        4.459983808000175,
+        4.459983808000175
+      ],
+      "source_sha256": {
+        ".cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        ".cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
+        ".cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "a5",
+      "cycles": 38961,
+      "can_round_trips": 77922,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.0678814615486646,
+          "p95": 0.23999999046325726,
+          "max": 2.549999809265137
+        },
+        "c1": {
+          "mean": 0.010050927104249109,
+          "p95": 0.02549999953061341,
+          "max": 0.11449999988079068
+        }
+      },
+      "clean_seconds": 281.78537111799994,
+      "c1_bound_seconds": [
+        3.117821645999925,
+        3.1221818889999327
+      ],
+      "source_sha256": {
+        ".cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        ".cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
+        ".cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "a9",
+      "cycles": 286319,
+      "can_round_trips": 572638,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.0574873679806263,
+          "p95": 0.24000002861022907,
+          "max": 1.8600000095367433
+        },
+        "c1": {
+          "mean": 0.012787646152435335,
+          "p95": 0.04100000113248825,
+          "max": 0.20099999380111694
+        }
+      },
+      "clean_seconds": 1231.1594245750002,
+      "c1_bound_seconds": [
+        0.945385510000051,
+        1.1985475469998619
+      ],
+      "source_sha256": {
+        ".cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
+        ".cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
+        ".cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "b8",
+      "cycles": 160431,
+      "can_round_trips": 320862,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.09075655656311847,
+          "p95": 0.3199999809265135,
+          "max": 3.8600001049041746
+        },
+        "c1": {
+          "mean": 0.014587774875948546,
+          "p95": 0.047500000029802314,
+          "max": 0.2005000000223518
+        }
+      },
+      "clean_seconds": 465.3759713629988,
+      "c1_bound_seconds": [
+        1.233373995994043,
+        1.536421163000341
+      ],
+      "source_sha256": {
+        ".cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+        ".cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+        ".cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "b9",
+      "cycles": 90774,
+      "can_round_trips": 181548,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.0779309579021297,
+          "p95": 0.3100000107288361,
+          "max": 4.5400001907348635
+        },
+        "c1": {
+          "mean": 0.007145982184135228,
+          "p95": 0.020500003129243882,
+          "max": 0.14649999999999996
+        }
+      },
+      "clean_seconds": 680.5921192759997,
+      "c1_bound_seconds": [
+        0.7494592899999759,
+        0.9690469479999706
+      ],
+      "source_sha256": {
+        ".cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        ".cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        ".cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "ca",
+      "cycles": 327448,
+      "can_round_trips": 654896,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.015992715429034462,
+          "p95": 0.05000000029802365,
+          "max": 1.0699999809265135
+        },
+        "c1": {
+          "mean": 0.0030197859214003767,
+          "p95": 0.00799999958276748,
+          "max": 0.10800000715255731
+        }
+      },
+      "clean_seconds": 999.6971771089911,
+      "c1_bound_seconds": [
+        0.871117996999601,
+        0.871117996999601
+      ],
+      "source_sha256": {
+        ".cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+        ".cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+        ".cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    },
+    {
+      "route": "raptor02",
+      "cycles": 132881,
+      "can_round_trips": 265762,
+      "baseline_difference_from_recorded_path": {
+        "c0": {
+          "mean": 0.01159572992270899,
+          "p95": 0.06918421261012586,
+          "max": 0.39572428941726656
+        },
+        "c1": {
+          "mean": 0.017327589689457763,
+          "p95": 0.04501086750242392,
+          "max": 0.10240508272871374
+        }
+      },
+      "clean_seconds": 592.9243753840037,
+      "c1_bound_seconds": [
+        0.0,
+        0.0
+      ],
+      "source_sha256": {
+        ".cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
+        ".cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
+        ".cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
+        "tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
+        "openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
+      }
+    }
+  ],
+  "selected_runtime_source_sha256": "a653b9c16ff8e5ac91e7c1751eaeb7a77b50701d185ce426fe1074d1a58a07b4",
+  "opendbc_commit": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a"
+}

docs/ford_c1_pi.md

@@ -0,0 +1,151 @@
+# Ford C1 proportional feedback trial
+
+V6 adds **P = 0.25** to the selected-action controller. It responds to a steering
+shortfall immediately and subtracts demand immediately when the wheel exceeds
+the selected request. V5 accumulated correction over traveled distance. P has
+no stored correction to release when its error disappears.
+
+This is an initial drive-trial gain, not an identified PSCM calibration or a
+claim of improved physical tracking. Six Lightning routes establish command
+behavior across recorded scenarios. They cannot identify the best stable gain
+without observing the vehicle responding to the changed commands.
+
+## Command law
+
+With curvature in inverse meters, speed in meters per second and heading in radians:
+
+```text
+D = max(7 m, speed × 1 s)
+error = selected_limited_curvature - measured_steering_curvature
+base_C1 = clip(D × selected_limited_curvature, -0.5, +0.5)
+P = 0.25 × D × error
+I_increment = speed × error × fresh_measurement_elapsed_time
+C1 = amplitude_and_slew_limit(base_C1 + P + I)
+```
+
+P is 25% of the heading-equivalent tracking error, not a 25% multiplier on the
+model request. At matched curvature it is zero. It is stateless and can change
+with a new request even if a steering publication repeats; repeated steering
+publications still cannot integrate I twice. Driver override and fresh PSCM
+denied/inactive states clear both feedback terms. Fresh `limit=2` inhibits
+outward I accumulation while permitting unwind; P remains available inside the
+existing combined output envelope.
+
+The existing C1 amplitude limit (±0.5 rad) and slew (0.5 rad/s) apply to the sum.
+Anti-windup includes P when calculating I's available headroom. P can consume
+a slew interval that previously allowed I accumulation. The conditional I
+release rules, including [completed-unwind release](ford_unwind_catchup.md),
+remain. C0 retains the same 7 m mapping, base-heading overflow, cap and slew;
+neither P nor I spills into C0. C2/C3 stay zero. No plant, gain schedule or
+automatic gain learning is introduced.
+
+Onroad selection explicitly supplies `C1_PROPORTIONAL_GAIN = 0.25`. Direct
+`FordModelActionController()` and `ModelActionController()` construction defaults
+to zero P for v5 reference/replay compatibility. The existing default-off
+Sunnylink toggle selects v6 on any Ford CAN FD. Toggle off still selects
+upstream Ford control. See [installation and selection](ford_model_action_drive_test.md).
+
+## Lightning replay findings
+
+Routes `112`, `113`, `114`, `115`, `b9` and `ca` supplied 677,871 control cycles.
+Each was replayed with P gains 0, 0.1, 0.25 and 0.5, paired with diagnostic
+feedback delays 0, 0.2 and 0.4 s: 12 combinations and 8,134,452 candidate updates.
+Recorded model, driver, steering and PSCM inputs stayed fixed.
+
+Both command columns are replayed C1 in radians with left positive. The angle
+pair is the single recorded desired/actual wheel measurement, not a predicted
+outcome for either candidate.
+
+| Example | Desired / actual angle | V5 C1 | P=0.25 C1 |
+| --- | ---: | ---: | ---: |
+| 115, 207.908 s: late left entry | 94.2° / 51.2° | +0.1685 | +0.1825 |
+| 115, 208.099 s: entry continues | 111.0° / 72.8° | +0.2105 | +0.2245 |
+| 114, 473.086 s: well-tracked bend | 57.3° / 56.3° | +0.1855 | +0.1850 |
+| 113, 481.567 s: hanging right exit | −7.6° / −94.4° | +0.0645 | +0.0875 |
+| 115, 133.595 s: completed unwind | 6.0° / 29.9° | +0.0105 | 0.0000 |
+
+The completed-unwind example is excluded by the original quality/driver clean
+mask. It is useful for checking command release, not autonomous tracking
+attribution. Large-turn windows often contain interventions and require review
+of driver input before assigning a tracking result to the controller.
+
+Across 2,740.44 seconds of valid, feedback-enabled, clean samples with desired
+wheel angle below 30°, the duration-weighted mean absolute C1 change is
+0.001001 rad at P=0.25, versus 0.001961 at P=0.5. Per-route 95th-percentile
+changes at P=0.25 are 0.0025–0.0070 rad; the largest ordinary-cohort change is
+0.0350 rad. Small average command changes do not establish unchanged centering
+or stability.
+
+P=0.25 is an engineering choice between the tested smaller and larger responses,
+not an optimization result. At the late-entry example, adding a fixed 0.4 s
+feedback delay instead gives C1 +0.1420 rad. Across the ordinary cohort, that
+delayed P=0.25 variant changes C1 by 0.011255 rad on average. V6 therefore
+retains v5's feedback timing to isolate P. This does not identify or disprove
+the vehicle's physical delay. The diagnostic delay variants change only P and
+I integration targets; request-release decisions still use the current request.
+
+## Tuning and next-drive evidence
+
+Comma's [torque controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_torque.py)
+separates feedforward, P and I and aligns its torque feedback reference with
+steering delay. Its [angle PID controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_pid.py)
+uses desired-minus-measured steering angle directly. These different paths do
+not imply one delay setting should be copied into Ford C1.
+
+Use the same discipline: explicit parameters, separate term logging, fixed
+request conditions and measured response. Comma's
+[lateral maneuver report](https://blog.comma.ai/0111release/#lateral-maneuver-report)
+uses repeatable step/sine maneuvers to assess response. C1 is a path-heading
+request to another controller, not normalized steering torque; numerical torque
+gains and torque calibration cannot be copied across.
+
+For the next controlled evaluation, compare similar speeds and model requests:
+entry delay/shortfall, overshoot as the request relaxes, correction after catch-up,
+ordinary-bend centering and oscillation. Keep desired/actual tracking on original
+timestamps. Check C0/C1 caps, slew, driver input and fresh PSCM flags separately.
+More gain cannot remove hardware limits and can introduce oscillation. These
+logs all come from a Lightning; the gain is not yet validated across other
+PSCMs. No scripted maneuver mode is enabled by this change.
+
+Periodic `Ford C2-free path tracking` events identify
+`hypothesis=model-action-c1-pi-v6` and expose `heading_proportional`,
+`proportional_gain`, `feedback_curvature` and `feedback_error` alongside
+`heading_feedforward`, `heading_correction`, command and release diagnostics.
+`calibration_approved=false` remains.
+
+## Validation and reproduction
+
+The final selected path exactly matches the sweep's P=0.25, zero-delay variant
+on all six routes, including C0/C1, P, I and activation. Zero-P/zero-delay matches
+v5 exactly on every cycle. All variants preserve C0 and activation. Another
+200,000 seeded stress cycles check PI arithmetic, anti-windup, mirror symmetry,
+driver/PSCM arbitration, resets, amplitude/slew and zero-P parity. Sweep,
+selected replay and stress total **9,012,323 Float32/CAN round trips**.
+Encoding checks do not test vehicle motion.
+
+**776 tests and 9,145 subtests passed; 178 were skipped.** Coverage includes
+actual startup selection, controlsd request source/limiting, Float32 publication,
+100 Hz CAN encoding, checksums, both turn signs, integral release, Sunnylink
+persistence, toggle-off upstream behavior and Ford safety tests. Ruff,
+controller Ty and settings compilation passed. A hardware build/device boot
+and physical response tests have not been performed.
+
+Use the project's Python environment and built cereal/opendbc dependencies:
+
+```sh
+export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps
+export PYTHONDONTWRITEBYTECODE=1
+export PARAMS_ROOT=/tmp/ford-pi-test-params
+export LOG_ROOT=/tmp/ford-pi-test-logs
+python -m tools.ford_pscm_lab.pi_replay .cache/ford_route115 --output .cache/ford_pi_sweep/route115
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route115 --baseline 22d188776cb557acea459a1fca70812bdb2df46c --output .cache/ford_pi_sweep/selected115
+python -m tools.ford_pscm_lab.pi_stress --cycles 200000 --gain .25 --output .cache/ford_pi_sweep/stress.json
+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 opendbc_repo/opendbc/safety/tests/test_ford.py
+```
+
+Repeat both replay commands for the other five extracts. The machine-readable
+[validation record](ford_c1_pi_validation.json) records counts, source hashes,
+cohort definitions and sampled command changes. Publication time proxies the
+computation clock; full SubMaster state and selected maneuver-plan publications
+are not reconstructed. Real maneuver source selection is exercised in integration
+tests. Historical controller reports retain their original version scope.

docs/ford_c1_pi_validation.json

@@ -0,0 +1,373 @@
+{
+  "hypothesis": "model-action-c1-pi-v6",
+  "baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c",
+  "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+  "selected_gain": 0.25,
+  "selected_feedback_delay_s": 0.0,
+  "calibration_approved": false,
+  "scope": "Frozen recorded steering, model, driver and PSCM inputs. Command checks only; no predicted wheel response or physical tracking improvement score.",
+  "controller_sha256": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+  "routes": {
+    "112": {
+      "route": "84865544361f55cb_00000112--ec2edd4afc",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 108971,
+      "candidate_updates": 1307652,
+      "sweep_can_round_trips": 1307652,
+      "selected_can_round_trips": 108971,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 655.2228206790003,
+      "ordinary_mean_abs_c1_change_rad": 0.0008885702173989647,
+      "ordinary_p95_abs_c1_change_rad": 0.0025000000000000022,
+      "ordinary_max_abs_c1_change_rad": 0.02400000000000002,
+      "all_valid_max_abs_c1_change_rad": 0.054500000000000104,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/angles.npz": "9429813237b4988dd99a1461d1a9bd6f6d3056693a93a5dd1ec63567d19f2aa1"
+      },
+      "sweep_report_sha256": "413e844d20d7591d32d95b1d48a14db11fa089800315cb899e9f5a32a3e630f5",
+      "selected_report_sha256": "ab4ced7ec6b628ac4a941fb968e6e18d06f9c616f8704f1a6abd0081a4fb0e65"
+    },
+    "113": {
+      "route": "84865544361f55cb_00000113--3947b0487c",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 49614,
+      "candidate_updates": 595368,
+      "sweep_can_round_trips": 595368,
+      "selected_can_round_trips": 49614,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 187.8263240149995,
+      "ordinary_mean_abs_c1_change_rad": 0.0015004664599222238,
+      "ordinary_p95_abs_c1_change_rad": 0.007000000000000006,
+      "ordinary_max_abs_c1_change_rad": 0.025000000000000022,
+      "all_valid_max_abs_c1_change_rad": 0.08250000000000002,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/angles.npz": "2f280a1b5973901d878f1e36564d6e7ae6d817cac176149f2cb898ab7591d65b"
+      },
+      "sweep_report_sha256": "a712fdbe0602404ade075b0ccb540d5f571e5a55d37723fc2674c79b5f62252f",
+      "selected_report_sha256": "0098ef5b1dd0b8b1f0b742cfb5f8e80211eb60718a08e471927c78b9a7c740b3"
+    },
+    "114": {
+      "route": "84865544361f55cb_00000114--03902c6e04",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 61027,
+      "candidate_updates": 732324,
+      "sweep_can_round_trips": 732324,
+      "selected_can_round_trips": 61027,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 263.004525574,
+      "ordinary_mean_abs_c1_change_rad": 0.0010481495632417845,
+      "ordinary_p95_abs_c1_change_rad": 0.003500000000000003,
+      "ordinary_max_abs_c1_change_rad": 0.010999999999999954,
+      "all_valid_max_abs_c1_change_rad": 0.0655,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/angles.npz": "63aa2ff6a21e8613519bda476690e2eda6f42cb5dcf108606bc6072c9e29408d"
+      },
+      "sweep_report_sha256": "138c522c4278d6827eb6192acc2052ba4832f652c3194e2ca330d3a4458a2f85",
+      "selected_report_sha256": "52cdc595248505301501d815d7f6219c47e8cdcac2a059dc345498b3ef707875"
+    },
+    "115": {
+      "route": "84865544361f55cb_00000115--899b9bf91d",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 40037,
+      "candidate_updates": 480444,
+      "sweep_can_round_trips": 480444,
+      "selected_can_round_trips": 40037,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 165.0304544679998,
+      "ordinary_mean_abs_c1_change_rad": 0.0009869321832509364,
+      "ordinary_p95_abs_c1_change_rad": 0.0030000000000000027,
+      "ordinary_max_abs_c1_change_rad": 0.034999999999999976,
+      "all_valid_max_abs_c1_change_rad": 0.09900000000000003,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/angles.npz": "71415b0f49efbfeda495ae52bde4beb7e16f31a1f5570af515a365e756056d3b"
+      },
+      "sweep_report_sha256": "cdb08fede1f86427174c1291e40f63a3ac18044dfb6276e98e0bc184bbff7dc5",
+      "selected_report_sha256": "92d3e720d12018599a5883ecfb3c2dd1330e22235e07b3cd0dcc9bbc0aa7c3dd"
+    },
+    "b9": {
+      "route": "84865544361f55cb_000000b9--5da7fe66ad",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 90774,
+      "candidate_updates": 1089288,
+      "sweep_can_round_trips": 1089288,
+      "selected_can_round_trips": 90774,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 577.3471252719997,
+      "ordinary_mean_abs_c1_change_rad": 0.001220959786299961,
+      "ordinary_p95_abs_c1_change_rad": 0.0050000000000000044,
+      "ordinary_max_abs_c1_change_rad": 0.02350000000000002,
+      "all_valid_max_abs_c1_change_rad": 0.10250000000000004,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/angles.npz": "ce16212d9b13e41993d2c8df3246ddfe21324ad0cb213310e62eee1d685b197b"
+      },
+      "sweep_report_sha256": "13c9b2c4c0f89ec580e9c7a70c7556b4e40873d4d98561c9edfa1c12a0855669",
+      "selected_report_sha256": "733c3eb657cbae29fcb809ca42b85ed6fc03ab44665c5fe31020e266717ce53c"
+    },
+    "ca": {
+      "route": "84865544361f55cb_000000ca--1f70b49ec6",
+      "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+      "cycles": 327448,
+      "candidate_updates": 3929376,
+      "sweep_can_round_trips": 3929376,
+      "selected_can_round_trips": 327448,
+      "selected_matches_sweep_commands_p_i_valid_exactly": true,
+      "zero_gain_zero_delay_matches_v5_exactly": true,
+      "all_c0_and_activation_match_exactly": true,
+      "ordinary_clean_seconds": 892.0115341569954,
+      "ordinary_mean_abs_c1_change_rad": 0.0008244438245156879,
+      "ordinary_p95_abs_c1_change_rad": 0.0025000000000000022,
+      "ordinary_max_abs_c1_change_rad": 0.01200000000000001,
+      "all_valid_max_abs_c1_change_rad": 0.05149999999999999,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/angles.npz": "1681bda6db7dac332abf627d97e7452acd7e0faef0c196b11e55e22291cad073"
+      },
+      "sweep_report_sha256": "4ced4be50569d4025c9de7fb8085714deebfb6098238ab63e5a2e4c88460a94b",
+      "selected_report_sha256": "db0edb05105600f211e7ff833a324887f4402dc64ddceb44ee8ddec911de7533"
+    }
+  },
+  "examples": [
+    {
+      "description": "Late left entry",
+      "route": "115",
+      "time_s": 207.908066963,
+      "desired_angle_deg": 94.1796646118164,
+      "actual_angle_deg": 51.20000076293945,
+      "speed_mph": 12.099885821086458,
+      "strict_clean": true,
+      "c0_m_left_positive": 0.96,
+      "c1_rad_left_positive": {
+        "v5": 0.16849999999999998,
+        "p_025": 0.1825,
+        "p_025_delay_04": 0.14200000000000002
+      },
+      "p_rad_left_positive": 0.018101743422448635,
+      "i_rad_left_positive": 0.015372994845796784
+    },
+    {
+      "description": "Entry continues",
+      "route": "115",
+      "time_s": 208.09921410900006,
+      "desired_angle_deg": 111.01296997070312,
+      "actual_angle_deg": 72.80000305175781,
+      "speed_mph": 12.297558204224886,
+      "strict_clean": true,
+      "c0_m_left_positive": 1.13,
+      "c1_rad_left_positive": {
+        "v5": 0.21050000000000002,
+        "p_025": 0.22450000000000003,
+        "p_025_delay_04": 0.18100000000000005
+      },
+      "p_rad_left_positive": 0.016087211202830076,
+      "i_rad_left_positive": 0.02176835685651445
+    },
+    {
+      "description": "Completed right-turn unwind",
+      "route": "115",
+      "time_s": 133.59461515599992,
+      "desired_angle_deg": 5.989600658416748,
+      "actual_angle_deg": 29.899999618530273,
+      "speed_mph": 5.890273288393662,
+      "strict_clean": false,
+      "c0_m_left_positive": 0.1900000000000004,
+      "c1_rad_left_positive": {
+        "v5": 0.010500000000000065,
+        "p_025": 0.0,
+        "p_025_delay_04": -0.0030000000000000027
+      },
+      "p_rad_left_positive": -0.010158478980883956,
+      "i_rad_left_positive": -0.0015653052344988395
+    },
+    {
+      "description": "Large left overshoot; nearby driver input",
+      "route": "114",
+      "time_s": 168.957705716,
+      "desired_angle_deg": 278.3920593261719,
+      "actual_angle_deg": 450.20001220703125,
+      "speed_mph": 7.027778014508665,
+      "strict_clean": true,
+      "c0_m_left_positive": 5.11,
+      "c1_rad_left_positive": {
+        "v5": 0.34099999999999997,
+        "p_025": 0.2875,
+        "p_025_delay_04": 0.3125
+      },
+      "p_rad_left_positive": -0.07201755233108997,
+      "i_rad_left_positive": -0.13527950258838367
+    },
+    {
+      "description": "Well-tracked left bend",
+      "route": "114",
+      "time_s": 473.085523785,
+      "desired_angle_deg": 57.32655334472656,
+      "actual_angle_deg": 56.29999923706055,
+      "speed_mph": 27.33261651794824,
+      "strict_clean": true,
+      "c0_m_left_positive": 0.6699999999999999,
+      "c1_rad_left_positive": {
+        "v5": 0.1855,
+        "p_025": 0.18500000000000005,
+        "p_025_delay_04": 0.15900000000000003
+      },
+      "p_rad_left_positive": 0.0007143015310955292,
+      "i_rad_left_positive": 0.022011912629614844
+    },
+    {
+      "description": "Hanging right exit",
+      "route": "113",
+      "time_s": 481.56693996600006,
+      "desired_angle_deg": -7.566320896148682,
+      "actual_angle_deg": -94.4000015258789,
+      "speed_mph": 11.959057581279636,
+      "strict_clean": true,
+      "c0_m_left_positive": -0.5800000000000001,
+      "c1_rad_left_positive": {
+        "v5": 0.0645,
+        "p_025": 0.08750000000000002,
+        "p_025_delay_04": 0.034499999999999975
+      },
+      "p_rad_left_positive": 0.03597008844371885,
+      "i_rad_left_positive": 0.06765882642510383
+    }
+  ],
+  "ordinary_cohort": "Existing interval-clean angle mask, valid replay and feedback enabled, absolute desired wheel angle <30 degrees.",
+  "weighting": "Extracted interval duration weights for seconds and mean absolute command changes; percentiles are cycle-weighted.",
+  "ordinary_clean_seconds": 2740.4427841649945,
+  "ordinary_mean_abs_c1_change_by_setting_rad": [
+    {
+      "kp": 0.0,
+      "delay_s": 0.0,
+      "mean_abs_delta_rad": 0.0
+    },
+    {
+      "kp": 0.1,
+      "delay_s": 0.0,
+      "mean_abs_delta_rad": 0.0003946011107110312
+    },
+    {
+      "kp": 0.25,
+      "delay_s": 0.0,
+      "mean_abs_delta_rad": 0.0010009008647461168
+    },
+    {
+      "kp": 0.5,
+      "delay_s": 0.0,
+      "mean_abs_delta_rad": 0.001961192031401653
+    },
+    {
+      "kp": 0.0,
+      "delay_s": 0.2,
+      "mean_abs_delta_rad": 0.006131012841938232
+    },
+    {
+      "kp": 0.1,
+      "delay_s": 0.2,
+      "mean_abs_delta_rad": 0.006179941022213607
+    },
+    {
+      "kp": 0.25,
+      "delay_s": 0.2,
+      "mean_abs_delta_rad": 0.006305419932002006
+    },
+    {
+      "kp": 0.5,
+      "delay_s": 0.2,
+      "mean_abs_delta_rad": 0.006643212063186747
+    },
+    {
+      "kp": 0.0,
+      "delay_s": 0.4,
+      "mean_abs_delta_rad": 0.010964264260175235
+    },
+    {
+      "kp": 0.1,
+      "delay_s": 0.4,
+      "mean_abs_delta_rad": 0.011070256870936306
+    },
+    {
+      "kp": 0.25,
+      "delay_s": 0.4,
+      "mean_abs_delta_rad": 0.011255438766715855
+    },
+    {
+      "kp": 0.5,
+      "delay_s": 0.4,
+      "mean_abs_delta_rad": 0.01162257561022028
+    }
+  ],
+  "totals": {
+    "cycles": 677871,
+    "candidate_updates": 8134452,
+    "sweep_can_round_trips": 8134452,
+    "selected_can_round_trips": 677871,
+    "can_round_trips_including_stress": 9012323
+  },
+  "stress": {
+    "cycles": 200000,
+    "gain": 0.25,
+    "seed": 20260913,
+    "mirrored_updates": 200000,
+    "zero_gain_exact_v5_comparisons": 200000,
+    "can_round_trips": 200000,
+    "baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c",
+    "baseline_source_sha256": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e",
+    "release_cycles": 91,
+    "calibration_approved": false,
+    "checks": "Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.",
+    "scope": "Check PI arithmetic and CAN invariants without a model of vehicle response.",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/pi_stress.py": "07f3bd99c56e2184ba3402d7b2f324506ac0b786c7a15e422e9e2392c934544a",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f"
+    },
+    "formatting_only_final_source_ast_identical": true,
+    "final_script_sha256": "cff014beda81b0bfbaabd7219b8e2848464c5c45ac779669f80084d5be347a06"
+  },
+  "tests": {
+    "passed": 776,
+    "skipped": 178,
+    "subtests_passed": 9145,
+    "log_sha256": "5b9274e2557f41b630ad285b0426c916835c2997fc7f5c4fcad91aa4c6f64243",
+    "ruff": "passed",
+    "controller_ty": "passed",
+    "settings_compiler": "passed"
+  }
+}

docs/ford_c1_request_release.md

@@ -0,0 +1,128 @@
+# Ford changed-request correction release
+
+The Chestnut/Tee Time routes 112 and 113 ran `17a86842f` (C1 feedback v3).
+In route 113, an increasing turn request remained below its base C1 because
+negative correction from an earlier oversteer episode took time to return to
+zero. The existing reversal release did not apply: requested and measured
+curvature were already in the same turn direction.
+
+Version `model-action-c1-feedback-v4` retires a bounded amount of correction
+when a changed request and measured error both oppose that correction. This
+addresses software command delay. It does not establish improved wheel tracking
+or fix all the recorded hanging exits.
+
+## Rule
+
+On a fresh steering measurement, evaluate the previous selected curvature and
+the current selected curvature using today's existing heading reference:
+
+```text
+distance = max(7 m, speed * 1 s)
+change = clip(distance * desired, -0.5, 0.5)
+       - clip(distance * previous_desired, -0.5, 0.5)
+error = desired - measured
+```
+
+Retirement requires all of:
+
+- Feedback enabled and a previous feedback request available.
+- Heading change at least one existing C1 DBC step (0.0005 rad).
+- Change and current error agree in direction.
+- Stored correction opposes that direction.
+- The magnitude of `distance * error` is at least the correction magnitude.
+
+Move the correction toward zero by at most the heading change, without crossing
+zero. Then run the existing reversal release, elapsed-distance integration,
+PSCM arbitration and final output slew. The mismatch requirement is an
+engineering guard using the existing reference distance; it is not a fitted
+PSCM response threshold or proof of stability. It protects a larger learned
+correction from small target/measurement noise. There is no new tunable strength
+multiplier, and the existing 1:1 feedback-strength choice remains.
+
+The last selected curvature adds one control state. Duplicate steering samples
+do not advance this history or retire correction; the next fresh sample uses
+the net request change. A speed change alone cannot cause retirement because
+both requests are evaluated at the same current speed. Invalid input and
+disengagement reset the history. Driver override clears correction and prevents
+a pending request change from being applied later.
+
+C0 mapping and overflow, C2/C3 zeroing, amplitude/slew limits, sender cadence and
+all input/driver/PSCM gates remain unchanged. Toggle off still selects upstream
+Ford control on every platform. The existing default-off toggle selects v4 on
+Ford CAN FD vehicles. `request_release` logs signed radians retired on that
+cycle; periodic diagnostics do not capture every individual retirement.
+
+## Evidence and limits
+
+The regression command `python -m pytest -q -p no:cacheprovider
+openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py`
+initially returned **4 failed** on v3. It checks that an obsolete correction no
+longer delays a changed same-direction turn or unwind after the output slew
+has time to respond. Expanded cases cover small noise, matched tracking,
+insufficient error, speed-only changes, clipped base requests, duplicate
+measurements, override and reset. Integration tests exercise actual controlsd
+selection/limiting, both model and maneuver references, Float32 publication
+and Ford CAN packing.
+
+Frozen replay compares v4 with the deployed v3 on the same recorded model,
+measurement, driver and PSCM inputs:
+
+| Route | Control cycles | Retirement cycles | Largest C1 difference |
+| --- | ---: | ---: | ---: |
+| 112 | 108,971 | 414 | 0.0235 rad |
+| 113 | 49,614 | 119 | 0.0275 rad |
+| Historical b9 | 90,774 | 440 | 0.0350 rad |
+
+Activation and C0 are identical on every replay cycle. C2/C3 remain zero.
+Retirement changes subsequent correction history, so command differences can
+persist after a retirement cycle. In frozen measurements the vehicle cannot
+react to those differences. Command differences occur in ordinary bends too;
+these tests do not establish unchanged real-world centering or stability.
+
+In route 113, segment 3, old opposing correction reaches zero at **3:18.630**
+instead of **3:19.253**: **0.623 s earlier**. At 3:18.649, C1 magnitude is
+0.319 rad instead of 0.2925 rad. The PSCM limit flag still inhibits additional
+outward integration; retiring opposing correction cannot create new stored
+outward demand through that gate.
+
+The route 113 exit at 8:01.567 has **identical C1** in this replay. Route 112's
+11:40.555 overshoot changes C1 by only 0.0015 rad, slightly later in the unwind
+direction on the frozen history. These are material limits: the change does
+not solve those exits. C0's contribution and physical PSCM response remain
+unresolved. No counterfactual wheel-angle or tracking-error score is reported.
+
+The combined suite passes **717 tests and 9,145 subtests**, with 178 inherited
+or unsupported safety-test skips. Feedback stress and zero-error stress cover
+200,000 cycles each; the latter also covers 18,138 field-boundary cases.
+Together with the three route replays, these verify **667,497 Float32/CAN round
+trips**, separately from the integration suite. Stress compares each step to
+v3 after only the declared retirement and checks sign symmetry, bounds, slew,
+resets, arbitration and correction direction. Ruff, the controller Ty check
+and settings compilation pass. Numerical records are in
+`ford_c1_request_release_validation.json`.
+
+## Reproduction
+
+Use the project's native Python dependencies and unchanged opendbc revision
+`64aa61b9b3fd26e70a7caa915acab207ff3cd64a`. Route commands require the full-rlog
+extracts (`route.npz`, `model_paths.npz`, `metadata.json`) identified by the
+validation hashes. No original logs are modified.
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:opendbc_repo
+# Optional writable roots for the tests' temporary parameter stores and logs:
+export PARAMS_ROOT=/tmp/ford-v4-test-params
+export LOG_ROOT=/tmp/ford-v4-test-logs
+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 opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_v4/stress.json
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260912 --opendbc-revision 64aa61b9b3fd26e70a7caa915acab207ff3cd64a --output .cache/ford_v4/zero_error.json
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route112 --baseline 17a86842f --output .cache/ford_v4/route112
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route113 --baseline 17a86842f --output .cache/ford_v4/route113
+python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb9 --baseline 17a86842f --output .cache/ford_v4/routeb9
+```
+
+Publication time approximates the computation clock; full SubMaster health is
+not reconstructable. These routes have no selected maneuver-plan publications;
+that source is covered by integration tests. No device build, boot, installation
+or physical steering test is performed offline.

docs/ford_c1_request_release_validation.json

@@ -0,0 +1,214 @@
+{
+  "hypothesis": "model-action-c1-feedback-v4",
+  "baseline_revision": "17a86842f97f65216443a5d89648c8ace8518758",
+  "scope": "Software command delay and invariants only; no counterfactual wheel motion or proven physical tracking improvement. Hanging exits remain unresolved.",
+  "calibration_approved": false,
+  "tests": {
+    "passed": 717,
+    "subtests_passed": 9145,
+    "skipped": 178,
+    "log_sha256": "1864f8618b9d788f67e57766cf7b9ab9eda8e98a2ad0caf1c668bb9936b6eb7d",
+    "initial_regression": "4 failures on v3; same-turn command delay tests pass on v4",
+    "environment": "PARAMS_ROOT and LOG_ROOT point to dedicated temporary directories; initial sandbox path failures resolved without changing tests."
+  },
+  "feedback_stress": {
+    "cycles": 200000,
+    "mirrored_updates": 200000,
+    "can_round_trips": 200000,
+    "carryover_release_count": 165,
+    "baseline_revision": "17a86842f97f65216443a5d89648c8ace8518758",
+    "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f",
+    "request_release_cycles": 20454,
+    "exact_unchanged_state_and_commands_without_request_release": 179546,
+    "exact_v3_match_after_only_declared_retirement": 200000,
+    "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, bounded request retirement, carryover direction/confirmation, integration, PSCM limits, CAN.",
+    "scope": "Numerical software invariants only; no model of vehicle motion.",
+    "calibration_approved": false,
+    "controller_sha256": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442"
+  },
+  "zero_error_stress": {
+    "seed": 20260912,
+    "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.4000000000000019,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
+    "opendbc_import_head": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "cec2619285dd41274562ac035ee8ea0a389269a0c4ef1b62efa6252ad1a714aa",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f"
+    }
+  },
+  "replays": {
+    "112": {
+      "baseline_revision": "17a86842f",
+      "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f",
+      "calibration_approved": false,
+      "cycles": 108971,
+      "active_cycles": 91414,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 17557,
+        "active": 91414
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 1.43945091800002,
+      "max_abs_offset_overflow_target_m": 0.5727187991142273,
+      "request_release_cycles": 414,
+      "request_release_seconds": 4.192443282002046,
+      "max_abs_request_release_rad": 0.008723706007003784,
+      "feedback_enabled_seconds": 840.7664582650004,
+      "pscm_limit_2_seconds": 14.441855805999936,
+      "c1_changed_cycles": 58152,
+      "max_abs_c1_change_rad": 0.023500000000000076,
+      "max_abs_correction_rad": 0.205313389369823,
+      "can_round_trips": 108971,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "2e9ea03d4947c7bb3a02c864e0dbc7c9bf031af51dba5e44a8aaa137c3aac6b2",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "targeted_points": [
+        {
+          "time_s": 700.5553145380001,
+          "baseline_c0_c1": [
+            -0.34999999999999964,
+            0.010000000000000009
+          ],
+          "candidate_c0_c1": [
+            -0.34999999999999964,
+            0.008500000000000008
+          ]
+        }
+      ]
+    },
+    "113": {
+      "baseline_revision": "17a86842f",
+      "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f",
+      "calibration_approved": false,
+      "cycles": 49614,
+      "active_cycles": 27207,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 22407,
+        "active": 27207
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 2.7607263189997866,
+      "max_abs_offset_overflow_target_m": 0.6065444126725197,
+      "request_release_cycles": 119,
+      "request_release_seconds": 1.235422420998475,
+      "max_abs_request_release_rad": 0.009946223348379135,
+      "feedback_enabled_seconds": 243.3033761190004,
+      "pscm_limit_2_seconds": 14.003248144999816,
+      "c1_changed_cycles": 17825,
+      "max_abs_c1_change_rad": 0.027500000000000024,
+      "max_abs_correction_rad": 0.16966817302181283,
+      "can_round_trips": 49614,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "2e9ea03d4947c7bb3a02c864e0dbc7c9bf031af51dba5e44a8aaa137c3aac6b2",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5673630d31910fcfa5a3cc9a8d533b6b8fe9a76e2627bf1f67b52b3550ec7442"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "old_correction_zero_s": 199.2531750590001,
+      "new_correction_zero_s": 198.62974550599984,
+      "earlier_correction_zero_s": 0.6234295530002782,
+      "targeted_points": [
+        {
+          "time_s": 198.64943081699994,
+          "baseline_c0_c1": [
+            2.16,
+            0.2925
+          ],
+          "candidate_c0_c1": [
+            2.16,
+            0.319
+          ]
+        },
+        {
+          "time_s": 481.56693996600006,
+          "baseline_c0_c1": [
+            0.5800000000000001,
+            -0.0645
+          ],
+          "candidate_c0_c1": [
+            0.5800000000000001,
+            -0.0645
+          ]
+        }
+      ]
+    },
+    "b9": {
+      "baseline_revision": "17a86842f",
+      "baseline_source_sha256": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f",
+      "calibration_approved": false,
+      "cycles": 90774,
+      "active_cycles": 86474,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 4300,
+        "active": 86474
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 5.703841178999909,
+      "max_abs_offset_overflow_target_m": 4.280821338295937,
+      "request_release_cycles": 440,
+      "request_release_seconds": 4.520361682000512,
+      "max_abs_request_release_rad": 0.017186015844345093,
+      "feedback_enabled_seconds": 816.0284774219999,
+      "pscm_limit_2_seconds": 9.308613716000167,
+      "c1_changed_cycles": 48629,
+      "max_abs_c1_change_rad": 0.03500000000000003,
+      "max_abs_correction_rad": 0.18457476562660308,
+      "can_round_trips": 90774,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "cd76c6106b2e41e905b752f1638d5b3e0feaa10ec21e038268df33183a91c640",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed."
+    }
+  },
+  "float32_can_round_trips_excluding_integration_tests": 667497,
+  "checks": {
+    "ruff": true,
+    "controller_ty": true,
+    "settings_compilation": true,
+    "toggle_off_upstream_integration": true
+  },
+  "final_source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py": "4b4a69d67eba0ff9d5db0a50a4f868c5eb5f7c8c79d70832500554febc7d28fe",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "7b2429a5c40e5067b8edea4c11e9cdd4c6271d09f7982e30126eb42b93425a50",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "5943a37f6e3865297ac543fb922a3c8b6e016c5af3eff589f518bd9615bbdb4f",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "df883702a847465815feb6c4fbf88130c27e69d7e3e256c9962d99a9738ee353",
+    "tools/ford_pscm_lab/feedback_replay.py": "cd76c6106b2e41e905b752f1638d5b3e0feaa10ec21e038268df33183a91c640"
+  },
+  "source_note": "Controller comment/docstring cleanup followed feedback stress and routes 112/113. The recorded tested hashes are retained; b9 and zero-error stress record the final controller source. No executable controller change followed those runs."
+}

docs/ford_curvature_c0_v8.md

@@ -0,0 +1,99 @@
+# Desired-curvature C0 on continuous PI
+
+This local candidate changes C0's reference from the live model path's lateral
+position at 7 m to a circular arc of the selected, upstream-limited desired
+curvature. It is based on v7 (`08b3a14ad`), with the same P=0.50/I=0.25 controller.
+No new gain, state, release condition, reference delay or output limit is added.
+The candidate is on `codex/ford-curvature-c0-trial`; this evaluation does not
+publish it over the v7 onroad branch.
+
+For selected curvature k and the existing 7 m reference distance:
+
+```text
+arc C0 = (1 − cos(7 × k)) / k, or 0 when k = 0
+       ≈ 24.5 × k for small curvature
+C0 target = clip(arc C0 + 7 × clipped-away base C1, −5.11, +5.11)
+```
+
+The implementation uses the equivalent squared-sinc expression to avoid
+subtracting nearly equal floating-point numbers near zero. C0 keeps its 4 m/s
+output slew and Float32/CAN quantization. C1 keeps the existing mapping and PI
+law, ±0.5 rad bound and 0.5 rad/s slew. C2 and C3 stay zero.
+
+This is a geometric reference choice, not a model of the PSCM. The arc starts
+at zero lateral position and heading. Independent live model-path position and
+heading are omitted, while selected curvature can still include the model's
+centering decision. Valid live model geometry remains a health gate. Short valid
+paths do not shorten the synthetic 7 m arc. Both C0 and C1 use the selected
+request, including the maneuver source when selected by controlsd.
+
+## What the recorded routes show
+
+All fourteen previous routes were replayed with v7 and this C0 replacement:
+Lightning 112–117, a0, a2, a5, a9, b8, b9, ca and Raptor 02. On all 1,578,250
+control cycles, C1, P, I, activation, feedforward, overflow and feedback/PSCM
+gates match exactly. The v7 baseline also reproduces its archived commands
+exactly. C0 matches the earlier isolated geometry experiment, but C1 no longer
+has the release rules that coupled it to C0 in that experiment.
+
+Duration-weighted clean samples, grouped by requested steering-wheel angle:
+
+| Absolute requested wheel angle | v7 mean absolute C0 | Curvature C0 | Reduction |
+| --- | ---: | ---: | ---: |
+| Under 5° | 0.0136 m | 0.0072 m | 46.7% |
+| 5–30° | 0.0965 m | 0.0608 m | 37.0% |
+| 30–90° | 0.5064 m | 0.2916 m | 42.4% |
+| At least 90° | 2.6314 m | 1.5282 m | 41.9% |
+
+The clean cohort is 6,654.52 s with the existing quality/driver mask and margins,
+speed at least 2 m/s and replay feedback enabled. Only 36.20 s have a requested
+wheel angle of at least 90°. These are command magnitudes, not torque or tracking
+scores, and do not classify driver interventions as controller failures.
+
+At route 117, 128.272 s (right entry), C0 changes from −2.51 m to −1.35 m while
+C1 remains −0.427 rad. At 142.670 s (request relaxing/reversing), C0 changes from
++0.18 m to −0.02 m while C1 remains −0.0425 rad. Both comparisons are replayed
+on the same recorded vehicle motion. The new C0 follows the selected request
+more directly, but it supplies less C0 during the entry as well as the exit.
+
+## Validation and interpretation
+
+- 694 Ford/controlsd, tracked PSCM lab, Sunnylink, params, sender and safety
+  tests pass; 9,145 subtests pass and 178 platform tests skip. Historical
+  untracked offline experiment tests are outside this deployment suite.
+- The isolated two-controller replay performs 3,156,500 Float32/CAN checks.
+- Production selection/adapter replay exactly reproduces the isolated candidate
+  on every route cycle, adding 1,578,250 Float32/CAN checks.
+- A 20,000-cycle scalar geometry/PI stress with mirrored and unrelated model
+  paths adds 60,000 checks. Total: 4,794,750 CAN round trips.
+- Tests cover zero/tiny curvature, signs, circular geometry, short/malformed
+  paths, selected maneuver requests, overflow, unwind, duplicate measurements,
+  model/driver/PSCM gates, caps/slew and toggle-off upstream Ford fallback.
+- Ruff, production Ty and diff whitespace checks pass.
+
+Software C1 parity does not guarantee identical physical unwind: changing C0
+changes the PSCM's input and therefore the vehicle response and future feedback.
+Smaller C0 is not established as better or worse tracking. No device build,
+boot or drive of this candidate is claimed. Collecting the promising v7 drive's
+logs before replacing it would preserve a useful comparison.
+
+## Reproduction
+
+Use the built cereal/opendbc environment and the same local route extracts:
+
+```sh
+export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps
+export PYTHONDONTWRITEBYTECODE=1
+export PARAMS_ROOT=/tmp/ford-c0-params
+export LOG_ROOT=/tmp/ford-c0-logs
+python -m tools.ford_pscm_lab.curvature_c0_v7_replay .cache/ford_route117 --output .cache/ford_curvature_c0_v7/117
+python -m tools.ford_pscm_lab.curvature_c0_validate --output .cache/ford_curvature_c0_v7/production --workers 4
+python -m tools.ford_pscm_lab.curvature_c0_production_stress --cycles 20000 --output .cache/ford_curvature_c0_v7/production_stress.json
+```
+
+Repeat the first command for each label before validating all routes. Raptor
+uses input `.cache/ford_raptor_route02` and output label `raptor02`. The first
+replay loads isolated copies of pinned v7; the second tests this checkout's
+actual selector and adapter. The validation JSON records source and extract
+hashes, settings, example points and both sets of route reports. Older v7-only
+production validation commands should run from the v7 commit.

docs/ford_curvature_c0_v8_validation.json

@@ -0,0 +1,890 @@
+{
+  "scope": "Curvature-derived C0 candidate on the continuous PI controller; fixed recorded motion, no physical tracking prediction.",
+  "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+  "branch": "codex/ford-curvature-c0-trial",
+  "deployment_at_evaluation": "local candidate; hiimisaac-dev remains v7",
+  "hypothesis": "model-action-curvature-c0-pi-v8",
+  "source_sha256": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc",
+  "kp": 0.5,
+  "ki": 0.25,
+  "station_m": 7.0,
+  "cycles": 1578250,
+  "route_count": 14,
+  "same_c1_and_integral_on_every_recorded_cycle": true,
+  "baseline_exactly_matches_archived_v7": true,
+  "c0_commands_match_prior_geometry_experiment": true,
+  "can_round_trips": 4794750,
+  "tests": {
+    "passed": 694,
+    "skipped": 178,
+    "subtests_passed": 9145,
+    "scope": "Same Ford controls, tracked PSCM lab, car status, Sunnylink, params, Ford car and safety suite as v7; historical untracked experiments excluded."
+  },
+  "ruff": "pass",
+  "ty_production": "pass",
+  "bins": [
+    {
+      "requested_wheel_angle_degrees": "0-5",
+      "seconds": 4863.242382185041,
+      "mean_abs_c0_m": [
+        0.013572257514406704,
+        0.007239040142159917
+      ],
+      "reduction_pct": 46.662962042417725
+    },
+    {
+      "requested_wheel_angle_degrees": "5-30",
+      "seconds": 1576.952442509967,
+      "mean_abs_c0_m": [
+        0.09652124113032315,
+        0.06077627429873566
+      ],
+      "reduction_pct": 37.03326481610879
+    },
+    {
+      "requested_wheel_angle_degrees": "30-90",
+      "seconds": 178.12595563801986,
+      "mean_abs_c0_m": [
+        0.5064290157685561,
+        0.2916269430470148
+      ],
+      "reduction_pct": 42.4150406144399
+    },
+    {
+      "requested_wheel_angle_degrees": "90-inf",
+      "seconds": 36.19569558700505,
+      "mean_abs_c0_m": [
+        2.6314148997254545,
+        1.528200796263899
+      ],
+      "reduction_pct": 41.92474944094366
+    }
+  ],
+  "examples": [
+    {
+      "route": "116",
+      "time": 112.00019806100002,
+      "desired_angle": 59.83296203613281,
+      "actual_angle": 60.900001525878906,
+      "c0_left_positive": [
+        0.7599999999999998,
+        0.33999999999999986
+      ],
+      "c1_both_left_positive": 0.13550000000000006,
+      "clean": true
+    },
+    {
+      "route": "117",
+      "time": 128.2721161039999,
+      "desired_angle": -226.72189331054688,
+      "actual_angle": -185.1999969482422,
+      "c0_left_positive": [
+        -2.5100000000000002,
+        -1.35
+      ],
+      "c1_both_left_positive": -0.427,
+      "clean": true
+    },
+    {
+      "route": "116",
+      "time": 673.6933639450001,
+      "desired_angle": -152.83456420898438,
+      "actual_angle": -272.1000061035156,
+      "c0_left_positive": [
+        -2.21,
+        -0.9199999999999999
+      ],
+      "c1_both_left_positive": -0.183,
+      "clean": true
+    },
+    {
+      "route": "117",
+      "time": 142.52000677500018,
+      "desired_angle": 6.6218461990356445,
+      "actual_angle": 40.599998474121094,
+      "c0_left_positive": [
+        0.2599999999999998,
+        0.040000000000000036
+      ],
+      "c1_both_left_positive": -0.024499999999999966,
+      "clean": true
+    },
+    {
+      "route": "117",
+      "time": 142.66993146999994,
+      "desired_angle": -2.186957597732544,
+      "actual_angle": 28.5,
+      "c0_left_positive": [
+        0.17999999999999972,
+        -0.019999999999999574
+      ],
+      "c1_both_left_positive": -0.04249999999999998,
+      "clean": true
+    }
+  ],
+  "replay_reports": [
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 108971,
+      "controller_updates": 217942,
+      "active_cycles_per_controller": 91414,
+      "can_round_trips": 217942,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 919.4963447830002,
+      "clean_seconds": 678.5721400560004,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/angles.npz": "9429813237b4988dd99a1461d1a9bd6f6d3056693a93a5dd1ec63567d19f2aa1",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 49614,
+      "controller_updates": 99228,
+      "active_cycles_per_controller": 27207,
+      "can_round_trips": 99228,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 273.884368861,
+      "clean_seconds": 195.66672679699968,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/angles.npz": "2f280a1b5973901d878f1e36564d6e7ae6d817cac176149f2cb898ab7591d65b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 61027,
+      "controller_updates": 122054,
+      "active_cycles_per_controller": 48419,
+      "can_round_trips": 122054,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 487.557504183,
+      "clean_seconds": 290.3177928490001,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/angles.npz": "63aa2ff6a21e8613519bda476690e2eda6f42cb5dcf108606bc6072c9e29408d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 40037,
+      "controller_updates": 80074,
+      "active_cycles_per_controller": 33976,
+      "can_round_trips": 80074,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 341.89711117800005,
+      "clean_seconds": 176.48340830000006,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/angles.npz": "71415b0f49efbfeda495ae52bde4beb7e16f31a1f5570af515a365e756056d3b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 68793,
+      "controller_updates": 137586,
+      "active_cycles_per_controller": 52456,
+      "can_round_trips": 137586,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 527.966444073,
+      "clean_seconds": 218.019997628,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/angles.npz": "68e699e01c3ea291e6602ee53edf9cee27c0b461e16c57c3acc0ad12bf29b2aa",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 27301,
+      "controller_updates": 54602,
+      "active_cycles_per_controller": 15841,
+      "can_round_trips": 54602,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 159.4681704720001,
+      "clean_seconds": 74.58932843500065,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/angles.npz": "0575b2d888a6848649b9fb1f7df7c72140f0abbceaa1c451b6eecd034729ef7b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 114582,
+      "controller_updates": 229164,
+      "active_cycles_per_controller": 105382,
+      "can_round_trips": 229164,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 1061.1945591,
+      "clean_seconds": 884.2159145080002,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/angles.npz": "feb7b558d75292ab69ce8657540f3295bbc1b0c915d27f39d831bdece3fac534",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 71111,
+      "controller_updates": 142222,
+      "active_cycles_per_controller": 43430,
+      "can_round_trips": 142222,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 437.0644468180001,
+      "clean_seconds": 332.6767952400001,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/angles.npz": "2424b32f02f7e5079f8ae6ffdd30a811820d1d1bfa5b17636c5457eb67d64ecb",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 38961,
+      "controller_updates": 77922,
+      "active_cycles_per_controller": 35677,
+      "can_round_trips": 77922,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 359.186785903,
+      "clean_seconds": 249.91415584299997,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/angles.npz": "5466a674cae3f63eca8758f9f370a8c6cdee3dbb6bcf115f85e3b2e95cf5c1fb",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 286319,
+      "controller_updates": 572638,
+      "active_cycles_per_controller": 152043,
+      "can_round_trips": 572638,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 1530.3877326270015,
+      "clean_seconds": 1093.2748892979992,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/angles.npz": "cfcc9743aacfadc7be97e9ca130453c2ee6f7870edc3e98ccf9a873dac6d4897",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 160431,
+      "controller_updates": 320862,
+      "active_cycles_per_controller": 68217,
+      "can_round_trips": 320862,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 686.4382411050101,
+      "clean_seconds": 390.78219015704235,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/angles.npz": "6f4ea54d101595a48d301a0914c940d6ad932249b14d4eaa4fd7e43a11dc6f92",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 90774,
+      "controller_updates": 181548,
+      "active_cycles_per_controller": 86474,
+      "can_round_trips": 181548,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 870.449417766,
+      "clean_seconds": 604.9269013949997,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/angles.npz": "ce16212d9b13e41993d2c8df3246ddfe21324ad0cb213310e62eee1d685b197b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 327448,
+      "controller_updates": 654896,
+      "active_cycles_per_controller": 118756,
+      "can_round_trips": 654896,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 1194.5832199259985,
+      "clean_seconds": 894.0687052669919,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/angles.npz": "1681bda6db7dac332abf627d97e7452acd7e0faef0c196b11e55e22291cad073",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    },
+    {
+      "scope": "Circular-arc C0 experiment against the pinned continuous PI v7 controller.\n\nBoth controllers run on identical recorded motion. Replace only the encoder's\nC0 target, retaining path validity, C1 overflow, continuous PI feedback and\noutput limits. No onroad selector or production module is modified.\n",
+      "baseline_revision": "08b3a14ad46260fda0f8d3a1c2cee2d272504153",
+      "baseline_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8",
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "proportional_gain": 0.5,
+      "integral_gain": 0.25,
+      "station_m": 7.0,
+      "cycles": 132881,
+      "controller_updates": 265762,
+      "active_cycles_per_controller": 64776,
+      "can_round_trips": 265762,
+      "same_activation_p_feedforward_overflow_and_feedback_gates": true,
+      "changed_c1_cycles": 0,
+      "max_abs_c1_change_rad": 0.0,
+      "changed_integral_cycles": 0,
+      "pscm_status_fresh_active_seconds": 651.3446633269996,
+      "clean_seconds": 571.0075301469988,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/angles.npz": "82af2973ad40c1df392e11bdc5dd5895bfc6c170fc894735123af7704d95cd57",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_v7_replay.py": "fcd8a10db3d68024dbc836dce1b4c9a459912837b1a4234f61b2dd508c6d5e9f"
+      },
+      "limitations": [
+        "No new steering trace or physical tracking score: recorded motion stays fixed.",
+        "The baseline is v7 on every route, regardless of its recorded controller version.",
+        "C1 and integral state match exactly on fixed recorded motion; physical feedback may differ.",
+        "Publication time proxies computation time; full SubMaster state is unavailable.",
+        "The synthetic arc keeps the original model-health gates for a controlled comparison."
+      ]
+    }
+  ],
+  "production_parity": [
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "112",
+      "cycles": 108971,
+      "can_round_trips": 108971,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/112/report.json": "1be0b52e00e8f67043ce35e19cc6612f788d6e3853b40d1bc2dd372b2ff4e70d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/112/commands.npz": "3951aa6160295ee6c3835517b39c7c835074b93b1bda5fb013d4ef56f88b8f1e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "113",
+      "cycles": 49614,
+      "can_round_trips": 49614,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/113/report.json": "c6e98c0f2e6f43f1ede8d8104b77606a91fb3e95efa8348f7f089e01a4f6c823",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/113/commands.npz": "0a7aa93e8e3e64659022dac254b476e06fb95649c4b97e1de66e4808fae73d6f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "114",
+      "cycles": 61027,
+      "can_round_trips": 61027,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/114/report.json": "b7f1d341f4f33d2c16dcfa2694b36cafe0bf76405d0e7b61bc4e469caef6b1dc",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/114/commands.npz": "3b79d094ecb89d3f3554c492d83826ec60662c236d6854891feba95fffcd7bc7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "115",
+      "cycles": 40037,
+      "can_round_trips": 40037,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/115/report.json": "e66d2be86136b0f6fdbcc2998673b2d1e8487f0543f99e4618e46a2f429538e4",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/115/commands.npz": "9a72eb6bab9116cc83210c92dfb12c5d63a2e4b3103ccdd3f5e70214f3f0e142",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "116",
+      "cycles": 68793,
+      "can_round_trips": 68793,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/116/report.json": "501b944c65f41f6c1e05fccdc6c38f3a80450db6c4696adc35bf85640a04df94",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/116/commands.npz": "2469c545d0ce62894d59e07799942ebf40bec583693391aaa1dd9e75c218a396",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "117",
+      "cycles": 27301,
+      "can_round_trips": 27301,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/117/report.json": "896b30e7ded613f1b05b693067fc2f19155016b6a33f26f881bbfff2b193019d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/117/commands.npz": "6d88bfdb54fde00240954db9dbbf1f3e5059bfac9b6161c138f0069213ac35a8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a0",
+      "cycles": 114582,
+      "can_round_trips": 114582,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a0/report.json": "52cd87929fbdc721450c735bd02ac049260cafa439af7dcdcd405e977dc9ea22",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a0/commands.npz": "0f718892b0802cbf754ba33c9c9c6da8a3e915e88fa7a4e008b9b33d63a429e0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a2",
+      "cycles": 71111,
+      "can_round_trips": 71111,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a2/report.json": "fbaf59a1499df5dba0f790f76f211efefb3b68f327e9e49c763fb175f6f8e7a9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a2/commands.npz": "159f76d2d511af7f423941cc9d168c7112a8aec7a34e05bbd043f65a2324b325",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a5",
+      "cycles": 38961,
+      "can_round_trips": 38961,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a5/report.json": "ba9cedd6d9f2bbe2caa3ad0eeb298fa3334750ad60086422d7d48f3b690480f7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a5/commands.npz": "8ac5c77a29824b8f621c3071309990d320a1a61cc968ad55f76408a530680913",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "a9",
+      "cycles": 286319,
+      "can_round_trips": 286319,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a9/report.json": "677a2f66ecc5696fa44190c5e3ee27cd3dc45f1deda9ffe3a2f330b4e1d0d20d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/a9/commands.npz": "e109e77124dc2296a00f65563c1b1ec3c960b414473a3cc7c6caaafbe7e65037",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "b8",
+      "cycles": 160431,
+      "can_round_trips": 160431,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/b8/report.json": "f48796acdfad87a0caaaddac2a41945c97311e85fa57f9fc73a1730d264ddba4",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/b8/commands.npz": "554891332a496498ff64e08e4eba4210125fd5f2da8307231dd6d9377becfe0a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "b9",
+      "cycles": 90774,
+      "can_round_trips": 90774,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/b9/report.json": "3812983a690e5eafca93ea6af05620fe82e4e0cc8efc07e24505a63664444d66",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/b9/commands.npz": "2ffc685e29463a7b89d39bfb3583a37eb10a57d7201a24394550fcf9a89e3c4c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "ca",
+      "cycles": 327448,
+      "can_round_trips": 327448,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/ca/report.json": "753760c726ef1b85daf83b7108f758f9304dab408b49018f303616b99c96b81f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/ca/commands.npz": "3fa932ac86e14440d325a6cc4677b47cd75c5c6019b929d3a5d078a62c5590df",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    },
+    {
+      "scope": "Verify production commands against the archived curvature-C0 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n",
+      "route": "raptor02",
+      "cycles": 132881,
+      "can_round_trips": 132881,
+      "production_matches_archived_trial_exactly": true,
+      "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+      "hypothesis": "model-action-curvature-c0-pi-v8",
+      "kp": 0.5,
+      "ki": 0.25,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/raptor02/report.json": "af6b8fa4e4bfe3d5543794cdde4fbe4fd7560addf02717c2b6144c3987effafa",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_curvature_c0_v7/raptor02/commands.npz": "99ee65b866eda85e5c5d54f8896a7f952504826753eca31a01a29c7cf36c6482",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_validate.py": "c91ead82b0320725276141b1bc9b84b132cd6345a144ea826ba2b7f069870932",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+      }
+    }
+  ],
+  "production_stress": {
+    "cycles": 20000,
+    "kp": 0.5,
+    "ki": 0.25,
+    "controller_updates": 60000,
+    "can_round_trips": 60000,
+    "seed": 20260913,
+    "independent_c0_comparisons": 20000,
+    "checks": "Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/curvature_c0_production_stress.py": "4bf031a793efe9ebc066e5b6db6897ca8e844d5e34c6faecd4701a4d8f04132a",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc"
+    }
+  },
+  "limitations": [
+    "C1 parity applies only to fixed recorded inputs; changing C0 changes physical response and subsequent feedback.",
+    "Curvature arc omits independent live model position/heading; seven meters remains an engineering reference choice.",
+    "Smaller C0 is not proof of better or worse tracking; no candidate hardware drive has been performed."
+  ]
+}

docs/ford_direct_path_v9.md

@@ -0,0 +1,70 @@
+# Ford direct C0/C1 requests — v9
+
+The v8 maneuver route `84865544361f55cb/0000011c--99f4537696` showed
+commands being delayed by our extra 4 m/s C0 and 0.5 rad/s C1 slews.
+Those values were controller choices, not measurements of the PSCM's limits.
+For example, a C1 target reversal from +0.10 to -0.10 rad required at least
+0.4 seconds in our output stage alone.
+
+V9 sends the current bounded C0 and C1 requests on each update. There is no
+additional actuator ramp, zero hold, reversal mode, or initial engagement ramp.
+C0 remains the selected-curvature arc at 7 m plus base-C1 overflow. C1 remains
+base heading plus proportional and integral feedback, with P=0.50 and I=0.25.
+These are calculated commands, not a known inverse of the PSCM's response.
+
+Integral cancellation happens first. New integral accumulation fits the
+combined command's magnitude headroom, rather than the old slew headroom.
+This removes a second dependence on the old ramp. Fresh-measurement cadence,
+driver override, PSCM arbitration, stale/invalid input reset, and C2/C3=0 remain.
+
+The default-off Sunnylink `FordModelActionController` selector still applies
+to any Ford CAN FD vehicle. Toggle off restores upstream Ford control.
+Logs identify `model-action-direct-c0-c1-pi-v9`.
+
+## Remaining bounds
+
+- Selected desired curvature still passes through upstream `clip_curvature`:
+  3 m/s² lateral acceleration adjusted for roll, 5 m/s³ lateral jerk, and
+  absolute curvature 0.2 m⁻¹. These bound the reference; they are not proof of
+  a physical acceleration or jerk bound under custom C0/C1 feedback.
+- C0 remains within ±5.11 m and C1 within ±0.50 rad. The downstream packer
+  retains the actual wire ranges, preventing out-of-range values wrapping.
+- Driver, CAN, timing, service health, and fault gates remain intact.
+- Panda safety, the 100 Hz sender, and ramp-type selection are unchanged.
+
+No assumption is made that extended path mode independently enforces ISO
+limits. The absence of `LimitReached` is not evidence of unrestricted authority.
+
+## Offline validation
+
+The same-cycle reversal regression failed on v8 in both directions at 2, 10,
+and 100 ms timesteps, then passed after the change. Zero-error release reaches
+zero immediately. Full controlsd/publication/CAN tests check current-request
+output, upstream reference selection, field packing, driver and PSCM gates,
+integral cancellation and anti-windup, invalid input, and toggle-off fallback.
+
+- 358 tests and 23 subtests passed across controller and Ford sender suites.
+- 20,000 seeded randomized cases produced 60,000 controller updates and CAN
+  round trips, checking independent arithmetic, symmetry, bounds and resets.
+- Routes 11c, 119 and 11a supplied 461,340 input cycles: 922,680 baseline/v9
+  controller updates and CAN round trips. V8 and v9 activation, feedforward,
+  proportional feedback and feedback gates matched. Every active v9 output
+  matched its current bounded request within wire quantization.
+- Ruff passed for changed production/tests and the new replay/stress tools.
+
+| Route | Input cycles | Maximum C0 difference | Maximum C1 difference |
+|---|---:|---:|---:|
+| 11c maneuver suite | 54,146 | 0.09 m | 0.1605 rad |
+| 119 | 171,423 | 2.42 m | 0.4950 rad |
+| 11a | 235,771 | 3.36 m | 0.3710 rad |
+
+The maximum differences include engagement and other transitions. Removing
+slews permits abrupt changes; these are not predictions of wheel motion.
+Recorded motion, driver input and PSCM feedback stay fixed in replay.
+Publication timestamps approximate computation time; replay is not a claim of
+exact onroad command parity. Synthetic reference freshness is approximated
+from valid maneuver publications. Physical tracking and stability are unvalidated.
+
+The reproducible tools are `tools/ford_pscm_lab/direct_path_replay.py` and
+`tools/ford_pscm_lab/direct_path_production_stress.py`. Machine-readable checks
+are collected in `ford_direct_path_v9_validation.json`.

docs/ford_direct_path_v9_validation.json

@@ -0,0 +1,125 @@
+{
+  "version": "model-action-direct-c0-c1-pi-v9",
+  "baseline": "57ae29f257498f58170e2beec140c0f8103c1b43",
+  "tests": {
+    "passed": 358,
+    "subtests_passed": 23
+  },
+  "stress": {
+    "cycles": 20000,
+    "kp": 0.5,
+    "ki": 0.25,
+    "controller_updates": 60000,
+    "can_round_trips": 60000,
+    "seed": 20260913,
+    "independent_c0_comparisons": 20000,
+    "checks": "Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, amplitude bounds, current commands, resets, CAN.",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/direct_path_production_stress.py": "ae0d0e3a32cddefc4072a58664f5afca84019067c8c5d9fa24e89dd1cbef2b99",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "fbc2309c15289db184aa54bda697feb7cfc26a218998b5b21f063381f47aad45"
+    }
+  },
+  "routes": {
+    "11c": {
+      "scope": "Compare v8 and direct C0/C1 on fixed logged inputs, without predicting motion.",
+      "baseline": "57ae29f257498f58170e2beec140c0f8103c1b43",
+      "baseline_source_sha256": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc",
+      "cycles": 54146,
+      "controller_updates": 108292,
+      "can_round_trips": 108292,
+      "status_counts": {
+        "inactive": 5066,
+        "active": 49080
+      },
+      "identical_validity_feedforward_p_and_feedback_gates": true,
+      "all_candidate_outputs_match_current_bounded_request": true,
+      "changed_c0_cycles": 73,
+      "changed_c1_cycles": 17133,
+      "max_abs_c0_change_m": 0.08999999999999986,
+      "max_abs_c1_change_rad": 0.16049999999999998,
+      "max_abs_integral_rad": 0.02639216769448115,
+      "limitations": [
+        "Recorded motion remains fixed; this cannot establish improved tracking or stability.",
+        "Publication time proxies computation time; reconstructed baseline is not exact onroad parity.",
+        "Synthetic reference freshness is approximated from valid publications; upstream selection itself is unchanged."
+      ],
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_maneuver_11c/analysis/route.npz": "1f3c37a7c7ae85ba69e9958435a9568f8244fa862f43ad1ee2a0192d1df13966",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_maneuver_11c/analysis/model_paths.npz": "0ff433da3dde1d4d7f10596cd53558ec5de9d56fbc49aec3e8afb4c9872377d5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/direct_path_replay.py": "2a1baff08152230929d56b23b8f7fb03e1ae9c2b30c5e152a2be431188fc20e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "fbc2309c15289db184aa54bda697feb7cfc26a218998b5b21f063381f47aad45"
+      }
+    },
+    "119": {
+      "scope": "Compare v8 and direct C0/C1 on fixed logged inputs, without predicting motion.",
+      "baseline": "57ae29f257498f58170e2beec140c0f8103c1b43",
+      "baseline_source_sha256": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc",
+      "cycles": 171423,
+      "controller_updates": 342846,
+      "can_round_trips": 342846,
+      "status_counts": {
+        "inactive": 87482,
+        "active": 83941
+      },
+      "identical_validity_feedforward_p_and_feedback_gates": true,
+      "all_candidate_outputs_match_current_bounded_request": true,
+      "changed_c0_cycles": 199,
+      "changed_c1_cycles": 16426,
+      "max_abs_c0_change_m": 2.42,
+      "max_abs_c1_change_rad": 0.495,
+      "max_abs_integral_rad": 0.05010140673563736,
+      "limitations": [
+        "Recorded motion remains fixed; this cannot establish improved tracking or stability.",
+        "Publication time proxies computation time; reconstructed baseline is not exact onroad parity.",
+        "Synthetic reference freshness is approximated from valid publications; upstream selection itself is unchanged."
+      ],
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route119/route.npz": "415a099935fef152123b7422870442d6cfe6302bab9b8983ce3b3ffc71a7702b",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route119/model_paths.npz": "dfd41383bea486ccd0a476e94e612921a44a099a0c1213ff132ab81df3aa94d7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/direct_path_replay.py": "2a1baff08152230929d56b23b8f7fb03e1ae9c2b30c5e152a2be431188fc20e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "fbc2309c15289db184aa54bda697feb7cfc26a218998b5b21f063381f47aad45"
+      }
+    },
+    "11a": {
+      "scope": "Compare v8 and direct C0/C1 on fixed logged inputs, without predicting motion.",
+      "baseline": "57ae29f257498f58170e2beec140c0f8103c1b43",
+      "baseline_source_sha256": "6f8c4a6f7fa54fa2e23f32f1e83ace83da5f0b6f45011941f1902a1dae778ffc",
+      "cycles": 235771,
+      "controller_updates": 471542,
+      "can_round_trips": 471542,
+      "status_counts": {
+        "inactive": 103039,
+        "active": 132732
+      },
+      "identical_validity_feedforward_p_and_feedback_gates": true,
+      "all_candidate_outputs_match_current_bounded_request": true,
+      "changed_c0_cycles": 689,
+      "changed_c1_cycles": 16106,
+      "max_abs_c0_change_m": 3.3600000000000003,
+      "max_abs_c1_change_rad": 0.371,
+      "max_abs_integral_rad": 0.05163860736233724,
+      "limitations": [
+        "Recorded motion remains fixed; this cannot establish improved tracking or stability.",
+        "Publication time proxies computation time; reconstructed baseline is not exact onroad parity.",
+        "Synthetic reference freshness is approximated from valid publications; upstream selection itself is unchanged."
+      ],
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route11a/route.npz": "428c203d090731386f06bf9fdeefe608f1999c43c52ff39cab7ee86ec0ac804f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route11a/model_paths.npz": "e2b827fd7c3dfbf66d7872a56b57eaec13c600a9a48e12dae59be81c31b0978f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/direct_path_replay.py": "2a1baff08152230929d56b23b8f7fb03e1ae9c2b30c5e152a2be431188fc20e7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "fbc2309c15289db184aa54bda697feb7cfc26a218998b5b21f063381f47aad45"
+      }
+    }
+  },
+  "limits_removed": [
+    "C0 4 m/s extra slew",
+    "C1 0.5 rad/s extra slew and associated integral slew headroom"
+  ],
+  "limits_preserved": [
+    "upstream selected-curvature acceleration/jerk/curvature bounds",
+    "C0/C1 magnitude and wire packing bounds",
+    "driver/fault/validity/freshness/PSCM feedback gates"
+  ],
+  "physical_tracking_validated": false,
+  "note": "Replay/stress script hashes identify their execution versions before formatting-only lint cleanup; production controller is unchanged after validation."
+}

docs/ford_model_action_candidate.md

@@ -0,0 +1,155 @@
+# 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. The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md)
+to this original mapping; the historical no-feedback description below is
+not the current controller specification.
+
+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_drive_test.md

@@ -0,0 +1,124 @@
+# Ford selected-action drive-test branch
+
+This v13 trial increases C1's proportional gain from 0.50 to **0.75**, retaining
+I=0.25, [curvature-derived C0](ford_curvature_c0_v8.md), direct C0/C1 requests,
+and [continuous C1 PI feedback](ford_c1_minimal_pi.md).
+Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 defaults to a 7 m circular arc from selected desired curvature. An on-device toggle can instead use max(7 m, speed × 1 second).
+[Base C1 overflow allocation to C0](ford_c1_overflow.md) remains.
+It is selectable on **any Ford CAN FD vehicle**
+through the existing persistent, default-off Sunnylink
+toggle. Offline checks establish software behavior; physical tracking,
+turn-exit behavior and closed-loop stability remain unvalidated.
+
+Both base commands use selected, upstream-limited desired curvature. The +0.40 s
+low-speed model preview from `b720e9f1b` remains: full offset at 15 mph and below,
+tapering to zero at 30 mph. The trial changes only the immediate error correction;
+PSCM `LimitReached` handling, integral gain, field bounds, and selection are retained.
+See [P=0.75 replay results](ford_c1_p75_trial.md) for scope, tradeoffs, and reproduction.
+
+## Select and restore
+
+1. Install branch `hiimisaac-dev` from `sunnypilot/sunnypilot` using the device's
+   normal branch-switch process and allow its build to finish.
+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; a stored toggle change or disengagement alone cannot swap an active
+   controller. Initial physical evaluation remains controlled testing.
+
+The startup event `Ford path controller selected` should report
+`FordModelActionController`. Periodic `Ford C2-free path tracking` events
+identify **`hypothesis=model-action-curvature-c0-distance-pi-v13`**. They report desired and measured
+curvature, base heading, proportional and accumulated correction, applied heading,
+feedback timing and driver/PSCM gating. `proportional_gain=0.75` and
+`integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0
+target in meters before C0 amplitude limits. `calibration_approved=false`
+remains. The retired request/unwind/reversal diagnostic fields are removed.
+
+Turning the toggle off and completing another offroad-to-onroad cycle restores
+**upstream Ford curvature control**: 20 Hz steering messages, limited mode on
+CAN FD, zero C0/C1/C3, and upstream curvature limiting and platform-specific
+overshoot handling. Stored observer or retired controller settings cannot select
+a custom controller. The observer toggle is no longer exposed. The experiment
+only runs on Ford CAN FD vehicles; legacy Ford uses upstream control as well.
+See [toggle-off validation](ford_upstream_fallback.md).
+
+## C0 distance toggle on comma four
+
+With the experimental Ford controller enabled, open **Settings → toggles → C0: 1 second**.
+The toggle is visible for Ford CAN FD vehicles and can be changed while disengaged.
+
+- **Off (default):** C0 uses a fixed 7 m arc.
+- **On:** C0 uses a distance of max(7 m, speed × 1 second), matching the base C1 distance.
+
+Disengage assistance, change the toggle, and remain disengaged for at least three seconds
+before reengaging. This setting uses the existing three-second runtime parameter refresh;
+**no ignition cycle or controlsd restart is required**. Engaged or paused MADS and stale
+engagement messages prevent applying a change. A mode change resets the PI correction and
+adapter timestamps. Reapplying the same value does not reset anything.
+
+The persistent parameter is `FordC0TimeBased`. It cannot enable the experimental controller
+by itself. The existing Sunnylink controller-selection toggle still requires an onroad cycle.
+C1, the gains, the 7 m heading-overflow allocation, the upstream reference limits and the CAN
+field bounds are unchanged. Below 7 m/s (about 15.7 mph), both distance modes are identical.
+At 20/30/60 mph the enabled distance is approximately 8.9/13.4/26.8 m, respectively; C0 can
+therefore be substantially larger, especially at higher speeds. Its release still follows the
+current selected curvature immediately, with no additional slew.
+
+The `Ford C0 distance changed` event records an applied switch. Periodic tracking events
+include `c0_time_based` and the actual `offset_distance` in meters, including the default mode.
+Offline checks verify selection, runtime switching, resets, unchanged C1 and CAN encoding;
+they do not establish which distance the PSCM follows better.
+
+Validation on 2026-09-14: 410 tests and 25 subtests passed, plus Ruff and the local comma four
+UI construction/write/refresh/visibility/render check. The 54,146-cycle maneuver-route replay
+(`84865544361f55cb/0000011c--99f4537696`) matched `775012167` exactly with the new toggle off.
+With it on, C0 changed in 35,668 cycles (maximum difference 0.74 m), while C1 and accumulated
+correction remained identical on the same recorded motion. The two comparisons completed
+216,584 controller updates and CAN round trips. No vehicle build, installation or road test
+was performed for this change.
+
+## Wiring and validation
+
+`controlsd` supplies the selected, upstream-limited desired curvature and the
+measured steering-derived curvature already used in its tracking diagnostics.
+Fresh steering publications advance C1 integration. P responds to the current
+error without accumulating. Repeated publications use current feedforward and P
+but cannot integrate the same elapsed interval twice.
+Driver override clears P and I. A fresh PSCM reached-limit flag stops
+extra outward accumulation while preserving unwind and base model changes.
+With fresh feedback, the part of the error increment that cancels existing I
+is applied before the ordinary accumulation clamp. Any remainder must fit the
+combined feedforward/P/I amplitude envelope. There is no C0 confirmation
+threshold or remembered turn direction. Zero error removes P and holds I; it
+does not trigger a release. Final command limits still apply.
+
+C0 starts with the selected-distance circular arc of selected desired curvature. It does not
+add independent live model-path position or heading. Valid model geometry is
+still required as a health gate. When the raw base heading
+exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading.
+Accumulated C1 feedback does not spill into C0. The extra target returns to zero
+as the base heading falls below the cap. Applied C0 changes in that same update.
+C2 and C3 remain zero. The
+existing field bounds, 100 Hz custom sender and Float32 publication remain in
+place. An explicit selection flag distinguishes upstream mode from an invalid
+experimental command; invalid experimental input cannot switch to upstream.
+The opendbc sender restores upstream behavior when that flag is false.
+
+[Direct-command validation](ford_direct_path_v9.md) records the same command law introduced in v9.
+[Curvature-C0](ford_curvature_c0_v8.md) and its validation JSON record v8.
+[Continuous PI](ford_c1_minimal_pi.md) and its validation JSON record v7.
+[Proportional feedback](ford_c1_pi.md) and its validation JSON record v6.
+[Completed-unwind release](ford_unwind_catchup.md) and
+`ford_unwind_catchup_validation.json` record v5. [Changed-request release](ford_c1_request_release.md) and its
+validation JSON record v4. The [overflow specification](ford_c1_overflow.md) and
+`ford_c1_overflow_validation.json` record v3. The carryover specification and `ford_c1_carryover_validation.json`
+record the previous experiment. `ford_c1_feedback_validation.json` records the initial feedback
+version at `5fbb583e5`. `ford_model_action_validation.json` and
+`ford_model_action_drive_test_validation.json` are historical records for the
+original offline candidate and its first wiring, respectively; their counts
+and coverage are not claims about the current version.
+
+The full hardware build and device boot are not performed by these offline
+checks. Pushing the branch does not install it on the device or change its
+stored toggle.

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_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_unwind_catchup.md

@@ -0,0 +1,122 @@
+# Ford completed-unwind correction release
+
+Version `model-action-c1-feedback-v5` releases dominant C1 correction after a
+confirmed unwind reaches the selected curvature. This fixes stored correction
+continuing to request a new turn after its original unwind is complete.
+
+Route 115 (`codex-last2`) ran v4, `6df5eabb7`. Near 2:13.2, desired and actual
+steering were both near zero after a right turn, but C1 still requested about
+0.1165 rad left. About 0.114 rad was accumulated unwind correction. V4's
+changed-request release did not apply: the small new left request was increasing
+while the measured error called for less left steering. The earlier reversal
+release also did not apply because measured and requested curvature were
+already on the same side.
+
+## Rule
+
+On a fresh steering measurement, remember an unwind direction when the selected
+curvature relaxes toward zero (or crosses it), measured curvature remains on
+the previous side, and measured error calls for leaving that old turn.
+
+When measured error reaches or passes zero in that unwind direction, release
+the stored correction only if all of these agree:
+
+- The selected curvature has reached zero or crossed into the unwind direction.
+- Correction points in that direction and exceeds the magnitude of base C1.
+- Original model C0 and applied C0 both confirm that direction by at least the
+  existing 0.01 m DBC step.
+
+Consume the unwind marker at this first catch-up, even if the other conditions
+prevent release. A steady old-side bend, neutral/conflicting C0, or a correction
+smaller than base C1 retains its correction. Steady requests cannot arm the
+marker. Duplicate steering publications cannot arm or consume it. Driver/PSCM
+feedback inhibition and controller resets clear it; correction reversing
+direction also clears it.
+
+After retirement, the v4 command law still runs: bounded changed-request
+release, reversal release, measured-error integration, PSCM arbitration, and
+final C1 slew. Removing stored correction therefore does not jump the output.
+There is one additional control state, `unwind_direction`; `unwind_release`
+only reports the signed correction retired on the current cycle. Periodic
+diagnostics may miss individual release cycles.
+
+This is a conditional correction-reset policy, not a PSCM plant model. It adds
+no strength multiplier. The existing 1:1 feedback choice, C0 mapping/overflow,
+C2/C3 zeroing, amplitude/slew limits, sender cadence and input gates remain.
+Toggle off still selects upstream Ford control; toggle on selects the experiment
+on Ford CAN FD platforms. See [selection and restore](ford_model_action_drive_test.md).
+
+## Exact exit replay
+
+Frozen route 115 measurements trigger one release at **2:13.203501**. The
+selected steering angle is 0.469 degrees left and measured angle is 0.500 degrees
+left. The controller retires 0.114026 rad of left unwind correction. Its first
+C1 output moves from 0.1165 to 0.1110 rad left, respecting the original slew.
+
+At **2:13.594615**, old C1 is **0.1240 rad left**, versus **0.0105 rad left** in
+v5. C0 is identical. The earlier unwind (2:09 through 2:13.2), comparison turn
+(3:20 through 3:34), and comparison bend (5:33 through 5:45) have identical
+commands throughout their windows.
+
+The recorded wheel motion stays fixed in this replay. It does not predict a
+new steering angle, prove stability, or establish that the full overshoot is
+fixed. This maneuver also includes driver input and a changing C0 request;
+neither its whole swing nor every hanging exit can be attributed to stored I.
+
+## Validation
+
+Four targeted regressions failed on v4 because correction persisted after
+catch-up; all now pass. Expanded tests cover both directions, fresh/duplicate
+feedback, catch-up confirmation, steady tracking/noise, old-side bends, C0
+agreement, dominant correction, reset/override, limit-reached behavior and slew.
+Integration exercises actual controlsd request selection/limiting for model
+and maneuver sources, Float32 publication, and Ford CAN packing/checksums.
+
+The combined suite passes **753 tests and 9,145 subtests**, with **178 inherited
+or unsupported safety-test skips**. Random feedback stress covers 200,000 cycles
+and their mirrors. Each cycle exactly matches v4 after only the declared new
+retirement; 59 cycles retire correction. Independent zero-error checks cover
+200,000 cycles and 18,138 field-boundary cases. Ruff, controller Ty and settings
+compilation checks pass.
+
+| Frozen route | Cycles | New releases | Changed C1 cycles | Largest C1 difference |
+| --- | ---: | ---: | ---: | ---: |
+| 114 | 61,027 | 4 | 2,849 | 0.0150 rad |
+| 115 | 40,037 | 1 | 384 | 0.1140 rad |
+| 112 | 108,971 | 14 | 30,036 | 0.0720 rad |
+| 113 | 49,614 | 1 | 951 | 0.0050 rad |
+| Historical b9 | 90,774 | 16 | 7,013 | 0.1435 rad |
+
+All routes compare v5 against v4 with the same recorded inputs. Activation and
+C0 match exactly on all 350,423 cycles. Releases also occur at smaller exits;
+changed history can affect subsequent ordinary bends. These results do not
+establish unchanged physical centering. Route 114's large segment-2 overshoot
+does not trigger this new release, so it remains a separate unresolved case.
+
+The five replays and two stress runs verify **768,561 Float32/CAN round trips**,
+separately from the integration suite. Exact source hashes and numerical
+results are in `ford_unwind_catchup_validation.json`.
+
+## Reproduction
+
+Use the native project Python dependencies and unchanged opendbc revision
+`64aa61b9b3fd26e70a7caa915acab207ff3cd64a`. Route replay requires the full-rlog
+extracts identified by validation hashes; the original logs are not modified.
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:opendbc_repo
+export PARAMS_ROOT=/tmp/ford-v5-test-params
+export LOG_ROOT=/tmp/ford-v5-test-logs
+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 opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_unwind_catchup/stress.json
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260913 --opendbc-revision 64aa61b9b3fd26e70a7caa915acab207ff3cd64a --output .cache/ford_unwind_catchup/zero_error.json
+for route in 114 115 112 113 b9; do
+  python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route${route} --baseline 6df5eabb7e7f6bc4e206644d5ca9069df820124e --output .cache/ford_unwind_catchup/route${route}
+done
+```
+
+Publication time approximates the computation clock; full SubMaster health is
+not reconstructable. These route replays do not reconstruct selected maneuver
+messages; integration tests cover that source. No device build, boot,
+installation or physical steering test is performed offline.

docs/ford_unwind_catchup_validation.json

@@ -0,0 +1,346 @@
+{
+  "hypothesis": "model-action-c1-feedback-v5",
+  "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+  "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+  "scope": "Software command release and numerical invariants only; no counterfactual steering motion, physical stability or improved tracking claim.",
+  "calibration_approved": false,
+  "tests": {
+    "passed": 753,
+    "subtests_passed": 9145,
+    "skipped": 178,
+    "log_sha256": "64c683622cc91125e32cb0d78f4a5340b8d58fa149d90b06361629394489731d",
+    "initial_regression": "4 failed on v4: stored unwind correction remains after catch-up; all pass on v5",
+    "regression_log_sha256": "0ced2a0686cccba3c321c58749a679843a3179fa57078a6b030d20f0f9ae33e2"
+  },
+  "feedback_stress": {
+    "cycles": 200000,
+    "mirrored_updates": 200000,
+    "can_round_trips": 200000,
+    "carryover_release_count": 181,
+    "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+    "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+    "seed": 20260913,
+    "request_release_cycles": 20214,
+    "unwind_release_cycles": 59,
+    "exact_unchanged_state_and_commands_without_unwind_release": 199941,
+    "exact_v4_match_after_only_declared_retirement": 200000,
+    "checks": "Symmetry, resets, amplitude/slew, bounded retirement, carryover confirmation, integration, PSCM limits, CAN.",
+    "scope": "Numerical software invariants only; no model of vehicle motion.",
+    "calibration_approved": false,
+    "controller_sha256": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+  },
+  "zero_error_stress": {
+    "seed": 20260913,
+    "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.4000000000000019,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.",
+    "opendbc_import_head": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "cec2619285dd41274562ac035ee8ea0a389269a0c4ef1b62efa6252ad1a714aa",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f"
+    }
+  },
+  "replays": {
+    "114": {
+      "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+      "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "calibration_approved": false,
+      "cycles": 61027,
+      "active_cycles": 48419,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 12608,
+        "active": 48419
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 6.548916987999917,
+      "max_abs_offset_overflow_target_m": 1.233181856572628,
+      "request_release_cycles": 288,
+      "request_release_seconds": 2.98796386799998,
+      "max_abs_request_release_rad": 0.006280367394214892,
+      "unwind_release_cycles": 4,
+      "max_abs_unwind_release_rad": 0.01457856219656457,
+      "feedback_enabled_seconds": 438.3550780740002,
+      "pscm_limit_2_seconds": 8.604224759000118,
+      "c1_changed_cycles": 2849,
+      "max_abs_c1_change_rad": 0.015000000000000013,
+      "max_abs_correction_rad": 0.2921633626620207,
+      "can_round_trips": 61027,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "report_sha256": "9a9d7a6c92ec2cc1e19dc6b6e628e402cbe3f6344e09ccbee1c5c76deedebc7e",
+      "commands_sha256": "fc1a5249cc9a15e3369f177a88781377c327e27b681e1028ad26063cb70167a5"
+    },
+    "115": {
+      "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+      "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "calibration_approved": false,
+      "cycles": 40037,
+      "active_cycles": 33976,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 6061,
+        "active": 33976
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 2.095635206999873,
+      "max_abs_offset_overflow_target_m": 0.6360401958227158,
+      "request_release_cycles": 181,
+      "request_release_seconds": 1.7931359259980582,
+      "max_abs_request_release_rad": 0.005393094571379859,
+      "unwind_release_cycles": 1,
+      "max_abs_unwind_release_rad": 0.11402585053291069,
+      "unwind_releases": [
+        {
+          "time_s": 133.203500567,
+          "retired_rad": -0.11402585053291069,
+          "correction_after_rad": 0.0,
+          "base_c1_rad": -0.0025691102200653404,
+          "desired_angle_deg": 0.46853354573249817,
+          "actual_angle_deg": 0.5,
+          "baseline_c0_c1": [
+            -0.04999999999999982,
+            -0.11650000000000005
+          ],
+          "candidate_c0_c1": [
+            -0.04999999999999982,
+            -0.11099999999999999
+          ]
+        }
+      ],
+      "feedback_enabled_seconds": 306.2414766659987,
+      "pscm_limit_2_seconds": 6.4724571650002645,
+      "c1_changed_cycles": 384,
+      "max_abs_c1_change_rad": 0.1140000000000001,
+      "max_abs_correction_rad": 0.13572457044904282,
+      "can_round_trips": 40037,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "report_sha256": "e3e4183e14ce20b2d3b0938028a5a4ab3200f72dd46e898174751aa94912ad84",
+      "commands_sha256": "ba9b6411a432430bb1380f63fe011ba91235b393f3785c17d2904657c95d27d9"
+    },
+    "112": {
+      "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+      "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "calibration_approved": false,
+      "cycles": 108971,
+      "active_cycles": 91414,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 17557,
+        "active": 91414
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 1.43945091800002,
+      "max_abs_offset_overflow_target_m": 0.5727187991142273,
+      "request_release_cycles": 381,
+      "request_release_seconds": 3.8611647240012985,
+      "max_abs_request_release_rad": 0.008723706007003784,
+      "unwind_release_cycles": 14,
+      "max_abs_unwind_release_rad": 0.07267236868778636,
+      "feedback_enabled_seconds": 840.7664582650004,
+      "pscm_limit_2_seconds": 14.441855805999936,
+      "c1_changed_cycles": 30036,
+      "max_abs_c1_change_rad": 0.07200000000000006,
+      "max_abs_correction_rad": 0.205313389369823,
+      "can_round_trips": 108971,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "report_sha256": "aaa1d819cf344de668ea92eb58714a6ae537054a0feadc05a0fa15097a57c553",
+      "commands_sha256": "9f191699d19af8f123e9245a77827a2eca27d4b3191f9acddf9389edbda0ef24"
+    },
+    "113": {
+      "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+      "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "calibration_approved": false,
+      "cycles": 49614,
+      "active_cycles": 27207,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 22407,
+        "active": 27207
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 2.7607263189997866,
+      "max_abs_offset_overflow_target_m": 0.6065444126725197,
+      "request_release_cycles": 119,
+      "request_release_seconds": 1.2281319819985583,
+      "max_abs_request_release_rad": 0.009946223348379135,
+      "unwind_release_cycles": 1,
+      "max_abs_unwind_release_rad": 0.005007614799767142,
+      "feedback_enabled_seconds": 243.3033761190004,
+      "pscm_limit_2_seconds": 14.003248144999816,
+      "c1_changed_cycles": 951,
+      "max_abs_c1_change_rad": 0.00500000000000006,
+      "max_abs_correction_rad": 0.16966817302181283,
+      "can_round_trips": 49614,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "report_sha256": "032aaf4b2dd3a54264330c9c6367ad4a63f49d779ce9acd4c3c1d355d731d545",
+      "commands_sha256": "c1d4e59718400d21d724d502e2befa315db56802a7622d3d68e8b3af7daea319"
+    },
+    "b9": {
+      "baseline_revision": "6df5eabb7e7f6bc4e206644d5ca9069df820124e",
+      "baseline_source_sha256": "1a4ce5f5f63b4d2f1f6e0537c9b2ca7c463ca44b427349d71d28fb8a1138b00f",
+      "calibration_approved": false,
+      "cycles": 90774,
+      "active_cycles": 86474,
+      "validity_matches_baseline_exactly": true,
+      "status_counts": {
+        "inactive": 4300,
+        "active": 86474
+      },
+      "c0_matches_baseline_exactly": true,
+      "c0_changed_cycles": 0,
+      "max_abs_c0_change_m": 0.0,
+      "offset_overflow_seconds": 5.703841178999909,
+      "max_abs_offset_overflow_target_m": 4.280821338295937,
+      "request_release_cycles": 416,
+      "request_release_seconds": 4.241454379000558,
+      "max_abs_request_release_rad": 0.017186015844345093,
+      "unwind_release_cycles": 16,
+      "max_abs_unwind_release_rad": 0.15648483206475247,
+      "feedback_enabled_seconds": 816.0284774219999,
+      "pscm_limit_2_seconds": 9.308613716000167,
+      "c1_changed_cycles": 7013,
+      "max_abs_c1_change_rad": 0.14349999999999996,
+      "max_abs_correction_rad": 0.18457476562660308,
+      "can_round_trips": 90774,
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "e869520839aef948ccbf20b44e3efb6ec854a539ec0d66c07779825bcd8037a7",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e"
+      },
+      "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.",
+      "reference_limit": "Uses exact consumed model publication as reference; selected maneuver-plan messages are not reconstructed.",
+      "report_sha256": "d66308cefae9956d2b67b8d9ceb83804054e838c70e84999ba758ab1cbf21d5d",
+      "commands_sha256": "406275e9c230a2975fd8c9d0824d3a902283d3a5cb597328026578db66c2cb6b"
+    }
+  },
+  "checks": {
+    "ruff": true,
+    "controller_ty": true,
+    "settings_compilation": true
+  },
+  "limitations": [
+    "Recorded driver input and PSCM response remain fixed during replay.",
+    "No device build, boot or physical test performed.",
+    "Releases occur at smaller exits too; unchanged real-world centering is not established.",
+    "Route 114 segment-2 overshoot does not trigger this release."
+  ],
+  "float32_can_round_trips_excluding_integration_tests": 768561,
+  "exit_points_left_positive": [
+    {
+      "time_s": 131.19208205699988,
+      "baseline_c1_rad": -0.10949999999999999,
+      "candidate_c1_rad": -0.10949999999999999,
+      "baseline_correction_rad": 0.04102452737994574,
+      "candidate_correction_rad": 0.04102452737994574
+    },
+    {
+      "time_s": 133.19194825599993,
+      "baseline_c1_rad": 0.11650000000000005,
+      "candidate_c1_rad": 0.11650000000000005,
+      "baseline_correction_rad": 0.11402585053291069,
+      "candidate_correction_rad": 0.11402585053291069
+    },
+    {
+      "time_s": 133.203500567,
+      "baseline_c1_rad": 0.11650000000000005,
+      "candidate_c1_rad": 0.11099999999999999,
+      "baseline_correction_rad": 0.1140256728569634,
+      "candidate_correction_rad": -0.0
+    },
+    {
+      "time_s": 133.40571050699987,
+      "baseline_c1_rad": 0.118,
+      "candidate_c1_rad": 0.009500000000000064,
+      "baseline_correction_rad": 0.11367557589137142,
+      "candidate_correction_rad": -0.0
+    },
+    {
+      "time_s": 133.59461515599992,
+      "baseline_c1_rad": 0.124,
+      "candidate_c1_rad": 0.010500000000000065,
+      "baseline_correction_rad": 0.1120761218192909,
+      "candidate_correction_rad": -0.0015653052344988395
+    },
+    {
+      "time_s": 207.90007524899988,
+      "baseline_c1_rad": 0.16449999999999998,
+      "candidate_c1_rad": 0.16449999999999998,
+      "baseline_correction_rad": 0.016763380618580685,
+      "candidate_correction_rad": 0.016763380618580685
+    },
+    {
+      "time_s": 338.168560822,
+      "baseline_c1_rad": -0.173,
+      "candidate_c1_rad": -0.173,
+      "baseline_correction_rad": -0.024831306563109386,
+      "candidate_correction_rad": -0.024831306563109386
+    }
+  ],
+  "identical_route115_command_windows_s": [
+    [
+      129.0,
+      133.2
+    ],
+    [
+      200.0,
+      214.0
+    ],
+    [
+      333.0,
+      345.0
+    ]
+  ]
+}

docs/ford_upstream_fallback.md

@@ -0,0 +1,89 @@
+# Ford toggle-off upstream restoration
+
+`FordModelActionController` is the only setting that can select custom Ford
+steering. It defaults false. With it false or absent, no custom path controller
+is created and normal lateral-control curvature passes unchanged to the Ford
+sender. A stored `FordPscmObserver` or retired virtual-angle setting cannot
+override that choice. The observer toggle is removed from Sunnylink; its stored
+parameter remains readable for compatibility but has no selection effect.
+
+Selection remains fixed for the lifetime of controlsd. Sunnylink changes require
+a real offroad-to-onroad cycle, as before. The startup diagnostic reports
+`controller=upstream` when the experiment is not selected.
+
+## Sender behavior
+
+The new `fordLateralPath.enabled` field conveys startup selection independently
+of `valid`. Its default is false. The sender uses custom mode only when this
+field is true on a Ford CAN FD vehicle. This prevents invalid model geometry
+or disengagement in the selected experiment from choosing a different controller.
+
+Toggle-off restores the upstream Ford sender:
+
+- 20 Hz steering messages on both CAN FD and legacy Ford.
+- CAN FD limited mode 1 while active, mode 0 while inactive, with upstream ramp
+  type 0, counters and checksums.
+- C0, C1 and C3 zero; C2 follows upstream actuator curvature.
+- Upstream curvature amplitude/rate limits and the measured-curvature error
+  clamp above 9 m/s.
+- Upstream anti-overshoot handling for Bronco Sport and F-150 MK14.
+
+The reference is the upstream implementation already merged into this branch,
+opendbc `f95f996f5917dcbbf2e32fe51b606a24cf836af6`. Its Ford sender differs from
+the locally available comma opendbc `3e92d112129507debe45364891954db70238997a`
+only in sunnypilot's additional `CP_SP`/`CC_SP` interface arguments. This change
+restores that implementation; it does not upgrade unrelated upstream code.
+
+Toggle-on retains the previous custom 100 Hz sender, mode 2, ramp type 3 and
+existing path limits. The model-action controller's command law and diagnostic
+identity `model-action-c1-feedback-v3` are unchanged. Legacy Ford always uses
+upstream control. The opendbc dependency is now
+`64aa61b9b3fd26e70a7caa915acab207ff3cd64a`. No Panda safety code is changed;
+its existing limited-mode checks already use 20 Hz curvature limits.
+
+## Validation
+
+- Combined Ford, Sunnylink, parameter, logging, replay-tool and Ford safety
+  suite: **683 passed, 178 existing skips, 9,145 subtests passed**.
+- Real startup → controlsd → Float32 publication → conversion → Ford sender:
+  14 new toggle-off cases, covering all six CAN FD platforms plus legacy
+  Escape, with both stored observer settings. They preserve the upstream
+  actuator output, including when custom model geometry is missing, and verify
+  20 Hz cadence, engage/disengage/reengage, zero path terms, mode, ramp,
+  counters and checksums across 4,200 control cycles / 840 steering messages.
+- The existing toggle-on, stale-input, invalid-input and 100 Hz integration
+  regressions continue to pass.
+- Additional Ford interface fuzz checks: **11 passed**, 60 generated examples
+  each, with real Cap'n Proto conversion and car-interface application. The
+  initially missing neural-network-data dependency was initialized at the
+  repository's existing pin `03cac2d30e111e0689c0429cb8c1fe6cb5a905af`.
+- Packet equivalence: **55,000 toggle-off cycles across all 11 Ford platforms**
+  match the pinned upstream sender exactly. **30,000 toggle-on cycles across
+  six CAN FD platforms** match the previous custom sender exactly. All 90,305
+  outgoing packets and returned actuator values match, including invalid paths,
+  inactive periods, both turn directions and speed boundaries. Disabled
+  selection also ignores deliberately nonzero, valid custom path fields.
+- Ruff, controller type check, generated Sunnylink schema check and diff
+  whitespace checks pass.
+
+The packet comparison loads the exact old controller **and its old CAN builder**
+from trusted local Git sources. It does not compare two aliases of the modified
+code. Results and source hashes are in `ford_upstream_fallback_validation.json`.
+No device build, boot or physical steering validation is claimed.
+
+## Reproduction
+
+Use the pinned opendbc dependency and native project dependencies:
+
+```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 opendbc_repo/opendbc/safety/tests/test_ford.py
+FUZZ_SEED=20260911 python -m pytest -q -p no:cacheprovider openpilot/selfdrive/car/tests/test_car_interfaces.py -k FORD
+python -m tools.ford_pscm_lab.upstream_fallback_check --cycles 5000 --output .cache/ford_upstream_fallback/equivalence.json
+python openpilot/sunnypilot/sunnylink/tools/compile_settings_ui.py --check
+```
+
+The comparison requires both pinned baseline commits in the local opendbc Git
+object store. The previous overflow/replay records describe their historical
+source hashes; the comparison here establishes unchanged toggle-on sender output.

docs/ford_upstream_fallback_validation.json

@@ -0,0 +1,190 @@
+{
+  "created_at_utc": "2026-09-11T16:11:23.341760+00:00",
+  "scope": "Default-off upstream Ford control, including CAN sender; exact software comparison only.",
+  "parent_commit": "b81c00f5b9c3658d72675ec3ee0ac07e0ef14807",
+  "opendbc_commit": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a",
+  "target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "selection": {
+    "param": "FordModelActionController",
+    "default": false,
+    "off": "upstream",
+    "on": "Ford CAN FD model-action v3",
+    "activation": "Existing controlsd startup; real offroad-to-onroad cycle",
+    "observer_setting": "Ignored for control selection; no longer exposed in Sunnylink",
+    "sender_field": "fordLateralPath.enabled defaults false, independent of valid"
+  },
+  "custom_command_law_ast_matches_parent": [
+    "_packed",
+    "_finite",
+    "encode_model_action",
+    "ModelActionController",
+    "FordModelActionController"
+  ],
+  "tests": {
+    "combined": "683 passed, 178 skipped, 9145 subtests passed in 9.61s",
+    "additional_ford_interface_fuzz": "11 passed, 258 non-Ford deselected in 8.15s; 60 examples per Ford platform",
+    "fuzz_seed": 20260911,
+    "dependency_setup": "Initialized existing neural-network-data pin 03cac2d30e111e0689c0429cb8c1fe6cb5a905af after missing-model-data failure.",
+    "new_toggle_off_integration_cases": 14,
+    "new_integration_cycles": 4200,
+    "new_steering_frames": 840,
+    "ruff_changed_python": "pass",
+    "ty_controller": "pass",
+    "settings_compiler_check": "pass",
+    "diff_check": "pass"
+  },
+  "packet_equivalence": {
+    "scope": "Compare all outgoing Ford packets with pinned upstream and custom senders.\n\nUses trusted local Git sources, identical synthetic inputs, and the actual CAN\npackers. Establishes software equivalence, not physical steering performance.\n",
+    "seed": 20260911,
+    "upstream_revision": "f95f996f5917dcbbf2e32fe51b606a24cf836af6",
+    "previous_custom_revision": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+    "upstream_source_sha256": {
+      "opendbc/car/ford/fordcan.py": "8b3c74bff68146cf9f97d17203b7deebb9254561d9591a4a92d978bf01808a75",
+      "opendbc/car/ford/carcontroller.py": "c7e590c13cfe2434d77d6224359d659bb5092a3fdd65b12c7d8d9eddfb3deada"
+    },
+    "previous_custom_source_sha256": {
+      "opendbc/car/ford/fordcan.py": "5b73c568149bde299f71f92f034f3032a94ecae4ee4bae8af938f34ef9590062",
+      "opendbc/car/ford/carcontroller.py": "b2d327a1833fb1f0d09ee17f54c9c8d45517fa29beb04a4543cfbf1b43f1a65e"
+    },
+    "results": [
+      {
+        "fingerprint": "FORD_BRONCO_SPORT_MK1",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 5665
+      },
+      {
+        "fingerprint": "FORD_ESCAPE_MK4",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 5665
+      },
+      {
+        "fingerprint": "FORD_ESCAPE_MK4_5",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_ESCAPE_MK4_5",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      },
+      {
+        "fingerprint": "FORD_EXPLORER_MK6",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 5665
+      },
+      {
+        "fingerprint": "FORD_EXPEDITION_MK4",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_EXPEDITION_MK4",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      },
+      {
+        "fingerprint": "FORD_F_150_MK14",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_F_150_MK14",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      },
+      {
+        "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_F_150_LIGHTNING_MK1",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      },
+      {
+        "fingerprint": "FORD_FOCUS_MK4",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 5665
+      },
+      {
+        "fingerprint": "FORD_MAVERICK_MK1",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 5665
+      },
+      {
+        "fingerprint": "FORD_MUSTANG_MACH_E_MK1",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_MUSTANG_MACH_E_MK1",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      },
+      {
+        "fingerprint": "FORD_RANGER_MK2",
+        "custom_enabled": false,
+        "cycles": 5000,
+        "identical_packets": 3165
+      },
+      {
+        "fingerprint": "FORD_RANGER_MK2",
+        "custom_enabled": true,
+        "cycles": 5000,
+        "identical_packets": 7165
+      }
+    ],
+    "total_cycles": 85000,
+    "total_identical_packets": 90305,
+    "candidate_source_sha256": {
+      "opendbc/car/ford/carcontroller.py": "6d33f288de87e3baa69e9161b1b85367dc1d8542c06c3d3a9ce2d1f2347dbd30",
+      "opendbc/car/ford/fordcan.py": "0e241f19f152df897b294d4562bfd729dbfcbc56bcc9770379f76922f2864cb8",
+      "opendbc/car/structs.py": "82ecc4de1e5fda486d68fcf67903098dd083a56b78e65866744f42d5fb97b385"
+    },
+    "checker_sha256": "1d33a07cc5e6188c6d1b5de2a2a603efaee691d25d91b7bcd843ab4909753ae1"
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "167ae5a01fdd7ea014e6ad3fe9d0b6e31c67de8ba057ec5ecf18ab38fc16353f",
+    "openpilot/selfdrive/controls/controlsd.py": "c9b68431d212178ae2177b16ddba4cf023ece84c7c0ea8a1db02a2527dc27aba",
+    "openpilot/cereal/custom.capnp": "c877eac4a77ea4cb42447edcf38da4baf20708e8993124852234008e0a084664",
+    "openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py": "45bfaafa9a96d3ccbb56f34ec0b9a71abb7cd7f01e7e80620796d13c222e4720",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "3dc4f7236937358aad09b544577a796e47c15dbc96607739ff0874b900d55089",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "073b16ffa611d654b954711f9e4f24df95477dfc0c09e75eff89d02eb29d7f2a",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "5b082f3c1f6dc596a40a2011c71928debeb04fa70af36841f6f9a237a9ca439e",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "e37a662618b6ccd2620ac355b4cc40a2253707bb4ba1d5d4631fdc89fd01a800",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "36ac7f6177de2679d35c5f7f77234336e17a8632d31b195f40aec6014efb8577",
+    "openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py": "53a3f1f807c638661c8ef60b5dc5c28ecf5604a9d35b610b8e4a5f5d1d99eedb",
+    "tools/ford_pscm_lab/feedback_replay.py": "860aff9fd00d26b2bd7c2b627286918d3b52c25cc31b0b0768a51fc55b0df37e"
+  },
+  "validation_environment": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps",
+    "PYTHONDONTWRITEBYTECODE": "1",
+    "LOG_ROOT": "/private/tmp/ford-upstream-logs",
+    "PARAMS_ROOT": "/private/tmp/ford-upstream-params"
+  },
+  "panda_safety_changed": false,
+  "limitations": [
+    "No full device build, boot, installation or physical steering validation.",
+    "Upstream means the pinned upstream implementation merged into this branch, not an upgrade to unrelated latest source."
+  ]
+}

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,15 @@ 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;
+  enabled @5 :Bool;          # Startup-selected custom controller; independent of command validity.
+}
+
 struct BackupManagerSP @0xf98d843bfd7004a3 {
   backupStatus @0 :Status;
   restoreStatus @1 :Status;
@@ -447,6 +457,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 +490,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

@@ -2119,6 +2119,9 @@ struct Joystick {
   # convenient for debug and live tuning
   axes @0: List(Float32);
   buttons @1: List(Bool);
+  fordChannel @2 :FordChannel;
+
+  enum FordChannel { standard @0; c0 @1; c1 @2; }
 }
 
 struct DriverStateV2 {
@@ -2642,7 +2645,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 = std::min(filtered.size(), out_size);
-    for (size_t i = 0; i < count; i++) {
-      out[i] = return_string(filtered[i]);
+    size_t count = 0;
+    for (const auto &key : handle->keys) {
+      if (flag == ALL || (handle->params.getKeyFlag(key) & flag)) {
+        // 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,9 @@ 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"}},
+    {"FordC0TimeBased", {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
@@ -13,6 +14,8 @@ from openpilot.common.swaglog import cloudlog
 from opendbc.car.car_helpers import interfaces
 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
 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 +47,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 +55,13 @@ class Controls(ControlsExt):
     self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
+    self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
+                                                              c0_time_based=self.params.get_bool("FordC0TimeBased"))
+    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__ if self.ford_model_action else "upstream")
+    self.ford_path = FordPath()
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -155,6 +165,28 @@ 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, current_curvature=self.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]),
+          driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
+          pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
+        )
+        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)
+        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,216 @@
+"""Opt-in Ford C2-free model mapping with measured-curvature PI feedback.
+
+C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s),
+including base-heading overflow. C1
+combines the selected curvature's heading with proportional and integrated
+tracking error. Reference distance and gains are explicit trial choices.
+Commands use the current bounded request without an additional C0/C1 slew.
+"""
+import math
+import struct
+
+import numpy as np
+
+from opendbc.car.ford.values import CarControllerParams, FordFlags
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
+
+
+OFFSET_STATION_M = 7.0
+HEADING_TIME_S = 1.0
+C1_PROPORTIONAL_GAIN = 0.75  # Drive-trial gains, not a learned calibration.
+C1_INTEGRAL_GAIN = 0.25
+CALIBRATION_APPROVED = False
+
+
+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, *, c0_time_based=False):
+  """Encode a circular-arc offset and max(7, v*1s)*selected curvature.
+
+  The arc starts at zero lateral position and heading. Original model geometry
+  remains a health gate; selected curvature supplies both path commands.
+  """
+  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()
+  # (1-cos(S*k))/k, using sinc to avoid cancellation near zero curvature.
+  distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S) if c0_time_based else OFFSET_STATION_M
+  half_heading = .5*distance*desired_curvature
+  sinc = math.sin(half_heading)/half_heading if half_heading else 1.
+  c0 = .5*desired_curvature*distance**2*sinc**2
+  c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
+  return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
+
+
+class ModelActionController:
+  """Integrated tracking error is the only accumulated correction.
+
+  Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
+  """
+  __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based')
+
+  def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
+    if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.:
+      raise ValueError('PI gains must be finite and nonnegative')
+    self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain)
+    self.c0_time_based = bool(c0_time_based)
+    self.reset()
+
+  def reset(self):
+    self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0.
+
+  def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
+             feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None):
+    feedback_dt = dt if feedback_dt is None else feedback_dt
+    reference = desired_curvature if feedback_curvature is None else feedback_curvature
+    if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference) or not .002 <= dt <= .1
+        or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.):
+      self.reset()
+      return FordPath()
+    target = encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based)
+    if not target.valid:
+      self.reset()
+      return FordPath()
+    self.feedback_curvature = reference
+    error = reference-current_curvature
+    self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*error if feedback_enabled else 0.
+    if not _finite(self.proportional):
+      self.reset()
+      return FordPath()
+    base = float(np.clip(target.path_angle, -.5, .5))
+    offset = float(np.clip(target.path_offset+OFFSET_STATION_M*(target.path_angle-base), -5.11, 5.11))
+    self.c0 = offset
+    if feedback_enabled:
+      increment = self.integral_gain*error*speed*feedback_dt
+      if not _finite(increment):
+        self.reset()
+        return FordPath()
+      direction = current_curvature if current_curvature else self.c1
+      if pscm_limited and increment*direction > 0.:
+        increment = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.)))
+      # Retire existing I before limiting new accumulation; never cross zero
+      # through this step. New I is bounded by the combined command's range.
+      relief = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.)))
+      self.correction += relief
+      increment -= relief
+      request = base+self.proportional+self.correction
+      self.correction += float(np.clip(increment, min(-.5-request, 0.), max(.5-request, 0.)))
+    else:
+      self.correction = 0.
+    self.c1 = float(np.clip(base+self.proportional+self.correction, -.5, .5))
+    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 core.
+  Feedback advances once per fresh steering measurement; repeated samples
+  still use the current request. Raw model geometry is checked on every cycle.
+
+  CAN yaw remains a health gate, not the feedback measurement. Driver override
+  clears the correction. Fresh PSCM limits only inhibit outward integration;
+  neither a limit nor a repeated measurement freezes the model request.
+  """
+  def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
+    self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based)
+    self.hypothesis = 'model-action-curvature-c0-distance-pi-v13'
+    self.reset()
+
+  def set_c0_time_based(self, enabled, *, lateral_engaged):
+    """Apply a distance change only after lateral assistance is disengaged."""
+    if lateral_engaged or self.core.c0_time_based == bool(enabled):
+      return False
+    self.core.c0_time_based = bool(enabled)
+    self.reset('c0_distance_changed')
+    return True
+
+  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': self.hypothesis,
+                        'c0_time_based': self.core.c0_time_based,
+                        'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
+
+  def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time,
+             reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None,
+             feedback_curvature=None):
+    reason = None
+    if not active:
+      reason = 'inactive'
+    elif not valid:
+      reason = 'invalid_service'
+    elif not _finite(desired_curvature, current_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 or abs(current_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
+    feedback_dt = 0. if self.last_measurement_time is None else measurement_time-self.last_measurement_time
+    if not .002 <= dt <= .1 or not 0. <= feedback_dt <= .15 or (
+      self.last_model_time is not None and model_time < self.last_model_time
+    ):
+      self.reset('timing_reset')
+      return FordPath()
+    status_fresh = (pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0
+                    and -.005 <= now-pscm_status.canMonoTime*1e-9 <= .15)
+    pscm_limited = bool(status_fresh and pscm_status.limit == 2)
+    driver_override = bool(driver_pressed or not _finite(driver_torque)
+                           or abs(driver_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
+                           or (status_fresh and pscm_status.limit == 3))
+    feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2)))
+    command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt,
+                               feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited,
+                               feedback_curvature=feedback_curvature)
+    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
+    raw_heading = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
+    base_heading = float(np.clip(raw_heading, -.5, .5))
+    self.diagnostics = {'status': 'active', 'hypothesis': self.hypothesis,
+                        'c0_time_based': self.core.c0_time_based,
+                        'offset_distance': max(OFFSET_STATION_M, speed*HEADING_TIME_S) if self.core.c0_time_based else OFFSET_STATION_M,
+                        'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
+                        '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,
+                        'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt,
+                        'heading_feedforward': base_heading,
+                        'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading),
+                        'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
+                        'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain,
+                        'integral_gain': self.core.integral_gain, 'feedback_curvature': self.core.feedback_curvature,
+                        'feedback_error': self.core.feedback_curvature-current_curvature,
+                        'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh),
+                        'command': (command.path_offset, command.path_angle, 0., 0.)}
+    return command
+
+
+def select_model_action_controller(CP, enabled, *, c0_time_based=False):
+  """Only opt-in Ford CAN FD vehicles override upstream curvature control."""
+  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD
+  if enabled and compatible:
+    return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, c0_time_based=c0_time_based)
+  return None

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_c0_distance.py

@@ -0,0 +1,167 @@
+"""C0 distance mapping and live setting changes, without vehicle hardware."""
+import ast
+import math
+from pathlib import Path
+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.common.params import Params, ParamKeyFlag, ParamKeyType
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController, encode_model_action
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import _method, update
+from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
+
+
+@pytest.mark.parametrize('speed', [.3, 3., 7., 8.94, 13.41, 26.82, 55.])
+@pytest.mark.parametrize('curvature', [-.03, -1e-9, 0., 1e-9, .03])
+def test_arc_distance_changes_only_c0_above_seven_meters_per_second(speed, curvature):
+  fixed = encode_model_action(straight(), curvature, speed)
+  timed = encode_model_action(straight(), curvature, speed, c0_time_based=True)
+  distance = max(7., speed)
+  # Independent small-angle expansion avoids cancellation at nearly zero k.
+  expected = (.5*curvature*distance**2 if abs(curvature) < 1e-6 else (1-math.cos(curvature*distance))/curvature)
+  assert timed.path_offset == pytest.approx(expected)
+  assert timed.path_angle == fixed.path_angle
+  assert timed.curvature == timed.curvature_rate == 0.
+  if speed <= 7.:
+    assert timed == fixed
+
+
+@pytest.mark.parametrize('enabled', [False, True])
+def test_reversal_and_zero_request_remain_immediate_on_the_wire(enabled):
+  core = ModelActionController(c0_time_based=enabled)
+  packer = CANPacker('ford_lincoln_base_pt')
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+  bus = CanBus(fingerprint={0: {}})
+  for i, k in enumerate([.01]*100+[-.01, 0.]):
+    command = core.update(straight(), k, current_curvature=k, speed=20., dt=.01)
+    packet = create_lat_ctl2_msg(packer, bus, 2, -command.path_offset, -command.path_angle, 0., 0., i % 16)
+    parser.update([i*10_000_000, [packet]])
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-command.path_offset)
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-command.path_angle)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert command.path_offset*k >= 0. and command.path_angle*k >= 0.
+    if k == 0.:
+      assert command == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_same_feedback_produces_identical_c1_and_integral_in_both_modes():
+  cores = [ModelActionController(c0_time_based=mode) for mode in (False, True)]
+  model = straight()
+  for i in range(2000):
+    k = .03*math.sin(i*.03)
+    kwargs = {'current_curvature': .02*math.sin(i*.03-.5), 'speed': 20., 'dt': .01,
+              'feedback_enabled': i % 77 != 0, 'pscm_limited': i % 3 == 0}
+    outputs = [core.update(model, k, **kwargs) for core in cores]
+    assert outputs[0].path_angle == outputs[1].path_angle
+    assert cores[0].correction == cores[1].correction
+    assert cores[0].proportional == cores[1].proportional
+    for out in outputs:
+      assert abs(out.path_offset) <= 5.110001 and abs(out.path_angle) <= .500001
+
+
+@pytest.mark.parametrize('initial', [False, True])
+def test_change_resets_feedback_and_timestamps_but_a_noop_does_not(initial):
+  controller = FordModelActionController(c0_time_based=initial)
+  for i in range(20):
+    update(controller, 1.+i*.01, current_curvature=0.)
+  assert controller.core.correction > 0.
+  before = controller.diagnostics.copy()
+  assert not controller.set_c0_time_based(not initial, lateral_engaged=True)
+  assert controller.diagnostics == before and controller.core.c0_time_based == initial
+  assert not controller.set_c0_time_based(initial, lateral_engaged=False)
+  assert controller.diagnostics == before
+  assert controller.set_c0_time_based(not initial, lateral_engaged=False)
+  assert controller.core.correction == controller.core.proportional == controller.core.c0 == controller.core.c1 == 0.
+  assert controller.last_time is controller.last_measurement_time is controller.last_model_time is None
+  assert controller.diagnostics['status'] == 'c0_distance_changed'
+  assert controller.diagnostics['c0_time_based'] == (not initial)
+  assert update(controller, 10.) == update(FordModelActionController(c0_time_based=not initial), 10.)
+  assert controller.diagnostics['offset_distance'] == (7. if initial else 20.)
+
+
+@pytest.fixture
+def runtime(tmp_path):
+  params = Params(str(tmp_path))
+  params.put_bool('FordModelActionController', True, block=True)
+  controls = startup(params=params)
+  controls.CP.lateralTuning = SimpleNamespace(which=lambda: 'angle')
+  controls._param_update_time = 0.
+  controls.blinker_pause_lateral = SimpleNamespace(get_params=lambda: None)
+  clock = SimpleNamespace(now=4., monotonic=lambda: clock.now)
+  events = []
+  filename = Path(__file__).resolve().parents[3]/'sunnypilot/selfdrive/controls/controlsd_ext.py'
+  method = _method(filename, 'ControlsExt', 'get_params_sp')
+  env = {'time': clock, 'PARAMS_UPDATE_PERIOD': 3., 'messaging': SimpleNamespace(SubMaster=object),
+         'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: events.append((args, kwargs)))}
+  exec(compile(ast.Module(body=[method], type_ignores=[]), str(filename), 'exec'), env)
+  controls.refresh = lambda sm: env['get_params_sp'](controls, sm)
+  return controls, params, clock, events
+
+
+class EngagementMessages(dict):
+  healthy = True
+
+  def all_checks(self, services):
+    return self.healthy and all(service in self for service in services)
+
+
+@pytest.mark.parametrize('mads_available', [False, True])
+def test_running_process_defers_changes_until_disengaged_and_honors_poll_period(runtime, mads_available):
+  controls, params, clock, events = runtime
+  mads = SimpleNamespace(available=mads_available, enabled=True, active=False)  # includes a paused MADS state
+  standard = SimpleNamespace(enabled=True, active=False)
+  sm = EngagementMessages(selfdriveStateSP=SimpleNamespace(mads=mads), selfdriveState=standard)
+  params.put_bool('FordC0TimeBased', True, block=True)
+  controller = controls.ford_path_controller
+  update(controller, current_curvature=0.)
+  controls.refresh(sm)
+  assert not controller.core.c0_time_based
+  mads.enabled = standard.enabled = False
+  clock.now = 5.
+  controls.refresh(sm)
+  assert not controller.core.c0_time_based  # next scheduled refresh has not run yet
+  clock.now = 7.01
+  controls.refresh(sm)
+  assert controller.core.c0_time_based and controller.last_time is None
+  assert controls.ford_path_controller is controller  # same controlsd/controller instance
+  assert len(events) == 1
+  params.put_bool('FordC0TimeBased', False, block=True)
+  sm.healthy = False
+  clock.now += 3.01
+  controls.refresh(sm)
+  assert controller.core.c0_time_based  # stale engagement data cannot permit a swap
+  sm.healthy = True
+  clock.now += 3.01
+  controls.refresh(sm)
+  assert not controller.core.c0_time_based and len(events) == 2
+
+
+def test_setting_is_persistent_default_off_and_cannot_enable_custom_control(tmp_path):
+  params = Params(str(tmp_path))
+  assert params.get_default_value('FordC0TimeBased') is False
+  assert params.get_type('FordC0TimeBased') == ParamKeyType.BOOL
+  for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP):
+    assert b'FordC0TimeBased' in params.all_keys(flag)
+  params.put_bool('FordC0TimeBased', True, block=True)
+  assert startup(params=params).ford_path_controller is None
+  params.put_bool('FordModelActionController', True, block=True)
+  assert startup(params=params).ford_path_controller.core.c0_time_based
+  params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
+  assert Params(str(tmp_path)).get_bool('FordC0TimeBased')
+
+
+@pytest.mark.parametrize('speed', [3., 10., 20., 35., 55.])
+def test_timed_mode_retains_bounds_at_extreme_and_nonfinite_requests(speed):
+  core = ModelActionController(c0_time_based=True)
+  for curvature in np.linspace(-1., 1., 101):
+    out = core.update(straight(), curvature, current_curvature=0., speed=speed, dt=.01)
+    assert out.valid and abs(out.path_offset) <= 5.110001 and abs(out.path_angle) <= .500001
+  for invalid in (math.nan, math.inf, -math.inf):
+    assert core.update(straight(), invalid, current_curvature=0., speed=speed, dt=.01) == FordPath()

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -0,0 +1,61 @@
+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.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 (None, FordModelActionController()):
+      with self.subTest(controller=type(controller).__name__):
+        record = self.emit_controls_event('Ford path controller selected',
+                                          SimpleNamespace(ford_path_controller=controller, ford_model_action=controller is not None))
+        self.assertEqual(record['controller'], 'upstream' if controller is None else 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), .03, current_curvature=.015, yaw_rate=.3, 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=.03, curvature=.015,
+                                 sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
+      record = self.emit_controls_event('Ford C2-free path tracking', controls)
+      self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v13')
+      self.assertIs(record['calibration_approved'], False)
+      self.assertEqual(record['command'][2:], [0., 0.])
+      self.assertEqual(record['status'], controller.diagnostics['status'])
+      if active and valid:
+        self.assertAlmostEqual(record['offset_overflow'], .7)

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -0,0 +1,232 @@
+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))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('dt', [.002, .01, .1])
+def test_reversal_sends_current_bounded_request_in_same_cycle(sign, dt):
+  controller = ModelActionController()
+  model = straight()
+  for _ in range(100):
+    controller.update(model, sign*.005, current_curvature=sign*.005, speed=20., dt=.01)
+  # A new opposite request must not retain the previous command's sign while
+  # an extra actuator ramp catches up. Matched feedback isolates that ramp.
+  out = controller.update(model, -sign*.005, current_curvature=-sign*.005, speed=20., dt=dt)
+  target = encode_model_action(model, -sign*.005, 20.)
+  assert out.path_angle == pytest.approx(-sign*.1)
+  assert out.path_offset == pytest.approx(target.path_offset, abs=.005)
+  out = controller.update(model, 0., current_curvature=0., speed=20., dt=dt)
+  assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_selected_action_controls_both_fields_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.
+  assert encode_model_action(model, -.004, 20.).path_offset < 0.
+
+
+def test_arc_offset_uses_selected_curvature_and_is_not_scaled_with_speed():
+  for speed in (2., 7., 20., 35.):
+    target = encode_model_action(straight(.4), 0., speed)
+    assert target == FordPath(True, 0., 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_three_control_states_are_sufficient_for_every_next_output():
+  controller = ModelActionController()
+  assert not hasattr(controller, '__dict__')
+  for i in range(300):
+    copied = ModelActionController()
+    copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction
+    model = straight(.2*math.sin(i*.1))
+    kwargs = {'speed': 20., 'dt': .01}
+    desired = .005*math.cos(i*.03)
+    assert controller.update(model, desired, current_curvature=0., **kwargs) == copied.update(model, desired, current_curvature=0., **kwargs)
+
+
+def test_held_turn_releases_without_a_bias_tail_or_sign_reversal():
+  for sign in (-1., 1.):
+    controller = ModelActionController()
+    for _ in range(400):
+      out = controller.update(circle(sign*.01), sign*.01, current_curvature=sign*.01, speed=20., dt=.01)
+    assert out.path_angle == pytest.approx(sign*.2)
+    previous = np.array([out.path_offset, out.path_angle])
+    for desired in sign*np.linspace(.01, 0., 101):
+      out = controller.update(straight(), desired, current_curvature=desired, speed=20., dt=.01)
+      values = np.array([out.path_offset, out.path_angle])
+      assert (abs(values) <= abs(previous)+1e-8).all()
+      assert (sign*values >= -1e-8).all()
+      previous = values
+    assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_current_request_releases_both_outputs_in_one_cycle():
+  controller = ModelActionController()
+  for _ in range(150):
+    controller.update(straight(1.), .04, current_curvature=.04, speed=20., dt=.01)
+  # C0 starts near .974 + 7*(.8-.5) = 3.074 m, including heading overflow.
+  out = controller.update(straight(), 0., current_curvature=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, current_curvature=.01, speed=20., dt=.01)
+  kwargs = {'speed': 20., 'dt': .01, 'active': True, 'valid': True}
+  kwargs.update(overrides)
+  assert controller.update(straight(), 0., current_curvature=0., **kwargs) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+  assert controller.update(straight(), 0., current_curvature=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: {}})
+  for i in range(600):
+    sign = 1. if i < 300 else -1.
+    out = controller.update(straight(sign*8.), sign*.1, current_curvature=sign*.1, speed=30., dt=.01)
+    fields = np.array([out.path_offset, out.path_angle])
+    assert (abs(fields) <= [5.1100001, .5000001]).all()
+    np.testing.assert_allclose(fields, sign*np.array([5.11, .5]), atol=1e-7)
+    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_valid_path_does_not_shorten_the_selected_curvature_arc():
+  model = make_model([0., 1.], [0., .1], [0., 0.])
+  target = encode_model_action(model, .01, 20.)
+  assert target == encode_model_action(straight(), .01, 20.)
+  assert target.path_offset == pytest.approx((1-math.cos(.07))/.01)
+
+
+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, current_curvature=.01, speed=20., dt=.01)
+  assert controller.update(model, .01, current_curvature=.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), current_curvature=kwargs['desired_curvature'], **kwargs)
+  kwargs[field] = value
+  assert controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **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, current_curvature=.01, speed=20., dt=.01)
+  assert controller.update(model, .01, current_curvature=.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), current_curvature=kwargs['desired_curvature'], **kwargs).valid == valid
+
+
+def test_unrelated_live_model_position_and_heading_do_not_change_selected_arc():
+  x = np.array([0., 6., 12.])
+  y = .4+x*.75
+  target = encode_model_action(make_model(x, y, [2., -2., 1.]), -.01, 20.)
+  assert target.path_offset == pytest.approx(-(1-math.cos(.07))/.01)
+  assert target.path_angle == pytest.approx(-.2)
+
+
+def test_duplicate_stations_keep_valid_geometry_and_current_request():
+  model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
+  assert encode_model_action(model, .01, 20.) == encode_model_action(straight(), .01, 20.)
+  out = ModelActionController().update(model, .01, current_curvature=.01, speed=20., dt=.002)
+  assert out.path_offset == pytest.approx(.24)
+  assert out.path_angle == pytest.approx(.2)
+
+
+@pytest.mark.parametrize('curvature', [-1., -.2, -.1, -.01, -.001, .001, .01, .1, .2, 1.])
+def test_desired_curvature_arc_matches_circle_geometry(curvature):
+  target = encode_model_action(straight(.4), curvature, 20.)
+  assert target.valid
+  assert target.path_offset == pytest.approx((1-math.cos(7.*curvature))/curvature, abs=1e-12)
+
+
+@pytest.mark.parametrize('curvature', [-1e-12, -1e-100, 0., 1e-100, 1e-12])
+def test_near_zero_arc_is_finite_continuous_and_keeps_direction(curvature):
+  target = encode_model_action(straight(.4), curvature, 20.)
+  assert target.valid and math.isfinite(target.path_offset)
+  assert target.path_offset == pytest.approx(24.5*curvature, rel=1e-12, abs=0.)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_large_opposing_model_offset_cannot_override_current_curvature_request(sign):
+  controller = ModelActionController()
+  # Route 120: the path still requests a large right offset as curvature turns left.
+  desired = -sign*.000441
+  out = controller.update(straight(sign*3.3), desired, current_curvature=desired, speed=2.02, dt=.01)
+  assert out.path_offset == pytest.approx(-sign*.01)
+  assert out.path_angle == pytest.approx(-sign*.003, abs=.00025)
+  out = controller.update(straight(sign*3.3), 0., current_curvature=0., speed=2.02, dt=.01)
+  assert out == FordPath(True, 0., 0., 0., 0.)

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -0,0 +1,526 @@
+"""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.fordcan import calculate_lat_ctl2_checksum
+from opendbc.car.ford.values import CAR, CarControllerParams, 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, encode_model_action
+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 CANFD_CARS, car_params, startup
+
+
+def assert_current_request(core, desired, speed):
+  target = encode_model_action(straight(), desired, speed)
+  base = min(.5, max(-.5, target.path_angle))
+  assert core.c0 == pytest.approx(min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base))))
+  assert core.c1 == pytest.approx(min(.5, max(-.5, base+core.proportional+core.correction)))
+
+
+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)
+  kwargs.setdefault('current_curvature', kwargs['desired_curvature'])  # Preserve feedforward-only compatibility probes.
+  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_with_current_request(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(.24)
+
+
+@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(.2)
+
+
+@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_use_current_request_and_revalidate_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(.24)
+  assert result.path_angle == pytest.approx(.2)
+  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_relaxing_selected_request_releases_both_fields_despite_growing_model_path():
+  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 released.path_offset == pytest.approx(0.)
+    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, 'carStateSP': 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('fingerprint', CANFD_CARS)
+def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, fingerprint):
+  call, publication = pipeline
+  sm = Subscriptions(maneuver)
+  controls = startup(car_params(carFingerprint=fingerprint))
+  controller = controls.ford_path_controller
+  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
+  model = straight(.4)
+  model.action = SimpleNamespace(desiredCurvature=.1)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=-.0072, 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 = (-1 if maneuver else 1)*.000125
+  assert controls.desired_curvature == pytest.approx(expected_curvature)
+  assert controller.core.proportional == pytest.approx(.75*20.*expected_curvature)
+  assert controller.core.correction == 0.  # First measurement has no elapsed feedback time.
+  assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0045)
+  assert controls.ford_path.path_offset == pytest.approx(0.)  # Limited curvature arc is below one C0 step.
+  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=fingerprint)
+  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)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline, sign):
+  call, publication = pipeline
+  controls, sm = startup(), Subscriptions(False)
+  controls.sm, controls.desired_curvature = sm, sign*.004
+  model = straight(.4)
+  model.action = SimpleNamespace(desiredCurvature=sign*.004)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=.2, canValid=True, steeringPressed=False, steeringTorque=0.)
+  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)
+  frame = 0
+  # Every fresh error sample integrates within amplitude headroom.
+  # Matched steering removes P and preserves I.
+  for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.005),
+                                           (sign*.004, 0., 100, sign*.005), (sign*.005, 0., 100, 0.),
+                                           (sign*.003, 0., 100, sign*.005), (0., 1.0625, 5, 0.)]:
+    for _ in range(count):
+      now = 1.+frame*.01
+      controls.curvature, cs.steeringTorque = measured, torque
+      sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
+      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 now=now: now)}
+      exec(call, environment)
+      msg = custom.CarControlSP.new_message()
+      exec(publication, {'self': controls, 'CC_SP': msg})
+      _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
+      received = parser.update([round(now*1e9), packets])
+      assert parser.dbc.name_to_msg['LateralMotionControl2'].address in received
+      wire = parser.vl['LateralMotionControl2']
+      assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+      assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+      assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+      assert wire['LatCtl_D2_Rq'] == 2
+      assert wire['LatCtlPath_No_Cnt'] == frame % 16
+      address = parser.dbc.name_to_msg['LateralMotionControl2'].address
+      packet = next(packet for packet in packets if packet[0] == address)
+      assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
+      frame += 1
+    core = controls.ford_path_controller.core
+    expected_p = .75*20.*(sign*.004-measured) if torque == 0. else 0.
+    assert core.proportional == pytest.approx(expected_p)
+    assert core.correction == pytest.approx(expected)
+    assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
+    assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025)
+    assert controls.ford_path.path_offset == pytest.approx(sign*.1)
+
+
+@pytest.mark.parametrize('service_valid', [False, True])
+def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, service_valid):
+  controls, sm = startup(), Subscriptions(False)
+  controls.sm, controls.desired_curvature = sm, .004
+  model = straight(.4)
+  model.action = SimpleNamespace(desiredCurvature=.004)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  for frame in range(103):
+    now = 1.+frame*.01
+    controls.curvature = .004 if frame < 100 else .003
+    sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
+    sm.valid['carStateSP'] = service_valid
+    status = sm['carStateSP'].fordPscmStatus
+    status.valid, status.canMonoTime, status.limit, status.lateralState = True, round(now*1e9), 2, 2
+    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 now=now: now)})
+  controller = controls.ford_path_controller
+  assert controller.diagnostics['pscm_limited'] is service_valid
+  assert controller.core.proportional == pytest.approx(.015)
+  # All three fresh samples may integrate unless the valid PSCM limit blocks it.
+  assert controller.core.correction == pytest.approx(0. if service_valid else .00015)
+  assert cc.latActive and controls.ford_path.valid
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('maneuver', [False, True])
+@pytest.mark.parametrize('same_turn', [False, True])
+def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn):
+  call, publication = pipeline
+  controls, sm = startup(), Subscriptions(maneuver)
+  controls.sm, controls.desired_curvature = sm, sign*.004
+  core = controls.ford_path_controller.core
+  cc = structs.CarControl(latActive=True)
+  speed = 10. if same_turn else 20.
+  cs = SimpleNamespace(vEgo=speed, yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  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=speed, vEgoRaw=speed), 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 frame in range(280):
+    now = 1.+frame*.01
+    desired = sign*(.004 if frame < 200 else .01 if same_turn else -.001)
+    model = straight(sign*(.2 if frame < 200 or same_turn else -.2))
+    model.action = SimpleNamespace(desiredCurvature=-desired if maneuver else desired)
+    sm.messages['lateralManeuverPlan'].desiredCurvature = desired
+    controls.curvature = sign*(.004 if frame < 100 else .006 if same_turn else .001 if frame < 200 else .003)
+    sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9))
+    before = core.c0, core.c1, core.correction
+    exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
+                'time': SimpleNamespace(monotonic=lambda now=now: now)})
+    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01
+    assert abs(core.correction-before[2]) <= abs(increment)+1e-10
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
+    received = parser.update([round(now*1e9), packets])
+    address = parser.dbc.name_to_msg['LateralMotionControl2'].address
+    assert address in received
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16
+    packet = next(packet for packet in packets if packet[0] == address)
+    assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
+    if frame == 199:
+      assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
+  assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
+  if same_turn:
+    assert controls.desired_curvature == pytest.approx(sign*.01)
+    assert sign*controls.ford_path.path_angle >= speed*.01  # No old unwind correction left below the new base.
+  else:
+    assert sign*controls.ford_path.path_angle < 0.
+  assert controls.ford_path.path_offset == pytest.approx(sign*(.24 if same_turn else -.02))
+  controls.ford_path_controller.reset()
+  assert core.c0 == core.c1 == core.correction == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('maneuver', [False, True])
+def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, sign, maneuver):
+  call, publication = pipeline
+  controls, sm = startup(), Subscriptions(maneuver)
+  controls.sm, controls.desired_curvature = sm, -sign*.02
+  core = controls.ford_path_controller.core
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=4., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  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=4., vEgoRaw=4.), 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 frame in range(180):
+    now = 1.+frame*.01
+    desired = -sign*(.02 if frame < 30 else .01 if frame < 80 else 0.)
+    controls.curvature = -sign*(.02 if frame < 30 else .04 if frame < 80 else .01 if frame < 130 else 0.)
+    model = straight(sign*(-.5 if frame < 80 else .05))
+    model.action = SimpleNamespace(desiredCurvature=-desired if maneuver else desired)
+    sm.messages['lateralManeuverPlan'].desiredCurvature = desired
+    sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9))
+    before = core.c0, core.c1, core.correction
+    exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
+                'time': SimpleNamespace(monotonic=lambda now=now: now)})
+    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    if frame == 129:
+      assert 0. < sign*core.correction < .02
+    if frame == 130:
+      # Zero measured error removes P, but does not arbitrarily erase I.
+      assert core.correction == before[2]
+      assert core.proportional == 0.
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
+    received = parser.update([round(now*1e9), packets])
+    address = parser.dbc.name_to_msg['LateralMotionControl2'].address
+    assert address in received
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16
+    packet = next(packet for packet in packets if packet[0] == address)
+    assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
+  assert controls.ford_path.path_angle == pytest.approx(core.correction, abs=.00025)
+  assert 0. < sign*core.correction < .02
+  assert controls.ford_path.path_offset == pytest.approx(0.)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('fingerprint', CANFD_CARS)
+def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerprint):
+  call, publication = pipeline
+  controls, sm = startup(car_params(carFingerprint=fingerprint)), Subscriptions(False)
+  controls.sm, controls.desired_curvature = sm, sign*.1
+  core = controls.ford_path_controller.core
+  model = straight(sign*.2)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint=fingerprint)
+  downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
+  vehicle = SimpleNamespace(out=structs.CarState(vEgo=5., vEgoRaw=5.), 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 frame in range(400):
+    now = 1.+frame*.01
+    desired = sign*(.1 if frame < 150 else .04)
+    model.action = SimpleNamespace(desiredCurvature=desired)
+    # Match the selected request after its real upstream limiter, isolating base allocation.
+    controls.curvature = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.)[0]
+    sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
+    exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
+                'time': SimpleNamespace(monotonic=lambda now=now: now)})
+    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    assert core.correction == 0.
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
+    received = parser.update([round(now*1e9), packets])
+    address = parser.dbc.name_to_msg['LateralMotionControl2'].address
+    assert address in received
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16
+    packet = next(packet for packet in packets if packet[0] == address)
+    assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
+    if frame == 149:
+      assert controls.desired_curvature == pytest.approx(sign*.1)
+      assert controls.ford_path.path_offset == pytest.approx(sign*((1-math.cos(.7))/.1+1.4), abs=.005)
+      assert controls.ford_path.path_angle == pytest.approx(sign*.5)
+      assert controls.ford_path_controller.diagnostics['offset_overflow'] == pytest.approx(sign*1.4)
+  assert controls.ford_path.path_offset == pytest.approx(sign*(1-math.cos(.28))/.04, abs=.005)
+  assert controls.ford_path.path_angle == pytest.approx(sign*.28)
+  assert controls.ford_path_controller.diagnostics['offset_overflow'] == 0.
+
+
+@pytest.mark.parametrize('fingerprint', [*CANFD_CARS, CAR.FORD_ESCAPE_MK4])
+@pytest.mark.parametrize('observer', [False, True])
+def test_toggle_off_preserves_upstream_actuators_and_can(pipeline, fingerprint, observer):
+  call, publication = pipeline
+  settings = {'FordModelActionController': False, 'FordPscmObserver': observer}
+  flags = CAR(fingerprint).config.flags
+  controls = startup(car_params(carFingerprint=fingerprint, flags=flags), SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
+  assert controls.ford_path_controller is None
+  sm = Subscriptions(False)
+  controls.sm, controls.desired_curvature, controls.curvature = sm, .004, 0.
+  # Missing custom model geometry must not inhibit the upstream actuator output.
+  model = SimpleNamespace(action=SimpleNamespace(desiredCurvature=.004))
+  cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  cp = structs.CarParams(flags=int(flags), carFingerprint=fingerprint)
+  downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
+  vehicle = SimpleNamespace(out=structs.CarState(vEgo=5., vEgoRaw=5.), acc_tja_status_stock_values=defaultdict(int),
+                            lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
+  canfd = bool(flags & FordFlags.CANFD)
+  name = 'LateralMotionControl2' if canfd else 'LateralMotionControl'
+  parser = CANParser('ford_lincoln_base_pt', [(name, 20)], downstream.CAN.main)
+  address = parser.dbc.name_to_msg[name].address
+  sent = 0
+  for frame in range(300):
+    active = not 100 <= frame < 200
+    cc = structs.CarControl(latActive=active)
+    cc.actuators.curvature = -.003 if active else 0.  # Distinct from desired curvature; produced by LaC upstream.
+    before = cc.actuators.curvature
+    now = 1.+frame*.01
+    sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
+    exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
+                'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature})
+    assert cc.actuators.curvature == before and cc.latActive == active
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    assert not msg.fordLateralPath.enabled and not msg.fordLateralPath.valid
+    converted = convert_carControlSP(msg.as_reader())
+    assert not converted.fordLateralPath.enabled
+    _, packets = downstream.update(cc.as_reader(), converted, vehicle, round(now*1e9))
+    received = parser.update([round(now*1e9), packets])
+    assert (address in received) == (frame % CarControllerParams.STEER_STEP == 0)
+    if address in received:
+      sent += 1
+      wire = parser.vl[name]
+      assert wire['LatCtlPathOffst_L_Actl'] == wire['LatCtlPath_An_Actl'] == 0.
+      assert wire['LatCtlCrv_NoRate2_Actl' if canfd else 'LatCtlCurv_NoRate_Actl'] == 0.
+      assert wire['LatCtl_D2_Rq' if canfd else 'LatCtl_D_Rq'] == int(active)
+      assert wire['LatCtlRampType_D_Rq'] == 0
+      if canfd:
+        count = frame // CarControllerParams.STEER_STEP % 16
+        assert wire['LatCtlPath_No_Cnt'] == count
+        packet = next(packet for packet in packets if packet[0] == address)
+        assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(int(active), count, packet[1])
+      if frame in (95, 295):
+        assert wire['LatCtlCurv_No_Actl'] == pytest.approx(.003)
+      elif not active:
+        assert wire['LatCtlCurv_No_Actl'] == 0.
+  assert sent == 60

openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py

@@ -0,0 +1,180 @@
+"""C1 feedback behavior; these tests do not simulate a Ford steering plant."""
+import math
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+def tick(controller, desired, measured, **overrides):
+  kwargs = {'current_curvature': measured, 'speed': 20., 'dt': .01}
+  kwargs.update(overrides)
+  return controller.update(straight(.4), desired, **kwargs)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
+  controller, matched = ModelActionController(proportional_gain=0., integral_gain=1.), ModelActionController(proportional_gain=0., integral_gain=1.)
+  for _ in range(100):
+    tick(controller, sign*.004, sign*.004)
+  for _ in range(100):
+    out = tick(controller, sign*.004, sign*.003)
+    baseline = tick(matched, sign*.004, sign*.004)
+  assert controller.correction == pytest.approx(sign*.02)
+  assert out.path_angle == pytest.approx(sign*.1)
+  assert out.path_offset == baseline.path_offset == pytest.approx(sign*.1)
+  for _ in range(100):
+    out = tick(controller, sign*.004, sign*.004)
+  assert controller.correction == pytest.approx(sign*.02)
+  assert out.path_angle == pytest.approx(sign*.1)
+  for _ in range(200):
+    out = tick(controller, sign*.004, sign*.005)
+  assert controller.correction == pytest.approx(-sign*.02)
+  assert out.path_angle == pytest.approx(sign*.06)
+  assert out.curvature == out.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_amplitude_limit_does_not_store_unavailable_feedback(sign):
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  # Fresh error accumulates immediately while the combined command has room.
+  for i in range(10):
+    tick(controller, sign*.01, 0.)
+    assert controller.correction == pytest.approx(sign*.002*(i+1))
+  for _ in range(1000):
+    tick(controller, sign*.01, -sign*.9)
+    assert controller.c1 == pytest.approx(sign*.2+controller.correction)
+    assert abs(controller.correction) <= .3000000001
+  assert controller.c1 == pytest.approx(sign*.5)
+  assert controller.correction == pytest.approx(sign*.3)
+  for _ in range(200):
+    tick(controller, sign*.01, 0.)
+  assert controller.correction == pytest.approx(sign*.3)
+  tick(controller, sign*.01, sign*.02)
+  assert sign*controller.correction < .3  # Unwind is allowed at the cap.
+  assert sign*controller.c1 < .5
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign):
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  for _ in range(100):
+    tick(controller, sign*.004, sign*.004)
+  for _ in range(100):
+    tick(controller, sign*.004, sign*.003, pscm_limited=True)
+  assert controller.correction == 0.
+  out = tick(controller, sign*.004, sign*.005, pscm_limited=True)
+  assert sign*controller.correction < 0.
+  # A limit cannot stall the new model request itself or its unwind command.
+  for _ in range(100):
+    out = tick(controller, 0., 0., pscm_limited=True)
+  assert abs(out.path_angle) < .001
+  assert out.path_offset == pytest.approx(0.)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign):
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  controller.correction = -sign*.02
+  controller.c1 = sign*.06
+  for _ in range(200):
+    out = tick(controller, sign*.004, sign*.003, pscm_limited=True)
+  assert controller.correction == pytest.approx(0.)
+  assert out.path_angle == pytest.approx(sign*.08)
+
+
+def test_driver_intervention_clears_feedback_in_current_command():
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  for _ in range(100):
+    tick(controller, .004, .004)
+  for _ in range(100):
+    tick(controller, .004, .003)
+  assert controller.correction > 0.
+  tick(controller, .004, -.01, feedback_enabled=False)
+  assert controller.correction == 0.
+  assert controller.c1 == pytest.approx(.08)
+  for _ in range(100):
+    out = tick(controller, .004, -.01, feedback_enabled=False)
+  assert controller.correction == 0.
+  assert out.path_angle == pytest.approx(.08)
+
+
+@pytest.mark.parametrize('field,value', [('current_curvature', math.nan), ('current_curvature', None),
+                                       ('current_curvature', 1.01), ('feedback_dt', math.nan),
+                                       ('feedback_dt', -.001), ('feedback_dt', .151), ('active', False)])
+def test_bad_feedback_inputs_and_disengagement_clear_every_control_state(field, value):
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  controller.correction = .03
+  out = tick(controller, .004, .003, **{field: value})
+  assert out == FordPath()
+  assert (controller.c0, controller.c1, controller.correction) == (0., 0., 0.)
+
+
+def adapter_tick(controller, now, **overrides):
+  kwargs = {'current_curvature': .003, 'speed': 20., 'yaw_rate': 0., 'now': now,
+            'measurement_time': now, 'model_time': now, 'reference_time': now, 'active': True}
+  kwargs.update(overrides)
+  return controller.update(straight(.4), .004, **kwargs)
+
+
+def status(now, **overrides):
+  fields = {'valid': True, 'canMonoTime': round(now*1e9), 'limit': 0, 'lateralState': 2, 'denied': False}
+  fields.update(overrides)
+  return SimpleNamespace(**fields)
+
+
+def test_repeated_steering_samples_do_not_reintegrate_error():
+  controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
+  for i in range(100):
+    adapter_tick(controller, 1.+i*.01, current_curvature=.004)
+  before = controller.core.correction
+  for i in range(1, 6):
+    adapter_tick(controller, 1.99+i*.01, measurement_time=1.99)
+    assert controller.core.correction == before
+  adapter_tick(controller, 2.05)
+  assert controller.core.correction == pytest.approx(.02*.06)
+  assert controller.diagnostics['feedback_dt'] == pytest.approx(.06)
+
+
+@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': 1.01},
+                                     {'driver_torque': -1.01}, {'driver_torque': math.nan},
+                                     {'pscm_status': status(2.01, limit=3)},
+                                     {'pscm_status': status(2.01, denied=True)},
+                                     {'pscm_status': status(2.01, lateralState=1)}])
+def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides):
+  controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
+  for i in range(101):
+    adapter_tick(controller, 1.+i*.01)
+  assert controller.core.correction > 0.
+  assert adapter_tick(controller, 2.01, **overrides).valid
+  assert controller.core.correction == 0.
+  assert not controller.diagnostics['feedback_enabled']
+
+
+@pytest.mark.parametrize('overrides,limited', [({}, True), ({'valid': False}, False),
+                                            ({'canMonoTime': 0}, False), ({'canMonoTime': 1_800_000_000}, False),
+                                            ({'canMonoTime': 2_020_000_000}, False), ({'limit': 1}, False)])
+def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, limited):
+  controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
+  for i in range(100):
+    adapter_tick(controller, 1.+i*.01, current_curvature=.004)
+  adapter_tick(controller, 2., pscm_status=status(2., **{'limit': 2, **overrides}))
+  assert controller.diagnostics['pscm_limited'] is limited
+  assert (controller.core.correction == 0.) is limited
+
+
+def test_measurement_cadence_preserves_elapsed_distance_integration():
+  results = []
+  for period in (1, 2, 5):
+    controller = FordModelActionController(proportional_gain=0., integral_gain=1.)
+    for i in range(101):
+      now = 1.+i*.01
+      adapter_tick(controller, now, current_curvature=.004)
+    for i in range(1, 101):
+      now = 2.+i*.01
+      adapter_tick(controller, now, measurement_time=2.+(i//period)*period*.01)
+    results.append(controller.core.correction)
+  assert results == pytest.approx([.02, .02, .02])

openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py

@@ -0,0 +1,83 @@
+"""C1 overflow allocation and release; no assumptions about PSCM response."""
+import math
+
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
+from openpilot.selfdrive.controls.tests.test_ford_model_action import make_model, straight
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('speed', [3., 7., 20., 55.])
+@pytest.mark.parametrize('heading', [.4, .5, .6, .8])
+def test_clipped_base_heading_preserves_the_seven_metre_reference(sign, speed, heading):
+  controller = ModelActionController()
+  desired = sign*heading/max(7., speed)
+  for _ in range(150):
+    out = controller.update(straight(sign*.2), desired, current_curvature=desired, speed=speed, dt=.01)
+  assert controller.correction == 0.
+  arc = (1-math.cos(7.*desired))/desired
+  assert controller.c0 == pytest.approx(arc+sign*7.*max(heading-.5, 0.))
+  assert out.path_offset == pytest.approx(controller.c0, abs=.005)
+  assert out.path_angle == pytest.approx(sign*min(heading, .5))
+  assert out.path_offset+7.*out.path_angle == pytest.approx(arc+sign*7.*heading, abs=.005)
+  assert out.curvature == out.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_combined_offset_is_clipped_after_allocating_heading(sign):
+  controller = ModelActionController()
+  for _ in range(200):
+    out = controller.update(straight(sign*4.), sign*.2, current_curvature=sign*.2, speed=7., dt=.01)
+  assert out.path_offset == pytest.approx(sign*5.11)
+  assert out.path_angle == pytest.approx(sign*.5)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_overflow_uses_existing_reference_with_short_model_path(sign):
+  controller = ModelActionController()
+  model = make_model([0., 1.], [0., sign*.2], [0., 0.])
+  for _ in range(150):
+    out = controller.update(model, sign*.03, current_curvature=sign*.03, speed=20., dt=.01)
+  assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_extra_offset_releases_immediately_without_stored_overflow(sign):
+  controller = ModelActionController()
+  for _ in range(200):
+    controller.update(straight(sign*.2), sign*.04, current_curvature=sign*.04, speed=20., dt=.01)
+  start = sign*((1-math.cos(.28))/.04+2.1)
+  target = sign*(1-math.cos(.14))/.02
+  assert controller.c0 == pytest.approx(start)
+  for _ in range(70):
+    before = controller.c0
+    out = controller.update(straight(sign*.2), sign*.02, current_curvature=sign*.02, speed=20., dt=.01)
+    assert sign*controller.c0 >= sign*target-1e-10
+    assert sign*controller.c0 <= sign*before+1e-10
+    assert controller.c0 == pytest.approx(target)
+  assert out.path_offset == pytest.approx(sign*(1-math.cos(.14))/.02, abs=.005)
+  assert out.path_angle == pytest.approx(sign*.4)
+  assert controller.correction == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign):
+  controller = ModelActionController(proportional_gain=0., integral_gain=1.)
+  for _ in range(200):
+    out = controller.update(straight(sign*.2), sign*.02, current_curvature=0., speed=20., dt=.01)
+  assert out.path_angle == pytest.approx(sign*.5)
+  assert controller.correction == pytest.approx(sign*.1)
+  assert out.path_offset == pytest.approx(sign*(1-math.cos(.14))/.02, abs=.005)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('enabled,limited', [(False, False), (True, True)])
+def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, limited):
+  controller = ModelActionController()
+  for _ in range(150):
+    out = controller.update(straight(sign*.2), sign*.03, current_curvature=sign*.02, speed=20., dt=.01,
+                            feedback_enabled=enabled, pscm_limited=limited)
+  assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005)
+  assert out.path_angle == pytest.approx(sign*.5)
+  assert controller.correction == 0.

openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py

@@ -0,0 +1,102 @@
+"""Explicit PI experiment semantics; no simulated PSCM response."""
+import math
+
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+@pytest.mark.parametrize('gain', [.1, .25, .5, .75])
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_p_responds_without_waiting_for_integral_and_disappears_at_catchup(gain, sign):
+  controller = ModelActionController(proportional_gain=gain)
+  controller.c1 = sign*.2
+  out = controller.update(straight(), sign*.01, current_curvature=sign*.009,
+                          speed=20., dt=.1, feedback_dt=0.)
+  assert controller.proportional == pytest.approx(sign*gain*.02)
+  assert controller.correction == 0.
+  assert out.path_angle == pytest.approx(sign*(.2+gain*.02))
+  out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.1)
+  assert controller.proportional == controller.correction == 0.
+  assert out.path_angle == pytest.approx(sign*.2)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_aligned_feedback_does_not_replace_current_feedforward_or_path(sign):
+  controller = ModelActionController(proportional_gain=.25)
+  controller.c0, controller.c1 = sign*.4, sign*.2
+  out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008,
+                          feedback_curvature=sign*.008, speed=20., dt=.1)
+  assert controller.proportional == controller.correction == 0.
+  assert out.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=.005)
+  assert out.path_angle == pytest.approx(sign*.2)
+  # Latest request is ahead of measured steering, but the delay-aligned target
+  # has already been exceeded. P and I must use the explicit feedback target.
+  out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008,
+                          feedback_curvature=sign*.006, speed=20., dt=.1)
+  assert controller.proportional == pytest.approx(-sign*.01)
+  assert sign*controller.correction < 0.
+  assert sign*out.path_angle < .2
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('gain', [.5, .75])
+def test_pi_combined_request_obeys_amplitude_and_does_not_wind_up_behind_p(sign, gain):
+  controller = ModelActionController(proportional_gain=gain)
+  for _ in range(200):
+    out = controller.update(straight(), sign*.01, current_curvature=-sign*.1, speed=20., dt=.01)
+    assert controller.c1 == pytest.approx(sign*.5)
+    assert abs(out.path_angle) <= .50000001
+    assert controller.correction == 0.  # Feedforward + P alone exceeds the cap.
+  assert out.path_angle == pytest.approx(sign*.5)
+  for _ in range(60):
+    out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.01)
+  assert out.path_angle == pytest.approx(sign*.2)
+  assert controller.correction == controller.proportional == 0.
+
+
+@pytest.mark.parametrize('gain', [-.1, math.nan, math.inf, None, 'bad'])
+def test_invalid_gain_is_rejected(gain):
+  with pytest.raises(ValueError):
+    ModelActionController(proportional_gain=gain)
+
+
+@pytest.mark.parametrize('feedback', [math.nan, math.inf, 'bad', 1.001])
+def test_invalid_feedback_target_clears_all_output(feedback):
+  controller = ModelActionController(proportional_gain=.25)
+  controller.c1, controller.correction = .2, .01
+  out = controller.update(straight(), .01, current_curvature=.008, feedback_curvature=feedback, speed=20., dt=.01)
+  assert out == FordPath()
+  assert controller.proportional == controller.correction == controller.c1 == 0.
+
+
+def test_overflowing_p_cannot_escape_as_an_active_command():
+  controller = ModelActionController(proportional_gain=1e308)
+  out = controller.update(straight(), 1., current_curvature=-1., speed=55., dt=.01)
+  assert out == FordPath()
+
+
+@pytest.mark.parametrize('limited', [False, True])
+def test_driver_override_clears_both_p_and_i(limited):
+  controller = ModelActionController(proportional_gain=.25)
+  controller.c1, controller.correction = .2, .01
+  controller.update(straight(), .01, current_curvature=.008, speed=20., dt=.01,
+                    feedback_enabled=False, pscm_limited=limited)
+  assert controller.proportional == controller.correction == 0.
+
+
+def test_adapter_logs_separate_feedforward_p_i_and_explicit_feedback_target():
+  controller = FordModelActionController(proportional_gain=.25)
+  for i in range(30):
+    now = 1.+i*.01
+    controller.update(straight(), .004, current_curvature=.002, feedback_curvature=.003,
+                      speed=20., yaw_rate=0., now=now, measurement_time=now, model_time=now,
+                      reference_time=now, active=True)
+  d = controller.diagnostics
+  assert d['heading_feedforward'] == pytest.approx(.08)
+  assert d['heading_proportional'] == pytest.approx(.005)
+  assert d['proportional_gain'] == .25 and d['feedback_curvature'] == .003
+  assert d['heading_correction'] > 0.
+  assert d['heading_request'] == pytest.approx(d['heading_feedforward']+d['heading_proportional']+d['heading_correction'])

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -0,0 +1,113 @@
+"""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 CAR, FordFlags
+from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
+from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+
+
+CANFD_CARS = [car for car in CAR if car.config.flags & FordFlags.CANFD]
+
+
+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_path_controller')
+  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,
+                 '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)))
+@pytest.mark.parametrize('fingerprint', [*CANFD_CARS, 'FORD_FUTURE_CANFD'])
+def test_actual_startup_priority(candidate, observer, fingerprint):
+  settings = {'FordModelActionController': candidate, 'FordPscmObserver': observer}
+  selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
+  if candidate:
+    assert type(selected.ford_path_controller) is FordModelActionController
+    assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
+    assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25
+    assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
+  else:
+    assert selected.ford_path_controller is None
+  assert selected.ford_model_action == candidate
+  assert selected.ford_path == FordPath()
+
+
+@pytest.mark.parametrize('overrides', [{'brand': 'tesla'}, {'flags': 0}, {'flags': 8}])
+@pytest.mark.parametrize('observer', [False, True])
+def test_other_vehicles_always_use_upstream(overrides, observer):
+  settings = {'FordModelActionController': False, 'FordPscmObserver': observer}
+  params = SimpleNamespace(get_bool=lambda key: settings.get(key, False))
+  before = startup(car_params(**overrides), params)
+  settings['FordModelActionController'] = True
+  after = startup(car_params(**overrides), params)
+  assert after.ford_path_controller is before.ford_path_controller is None
+  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)
+
+
+def test_candidate_accepts_canfd_with_additional_flags():
+  assert isinstance(startup(car_params(flags=FordFlags.CANFD | 8)).ford_path_controller, FordModelActionController)
+
+
+@pytest.mark.parametrize('observer', [False, True])
+@pytest.mark.parametrize('fingerprint', CANFD_CARS)
+def test_sunnylink_write_takes_effect_on_restart_and_restores_upstream(tmp_path, monkeypatch, observer, fingerprint):
+  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)
+  cp = car_params(carFingerprint=fingerprint)
+  old = startup(cp, params=params)
+  assert old.ford_path_controller is None
+  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'true').decode())
+  enabled = startup(cp, 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 startup(cp, params=params).ford_path_controller is None
+  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 startup(params=params).ford_path_controller is None
+  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_model_action_unwind.py

@@ -0,0 +1,79 @@
+import math
+
+import pytest
+
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_existing_integral_unwinds_in_current_output(sign):
+  core = ModelActionController(.5, .25)
+  core.c1, core.correction = sign*.07, sign*.03
+  core.update(straight(), sign*.002, current_curvature=sign*.004, speed=20., dt=.01)
+  assert core.correction == pytest.approx(sign*.0299)
+  assert core.c1 == pytest.approx(sign*.0499)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_unwinding_cannot_charge_opposite_correction_beyond_amplitude_limit(sign):
+  core = ModelActionController(.5, .25)
+  core.c1, core.correction = sign*.07, sign*.03
+  core.update(straight(), 0., current_curvature=sign*.5, speed=20., dt=.01, feedback_dt=.15)
+  assert core.correction == 0.
+  assert core.c1 == pytest.approx(-sign*.5)
+
+
+@pytest.mark.parametrize('ki', [0., .25, .5, 1.])
+def test_integral_gain_scales_fresh_error_only(ki):
+  core = ModelActionController(0., ki)
+  core.c1 = .04
+  core.update(straight(), .002, current_curvature=.001, speed=20., dt=.01)
+  assert core.correction == pytest.approx(ki*.0002)
+  before = core.correction
+  core.update(straight(), 0., current_curvature=.001, speed=20., dt=.01, feedback_dt=0.)
+  assert core.correction == before
+
+
+def test_zero_error_does_not_erase_holding_correction():
+  core = ModelActionController(.5, .25)
+  core.c1, core.correction = .14, .1
+  for _ in range(50):
+    core.update(straight(), .002, current_curvature=.002, speed=20., dt=.01)
+    assert core.correction == .1
+
+
+@pytest.mark.parametrize('ki', [-1., math.inf, math.nan])
+def test_invalid_integral_gain_is_rejected(ki):
+  with pytest.raises(ValueError):
+    ModelActionController(.25, ki)
+
+
+def test_overflowing_integral_increment_resets():
+  core = ModelActionController(.25, 1e308)
+  assert not core.update(straight(), 1., current_curvature=0., speed=55., dt=.01).valid
+  assert core.c0 == core.c1 == core.correction == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_centering_cannot_gate_continuous_heading_correction(sign):
+  commands = []
+  for offset in (-.1, -.010001, -.01, -.009999, 0., .009999, .01, .010001, .1):
+    core = ModelActionController()
+    core.c0, core.c1, core.correction = offset, sign*.07, sign*.03
+    out = core.update(straight(offset), sign*.002, current_curvature=sign*.004, speed=20., dt=.01)
+    commands.append((out.path_angle, core.correction))
+  assert len(set(commands)) == 1
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('limited', [False, True])
+@pytest.mark.parametrize('gain', [.5, .75])
+def test_duplicate_measurements_cannot_retire_integral(sign, limited, gain):
+  core = ModelActionController(gain, .25)
+  core.c1, core.correction = sign*.07, sign*.03
+  core.update(straight(), -sign*.002, current_curvature=sign*.004, speed=20., dt=.01,
+              feedback_dt=0., pscm_limited=limited)
+  assert core.correction == sign*.03
+  assert core.proportional == pytest.approx(-sign*.12*gain)
+  assert core.c1 == pytest.approx(-sign*(.01+.12*gain))

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/modeld/compile_modeld.py

@@ -37,6 +37,7 @@ from tinygrad.engine.jit import TinyJit
 
 
 NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
+MODELD_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
 
 
 def nv12_copy_size(stride: int, y_height: int, uv_height: int) -> int:
@@ -112,26 +113,58 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
   return frame_prepare_tinygrad
 
 
-def get_npy_shapes(input_shapes, state_pairs):
-  shapes = {'tfm': (3, 3), 'big_tfm': (3, 3)} | {
-    name: shape for name, (shape, _) in input_shapes.items() if name not in state_pairs and name != 'new_img'}
+def get_policy_npy_shapes(input_shapes):
+  dp = input_shapes['desire_pulse']  # (1, 25, 8)
+  tc = input_shapes['traffic_convention']  # (1, 2)
+  at = input_shapes['action_t']  # (1, 2)
+  fb = input_shapes['features_buffer']  # (1, T-1, ...) e.g. (1, 24, 32, 512) with spatial features
+  feat_dim = math.prod(fb[2:])
+  # TODO prev_feat shouldn't exist and be handled inside the JIT, but corrupt on QCOM for now
+  shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], feat_dim)}
   return shapes, [math.prod(s) for s in shapes.values()]
 
 
-def make_input_queues(input_shapes, state_pairs, device, frame_copy_size):
-  shapes, sizes = get_npy_shapes(input_shapes, state_pairs)
+def make_input_queues(input_shapes, frame_skip, device, frame_copy_size):
+  img = input_shapes['img']  # (1, 12, 128, 256)
+  fb = input_shapes['features_buffer']  # (1, T-1, ...), past features only; the model appends the current frame's feature
+  feat_dim = math.prod(fb[2:])
+  dp = input_shapes['desire_pulse']  # (1, 25, 8)
+  n_frames = img[1] // 6
+  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
+
+  policy_shapes, _ = get_policy_npy_shapes(input_shapes)
+  shapes = {'tfm': (3, 3), 'big_tfm': (3, 3)} | policy_shapes
+  sizes = [math.prod(s) for s in shapes.values()]
   packed_npy_size = sum(sizes) * np.dtype(np.float32).itemsize
   packed_input = np.zeros(packed_npy_size + 2 * frame_copy_size, dtype=np.uint8)
   packed_npy_inputs = packed_input[:packed_npy_size].view(np.float32)
   frames = packed_input[packed_npy_size:]
   frame_views = {'img': frames[:frame_copy_size], 'big_img': frames[frame_copy_size:]}
+  # views into the packed inputs, to be refilled at runtime
   npy = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)}
-  input_queues = {name: Tensor(np.zeros(shape, dtype=dtype.fmt), device=device).realize()
-                  for name, (shape, dtype) in input_shapes.items() if name in state_pairs}
-  input_queues['packed_npy_inputs'] = Tensor(packed_input, device='NPY').realize()
+  input_queues = {
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32), device=device).contiguous().realize(),
+    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
+    'packed_npy_inputs': Tensor(packed_input, device='NPY').realize(),
+  }
   return input_queues, npy, frame_views
 
 
+def shift_and_sample(buf, new_val, sample_fn):
+  buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
+  return sample_fn(buf)
+
+
+def sample_skip(buf, frame_skip):
+  return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
+
+
+def sample_desire(buf, frame_skip):
+  return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
+
+
 def make_warp(nv12, model_w, model_h):
   frame_prepare = make_frame_prepare(nv12, model_w, model_h)
 
@@ -149,27 +182,54 @@ def make_warp(nv12, model_w, model_h):
   return warp
 
 
-def make_run_model(warp, model_runner, input_shapes, state_pairs, frame_copy_size):
-  shapes, sizes = get_npy_shapes(input_shapes, state_pairs)
-  packed_npy_size = sum(sizes) * np.dtype(np.float32).itemsize
+def make_run_policy(model_runner, model_metadata, frame_skip):
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
+  model_input_dtypes = {name: spec.dtype for name, spec in model_runner.graph_inputs.items()}
 
-  def run_model(packed_npy_inputs, **state_inputs):
-    packed_input = packed_npy_inputs.to(Device.DEFAULT).realize()
+  def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
+    packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
+    Tensor.realize(packed_npy_inputs, warped)
+
+    img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn)
+
+    desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True))
+    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
+    feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn)
+
+    inputs = {
+      'img': img,
+      'big_img': big_img,
+      'features_buffer': feat_buf.reshape(model_metadata['input_shapes']['features_buffer']),
+      'desire_pulse': desire_buf,
+      'traffic_convention': traffic_convention,
+      'action_t': action_t,
+    }
+    inputs = {name: value.cast(model_input_dtypes[name]) for name, value in inputs.items()}
+    out = next(iter(model_runner(inputs).values())).cast('float32')
+    return out,
+  return run_policy
+
+
+def make_run_model(warp, run_policy, model_metadata, frame_copy_size):
+  _, policy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
+  packed_npy_size = (18 + sum(policy_sizes)) * np.dtype(np.float32).itemsize
+
+  def run_model(img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
+    packed_input = packed_npy_inputs.to(Device.DEFAULT)
+    Tensor.realize(packed_input)
     packed_npy_inputs = packed_input[:packed_npy_size].bitcast('float32')
-    inputs = {name: t.reshape(s) for (name, s), t in zip(shapes.items(), packed_npy_inputs.split(sizes), strict=True)}
     frame = packed_input[packed_npy_size:packed_npy_size + frame_copy_size]
     big_frame = packed_input[packed_npy_size + frame_copy_size:]
-    inputs['new_img'] = warp(inputs.pop('tfm'), inputs.pop('big_tfm'), frame, big_frame)
-    inputs = {name: value.cast(input_shapes[name][1]) for name, value in inputs.items()}
-    outputs = {name: value.contiguous() for name, value in model_runner(inputs | state_inputs).items()}
-    Tensor.realize(*outputs.values())
-    if state_pairs:
-      Tensor.realize(*(state_inputs[name].assign(outputs[next_name]) for name, next_name in state_pairs.items()))
-    return tuple(value for name, value in outputs.items() if name not in state_pairs.values())
+    tfm, big_tfm, policy_inputs = packed_npy_inputs.split([9, 9, sum(policy_sizes)])
+    warped = warp(tfm.reshape(3, 3), big_tfm.reshape(3, 3), frame, big_frame)
+    return run_policy(warped, img_q, big_img_q, feat_q, desire_q, policy_inputs)
   return run_model
 
 
-def compile_jit(jit, make_queues, benchmark_runs):
+def compile_jit(jit, input_keys, make_queues, benchmark_runs):
   if benchmark_runs < 1:
     raise ValueError("benchmark_runs must be at least 1")
 
@@ -185,7 +245,7 @@ def compile_jit(jit, make_queues, benchmark_runs):
         v[:] = rng.integers(0, 256, size=v.shape, dtype=np.uint8)
       Device.default.synchronize()
       st = time.perf_counter()
-      outs = fn(**input_queues)
+      outs = fn(**{k: input_queues[k] for k in input_keys})
       mt = time.perf_counter()
       Device.default.synchronize()
       et = time.perf_counter()
@@ -240,6 +300,7 @@ if __name__ == "__main__":
                  help='camera resolutions WxH (one or more)')
   p.add_argument('--onnx', required=True)
   p.add_argument('--output', required=True)
+  p.add_argument('--frame-skip', type=int, required=True)
   p.add_argument('--benchmark-runs', type=int, default=1,
                  help='timed loaded-JIT runs for each correctness seed')
   args = p.parse_args()
@@ -248,28 +309,24 @@ if __name__ == "__main__":
   model_w, model_h = args.model_size
 
   model_runner = OnnxRunner(model_path)
-  input_shapes = {name: (spec.shape, spec.dtype) for name, spec in model_runner.graph_inputs.items()}
-  state_pairs = {name: f'next_{name}' for name in input_shapes if f'next_{name}' in model_runner.graph_outputs}
   out = {
     'metadata': make_metadata_dict(model_path),
-    'input_shapes': input_shapes,
-    'state_pairs': state_pairs,
     'input_devices': {'model': Device.DEFAULT},
     'run_model': {},
   }
 
+  run_policy = make_run_policy(model_runner, out['metadata'], args.frame_skip)
+
   for cam_w, cam_h in args.camera_resolutions:
     nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
     frame_copy_size = nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-    make_model_queues = partial(make_input_queues, input_shapes, state_pairs,
+    make_model_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip,
                                 frame_copy_size=frame_copy_size)
     warp = make_warp(nv12, model_w, model_h)
-    run_model_jit = TinyJit(make_run_model(warp, model_runner, input_shapes, state_pairs, frame_copy_size), prune=True)
-    out['run_model'][(cam_w,cam_h)] = compile_jit(run_model_jit, make_model_queues, args.benchmark_runs)
+    run_model_jit = TinyJit(make_run_model(warp, run_policy, out['metadata'], frame_copy_size), prune=True)
+    out['run_model'][(cam_w,cam_h)] = compile_jit(run_model_jit, MODELD_INPUTS, make_model_queues,
+                                                  args.benchmark_runs)
 
   with open(args.output, "wb") as f:
     dump_oob(out, f)
-  with open(args.output, "rb") as f:
-    load_oob(f)
-    assert not f.read(1), "unexpected model buffer data"
   print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)")

openpilot/selfdrive/modeld/modeld.py

@@ -5,6 +5,8 @@ from functools import cached_property
 import os
 os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
 from tinygrad.device import Device
+import usb1
+import struct
 import threading
 import time
 import numpy as np
@@ -26,13 +28,14 @@ from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, should_stop, smooth_value, get_curvature_from_plan
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
-from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, nv12_copy_size
+from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, nv12_copy_size, MODELD_INPUTS
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
 from openpilot.common.file_chunker import open_file_chunked
+from openpilot.common.hardware.usb import CHESTNUT_USB_IDS
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
-from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob
+from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, chestnut_ready, modeld_pkl_path, load_oob
 
-from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
@@ -72,14 +75,45 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
                                 shouldStop=bool(stop))
 
 
-class ChestnutGpuState:
-  # GPU metrics require modeld's GPU context
+class ChestnutState:
+  # only modeld can access chestnut
   def __init__(self, pm: PubMaster, big: bool):
     self.pm = pm
     self.big = big
     self.valid = True
     self.sends = 0
     self.metrics = {}
+    self._asm_usb = None
+
+  def _close_asm_usb(self) -> None:
+    if self._asm_usb is not None:
+      self._asm_usb.close()
+      self._asm_usb = None
+
+  def _open_asm_usb(self):
+    context = usb1.USBContext()
+    for vendor_id, product_id in CHESTNUT_USB_IDS:
+      if (handle := context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)) is not None:
+        return handle
+    context.close()
+
+  def _read_ina(self) -> tuple[int, int, bool]:
+    if "AMD" in Device._opened_devices and self._asm_usb is None:
+      try:
+        raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5)
+        return struct.unpack('<Hh?', bytes(raw))
+      except Exception:
+        pass
+    if self._asm_usb is None:
+      self._asm_usb = self._open_asm_usb()
+    if self._asm_usb is None:
+      raise usb1.USBErrorNoDevice
+    try:
+      raw = self._asm_usb.controlRead(0xC0, 0xC0, 0, 0, 5, timeout=100)
+    except usb1.USBError:
+      self._close_asm_usb()
+      raise
+    return struct.unpack('<Hh?', bytes(raw))
 
   @cached_property
   def power_limit(self) -> int:
@@ -87,8 +121,8 @@ class ChestnutGpuState:
     return smu._send_msg(smu.smu_mod.PPSMC_MSG_GetPptLimit, 0, read_back_arg=True, timeout=100)
 
   def send(self) -> None:
-    msg = messaging.new_message('chestnutGpuState')
-    state = msg.chestnutGpuState
+    msg = messaging.new_message('chestnutState')
+    state = msg.chestnutState
     self.sends += 1
     if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
       try:
@@ -114,8 +148,21 @@ class ChestnutGpuState:
       for k, v in self.metrics.items():
         setattr(state, k, v)
 
-    msg.valid = not self.big or (self.valid and bool(self.metrics))
-    self.pm.send('chestnutGpuState', msg)
+    asm_valid = False
+    try:
+      # ASM runs on USB-C power, these still read without a gpu
+      state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina()
+      asm_valid = True
+    except Exception:
+      pass
+    if "AMD" in Device._opened_devices:
+      try:
+        state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0]
+      except Exception:
+        pass
+
+    msg.valid = asm_valid and (not self.big or self.valid)
+    self.pm.send('chestnutState', msg)
 
 
 class FrameMeta:
@@ -137,17 +184,17 @@ class ModelState(ModelStateBase):
     input_devices = jits['input_devices']
     self.model_device = input_devices['model']
     metadata = jits['metadata']
-    self.input_shapes = jits['input_shapes']
-    self.state_pairs = jits['state_pairs']
-    self.vision_input_names = ('img', 'big_img')
+    self.input_shapes = metadata['input_shapes']
+    self.vision_input_names = [k for k in self.input_shapes if 'img' in k]
     self.output_slices = metadata['output_slices']
 
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
     self.chestnut = chestnut
 
+    self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
     self.frame_copy_size = nv12_copy_size(*get_nv12_info(cam_w, cam_h)[:3])
     self.input_queues, self.npy, self.frame_views = make_input_queues(
-      self.input_shapes, self.state_pairs, device=self.model_device, frame_copy_size=self.frame_copy_size)
+      self.input_shapes, self.frame_skip, device=self.model_device, frame_copy_size=self.frame_copy_size)
     self.parser = Parser()
     self.run_model = jits['run_model'][(cam_w,cam_h)]
 
@@ -169,13 +216,14 @@ class ModelState(ModelStateBase):
     self.npy['tfm'][:,:] = transforms['img'][:,:]
     self.npy['big_tfm'][:,:] = transforms['big_img'][:,:]
 
-    outs, = self.run_model(**self.input_queues)
+    outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
     if after_enqueue is not None:
       after_enqueue()
     model_output = outs.numpy()[0]
     if self.chestnut and not np.all(np.isfinite(model_output)):
       raise RuntimeError("model output not finite")
     outputs_dict = self.parser.parse_outputs(self.slice_outputs(model_output, self.output_slices))
+    self.npy['prev_feat'][:] = model_output[self.output_slices['hidden_state']]
 
     if SEND_RAW_PRED:
       outputs_dict['raw_pred'] = model_output.copy()
@@ -187,19 +235,32 @@ class ModelState(ModelStateBase):
     dims = {'desire_pulse': ModelConstants.DESIRE_LEN, 'traffic_convention': 2, 'action_t': 2}
     self.run(dummy_frames, dict.fromkeys(self.vision_input_names, eye), {k: np.zeros(v, dtype=np.float32) for k, v in dims.items()})
     self.input_queues, self.npy, self.frame_views = make_input_queues(
-      self.input_shapes, self.state_pairs, device=self.model_device, frame_copy_size=self.frame_copy_size)
+      self.input_shapes, self.frame_skip, device=self.model_device, frame_copy_size=self.frame_copy_size)
     self.prev_desire[:] = 0
 
 
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  CHESTNUT = chestnut_present() and chestnut_compiled()
+  chestnut_available = chestnut_present() and chestnut_compiled()
+  CHESTNUT = False
+  if chestnut_available:
+    poller = messaging.Poller()
+    sock = messaging.sub_sock("chestnutState", poller=poller, conflate=True)
+    deadline = time.monotonic() + 4. / SERVICE_LIST['deviceState'].frequency
+    while not CHESTNUT and (remaining := deadline - time.monotonic()) > 0.:
+      if not poller.poll(round(remaining * 1000)):
+        break
+      msg = messaging.recv_one_or_none(sock)
+      CHESTNUT = msg is not None and msg.valid and chestnut_ready(msg.chestnutState)
   if CHESTNUT:
     os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
   params = Params()
   params.put_bool("ChestnutLoading", CHESTNUT)
-  params.remove("ChestnutActive")
+  if chestnut_available and not CHESTNUT:
+    params.put_bool("ChestnutActive", False)
+  else:
+    params.remove("ChestnutActive")
 
   config_realtime_process(7, 54)
 
@@ -243,7 +304,11 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
+    if model is None:
+      params.put_bool("ChestnutModelError", True)
     params.put_bool("ChestnutActive", model is not None)
+    if model is not None:
+      params.remove("ChestnutModelError")
 
   small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or CHESTNUT else None
   if model is None:
@@ -253,13 +318,13 @@ def main(demo=False):
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
-  chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -279,6 +344,7 @@ def main(demo=False):
   else:
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
+  ford_model_action = params.get_bool("FordModelActionController")
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
   # TODO Move smooth seconds to action function
@@ -328,6 +394,7 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay)
     lat_delay = sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lat_delay += get_ford_delay_offset(CP, ford_model_action, v_ego)
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
@@ -370,13 +437,14 @@ def main(demo=False):
     mt1 = time.perf_counter()
     try:
       send_chestnut = (chestnut_state is not None and
-                       run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0)
+                       run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
       model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
     except Exception:
       if not params.get_bool("ChestnutActive"):
         raise
       # fallback to small model
       cloudlog.exception("big model failed, fall back to small")
+      params.put_bool("ChestnutModelError", True)
       params.put_bool("ChestnutActive", False)
       assert small_model is not None
       model = small_model

openpilot/selfdrive/selfdrived/events.py

@@ -175,7 +175,20 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
   return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
 
 
+def ford_joystick_alert(CP, sm):
+  ad = sm['alertDebug']
+  if CP.brand == 'ford' and sm.valid['alertDebug'] and ad.alertText1 in ('Joystick Mode — C0 only', 'Joystick Mode — C1 only'):
+    return NormalPermanentAlert(ad.alertText1, ad.alertText2)
+  return None
+
+
+def joystick_permanent_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  return ford_joystick_alert(CP, sm) or NormalPermanentAlert("Joystick Mode")
+
+
 def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  if alert := ford_joystick_alert(CP, sm):
+    return alert
   gb = sm['carControl'].actuators.accel / 4.
   steer = sm['carControl'].actuators.torque
   vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
@@ -220,7 +233,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
 
   EventName.joystickDebug: {
     ET.WARNING: joystick_alert,
-    ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
+    ET.PERMANENT: joystick_permanent_alert,
   },
 
   EventName.longitudinalManeuver: {

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/settings.py

@@ -16,11 +16,13 @@ class SettingsBigButton(BigButton):
 
 
 class SettingsLayout(NavScroller):
+  toggles_layout = TogglesLayoutMici
+
   def __init__(self):
     super().__init__()
     self._params = Params()
 
-    toggles_panel = TogglesLayoutMici()
+    toggles_panel = self.toggles_layout()
     toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64))
     toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel))
 

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 math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
+    return calculate_volume_for_device(weighted_db, self.device_type)
 
   @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.widgets.label import UnifiedLabel
+from openpilot.system.ui.lib.application import FontWeight, TextAlignment
+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/mici/layouts/settings.py

@@ -11,6 +11,7 @@ from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton
 from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.models import ModelsLayoutMici
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.toggles import TogglesLayoutMiciSP
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
@@ -30,6 +31,8 @@ class SunnylinkBigButton(SettingsBigButton):
 
 
 class SettingsLayoutSP(OP.SettingsLayout):
+  toggles_layout = TogglesLayoutMiciSP
+
   def __init__(self):
     OP.SettingsLayout.__init__(self)
 

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

@@ -0,0 +1,26 @@
+from opendbc.car.ford.values import FordFlags
+
+from openpilot.selfdrive.ui.mici.layouts.settings.toggles import TogglesLayoutMici
+from openpilot.selfdrive.ui.mici.widgets.button import BigParamControl, GreyBigButton
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.multilang import tr
+
+
+class TogglesLayoutMiciSP(TogglesLayoutMici):
+  def __init__(self):
+    super().__init__()
+    self._ford_c0_toggle = BigParamControl(tr('C0: 1 second'), 'FordC0TimeBased')
+    self._ford_c0_help = GreyBigButton('', tr('off: fixed 7 m\non: 1 second, min 7 m\ndisengage 3 s to apply\nno ignition cycle'))
+    self._ford_c0_toggle.set_enabled(lambda: not ui_state.engaged)
+    self._scroller.add_widgets([self._ford_c0_toggle, self._ford_c0_help])
+    self._refresh_toggles += (('FordC0TimeBased', self._ford_c0_toggle),)
+    self._ford_c0_toggle.set_visible(False)
+    self._ford_c0_help.set_visible(False)
+
+  def _update_toggles(self):
+    super()._update_toggles()
+    cp = ui_state.CP
+    visible = bool(cp is not None and cp.brand == 'ford' and cp.flags & FordFlags.CANFD
+                   and ui_state.params.get_bool('FordModelActionController'))
+    self._ford_c0_toggle.set_visible(visible)
+    self._ford_c0_help.set_visible(visible)

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/livedelay/helpers.py

@@ -4,6 +4,11 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 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
+
+from opendbc.car import structs
+from opendbc.car.ford.values import FordFlags
+from opendbc.car.common.conversions import Conversions as CV
 from openpilot.common.params import Params
 
 
@@ -15,3 +20,11 @@ def get_lat_delay(params: Params, stock_lat_delay: float) -> float:
     return stock_lat_delay
 
   return float(params.get("LagdValueCache", return_default=True))
+
+
+def get_ford_delay_offset(CP: structs.CarParams, enabled: bool, v_ego: float) -> float:
+  """Extra model preview for the opt-in C0/C1 controller; never alters learned delay."""
+  if not enabled or CP.brand != 'ford' or not CP.flags & FordFlags.CANFD or not math.isfinite(v_ego):
+    return 0.
+  # Route 146 trial: full preview through 15 mph, fading to zero at 30 mph.
+  return .4 * max(0., min(1., (30. - v_ego / CV.MPH_TO_MS) / 15.))

openpilot/sunnypilot/livedelay/test_helpers.py

@@ -0,0 +1,55 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.ford.values import CAR, FordFlags
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
+
+
+@pytest.mark.parametrize('mph,expected', [(-1, .4), (0, .4), (10, .4), (15, .4), (20, .4*2/3),
+                                          (22.5, .2), (25, .4/3), (30, 0), (45, 0), (100, 0)])
+def test_preview_schedule(mph, expected):
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  assert get_ford_delay_offset(cp, True, mph*CV.MPH_TO_MS) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize('enabled', [False, True])
+@pytest.mark.parametrize('brand,flags', [('ford', 0), ('ford', 8), ('ford', FordFlags.CANFD),
+                                       ('ford', FordFlags.CANFD | 8), ('toyota', FordFlags.CANFD)])
+def test_only_enabled_ford_canfd_has_preview(enabled, brand, flags):
+  cp = SimpleNamespace(brand=brand, flags=flags)
+  expected = .4 if enabled and brand == 'ford' and flags & FordFlags.CANFD else 0.
+  assert get_ford_delay_offset(cp, enabled, 0.) == expected
+
+
+@pytest.mark.parametrize('vehicle', list(CAR))
+def test_all_ford_platforms_follow_canfd_gate(vehicle):
+  cp = SimpleNamespace(brand='ford', flags=vehicle.config.flags)
+  assert get_ford_delay_offset(cp, True, 5.) == (.4 if vehicle.config.flags & FordFlags.CANFD else 0.)
+
+
+@pytest.mark.parametrize('v_ego', [float('nan'), float('inf'), -float('inf')])
+def test_invalid_speed_does_not_add_preview(v_ego):
+  assert get_ford_delay_offset(SimpleNamespace(brand='ford', flags=FordFlags.CANFD), True, v_ego) == 0.
+
+
+def test_preview_is_continuous_and_recomputed_from_current_speed():
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  mph = np.linspace(0, 60, 12001)
+  delays = np.array([get_ford_delay_offset(cp, True, v*CV.MPH_TO_MS) for v in mph])
+  assert np.all((0 <= delays) & (delays <= .4))
+  assert np.all(np.diff(delays) <= 0)
+  assert np.max(np.abs(np.diff(delays))) <= .4/15*.005 + 1e-14
+  assert [get_ford_delay_offset(cp, True, v*CV.MPH_TO_MS) for v in [10, 40, 10]] == [.4, 0., .4]
+
+
+@pytest.mark.parametrize('learning,expected', [(True, .16894637), (False, .32)])
+def test_preview_does_not_replace_or_write_the_base_delay(learning, expected):
+  params = SimpleNamespace(get_bool=lambda key: learning, get=lambda key, **kwargs: .32)
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  base = get_lat_delay(params, .16894637)
+  assert base == expected
+  assert base + get_ford_delay_offset(cp, True, 0.) == pytest.approx(expected+.4)
+  assert get_lat_delay(params, .16894637) == expected

openpilot/sunnypilot/modeld_v2/compile_modeld.py

@@ -33,7 +33,6 @@ def _patch_tinygrad_fetch_fw():
 _patch_tinygrad_fetch_fw()
 
 import openpilot.selfdrive.modeld.compile_modeld as stock
-import openpilot.sunnypilot.modeld_v2.stock_dependencies as legacy
 from tinygrad import dtypes
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
@@ -42,7 +41,7 @@ from tinygrad.tensor import Tensor
 MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
 WARP_INPUTS = ['tfm', 'big_tfm']
 POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
-
+nv12_copy_size = stock.nv12_copy_size
 
 def _detect_desire_key(shapes: dict) -> str | None:
   return next((key for key in shapes if key.startswith('desire')), None)
@@ -153,8 +152,8 @@ def make_warp_queues(device=Device.DEFAULT):
 
 
 def make_run_policy(vision_runner, policy_runners: list, features_slice: slice, frame_skip: int, input_shapes: dict):
-  sample_skip_fn = partial(legacy.sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(legacy.sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(stock.sample_skip, frame_skip=frame_skip)
+  sample_desire_fn = partial(stock.sample_desire, frame_skip=frame_skip)
 
   desire_key = _detect_desire_key(input_shapes)
   road_key, wide_key = _detect_vision_keys(input_shapes)
@@ -171,14 +170,14 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
     warped_dev = warped.to(Device.DEFAULT)
     Tensor.realize(packed_npy_inputs_dev, warped_dev)
 
-    img = legacy.shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
-    big_img = legacy.shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)
+    img = stock.shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
+    big_img = stock.shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)
 
     unpacked_tensors = [tensor.reshape(shape) for tensor, shape in zip(packed_npy_inputs_dev.split(npy_sizes), npy_shapes.values(), strict=True)]
     unpacked_dict = dict(zip(npy_shapes.keys(), unpacked_tensors, strict=True))
 
     desire_dev = unpacked_dict['desire']
-    desire_buf = legacy.shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
+    desire_buf = stock.shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
 
     inputs = {desire_key: desire_buf}
     for key, tensor_val in unpacked_dict.items():
@@ -187,13 +186,13 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
 
     if 'prev_feat' in unpacked_dict:
       prev_feat_dev = unpacked_dict['prev_feat']
-      inputs['features_buffer'] = legacy.shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])
+      inputs['features_buffer'] = stock.shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])
 
     if vision_runner:
       vision_out_cast = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast('float32').realize()
       if 'features_buffer' not in inputs:
         new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
-        inputs['features_buffer'] = legacy.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+        inputs['features_buffer'] = stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
       policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32').realize() for pol_runner in policy_runners]
       return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])
 
@@ -204,7 +203,7 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
     policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32').realize()
     if 'features_buffer' not in inputs and features_slice is not None:
       new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
-      legacy.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+      stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
     return policy_out
 
   return run_policy
@@ -331,32 +330,16 @@ if __name__ == "__main__":
     output_data['run_model'] = {}
     derived_frame_skip = args.frame_skip or derive_frame_skip({}, model_metadata['input_shapes'])
     model_runner = OnnxRunner(args.supercombo_onnx)
-    new_img_model = 'new_img' in model_runner.graph_inputs
-
-    if new_img_model:
-      input_shapes = {name: (spec.shape, spec.dtype) for name, spec in model_runner.graph_inputs.items()}
-      state_pairs = {name: f'next_{name}' for name in input_shapes if f'next_{name}' in model_runner.graph_outputs}
-      output_data['metadata'] = {'model': model_metadata, **model_metadata, 'input_shapes': input_shapes, 'state_pairs': state_pairs}
-      for cam_w, cam_h in args.camera_resolutions:
-        print(f"Compiling unified run_model JIT for {cam_w}x{cam_h} (new architecture)...")
-        nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-        frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-        make_model_queues = partial(stock.make_input_queues, input_shapes, state_pairs, frame_copy_size=frame_copy_size)
-        warp = stock.make_warp(nv12, model_w, model_h)
-        run_model_jit = TinyJit(stock.make_run_model(warp, model_runner, input_shapes, state_pairs, frame_copy_size), prune=True)
-        output_data['run_model'][(cam_w, cam_h)] = compile_jit(run_model_jit, list(state_pairs.keys()) + ['packed_npy_inputs'], make_model_queues,
-                                                               benchmark_runs=args.benchmark_runs)
-    else:
-      run_policy = legacy.make_legacy_run_policy(model_runner, model_metadata, derived_frame_skip)
-      for cam_w, cam_h in args.camera_resolutions:
-        print(f"Compiling unified run_model JIT for {cam_w}x{cam_h}...")
-        nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-        frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-        make_model_queues = partial(stock.make_input_queues, model_metadata['input_shapes'], derived_frame_skip,
-                                    frame_copy_size=frame_copy_size)
-        warp = stock.make_warp(nv12, model_w, model_h)
-        run_model_jit = TinyJit(legacy.make_legacy_run_model(warp, run_policy, model_metadata, frame_copy_size), prune=True)
-        output_data['run_model'][(cam_w, cam_h)] = compile_jit(run_model_jit, POLICY_INPUTS, make_model_queues, benchmark_runs=args.benchmark_runs)
+    run_policy = stock.make_run_policy(model_runner, model_metadata, derived_frame_skip)
+    for cam_w, cam_h in args.camera_resolutions:
+      print(f"Compiling unified run_model JIT for {cam_w}x{cam_h}...")
+      nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+      frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
+      make_model_queues = partial(stock.make_input_queues, model_metadata['input_shapes'], derived_frame_skip,
+                                  frame_copy_size=frame_copy_size)
+      warp = stock.make_warp(nv12, model_w, model_h)
+      run_model_jit = TinyJit(stock.make_run_model(warp, run_policy, model_metadata, frame_copy_size), prune=True)
+      output_data['run_model'][(cam_w, cam_h)] = compile_jit(run_model_jit, stock.MODELD_INPUTS, make_model_queues, benchmark_runs=args.benchmark_runs)
   else:
     vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
     if args.model_type == 'vision_policy':
@@ -372,12 +355,13 @@ if __name__ == "__main__":
         output_data['metadata'][name] = make_metadata_dict(runner_arg)
 
     policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
-    first_policy_meta: dict = output_data['metadata'][policy_keys[0]] if policy_keys else {}
-    vision_meta: dict = output_data['metadata'].get('vision', {})
+    first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
+    vision_meta = output_data['metadata'].get('vision', {})
 
     derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
     all_shapes = {key: value for meta in output_data['metadata'].values() for key, value in meta['input_shapes'].items()}
-    feat_meta: dict = vision_meta or first_policy_meta
+    feat_meta = output_data['metadata'].get('vision') or output_data['metadata'].get('policy')
+    assert feat_meta is not None
     features_slice = feat_meta['output_slices']['hidden_state']
 
     print(f"Compiling run_policy JIT (model_size={model_w}x{model_h}, frame_skip={derived_frame_skip})...")

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -36,9 +36,12 @@ from openpilot.system import sentry
 from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
-from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
+from openpilot.selfdrive.modeld.modeld import ChestnutState
 
-from openpilot.selfdrive.modeld.compile_modeld import make_input_queues
+from openpilot.selfdrive.modeld.compile_modeld import (
+  MODELD_INPUTS,
+  make_input_queues as make_stock_input_queues,
+)
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
 from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
@@ -47,8 +50,7 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 from openpilot.sunnypilot.modeld_v2.compile_modeld import (derive_frame_skip, make_split_input_queues,
                                                            make_supercombo_input_queues, nv12_copy_size,
                                                            WARP_INPUTS, POLICY_INPUTS)
-from openpilot.sunnypilot.modeld_v2.stock_dependencies import make_legacy_stock_input_queues
-from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.modeld_v2.helpers import load_oob
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -139,14 +141,8 @@ class ModelState(ModelStateBase):
       self._vision_input_names = [key for key in self.input_shapes if 'img' in key]
       self.frame_skip = derive_frame_skip({}, self.input_shapes)
       if self.is_run_model:
-        self.state_pairs = model_metadata.get('state_pairs', {})
-        self.is_new_model = len(self.state_pairs) > 0
-        if self.is_new_model:
-          self.input_queues, self.numpy_inputs, self.frame_buffers = make_input_queues(self.input_shapes, self.state_pairs,
-                                                                                       device=self.DEV, frame_copy_size=self.frame_copy_size)
-        else:
-          self.input_queues, self.numpy_inputs, self.frame_buffers = make_legacy_stock_input_queues(self.input_shapes, self.frame_skip, device=self.DEV,
-                                                                                                    frame_copy_size=self.frame_copy_size)
+        self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
+          self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
         self.frame_views, self.npy = self.frame_buffers, self.numpy_inputs
         self.run_model, self.run_policy, self.warp = jits['run_model'][(cam_w, cam_h)], None, None
       else:
@@ -195,12 +191,8 @@ class ModelState(ModelStateBase):
     dummy_inputs = {k: np.zeros(v.shape, dtype=v.dtype) for k, v in self.numpy_inputs.items() if k not in ['tfm', 'big_tfm', 'prev_feat']}
     self.run(dummy_frames, transforms, dummy_inputs)
     if self.is_run_model:
-      if self.is_new_model:
-        self.input_queues, self.numpy_inputs, self.frame_buffers = make_input_queues(self.input_shapes, self.state_pairs, device=self.DEV,
-                                                                                     frame_copy_size=self.frame_copy_size)
-      else:
-        self.input_queues, self.numpy_inputs, self.frame_buffers = make_legacy_stock_input_queues(self.input_shapes, self.frame_skip, device=self.DEV,
-                                                                                                  frame_copy_size=self.frame_copy_size)
+      self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
+        self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
       self.frame_views = self.frame_buffers
       self.npy = self.numpy_inputs
     else:
@@ -250,10 +242,7 @@ class ModelState(ModelStateBase):
     self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
 
     if self.run_model is not None:
-      if self.is_new_model:
-        outs, = self.run_model(**self.input_queues)
-      else:
-        outs, = self.run_model(**{k: self.input_queues[k] for k in POLICY_INPUTS})
+      outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
       raw_outputs = outs
     else:
       assert self.warp is not None and self.run_policy is not None
@@ -384,7 +373,11 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
+    if model is None:
+      params.put_bool("ChestnutModelError", True)
     params.put_bool("ChestnutActive", model is not None)
+    if model is not None:
+      params.remove("ChestnutModelError")
 
   small_model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False) if model is None or CHESTNUT else None
   if model is None:
@@ -394,12 +387,12 @@ def main(demo=False):
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
-  chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
@@ -421,6 +414,7 @@ def main(demo=False):
   else:
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
+  ford_model_action = params.get_bool("FordModelActionController")
 
   # TODO Move smooth seconds to action function
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
@@ -473,6 +467,7 @@ def main(demo=False):
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
+    lat_delay += get_ford_delay_offset(CP, ford_model_action, v_ego)
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
@@ -521,12 +516,13 @@ def main(demo=False):
     mt1 = time.perf_counter()
     try:
       send_chestnut = (chestnut_state is not None and
-                       run_count % round(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0)
+                       run_count % round(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
       model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
     except Exception:
       if not params.get_bool("ChestnutActive"):
         raise
       cloudlog.exception("chestnut failed, falling back to small")
+      params.put_bool("ChestnutModelError", True)
       params.put_bool("ChestnutActive", False)
       assert small_model is not None
       model = small_model

openpilot/sunnypilot/modeld_v2/stock_dependencies.py

@@ -1,119 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 numpy as np
-from functools import partial
-from tinygrad import dtypes
-from tinygrad.device import Device
-from tinygrad.tensor import Tensor
-
-# The old openpilot/selfdrive/modeld/compile_modeld.py functions needed for legacy models
-# We freeze them here so they aren't lost.
-
-def shift_and_sample(buf, new_val, sample_fn):
-  buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
-  return sample_fn(buf)
-
-def sample_skip(buf, frame_skip):
-  return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
-
-def sample_desire(buf, frame_skip):
-  return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
-
-def _detect_desire_key(shapes: dict) -> str | None:
-  return next((key for key in shapes if key.startswith('desire')), None)
-
-def get_policy_npy_shapes(input_shapes: dict, is_supercombo: bool = False) -> tuple[dict, list[int]]:
-  desire_key = _detect_desire_key(input_shapes)
-  shapes = {}
-  if desire_key:
-    shapes['desire'] = (input_shapes[desire_key][2],)
-
-  for key, shape in input_shapes.items():
-    if key not in (desire_key, 'features_buffer') and 'img' not in key:
-      shapes[key] = tuple(shape)
-
-  if is_supercombo and 'features_buffer' in input_shapes:
-    fb = input_shapes['features_buffer']
-    feat_dim = math.prod(fb[2:])
-    shapes['prev_feat'] = (fb[0], feat_dim)
-
-  sizes = [int(np.prod(size)) for size in shapes.values()]
-  return shapes, sizes
-
-def make_legacy_run_policy(model_runner, model_metadata, frame_skip):
-  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
-  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
-  npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'], is_supercombo=True)
-  model_input_dtypes = {name: spec.dtype for name, spec in model_runner.graph_inputs.items()}
-
-  def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
-    packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
-    Tensor.realize(packed_npy_inputs, warped)
-
-    img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn)
-
-    desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True))
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-    feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn)
-
-    inputs = {
-      'img': img,
-      'big_img': big_img,
-      'features_buffer': feat_buf.reshape(model_metadata['input_shapes']['features_buffer']),
-      'desire_pulse': desire_buf,
-      'traffic_convention': traffic_convention,
-      'action_t': action_t,
-    }
-    inputs = {name: value.cast(model_input_dtypes.get(name, dtypes.float32)) for name, value in inputs.items()}
-    out = next(iter(model_runner(inputs).values())).cast('float32')
-    return out,
-  return run_policy
-
-def make_legacy_run_model(warp, run_policy, model_metadata, frame_copy_size):
-  _, policy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'], is_supercombo=True)
-  packed_npy_size = (18 + sum(policy_sizes)) * np.dtype(np.float32).itemsize
-
-  def run_model(img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
-    packed_input = packed_npy_inputs.to(Device.DEFAULT)
-    Tensor.realize(packed_input)
-    packed_npy_inputs = packed_input[:packed_npy_size].bitcast('float32')
-    frame = packed_input[packed_npy_size:packed_npy_size + frame_copy_size]
-    big_frame = packed_input[packed_npy_size + frame_copy_size:]
-    tfm, big_tfm, policy_inputs = packed_npy_inputs.split([9, 9, sum(policy_sizes)])
-    warped = warp(tfm.reshape(3, 3), big_tfm.reshape(3, 3), frame, big_frame)
-    return run_policy(warped, img_q, big_img_q, feat_q, desire_q, policy_inputs)
-  return run_model
-
-def make_legacy_stock_input_queues(input_shapes, frame_skip, device, frame_copy_size):
-  img = input_shapes['img']  # (1, 12, 128, 256)
-  fb = input_shapes['features_buffer']  # (1, T-1, ...), past features only; the model appends the current frame's feature
-  feat_dim = math.prod(fb[2:])
-  dp = input_shapes['desire_pulse']  # (1, 25, 8)
-  n_frames = img[1] // 6
-  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
-
-  policy_shapes, _ = get_policy_npy_shapes(input_shapes, is_supercombo=True)
-  shapes = {'tfm': (3, 3), 'big_tfm': (3, 3)} | policy_shapes
-  sizes = [math.prod(s) for s in shapes.values()]
-  packed_npy_size = sum(sizes) * np.dtype(np.float32).itemsize
-  packed_input = np.zeros(packed_npy_size + 2 * frame_copy_size, dtype=np.uint8)
-  packed_npy_inputs = packed_input[:packed_npy_size].view(np.float32)
-  frames = packed_input[packed_npy_size:]
-  frame_views = {'img': frames[:frame_copy_size], 'big_img': frames[frame_copy_size:]}
-  # views into the packed inputs, to be refilled at runtime
-  npy = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)}
-  input_queues = {
-    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32), device=device).contiguous().realize(),
-    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
-    'packed_npy_inputs': Tensor(packed_input, device='NPY').realize(),
-  }
-  return input_queues, npy, frame_views

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -51,6 +51,14 @@ class ControlsExt(ModelStateBase):
     if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
       self.blinker_pause_lateral.get_params()
 
+      if getattr(self, 'ford_model_action', False) and sm.all_checks(['selfdriveState', 'selfdriveStateSP']):
+        mads = sm['selfdriveStateSP'].mads
+        # Use the engagement state, not a temporary pause from blinkers or a
+        # standstill/fault gate, so a pause cannot swap the command mapping.
+        lateral_engaged = mads.enabled if mads.available else sm['selfdriveState'].enabled
+        if self.ford_path_controller.set_c0_time_based(self.params.get_bool('FordC0TimeBased'), lateral_engaged=lateral_engaged):
+          cloudlog.event('Ford C0 distance changed', c0_time_based=self.ford_path_controller.core.c0_time_based)
+
       if self.CP.lateralTuning.which() == 'torque':
         self.lat_delay = get_lat_delay(self.params, sm["lateralDelay"].lateralDelay)
 
@@ -104,6 +112,15 @@ 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.enabled = getattr(self, 'ford_model_action', False)
+      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,25 @@
     }
   ],
   "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 desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.",
+          "details": "Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "enablement": [
+            {
+              "type": "offroad_only"
+            }
+          ]
+        }
+      ]
+    },
     "hyundai": {
       "title": "Hyundai / Kia / Genesis Settings",
       "description": "",

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

@@ -6,6 +6,18 @@ 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 desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.
+    details: Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    enablement:
+    - $ref: '#/macros/offroad'
 - 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,26 @@ 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 "Turning it off restores upstream Ford curvature control" in candidate["details"]
+    assert "regardless of any previously stored experimental settings" 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
+    assert "FordPscmObserver" not in keys
+
   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)