Ford: add measured-curvature feedback to C1

Add a distance-integrated steering-curvature correction to the restored v1
heading request. Hold the correction at zero error, allow unwind at limits,
and clear it for driver override or inactive PSCM control. Preserve C0,
C2=C3=0, final output limits, the 100 Hz sender and the existing opt-in toggle.

Validate build/hold/unwind, measurement cadence, anti-windup and the actual
controlsd-to-CAN path. The combined suite passes 511 tests and 9,146 subtests;
178 inherited or unsupported safety variants skip. Stress and frozen b8
replay pass 578,569 Float32/CAN round trips. These checks do not establish
physical tracking or stability. Record reproduction steps and source hashes.

docs/ford_c1_feedback.md

@@ -0,0 +1,124 @@
+# Ford C1 feedback experiment
+
+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_model_action_candidate.md

@@ -5,7 +5,9 @@ stage committed as `7ca3c6e3b`. The candidate is now available behind a
 separate default-off Sunnylink toggle; see
 [drive-test setup and validation](ford_model_action_drive_test.md).
 The counts, source hashes and selector status below describe that earlier
-stage, not the subsequent wiring change.
+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`,

docs/ford_model_action_drive_test.md

@@ -1,83 +1,53 @@
 # Ford selected-action drive-test branch
 
-The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind
-its own persistent, default-off Sunnylink toggle. The command law and input
-gates from the [offline candidate](ford_model_action_candidate.md) are unchanged.
-`calibration_approved=false`: offline checks do not establish physical tracking,
-turn-exit behavior or closed-loop stability.
+The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md)
+to the restored original v1 mapping. It is selectable on the **Ford CAN FD
+F-150 Lightning** through the existing persistent, default-off Sunnylink
+toggle. Offline checks establish software behavior; physical tracking,
+turn-exit behavior and closed-loop stability remain unvalidated.
 
 ## Select and restore
 
-1. Install branch `hiimisaac-dev` from
-   `sunnypilot/sunnypilot` on the device using your normal branch-switch process.
-   Allow its build to finish before changing the setting.
+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; changing a stored toggle or disengaging alone cannot swap an active
+   starts; a stored toggle change or disengagement alone cannot swap an active
    controller. Initial physical evaluation remains controlled testing.
 
-The startup log event `Ford path controller selected` should report
+The startup event `Ford path controller selected` should report
 `FordModelActionController`. Periodic `Ford C2-free path tracking` events
-identify `hypothesis=model-action-c0-c1-v1` and report the command tuple.
+identify **`hypothesis=model-action-c1-feedback-v1`**. They report desired and
+measured curvature, base heading, accumulated correction, applied heading,
+feedback timing and driver/PSCM gating. `calibration_approved=false` remains.
 
-Turning the new toggle off and completing another offroad-to-onroad cycle
-restores **PSCM Coefficient Observer** if selected, otherwise the original
-Ford path controller. The stored observer selection is preserved. The candidate
-takes priority on the supported vehicle, independently of EPS firmware query
-results. Other vehicles retain their existing selection.
-
-The v8 implementation, its Sunnylink toggle and its dedicated tests are removed.
-A leftover `FordVirtualAngleController=1` file cannot enable the new controller.
-The shared Float32/CAN rounding helper now lives in `ford_model_action.py`;
-unused v8 PSCM-feedback plumbing is removed. Historical v8 route evidence remains
-in Git history and the archived validation documents.
+Turning the toggle off and completing another offroad-to-onroad cycle restores
+**PSCM Coefficient Observer** if selected, otherwise the original Ford path
+controller. The stored observer selection is preserved. The experiment takes
+priority on the supported vehicle; other vehicles retain their existing
+controller. A leftover `FordVirtualAngleController` parameter has no effect.
 
 ## Wiring and validation
 
-`Controls.__init__` selects the candidate once at startup. It shares the
-existing Ford call path, selected upstream-limited curvature, service gates,
-invalid-output disengagement, Float32 publication and downstream CAN builder.
-C2 and C3 stay zero. No opendbc pointer or Panda safety change is included.
+`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 feedback. Repeated publications may
+advance output slew but cannot integrate the same elapsed interval twice.
+Driver override clears the correction. A fresh PSCM reached-limit flag stops
+extra outward accumulation while preserving unwind and base model changes.
 
-Sunnylink publishes the toggle through its generated settings schema and
-writes the registered Boolean through the existing parameter endpoint. The
-offroad UI rule and `needs_onroad_cycle` metadata describe when it can be
-changed and when it takes effect. An onroad backend write changes storage
-only; the controller continues using its startup selection.
+C0 retains the original 7 m model-path mapping. C2 and C3 remain zero. The
+existing output limits, 100 Hz sender, Float32 publication and CAN builder
+remain in place. No opendbc submodule or Panda safety change is required.
 
-Native validation also exposed a pre-existing `params_keys_by_flag` bug:
-every returned buffer referenced the same reusable string. Sunnylink backup
-key enumeration could therefore return corrupted names. The bridge now
-returns separate strings owned by the parameter handle. Regression tests
-check distinct registered keys across flags, and toggle tests check its
-persistence and backup registration using the rebuilt native library.
-
-The current validation record is `ford_model_action_drive_test_validation.json`.
-The final combined Ford, Params and Sunnylink suite passes **284 tests and
-26 subtests**, with no skips. The candidate has **100% statement and branch
-coverage** (87 statements, 26 branches). Ruff, Ty, settings compilation and
-both review axes pass. The fresh route/stress runs check **485,238 Float32/CAN
-round trips**, including 200,000 randomized and 200,000 mirrored core updates.
-The controller is 145 total lines, including 95 code lines excluding comments,
-blanks and docstrings; v8's 469-line module is removed.
-
-The previous 133,550-cycle route reconstruction, 485,238 packing round trips
-and mutation probes remain recorded separately in
-`ford_model_action_validation.json` at the offline-stage source hashes.
-
-## Reproduce deployment checks
-
-Initialize the branch's exact opendbc submodule (`c21a9013700734dd20b09e05aa68329ad8cc20f9`)
-and build the native Params library from this branch before testing.
-
-```sh
-export PYTHONDONTWRITEBYTECODE=1
-export PYTHONPATH=.:opendbc_repo
-python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py
-python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_model_action_drive_test/stress.json
-```
+See [the feedback specification and validation](ford_c1_feedback.md) and
+`ford_c1_feedback_validation.json` for the current evidence and reproduction
+commands. `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 feedback version.
 
 The full hardware build and device boot are not performed by these offline
-tests. Installing the branch and enabling the toggle are separate actions;
-pushing the branch does not change a device's selected software or settings.
+checks. Pushing the branch does not install it on the device or change its
+stored toggle.

openpilot/selfdrive/controls/controlsd.py

@@ -173,11 +173,13 @@ class Controls(ControlsExt):
       if self.ford_model_action:
         reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
         self.ford_path = self.ford_path_controller.update(
-          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
+          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

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -1,14 +1,15 @@
-"""Experimental Ford C2-free controller: nearby offset and selected-action heading.
+"""Experimental Ford C2-free controller with measured-curvature C1 feedback.
 
 Selected only by its explicit toggle. The 7 m station and one-second scale are
-engineering choices, not identified PSCM gains or physical calibration.
+engineering choices. Feeding integrated heading mismatch into C1 at 1:1 is an
+explicit feedback-strength choice, not an identified PSCM model or calibration.
 """
 import math
 import struct
 
 import numpy as np
 
-from opendbc.car.ford.values import FordFlags
+from opendbc.car.ford.values import CarControllerParams, FordFlags
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
 
 
@@ -51,21 +52,24 @@ def encode_model_action(model, desired_curvature, speed):
 
 
 class ModelActionController:
-  """Only two control states: unquantized, independently slewed C0 and C1.
+  """Unquantized C0/C1 slew positions and one C1 feedback correction.
 
-  Freshness and engagement belong to the caller. No measured yaw, model
-  history, heading integral, blending or release modes enter the law.
+  Feedback integrates requested minus measured curvature over traveled distance.
+  Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
   """
-  __slots__ = ('c0', 'c1')
+  __slots__ = ('c0', 'c1', 'correction')
 
   def __init__(self):
     self.reset()
 
   def reset(self):
-    self.c0 = self.c1 = 0.
+    self.c0 = self.c1 = self.correction = 0.
 
-  def update(self, model, desired_curvature, *, speed, dt, active=True, valid=True):
-    if not active or not valid or not _finite(dt) or not .002 <= dt <= .1:
+  def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
+             feedback_dt=None, feedback_enabled=True, pscm_limited=False):
+    feedback_dt = dt if feedback_dt is None else feedback_dt
+    if (not active or not valid or not _finite(dt, feedback_dt, current_curvature) or not .002 <= dt <= .1
+        or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1.):
       self.reset()
       return FordPath()
     target = encode_model_action(model, desired_curvature, speed)
@@ -73,8 +77,27 @@ class ModelActionController:
       self.reset()
       return FordPath()
     c0 = float(np.clip(target.path_offset, -5.11, 5.11))
-    c1 = float(np.clip(target.path_angle, -.5, .5))
+    base_c1 = float(np.clip(target.path_angle, -.5, .5))
+    lower = max(-.5, self.c1-.5*dt)
+    upper = min(.5, self.c1+.5*dt)
+    if not feedback_enabled:
+      self.correction = 0.
+    else:
+      increment = (desired_curvature-current_curvature)*speed*feedback_dt
+      # LimitReached inhibits only extra demand in the measured turn direction.
+      # Opposing correction and changes to the model request remain available.
+      direction = current_curvature if current_curvature else self.c1
+      if pscm_limited and increment*direction > 0.:
+        # An old opposing correction may return to zero; don't trap it below
+        # the base request just because the PSCM now reports a limit.
+        increment = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.)))
+      # Integrate only as far as this cycle's amplitude/slew envelope permits.
+      # If the base moved outside that envelope, allow increments toward it;
+      # never rewrite existing correction merely because the base changed.
+      request = base_c1+self.correction
+      self.correction += float(np.clip(increment, min(lower-request, 0.), max(upper-request, 0.)))
     self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
+    c1 = float(np.clip(base_c1+self.correction, -.5, .5))
     self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt))
     return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.)
 
@@ -83,13 +106,13 @@ class FordModelActionController:
   """Input adapter for the opt-in selected-action controller.
 
   controlsd owns upstream selection/limiting and service health. This adapter
-  checks ages and clock order, then supplies elapsed time to the two-state
-  core. Its timestamps and diagnostics never affect the targets. Raw model
-  geometry is checked on every cycle, even at a repeated model timestamp.
+  checks ages and clock order, then supplies elapsed time to the three-state
+  core. Feedback advances once per fresh steering measurement; repeated samples
+  can still advance output slew. Raw model geometry is checked on every cycle.
 
-  Yaw is checked only for the inherited finite/range input gate. Engagement
-  and downstream driver arbitration still apply. This controller does not use
-  PSCM status or driver torque as control-law inputs.
+  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):
     self.core = ModelActionController()
@@ -98,41 +121,54 @@ class FordModelActionController:
   def reset(self, status='inactive'):
     self.core.reset()
     self.last_time = self.last_measurement_time = self.last_model_time = None
-    self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-v1',
+    self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v1',
                         'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
 
-  def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
-             active, valid=True):
+  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):
     reason = None
     if not active:
       reason = 'inactive'
     elif not valid:
       reason = 'invalid_service'
-    elif not _finite(desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
+    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:
+    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
-    if not .002 <= dt <= .1 or (self.last_measurement_time is not None and measurement_time < self.last_measurement_time) or (
+    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()
-    command = self.core.update(model, desired_curvature, speed=speed, dt=dt)
+    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)
     if not command.valid:
       self.reset('invalid_path')
       return command
     self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
-    self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-v1',
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v1',
                         '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': float(np.clip(max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature, -.5, .5)),
+                        'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
+                        '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
 

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -48,12 +48,12 @@ class TestFordControlsLogging(unittest.TestCase):
   def test_candidate_diagnostics_identify_the_experiment_and_do_not_claim_calibration(self):
     controller = FordModelActionController()
     for active, valid in ((False, True), (True, True), (True, False)):
-      controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1.,
+      controller.update(circle(.01), .005, current_curvature=.0025, yaw_rate=.05, speed=20., now=1.,
                         measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid)
       controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
                                  sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
       record = self.emit_controls_event('Ford C2-free path tracking', controls)
-      self.assertEqual(record['hypothesis'], 'model-action-c0-c1-v1')
+      self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v1')
       self.assertIs(record['calibration_approved'], False)
       self.assertEqual(record['command'][2:], [0., 0.])
       self.assertEqual(record['status'], controller.diagnostics['status'])

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -42,27 +42,27 @@ def test_centering_information_is_independent_of_action_and_not_scaled_with_spee
     assert target.path_angle == pytest.approx(sign*.2)  # No 10 m cap at highway speed.
 
 
-def test_two_actuator_positions_are_sufficient_for_every_next_output():
+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 = controller.c0, controller.c1
+    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, **kwargs) == copied.update(model, desired, **kwargs)
+    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, speed=20., dt=.01)
+      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, speed=20., dt=.01)
+      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()
@@ -73,13 +73,13 @@ def test_held_turn_releases_without_a_bias_tail_or_sign_reversal():
 def test_current_model_replacement_leaves_only_independent_actuator_slew():
   controller = ModelActionController()
   for _ in range(150):
-    controller.update(straight(1.), .04, speed=20., dt=.01)
+    controller.update(straight(1.), .04, current_curvature=.04, speed=20., dt=.01)
   for _ in range(25):
-    out = controller.update(straight(), 0., speed=20., dt=.01)
+    out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01)
   assert out.path_offset == pytest.approx(0.)
   assert out.path_angle > 0.  # C1 cannot hold C0 during its longer release.
   for _ in range(75):
-    out = controller.update(straight(), 0., speed=20., dt=.01)
+    out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01)
   assert out == FordPath(True, 0., 0., 0., 0.)
 
 
@@ -87,12 +87,12 @@ def test_current_model_replacement_leaves_only_independent_actuator_slew():
 def test_invalid_or_inactive_input_clears_state_before_reengagement(overrides):
   controller = ModelActionController()
   for _ in range(100):
-    controller.update(straight(.5), .01, speed=20., dt=.01)
+    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., **kwargs) == FordPath()
+  assert controller.update(straight(), 0., current_curvature=0., **kwargs) == FordPath()
   assert (controller.c0, controller.c1) == (0., 0.)
-  assert controller.update(straight(), 0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.)
+  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():
@@ -108,7 +108,7 @@ def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2(
   previous = np.zeros(2)
   for i in range(600):
     sign = 1. if i < 300 else -1.
-    out = controller.update(straight(sign*8.), sign*.1, speed=30., dt=.01)
+    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()
     assert (abs(fields-previous) <= [.0500001, .0055001]).all()
@@ -133,8 +133,8 @@ def test_short_path_holds_available_endpoint_without_extrapolation():
 def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
   model = make_model([0., 1e308, -1e308], [0., 0., 0.], [0., 0., 0.])
   controller = ModelActionController()
-  controller.update(straight(.4), .01, speed=20., dt=.01)
-  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  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.)
 
 
@@ -143,9 +143,9 @@ def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
 def test_malformed_numeric_input_resets_without_throwing(field, value):
   controller = ModelActionController()
   kwargs = {'speed': 20., 'dt': .01, 'desired_curvature': .01}
-  controller.update(straight(.4), **kwargs)
+  controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs)
   kwargs[field] = value
-  assert controller.update(straight(.4), **kwargs) == FordPath()
+  assert controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs) == FordPath()
   assert (controller.c0, controller.c1) == (0., 0.)
 
 
@@ -160,8 +160,8 @@ def test_malformed_numeric_input_resets_without_throwing(field, value):
 ])
 def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model):
   controller = ModelActionController()
-  controller.update(straight(.4), .01, speed=20., dt=.01)
-  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  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.)
 
 
@@ -173,7 +173,7 @@ def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model):
 def test_domain_and_elapsed_time_boundaries(field, value, valid):
   kwargs = {'speed': 20., 'desired_curvature': .01, 'dt': .01}
   kwargs[field] = value
-  assert ModelActionController().update(straight(.4), **kwargs).valid == valid
+  assert ModelActionController().update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs).valid == valid
 
 
 def test_arc_station_not_forward_x_or_model_heading_determines_offset():
@@ -188,6 +188,6 @@ def test_arc_station_not_forward_x_or_model_heading_determines_offset():
 def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
   model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
   assert encode_model_action(model, .01, 20.) == FordPath(True, .4, .2, 0., 0.)
-  out = ModelActionController().update(model, .01, speed=20., dt=.002)
+  out = ModelActionController().update(model, .01, current_curvature=.01, speed=20., dt=.002)
   assert out.path_offset == pytest.approx(.01)
   assert out.path_angle == pytest.approx(.001)

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -15,6 +15,7 @@ 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 FordFlags
 from openpilot.cereal import custom
 from openpilot.selfdrive.car.helpers import convert_carControlSP
@@ -29,6 +30,7 @@ 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)
 
 
@@ -149,7 +151,7 @@ class Subscriptions:
   frame = 1
 
   def __init__(self, maneuver):
-    self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True}
+    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)}
@@ -217,3 +219,73 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
   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
+  for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02),
+                                           (sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.),
+                                           (sign*.003, 0., 100, sign*.02), (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
+    assert controls.ford_path_controller.core.correction == pytest.approx(expected)
+    assert controls.ford_path.path_angle == pytest.approx(sign*.08+expected)
+    assert controls.ford_path.path_offset == pytest.approx(.4)
+
+
+@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(101):
+    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.correction == pytest.approx(0. if service_valid else .0002)
+  assert cc.latActive and controls.ford_path.valid

openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py

@@ -0,0 +1,182 @@
+"""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(), ModelActionController()
+  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(.4)
+  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_and_slew_limits_do_not_store_unavailable_feedback(sign):
+  controller = ModelActionController()
+  # The unchanged model request is already ahead of the output slew.
+  for _ in range(10):
+    tick(controller, sign*.01, 0.)
+    assert controller.correction == 0.
+  for _ in range(1000):
+    before = controller.c1
+    tick(controller, sign*.01, -sign*.9)
+    assert abs(controller.c1-before) <= .0050000001
+    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()
+  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 slew.
+  for _ in range(100):
+    out = tick(controller, 0., 0., pscm_limited=True)
+  assert abs(out.path_angle) < .001
+  assert out.path_offset == pytest.approx(.4)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign):
+  controller = ModelActionController()
+  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_through_existing_output_slew():
+  controller = ModelActionController()
+  for _ in range(100):
+    tick(controller, .004, .004)
+  for _ in range(100):
+    tick(controller, .004, .003)
+  assert controller.correction > 0.
+  previous = controller.c1
+  tick(controller, .004, -.01, feedback_enabled=False)
+  assert controller.correction == 0.
+  assert abs(controller.c1-previous) <= .0050000001
+  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()
+  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_only_advance_output_slew():
+  controller = FordModelActionController()
+  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()
+  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()
+  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()
+    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/sunnypilot/sunnylink/settings_ui.json

@@ -2183,8 +2183,8 @@
           "widget": "toggle",
           "needs_onroad_cycle": true,
           "title": "Selected-Action Path Tracking (Experimental)",
-          "description": "Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.",
-          "details": "Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "description": "Follow the selected steering plan using model-path centering and measured steering feedback on the Ford CAN FD F-150 Lightning.",
+          "details": "Keeps the nearby model-path centering request and adjusts the heading request using the difference between requested and measured steering. 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. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
           "enablement": [
             {
               "type": "offroad_only"

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

@@ -14,8 +14,8 @@ sections:
     widget: toggle
     needs_onroad_cycle: true
     title: Selected-Action Path Tracking (Experimental)
-    description: Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.
-    details: Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    description: Follow the selected steering plan using model-path centering and measured steering feedback on the Ford CAN FD F-150 Lightning.
+    details: Keeps the nearby model-path centering request and adjusts the heading request using the difference between requested and measured steering. 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. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
     enablement:
     - $ref: '#/macros/offroad'
   - key: FordPscmObserver

tools/ford_pscm_lab/feedback_replay.py

@@ -0,0 +1,181 @@
+"""Inspect C1 feedback commands on frozen route measurements, never vehicle motion.
+
+Input: route.npz, model_paths.npz and metadata.json from the full-rlog extractor.
+The recorded desired/actual curvature, clocks, driver input and PSCM flags stay
+fixed. This verifies software behavior, not counterfactual physical tracking.
+"""
+import argparse
+from collections import Counter
+import hashlib
+import json
+from pathlib import Path
+import subprocess
+from types import ModuleType, SimpleNamespace
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib import ford_model_action
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
+from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency
+
+
+BASELINE = 'a7d70e2b0890184636827351e4789d866f2a7c97'
+OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9'
+
+
+def original_controller():
+  root = Path(__file__).resolve().parents[2]
+  source = subprocess.check_output(['git', '-C', str(root), 'show',
+                                   f'{BASELINE}:openpilot/selfdrive/controls/lib/ford_model_action.py'], text=True)
+  module = ModuleType('ford_original_v1')
+  exec(compile(source, f'{BASELINE}:ford_model_action.py', 'exec'), module.__dict__)
+  return module.FordModelActionController(), hashlib.sha256(source.encode()).hexdigest()
+
+
+def replay(directory, output):
+  directory, output = directory.resolve(), output.resolve()
+  if output == directory or directory in output.parents:
+    raise ValueError('Output must be outside the source route directory')
+  verify_dependency(OPENDBC)
+  metadata = json.loads((directory/'metadata.json').read_text())
+  with np.load(directory/'route.npz', allow_pickle=False) as z:
+    r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')}
+  with np.load(directory/'model_paths.npz', allow_pickle=False) as z:
+    model_ns, paths = z['ns'], z['paths']
+  for stream in r.values():
+    if np.any(np.diff(stream['t']) < 0):
+      raise ValueError('Source stream contains a backward clock')
+  c = r['controls']
+  t = c['t']
+  cs, pa, ps = (sample(r[k], t) for k in ('cs', 'params', 'pscm'))
+  cc = sample(r['cc'], t, nearest=True)
+  mi = np.clip(np.searchsorted(r['model']['ns'], c['model_ns']), 0, len(r['model']['ns'])-1)
+  exact = r['model']['ns'][mi] == c['model_ns']
+  np.testing.assert_array_equal(model_ns, r['model']['ns'])
+  models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths]
+  old, baseline_hash = original_controller()
+  controller, wire_check = FordModelActionController(), WireCheck()
+  baseline = np.zeros((len(t), 4))
+  commands = np.zeros_like(baseline)
+  old_valid = np.zeros(len(t), bool)
+  valid = np.zeros(len(t), bool)
+  correction = np.zeros(len(t))
+  feedback_dt = np.zeros(len(t))
+  feedback_enabled = np.zeros(len(t), bool)
+  pscm_limited = np.zeros(len(t), bool)
+  reasons = Counter()
+  for i, now in enumerate(t):
+    model_time = r['model']['t'][mi[i]]
+    service_valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i]
+                         and pa['valid'][i] and r['model']['valid'][mi[i]] and exact[i]
+                         and abs(cc['t'][i]-now) < .005 and 0. <= now-pa['t'][i] <= .15)
+    common = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now,
+              'measurement_time': cs['t'][i], 'model_time': model_time, 'reference_time': model_time,
+              'active': bool(cc['active'][i]), 'valid': service_valid}
+    model = models[mi[i]] if exact[i] else None
+    status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
+                             limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
+    previous = old.update(model, c['desired'][i], **common)
+    command = controller.update(model, c['desired'][i], current_curvature=c['measured'][i],
+                                 driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i], pscm_status=status, **common)
+    for destination, result in ((baseline, previous), (commands, command)):
+      destination[i] = result.path_offset, result.path_angle, result.curvature, result.curvature_rate
+    old_valid[i], valid[i] = previous.valid, command.valid
+    d = controller.diagnostics
+    reasons[d['status']] += 1
+    correction[i] = controller.core.correction
+    feedback_dt[i] = d.get('feedback_dt', 0.)
+    feedback_enabled[i] = d.get('feedback_enabled', False)
+    pscm_limited[i] = d.get('pscm_limited', False)
+    wire_check.check(command)
+  field_checks(commands, valid, t)
+  np.testing.assert_array_equal(valid, old_valid)
+  np.testing.assert_array_equal(commands[:, 0], baseline[:, 0])
+  assert np.all(correction[~feedback_enabled] == 0.)
+  assert np.all(abs(correction) <= 1.+1e-10)
+  weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
+  report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': baseline_hash,
+            'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()),
+            'validity_and_c0_match_original_v1_exactly': True, 'status_counts': dict(reasons),
+            'feedback_enabled_seconds': float(weight[feedback_enabled].sum()),
+            'pscm_limit_2_seconds': float(weight[pscm_limited & valid].sum()),
+            'c1_changed_cycles': int((abs(commands[:, 1]-baseline[:, 1]) > 1e-8).sum()),
+            'max_abs_c1_change_rad': float(abs(commands[:, 1]-baseline[:, 1]).max()),
+            'max_abs_correction_rad': float(abs(correction).max()), 'can_round_trips': wire_check.count,
+            'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in
+                              (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
+                               Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())},
+            '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.'}
+  report['example_points'] = []
+  for seconds in (130.937, 235.396, 236.250, 252.493, 674.430, 1534.519, 1562.507):
+    i = int(np.argmin(abs(t-metadata['t0']-seconds)))
+    report['example_points'].append({'time_s': float(t[i]-metadata['t0']), 'old_c0_c1': baseline[i, :2].tolist(),
+                                     'candidate_c0_c1': commands[i, :2].tolist(), 'correction_rad': float(correction[i]),
+                                     'feedback_enabled': bool(feedback_enabled[i]), 'pscm_limited': bool(pscm_limited[i])})
+  output.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(output/'commands.npz', t=t-metadata['t0'], baseline=baseline, candidate=commands, valid=valid,
+                      correction=correction, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited)
+  (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
+  print(json.dumps({k: v for k, v in report.items() if k != 'source_sha256'}, indent=2))
+
+
+def stress(cycles, output):
+  verify_dependency(OPENDBC)
+  rng = np.random.default_rng(20260909)
+  controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck()
+  for i in range(cycles):
+    desired, measured = rng.uniform(-.1, .1, 2)
+    speed, dt, offset = rng.uniform(.3, 55.), rng.uniform(.002, .1), rng.uniform(-8., 8.)
+    active, enabled, limited = i % 211 != 0, i % 97 != 0, i % 7 == 0
+    feedback_dt = 0. if i % 5 == 0 else rng.uniform(.002, .15)
+    previous = controller.c0, controller.c1, controller.correction
+    args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active,
+            'feedback_enabled': enabled, 'pscm_limited': limited}
+    def model(y):
+      return SimpleNamespace(position=SimpleNamespace(x=[0., 20.], y=[y, y]), orientation=SimpleNamespace(z=[0., 0.]))
+    out = controller.update(model(offset), desired, current_curvature=measured, **args)
+    other = mirror.update(model(-offset), -desired, current_curvature=-measured, **args)
+    state = controller.c0, controller.c1, controller.correction
+    mirrored = mirror.c0, mirror.c1, mirror.correction
+    np.testing.assert_allclose(state, -np.array(mirrored), rtol=0., atol=1e-10)
+    assert abs(controller.c0) <= 5.11+1e-10 and abs(controller.c1) <= .5+1e-10 and abs(controller.correction) <= 1.+1e-10
+    if active:
+      assert abs(controller.c0-previous[0]) <= 4.*dt+1e-10
+      assert abs(controller.c1-previous[1]) <= .5*dt+1e-10
+      if enabled:
+        delta = controller.correction-previous[2]
+        request = (desired-measured)*speed*feedback_dt
+        assert delta*request >= -1e-10 and abs(delta) <= abs(request)+1e-10
+        if limited and request*(measured if measured else previous[1]) > 0.:
+          assert abs(controller.correction) <= abs(previous[2])+1e-10
+          assert controller.correction*previous[2] >= -1e-10
+      else:
+        assert controller.correction == 0.
+    else:
+      assert state == (0., 0., 0.)
+    assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0.
+    wire.check(out)
+  report = {'cycles': cycles, 'mirrored_updates': cycles, 'can_round_trips': wire.count,
+            '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': hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()}
+  output.parent.mkdir(parents=True, exist_ok=True)
+  output.write_text(json.dumps(report, indent=2)+'\n')
+  print(json.dumps(report, indent=2))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  sub = parser.add_subparsers(dest='mode', required=True)
+  route = sub.add_parser('route')
+  route.add_argument('directory', type=Path)
+  route.add_argument('--output', type=Path, required=True)
+  random = sub.add_parser('stress')
+  random.add_argument('--cycles', type=int, default=200_000)
+  random.add_argument('--output', type=Path, required=True)
+  args = parser.parse_args()
+  if args.mode == 'route':
+    replay(args.directory, args.output)
+  else:
+    stress(args.cycles, args.output)

tools/ford_pscm_lab/model_action_replay.py

@@ -1,4 +1,8 @@
-"""Replay the selected core and its adapter on route90/95 original-time extracts.
+"""Check zero-error compatibility on route90/95 original-time extracts.
+
+Measured curvature is deliberately set to the requested curvature in this
+historical compatibility check, so feedback stays zero. This is not a replay
+of the new feedback controller's response to recorded steering measurements.
 
 The historical pass deliberately uses the archived eligibility mask to check
 command compatibility. The separate adapter pass reconstructs input eligibility
@@ -132,11 +136,13 @@ def run(directory, output):
   reasons = Counter()
   for i, now in enumerate(t):
     selected_model = models[mi[i]] if exact[i] else None
-    old_gate = core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=dt[i], active=bool(baseline['valid'][i]))
+    old_gate = core.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i],
+                           speed=cs['speed'][i], dt=dt[i], active=bool(baseline['valid'][i]))
     commands[i] = old_gate.path_offset, old_gate.path_angle, old_gate.curvature, old_gate.curvature_rate
     valid[i] = old_gate.valid
     wire.check(old_gate)
-    new_gate = adapter.update(selected_model, controls['desired'][i], speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now,
+    new_gate = adapter.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i],
+                              speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now,
                               measurement_time=cs['t'][i], model_time=model['t'][i], reference_time=model['t'][i],
                               active=bool(cc['active'][i]), valid=bool(services_valid[i]))
     adapted[i] = new_gate.path_offset, new_gate.path_angle, new_gate.curvature, new_gate.curvature_rate
@@ -146,7 +152,8 @@ def run(directory, output):
     # Isolate the adapter's fresh 10 ms engagement tick from the archived
     # harness, which used the preceding publication interval even on engage.
     entry_dt = dt[i] if i > 0 and baseline['valid'][i-1] else .01
-    expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=entry_dt,
+    expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i],
+                                               speed=cs['speed'][i], dt=entry_dt,
                                                active=bool(baseline['valid'][i]))
     assert new_gate == expected_adapter, f'Unexplained adapter difference at cycle {i}'
   np.testing.assert_array_equal(valid, baseline['valid'])
@@ -186,7 +193,7 @@ def run(directory, output):
   sources = [Path(__file__), Path(ford_model_action.__file__),
              root/'openpilot/selfdrive/controls/lib/ford_path.py', directory/'route.npz', directory/'metadata.json',
              directory/'encoder_comparison.npz', directory/'encoder_comparison.json', directory/'pose_candidate/pose_replay.npz']
-  report = {'scope': 'Command construction and adapter reconstruction only; no counterfactual closed-loop score.',
+  report = {'scope': 'Zero-error compatibility only: measured curvature is set to requested curvature. No physical tracking score.',
             'calibration_approved': False, 'executes_live_selector': False, 'cycles': len(t),
             'core_active_cycles': int(valid.sum()), 'core_exact_archived_match': True, 'cohorts_reproduced': True,
             'adapter_active_cycles': int(adapter_valid.sum()), 'adapter_status_counts': dict(reasons),

tools/ford_pscm_lab/stress_model_action.py

@@ -1,4 +1,7 @@
-"""Deterministic numerical stress and exhaustive field-boundary CAN checks.
+"""Zero-error numerical stress and exhaustive field-boundary CAN checks.
+
+Measured curvature equals requested curvature here to preserve the independent
+feedforward oracle. Feedback behavior is covered by its own tests and replay.
 
 Analytic straight/rotated paths supply an independent y(7) oracle. The
 reference slew uses scalar arithmetic. Packing is checked against direct
@@ -64,8 +67,8 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
     model, mirror = line(offset, heading), line(-offset, -heading)
     if i % 401 == 0:
       model.position.y[4] = mirror.position.y[4] = math.nan
-    out = controller.update(model, desired, speed=speed, dt=dt, active=active, valid=valid)
-    other = mirrored.update(mirror, -desired, speed=speed, dt=dt, active=active, valid=valid)
+    out = controller.update(model, desired, current_curvature=desired, speed=speed, dt=dt, active=active, valid=valid)
+    other = mirrored.update(mirror, -desired, current_curvature=-desired, speed=speed, dt=dt, active=active, valid=valid)
     expected_valid = active and valid and dt <= .1 and i % 401 != 0
     assert out.valid == other.valid == expected_valid
     previous = np.array([c0, c1])
@@ -99,14 +102,14 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
         selected = float(np.clip(scalar, low, high))
         offset, heading = (selected, 0.) if field == 0 else (0., selected)
         controller.c0, controller.c1 = offset, heading
-        out = controller.update(line(offset), heading/20., speed=20., dt=.01)
+        out = controller.update(line(offset), heading/20., current_curvature=heading/20., speed=20., dt=.01)
         check_raw_packing(wire, controller, out)
         boundary_cases += 1
   report = {'seed': seed, 'random_cycles': cycles, 'mirrored_core_updates': cycles,
             'invalid_or_inactive_resets': resets, 'field_boundary_cases': boundary_cases,
             'float32_can_round_trips': wire.count, 'analytic_targets_scalar_slew_and_mirror_checks_pass': True,
             'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(),
-            'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.',
+            'calibration_approved': False, 'scope': 'Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.',
             'opendbc_import_head': revision(dependency),
             'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in
                               (Path(__file__), Path(ford_model_action.__file__), Path(__file__).with_name('model_action_replay.py'))}}