Ford: sample position and heading from one shared model point

Use the model-predicted one-second station with the existing seven-metre minimum and endpoint hold for both C0 and C1. Remove the extra yaw forecast and scalar-curvature heading reconstruction; retain C2/C3 zero, independent limits and slew, the existing toggle, and 20Hz sends. Reject scalar-only maneuver references explicitly.

Validate model clocks, geometry, CAN delivery, Panda TX acceptance, and randomized boundaries offline. Recorded-input replay does not establish improved physical tracking.

Assisted-by: OpenAI Codex

docs/ford_model_action_drive_test.md

@@ -1,123 +1,80 @@
-# Ford selected-action drive-test branch
+# Ford model-point drive-test branch
 
-The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind
-its own persistent, default-off Sunnylink toggle. Version 6 uses calibrated
-measured turn rate for C0's existing 150 ms vehicle-pose forecast. C1 still uses
-selected, upstream-limited curvature. Raw Ford yaw remains an input-health and
-diagnostic signal; calibrated motion now affects C0. This is yaw feedback whose
-sensitivity depends on the existing preview time and path distance. There is no
-fitted PSCM plant model, added strength multiplier, new filter or extra core state.
-`calibration_approved=false`: offline checks do not establish physical tracking,
-turn-exit behavior or closed-loop stability.
+The default-off **Selected-Action Path Tracking (Experimental)** Sunnylink toggle
+now selects the v7 model-point candidate on the **Ford CAN FD F-150 Lightning**.
+The stored key remains `FordModelActionController`; an already enabled setting
+selects this revision after updating and completing an offroad-to-onroad cycle.
 
-The current cadence experiment transmits this controller's LMC2 requests at
-**20 Hz (50 ms)** while its calculation remains at **100 Hz**. See
-[cadence validation](ford_model_action_cadence.md). The v6 control law and
-per-second C0/C1 slew are unchanged; this is not a demonstrated tracking fix.
+The controller reads **model lateral position and model heading at the same
+point**. Start with the model's predicted distance at one second, enforce the
+existing seven-metre minimum, and hold the available endpoint when necessary.
+C0 is that point's lateral position in metres; C1 is its unwrapped heading in
+radians. C2 and C3 stay zero. The 150 ms yaw-based forecast and the reconstruction
+of heading from selected curvature are removed.
+
+This point choice is an engineering guess, not an identified Ford reference or
+PSCM calibration. `calibration_approved=false`: offline tests do not establish
+physical tracking, turn-exit behavior, or stability across different PSCMs.
+See [the model-point decision and validation](ford_model_points.md).
 
 ## Select and restore
 
-1. Install branch `hiimisaac-dev` from
-   `sunnypilot/sunnypilot` on the device using your normal branch-switch process.
-   Allow its build to finish before changing the setting.
-2. While offroad, open Sunnylink device settings → Vehicle → Ford and enable
-   **Selected-Action Path Tracking (Experimental)** (`FordModelActionController`).
+1. Install branch `hiimisaac-dev` from `sunnypilot/sunnypilot` and allow the build
+   to finish.
+2. While offroad, open Sunnylink device settings → Vehicle → Ford. Keep or enable
+   **Selected-Action Path Tracking (Experimental)**.
 3. Complete a real offroad-to-onroad cycle. `card` snapshots the toggle into
    `CarParamsSP`; the sender and `controlsd` share that selection. Changing a
-   stored toggle or disengaging alone cannot swap an active
-   controller. Initial physical evaluation remains controlled testing.
+   stored toggle or disengaging alone cannot swap an active controller.
 
-The startup log event `Ford path controller selected` should report
-`FordModelActionController`. Periodic `Ford C2-free path tracking` events
-identify `hypothesis=model-action-measured-pose-v6`. Active events report
-`pose_source=measured` when fresh calibrated motion is used, otherwise
-`pose_source=requested`. They also include `pose_yaw_rate`, `pose_age`, raw host
-`yaw_rate`, selected curvature and the command tuple.
+The startup event `Ford path controller selected` reports
+`FordModelActionController`. Periodic `Ford C2-free path tracking` events report
+`hypothesis=model-pose-one-second-v7`, `pose_source=model`, `preview_time_s=1.0`
+and `minimum_station_m=7.0`, plus input ages, slew state and the command tuple.
+Selected desired curvature is still logged, but no longer constructs C0/C1.
 
-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.
+Turning the toggle off and completing another offroad-to-onroad cycle restores
+**PSCM Coefficient Observer** if selected, otherwise the original Ford path
+controller. Other vehicles keep their previous selection. The retired v8 toggle
+cannot select this candidate.
 
-The v8 implementation, its Sunnylink toggle and its dedicated tests are removed.
-A leftover `FordVirtualAngleController=1` file cannot enable the new controller.
-The shared Float32/CAN rounding helper now lives in `ford_model_action.py`;
-unused v8 PSCM-feedback plumbing is removed. Historical v8 route evidence remains
-in Git history and the archived validation documents.
+## Wiring and limits
 
-## Wiring and validation
+Both model fields must have matching, finite, strictly increasing time arrays
+starting at zero. Malformed geometry, stale required services, invalid timing,
+or disengagement resets both actuator states. Freshness still requires model,
+car-state and reference publications no older than 150 ms, with at most 5 ms
+future skew. The valid control timestep remains 2–100 ms.
 
-`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. The cadence experiment updates the opendbc sender and
-submodule pointer. Panda safety is unchanged.
+Only the two unquantized C0/C1 slew positions persist in the core. Field caps are
+±5.11 m / ±0.5 rad and slew rates are 4 m/s / 0.5 rad/s. Calculation stays at
+100 Hz; the existing [cadence experiment](ford_model_action_cadence.md) sends this
+candidate at 20 Hz. Float32 publication, host-to-wire negation, packing and
+Panda safety are unchanged. The opendbc pin remains
+`87ca78e6e641eefb2d654f260a6ab08df3058bd5`.
 
-Measured-pose use requires healthy `deviceMotion` and `extrinsicsCalibration`
-services, calibrated extrinsics no older than 1 s, valid angular velocity and
-sensor/input flags, and finite calibrated yaw within ±3 rad/s. Both the motion
-publication and its embedded filter-state timestamp must be no older than
-150 ms; all three age checks allow at most 5 ms future skew. Calibration-only
-updates rebuild the cached pose before the candidate uses it.
-Unavailable, unhealthy or stale optional motion falls back to the v5
-requested-curvature forecast without resetting C0/C1 slew. Existing base-input
-failures still invalidate the command. C1, the ±5.11 m / ±0.5 rad field caps,
-4 m/s / 0.5 rad/s slew rates, packing and two core states are unchanged.
+Normal operation uses the model's point directly. The upstream scalar curvature
+and its clipping still exist for logging/other controllers, but no longer bound
+this candidate's heading target. Its C0/C1 field caps and slew still apply;
+passing Panda TX checks does not establish an actual vehicle acceleration bound.
+Lateral maneuver test mode supplies only a scalar curvature, not a model pose.
+It explicitly invalidates/disengages this candidate as `unsupported_reference`.
+Optional measured motion is no longer a command input.
 
-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.
+## Reproduce offline checks
 
-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_measured_pose_validation.json`.
-Its runtime replay exactly matches the reviewed offline candidate across
-340,757 recorded cycles and 681,514 Float32/CAN round trips. C1 and eligibility
-match v5 on all four extracts. The replay checks frozen recorded inputs; it does
-not simulate how the vehicle would respond to different commands.
-
-The candidate adds C0 during the recorded weak-bend turn-rate shortfalls and
-reduces it during the older overshoot example. Tight-turn C1 saturation remains.
-On the 70.24-second sustained quiet-path cohort, C0 amplitude RMS decreases but
-per-cycle change RMS rises about 28%; physical centering and tracking still need
-evaluation. All four routes came from the same truck, so cross-PSCM performance
-has not been demonstrated.
-
-`ford_model_action_no_yaw_damping_validation.json` archives v5 checks;
-the [v5 notes](ford_model_action_no_yaw_damping.md) explain the prior damping removal.
-`ford_model_action_full_prediction_validation.json` archives v4 checks;
-the [full-prediction notes](ford_model_action_full_prediction.md) explain cap removal
-and remaining physical uncertainty. `ford_model_action_prediction_validation.json`
-archives the capped v3 evaluation. `ford_model_action_damping_validation.json`
-archives the preceding v2 checks at their recorded source hashes.
-`ford_model_action_drive_test_validation.json` archives v1 wiring validation
-at the recorded source hashes, including 284 tests and 26 subtests. Its counts
-and 145-line controller size describe v1. The 469-line v8 module remains removed.
-
-The original 133,550-cycle route reconstruction, 485,238 packing round trips
-and mutation probes remain recorded separately in
-`ford_model_action_validation.json` at the offline-stage source hashes.
-
-## Reproduce deployment checks
-
-Initialize the branch's pinned opendbc submodule with `git submodule update --init opendbc_repo`
-and build the native Params library from this branch before testing.
+Initialize the pinned submodule and build the project's native Python dependencies:
 
 ```sh
+git submodule update --init opendbc_repo
 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 "$(git -C opendbc_repo rev-parse HEAD)" --output .cache/ford_model_action_drive_test/stress.json
+python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests openpilot/sunnypilot/mads/tests openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/common/tests/test_params.py opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision 87ca78e6e641eefb2d654f260a6ab08df3058bd5 --output .cache/ford_model_points/stress.json
 ```
 
-The full hardware build and device boot are not performed by these offline
-tests. Installing the branch and enabling the toggle are separate actions;
-pushing the branch does not change a device's selected software or settings.
+Historical v1–v6 validation files retain their original source hashes and apply
+to those revisions. In particular, `ford_model_action_measured_pose_validation.json`
+describes v6, not the current model-point mapping. The hardware build and device
+boot are not performed by these offline checks. Pushing a branch does not update
+a device or change its stored settings.

docs/ford_model_points.md

@@ -0,0 +1,77 @@
+# Ford model-point candidate v7
+
+## Decision
+
+Use the model's own position and orientation at one shared path point. This
+implements the request to follow model geometry through C0/C1, with C2/C3 zero,
+without a fitted PSCM plant, new strength gain, yaw integral, or release mode.
+
+Let `s(t)` be cumulative planar arc distance along model position. Choose:
+
+```
+station = min(path_end, max(7 metres, s(1 second)))
+C0_target = model.position.y at station
+C1_target = unwrapped model.orientation.z at station
+C2 = C3 = 0
+```
+
+Use the published model timestamps, not `speed × 1 second`, so the point also
+follows the model's predicted acceleration/braking. The seven-metre floor keeps
+the existing low-speed preview distance; it can select a time beyond one second.
+If the entire path is shorter, both fields hold the same endpoint. Interpolation
+uses the same arc segment and weights for position and heading.
+
+One second keeps C1 near the old one-second heading scale in ordinary driving.
+Using that same point for C0 represents a meaningful change in faster bends.
+This is a chosen local approximation; Ford's expected reference point and
+preview are unknown. Matching metres/radians does not prove PSCM equivalence.
+
+The former 150 ms vehicle-pose forecast is removed. C1 no longer uses
+`max(7, speed) × selected desiredCurvature`. Measured yaw remains an input-health
+check only; calibrated yaw has no command role. Freshness, independent slew,
+packing, the shared startup toggle, 20 Hz sends, and Panda safety remain.
+The upstream scalar curvature is still logged but does not limit this geometry;
+C0/C1 retain their existing amplitude and slew limits. The scalar-only lateral
+maneuver test reference is explicitly unsupported and disengages this candidate.
+
+## Offline evidence
+
+The broad run passed **572 tests and 9,146 subtests**, with 178 inherited safety
+cases skipped as inapplicable. Tests of the removed yaw forecast were retired;
+new tests cover actual model clocks, a nonconstant-speed trajectory, shared-point
+sampling, the distance floor, endpoint holding, heading unwrap, source selection,
+invalid geometry, reset, independent slew, and actual CAN delivery.
+
+The stress run covers 200,000 random cycles plus mirrored turns, 18,138 field
+boundary cases, and 218,138 Float32/CAN round trips. An independent scalar oracle
+checks analytic model points and slew. No vehicle plant is simulated.
+
+Five recorded routes (a5, a2, a0, 9b, 9e) supply **379,718 controller cycles** and
+**76,294 model frames**. Original rlog hashes and position/orientation timestamps
+were checked. An independently implemented segment-weight oracle matches every
+sampled target and eligible slew state. Every cycle passed packing/bound checks;
+all **75,947** scheduled 20 Hz requests passed the actual unchanged Panda TX hook.
+This is TX acceptance with controlled eligibility, not a full Panda RX watchdog
+or vehicle-response replay. The [validation manifest](ford_model_points_validation.json)
+binds the results to their source and input hashes.
+
+On a5's 61.24-second driver-clean ordinary-bend cohort, mean absolute C0 changes
+from 0.075 m recorded to 0.167 m replayed; C1 changes from 0.02287 to 0.02224 rad.
+Neither field target clips in that cohort. These are command differences, not
+predicted changes in steering strength or tracking error.
+
+| a5 time | Recorded C0 / C1 | Candidate C0 / C1 | Selected station / time |
+| --- | --- | --- | --- |
+| 107.995 s, ordinary bend | +0.20 m / +0.068 rad | +0.55 m / +0.0685 rad | 13.71 m / 1.00 s |
+| 390.681 s, sustained bend | −0.52 m / −0.1235 rad | −0.75 m / −0.1215 rad | 9.37 m / 1.00 s |
+| 374.889 s, tight turn | −3.76 m / −0.50 rad | −3.46 m / −0.50 rad | 7.00 m / 1.49 s |
+| 17.607 s, exit overshoot | −0.01 m / −0.015 rad | −0.05 m / −0.012 rad | 7.00 m / 1.85 s |
+
+The exit example retains more C0 into the turn than v6; physical unwind behavior
+must be evaluated. Tight-turn C1 clipping remains (14.92 eligible seconds on a5).
+No root cause or physical fix is proven by frozen inputs. The latest a5 road
+logs used 100 Hz sends, whereas the immediately preceding code revision already
+changed to 20 Hz; a comparison against that drive also includes the cadence change.
+
+`calibration_approved=false` remains. Installation instructions and restore
+behavior are in the [drive-test guide](ford_model_action_drive_test.md).

docs/ford_model_points_validation.json

@@ -0,0 +1,391 @@
+{
+  "baseline_root": "e1cd61166c529f39cb47c815db47e80de720a778",
+  "opendbc": "87ca78e6e641eefb2d654f260a6ab08df3058bd5",
+  "scope": "Model-point construction only; physical tracking not validated.",
+  "calibration_approved": false,
+  "tests": {
+    "passed": 572,
+    "subtests_passed": 9146,
+    "inapplicable_skips": 178
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+    "openpilot/selfdrive/controls/controlsd.py": "bea720b8ec68d6b8a6ad4376ae37324e2404833fb13097a8ab7710a1cedf37a5",
+    "tools/ford_pscm_lab/stress_model_action.py": "0221f85ea2f5757d3b54d077ed8be1faaa6a5d2ed8371de1137f3b6f3d70bf47",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "ec5e0d9023f7260ae619a4cc6ec538d39053ac13e4515bd0de7d854b79d8abf6",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "62e5178a8deee32ef5a88026094cedde359130741688e8a4bcfa82155ef4cca3"
+  },
+  "routes": {
+    "a5": {
+      "route": "a5",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 38961,
+      "models": 7817,
+      "round_trips": 38961,
+      "panda_accepted_sends": 7793,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 359.186785903,
+          "old_mean_abs_c0": 0.34434291232781916,
+          "new_mean_abs_c0": 0.3706073530276498,
+          "old_mean_abs_c1": 0.05784747586810226,
+          "new_mean_abs_c1": 0.06183462577629167,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.919213676999902
+        },
+        "road_speed": {
+          "seconds": 261.730126543,
+          "old_mean_abs_c0": 0.10481662146430282,
+          "new_mean_abs_c0": 0.16319455033344274,
+          "old_mean_abs_c1": 0.025151988851597765,
+          "new_mean_abs_c1": 0.02471229299756163,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 97.45665935999997,
+          "old_mean_abs_c0": 0.9876160024355487,
+          "new_mean_abs_c0": 0.9276362872461182,
+          "old_mean_abs_c1": 0.14565465097969882,
+          "new_mean_abs_c1": 0.16153056158564297,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.919213676999902
+        },
+        "clean": {
+          "seconds": 249.91415584299997,
+          "old_mean_abs_c0": 0.08470877473478762,
+          "new_mean_abs_c0": 0.1191542994782463,
+          "old_mean_abs_c1": 0.017726187650852487,
+          "new_mean_abs_c1": 0.018543416153046232,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 1.056498005999913
+        },
+        "clean_ordinary_bends": {
+          "seconds": 61.23773978999962,
+          "old_mean_abs_c0": 0.07537829679720884,
+          "new_mean_abs_c0": 0.16659385323910988,
+          "old_mean_abs_c1": 0.022868833214909796,
+          "new_mean_abs_c1": 0.022235380107927534,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [
+        {
+          "t": 17.606574576,
+          "old_c0_c1": [
+            -0.009999999776482582,
+            -0.014999999664723873
+          ],
+          "new_c0_c1": [
+            -0.04999999999999982,
+            -0.01200000000000001
+          ],
+          "raw_target": [
+            -0.04535145975205448,
+            -0.012038111718131041
+          ],
+          "sample_station_m": 7.0,
+          "sample_time_s": 1.8525775632377612
+        },
+        {
+          "t": 107.99483939600002,
+          "old_c0_c1": [
+            0.20000000298023224,
+            0.06800000369548798
+          ],
+          "new_c0_c1": [
+            0.5499999999999998,
+            0.0685
+          ],
+          "raw_target": [
+            0.548033630847931,
+            0.06853501158101219
+          ],
+          "sample_station_m": 13.711669224561392,
+          "sample_time_s": 1.0
+        },
+        {
+          "t": 374.888921539,
+          "old_c0_c1": [
+            -3.759999990463257,
+            -0.5
+          ],
+          "new_c0_c1": [
+            -3.46,
+            -0.5
+          ],
+          "raw_target": [
+            -3.497294441592979,
+            -0.6981158781127829
+          ],
+          "sample_station_m": 7.0,
+          "sample_time_s": 1.490557194102553
+        },
+        {
+          "t": 390.68080671100006,
+          "old_c0_c1": [
+            -0.5199999809265137,
+            -0.12349999696016312
+          ],
+          "new_c0_c1": [
+            -0.75,
+            -0.12150000000000005
+          ],
+          "raw_target": [
+            -0.7522029416901727,
+            -0.1214686983398029
+          ],
+          "sample_station_m": 9.370794504969545,
+          "sample_time_s": 1.0
+        }
+      ],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a5_models.npz": "84cf4b6e0102d8d5a3f6c3e29bf332e929d7f28d0262eb46220fccc631b902aa"
+      }
+    },
+    "a2": {
+      "route": "a2",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 71111,
+      "models": 14287,
+      "round_trips": 71111,
+      "panda_accepted_sends": 14223,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 437.0644468180001,
+          "old_mean_abs_c0": 0.2155311761983153,
+          "new_mean_abs_c0": 0.31863289785799315,
+          "old_mean_abs_c1": 0.04341179516514646,
+          "new_mean_abs_c1": 0.045887913019549746,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 11.38161301699995
+        },
+        "road_speed": {
+          "seconds": 328.02165902499996,
+          "old_mean_abs_c0": 0.06680861343584246,
+          "new_mean_abs_c0": 0.21392942512598495,
+          "old_mean_abs_c1": 0.025146318618058133,
+          "new_mean_abs_c1": 0.025146126320953914,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 109.04278779300012,
+          "old_mean_abs_c0": 0.6629172230811673,
+          "new_mean_abs_c0": 0.6336010633494195,
+          "old_mean_abs_c1": 0.09835785845941926,
+          "new_mean_abs_c1": 0.10828319309008405,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 11.38161301699995
+        },
+        "clean": {
+          "seconds": 332.6767952400001,
+          "old_mean_abs_c0": 0.07338523837198829,
+          "new_mean_abs_c0": 0.1946808147540538,
+          "old_mean_abs_c1": 0.023104103590461375,
+          "new_mean_abs_c1": 0.022903078905402382,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 2.902278352000053
+        },
+        "clean_ordinary_bends": {
+          "seconds": 111.90846122999926,
+          "old_mean_abs_c0": 0.07917757541079931,
+          "new_mean_abs_c0": 0.3589047266588909,
+          "old_mean_abs_c1": 0.03830142702025842,
+          "new_mean_abs_c1": 0.03807353528971872,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a2_models.npz": "c3a366796ca5fb27f8c0dc85732ccd6f0ec61b4c8646af3dfe55600e026c0810"
+      }
+    },
+    "a0": {
+      "route": "a0",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 114582,
+      "models": 23038,
+      "round_trips": 114582,
+      "panda_accepted_sends": 22917,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 1061.1945591,
+          "old_mean_abs_c0": 0.16681197800600925,
+          "new_mean_abs_c0": 0.25477442195222594,
+          "old_mean_abs_c1": 0.036905620742387873,
+          "new_mean_abs_c1": 0.0381744756199198,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.894412850999998
+        },
+        "road_speed": {
+          "seconds": 901.0129238640001,
+          "old_mean_abs_c0": 0.038282177648169546,
+          "new_mean_abs_c0": 0.13938686570168032,
+          "old_mean_abs_c1": 0.016746723820478274,
+          "new_mean_abs_c1": 0.01614716305741802,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 160.18163523600006,
+          "old_mean_abs_c0": 0.889785064486564,
+          "new_mean_abs_c0": 0.9038231052276234,
+          "old_mean_abs_c1": 0.15029831167554059,
+          "new_mean_abs_c1": 0.1620769021843944,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.894412850999998
+        },
+        "clean": {
+          "seconds": 884.2159145080002,
+          "old_mean_abs_c0": 0.05133685312289168,
+          "new_mean_abs_c0": 0.13569127462997554,
+          "old_mean_abs_c1": 0.017060990972111164,
+          "new_mean_abs_c1": 0.01680535356441334,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.094436286000047
+        },
+        "clean_ordinary_bends": {
+          "seconds": 229.50590253700045,
+          "old_mean_abs_c0": 0.06277285849992259,
+          "new_mean_abs_c0": 0.29272144832989233,
+          "old_mean_abs_c1": 0.032915757288861924,
+          "new_mean_abs_c1": 0.03226126900978203,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a0_models.npz": "72a72c950d244f5afcffe8cb1cd09b9731297f7565c93814197c728d4b25e8cb"
+      }
+    },
+    "9b": {
+      "route": "9b",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 71396,
+      "models": 14346,
+      "round_trips": 71396,
+      "panda_accepted_sends": 14280,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 651.159063169,
+          "old_mean_abs_c0": 0.09686840630728118,
+          "new_mean_abs_c0": 0.16616909035237892,
+          "old_mean_abs_c1": 0.020769878239136775,
+          "new_mean_abs_c1": 0.02325818302804746,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.1034965929999885
+        },
+        "road_speed": {
+          "seconds": 549.860784016,
+          "old_mean_abs_c0": 0.025487998809066218,
+          "new_mean_abs_c0": 0.10707029577162376,
+          "old_mean_abs_c1": 0.011795500864550536,
+          "new_mean_abs_c1": 0.01141539602134342,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 101.29827915299992,
+          "old_mean_abs_c0": 0.484330929447169,
+          "new_mean_abs_c0": 0.48696535455636186,
+          "old_mean_abs_c1": 0.06948401455523531,
+          "new_mean_abs_c1": 0.08754243546441211,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.1034965929999885
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_route9b/route.npz": "8942b281b76eac012ee660f8ae21bbcad0df95d515291d2497ce5897d5738647",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/9b_models.npz": "fa444360a2775b0df38f274ab4c99568bbe47a3d2777c31ab0193705fce2b1fb"
+      }
+    },
+    "9e": {
+      "route": "9e",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 83668,
+      "models": 16806,
+      "round_trips": 83668,
+      "panda_accepted_sends": 16734,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 751.5479497329998,
+          "old_mean_abs_c0": 0.09489001052394747,
+          "new_mean_abs_c0": 0.16056913479509022,
+          "old_mean_abs_c1": 0.02256825583285058,
+          "new_mean_abs_c1": 0.022300154628921095,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 7.398116462998587
+        },
+        "road_speed": {
+          "seconds": 624.6602415809975,
+          "old_mean_abs_c0": 0.023757802428838078,
+          "new_mean_abs_c0": 0.10203005034223132,
+          "old_mean_abs_c1": 0.01124063950870769,
+          "new_mean_abs_c1": 0.010770200237869725,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 126.88770815200223,
+          "old_mean_abs_c0": 0.4450694167092398,
+          "new_mean_abs_c0": 0.4487533818653312,
+          "old_mean_abs_c1": 0.07833340166635812,
+          "new_mean_abs_c1": 0.07906139809550021,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 7.398116462998587
+        },
+        "clean": {
+          "seconds": 639.296275887018,
+          "old_mean_abs_c0": 0.02396620128468532,
+          "new_mean_abs_c0": 0.09600545527466989,
+          "old_mean_abs_c1": 0.01021397220255641,
+          "new_mean_abs_c1": 0.010198642694698857,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "clean_ordinary_bends": {
+          "seconds": 168.17771330400137,
+          "old_mean_abs_c0": 0.05087679617819314,
+          "new_mean_abs_c0": 0.2527786285635614,
+          "old_mean_abs_c1": 0.02663914589176354,
+          "new_mean_abs_c1": 0.026386933889696897,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_route9e/route.npz": "9f169338caf2b5a5377332d15914d1dde57be8b922e25c1bbe16264e0f1be6fb",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/9e_models.npz": "acacf8f0d16fd986b6d74e4a02d704fa5b5b3327fe66dcbe149806f2a5bc3d65"
+      }
+    }
+  }
+}

openpilot/selfdrive/controls/controlsd.py

@@ -87,7 +87,7 @@ class Controls(ControlsExt):
     self.sm.update(15)
     if self.sm.updated["extrinsicsCalibration"]:
       self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration'])
-    if self.sm.updated["deviceMotion"] or (self.ford_model_action and self.sm.updated["extrinsicsCalibration"]):
+    if self.sm.updated["deviceMotion"]:
       device_motion = Pose.from_device_motion(self.sm['deviceMotion'])
       self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion)
 
@@ -174,19 +174,12 @@ class Controls(ControlsExt):
         assert isinstance(self.ford_path_controller, FordModelActionController)
         reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
         now = time.monotonic()
-        motion = self.sm['deviceMotion']
-        pose_valid = (self.calibrated_pose is not None and self.pose_calibrator.calib_valid and
-                      self.sm.all_checks(['deviceMotion', 'extrinsicsCalibration']) and
-                      motion.angularVelocityDevice.valid and motion.sensorsOK and motion.inputsOK and
-                      -.005 <= now - self.sm.logMonoTime['deviceMotion'] * 1e-9 <= .15 and
-                      -.005 <= now - self.sm.logMonoTime['extrinsicsCalibration'] * 1e-9 <= 1.)
         self.ford_path = self.ford_path_controller.update(
           ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=now,
           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,
-          pose_yaw_rate=self.calibrated_pose.angular_velocity.z if pose_valid else None,
-          pose_time=motion.timestamp * 1e-9, pose_valid=pose_valid,
+          reference_source=reference_service,
           active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
         )
         if not self.ford_path.valid:

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -1,7 +1,7 @@
-"""Experimental Ford C2-free controller: nearby offset and selected-action heading.
+"""Experimental Ford C2-free controller using one shared model-path point.
 
-Selected only by its explicit toggle. The 7 m station and one-second scale are
-engineering choices, not identified PSCM gains or physical calibration.
+The one-second preview and seven-metre minimum are engineering choices,
+not identified Ford reference points or PSCM calibration.
 """
 import math
 import struct
@@ -12,32 +12,9 @@ from opendbc.car.ford.values import FordFlags
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
 
 
-OFFSET_STATION_M = 7.0
-HEADING_TIME_S = 1.0
+MIN_STATION_M = 7.0
+PREVIEW_TIME_S = 1.0
 CALIBRATION_APPROVED = False
-PREDICTION_TIME_S = .15  # geometric preview, not an identified actuator delay
-
-
-def _predict_offset(path, c0, pose_curvature, speed):
-  """Read the same path from a predicted constant-curvature vehicle pose.
-
-  A matched constant-radius path retains its offset. Developing/flattening
-  bends can move the target earlier. The core retains field limits and slew.
-  Use only available geometry; shortened horizons taper prediction to zero.
-  """
-  station, longitudinal, lateral, _ = path
-  distance = min(speed*PREDICTION_TIME_S, max(0., station[-1]-OFFSET_STATION_M))
-  if distance == 0.:
-    return c0
-  x = float(np.interp(OFFSET_STATION_M+distance, station, longitudinal))
-  y = float(np.interp(OFFSET_STATION_M+distance, station, lateral))
-  rotation = pose_curvature*distance
-  # (1-cos(rotation))/curvature, evaluated without cancellation or division by zero.
-  translation = distance*math.sin(rotation/2)*float(np.sinc(rotation/(2*math.pi)))
-  predicted = math.cos(rotation)*y-math.sin(rotation)*x+translation
-  if not _finite(predicted):
-    return c0
-  return predicted
 
 
 def _packed(value, resolution, offset):
@@ -53,40 +30,35 @@ def _finite(*values):
     return False
 
 
-def encode_model_action(model, desired_curvature, speed, *, pose_yaw_rate=None):
-  """Encode predicted y(7) and max(7, v*1s)*selected limited curvature.
+def encode_model_action(model, desired_curvature, speed):
+  """Sample model lateral position and heading at the same arc station.
 
-  Preserve the reviewed core's endpoint hold when the path ends before 7 m.
-  This samples the available geometry; it does not extrapolate an unseen path.
-  Calibrated measured yaw predicts the vehicle pose when available; otherwise
-  retain the selected-curvature prediction. This is geometric yaw feedback,
-  whose sensitivity depends on the existing preview time and path distance.
+  Use the model's distance at one second, with a seven-metre minimum and an
+  endpoint hold. Selected curvature remains a health/diagnostic input; it does
+  not reconstruct heading or rotate the model geometry in this experiment.
   """
   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:
+    times = [float(t) for t in model.position.t]
+    heading_times = [float(t) for t in model.orientation.t]
+  except (AttributeError, TypeError, ValueError, OverflowError):
     return FordPath()
   if path is None or not all(_finite(*values) for values in path):
     return FordPath()
-  station, _, lateral, _ = path
-  c0 = float(np.interp(min(OFFSET_STATION_M, station[-1]), station, lateral))
-  pose_curvature = desired_curvature
-  if pose_yaw_rate is not None and _finite(pose_yaw_rate) and abs(pose_yaw_rate) <= 3:
-    pose_curvature = pose_yaw_rate / speed
-  c0 = _predict_offset(path, c0, pose_curvature, speed)
-  c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
+  station, _, lateral, heading = path
+  if (len(times) != len(station) or not times or times[0] != 0. or times != heading_times or
+      not _finite(*times) or any(b <= a for a, b in zip(times, times[1:], strict=False))):
+    return FordPath()
+  sample_station = min(station[-1], max(MIN_STATION_M, float(np.interp(PREVIEW_TIME_S, times, station))))
+  c0 = float(np.interp(sample_station, station, lateral))
+  c1 = float(np.interp(sample_station, station, heading))
   return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
 
 
 class ModelActionController:
-  """Only two control states: unquantized, independently slewed C0 and C1.
-
-  Freshness and engagement belong to the caller. Raw Ford yaw checks input
-  health only. A separate calibrated yaw input can change the offset forecast;
-  heading always follows the selected limited curvature.
-  """
+  """Only two states: independently slewed C0/C1 model-point requests."""
   __slots__ = ('c0', 'c1')
 
   def __init__(self):
@@ -95,12 +67,12 @@ class ModelActionController:
   def reset(self):
     self.c0 = self.c1 = 0.
 
-  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True, pose_yaw_rate=None):
+  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True):
     # Raw Ford yaw remains an input-health check, not a pose measurement.
     if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3:
       self.reset()
       return FordPath()
-    target = encode_model_action(model, desired_curvature, speed, pose_yaw_rate=pose_yaw_rate)
+    target = encode_model_action(model, desired_curvature, speed)
     if not target.valid:
       self.reset()
       return FordPath()
@@ -112,17 +84,11 @@ class ModelActionController:
 
 
 class FordModelActionController:
-  """Input adapter for the opt-in selected-action controller.
+  """Freshness, engagement and reference checks for model-point tracking.
 
-  controlsd owns upstream selection/limiting and service health. This adapter
-  checks ages and clock order, then supplies elapsed time to the two-state
-  core. Pose age gates measured-motion use; diagnostics do not feed back into
-  the core. Raw model geometry is checked even at a repeated model timestamp.
-
-  Raw Ford yaw supplies diagnostics and input-health checks. Fresh, healthy
-  calibrated motion supplies pose prediction; unavailable motion falls back
-  to the existing requested-pose forecast without resetting the slew states.
-  PSCM status and driver torque are not control-law inputs.
+  Scalar-only maneuver references cannot supply this controller's model pose.
+  Reject them explicitly rather than silently following a different reference.
+  Measured yaw checks input health only; it never modifies valid geometry.
   """
   def __init__(self):
     self.core = ModelActionController()
@@ -131,14 +97,16 @@ 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-measured-pose-v6',
+    self.diagnostics = {'status': status, 'hypothesis': 'model-pose-one-second-v7',
                         '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, pose_yaw_rate=None, pose_time=None, pose_valid=False):
+             active, valid=True, reference_source="modelV2"):
     reason = None
     if not active:
       reason = 'inactive'
+    elif reference_source != 'modelV2':
+      reason = 'unsupported_reference'
     elif not valid:
       reason = 'invalid_service'
     elif not _finite(desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
@@ -157,21 +125,15 @@ class FordModelActionController:
     ):
       self.reset('timing_reset')
       return FordPath()
-    pose_age = now - pose_time if _finite(pose_time) else None
-    if not _finite(pose_age):
-      pose_age = None
-    use_pose = (pose_valid and pose_yaw_rate is not None and pose_age is not None and
-                _finite(pose_yaw_rate) and abs(pose_yaw_rate) <= 3 and -.005 <= pose_age <= .15)
-    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate,
-                               pose_yaw_rate=pose_yaw_rate if use_pose else None)
+    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate)
     if not command.valid:
       self.reset('invalid_path')
       return command
     self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
-    self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-measured-pose-v6',
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-pose-one-second-v7',
                         'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
-                        'yaw_rate': yaw_rate, 'pose_source': 'measured' if use_pose else 'requested',
-                        'pose_yaw_rate': pose_yaw_rate if use_pose else None, 'pose_age': pose_age,
+                        'yaw_rate': yaw_rate, 'pose_source': 'model',
+                        'preview_time_s': PREVIEW_TIME_S, 'minimum_station_m': MIN_STATION_M,
                         'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
                         'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
                         'command': (command.path_offset, command.path_angle, 0., 0.)}

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -49,16 +49,15 @@ class TestFordControlsLogging(unittest.TestCase):
     controller = FordModelActionController()
     for active, valid in ((False, True), (True, True), (True, False)):
       controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1.,
-                        measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid,
-                        pose_yaw_rate=.04, pose_time=.98, pose_valid=True)
+                        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-measured-pose-v6')
+      self.assertEqual(record['hypothesis'], 'model-pose-one-second-v7')
       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.assertEqual(record['pose_source'], 'measured')
-        self.assertAlmostEqual(record['pose_yaw_rate'], .04)
-        self.assertAlmostEqual(record['pose_age'], .02)
+        self.assertEqual(record['pose_source'], 'model')
+        self.assertEqual(record['preview_time_s'], 1.)
+        self.assertEqual(record['minimum_station_m'], 7.)

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -12,7 +12,8 @@ from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionContro
 
 
 def make_model(x, y, heading):
-  return SimpleNamespace(position=SimpleNamespace(x=x, y=y), orientation=SimpleNamespace(z=heading))
+  times = np.linspace(0., 3., len(x))
+  return SimpleNamespace(position=SimpleNamespace(t=times, x=x, y=y), orientation=SimpleNamespace(t=times, z=heading))
 
 
 def circle(curvature):
@@ -20,26 +21,26 @@ def circle(curvature):
   return make_model(np.sin(curvature*s)/curvature, (1-np.cos(curvature*s))/curvature, curvature*s)
 
 
-def straight(offset=0.):
+def straight(offset=0., heading=0.):
   x = np.linspace(0., 60., 121)
-  return make_model(x, np.full_like(x, offset), np.zeros_like(x))
+  return make_model(x*np.cos(heading), offset+x*np.sin(heading), np.full_like(x, heading))
 
 
-def test_selected_action_controls_heading_even_when_model_previews_another_turn():
+def test_model_heading_is_used_even_when_scalar_action_differs():
   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.
+  for desired in (0., -.004, .004):
+    target = encode_model_action(model, desired, 20.)
+    assert target.path_angle == pytest.approx(.4)
+    assert target.path_offset == pytest.approx((1-math.cos(.4))/.02)
 
 
-def test_straight_centering_and_matched_curves_keep_base_gain_across_speed():
+def test_straight_centering_and_matched_model_circles():
   for speed in (2., 7., 20., 35.):
-    target = encode_model_action(straight(.4), 0., speed)
-    assert target == FordPath(True, .4, 0., 0., 0.)
+    assert encode_model_action(straight(.4), 0., speed) == FordPath(True, .4, 0., 0., 0.)
   for sign in (-1, 1):
     target = encode_model_action(circle(sign*.01), sign*.01, 20.)
-    assert target.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=1e-6)
-    assert target.path_angle == pytest.approx(sign*.2)  # No 10 m cap at highway speed.
+    assert target.path_offset == pytest.approx(sign*(1-math.cos(.2))/.01)
+    assert target.path_angle == pytest.approx(sign*.2)
 
 
 def test_two_actuator_positions_are_sufficient_for_every_next_output():
@@ -54,31 +55,26 @@ def test_two_actuator_positions_are_sufficient_for_every_next_output():
     assert controller.update(model, desired, **kwargs) == copied.update(model, desired, **kwargs)
 
 
-def test_held_turn_releases_with_geometric_countersteering_and_no_retained_bias():
+def test_held_turn_releases_using_new_model_geometry_without_retained_bias():
   for sign in (-1., 1.):
     controller = ModelActionController()
     for _ in range(400):
       out = controller.update(circle(sign*.01), sign*.01, speed=20., dt=.01)
     assert out.path_angle == pytest.approx(sign*.2)
     previous = np.array([controller.c0, controller.c1])
-    opposed = False
-    for desired in sign*np.linspace(.01, 0., 101):
-      out = controller.update(straight(), desired, speed=20., dt=.01)
-      rotation = desired*3.
-      predicted = (1.-math.cos(rotation))/desired-10.*math.sin(rotation) if desired else 0.
-      expected = previous+np.clip([predicted, 20.*desired]-previous, [-.04, -.005], [.04, .005])
+    for _ in range(100):
+      out = controller.update(straight(), sign*.01, speed=20., dt=.01)
+      expected = previous+np.clip(-previous, [-.04, -.005], [.04, .005])
       values = np.array([controller.c0, controller.c1])
       np.testing.assert_allclose(values, expected, atol=1e-10)
-      opposed |= sign*out.path_offset < 0.
       previous = values
-    assert opposed
     assert out == FordPath(True, 0., 0., 0., 0.)
 
 
 def test_current_model_replacement_leaves_only_independent_actuator_slew():
   controller = ModelActionController()
   for _ in range(150):
-    controller.update(straight(1.), .04, speed=20., dt=.01)
+    controller.update(straight(1.-20*math.sin(.4), .4), .04, speed=20., dt=.01)
   for _ in range(25):
     out = controller.update(straight(), 0., speed=20., dt=.01)
   assert out.path_offset == pytest.approx(0.)
@@ -113,7 +109,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*.8), sign*.1, speed=30., dt=.01)
     fields = np.array([out.path_offset, out.path_angle])
     assert (abs(fields) <= [5.1100001, .5000001]).all()
     assert (abs(fields-previous) <= [.0500001, .0055001]).all()
@@ -132,7 +128,7 @@ def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2(
 
 def test_short_path_holds_available_endpoint_without_extrapolation():
   model = make_model([0., 1.], [0., .1], [0., 0.])
-  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, .2, 0., 0.)
+  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, 0., 0., 0.)
 
 
 def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
@@ -181,14 +177,13 @@ def test_domain_and_elapsed_time_boundaries(field, value, valid):
   assert ModelActionController().update(straight(.4), **kwargs).valid == valid
 
 
-def test_arc_station_not_forward_x_or_model_heading_determines_offset():
+def test_arc_station_floor_not_forward_x_determines_offset():
   x = np.array([0., 6., 12.])
   y = .4+x*.75
-  target = encode_model_action(make_model(x, y, [2., -2., 1.]), -.01, 20.)
-  # Arc length is 1.25*x. At station 10, x=8 and y=6.4; use the full predicted pose.
-  expected = math.cos(-.03)*6.4-math.sin(-.03)*8.+(1.-math.cos(-.03))/-.01
-  assert target.path_offset == pytest.approx(expected)
-  assert target.path_angle == pytest.approx(-.2)
+  target = encode_model_action(make_model(x, y, [.4, .4, .4]), -.01, 20.)
+  # At one second arc station is 5 m; the 7 m minimum gives x=5.6, y=4.6.
+  assert target.path_offset == pytest.approx(4.6)
+  assert target.path_angle == pytest.approx(.4)
 
 
 def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
@@ -196,4 +191,4 @@ def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
   assert encode_model_action(model, 0., 20.) == FordPath(True, .4, 0., 0., 0.)
   out = ModelActionController().update(model, .01, speed=20., dt=.002)
   assert out.path_offset == pytest.approx(.01)
-  assert out.path_angle == pytest.approx(.001)
+  assert out.path_angle == 0.

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -16,18 +16,17 @@ from opendbc.can import CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.ford.carcontroller import CarController
 from opendbc.car.ford.values import FordFlags
-from openpilot.cereal import custom, log
+from openpilot.cereal import custom
 from openpilot.selfdrive.car.helpers import convert_carControlSP
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
 from openpilot.selfdrive.controls.lib.ford_path import FordPath
 from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
 from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
-from openpilot.selfdrive.locationd.helpers import Pose, PoseCalibrator
 
 
 def update(controller, now=1., **overrides):
-  kwargs = {'model': straight(.4), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now,
+  kwargs = {'model': straight(.4, .1), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now,
                 'model_time': now, 'measurement_time': now, 'reference_time': now, 'active': True}
   kwargs.update(overrides)
   return controller.update(**kwargs)
@@ -82,7 +81,7 @@ def test_repeated_measurements_do_not_freeze_slew_or_cache_invalid_model_geometr
   controller = FordModelActionController()
   for i in range(10):
     result = update(controller, 1.+i*.01, measurement_time=1., model_time=1., reference_time=1.)
-  assert result.path_offset == pytest.approx(.14)  # Full preview accounts for selected turning toward the offset path.
+  assert result.path_offset == pytest.approx(.4)
   assert result.path_angle == pytest.approx(.05)
   broken = straight(.4)
   broken.position.y[5] = math.nan
@@ -96,7 +95,7 @@ def test_yaw_offset_does_not_change_the_base():
   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)
+  assert outputs[0].path_angle == pytest.approx(.1)
 
 
 def test_reference_source_can_change_to_an_older_but_fresh_publication():
@@ -105,7 +104,7 @@ def test_reference_source_can_change_to_an_older_but_fresh_publication():
   assert update(controller, 1.01, reference_time=.98).valid
 
 
-def test_release_keeps_current_geometry_and_may_grow_c0_while_c1_decreases():
+def test_current_model_geometry_controls_both_fields_independently_of_scalar_action():
   for sign in (-1., 1.):
     controller = FordModelActionController()
     for i in range(100):
@@ -113,11 +112,10 @@ def test_release_keeps_current_geometry_and_may_grow_c0_while_c1_decreases():
     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)
+    assert abs(after.path_angle) > abs(before.path_angle)
     for i in range(100):
       released = update(controller, 3.+i*.01, model=circle(sign*.02), desired_curvature=0.)
-    assert abs(released.path_offset) > abs(after.path_offset)  # Less expected turning raises the future-frame offset.
-    assert released.path_angle == pytest.approx(0.)
+    assert released == after  # A scalar reference change does not fabricate a different model pose.
 
 
 def _method(filename, class_name, method):
@@ -168,21 +166,17 @@ class Subscriptions:
 @pytest.mark.parametrize('maneuver', [False, True])
 @pytest.mark.parametrize('host_yaw', [.0072, .3])
 @pytest.mark.parametrize('initial_curvature', [0., .005])
-@pytest.mark.parametrize('calibrated_yaw', [None, -.05, .05])
-def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature, calibrated_yaw):
+def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature):
   call, publication = pipeline
   sm = Subscriptions(maneuver)
   controls = startup()
   controller = controls.ford_path_controller
-  if calibrated_yaw is not None:
-    controls.pose_calibrator.calib_valid = True
-    controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(z=calibrated_yaw))
   initial_curvature *= -1 if maneuver else 1
   controls.sm, controls.desired_curvature, controls.curvature = sm, initial_curvature, 0.
   if initial_curvature:
     # Start at the old target so startup slew cannot hide prediction on the real call path.
-    controller.core.c0, controller.core.c1 = .4, 20.*initial_curvature
-  model = straight(.4)
+    controller.core.c0, controller.core.c1 = .4, .1
+  model = straight(.4, .1)
   model.action = SimpleNamespace(desiredCurvature=.1)
   cc = structs.CarControl(latActive=True)
   cs = SimpleNamespace(vEgo=20., yawRate=-host_yaw, canValid=True, steeringPressed=False, steeringTorque=0.)
@@ -192,17 +186,16 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
   exec(call, environment)
   expected_curvature = initial_curvature+(-1 if maneuver else 1)*.000125
   assert controls.desired_curvature == pytest.approx(expected_curvature)
-  assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
-  expected_offset = .04
-  if initial_curvature:
-    grows = maneuver if calibrated_yaw is None else calibrated_yaw < 0.
-    expected_offset = .44 if grows else .36
-  assert controls.ford_path.path_offset == pytest.approx(expected_offset)
-  assert controller.diagnostics['yaw_rate'] == host_yaw
-  assert controller.diagnostics['pose_source'] == ('requested' if calibrated_yaw is None else 'measured')
-  assert controller.diagnostics['pose_yaw_rate'] == calibrated_yaw
-  assert cc.latActive and cc.actuators.curvature == 0.
-  assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
+  if maneuver:
+    assert controls.ford_path == FordPath() and not cc.latActive
+    assert controller.diagnostics['status'] == 'unsupported_reference'
+  else:
+    assert controls.ford_path.path_offset == pytest.approx(.44 if initial_curvature else .04)
+    assert controls.ford_path.path_angle == pytest.approx(.1 if initial_curvature else .005)
+    assert controller.diagnostics['yaw_rate'] == host_yaw
+    assert controller.diagnostics['pose_source'] == 'model'
+    assert cc.latActive and cc.actuators.curvature == 0.
+    assert controller.diagnostics['reference_age'] == pytest.approx(.02)
 
   cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
   downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, controls.CP_SP)
@@ -226,7 +219,7 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
     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)
+    assert wire['LatCtl_D2_Rq'] == (0 if fail or maneuver else 2)
 
 
 @pytest.mark.parametrize('maneuver', [False, True])
@@ -246,108 +239,31 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
   assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)
 
 
-@pytest.mark.parametrize('fault', ['missing_pose', 'uncalibrated', 'motion_service', 'calibration_service', 'yaw_invalid',
-                                 'sensors_invalid', 'inputs_invalid', 'stale_calibration', 'future_calibration',
-                                 'stale_motion', 'future_motion', 'stale_estimate', 'future_estimate', 'missing_estimate',
-                                 'nan_yaw', 'infinite_yaw', 'yaw_range'])
-def test_actual_controlsd_unhealthy_pose_retains_requested_pose_and_engagement(pipeline, fault):
+@pytest.mark.parametrize('service', ['deviceMotion', 'extrinsicsCalibration'])
+def test_optional_pose_services_do_not_modify_model_point_requests(pipeline, service):
   sm = Subscriptions(False)
+  sm.failed.add(service)
   controls = startup()
   controls.sm, controls.desired_curvature, controls.curvature = sm, .01, 0.
-  controls.pose_calibrator.calib_valid = True
-  controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(z=.05))
-  motion = sm.messages['deviceMotion']
-  if fault == 'missing_pose':
-    controls.calibrated_pose = None
-  elif fault == 'uncalibrated':
-    controls.pose_calibrator.calib_valid = False
-  elif fault == 'motion_service':
-    sm.failed.add('deviceMotion')
-  elif fault == 'calibration_service':
-    sm.failed.add('extrinsicsCalibration')
-  elif fault == 'yaw_invalid':
-    motion.angularVelocityDevice.valid = False
-  elif fault == 'sensors_invalid':
-    motion.sensorsOK = False
-  elif fault == 'inputs_invalid':
-    motion.inputsOK = False
-  elif fault == 'stale_calibration':
-    sm.logMonoTime['extrinsicsCalibration'] = -1_000_000
-  elif fault == 'future_calibration':
-    sm.logMonoTime['extrinsicsCalibration'] = 1_006_000_000
-  elif fault == 'stale_motion':
-    sm.logMonoTime['deviceMotion'] = 849_000_000
-  elif fault == 'future_motion':
-    sm.logMonoTime['deviceMotion'] = 1_006_000_000
-  elif fault == 'stale_estimate':
-    motion.timestamp = 849_000_000
-  elif fault == 'future_estimate':
-    motion.timestamp = 1_006_000_000
-  elif fault == 'missing_estimate':
-    motion.timestamp = 0
-  elif fault == 'nan_yaw':
-    controls.calibrated_pose.angular_velocity.z = math.nan
-  elif fault == 'infinite_yaw':
-    controls.calibrated_pose.angular_velocity.z = math.inf
-  elif fault == 'yaw_range':
-    controls.calibrated_pose.angular_velocity.z = 3.01
-  controller = controls.ford_path_controller
-  controller.core.c0, controller.core.c1 = .4, .2
+  controls.calibrated_pose = None
+  controls.ford_path_controller.core.c0, controls.ford_path_controller.core.c1 = .4, .1
   cc = structs.CarControl(latActive=True)
   cs = SimpleNamespace(vEgo=20., yawRate=-.3, canValid=True, steeringPressed=False, steeringTorque=0.)
-  model = straight(.4)
+  model = straight(.4, .1)
   model.action = SimpleNamespace(desiredCurvature=.01)
-  exec(pipeline[0], {'FordModelActionController': FordModelActionController, '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(pipeline[0], {'FordModelActionController': FordModelActionController, '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 cc.latActive and controls.ford_path.valid
-  assert controls.ford_path.path_offset == pytest.approx(.36)
-  assert controls.ford_path.path_angle == pytest.approx(.195)  # Upstream acceleration limiting still applies.
-  assert controller.diagnostics['pose_source'] == 'requested'
-  assert controller.diagnostics['pose_yaw_rate'] is None
-  json.dumps(controller.diagnostics, allow_nan=False)
+  assert controls.ford_path.path_offset == pytest.approx(.44)
+  assert controls.ford_path.path_angle == pytest.approx(.1)
+  assert controls.ford_path_controller.diagnostics['pose_source'] == 'model'
 
 
-@pytest.mark.parametrize('pose_time', [None, math.nan, math.inf, -math.inf, 'bad', .849, 1.006])
-def test_invalid_pose_timestamp_uses_fallback_without_a_reset(pose_time):
+def test_maneuver_reference_clears_existing_model_point_requests():
   controller = FordModelActionController()
-  controller.core.c0, controller.core.c1 = .4, .2
-  out = update(controller, pose_yaw_rate=.05, pose_time=pose_time, pose_valid=True)
-  assert out.path_offset == pytest.approx(.36)
-  assert out.path_angle == pytest.approx(.2)
-  assert controller.diagnostics['pose_source'] == 'requested'
-  json.dumps(controller.diagnostics, allow_nan=False)
-
-
-def test_pose_fallback_and_recovery_preserve_slew_states():
-  controller = FordModelActionController()
-  controller.core.c0, controller.core.c1 = .4, .2
-  states = []
-  for i, healthy in enumerate((True, False, True)):
-    now = 1.+i*.01
-    out = update(controller, now, pose_yaw_rate=-.1, pose_time=now, pose_valid=healthy)
-    states.append(controller.core.c0)
-    assert out.path_angle == pytest.approx(.2)
-    assert controller.diagnostics['pose_source'] == ('measured' if healthy else 'requested')
-  assert states == pytest.approx([.44, .4, .44])
-
-
-def test_calibration_only_update_refreshes_pose_before_ford_uses_it():
-  controls = startup()
-  controls.pose_calibrator = PoseCalibrator()
-  motion = log.DeviceMotion.new_message()
-  motion.angularVelocityDevice.x = .5
-  motion.angularVelocityDevice.z = .1
-  controls.calibrated_pose = controls.pose_calibrator.build_calibrated_pose(Pose.from_device_motion(motion))
-  sm = Subscriptions(False)
-  sm.updated = {'extrinsicsCalibration': True, 'deviceMotion': False}
-  sm.update = lambda timeout: None
-  sm.messages.update(deviceMotion=motion, extrinsicsCalibration=log.ExtrinsicsCalibration.new_message(
-    rpyCalib=[0., .2, 0.], calStatus='calibrated'))
-  controls.sm = sm
-  controls_file = Path(__file__).resolve().parents[3]/'selfdrive/controls/controlsd.py'
-  method = _method(controls_file, 'Controls', 'update')
-  environment = {'Pose': Pose}
-  exec(compile(ast.Module(body=[method], type_ignores=[]), str(controls_file), 'exec'), environment)
-  environment['update'](controls)
-  assert controls.pose_calibrator.calib_valid
-  assert controls.calibrated_pose.angular_velocity.z == pytest.approx(math.sin(.2)*.5+math.cos(.2)*.1)
+  update(controller)
+  assert update(controller, 1.01, reference_source='lateralManeuverPlan') == FordPath()
+  assert controller.diagnostics['status'] == 'unsupported_reference'
+  assert (controller.core.c0, controller.core.c1) == (0., 0.)
+  assert update(controller, 1.02).path_angle == pytest.approx(.005)

openpilot/selfdrive/controls/tests/test_ford_model_action_cadence.py

@@ -88,7 +88,7 @@ def test_core_slew_per_second_and_actual_panda_acceptance():
   safety.set_controls_allowed(True)
   frames = []
   for frame in range(100):
-    command = core.update(straight(10.), .1, speed=20., dt=.01, pose_yaw_rate=0.)
+    command = core.update(straight(10., 1.), .1, speed=20., dt=.01)
     assert core.c0 == pytest.approx((frame + 1) * .04)
     assert core.c1 == pytest.approx((frame + 1) * .005)
     sp.fordLateralPath.valid = command.valid

openpilot/selfdrive/controls/tests/test_ford_model_action_measured_pose.py

@@ -1,89 +0,0 @@
-import math
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
-from openpilot.selfdrive.controls.lib.ford_path import _model_path
-
-
-def circle(k, length=70.0):
-  s = np.linspace(0.0, length, 1401)
-  if k:
-    x, y = np.sin(k * s) / k, (1 - np.cos(k * s)) / k
-  else:
-    x, y = s, np.zeros_like(s)
-  return SimpleNamespace(position=SimpleNamespace(x=x, y=y), orientation=SimpleNamespace(z=k * s))
-
-
-@pytest.mark.parametrize('speed', [0.3, 4.0, 15.0, 30.0, 55.0])
-@pytest.mark.parametrize('curvature', [-0.08, -0.005, 0.0, 0.005, 0.08])
-def test_measured_pose_equals_requested_pose_when_tracking(speed, curvature):
-  m = circle(curvature)
-  # Values over the measurement's health range fall back to the same baseline.
-  assert encode_model_action(m, curvature, speed, pose_yaw_rate=speed * curvature) == encode_model_action(m, curvature, speed)
-
-
-@pytest.mark.parametrize('measured_curvature', [-0.02, 0.0, 0.004, 0.008, 0.02])
-@pytest.mark.parametrize('speed', [4.0, 15.0, 30.0])
-def test_offset_is_exact_rigid_transform_using_measured_pose(speed, measured_curvature):
-  m = circle(0.008)
-  out = encode_model_action(m, 0.008, speed, pose_yaw_rate=speed * measured_curvature)
-  s, x, y, _ = _model_path(m)
-  d = min(speed * 0.15, max(0.0, s[-1] - 7.0))
-  angle = measured_curvature * d
-  vehicle_x = math.sin(angle) / measured_curvature if measured_curvature else d
-  vehicle_y = (1 - math.cos(angle)) / measured_curvature if measured_curvature else 0.0
-  target_x = np.interp(7.0 + d, s, x)
-  target_y = np.interp(7.0 + d, s, y)
-  expected = -math.sin(angle) * (target_x - vehicle_x) + math.cos(angle) * (target_y - vehicle_y)
-  assert out.path_offset == pytest.approx(expected, abs=1e-12)
-  assert out.path_angle == encode_model_action(m, 0.008, speed).path_angle
-
-
-@pytest.mark.parametrize('invalid', [None, float('nan'), float('inf'), -float('inf'), 3.01, -3.01])
-def test_unavailable_measurement_retains_baseline_target(invalid):
-  m = circle(0.01)
-  assert encode_model_action(m, 0.01, 15.0, pose_yaw_rate=invalid) == encode_model_action(m, 0.01, 15.0)
-
-
-def test_undertracking_adds_offset_and_overtracking_releases_offset():
-  m = circle(0.01)
-  base = encode_model_action(m, 0.01, 15.0)
-  under = encode_model_action(m, 0.01, 15.0, pose_yaw_rate=0.10)
-  over = encode_model_action(m, 0.01, 15.0, pose_yaw_rate=0.20)
-  assert under.path_offset > base.path_offset > over.path_offset
-  assert under.path_angle == base.path_angle == over.path_angle
-
-
-def test_two_state_core_keeps_caps_slew_and_reset():
-  c = ModelActionController()
-  old = (0.0, 0.0)
-  for _ in range(200):
-    out = c.update(circle(0.2), 0.2, speed=20.0, dt=0.01, pose_yaw_rate=0.1)
-    assert out.valid and out.curvature == out.curvature_rate == 0.0
-    assert abs(c.c0 - old[0]) <= 0.0400000001
-    assert abs(c.c1 - old[1]) <= 0.0050000001
-    assert abs(out.path_offset) <= 5.11 and abs(out.path_angle) <= 0.5
-    old = c.c0, c.c1
-  assert c.__slots__ == ('c0', 'c1')
-  assert not c.update(circle(0.01), 0.01, speed=15.0, dt=0.01, active=False, pose_yaw_rate=0.1).valid
-  assert (c.c0, c.c1) == (0.0, 0.0)
-
-
-def test_measurement_fallback_and_recovery_keep_slew_and_selected_c1():
-  c = ModelActionController()
-  original = ModelActionController()
-  old = (0.0, 0.0)
-  for index in range(180):
-    curvature = 0.02 if index < 90 else -0.015
-    measured = 0.05 if index < 50 else None if index < 100 else -0.03
-    model = circle(curvature)
-    out = c.update(model, curvature, speed=15.0, dt=0.01, pose_yaw_rate=measured)
-    ref = original.update(model, curvature, speed=15.0, dt=0.01)
-    assert out.path_angle == ref.path_angle
-    assert abs(c.c0 - old[0]) <= 0.0400000001
-    assert abs(c.c1 - old[1]) <= 0.0050000001
-    assert out.curvature == out.curvature_rate == 0.0
-    old = c.c0, c.c1

openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py

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

openpilot/selfdrive/controls/tests/test_ford_model_points.py

@@ -0,0 +1,54 @@
+"""Model geometry contract, independent of any PSCM response model."""
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import encode_model_action
+
+
+def model_points(t, x, y, heading):
+  return SimpleNamespace(position=SimpleNamespace(t=t, x=x, y=y), orientation=SimpleNamespace(t=t, z=heading))
+
+
+def test_both_fields_sample_the_same_model_time_without_constant_speed_assumption():
+  # Accelerating plan: one second is 12 m along this straight inclined path.
+  s = np.array([0., 4., 12., 30.])
+  heading = .1
+  m = model_points([0., .5, 1., 2.], s*np.cos(heading), .3+s*np.sin(heading), np.full(4, heading))
+  out = encode_model_action(m, -.01, 20.)
+  assert out.path_offset == pytest.approx(.3+12*np.sin(heading))
+  assert out.path_angle == pytest.approx(.1)
+  assert out.curvature == out.curvature_rate == 0.
+  assert encode_model_action(m, .01, 30.) == out
+
+
+def test_low_speed_floor_uses_one_shared_seven_metre_station():
+  m = model_points([0., 1., 2.], [0., 3., 9.], [0., 0., 0.], [0., .03, .09])
+  out = encode_model_action(m, .01, 3.)
+  assert out.path_offset == 0.
+  assert out.path_angle == pytest.approx(.07)
+
+
+def test_short_plan_holds_both_endpoint_values_without_extrapolation():
+  m = model_points([0., .5], [0., 2.], [0., .4], [0., .2])
+  out = encode_model_action(m, -.01, 20.)
+  assert out.path_offset == .4
+  assert out.path_angle == pytest.approx(.2)
+
+
+@pytest.mark.parametrize('times', [[], [0.], [0., 0., 1.], [0., 1., .5], [0., float('nan'), 1.], [.1, .5, 1.]])
+def test_invalid_model_clock_cannot_publish_an_active_path(times):
+  m = model_points(times, [0., 10., 20.], [0., .1, .4], [0., .02, .04])
+  assert not encode_model_action(m, .01, 20.).valid
+
+
+def test_orientation_and_position_must_describe_the_same_times():
+  m = model_points([0., .5, 1.], [0., 10., 20.], [0., .1, .4], [0., .02, .04])
+  m.orientation.t = [0., .6, 1.]
+  assert not encode_model_action(m, .01, 20.).valid
+
+
+def test_model_heading_unwraps_before_interpolation():
+  m = model_points([0., .5, 1.5], [0., 10., 30.], [0., 0., 0.], [3., 3.1, -3.1])
+  assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(np.pi)

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 measured vehicle motion to predict the nearby model path, plus a heading request based on selected planned curvature. If the motion measurement is unavailable, prediction uses the requested turn rate. The existing command limits and rate limits remain in effect. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "description": "Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning.",
+          "details": "Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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 measured vehicle motion to predict the nearby model path, plus a heading request based on selected planned curvature. If the motion measurement is unavailable, prediction uses the requested turn rate. The existing command limits and rate limits remain in effect. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    description: Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning.
+    details: Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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/stress_model_action.py

@@ -1,8 +1,8 @@
 """Deterministic numerical stress and exhaustive field-boundary CAN checks.
 
-Analytic straight/rotated paths and an independent rigid-motion transform
-check geometric prediction. The reference slew uses scalar arithmetic. Packing is checked against direct
-Float32/CAN packing of the continuous state, independently of host _packed.
+Analytic inclined lines check the shared model point; the slew oracle uses
+scalar arithmetic. Packing is checked against direct Float32/CAN packing of the
+continuous state, independently of host _packed.
 No synthetic plant is fitted or used to claim vehicle tracking performance.
 """
 import argparse
@@ -21,10 +21,10 @@ from opendbc.car.ford.fordcan import create_lat_ctl2_msg
 from tools.ford_pscm_lab.model_action_replay import PINNED_OPENDBC, WireCheck, verify_dependency, revision
 
 
-def line(offset, heading=0.):
+def line(offset, heading=0., speed=20.):
   s = np.linspace(0., 30., 33)
-  return SimpleNamespace(position=SimpleNamespace(x=s*math.cos(heading), y=offset+s*math.sin(heading)),
-                         orientation=SimpleNamespace(z=np.full_like(s, heading)))
+  return SimpleNamespace(position=SimpleNamespace(t=s/speed, x=s*math.cos(heading), y=offset+s*math.sin(heading)),
+                         orientation=SimpleNamespace(t=s/speed, z=np.full_like(s, heading)))
 
 
 def check_raw_packing(wire, controller, path):
@@ -62,7 +62,7 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
     valid = i % 211 != 0
     if i % 307 == 0:
       dt = .101
-    model, mirror = line(offset, heading), line(-offset, -heading)
+    model, mirror = line(offset, heading, speed), line(-offset, -heading, speed)
     if i % 401 == 0:
       model.position.y[4] = mirror.position.y[4] = math.nan
     out = controller.update(model, desired, speed=speed, dt=dt, yaw_rate=yaw, active=active, valid=valid)
@@ -71,16 +71,8 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
     assert out.valid == other.valid == expected_valid
     previous = np.array([c0, c1])
     if expected_valid:
-      distance, rotation = speed*.15, desired*speed*.15
-      # Independent full pose transform, including stable small-angle series.
-      if abs(rotation) < 1e-5:
-        ego_x = distance*(1.-rotation**2/6.+rotation**4/120.)
-        ego_y = distance*(rotation/2.-rotation**3/24.+rotation**5/720.)
-      else:
-        ego_x, ego_y = math.sin(rotation)/desired, (1.-math.cos(rotation))/desired
-      future_x, future_y = (7.+distance)*math.cos(heading), offset+(7.+distance)*math.sin(heading)
-      predicted = math.cos(rotation)*(future_y-ego_y)-math.sin(rotation)*(future_x-ego_x)
-      target = (max(-5.11, min(5.11, predicted)), max(-.5, min(.5, max(7., speed)*desired)))
+      station = min(30., max(7., speed))
+      target = (max(-5.11, min(5.11, offset+station*math.sin(heading))), max(-.5, min(.5, heading)))
       # Independent scalar slew oracle. Valid measured yaw cannot alter demand.
       c0 += max(-4.*dt, min(4.*dt, target[0]-c0))
       c1 += max(-.5*dt, min(.5*dt, target[1]-c1))
@@ -110,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-20.*math.sin(heading), heading), 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,
             'valid_yaw_does_not_affect_targets_checked': True,
-            'full_geometric_prediction_checked': True,
+            'shared_model_point_checked': True,
             'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(),
             'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.',
             'opendbc_import_head': revision(dependency),