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.
Ford: restore original selected-curvature v1 controller
2026-09-10 04:53:43 +08:00 · 2026-09-09 10:46:41 -04:00 · 2026-09-08 21:44:44 -04:00 · 2026-09-08 20:37:34 -04:00 · 2026-09-08 16:37:58 -04:00 · 2026-09-08 13:53:15 -04:00
72 changed files with 6339 additions and 27 deletions
@@ -9,6 +9,7 @@
 *.ttf filter=lfs diff=lfs merge=lfs -text
 *.otf filter=lfs diff=lfs merge=lfs -text
 *.wav filter=lfs diff=lfs merge=lfs -text
 openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
 openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
 openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text
@@ -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`.
@@ -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."
  ]
 }
@@ -0,0 +1,155 @@
 # Offline Ford selected-action candidate
 This document and `ford_model_action_validation.json` record the offline
 stage committed as `7ca3c6e3b`. The candidate is now available behind a
 separate default-off Sunnylink toggle; see
 [drive-test setup and validation](ford_model_action_drive_test.md).
 The counts, source hashes and selector status below describe that earlier
 stage. The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md)
 to this original mapping; the historical no-feedback description below is
 not the current controller specification.
 The decision is `C0 = current model y(7 m)`,
 `C1 = max(7 m, speed × 1 s) × selected upstream-limited desiredCurvature`,
 with C2=C3=0. The 7 m station and one-second scale are engineering choices,
 not identified PSCM gains. `calibration_approved=false`.
 `openpilot/selfdrive/controls/lib/ford_model_action.py` contains the core
 and a separate adapter compatible with the existing controlsd call.
 At that stage, the production selector, v8 implementation, settings, opendbc
 submodule and Panda safety remained unchanged. Tests injected the adapter
 offline; there was no production setting. No hardware or CAN transmission
 occurs in the lab tools.
 ## Construction and integration
 Only the unquantized C0 and C1 slew positions persist in the core.
 Each field is clipped independently (±5.11 m / ±0.5 rad), slewed independently
 (4 m/s / 0.5 rad/s), then packed using the existing Float32/sign-negation
 rounding contract (0.01 m / 0.0005 rad). Heading overflow is not transferred
 to C0. No yaw integral, blend, additional curvature contribution, reference
 filter, turn modes, or 10 m C1 cap is introduced.
 The selected standalone implementation from worktree 3548 is the provenance
 for this law. Its two-state packer has been moved into the library core so
 the controller does not depend on experimental lab code. Invalid numeric
 types, overflowing arc geometry and malformed paths reset the core instead
 of throwing or retaining a command.
 Arc stations use cumulative model x/y distance, not forward x. As in the
 reviewed standalone core, a path ending before 7 m holds its available
 endpoint instead of extrapolating. This matters: route95 contains 44 active
 cycles with 5.45–6.94 m of path at 2.78–3.46 m/s. A tested strict 7 m
 coverage gate would have introduced disengagements and was removed. There
 is no speed-dependent C0 horizon beyond this existing endpoint behavior.
 The adapter retains the existing input age allowance (−5 to +150 ms),
 speed domain (0.3–55 m/s), yaw sanity bound (±3 rad/s), selected curvature
 sanity bound (±1/m), and control interval (2–100 ms). It rejects backward
 model/measurement timestamps and invalid services. Repeated timestamps may
 continue slew, but geometry is validated again on each tick. Disengagement,
 invalid inputs and timing faults clear all command and adapter timing state.
 The first valid tick after reset uses 10 ms, as v8 does.
 controlsd still owns reference selection, upstream curvature limiting,
 service health and engagement. Tests execute its actual source-selection
 and limiter code, its Ford call, Float32 publication in ControlsExt, conversion
 to CarControlSP, and the pinned Ford CarController's in-memory CAN builder.
 Both model-action and maneuver-planner selection are covered, including
 disabling latActive after invalid output. Only the test chooses the adapter.
 Yaw is not an input to the control law. The adapter checks it solely for the
 inherited invalid-input policy. Driver override and optional PSCM status
 do not modify the candidate base; existing engagement and downstream driver
 arbitration remain responsible for authorization, as with v8's base request.
 ## Offline evidence
 The checked-in `ford_model_action_validation.json` records the completed
 checks and source hashes. Full arrays and detailed reports are generated
 locally under `.cache/ford_model_action/`; original route files are read-only.
 Completed validation: **264 Ford tests and 150 subtests pass**, including
 120 new core/adapter/replay-validator cases. The candidate module has 100%
 statement and branch coverage (78 statements, 24 branches). Ruff and Ty pass.
 The 200,000-cycle numerical stress test also checks 200,000 mirrored core
 updates and 18,138 field-boundary cases. Across route and stress runs,
 485,238 Float32/CAN round trips pass. Eight deliberately injected faults
 (heading gain/cap, erased C0, wrong C0 slew, retained invalid state, stale
 model acceptance, model clock rollback and reversed C0 sign) are all caught
 by the tests. Mutation runs replace code only inside isolated Python
 processes; production source files are never modified by those probes.
 Independent Standards and Spec reviews reported zero findings. The full
 suite's Params setting test uses an existing local native library from
 worktree 3548 after checking relevant source files are byte-identical;
 its hash and provenance are in the manifest. That library is an ignored
 test dependency, not part of this change. This is the full relevant Ford
 suite, not the hardware-dependent test suite for every openpilot subsystem.
 The replay has two separate passes:
 * Core compatibility uses the archived eligibility mask and requires exact
  equality with the independently implemented `action_heading` commands.
 * Adapter reconstruction derives eligibility from recorded service streams
  independently of the archived output mask. It retains original timestamps,
  gaps and consumed model frames. Controls publication time proxies the
  unlogged computation clock, and complete SubMaster health is unavailable.
 All 54,738 route95 and 78,812 route90 core cycles match exactly, including
 37,614 and 73,055 active cycles. The adapter preserves those active counts.
 Its 59 / 19 changed commands arise solely from the fresh 10 ms engagement
 tick instead of the archived harness's preceding publication interval;
 the replay checks that attribution on every cycle. Maximum differences are
 0.01 m / 0.001 rad (95) and 0.02 m / 0.002 rad (90).
 Every core and adapter replay output is round-tripped through Float32 and
 the real CAN packer/parser, including zero C2/C3, signs, mode and counter.
 Continuous field slew and quantization allowance are checked separately
 from immediate invalid-command resets. The original driver-clean cohorts,
 speed strata and command RMS are reproduced without redoing the encoder search.
 The numerical stress harness uses analytic rotated paths, scalar slew
 arithmetic, mirrored requests, irregular intervals and invalid-input resets.
 It also sweeps every representable host field value and the Float32 values
 immediately below, at and above every half-quantum boundary. Direct CAN
 packing of the continuous state must agree with the host's quantized output.
 The unit tests cover releases, reversals, clipping, service freshness,
 clock resets, malformed inputs, endpoint fallback and actual integration.
 ## Limits of the result
 On turns at ≥15 m/s, candidate C0 RMS is 79%/81% below v8 on routes95/90,
 while C1 is 33%/41% higher. Those are command changes, not evidence of
 equivalent steering authority. The PSCM's independent C0/C1 response remains
 unknown. Replay cannot establish physical model following, strong turns,
 centering, overshoot, oscillation or closed-loop stability.
 The release probe is intentionally explicit: a model bend can increase
 while selected curvature decreases. At 20 m/s, one synthetic probe changes
 C0/C1 from 0.24 m / 0.10 rad to 0.49 m / 0.08 rad. Zero selected curvature
 sets the C1 target to zero but does not erase a nonzero current model C0.
 Removing a yaw-integral tail does not prove that physical overshoot is solved.
 No additional release policy or unsupported plant model is added to hide
 that uncertainty.
 ## Reproduce
 From this worktree, use the logged construction dependency explicitly:
 ```sh
 export PYTHONDONTWRITEBYTECODE=1
 export PYTHONPATH=.:/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo
 PY=/Users/ibpersonal/dev/sunnypilot/.venv/bin/python
 EVIDENCE=/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904
 $PY -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py
 $PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route95" --output .cache/ford_model_action/route95
 $PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route90" --output .cache/ford_model_action/route90
 $PY -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --output .cache/ford_model_action/stress.json
 ```
 The route replay refuses an opendbc revision other than
 `72a775d35e54c21ff5c5798acef22016eedcc0a7`. Stress defaults to this pin and
 also accepts an explicitly required commit with `--opendbc-revision` for
 deployment checks. A mismatch still fails. This historical pin reproduces
 logged construction; it does not change the merge's submodule pointer.
@@ -0,0 +1,53 @@
 # Ford selected-action drive-test branch
 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` using the device's
   normal branch-switch process and allow its build to finish.
 2. While offroad, open Sunnylink device settings → Vehicle → Ford and enable
   **Selected-Action Path Tracking (Experimental)** (`FordModelActionController`).
 3. Complete a real offroad-to-onroad cycle. Selection occurs when `controlsd`
   starts; a stored toggle change or disengagement alone cannot swap an active
   controller. Initial physical evaluation remains controlled testing.
 The startup event `Ford path controller selected` should report
 `FordModelActionController`. Periodic `Ford C2-free path tracking` events
 identify **`hypothesis=model-action-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 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
 `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.
 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.
 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
 checks. Pushing the branch does not install it on the device or change its
 stored toggle.
@@ -0,0 +1,145 @@
 {
  "date": "2026-09-07",
  "baseline_commit": "7ca3c6e3b3e659c6f446039501c5826bbd14092e",
  "branch": "codex/ford-model-action-drive-test",
  "scope": "Default-off Sunnylink selection and v8 retirement; offline validation only. No device installation or physical performance validation.",
  "calibration_approved": false,
  "production_selector_changed": true,
  "toggle": "FordModelActionController",
  "default_enabled": false,
  "v8_removed": true,
  "panda_safety_changed": false,
  "opendbc_submodule_changed": false,
  "deployment_opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
  "controller_size": {
    "total_lines": 145,
    "code_lines_excluding_blanks_comments_docstrings": 95,
    "core_persistent_values": 2,
    "adapter_timestamps": 3,
    "removed_v8_module_lines": 469
  },
  "tests": {
    "combined_ford_params_sunnylink_suite": "284 passed, 26 subtests passed in 2.63s",
    "suite_log_sha256": "2e223a507f0630481cf6f83b9f8893d226f3f4273a79a09fc35905aa875b1d2c",
    "coverage": {
      "covered_lines": 87,
      "num_statements": 87,
      "percent_covered": 100.0,
      "percent_covered_display": "100",
      "missing_lines": 0,
      "excluded_lines": 0,
      "percent_statements_covered": 100.0,
      "percent_statements_covered_display": "100",
      "num_branches": 26,
      "num_partial_branches": 0,
      "covered_branches": 26,
      "missing_branches": 0,
      "percent_branches_covered": 100.0,
      "percent_branches_covered_display": "100"
    },
    "ruff": "pass",
    "ty_controller_and_lab": "pass",
    "settings_compiler_check": "pass",
    "standards_review_remaining_findings": 0,
    "spec_review_remaining_findings": 0,
    "resolved_review_finding": "Updated YAML authoring source and regenerated settings JSON before final compiler/schema suite."
  },
  "routes": {
    "route95": {
      "cycles": 54738,
      "core_active_cycles": 37614,
      "core_exact_archived_match": true,
      "cohorts_reproduced": true,
      "adapter_active_cycles": 37614,
      "adapter_exact_match_with_fresh_engagement_dt": true,
      "adapter_validity_differs_from_archive_cycles": 0,
      "adapter_command_differs_from_archive_cycles": 59,
      "adapter_max_absolute_command_difference_c0_c1": [
        0.010000000000000675,
        0.0010000000000000009
      ],
      "field_slew_zero_c2_c3_pass": true,
      "float32_can_round_trips": 109476,
      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
      "report_sha256": "72fab710dc81c8c7d9b75d371b97fec32402d3b01fa05517dfa814d8daff3134"
    },
    "route90": {
      "cycles": 78812,
      "core_active_cycles": 73055,
      "core_exact_archived_match": true,
      "cohorts_reproduced": true,
      "adapter_active_cycles": 73055,
      "adapter_exact_match_with_fresh_engagement_dt": true,
      "adapter_validity_differs_from_archive_cycles": 0,
      "adapter_command_differs_from_archive_cycles": 19,
      "adapter_max_absolute_command_difference_c0_c1": [
        0.020000000000000462,
        0.0020000000000000018
      ],
      "field_slew_zero_c2_c3_pass": true,
      "float32_can_round_trips": 157624,
      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
      "report_sha256": "cc2224bae597a341697a7681560a077cb209d77e4d060c0850997691c57d32fb"
    }
  },
  "stress": {
    "seed": 20260907,
    "random_cycles": 200000,
    "mirrored_core_updates": 200000,
    "invalid_or_inactive_resets": 3537,
    "field_boundary_cases": 18138,
    "float32_can_round_trips": 218138,
    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
    "direct_raw_float32_packing_matches_host_output": true,
    "max_continuous_step_c0_c1": [
      0.40000000000000147,
      0.05000000000000002
    ],
    "calibration_approved": false,
    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
  },
  "total_float32_can_round_trips": 485238,
  "native_params": {
    "source": "Rebuilt locally from this branch with clang++ and generated Capnp headers; ignored test dependency, not committed binary.",
    "library_sha256": "270bf43241cf7c02cc432cf78ec9411a62d7653ca445695efe785ae82241aa09",
    "sources_sha256": {
      "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
      "openpilot/common/params.cc": "a5adacb1d47cb3bf6e0d87d44ce158b41982d7eaf2e8114e32c48d3a6631304c",
      "openpilot/common/params.h": "ed03d137e126ecd6f1608016020af18c0339fb987e27d0a2aa6830bba396970c",
      "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
      "openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
      "openpilot/common/swaglog.cc": "9c2f88a2f1c3c4253b73defb264cc367a13ade23e02928e1d469b5c5833df176"
    }
  },
  "test_dependency_notes": {
    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
    "pyyaml": "6.0.3 from local uv cache",
    "jsonschema": "Local cached package appended after venv to run schema validator without skips",
    "hardware_build_and_device_boot": "not performed"
  },
  "source_sha256": {
    "docs/ford_model_action_candidate.md": "c968132348d20891a9396f6e69db1315d570505796e19e09ffbb2da748e6e687",
    "docs/ford_virtual_angle_experiment.md": "da6322f3c3d2d81463e44c50cc6cad1a962f97008ff9425c314da333ebe47a87",
    "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
    "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
    "openpilot/common/tests/test_params.py": "557a1f616af5fd9f5e623fbee0fd6f44cb059a30c290c68ce2b57e9bcceed081",
    "openpilot/selfdrive/controls/controlsd.py": "102b383e5beff43b8dd7c219178bef62e8a4b54443ebe682694606862bcd4e7f",
    "openpilot/selfdrive/controls/lib/ford_model_action.py": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "1c9448d88d8021e5d34a5dccd14a17c6c1bc64b5531342bfc6d15574d9d3e710",
    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "2c5b14f814e84e59d749f61a43ef1dcfe06253f6e185443fa123c37525ac8466",
    "openpilot/sunnypilot/sunnylink/settings_ui.json": "9974df3ac4cc58ae78d47848cd18ef4aca1bcbb00edb257f28b4220d92890528",
    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "7a3fa18e562d5a03a5b85c72ee3f3ebeb836c497285dcd9aaad24f8fb4dd6942",
    "openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py": "3db566612381fd87d3655a3ccff470be7da998c4ea7f6365f53e58cdb9c0ffb7",
    "tools/ford_pscm_lab/model_action_replay.py": "827a6dc488d554bdf6e87438c6a2a985b3195bf6d01ab09002bfd6049d22a868",
    "tools/ford_pscm_lab/stress_model_action.py": "2d5c72cc4b8ae214f2f5a19a050fe138f5c2ab0294d92d24e2481af5f8185613",
    "tools/ford_pscm_lab/test_model_action_replay.py": "ebf6bcd9260745100311521f8e11e85b7aebdd5561ab0876bfc2e802429d6896",
    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
    "docs/ford_model_action_drive_test.md": "d825b177cd099efd797fe89b7041695d6d41e4b9e8bba6bcaeb32aece164262b"
  },
  "deployment_target": {
    "repository": "sunnypilot/sunnypilot",
    "branch": "hiimisaac-dev",
    "validated_code_commit": "ea1ed70c718d32539ef6b9a89b89c0e297c92e06"
  }
 }
@@ -0,0 +1,220 @@
 {
  "date": "2026-09-07",
  "baseline_commit": "c4b3c55c826fca1ce09618e418e95f0a24478d96",
  "calibration_approved": false,
  "production_selector_changed": false,
  "vehicle_settings_changed": false,
  "panda_safety_changed": false,
  "scope": "Offline command construction, adapter integration, numerical fault probes and Ford regression tests. Physical response remains unvalidated.",
  "controller_size": {
    "total_lines": 131,
    "code_lines_excluding_blanks_comments_docstrings": 86,
    "core_persistent_values": 2,
    "adapter_timestamps": 3
  },
  "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
  "tests": {
    "ford_suite": "264 passed, 150 subtests passed in 14.29s",
    "new_core_adapter_tests": 107,
    "new_replay_validator_tests": 13,
    "controller_coverage": {
      "statements": 78,
      "missing_statements": 0,
      "branches": 24,
      "partial_branches": 0,
      "percent": 100
    },
    "ruff": "pass",
    "ty_controller_and_lab": "pass"
  },
  "routes": {
    "route95": {
      "cycles": 54738,
      "core_active_cycles": 37614,
      "core_exact_archived_match": true,
      "cohorts_reproduced": true,
      "adapter_active_cycles": 37614,
      "adapter_status_counts": {
        "inactive": 17124,
        "active": 37614
      },
      "adapter_exact_match_with_fresh_engagement_dt": true,
      "core_active_path_shorter_than_7m_cycles": 44,
      "adapter_validity_differs_from_archive_cycles": 0,
      "adapter_command_differs_from_archive_cycles": 59,
      "adapter_max_absolute_command_difference_c0_c1": [
        0.010000000000000675,
        0.0010000000000000009
      ],
      "field_slew_zero_c2_c3_pass": true,
      "float32_can_round_trips": 109476,
      "turn_speed_15_55": {
        "seconds": 33.332073582999925,
        "core_c0_c1_rms": [
          0.07257996778378971,
          0.03628926246224237
        ],
        "recorded_v8_c0_c1_rms": [
          0.3416081588451539,
          0.027322786376822172
        ],
        "adapter_eligible_seconds": 33.332073582999925,
        "adapter_c0_c1_rms": [
          0.07257996778378971,
          0.03628926246224237
        ]
      },
      "input_sha256": {
        "route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
        "encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
        "pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
      },
      "report_sha256": "cf4e3804f2ebdb0e50f3c49636bf60a30e3c1180411b582826a115970ab972fc"
    },
    "route90": {
      "cycles": 78812,
      "core_active_cycles": 73055,
      "core_exact_archived_match": true,
      "cohorts_reproduced": true,
      "adapter_active_cycles": 73055,
      "adapter_status_counts": {
        "inactive": 5757,
        "active": 73055
      },
      "adapter_exact_match_with_fresh_engagement_dt": true,
      "core_active_path_shorter_than_7m_cycles": 0,
      "adapter_validity_differs_from_archive_cycles": 0,
      "adapter_command_differs_from_archive_cycles": 19,
      "adapter_max_absolute_command_difference_c0_c1": [
        0.020000000000000462,
        0.0020000000000000018
      ],
      "field_slew_zero_c2_c3_pass": true,
      "float32_can_round_trips": 157624,
      "turn_speed_15_55": {
        "seconds": 47.53485040600012,
        "core_c0_c1_rms": [
          0.08686835454709245,
          0.043216347944464766
        ],
        "recorded_v8_c0_c1_rms": [
          0.4471065308273131,
          0.030663943784303503
        ],
        "adapter_eligible_seconds": 47.53485040600012,
        "adapter_c0_c1_rms": [
          0.08686835454709245,
          0.043216347944464766
        ]
      },
      "input_sha256": {
        "route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
        "encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
        "pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
      },
      "report_sha256": "b95247bdf6bbec16e5dc4781eaf7a678aca787418251cd161f6438e3173ad490"
    }
  },
  "stress": {
    "seed": 20260907,
    "random_cycles": 200000,
    "mirrored_core_updates": 200000,
    "invalid_or_inactive_resets": 3537,
    "field_boundary_cases": 18138,
    "float32_can_round_trips": 218138,
    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
    "direct_raw_float32_packing_matches_host_output": true,
    "max_continuous_step_c0_c1": [
      0.40000000000000147,
      0.05000000000000002
    ],
    "calibration_approved": false,
    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
    "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
  },
  "mutation_checks": {
    "mutations": [
      {
        "mutation": "halve_heading",
        "detected_by_tests": true,
        "failed_tests": 9
      },
      {
        "mutation": "cap_heading_preview",
        "detected_by_tests": true,
        "failed_tests": 9
      },
      {
        "mutation": "erase_centering",
        "detected_by_tests": true,
        "failed_tests": 15
      },
      {
        "mutation": "slow_c0_to_c1_rate",
        "detected_by_tests": true,
        "failed_tests": 11
      },
      {
        "mutation": "retain_invalid_state",
        "detected_by_tests": true,
        "failed_tests": 18
      },
      {
        "mutation": "ignore_model_freshness",
        "detected_by_tests": true,
        "failed_tests": 2
      },
      {
        "mutation": "ignore_model_clock_rollback",
        "detected_by_tests": true,
        "failed_tests": 1
      },
      {
        "mutation": "reverse_c0_wire_sign",
        "detected_by_tests": true,
        "failed_tests": 11
      }
    ],
    "all_detected": true
  },
  "native_test_dependency": {
    "scope": "Native dependency for inherited Params selection test only; copied existing local build, not rebuilt.",
    "source_library": "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/openpilot/common/libparams_c.dylib",
    "sha256": "ddde738108eab18b75f085c865c43aa197fd79a0384b390b1515ff90116e20e0",
    "byte_identical_source_files": [
      "openpilot/common/params.cc",
      "openpilot/common/params.h",
      "openpilot/common/params_c.cc",
      "openpilot/common/params.py",
      "openpilot/common/params_keys.h",
      "openpilot/common/queue.h",
      "openpilot/common/util.cc",
      "openpilot/common/util.h",
      "openpilot/common/hardware/hw.h"
    ]
  },
  "review": {
    "standards_findings": 0,
    "spec_findings": 0,
    "method": "Independent parallel read-only reviews; 120 focused tests independently passed."
  },
  "source_sha256": {
    "openpilot/selfdrive/controls/lib/ford_model_action.py": "cb6353f00f2f5c84df4e606c6b7e20650f8c1e908b9fd71890f72aa4a5e42592",
    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "c3b971622cc4041575aeec1826d45b76cebab9a2f77b2b9525c85d4184961295",
    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "5aaa29c1f11080b7df0165fd0202a29b065053809e5f9c4b9ed8abfcce47e41b",
    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
    "tools/ford_pscm_lab/model_action_replay.py": "c114c479bd22e4fc61a3e8d3ee7fae5d71d1d80ec4b952faad8f8f3692fb1508",
    "tools/ford_pscm_lab/stress_model_action.py": "a78a50eed1f801f3b096d694ab8c2fd70804b6c250465b4152f38d83770a982b",
    "tools/ford_pscm_lab/test_model_action_replay.py": "09d024c59d44b83ec081d6416d0f946a7719a73f22213af1f4ddc03dc4e6f4ac"
  },
  "artifacts": {
    "directory": ".cache/ford_model_action",
    "route_reports": [
      "route95/report.json",
      "route90/report.json"
    ],
    "stress_report": "stress.json",
    "mutation_report": "mutations/report.json",
    "test_log": "ford_suite.txt"
  }
 }
@@ -0,0 +1,327 @@
 # Ford C2-free model-pose tracking with measured feedback
 This experiment is retired. Its implementation, setting and dedicated tests
 were removed from the selected-action drive-test branch. For current setup,
 see [Ford selected-action drive testing](ford_model_action_drive_test.md).
 The material below is historical; it does not describe an available toggle.
 Hypothesis `model-pose-c0-c1-feedback-v8` retains the model-pose C0/C1 base
 and adds two guarded release policies. When measured turning exceeds both
 current and delayed requests, a separate output guard prevents same-direction
 C0/C1 growth, including while feedback history rebuilds after driver input.
 When turning instead falls below both requests and is no longer increasing,
 bounded C1 tracking can use remaining release-entry command headroom.
 Existing opposing-bias recovery still stops at zero bias. Geometry, blending,
 feedback gain, slew rates and field limits are unchanged; C2/C3 remain zero.
 This is an experimental outer controller around the multivariable PSCM.
 Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle
 servo. V8 has offline validation only. Command replay cannot establish the
 truck's response, closed-loop stability, or an overshoot improvement.
 ## Evidence and scope
 Route80 ran v3 and contains both sustained under-response and over-response.
 Representative eligible windows had median CAN response/request ratios of
 0.78, 1.77 and 0.69 with a declared 0.2-second comparison interval. These
 are descriptive tracking ratios, not identified controller gains.
 V4 replaced separate model-heading C1 with selected-curvature C1 and reduced
 heading demand in several large maneuvers. The user subsequently reported
 weak turning and steering repeatedly stopping near 85 degrees. Older logs
 contain larger wheel angles; the inspected host code has no fixed 85-degree
 wheel stop, although upstream curvature limits depend on speed.
 Route83 had the Sunnylink toggle on, but omitted EPS firmware responses.
 The former firmware gate selected the default `FordPathController`; replay
 reproduced its recorded C0/C1/C2 requests. Its favorable turns are evidence
 for the existing model-pose construction, not validation of v5 or v6.
 V6 reuses that construction while replacing its remaining C2 request with
 C0/C1 geometry. Removing C2 changes the request received by the PSCM, so
 matching large C0/C1 commands does not guarantee matching vehicle motion.
 Route8a ran v6 and was reported as the best drive. Route8e ran v7 throughout
 with the experiment enabled; it includes entry lag and excessive turning
 while requests release. Fixed-input v6/v7 replay produced identical commands
 in the main reversal and over-response examples, so the v7 recovery change
 does not directly explain their command behavior. In the over-response
 example, model C0/C1 grew while selected curvature fell and driver resets
 repeatedly removed feedback history. Another exit remained deficient after
 opposing bias reached zero. These observations motivate the v8 guards; they
 do not isolate an EPS transfer function or demonstrate the proposed response.
 ## Base request
 controlsd selects valid `lateralManeuverPlan.desiredCurvature`, otherwise
 `modelV2.action.desiredCurvature`, after the existing curvature limiter.
 This action already includes upstream delay handling; it receives no extra
 response advance here.
 The model contribution uses the existing allocator's raw forward pose and
 bounded short-pose correction. `_model_pose` advances 0.1 seconds, retains
 the model's remaining forward geometry, and separately corrects the short
 pose using measured curvature and its recent change. Its offset preview is
 up to 7 m and its heading preview is up to max(7 m, speed × 1 s), bounded by
 available path length. This raw pose is not passed through a second model
 filter. The filtered, ego-aligned reference remains available for comparison
 and the existing geometry-validity checks.
 ```text
 share(k) = clip((k - 0.006/m) / (0.012/m - 0.006/m), 0, 1)
 aligned = desired_curvature × model_forward_heading > 0
 model_share = min(share(abs(desired_curvature)), share(model_curvature_demand))
              if aligned, otherwise 0
 model_pair = existing_pose_encoder(model_pose, model_share, C2=0)
 remaining_curvature = desired_curvature × (1 - model_share)
 L0 = max(8 m, speed × 1 s)
 L1 = max(7 m, speed × 1 s)
 curvature_C0 = 0.5 × remaining_curvature × L0²
 curvature_C1 = remaining_curvature × L1
 C0_base = clip(model_pair.C0 + curvature_C0, ±5.11 m)
 C1_base = clip(model_pair.C1 + curvature_C1, ±0.5 rad)
 ```
 `model_curvature_demand` is the larger absolute curvature implied by the
 forward offset and heading previews. The share uses the existing allocator's
 0.006–0.012/m thresholds. Both model and action must request a substantial
 turn in the same direction before model pose supplies the full base.
 Small, flat, opposed or zero requests use the curvature contribution; zero
 action produces a zero base. Partial shares combine both contributions.
 The existing pose encoder retains its quantization and field-allocation rules.
 The residual-curvature lift is geometric, not a claim of EPS equivalence to C2.
 The inherited pose encoder allocates heading overflow using its asymmetric
 limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad
 heading bound. On clipped tails, this can leave mirrored C0 requests differing
 by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie
 below that heading cap; full model-base odd symmetry is not claimed.
 ## Measured feedback and limits
 ```text
 past_request = selected curvature held at or before (measurement_time - delay)
 yaw_error = measured_speed × past_request - measured_yaw_rate
 bias_trial = released_bias + feedback_gain × yaw_error × measurement_dt
 C1_unconstrained = clip(C1_base + accepted_bias, ±0.5 rad)
 C1_target = temporary_backoff_ceiling(C1_unconstrained) if backoff_active
            otherwise C1_unconstrained
 ```
 Measured yaw is negated Ford CAN yaw, matching the control sign convention.
 The historical request uses zero-order hold; it never interpolates toward a
 future publication. Nominal comparison delay is `CP.steerActuatorDelay`
 (0.2 seconds on the source vehicle). Feedback compares against selected
 curvature, not curvature inferred from the model-pose coefficients.
 | Quantity | Value |
 |---|---:|
 | C0 / C1 final bounds | ±5.11 m / ±0.5 rad |
 | Independent C0 / C1 slew | 4 m/s / 0.5 rad/s |
 | Feedback integration scale | 1.0 |
 | Feedback minimum speed | 2 m/s |
 | Maximum PSCM/core input age | 150 ms |
 | Allowed timestamp lead | 5 ms |
 | Release comparison tolerance | one C1 wire quantum, 0.0005 rad |
 The integration scale, preview distances and blend thresholds are effective
 gains; none establishes stability. No wheel-response gain is fitted.
 Zero yaw error retains acquired bias while an eligible turn continues.
 Host anti-windup admits reachable correction within the combined C1 field
 and slew limits. Feedback overflow is not transferred into C0.
 The release logic scales bias as the bounded base decreases and resets on
 zero/reversal. When delayed curvature still represents a stronger or opposing
 request, or PSCM reports LimitReached, new integration is normally frozen.
 One exception permits measured-error backoff: measured turning must exceed
 both the delayed and current selected yaw requests in the base's direction,
 and total heading must still have the base's sign. Exceeding only an older,
 smaller request during turn-in does not qualify. The accepted increment may
 only reduce that existing total toward zero; it cannot grow the request or
 carry it through zero. Existing host field and slew limits still apply.
 The existing release-recovery exception requires fresh valid PSCM status with
 limit below 2, retained bias opposing the base, and both current and delayed
 requests aligned with that base. Measured turning must be below both requests
 in their direction. It then uses the current yaw deficit × the existing
 feedback gain × measurement interval to unwind only the opposing bias toward
 zero. The increment is clipped so recovery cannot cross zero bias or create
 demand beyond the existing base. Common host anti-windup still limits what
 can be accepted. A separate release-tracking exception is described below;
 other constrained cases remain frozen. PSCM limit 2 never permits either
 request-increasing exception.
 The no-new-bias restriction applies to `release_recovery`. It does not apply
 to the separate bounded `release_tracking` branch. Once release ends,
 ordinary eligible integration can add correction beyond the base as before;
 its existing limits and guards are unchanged.
 `release_recovery` and `feedback_recovery_active=true` indicate that the
 recovery branch actually changed bias on that update. If host anti-windup
 blocks the entire increment, the status remains `host_limit` and the flag is
 false. Recovery is evaluated only on fresh measurements; the flag is false
 on repeated-measurement updates and after reset.
 Diagnostics distinguish `release_backoff` and `pscm_backoff`; a release takes
 precedence when both conditions apply. While `feedback_backoff_active` is
 true, total C1 is also capped at the preceding continuous heading request in
 the current request direction and at zero in the opposite direction. This
 ceiling affects the output only: it is not stored or projected into bias.
 The measured-error increment can still update bias under the normal limits,
 but a changing model base does not create persistent integral suppression.
 The ceiling persists between repeated measurements; C1 cannot grow or reverse
 while it applies. The next fresh measurement clears it unless backoff is
 again warranted. It does not cap C0, and normal feedback has its own rules
 outside backoff. Independent slew remains 0.5 rad/s for C1 and 4 m/s for C0.
 Backoff still compares against the delayed reference, so response lag remains.
 Reducing a request does not demonstrate that physical overshoot is resolved.
 ## V8 release guard and tracking
 `ReleaseGuard` retains selected-request history independently of feedback
 bias history. Driver-related feedback resets do not erase that reference,
 but the guard still requires current fresh valid PSCM status, no current
 driver override, and the existing input and speed eligibility. Invalid core
 input or disengagement resets its history with the controller.
 During release, measured yaw must exceed both the current and delay-matched
 requests in the requested turn direction. Only then does the guard cap
 same-direction C0/C1 growth at each preceding continuous request. Terms
 already reducing the turn, including an opposing C0 centering offset, remain
 available. The guard follows base allocation and C1 feedback, so changing
 model geometry cannot bypass it. Its ceilings affect outputs, never stored
 bias. No scalar-curvature cap replaces strong model geometry during turn-in
 or undertracking. Existing independent slew and field limits still apply.
 `release_tracking` addresses an eligible release deficit once bias is zero
 or already in the base's direction. Both current and delayed requests must
 align with that base, measured turning must be below both, and measured
 curvature must not be rising in the turn direction across the response
 interval by more than one C1 wire quantum after scaling by heading preview.
 Fresh valid PSCM status with limit below 2 is required. The current yaw deficit
 uses the existing integration gain and measurement interval;
 new C1 tracking increments are limited by command headroom captured at
 release entry, tapered with remaining desired curvature. The allowance is
 `max(0, entry_command_magnitude - abs(base)) × min(1, abs(desired) / entry_reference)`
 above the current base; any existing same-direction bias consumes it first.
 This limits new tracking integration, not the existing model base or bias.
 Only that additional allowance is tapered; strong model geometry remains
 available. A brief pause does not reacquire a higher entry
 ceiling; a full response interval without release ends the retained episode.
 Common host anti-windup, field and slew bounds still apply. Opposing bias
 continues through `release_recovery`, which stops at zero, before any separate
 tracking exception can be considered.
 Neither exception relaxes the PSCM LimitReached growth restriction. The
 reference delay and finite response time remain; these output policies are
 command-construction changes, not evidence of improved physical tracking.
 ## PSCM status and driver handling
 card publishes `Lane_Assist_Data3_FD1` in `carStateSP.fordPscmStatus`, retaining
 the original CAN receipt timestamp. Republishing carStateSP or receiving
 unrelated frames cannot refresh it. The opendbc submodule is unchanged.
 Feedback requires valid fresh status, InProgress lateral state (2), capability
 LimitedModeAvailable or ExtendedModeAvailable (1 or 2), and no denial.
 Missing, malformed, stale, backward-timestamped, denied or unavailable status
 clears feedback bias/history and disables the separate release guard,
 leaving the base subject to its core validity gates.
 LimitReached (2) permits only the bounded request-reducing backoff described
 above and otherwise freezes integration. LimitWithDriverActive (3) clears
 feedback. Backoff still requires fresh, valid, InProgress status with an
 available capability and no denial. These generic PSCM reports do not identify
 a specific torque or rate limit.
 `steeringPressed`, raw torque above the existing Ford driver allowance, or
 nonfinite torque clear feedback. Below 2 m/s feedback also clears. A fresh
 feedback reference interval is required after override; the independent
 release guard can use retained valid request history once its current gates
 are satisfied. Base requests retain normal
 PSCM driver arbitration while lateral control remains authorized; an unset
 override flag cannot rule out subthreshold driver influence.
 ## Gates and Sunnylink selection
 Core model/action/car-state freshness, finite-value, clock and speed checks
 remain in place. Invalid core inputs reset both commands and clear latActive.
 Raw model geometry is validated on every update, including repeated model
 timestamps; an invalid raw path cannot reuse the cached valid reference.
 Missing PSCM status disables feedback, not an otherwise valid base request.
 Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the
 `FordVirtualAngleController` key, default-off setting and offroad/onroad cycle
 requirement. Enabled selects v8 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
 regardless of missing or different EPS firmware-query results. Other platforms
 retain their existing controller. V8 takes priority over PSCM Coefficient
 Observer while selected; disabling and cycling offroad/onroad restores the
 previous selection. Controller selection does not force lateral engagement.
 The analyzed firmware is `RL38-14D003-AA`; removing the eligibility check
 is not validation of other firmware. No live device setting is changed.
 ## Diagnostics and verification
 The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v8.
 `model_offset_base` / `model_heading_base` report the already weighted and
 encoded model contribution; `curvature_offset_base` / `curvature_heading_base`
 report the residual-curvature contribution. `model_share` and `base_guard`
 identify model-pose, blended, curvature-only, opposed-model and zero-request
 cases. `heading_base` is the bounded pre-feedback C1. `offset_target` and
 `heading_target` are the final targets after the independent release guard;
 `offset_target_unguarded` and `heading_target_unguarded` retain the inputs to
 that guard. The latter C1 already includes its normal feedback/backoff policy.
 The event retains source timestamps, measured curvature/yaw, final commands,
 slew scales, feedback bias/status/history, raw torque and PSCM status/age.
 `feedback_backoff_active` records the persistent heading ceiling, including
 cycles whose feedback status is `no_new_measurement`.
 `release_guard_active` and `release_guard_reference_curvature` expose the
 independent C0/C1 guard and its retained delayed reference.
 `feedback_release_tracking_active`, `feedback_release_ceiling` and
 `feedback_curvature_delta` identify accepted release
 tracking, the total-heading threshold used to admit new bias, and the
 measured-curvature change across the response interval (1/m). The tracking
 flag is true only when the branch accepts a bias change on a new measurement;
 it is false on repeated measurements. The ceiling/trend fields can describe
 an evaluated condition even when no increment is accepted.
 `feedback_recovery_active` records an accepted recovery increment on this
 update only; it does not persist between measurements.
 `feedback_yaw_error` retains its delayed-reference meaning. Recovery instead
 uses current error, reconstructed from logged `desired_curvature`,
 synchronized car-state speed and `yaw_rate`; those two errors can differ.
 During backoff or the independent release guard, `heading_target` can be lower in the request direction than
 the bounded sum of `heading_base` and `heading_bias`, because the temporary
 ceiling is not part of the stored bias.
 `model_heading_target` remains a filtered comparison reference; it is not the
 weighted model contribution. `angleState.saturated` is not an EPS-limit signal.
 Validation must cover large recorded maneuvers, flat-model centering, both
 turn directions, model/action disagreement, share transitions, release and
 reversal, release/limit backoff without growth or zero crossing, status/driver
 resets, reference causality, bounds, slew and CAN packing with C2/C3 zero.
 Recovery checks cover both directions, stopping at zero bias, repeated
 measurements, current-and-delayed agreement, and rejection at PSCM limit 2.
 Old v3/v4 command-equality expectations do not define
 v8 success. Guard checks also cover driver reset/history rebuilding,
 same-direction growth, opposing coefficients, repeated measurements,
 undertracking and invalid-status inhibition. Tracking checks cover delayed
 curvature trends and tapered release-entry headroom. Historical v5–v7 replay
 results remain historical observations.
 The v8 recorded-input fixture contains 15,273 cycles with 4,879 selected
 evidence samples. Base allocation and output eligibility match v7. In the
 clean deficient exit, median absolute C1 changes from 0.0665 to 0.0845 rad
 while C0 stays unchanged. The growth guard also acts while feedback history
 rebuilds; the largest over-growth witness includes nearby driver input and
 is excluded from the strict autonomous tracking score. Both good comparison
 curves in that fixture retain their median requests, and the older large-turn
 fixtures retain their required command scale.
 On the earlier good drive, one comparison curve retains extra C1 after
 eligible release tracking: median magnitude changes from 0.121 to 0.128 rad.
 In its 103–110 s interval, tracking increments occur only while measured
 turning falls short, with a median current response/request ratio of 0.895.
 Acquired bias can persist after matching, as with ordinary integral feedback.
 This collateral command change remains a reason to compare new vehicle logs.
 Replay fixes recorded motion and planner outputs, so enabled vehicle logs
 are still required to assess tracking error, oscillation and interventions.
@@ -383,6 +383,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
  leadOne @2 :LeadData;
  leadTwo @3 :LeadData;
  intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
  fordLateralPath @5 :FordLateralPath;
  struct Param {
    key @0 :Text;
@@ -403,6 +404,14 @@ struct CarControlSP @0xa5cd762cd951a455 {
  }
 }
 struct FordLateralPath {
  pathOffset @0 :Float32;     # c0 [m]
  pathAngle @1 :Float32;      # c1 [rad]
  curvature @2 :Float32;      # c2 [1/m]
  curvatureRate @3 :Float32;  # c3 [1/m^2]
  valid @4 :Bool;
 }
 struct BackupManagerSP @0xf98d843bfd7004a3 {
  backupStatus @0 :Status;
  restoreStatus @1 :Status;
@@ -447,6 +456,16 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 struct CarStateSP @0xb86e6369214c01c8 {
  speedLimit @0 :Float32;
  fordPscmStatus @1 :FordPscmStatus;
  struct FordPscmStatus {
    valid @0 :Bool;
    canMonoTime @1 :UInt64;  # Last accepted Lane_Assist_Data3_FD1 CAN receipt, not carStateSP publication time.
    lateralState @2 :UInt8;  # LatCtlSte_D_Stat
    limit @3 :UInt8;  # LatCtlLim_D_Stat: generic lateral limit, not a torque/rate diagnosis.
    capability @4 :UInt8;  # LatCtlCpblty_D_Stat
    denied @5 :Bool;  # LaActDeny_B_Actl
  }
 }
 struct LiveMapDataSP @0xf416ec09499d9d19 {
@@ -470,7 +489,30 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
  }
 }
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
  enabled @0 :Bool;
  madsDistanceMeters @1 :Float64;
  fullAssistDistanceMeters @2 :Float64;
  event @3 :Event;
  struct Event {
    id @0 :UInt64;
    category @1 :Category;
    distanceMeters @2 :Float64;
    previousDistanceMeters @3 :Float64;
    unit @4 :Unit;
  }
  enum Category {
    none @0;
    mads @1;
    fullAssist @2;
  }
  enum Unit {
    imperial @0;
    metric @1;
  }
 }
 struct CustomReserved11 @0xc2243c65e0340384 {
@@ -2642,7 +2642,7 @@ struct Event {
    carStateSP @114 :Custom.CarStateSP;
    liveMapDataSP @115 :Custom.LiveMapDataSP;
    modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
    customReserved11 @137 :Custom.CustomReserved11;
    customReserved12 @138 :Custom.CustomReserved12;
    customReserved13 @139 :Custom.CustomReserved13;
@@ -90,6 +90,7 @@ _services: dict[str, tuple] = {
  "carParamsSP": (True, 0.02, 1),
  "carControlSP": (True, 100., 10),
  "carStateSP": (True, 100., 10),
  "assistedDrivingMilestoneState": (True, 10., 1),
  "liveMapDataSP": (True, 1., 1),
  "modelDataV2SP": (True, 20., None, QueueSize.BIG),
  "liveLocationKalman": (True, 20.),
@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
 }
 Params::~Params() {
  flushNonBlockingWrites();
 }
 void Params::flushNonBlockingWrites() {
  if (future.valid()) {
    future.wait();
  }
@@ -75,6 +75,7 @@ public:
    return put(key.c_str(), val ? "1" : "0", 1);
  }
  void putNonBlocking(const std::string &key, const std::string &val);
  void flushNonBlockingWrites();
  inline void putBoolNonBlocking(const std::string &key, bool val) {
    putNonBlocking(key, val ? "1" : "0");
  }
@@ -73,6 +73,7 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
 params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
 params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
 params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
 params_flush = _bind("params_flush", [ParamsHandle])
 params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
 params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
 params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
@@ -178,6 +179,10 @@ class Params:
  def put_bool(self, key, val, block=False):
    params_put_bool(self.p, self.check_key(key), val, block)
  def flush(self):
    """Wait for all prior nonblocking writes from this Params instance."""
    params_flush(self.p)
  def remove(self, key):
    params_remove(self.p, self.check_key(key))
@@ -133,6 +133,12 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
  });
 }
 void params_flush(ParamsHandle *handle) noexcept {
  translate_exceptions([&]() {
    handle->params.flushNonBlockingWrites();
  });
 }
 int params_remove(ParamsHandle *handle, const char *key) noexcept {
  return translate_exceptions(-1, [&]() {
    return handle->params.remove(key);
@@ -162,12 +168,15 @@ ParamsBuffer params_key_at(ParamsHandle *handle, size_t index) noexcept {
 size_t params_keys_by_flag(ParamsHandle *handle, uint32_t flag, ParamsBuffer *out, size_t out_size) noexcept {
  return translate_exceptions(size_t{0}, [&]() {
-    auto filtered = handle->params.allKeys(static_cast<ParamKeyFlag>(flag));
+    size_t count = 0;
-    size_t count = std::min(filtered.size(), out_size);
+    for (const auto &key : handle->keys) {
-    for (size_t i = 0; i < count; i++) {
+      if (flag == ALL || (handle->params.getKeyFlag(key) & flag)) {
-      out[i] = return_string(filtered[i]);
+        // Each buffer borrows a different string, stable for the handle's lifetime.
        if (count < out_size) out[count] = {key.data(), key.size()};
        ++count;
      }
    }
-    return filtered.size();
+    return count;
  });
 }
@@ -143,6 +143,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    // --- sunnypilot params --- //
    {"ApiCache_DriveStats", {PERSISTENT, JSON}},
    {"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
    {"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
    {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
    {"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}},  // seconds
@@ -163,6 +165,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
    {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
    {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
    {"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
    {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
    {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -172,7 +175,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
    {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
    {"LastGPSPositionLLK", {PERSISTENT, STRING}},
    {"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
    {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
    {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
    {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
    {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
@@ -232,6 +237,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"BackupManager_RestoreVersion", {PERSISTENT, STRING}},
    // sunnypilot car specific params
    {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
    {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
    assert q.get("CarParams") is None
    assert q.get("CarParams", True) == b"1"
  def test_flush_non_blocking_writes(self):
    self.params.put("DongleId", "first")
    self.params.put("DongleId", "last")
    self.params.flush()
    assert self.params.get("DongleId") == "last"
  def test_params_all_keys(self):
    keys = Params().all_keys()
@@ -126,6 +133,16 @@ class TestParams(OpenpilotTestCase):
    assert self.params.get("LiveParametersV2") is None
    assert self.params.get("LiveParametersV2", return_default=True) is None
  def test_filtered_keys_are_distinct_registered_strings(self):
    registered = set(self.params.all_keys())
    for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP, ParamKeyFlag.CLEAR_ON_MANAGER_START):
      filtered = self.params.all_keys(flag)
      assert len(filtered) > 1
      assert len(filtered) == len(set(filtered))
      assert set(filtered) <= registered
      assert all(key.decode('utf-8') for key in filtered)
      assert self.params.all_keys(flag) == filtered
  def test_params_get_type(self):
    # json
    self.params.put("ApiCache_FirehoseStats", {"a": 0}, block=True)
@@ -21,6 +21,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp
 from openpilot.selfdrive.car.ford_pscm_status import populate_ford_pscm_status
 from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
@@ -198,6 +199,7 @@ class Car:
    # Update carState from CAN
    CS, CS_SP = self.CI.update(can_list)
    CS_SP = convert_to_capnp(CS_SP)
    populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid)
    # Update radar tracks from CAN
    RD: structs.RadarDataT | None = self.RI.update(can_list)
@@ -0,0 +1,36 @@
 """Publish the Ford PSCM's actual CAN status without changing opendbc structs."""
 import math
 from opendbc.car import Bus
 from opendbc.car.ford.values import FordFlags
 MESSAGE = 'Lane_Assist_Data3_FD1'
 SIGNALS = ('LatCtlSte_D_Stat', 'LatCtlLim_D_Stat', 'LatCtlCpblty_D_Stat', 'LaActDeny_B_Actl')
 def populate_ford_pscm_status(CP, can_parsers, CS_SP, can_valid):
  if CP.brand != 'ford' or not CP.flags & FordFlags.CANFD:
    return
  status = CS_SP.init('fordPscmStatus')
  parser = can_parsers.get(Bus.pt)
  if parser is None:
    return
  values = parser.vl.get(MESSAGE, {})
  timestamps = parser.ts_nanos.get(MESSAGE, {})
  if any(signal not in values or signal not in timestamps for signal in SIGNALS):
    return
  received = timestamps[SIGNALS[0]]
  if received <= 0 or any(timestamps[signal] != received for signal in SIGNALS):
    return
  decoded = [values[signal] for signal in SIGNALS]
  if any(not math.isfinite(value) or int(value) != value or not 0 <= value <= maximum
         for value, maximum in zip(decoded, (7, 3, 3, 1), strict=True)):
    return
  status.canMonoTime = received
  status.lateralState, status.limit, status.capability = map(int, decoded[:3])
  status.denied = bool(decoded[3])
  # CI.update already checked all parser validity. Reading can_valid again here
  # would advance the parser's invalid-message counter a second time per tick.
  # Age is evaluated by the feedback consumer using this original CAN timestamp.
  status.valid = bool(can_valid)
@@ -63,5 +63,6 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
  struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
    **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
  )
  struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
  return struct_dataclass
@@ -0,0 +1,109 @@
 import ast
 from pathlib import Path
 from types import SimpleNamespace
 import unittest
 from openpilot.cereal import custom
 from openpilot.selfdrive.car.ford_pscm_status import MESSAGE, SIGNALS, populate_ford_pscm_status
 from openpilot.selfdrive.car.helpers import convert_to_capnp
 from opendbc.can import CANPacker, CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.ford.values import FordFlags
 class TestFordPscmStatus(unittest.TestCase):
  def setUp(self):
    self.cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
    self.packer = CANPacker('ford_lincoln_base_pt')
    self.parser = CANParser('ford_lincoln_base_pt', [(MESSAGE, 33), ('Yaw_Data_FD1', 100)], 0)
  def update_status(self, timestamp, *, lateral_state=2, limit=0, capability=2, denied=False):
    status = self.packer.make_can_msg(MESSAGE, 0, dict(zip(SIGNALS, (lateral_state, limit, capability, denied), strict=True)))
    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': 0.1})
    self.parser.update([(timestamp, [status, yaw])])
  def publish(self, *, can_valid=True):
    state_sp = convert_to_capnp(structs.CarStateSP(speedLimit=13.5))
    populate_ford_pscm_status(self.cp, {Bus.pt: self.parser}, state_sp, can_valid)
    return state_sp
  def test_decodes_status_and_preserves_receipt_time_across_other_can_messages(self):
    self.update_status(1_000_000_000, limit=2, capability=1, denied=True)
    original = self.publish()
    self.assertEqual(original.speedLimit, 13.5)
    status = original.fordPscmStatus
    self.assertTrue(status.valid)
    self.assertEqual(status.canMonoTime, 1_000_000_000)
    self.assertEqual((status.lateralState, status.limit, status.capability, status.denied), (2, 2, 1, True))
    # carStateSP may publish at 100 Hz while this 33 Hz message is absent. New
    # unrelated CAN must not freshen the timestamp of an old PSCM status.
    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': .2})
    self.parser.update([(1_080_000_000, [yaw])])
    copied = self.publish().fordPscmStatus
    self.assertEqual(copied.canMonoTime, 1_000_000_000)
    self.assertEqual((copied.limit, copied.capability, copied.denied), (2, 1, True))
    self.update_status(1_090_000_000, lateral_state=3, limit=3, capability=2)
    next_state = self.publish()
    with custom.CarStateSP.from_bytes(next_state.to_bytes()) as decoded:
      latest = decoded.fordPscmStatus
      self.assertTrue(latest.valid)
      self.assertEqual(latest.canMonoTime, 1_090_000_000)
      self.assertEqual((latest.lateralState, latest.limit, latest.capability, latest.denied), (3, 3, 2, False))
  def test_absent_parser_unseen_message_and_invalid_can_do_not_claim_valid_status(self):
    state = custom.CarStateSP.new_message()
    populate_ford_pscm_status(self.cp, {}, state, True)
    self.assertFalse(state.fordPscmStatus.valid)
    self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
    self.assertFalse(self.publish().fordPscmStatus.valid)
    self.update_status(1_000_000_000)
    invalid = self.publish(can_valid=False).fordPscmStatus
    self.assertFalse(invalid.valid)
    self.assertEqual(invalid.canMonoTime, 1_000_000_000)
  def test_mixed_timestamps_or_malformed_status_cannot_enable_feedback(self):
    self.update_status(1_000_000_000)
    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 990_000_000
    self.assertFalse(self.publish().fordPscmStatus.valid)
    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 1_000_000_000
    for value in (float('nan'), -1, 1.5, 4):
      self.parser.vl[MESSAGE]['LatCtlLim_D_Stat'] = value
      self.assertFalse(self.publish().fordPscmStatus.valid)
  def test_other_vehicles_and_legacy_messages_default_to_unavailable(self):
    for cp in (SimpleNamespace(brand='toyota'), SimpleNamespace(brand='ford', flags=0)):
      state = custom.CarStateSP.new_message(speedLimit=10.)
      populate_ford_pscm_status(cp, {}, state, True)
      self.assertFalse(state.fordPscmStatus.valid)
      self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
      self.assertEqual(state.speedLimit, 10.)
    # Old recordings/readers have no appended status pointer; defaults must
    # remain unavailable rather than interpreting zeroed enums as fresh data.
    self.assertFalse(custom.CarStateSP.new_message().fordPscmStatus.valid)
  def test_actual_card_update_populates_status_after_dataclass_conversion(self):
    self.update_status(1_000_000_000, limit=1)
    source_path = Path(__file__).resolve().parents[1] / 'card.py'
    source = ast.parse(source_path.read_text())
    car_class = next(n for n in source.body if isinstance(n, ast.ClassDef) and n.name == 'Car')
    method = next(n for n in car_class.body if isinstance(n, ast.FunctionDef) and n.name == 'state_update')
    statements = method.body
    first = next(i for i, n in enumerate(statements) if isinstance(n, ast.Assign) and ast.unparse(n.value) == 'self.CI.update(can_list)')
    last = next(i for i, n in enumerate(statements) if isinstance(n, ast.Expr) and isinstance(n.value, ast.Call)
                and isinstance(n.value.func, ast.Name) and n.value.func.id == 'populate_ford_pscm_status')
    self.assertGreater(last, first)
    code = compile(ast.Module(body=statements[first:last + 1], type_ignores=[]), str(source_path), 'exec')
    ci = SimpleNamespace(update=lambda _: (SimpleNamespace(canValid=True), structs.CarStateSP(speedLimit=11.)),
                         can_parsers={Bus.pt: self.parser})
    environment = {'self': SimpleNamespace(CP=self.cp, CI=ci), 'can_list': [], 'convert_to_capnp': convert_to_capnp,
                   'populate_ford_pscm_status': populate_ford_pscm_status}
    exec(code, environment)
    self.assertTrue(environment['CS_SP'].fordPscmStatus.valid)
    self.assertEqual(environment['CS_SP'].fordPscmStatus.canMonoTime, 1_000_000_000)
    self.assertEqual(environment['CS_SP'].fordPscmStatus.limit, 1)
 if __name__ == '__main__':
  unittest.main()
@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import math
 import time
 from numbers import Number
 from openpilot.cereal import log
@@ -11,8 +12,11 @@ from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority
 from openpilot.common.swaglog import cloudlog
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.ford.values import FordFlags
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -44,7 +48,7 @@ class Controls(ControlsExt):
    self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
    self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carStateSP', 'carOutput',
                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
                                  poll='selfdriveState')
    self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
@@ -52,6 +56,15 @@ class Controls(ControlsExt):
    self.steer_limited_by_safety = False
    self.curvature = 0.0
    self.desired_curvature = 0.0
    self.ford_pscm_observer = (self.CP.brand == "ford" and self.CP.flags & FordFlags.CANFD and
                               self.params.get_bool("FordPscmObserver"))
    self.ford_path_controller = FordPscmObserverPathController() if self.ford_pscm_observer else FordPathController()
    self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
                                                              self.ford_path_controller)
    self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
    if self.CP.brand == "ford":
      cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__)
    self.ford_path = FordPath()
    self.pose_calibrator = PoseCalibrator()
    self.calibrated_pose: Pose | None = None
@@ -155,6 +168,36 @@ class Controls(ControlsExt):
      actuators.curvature = float(lateral_output)
    else:
      actuators.steeringAngleDeg = float(lateral_output)
    if self.CP.brand == "ford":
      ford_model = model_v2 if self.sm.valid['modelV2'] else None
      if self.ford_model_action:
        reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
        self.ford_path = self.ford_path_controller.update(
          ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
          measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
          model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
          reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
          active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
          driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
          pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
        )
        if not self.ford_path.valid:
          CC.latActive = False
        if self.sm.frame % 20 == 0:
          cloudlog.event("Ford C2-free path tracking", model_mono_time=self.sm.logMonoTime['modelV2'],
                         measurement_mono_time=self.sm.logMonoTime['carState'],
                         reference_service=reference_service, reference_mono_time=self.sm.logMonoTime[reference_service],
                         measured_curvature=self.curvature,
                         **self.ford_path_controller.diagnostics)
      elif self.ford_pscm_observer:
        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
                                                          v_ego_raw=CS.vEgoRaw, active=CC.latActive)
      else:
        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
                                                          active=CC.latActive)
      actuators.curvature = float(self.ford_path.curvature)
    # Ensure no NaNs/Infs
    for p in ACTUATOR_FIELDS:
      attr = getattr(actuators, p)
@@ -0,0 +1,181 @@
 """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. 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 CarControllerParams, FordFlags
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
 OFFSET_STATION_M = 7.0
 HEADING_TIME_S = 1.0
 CALIBRATION_APPROVED = False
 def _packed(value, resolution, offset):
  """Mirror Float32 carControlSP and sign-reversed CANPacker rounding."""
  value = struct.unpack("f", struct.pack("f", value))[0]
  return -(math.floor((-value - offset) / resolution + 0.5) * resolution + offset)
 def _finite(*values):
  try:
    return all(math.isfinite(value) for value in values)
  except (TypeError, ValueError, OverflowError):
    return False
 def encode_model_action(model, desired_curvature, speed):
  """Encode y(7) and max(7, v*1s)*selected limited curvature.
  Preserve the reviewed core's endpoint hold when the path ends before 7 m.
  This samples the available geometry; it does not extrapolate an unseen path.
  """
  if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
    return FordPath()
  try:
    path = _model_path(model)
  except OverflowError:
    return FordPath()
  if path is None or not all(_finite(*values) for values in path):
    return FordPath()
  station, _, lateral, _ = path
  c0 = float(np.interp(min(OFFSET_STATION_M, station[-1]), station, lateral))
  c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
  return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
 class ModelActionController:
  """Unquantized C0/C1 slew positions and one C1 feedback correction.
  Feedback integrates requested minus measured curvature over traveled distance.
  Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
  """
  __slots__ = ('c0', 'c1', 'correction')
  def __init__(self):
    self.reset()
  def reset(self):
    self.c0 = self.c1 = self.correction = 0.
  def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
             feedback_dt=None, feedback_enabled=True, pscm_limited=False):
    feedback_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)
    if not target.valid:
      self.reset()
      return FordPath()
    c0 = float(np.clip(target.path_offset, -5.11, 5.11))
    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.)
 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 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.
  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()
    self.reset()
  def reset(self, status='inactive'):
    self.core.reset()
    self.last_time = self.last_measurement_time = self.last_model_time = None
    self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v1',
                        'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
  def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time,
             reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None):
    reason = None
    if not active:
      reason = 'inactive'
    elif not valid:
      reason = 'invalid_service'
    elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
      reason = 'nonfinite'
    elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)):
      reason = 'stale_input'
    elif not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1 or abs(current_curvature) > 1:
      reason = 'input_range'
    if reason is not None:
      self.reset(reason)
      return FordPath()
    dt = .01 if self.last_time is None else now - self.last_time
    feedback_dt = 0. if self.last_measurement_time is None else measurement_time-self.last_measurement_time
    if not .002 <= dt <= .1 or not 0. <= feedback_dt <= .15 or (
      self.last_model_time is not None and model_time < self.last_model_time
    ):
      self.reset('timing_reset')
      return FordPath()
    status_fresh = (pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0
                    and -.005 <= now-pscm_status.canMonoTime*1e-9 <= .15)
    pscm_limited = bool(status_fresh and pscm_status.limit == 2)
    driver_override = bool(driver_pressed or not _finite(driver_torque)
                           or abs(driver_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
                           or (status_fresh and pscm_status.limit == 3))
    feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2)))
    command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt,
                               feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited)
    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-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
 def select_model_action_controller(CP, enabled, previous_controller):
  """The separate default-off toggle takes priority on the CAN FD Lightning."""
  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD and CP.carFingerprint == 'FORD_F_150_LIGHTNING_MK1'
  if enabled and compatible:
    return FordModelActionController()
  return previous_controller
@@ -0,0 +1,368 @@
 from collections import deque
 from dataclasses import dataclass
 import math
 import numpy as np
 from opendbc.car.ford.values import CarControllerParams
 DBC_OFFSET = (-5.12, 5.11)
 DBC_ANGLE = (-0.5, 0.5235)
 DBC_CURVATURE = (-0.02, 0.02)
 DBC_CURVATURE_RATE = (-0.001024, 0.001023)
 DBC_OFFSET_RESOLUTION = 0.01
 DBC_ANGLE_RESOLUTION = 0.0005
 DBC_CURVATURE_RESOLUTION = 0.00002
 DBC_CURVATURE_RATE_RESOLUTION = 0.000001
 _PATH_MIN_LOOKAHEAD = 7.0
 _POSE_PREDICTION_TIME = 0.1
 _POSE_BLEND_CURVATURE = (0.006, 0.012)
 _PATH_OFFSET_RATE = 4.0
 _PATH_ANGLE_RATE = 1.0
 _PSCM_DT = 0.004
 _PSCM_C0_RATE = 1.5
 _PSCM_C1_RATE = 0.100006103515625
 _PSCM_C2_RATE = 0.0030059814453125
 _PSCM_SPEED_KPH = (0.0, 15.0, 40.0, 70.0, 100.0, 150.0, 200.0, 250.0)
 _PSCM_SPEED_GAIN = (32.0, 32.0, 32.0, 30.0, 30.0, 24.0, 12.0, 0.0)
 _PSCM_C0_EFFECTIVE_LIMIT = 1.0
 _PSCM_C1_EFFECTIVE_LIMIT = 0.349609375 / 10.0
@dataclass(frozen=True)
 class FordPath:
  valid: bool = False
  path_offset: float = 0.0
  path_angle: float = 0.0
  curvature: float = 0.0
  curvature_rate: float = 0.0
@dataclass(frozen=True)
 class FordPscmState:
  path_offset: float = 0.0
  path_angle: float = 0.0
  curvature: float = 0.0
@dataclass(frozen=True)
 class FordModelPose:
  path_offset: float
  path_angle: float
  offset_horizon: float
  curvature_demand: float
  forward_angle: float
 def _finite(value: float) -> float:
  return float(value) if math.isfinite(value) else 0.0
 def _sample(distance: float, distances: list[float], values: list[float]) -> float:
  return float(np.interp(distance, distances, values))
 def _blend_share(demand: float) -> float:
  lower, upper = _POSE_BLEND_CURVATURE
  return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
 def _model_path(model) -> tuple[list[float], list[float], list[float], list[float]] | None:
  try:
    x = [float(value) for value in model.position.x]
    y = [float(value) for value in model.position.y]
    heading = [float(value) for value in model.orientation.z]
  except (AttributeError, TypeError, ValueError):
    return None
  if len(x) < 2 or len(x) != len(y) or len(x) != len(heading):
    return None
  if not all(math.isfinite(value) for values in (x, y, heading) for value in values):
    return None
  distance = [0.0]
  for i in range(1, len(x)):
    distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]))
  if distance[-1] <= 0.0:
    return None
  unwrapped_heading = [heading[0]]
  for value in heading[1:]:
    delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi
    unwrapped_heading.append(unwrapped_heading[-1] + delta)
  return distance, x, y, unwrapped_heading
 def _predicted_pose(distance: float, current_curvature: float,
                    curvature_delta: float) -> tuple[float, float, float]:
  curvature = current_curvature + 0.5 * curvature_delta
  heading = curvature * distance
  if abs(curvature) < 1e-9:
    return distance, 0.0, 0.0
  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
 def _relative_pose(target_distance: float, path: tuple[list[float], list[float], list[float], list[float]],
                   vehicle_pose: tuple[float, float, float]) -> tuple[float, float]:
  distance, x, y, heading = path
  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
  dx = _sample(target_distance, distance, x) - vehicle_x
  dy = _sample(target_distance, distance, y) - vehicle_y
  cosine = math.cos(vehicle_heading)
  sine = math.sin(vehicle_heading)
  offset = -sine * dx + cosine * dy
  angle = math.atan2(math.sin(_sample(target_distance, distance, heading) - vehicle_heading),
                     math.cos(_sample(target_distance, distance, heading) - vehicle_heading))
  return offset, angle
 def _path_pose(target_distance: float,
               path: tuple[list[float], list[float], list[float], list[float]]) -> tuple[float, float, float]:
  distance, x, y, heading = path
  return (_sample(target_distance, distance, x), _sample(target_distance, distance, y),
          _sample(target_distance, distance, heading))
 def _bounded_feedback(feedforward: float, feedback: float, resolution: float, zero_path_limit: float) -> float:
  quantization_threshold = 0.5 * resolution
  limit = max(abs(feedforward) - resolution, 0.0) if abs(feedforward) >= quantization_threshold else zero_path_limit
  return float(np.clip(feedback, -limit, limit))
 def _model_pose(path: tuple[list[float], list[float], list[float], list[float]],
                current_curvature: float, curvature_delta: float, v_ego: float) -> FordModelPose:
  distance, _, _, _ = path
  advance = min(v_ego * _POSE_PREDICTION_TIME, distance[-1])
  offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1] - advance)
  angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1] - advance)
  # Keep the model's remaining path as feedforward. Measured vehicle motion is
  # a separate, short delay-aligned correction, so catching the requested
  # curvature cannot erase a turn that is still present in the model path.
  model_pose = _path_pose(advance, path)
  model_offset, _ = _relative_pose(advance + offset_horizon, path, model_pose)
  _, model_angle = _relative_pose(advance + angle_horizon, path, model_pose)
  vehicle_pose = _predicted_pose(advance, current_curvature, curvature_delta)
  feedback_offset, feedback_angle = _relative_pose(advance, path, vehicle_pose)
  gentle_curvature = _POSE_BLEND_CURVATURE[0]
  feedback_offset = _bounded_feedback(model_offset, feedback_offset, DBC_OFFSET_RESOLUTION,
                                       0.5 * gentle_curvature * advance ** 2)
  feedback_angle = _bounded_feedback(model_angle, feedback_angle, DBC_ANGLE_RESOLUTION,
                                      gentle_curvature * advance)
  offset_curvature = 2.0 * model_offset / max(offset_horizon, 1e-3) ** 2
  angle_curvature = model_angle / max(angle_horizon, 1e-3)
  return FordModelPose(model_offset + feedback_offset, model_angle + feedback_angle, offset_horizon,
                       max(abs(offset_curvature), abs(angle_curvature)), model_angle)
 def _encode_pose(pose: FordModelPose, pose_share: float, curvature: float) -> FordPath:
  path_offset = pose_share * pose.path_offset
  path_angle = pose_share * pose.path_angle
  if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION:
    path_offset = 0.0
  if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION:
    path_angle = 0.0
  limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
  path_offset += (path_angle - limited_path_angle) * pose.offset_horizon
  return FordPath(
    valid=True,
    path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
    path_angle=limited_path_angle,
    curvature=float(np.clip(curvature, *DBC_CURVATURE)),
    curvature_rate=0.0,
  )
 def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float,
                 current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath:
  pose = _model_pose(path, current_curvature, curvature_delta, v_ego)
  pose_share = _blend_share(max(pose.curvature_demand, abs(desired_curvature)))
  # Match upstream's C2-only normal driving, then continuously transfer the
  # command to the model pose for larger maneuvers. An opposing/finished model
  # path must unload sticky C2 and retain the fast pose needed to unwind it.
  c2_opposes_path = desired_curvature != 0.0 and desired_curvature * pose.forward_angle <= 0.0
  if c2_opposes_path:
    pose_share = 1.0
    curvature = 0.0
  else:
    curvature = desired_curvature * (1.0 - pose_share)
  return _encode_pose(pose, pose_share, curvature)
 class FordPathController:
  """Blend normal C2 following into the model's forward C0/C1 pose."""
  def __init__(self, dt: float = 0.01):
    self.dt = dt
    self._last_path = FordPath(valid=True)
    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
  def _limit(self, target: FordPath) -> FordPath:
    offset_delta = target.path_offset - self._last_path.path_offset
    angle_delta = target.path_angle - self._last_path.path_angle
    scale = min(
      1.0,
      _PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
      _PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
    )
    self._last_path = FordPath(
      True,
      self._last_path.path_offset + scale * offset_delta,
      self._last_path.path_angle + scale * angle_delta,
      self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
      0.0,
    )
    return self._last_path
  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
             v_ego: float = 0.0, active: bool = True) -> FordPath:
    if not active:
      self._last_path = FordPath(valid=True)
      self._curvature_history.clear()
      return FordPath()
    current_curvature = _finite(current_curvature)
    self._curvature_history.append(current_curvature)
    curvature_delta = (current_curvature - self._curvature_history[0]
                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
    path = _model_path(model) if model is not None else None
    if path is None:
      return self._limit(FordPath(valid=True))
    return self._limit(_encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
                                    max(_finite(v_ego), 0.0)))
 def _pscm_slew(value: float, target: float, rate: float, ticks: int) -> float:
  step = rate * _PSCM_DT * ticks
  return float(np.clip(target, value - step, value + step))
 def _pscm_speed_gain(v_ego: float) -> float:
  return float(np.interp(max(v_ego, 0.0) * 3.6, _PSCM_SPEED_KPH, _PSCM_SPEED_GAIN))
 def _wire_path(path: FordPath) -> FordPath:
  return FordPath(
    valid=path.valid,
    path_offset=round(path.path_offset / DBC_OFFSET_RESOLUTION) * DBC_OFFSET_RESOLUTION,
    path_angle=round(path.path_angle / DBC_ANGLE_RESOLUTION) * DBC_ANGLE_RESOLUTION,
    curvature=round(path.curvature / DBC_CURVATURE_RESOLUTION) * DBC_CURVATURE_RESOLUTION,
    curvature_rate=round(path.curvature_rate / DBC_CURVATURE_RATE_RESOLUTION) * DBC_CURVATURE_RATE_RESOLUTION,
  )
 def _pscm_contributions(state: FordPscmState, v_ego: float) -> tuple[float, float, float]:
  gain = _pscm_speed_gain(v_ego)
  return (
    float(np.clip(0.5 * gain * state.path_offset, -0.5 * gain, 0.5 * gain)),
    float(np.clip(10.0 * gain * state.path_angle, -0.349609375 * gain, 0.349609375 * gain)),
    float(np.clip(0.30078125 * gain * state.curvature * v_ego ** 2, -0.5 * gain, 0.5 * gain)),
  )
 class FordPscmObserver:
  """Mirror the firmware's held-command coefficient states at its 250 Hz step."""
  def __init__(self):
    self.state = FordPscmState()
    self.command = FordPath(valid=True)
    self._phase = 0.0
  def reset(self) -> None:
    self.state = FordPscmState()
    self.command = FordPath(valid=True)
    self._phase = 0.0
  def advance(self, elapsed: float) -> None:
    self._phase += max(elapsed, 0.0)
    ticks = int((self._phase + 1e-12) / _PSCM_DT)
    self._phase -= ticks * _PSCM_DT
    if ticks == 0:
      return
    self.state = FordPscmState(
      _pscm_slew(self.state.path_offset, self.command.path_offset, _PSCM_C0_RATE, ticks),
      _pscm_slew(self.state.path_angle, self.command.path_angle, _PSCM_C1_RATE, ticks),
      _pscm_slew(self.state.curvature, self.command.curvature + 10.0 * self.command.curvature_rate,
                  _PSCM_C2_RATE, ticks),
    )
  def set_command(self, command: FordPath) -> None:
    self.command = _wire_path(command)
 class FordPscmObserverPathController:
  """Compensate model-path commands for the PSCM coefficient state it still carries."""
  def __init__(self, dt: float = 0.01):
    self.dt = dt
    self._last_path = FordPath(valid=True)
    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
    self.observer = FordPscmObserver()
    self._sent_c2 = 0.0
  def _reset(self) -> None:
    self._last_path = FordPath(valid=True)
    self._curvature_history.clear()
    self.observer.reset()
    self._sent_c2 = 0.0
  def _command_for_state(self, target: FordPath, v_ego: float) -> FordPath:
    # The target describes the desired fully-settled PSCM contribution. C0 keeps
    # the remaining C1-saturated residual. C1 supplies the primary contribution
    # that the known slow C2 state does not yet provide, without a guessed gain.
    target_state = FordPscmState(target.path_offset, target.path_angle, target.curvature)
    target_contribution = sum(_pscm_contributions(target_state, v_ego))
    _, _, observed_c2 = _pscm_contributions(self.observer.state, v_ego)
    gain = _pscm_speed_gain(v_ego)
    required_fast = target_contribution - observed_c2
    c1_contribution = float(np.clip(required_fast, -0.349609375 * gain, 0.349609375 * gain))
    c0_contribution = required_fast - c1_contribution
    path_offset = c0_contribution / (0.5 * gain) if gain > 0.0 else 0.0
    path_angle = c1_contribution / (10.0 * gain) if gain > 0.0 else 0.0
    return FordPath(
      valid=True,
      path_offset=float(np.clip(path_offset, -_PSCM_C0_EFFECTIVE_LIMIT, _PSCM_C0_EFFECTIVE_LIMIT)),
      path_angle=float(np.clip(path_angle, -_PSCM_C1_EFFECTIVE_LIMIT, _PSCM_C1_EFFECTIVE_LIMIT)),
      curvature=target.curvature,
      curvature_rate=target.curvature_rate,
    )
  def _limit(self, target: FordPath, v_ego_raw: float) -> FordPath:
    path_offset = float(np.clip(target.path_offset,
                                self._last_path.path_offset - _PATH_OFFSET_RATE * self.dt,
                                self._last_path.path_offset + _PATH_OFFSET_RATE * self.dt))
    path_angle = float(np.clip(target.path_angle,
                               self._last_path.path_angle - _PATH_ANGLE_RATE * self.dt,
                               self._last_path.path_angle + _PATH_ANGLE_RATE * self.dt))
    curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(
      target.curvature, self._sent_c2, v_ego_raw, 0.0, True, CarControllerParams.LMC2_STEP,
    )
    self._sent_c2 = curvature
    self._last_path = FordPath(True, path_offset, path_angle, curvature, target.curvature_rate)
    self.observer.set_command(self._last_path)
    return self._last_path
  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
             v_ego: float = 0.0, v_ego_raw: float = 0.0, active: bool = True) -> FordPath:
    if not active:
      self._reset()
      return FordPath()
    self.observer.advance(self.dt)
    current_curvature = _finite(current_curvature)
    self._curvature_history.append(current_curvature)
    curvature_delta = (current_curvature - self._curvature_history[0]
                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
    path = _model_path(model) if model is not None else None
    if path is None:
      target = FordPath(valid=True)
    else:
      target = _encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
                            max(_finite(v_ego), 0.0))
    v_ego_raw = max(_finite(v_ego_raw), 0.0)
    command = self._command_for_state(target, v_ego_raw)
    return self._limit(command, v_ego_raw)
@@ -0,0 +1,229 @@
 {
  "description": "Curvature-driven C0 and full-heading C1 command regression; does not predict counterfactual wheel response. Contains geometry and control signals only, no GPS.",
  "fixture_sha256": "12782ac1b0d0637945f729a46ad03af16cd58188872b6a65f104e32c4db70e9b",
  "episodes": [
    {
      "name": "left_large",
      "route": "84865544361f55cb_00000077--4b55791ce6",
      "range_seconds": [
        809.5,
        815.0
      ],
      "evidence_seconds": [
        812.1,
        814.0
      ],
      "samples": 532
    },
    {
      "name": "right_large",
      "route": "84865544361f55cb_00000077--4b55791ce6",
      "range_seconds": [
        866.5,
        872.0
      ],
      "evidence_seconds": [
        869.5,
        871.0
      ],
      "samples": 547
    },
    {
      "name": "left_very_large",
      "route": "84865544361f55cb_00000077--4b55791ce6",
      "range_seconds": [
        880.0,
        884.0
      ],
      "evidence_seconds": [
        882.9,
        883.32
      ],
      "samples": 397
    },
    {
      "name": "right_plateau",
      "route": "84865544361f55cb_00000077--4b55791ce6",
      "range_seconds": [
        964.0,
        970.0
      ],
      "evidence_seconds": [
        967.2,
        968.93
      ],
      "samples": 583
    },
    {
      "name": "oscillation",
      "route": "84865544361f55cb_00000078--349f5b8695",
      "range_seconds": [
        29.0,
        37.0
      ],
      "evidence_seconds": [
        32.5,
        36.1
      ],
      "samples": 795
    },
    {
      "name": "weak_first",
      "route": "84865544361f55cb_0000007a--5a95fc717e",
      "range_seconds": [
        90.5,
        95.2
      ],
      "evidence_seconds": [
        93.5,
        95.08
      ],
      "samples": 467
    },
    {
      "name": "weak_second",
      "route": "84865544361f55cb_0000007a--5a95fc717e",
      "range_seconds": [
        119.5,
        125.3
      ],
      "evidence_seconds": [
        122.5,
        125.2
      ],
      "samples": 576
    }
  ],
  "sources": {
    "84865544361f55cb_00000077--4b55791ce6": [
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--0--rlog.zst",
        "bytes": 10342855,
        "sha256": "2f072eff3076f4d32dbc1a077f24fc85818abeafd13a4b970b1ba558a0d2ca52"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--1--rlog.zst",
        "bytes": 11688902,
        "sha256": "bbbbf7fc79b1c7133b83678a5202ac41cd8b3dcdd0bb358843d43fdb38f25e1e"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--2--rlog.zst",
        "bytes": 11875525,
        "sha256": "18d7e0224a61d068aeeef0e176da071f5e67d77fcb7ebaa7b5854756d4438add"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--3--rlog.zst",
        "bytes": 12489313,
        "sha256": "7869f95c87849018df07c680e0584145a31c407608f96f2fc77f35b729f02d2c"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--4--rlog.zst",
        "bytes": 12301118,
        "sha256": "2c2125fb2320b9bc6620fc586cedb6558b33e8475d0cce3fb361db4eea6a1c28"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--5--rlog.zst",
        "bytes": 12976655,
        "sha256": "b18c448786daf46cfa05bf0352396ce5851a09603d34ee5b989e096da5ee5180"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--6--rlog.zst",
        "bytes": 13223857,
        "sha256": "8c08b8d47ebca38c70aa94a1cdda25443ed3be94727bb487406b518471c88dd1"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--7--rlog.zst",
        "bytes": 13043701,
        "sha256": "638948d7e5853046773f82df8a531c518c42883c77f060e61ff683ccb59d52af"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--8--rlog.zst",
        "bytes": 12569024,
        "sha256": "41e5c83d2205964889579cf24967712dd0340da5f30f213409f2b1e04e6eb78a"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--9--rlog.zst",
        "bytes": 11926213,
        "sha256": "6034a90e817424c02755c2c0d9bdbad088c4286edfb887d9f2acbd60d7818da7"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--10--rlog.zst",
        "bytes": 13010961,
        "sha256": "23958a1ac8277977952c73e889fbfd9245bc2cf82b3af58233c245dbf1e76545"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--11--rlog.zst",
        "bytes": 13204208,
        "sha256": "f78d65b7b5927a8570f52d765f9af45431f8ab72260987f718321ad1b03c70af"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--12--rlog.zst",
        "bytes": 12562994,
        "sha256": "485621d1cf71605fccc8c679cb146b1f0954078849db7c74b1b65b166d741025"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--13--rlog.zst",
        "bytes": 12610836,
        "sha256": "0b4b0c01caae39a7dc4ff2219ab168029ea12c43b1966b8c204972743627c24b"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--14--rlog.zst",
        "bytes": 13114068,
        "sha256": "f18769800bd08c7614280916b50ec0274eca9b4765ad26d6549b72912321dac0"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--15--rlog.zst",
        "bytes": 13113162,
        "sha256": "abf0adc702db6f5fdd78a145709504c7061255bc9b9a61329a575707d5bd2f74"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--16--rlog.zst",
        "bytes": 13029294,
        "sha256": "a76ed74b889bda60e2d929123186df83653591d1e058fbd1497551fbbf23941e"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--17--rlog.zst",
        "bytes": 12429846,
        "sha256": "51d3aff2afa1a7ce3c5372a499b14394f22ac730e952273a69496c65ef726ed2"
      },
      {
        "name": "84865544361f55cb_00000077--4b55791ce6--18--rlog.zst",
        "bytes": 9241845,
        "sha256": "2a1646227f9cdb1ab7eaf79fa653b7f4443a629a703577ff6c38a715dbced3a4"
      }
    ],
    "84865544361f55cb_00000078--349f5b8695": [
      {
        "name": "84865544361f55cb_00000078--349f5b8695--0--rlog.zst",
        "bytes": 10919702,
        "sha256": "2d35f6c9ac9b09f8b86b3fe2fbe864e4af0d50c411e3dd56572b5aa113bf1973"
      },
      {
        "name": "84865544361f55cb_00000078--349f5b8695--1--rlog.zst",
        "bytes": 10600112,
        "sha256": "6cabb48ea0caeb2fddd58be35b5c7e7c42faf87fc01991c4603573bffe33eaeb"
      }
    ],
    "84865544361f55cb_0000007a--5a95fc717e": [
      {
        "name": "84865544361f55cb_0000007a--5a95fc717e--0--rlog.zst",
        "bytes": 10841486,
        "sha256": "bfc9e3308e5241e76cba57f2441043710fe8b18ade314221ee5e423f640c211a"
      },
      {
        "name": "84865544361f55cb_0000007a--5a95fc717e--1--rlog.zst",
        "bytes": 12567837,
        "sha256": "ab9eb05c5805e286a6ec639bbbbc1cf086bfcf1b440801ad000713db95dfa7fc"
      },
      {
        "name": "84865544361f55cb_0000007a--5a95fc717e--2--rlog.zst",
        "bytes": 11015857,
        "sha256": "af4f87f39be37f0a5b23b58de50c2ed8801bdbda6544cd6cd292525fc3bd4fb3"
      }
    ]
  },
  "pairing": "controlsState cycle time; causal carState speed/yaw/pressed; exact consumed model timestamp and geometry; nearest same-cycle carControl and carControlSP within 5ms.",
  "yaw_rate": "Negative carState.yawRate, matching the model/control curvature coordinate sign; no wheel-to-curvature conversion.",
  "desired_curvature": "Exact controlsState.desiredCurvature from the matching controlsState cycle. This is the post-selection, post-limiting request consumed by controlsd; it is not a wheel-angle-to-curvature fit.",
  "reference_time": "Exact consumed modelV2 publication time, in the same relative seconds as each episode. The extraction cache does not retain consumed lateralManeuverPlan timestamps or validity; model time is an explicit replay assumption and cannot verify alternate-reference freshness."
 }
@@ -0,0 +1,67 @@
 {
  "description": "Real route80 turn-command regressions. Signal-only fixture; no GPS. Counterfactual commands do not predict physical vehicle response.",
  "route": "84865544361f55cb_00000080--1643deea7e",
  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
  "frozen_v3_controller_sha256": "576f4ec6f2dbc93f7e6c93a69839f69447eb5a0c2f834bd48b24f84a163dc2eb",
  "fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
  "episodes": [
    {
      "name": "under_333_339",
      "range_seconds": [
        331.5,
        339.0
      ],
      "evidence_seconds": [
        333.0,
        339.0
      ],
      "samples": 745
    },
    {
      "name": "over_417_420",
      "range_seconds": [
        415.5,
        420.0
      ],
      "evidence_seconds": [
        417.0,
        420.0
      ],
      "samples": 447
    },
    {
      "name": "under_430_435",
      "range_seconds": [
        428.5,
        435.0
      ],
      "evidence_seconds": [
        430.0,
        435.0
      ],
      "samples": 646
    }
  ],
  "sources": [
    {
      "name": "84865544361f55cb_00000080--1643deea7e--5--rlog.zst",
      "bytes": 12531711,
      "sha256": "059482830794cb0eabe6069b75a9610b900bf2a93d7a6624f53c575cef997157"
    },
    {
      "name": "84865544361f55cb_00000080--1643deea7e--6--rlog.zst",
      "bytes": 12560505,
      "sha256": "147276789f5b14913adc4cd16db18f3d4bd27ce8497c9ff96fdf0315c219339f"
    },
    {
      "name": "84865544361f55cb_00000080--1643deea7e--7--rlog.zst",
      "bytes": 12660797,
      "sha256": "b311b6ace75819db52b9618154d68c7d12e2751d5046b6d174adb89ef87a223c"
    }
  ],
  "pairing": "Exact controlsState desiredCurvature and consumed model publication timestamp; causal carState speed, negative CAN yaw, and steeringPressed; nearest same-cycle carControl/carControlSP within 5 ms.",
  "reference_time": "Consumed modelV2 publication time. Controller audit confirms route80 used modelV2 as reference throughout.",
  "preroll": "Each episode starts from reset 1.5 s before evidence; v3_replay stores those exact cold-start commands and gates, while recorded stores original live path fields.",
  "benchmark_clean": "Existing route80 benchmark mask: whole interval request minus 0.5 s through response (0.2 s) plus 0.25 s active, unpressed, valid, fresh, and speed >= 2 m/s.",
  "expected_common_c1": "Independent shadow: clip(desiredCurvature * max(7 m, vEgo * 1 s), +/-0.5 rad), independently slewed at 0.5 rad/s and packed to Float32/sign-reversed CAN semantics. No subtraction of measured curvature."
 }
@@ -0,0 +1,13 @@
 {
  "description": "PSCM status and raw driver-torque overlay for the existing three route80 request windows. No GPS. No counterfactual vehicle response.",
  "fixture_sha256": "a9defdc5abdf26724358d606beb16becbdf30faa972974d49b179a9e004d7629",
  "base_fixture": "ford_curvature_heading_route80.npz",
  "base_fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
  "source_route": "84865544361f55cb_00000080--1643deea7e",
  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
  "samples": 1838,
  "source_cache_sha256": "1cd3e0c00805869ace1c5954dc682644f36f5eddb71785b69e4cb9da40f7f04f",
  "pairing": "Latest actual bus-0 EPS 972 frame at or before each controlsState cycle; raw steering torque from the exact causal carState used by the base fixture.",
  "timestamp_policy": "Actual CAN event logMonoTime in route-relative seconds, not the benchmark response-shifted status. The old route predates the new carStateSP status telemetry; source CAN timestamps are an explicit replay approximation.",
  "validity": "Replay validity uses the paired carState valid and canValid values; enum validity, availability and age are checked by the production feedback controller."
 }
@@ -0,0 +1,40 @@
 {
  "description": "Signal-only v6 turn-exit recovery regression; no location, device identity, or predicted new vehicle response.",
  "recorded_controller_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
  "baseline_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
  "response_delay": 0.20000000298023224,
  "samples": 9134,
  "models": 1843,
  "fixture_sha256": "41d5e3efcee03a9e02fcaf7bf456c050c6a671a5b7f7fc9706ddf4d27bad71b8",
  "windows": [
    {
      "name": "overturn_then_underturn",
      "range_s": [
        19.99615067150053,
        29.48070058550053
      ],
      "samples": 521
    },
    {
      "name": "well_tracked_curve_a",
      "range_s": [
        56.19474309950053,
        63.68808603150053
      ],
      "samples": 729
    },
    {
      "name": "well_tracked_curve_b",
      "range_s": [
        101.19780691350051,
        116.33885445450052
      ],
      "samples": 810
    }
  ],
  "selection": "One previously identified overturn-then-underturn event and two previously reported well-tracked curves; selected before recovery implementation.",
  "mask": "Whole t-0.5 through t+0.65 interval active, valid, fresh, unpressed, raw driver torque magnitude <=1 Nm; requested |curvature|*speed\u00b2 >=.5 m/s\u00b2.",
  "timing": "Exact consumed model publication; causal CAN/PSCM at estimated control computation time. Subtract observed median computation-to-publication delay; unsampled tick timing remains approximate.",
  "context": "At least 20 seconds prior context or the available start, extended before the latest observed reset. Overlapping episodes are merged.",
  "coordinates": "Times are local elapsed seconds; models contain only relative position.x/y and orientation.z arrays."
 }
@@ -0,0 +1,247 @@
 {
  "description": "Signal-only historical fallback evidence and frozen-v5 comparison; no GPS or inferred counterfactual vehicle response.",
  "route": "route83",
  "recorded_commit": "79a4caa1f6b71488949108aee9ae6ae6566347b1",
  "fixture_sha256": "d00312c430ace47000c05b8284ee8d56df56ec24bb17a9ea8f4dce83133527c3",
  "samples": 11744,
  "model_count": 2367,
  "source_cache_sha256": "53d786aff2e0b6338e1991320145305fda3101f7b76e50bd2929adbcaea95b28",
  "response_delay": 0.20000000298023224,
  "episodes": [
    [
      1861.2933736250002,
      1874.756970279
    ],
    [
      1878.07372132,
      1892.07372132
    ],
    [
      1950.874232409,
      1964.874232409
    ],
    [
      2440.9020600930003,
      2456.964158177
    ],
    [
      2580.722658577,
      2611.364366768
    ],
    [
      2734.478264791,
      2764.574374172
    ]
  ],
  "windows": [
    {
      "name": "successful_large_early",
      "role": "authority_target",
      "range_s": [
        1866.722720383,
        1874.756970279
      ],
      "samples": 426,
      "substantial_demand_required": true,
      "recorded_can_ratio_02s_median": 1.0233371460413845,
      "published_median_abs_c0_c1": [
        1.6002928018569946,
        0.2796146124601364
      ],
      "send_clamped_median_abs_c0_c1": [
        1.6002928018569946,
        0.2796146124601364
      ],
      "phase_samples": {
        "phase_turn_in": 15,
        "phase_held": 122,
        "phase_release": 402,
        "phase_reversal": 0
      }
    },
    {
      "name": "centering_reversal_positive_to_negative",
      "role": "reversal",
      "range_s": [
        1888.07372132,
        1892.07372132
      ],
      "samples": 396,
      "substantial_demand_required": false,
      "recorded_can_ratio_02s_median": null,
      "published_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "send_clamped_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "phase_samples": {
        "phase_turn_in": 283,
        "phase_held": 48,
        "phase_release": 104,
        "phase_reversal": 21
      }
    },
    {
      "name": "centering_reversal_negative_to_positive",
      "role": "reversal",
      "range_s": [
        1960.874232409,
        1964.874232409
      ],
      "samples": 397,
      "substantial_demand_required": false,
      "recorded_can_ratio_02s_median": null,
      "published_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "send_clamped_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "phase_samples": {
        "phase_turn_in": 154,
        "phase_held": 0,
        "phase_release": 183,
        "phase_reversal": 21
      }
    },
    {
      "name": "clean_release",
      "role": "release",
      "range_s": [
        2453.714158177,
        2456.964158177
      ],
      "samples": 323,
      "substantial_demand_required": false,
      "recorded_can_ratio_02s_median": null,
      "published_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "send_clamped_median_abs_c0_c1": [
        0.0,
        0.0
      ],
      "phase_samples": {
        "phase_turn_in": 4,
        "phase_held": 0,
        "phase_release": 305,
        "phase_reversal": 17
      }
    },
    {
      "name": "successful_smaller_positive",
      "role": "sign_coverage_only",
      "range_s": [
        2590.722658577,
        2600.918740146
      ],
      "samples": 175,
      "substantial_demand_required": true,
      "recorded_can_ratio_02s_median": 1.0960646334373787,
      "published_median_abs_c0_c1": [
        0.42173025012016296,
        0.1222948431968689
      ],
      "send_clamped_median_abs_c0_c1": [
        0.42173025012016296,
        0.1222948431968689
      ],
      "phase_samples": {
        "phase_turn_in": 170,
        "phase_held": 61,
        "phase_release": 0,
        "phase_reversal": 0
      }
    },
    {
      "name": "large_under_response",
      "role": "under_response_challenge",
      "range_s": [
        2604.2254721,
        2611.364366768
      ],
      "samples": 128,
      "substantial_demand_required": true,
      "recorded_can_ratio_02s_median": 0.7322859508492778,
      "published_median_abs_c0_c1": [
        2.4204851388931274,
        0.42145511507987976
      ],
      "send_clamped_median_abs_c0_c1": [
        2.4204851388931274,
        0.42145511507987976
      ],
      "phase_samples": {
        "phase_turn_in": 68,
        "phase_held": 96,
        "phase_release": 56,
        "phase_reversal": 0
      }
    },
    {
      "name": "successful_large_181deg",
      "role": "authority_target",
      "range_s": [
        2744.478264791,
        2750.573209708
      ],
      "samples": 207,
      "substantial_demand_required": true,
      "recorded_can_ratio_02s_median": 1.0087938914780248,
      "published_median_abs_c0_c1": [
        2.1044259071350098,
        0.3815947473049164
      ],
      "send_clamped_median_abs_c0_c1": [
        2.1044259071350098,
        0.3815947473049164
      ],
      "phase_samples": {
        "phase_turn_in": 137,
        "phase_held": 94,
        "phase_release": 64,
        "phase_reversal": 0
      }
    },
    {
      "name": "large_over_response_290deg",
      "role": "over_response_challenge_not_target",
      "range_s": [
        2760.493612962,
        2764.574374172
      ],
      "samples": 181,
      "substantial_demand_required": true,
      "recorded_can_ratio_02s_median": 1.2515789463064766,
      "published_median_abs_c0_c1": [
        4.737145900726318,
        0.5235000252723694
      ],
      "send_clamped_median_abs_c0_c1": [
        4.737145900726318,
        0.5
      ],
      "phase_samples": {
        "phase_turn_in": 139,
        "phase_held": 90,
        "phase_release": 41,
        "phase_reversal": 0
      }
    }
  ],
  "selection": "Authority targets require automatic turn windows with >=1 second strict torque eligibility, eligible |wheel|>=150 degrees, and whole-window CAN response ratio median 0.90..1.10 at fixed 0.2 s. No positive-request large turn qualifies.",
  "non_targets": "Positive smaller turn supplies sign coverage only. Under/over response and release/reversal windows are regression challenges, not authority targets.",
  "context": "At least 10 s pre-roll or available route start, extended to include the preceding feedback reset/sign reversal. Overlapping intervals are merged. First episode begins at the partial route boundary with unobserved earlier history.",
  "phase_policy": "Held means request curvature range over +/-0.25 s times speed squared <0.15 m/s2 at demand>=0.5. Turn-in/release compare current absolute curvature with the historical held request at measurement_time-delay, scaled by max(7,speed), using +/-0.0005 rad. These masks can overlap held; reversal means opposing delayed/current signs.",
  "wire_policy": "Published coefficients preserve Float32 values. Send-clamped copy caps C0 to +/-5.11 and C1 to +/-0.5 before packing. Actual decoded wire is normalized to controller sign, nearest within 15 ms; wire_time/fresh/mode expose timing approximation.",
  "model_schema": "models[model_index] contains position.x, position.y, orientation.z; Float32 conversion preserves the original model payload precision.",
  "v5_reference": "Frozen full sequential replay from command_replay.npz, whose source hash and limitations are recorded in command_replay.json.",
  "frozen_v5_revision": "09acf8ec2f327769f00ee53563ad2dd9225e37a7",
  "preroll_validation": "Compact reset replay exactly matches full sequential frozen-v5 C0/C1, gates and bias on all 2233 evidence samples."
 }
@@ -0,0 +1,156 @@
 {
  "description": "Anonymous recorded-input turn-exit regression fixture; command construction only, not simulated vehicle response.",
  "baseline_revision": "dfcfddb91ce2409511f5b2dbce25d06d5056b3d6",
  "baseline_hypothesis": "model-pose-c0-c1-feedback-v7",
  "baseline_source_hashes": {
    "controller_sha256": "4951a6352d89fcd66277bbfe682bd22e935a31b5a4db33e617ad21189b6705fd",
    "allocator_sha256": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a"
  },
  "fixture_sha256": "87a030c309061b7dc218715d05440c2077e465a8138079b46e8e8cee94201e54",
  "source_fixture_sha256": "d476110b83dc628ffbd094220e464d6d3114b709bda2977813c3217964d41086",
  "response_delay": 0.20000000298023224,
  "publication_latency_estimate_s": 0.0015483515003040793,
  "samples": 15273,
  "model_count": 3078,
  "evidence_samples": 4879,
  "context_policy": "At least twenty seconds prior context, extended before the last observed reset. Overlapping intervals are merged.",
  "provenance": "Selected from a recorded drive running the pinned baseline; request, model, driver and PSCM observations stay fixed during replay.",
  "baseline_policy": "Stored commands, validity and bias exactly match the complete baseline replay on evidence samples. Context outside evidence initializes state and is not an exact-output target.",
  "compact_full_baseline_evidence_parity": {
    "commands": {
      "exact": true,
      "max_difference": 0.0
    },
    "valid": {
      "exact": true,
      "max_difference": 0.0
    },
    "heading_bias": {
      "exact": true,
      "max_difference": 0.0
    }
  },
  "measurement_policy": "Controller computation time is estimated from publication time using the recorded median latency; exact vehicle motion under changed commands is unknown.",
  "clean_policy": "Every sample from request time minus 0.5 s through plus 0.65 s is active, valid, fresh, unpressed and within 1 Nm raw driver torque. Demand is absolute desired curvature times current speed squared; substantial means at least 0.5 m/s2.",
  "driver_policy": "All replay inputs retain driver interference; only comparison metrics use the clean mask. History-reset failures intentionally retain nearby driver context.",
  "coordinates": "Elapsed seconds shifted to the first fixture control cycle; model x/y/heading are vehicle-relative, not global position.",
  "retained_fields": [
    "t",
    "episode",
    "model_index",
    "models",
    "desired_curvature",
    "yaw_rate",
    "speed",
    "measurement_time",
    "model_time",
    "reference_time",
    "active",
    "valid",
    "pressed",
    "steering_torque",
    "pscm_timestamp",
    "pscm_valid",
    "pscm_lateral_state",
    "pscm_limit",
    "pscm_capability",
    "pscm_denied",
    "clean_rawtorque",
    "demand",
    "window_masks",
    "evidence",
    "baseline_commands",
    "baseline_valid",
    "baseline_heading_base",
    "baseline_heading_target",
    "baseline_heading_bias",
    "baseline_feedback_yaw_error",
    "baseline_feedback_reference_curvature",
    "baseline_status",
    "baseline_offset_target"
  ],
  "omitted_data": "No route/device identifiers, VIN, GPS, private paths, raw wheel angle, wheel rate, EPS torque, or absolute clock origins.",
  "baseline_status_meaning": "feedback_status from the pinned baseline",
  "windows": [
    {
      "name": "good_curve_a",
      "role": "comparison",
      "range_s": [
        20.0002130975003,
        25.0002130975003
      ],
      "samples": 496,
      "clean_substantial_samples": 259
    },
    {
      "name": "first_reversal",
      "role": "reversal",
      "range_s": [
        83.0002130975003,
        92.7002130975003
      ],
      "samples": 964,
      "clean_substantial_samples": 167
    },
    {
      "name": "good_curve_b",
      "role": "comparison",
      "range_s": [
        121.0002130975003,
        128.0002130975003
      ],
      "samples": 695,
      "clean_substantial_samples": 308
    },
    {
      "name": "second_reversal",
      "role": "reversal",
      "range_s": [
        133.5002130975003,
        138.9002130975003
      ],
      "samples": 537,
      "clean_substantial_samples": 191
    },
    {
      "name": "large_turn_driver_context_a",
      "role": "driver_context",
      "range_s": [
        150.0002130975003,
        157.0002130975003
      ],
      "samples": 695,
      "clean_substantial_samples": 0
    },
    {
      "name": "over_growth",
      "role": "over_response",
      "range_s": [
        182.0002130975003,
        191.0002130975003
      ],
      "samples": 897,
      "clean_substantial_samples": 66
    },
    {
      "name": "large_turn_driver_context_b",
      "role": "driver_context",
      "range_s": [
        199.0002130975003,
        205.0002130975003
      ],
      "samples": 595,
      "clean_substantial_samples": 281
    },
    {
      "name": "zero_bias_release",
      "role": "under_response",
      "range_s": [
        202.0002130975003,
        205.0002130975003
      ],
      "samples": 297,
      "clean_substantial_samples": 279
    }
  ]
 }
@@ -0,0 +1,59 @@
 import ast
 import io
 import json
 import logging
 from pathlib import Path
 from types import SimpleNamespace
 import unittest
 from openpilot.common.logging_extra import SwagFormatter, SwagLogger
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
 from openpilot.selfdrive.controls.lib.ford_path import FordPathController, FordPscmObserverPathController
 from openpilot.selfdrive.controls.tests.test_ford_model_action import circle
 class TestFordControlsLogging(unittest.TestCase):
  def emit_controls_event(self, event, controls):
    # Execute the actual controlsd call with the real logger and formatter,
    # without launching hardware-dependent Controls or opening logging IPC.
    source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
    source = ast.parse(source_path.read_text())
    calls = [node for node in ast.walk(source) if isinstance(node, ast.Call)
             and isinstance(node.func, ast.Attribute) and isinstance(node.func.value, ast.Name)
             and node.func.value.id == 'cloudlog' and node.args
             and isinstance(node.args[0], ast.Constant) and node.args[0].value == event]
    self.assertEqual(len(calls), 1)
    logger = SwagLogger()
    logger.setLevel(logging.INFO)  # disabled INFO logging would hide this crash
    stream = io.StringIO()
    handler = logging.StreamHandler(stream)
    handler.setFormatter(SwagFormatter(logger))
    logger.addHandler(handler)
    try:
      expression = ast.Expression(body=calls[0])
      eval(compile(expression, str(source_path), 'eval'), {'cloudlog': logger, 'self': controls, 'reference_service': 'modelV2'})
      record = json.loads(stream.getvalue())
    finally:
      handler.close()
    self.assertEqual(record['level'], 'INFO')
    self.assertEqual(record['msg']['event'], event)
    return record['msg']
  def test_startup_logs_selected_controller_without_crashing(self):
    for controller in (FordPathController(), FordPscmObserverPathController(), FordModelActionController()):
      with self.subTest(controller=type(controller).__name__):
        record = self.emit_controls_event('Ford path controller selected', SimpleNamespace(ford_path_controller=controller))
        self.assertEqual(record['controller'], type(controller).__name__)
  def test_candidate_diagnostics_identify_the_experiment_and_do_not_claim_calibration(self):
    controller = FordModelActionController()
    for active, valid in ((False, True), (True, True), (True, False)):
      controller.update(circle(.01), .005, 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-c1-feedback-v1')
      self.assertIs(record['calibration_approved'], False)
      self.assertEqual(record['command'][2:], [0., 0.])
      self.assertEqual(record['status'], controller.diagnostics['status'])
@@ -0,0 +1,193 @@
 import math
 from types import SimpleNamespace
 import numpy as np
 import pytest
 from opendbc.can import CANPacker, CANParser
 from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
 from openpilot.cereal import custom
 from openpilot.selfdrive.controls.lib.ford_path import FordPath
 from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
 def make_model(x, y, heading):
  return SimpleNamespace(position=SimpleNamespace(x=x, y=y), orientation=SimpleNamespace(z=heading))
 def circle(curvature):
  s = np.linspace(0., 60., 601)
  return make_model(np.sin(curvature*s)/curvature, (1-np.cos(curvature*s))/curvature, curvature*s)
 def straight(offset=0.):
  x = np.linspace(0., 60., 121)
  return make_model(x, np.full_like(x, offset), np.zeros_like(x))
 def test_selected_action_controls_heading_even_when_model_previews_another_turn():
  model = circle(.02)
  assert encode_model_action(model, 0., 20.).path_angle == 0.
  assert encode_model_action(model, -.004, 20.).path_angle == pytest.approx(-.08)
  assert encode_model_action(model, 0., 20.).path_offset > 0.
 def test_centering_information_is_independent_of_action_and_not_scaled_with_speed():
  for speed in (2., 7., 20., 35.):
    target = encode_model_action(straight(.4), 0., speed)
    assert target == FordPath(True, .4, 0., 0., 0.)
  for sign in (-1, 1):
    target = encode_model_action(circle(sign*.01), sign*.01, 20.)
    assert target.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=1e-6)
    assert target.path_angle == pytest.approx(sign*.2)  # No 10 m cap at highway speed.
 def test_three_control_states_are_sufficient_for_every_next_output():
  controller = ModelActionController()
  assert not hasattr(controller, '__dict__')
  for i in range(300):
    copied = ModelActionController()
    copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction
    model = straight(.2*math.sin(i*.1))
    kwargs = {'speed': 20., 'dt': .01}
    desired = .005*math.cos(i*.03)
    assert controller.update(model, desired, current_curvature=0., **kwargs) == copied.update(model, desired, current_curvature=0., **kwargs)
 def test_held_turn_releases_without_a_bias_tail_or_sign_reversal():
  for sign in (-1., 1.):
    controller = ModelActionController()
    for _ in range(400):
      out = controller.update(circle(sign*.01), sign*.01, current_curvature=sign*.01, speed=20., dt=.01)
    assert out.path_angle == pytest.approx(sign*.2)
    previous = np.array([out.path_offset, out.path_angle])
    for desired in sign*np.linspace(.01, 0., 101):
      out = controller.update(straight(), desired, current_curvature=desired, speed=20., dt=.01)
      values = np.array([out.path_offset, out.path_angle])
      assert (abs(values) <= abs(previous)+1e-8).all()
      assert (sign*values >= -1e-8).all()
      previous = values
    assert out == FordPath(True, 0., 0., 0., 0.)
 def test_current_model_replacement_leaves_only_independent_actuator_slew():
  controller = ModelActionController()
  for _ in range(150):
    controller.update(straight(1.), .04, current_curvature=.04, speed=20., dt=.01)
  for _ in range(25):
    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., current_curvature=0., speed=20., dt=.01)
  assert out == FordPath(True, 0., 0., 0., 0.)
@pytest.mark.parametrize('overrides', [{'active': False}, {'valid': False}, {'dt': .2}, {'speed': math.nan}])
 def test_invalid_or_inactive_input_clears_state_before_reengagement(overrides):
  controller = ModelActionController()
  for _ in range(100):
    controller.update(straight(.5), .01, current_curvature=.01, speed=20., dt=.01)
  kwargs = {'speed': 20., 'dt': .01, 'active': True, 'valid': True}
  kwargs.update(overrides)
  assert controller.update(straight(), 0., current_curvature=0., **kwargs) == FordPath()
  assert (controller.c0, controller.c1) == (0., 0.)
  assert controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.)
 def test_malformed_geometry_and_nonfinite_action_never_create_an_active_command():
  for model, desired in ((None, 0.), (straight(), math.nan), (straight(), math.inf)):
    assert not encode_model_action(model, desired, 20.).valid
 def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2():
  controller = ModelActionController()
  packer = CANPacker('ford_lincoln_base_pt')
  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
  bus = CanBus(fingerprint={0: {}})
  previous = np.zeros(2)
  for i in range(600):
    sign = 1. if i < 300 else -1.
    out = controller.update(straight(sign*8.), sign*.1, current_curvature=sign*.1, speed=30., dt=.01)
    fields = np.array([out.path_offset, out.path_angle])
    assert (abs(fields) <= [5.1100001, .5000001]).all()
    assert (abs(fields-previous) <= [.0500001, .0055001]).all()
    previous = fields
    message = custom.CarControlSP.new_message()
    message.fordLateralPath.pathOffset = out.path_offset
    message.fordLateralPath.pathAngle = out.path_angle
    packet = create_lat_ctl2_msg(packer, bus, 2, -message.fordLateralPath.pathOffset,
                                -message.fordLateralPath.pathAngle, out.curvature, out.curvature_rate, i % 16)
    parser.update([i*10_000_000, [packet]])
    decoded = parser.vl['LateralMotionControl2']
    assert decoded['LatCtlPathOffst_L_Actl'] == pytest.approx(-out.path_offset)
    assert decoded['LatCtlPath_An_Actl'] == pytest.approx(-out.path_angle)
    assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
 def test_short_path_holds_available_endpoint_without_extrapolation():
  model = make_model([0., 1.], [0., .1], [0., 0.])
  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, .2, 0., 0.)
 def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
  model = make_model([0., 1e308, -1e308], [0., 0., 0.], [0., 0., 0.])
  controller = ModelActionController()
  controller.update(straight(.4), .01, current_curvature=.01, speed=20., dt=.01)
  assert controller.update(model, .01, current_curvature=.01, speed=20., dt=.01) == FordPath()
  assert (controller.c0, controller.c1) == (0., 0.)
@pytest.mark.parametrize('value', [None, 'bad', 10**400])
@pytest.mark.parametrize('field', ['dt', 'speed', 'desired_curvature'])
 def test_malformed_numeric_input_resets_without_throwing(field, value):
  controller = ModelActionController()
  kwargs = {'speed': 20., 'dt': .01, 'desired_curvature': .01}
  controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs)
  kwargs[field] = value
  assert controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs) == FordPath()
  assert (controller.c0, controller.c1) == (0., 0.)
@pytest.mark.parametrize('model', [
  make_model([], [], []), make_model([0.], [0.], [0.]),
  make_model([0., 10.], [0.], [0., 0.]), make_model([0., 10.], [0., 0.], [0.]),
  make_model([0., 0.], [0., 0.], [0., 0.]),
  make_model([0., 10.], [0., math.nan], [0., 0.]), make_model([0., math.inf], [0., 0.], [0., 0.]),
  make_model([0., 10.], [0., 0.], [0., math.inf]),
  make_model([0., 10**400], [0., 0.], [0., 0.]),
  make_model([0., 10.], [0., 0.], [1e308, -1e308]),
 ])
 def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model):
  controller = ModelActionController()
  controller.update(straight(.4), .01, current_curvature=.01, speed=20., dt=.01)
  assert controller.update(model, .01, current_curvature=.01, speed=20., dt=.01) == FordPath()
  assert (controller.c0, controller.c1) == (0., 0.)
@pytest.mark.parametrize('field,value,valid', [
  ('speed', .2999, False), ('speed', .3, True), ('speed', 55., True), ('speed', 55.0001, False),
  ('desired_curvature', -1., True), ('desired_curvature', 1., True), ('desired_curvature', -1.0001, False),
  ('dt', .001999, False), ('dt', .002, True), ('dt', .1, True), ('dt', .100001, False), ('dt', 0., False),
 ])
 def test_domain_and_elapsed_time_boundaries(field, value, valid):
  kwargs = {'speed': 20., 'desired_curvature': .01, 'dt': .01}
  kwargs[field] = value
  assert ModelActionController().update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs).valid == valid
 def test_arc_station_not_forward_x_or_model_heading_determines_offset():
  x = np.array([0., 6., 12.])
  y = .4+x*.75
  target = encode_model_action(make_model(x, y, [2., -2., 1.]), -.01, 20.)
  # Arc length is 1.25*x on this line, so y(arc=7)=.4+.75*(7/1.25).
  assert target.path_offset == pytest.approx(4.6)
  assert target.path_angle == pytest.approx(-.2)
 def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
  model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
  assert encode_model_action(model, .01, 20.) == FordPath(True, .4, .2, 0., 0.)
  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)
@@ -0,0 +1,291 @@
 """Exercise the candidate through existing selection, publication and CAN code.
 Tests enable the candidate through controlsd's real startup selection.
 No hardware, IPC or CAN transmission is involved.
 """
 import ast
 from collections import defaultdict
 import json
 import math
 from pathlib import Path
 from types import SimpleNamespace
 import pytest
 from opendbc.can import CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.ford.carcontroller import CarController
 from opendbc.car.ford.fordcan import calculate_lat_ctl2_checksum
 from opendbc.car.ford.values import FordFlags
 from openpilot.cereal import custom
 from openpilot.selfdrive.car.helpers import convert_carControlSP
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
 from openpilot.selfdrive.controls.lib.ford_path import FordPath
 from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
 from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
 def update(controller, now=1., **overrides):
  kwargs = {'model': straight(.4), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now,
                'model_time': now, 'measurement_time': now, 'reference_time': now, 'active': True}
  kwargs.update(overrides)
  kwargs.setdefault('current_curvature', kwargs['desired_curvature'])  # Preserve feedforward-only compatibility probes.
  return controller.update(**kwargs)
@pytest.mark.parametrize('field', ['model_time', 'measurement_time', 'reference_time'])
@pytest.mark.parametrize('age', [.151, -.006])
 def test_stale_or_future_service_clears_commands_and_reengages_from_zero(field, age):
  controller = FordModelActionController()
  update(controller)
  assert update(controller, 1.01, **{field: 1.01-age}) == FordPath()
  assert controller.diagnostics['status'] == 'stale_input'
  assert update(controller, 1.02).path_offset == pytest.approx(.04)
@pytest.mark.parametrize('change,reason', [
  ({'now': 1.}, 'timing_reset'),
  ({'now': .99}, 'timing_reset'),
  ({'now': 1.001}, 'timing_reset'),
  ({'now': 1.101}, 'timing_reset'),
  ({'model_time': .999}, 'timing_reset'),
  ({'measurement_time': .999}, 'timing_reset'),
  ({'active': False}, 'inactive'),
  ({'valid': False}, 'invalid_service'),
  ({'model': None}, 'invalid_path'),
  ({'yaw_rate': math.nan}, 'nonfinite'),
  ({'yaw_rate': 3.01}, 'input_range'),
  ({'speed': 55.01}, 'input_range'),
  ({'desired_curvature': 1.01}, 'input_range'),
 ])
 def test_invalid_cycle_never_keeps_a_previous_active_request(change, reason):
  controller = FordModelActionController()
  update(controller)
  now = change.get('now', 1.01)
  assert update(controller, **dict(change, now=now)) == FordPath()
  assert controller.diagnostics['status'] == reason
  assert (controller.core.c0, controller.core.c1) == (0., 0.)
  assert update(controller, now+1.).path_angle == pytest.approx(.005)
@pytest.mark.parametrize('field', ['now', 'measurement_time', 'model_time', 'reference_time', 'speed', 'yaw_rate', 'desired_curvature'])
@pytest.mark.parametrize('value', [math.nan, math.inf, -math.inf, None])
 def test_nonfinite_input_never_raises_or_leaks_into_diagnostics(field, value):
  controller = FordModelActionController()
  update(controller)
  assert update(controller, **{field: value}) == FordPath()
  assert controller.diagnostics['status'] == 'nonfinite'
  json.dumps(controller.diagnostics, allow_nan=False)
 def test_repeated_measurements_do_not_freeze_slew_or_cache_invalid_model_geometry():
  controller = FordModelActionController()
  for i in range(10):
    result = update(controller, 1.+i*.01, measurement_time=1., model_time=1., reference_time=1.)
  assert result.path_offset == pytest.approx(.4)
  assert result.path_angle == pytest.approx(.05)
  broken = straight(.4)
  broken.position.y[5] = math.nan
  assert update(controller, 1.1, model=broken, model_time=1., measurement_time=1.) == FordPath()
  assert controller.diagnostics['status'] == 'invalid_path'
 def test_yaw_offset_does_not_change_the_base():
  controllers = [FordModelActionController() for _ in range(3)]
  variants = [{}, {'yaw_rate': .0072}, {'yaw_rate': -.0072}]
  for i in range(100):
    outputs = [update(c, 1.+i*.01, **kwargs) for c, kwargs in zip(controllers, variants, strict=True)]
    assert all(out == outputs[0] for out in outputs)
  assert outputs[0].path_angle == pytest.approx(.2)
 def test_reference_source_can_change_to_an_older_but_fresh_publication():
  controller = FordModelActionController()
  update(controller, reference_time=.99)
  assert update(controller, 1.01, reference_time=.98).valid
 def test_release_keeps_current_geometry_and_may_grow_c0_while_c1_decreases():
  for sign in (-1., 1.):
    controller = FordModelActionController()
    for i in range(100):
      before = update(controller, 1.+i*.01, model=circle(sign*.01), desired_curvature=sign*.005)
    for i in range(100):
      after = update(controller, 2.+i*.01, model=circle(sign*.02), desired_curvature=sign*.004)
    assert abs(after.path_offset) > abs(before.path_offset)
    assert abs(after.path_angle) < abs(before.path_angle)
    for i in range(100):
      released = update(controller, 3.+i*.01, model=circle(sign*.02), desired_curvature=0.)
    assert released.path_offset == after.path_offset
    assert released.path_angle == pytest.approx(0.)
 def _method(filename, class_name, method):
  tree = ast.parse(filename.read_text())
  cls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == class_name)
  return next(node for node in cls.body if isinstance(node, ast.FunctionDef) and node.name == method)
@pytest.fixture
 def pipeline():
  root = Path(__file__).resolve().parents[3]
  controls_file = root/'selfdrive/controls/controlsd.py'
  body = _method(controls_file, 'Controls', 'state_control').body
  # Execute the actual source choice, upstream limiter and Ford integration.
  selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']")
  limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and
                 isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature')
  branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
  call = compile(ast.Module(body=[selection, limiter, branch], type_ignores=[]), str(controls_file), 'exec')
  publication_file = root/'sunnypilot/selfdrive/controls/controlsd_ext.py'
  body = _method(publication_file, 'ControlsExt', 'state_control_ext').body
  publish = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'ford_path') or
             (isinstance(n, ast.If) and ast.unparse(n.test) == 'ford_path is not None')]
  assert len(publish) == 2
  publication = compile(ast.Module(body=publish, type_ignores=[]), str(publication_file), 'exec')
  return call, publication
 class Subscriptions:
  frame = 1
  def __init__(self, maneuver):
    self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True, 'carStateSP': True}
    self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000}
    self.failed = set()
    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)}
  def __getitem__(self, service):
    return self.messages[service]
  def all_checks(self, services):
    return not self.failed.intersection(services) and all(self.valid.get(s, True) for s in services)
@pytest.mark.parametrize('maneuver', [False, True])
 def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver):
  call, publication = pipeline
  sm = Subscriptions(maneuver)
  controls = startup()
  controller = controls.ford_path_controller
  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
  model = straight(.4)
  model.action = SimpleNamespace(desiredCurvature=.1)
  cc = structs.CarControl(latActive=True)
  cs = SimpleNamespace(vEgo=20., yawRate=-.0072, canValid=True, steeringPressed=False, steeringTorque=0.)
  environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
                     'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
  exec(call, environment)
  expected_curvature = (-1 if maneuver else 1)*.000125
  assert controls.desired_curvature == pytest.approx(expected_curvature)
  assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
  assert controls.ford_path.path_offset == pytest.approx(.04)
  assert cc.latActive and cc.actuators.curvature == 0.
  assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
  cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
  downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
  vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
                            lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
  for i, fail in enumerate((False, True)):
    if fail:
      sm.failed.add('modelV2')
      exec(call, environment)
      assert not cc.latActive and controls.ford_path == FordPath()
    msg = custom.CarControlSP.new_message()
    exec(publication, {'self': controls, 'CC_SP': msg})
    _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, (i+1)*10_000_000)
    parser.update([(i+1)*10_000_000, packets])
    wire = parser.vl['LateralMotionControl2']
    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
    assert wire['LatCtl_D2_Rq'] == (0 if fail else 2)
@pytest.mark.parametrize('maneuver', [False, True])
@pytest.mark.parametrize('failed', ['carState', 'modelV2', 'vehicleParameters', 'lateralManeuverPlan'])
 def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
  sm = Subscriptions(maneuver)
  sm.failed.add(failed)
  controls = startup()
  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
  cc = structs.CarControl(latActive=True)
  cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
  model = straight()
  model.action = SimpleNamespace(desiredCurvature=.1)
  exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
                         'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
  assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)
@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
@@ -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])
@@ -0,0 +1,100 @@
 """Exercise real startup selection and Sunnylink writes without starting hardware."""
 import ast
 import base64
 import itertools
 from pathlib import Path
 from types import SimpleNamespace
 import pytest
 from opendbc.car.ford.values import FordFlags
 from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
 from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
 def car_params(**overrides):
  return SimpleNamespace(**({'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1',
                             'carFw': []} | overrides))
 def startup(cp=None, params=None):
  filename = Path(__file__).resolve().parents[1]/'controlsd.py'
  tree = ast.parse(filename.read_text())
  cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Controls')
  body = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == '__init__').body
  start = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_pscm_observer')
  end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path')
  if params is None:
    params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController')
  controls = SimpleNamespace(CP=cp or car_params(), params=params)
  environment = {'self': controls, 'FordFlags': FordFlags, 'FordPath': FordPath,
                 'FordPathController': FordPathController, 'FordPscmObserverPathController': FordPscmObserverPathController,
                 'FordModelActionController': FordModelActionController,
                 'select_model_action_controller': select_model_action_controller,
                 'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
  exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment)
  return controls
@pytest.mark.parametrize('candidate,observer', list(itertools.product((False, True), repeat=2)))
 def test_actual_startup_priority(candidate, observer):
  settings = {'FordModelActionController': candidate, 'FordPscmObserver': observer}
  selected = startup(params=SimpleNamespace(get_bool=settings.__getitem__))
  previous = FordPscmObserverPathController if observer else FordPathController
  expected = FordModelActionController if candidate else previous
  assert type(selected.ford_path_controller) is expected
  assert selected.ford_model_action == candidate
  assert selected.ford_path == FordPath()
@pytest.mark.parametrize('overrides', [{'brand': 'tesla'}, {'flags': 0}, {'carFingerprint': 'FORD_F_150_MK14'}])
@pytest.mark.parametrize('observer', [False, True])
 def test_other_vehicles_keep_their_previous_selection(overrides, observer):
  settings = {'FordModelActionController': False, 'FordPscmObserver': observer}
  params = SimpleNamespace(get_bool=settings.__getitem__)
  before = startup(car_params(**overrides), params)
  settings['FordModelActionController'] = True
  after = startup(car_params(**overrides), params)
  assert type(after.ford_path_controller) is type(before.ford_path_controller)
  assert not after.ford_model_action
@pytest.mark.parametrize('firmware', [[], [SimpleNamespace(ecu='eps', fwVersion=b'other')]])
 def test_candidate_does_not_depend_on_eps_firmware_query(firmware):
  assert isinstance(startup(car_params(carFw=firmware)).ford_path_controller, FordModelActionController)
@pytest.mark.parametrize('observer', [False, True])
 def test_sunnylink_write_takes_effect_on_restart_and_restores_stored_selection(tmp_path, monkeypatch, observer):
  from openpilot.sunnypilot.sunnylink import utils
  params = Params(str(tmp_path))
  monkeypatch.setattr(utils, 'Params', lambda: params)
  assert params.get_default_value('FordModelActionController') is False
  assert params.get_type('FordModelActionController') == ParamKeyType.BOOL
  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.PERSISTENT)
  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.BACKUP)
  params.put_bool('FordPscmObserver', observer, block=True)
  old = startup(params=params)
  assert not isinstance(old.ford_path_controller, FordModelActionController)
  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'true').decode())
  enabled = startup(params=params)
  assert isinstance(enabled.ford_path_controller, FordModelActionController)
  assert not isinstance(old.ford_path_controller, FordModelActionController)
  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'false').decode())
  assert isinstance(enabled.ford_path_controller, FordModelActionController)
  assert type(startup(params=params).ford_path_controller) is type(old.ford_path_controller)
  assert params.get_bool('FordPscmObserver') == observer
 def test_stored_retired_toggle_cannot_enable_the_candidate(tmp_path):
  params = Params(str(tmp_path))
  Path(params.get_param_path('FordVirtualAngleController')).write_text('1')
  assert b'FordVirtualAngleController' not in params.all_keys()
  assert params.get_bool('FordModelActionController') is False
  assert type(startup(params=params).ford_path_controller) is FordPathController
  params.put_bool('FordModelActionController', True, block=True)
  params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
  assert not Path(params.get_param_path('FordVirtualAngleController')).exists()
  assert params.get_bool('FordModelActionController') is True
@@ -0,0 +1,422 @@
 import math
 from types import SimpleNamespace
 import numpy as np
 from openpilot.cereal import custom
 from openpilot.selfdrive.car.helpers import convert_carControlSP
 from openpilot.selfdrive.controls.lib.ford_path import (DBC_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPath, FordPathController,
                                                        FordPscmObserver, FordPscmObserverPathController, FordPscmState,
                                                        _bounded_feedback, _encode_path, _model_path, _predicted_pose,
                                                        _pscm_contributions, _relative_pose)
 def _path(curvature: float, speed: float = 8.0):
  t = np.linspace(0.0, 3.0, 61)
  distance = speed * t
  heading = curvature * distance
  x = np.zeros_like(distance)
  y = np.zeros_like(distance)
  for i in range(1, len(distance)):
    ds = distance[i] - distance[i - 1]
    average_heading = 0.5 * (heading[i] + heading[i - 1])
    x[i] = x[i - 1] + ds * math.cos(average_heading)
    y[i] = y[i - 1] + ds * math.sin(average_heading)
  return SimpleNamespace(
    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
    orientation=SimpleNamespace(z=heading.tolist()),
  )
 def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
  t = np.linspace(0.0, 3.0, 61)
  distance = speed * t
  curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
  heading = np.zeros_like(distance)
  x = np.zeros_like(distance)
  y = np.zeros_like(distance)
  for i in range(1, len(distance)):
    ds = distance[i] - distance[i - 1]
    heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
    average_heading = 0.5 * (heading[i] + heading[i - 1])
    x[i] = x[i - 1] + ds * math.cos(average_heading)
    y[i] = y[i - 1] + ds * math.sin(average_heading)
  return SimpleNamespace(
    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
    orientation=SimpleNamespace(z=heading.tolist()),
  )
 def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
  return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
 def _equivalent_curvature(command) -> float:
  return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
 def test_gentle_path_uses_only_c2():
  command = _command(_path(0.004, speed=20.0), 0.004, current_curvature=0.004, v_ego=20.0)
  assert command.valid
  assert command.path_offset == 0.0
  assert command.path_angle == 0.0
  assert np.isclose(command.curvature, 0.004, atol=1e-6)
  assert command.curvature_rate == 0.0
 def test_gentle_path_uses_only_c2_when_model_and_action_disagree():
  command = _command(_path(0.005), 0.002, current_curvature=0.005)
  assert command.path_offset == 0.0
  assert command.path_angle == 0.0
  assert np.isclose(command.curvature, 0.002, atol=1e-6)
 def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
  controller = FordPathController(dt=1.0)
  command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
  assert command.path_offset > 0.0
  assert command.path_angle > 0.0
  assert command.curvature < 0.012
  assert command.curvature_rate == 0.0
 def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
  constant = _command(_path(0.012), 0.012)
  growing = _command(_changing_path(0.0, 0.04), 0.012)
  assert _equivalent_curvature(growing) > _equivalent_curvature(constant)
  assert constant.curvature_rate == 0.0
  assert growing.curvature_rate == 0.0
 def test_local_tracking_error_corrects_without_replacing_forward_pose():
  model = _changing_path(0.0, 0.04)
  local_curvature = 0.5 * 0.04 * 2.0 / 7.0
  aligned = _command(model, 0.012, current_curvature=local_curvature)
  under = _command(model, 0.012, current_curvature=0.0)
  assert aligned.path_offset > 0.0
  assert aligned.path_angle > 0.0
  assert under.path_offset > aligned.path_offset
  assert under.path_angle > aligned.path_angle
 def test_large_maneuver_uses_fast_pose_and_zeros_c2():
  command = _command(_path(0.04), 0.04)
  assert command.path_offset > 0.5
  assert command.path_angle > 0.2
  assert command.curvature == 0.0
  assert command.curvature_rate == 0.0
 def test_model_pose_can_trigger_maneuver_when_action_is_late():
  command = _command(_path(0.04), 0.002)
  assert command.path_offset > 0.5
  assert command.path_angle > 0.2
  assert command.curvature == 0.0
 def test_gentle_model_pose_does_not_replace_a_collapsed_action():
  command = _command(_path(0.005), 0.0, current_curvature=0.005)
  assert command.path_offset == 0.0
  assert command.path_angle == 0.0
  assert command.curvature == 0.0
 def test_changing_gentle_curve_keeps_upstream_strength_c2():
  command = _command(_changing_path(0.0, 0.008), 0.004, current_curvature=0.0)
  assert np.isclose(command.curvature, 0.004)
  assert command.path_offset == 0.0
  assert command.path_angle == 0.0
 def test_action_only_maneuver_cannot_invent_large_model_pose():
  command = _command(_path(0.002), 0.04)
  assert 0.0 < command.path_offset < 0.1
  assert 0.0 < command.path_angle < 0.03
  assert command.curvature == 0.0
 def test_nearby_demands_blend_continuously_without_a_mode_threshold():
  low = _command(_path(0.0119), 0.0119)
  high = _command(_path(0.0121), 0.0121)
  assert abs(high.path_offset - low.path_offset) < 0.05
  assert abs(high.path_angle - low.path_angle) < 0.03
  assert abs(high.curvature - low.curvature) < 0.001
 def test_leaving_c2_normal_band_does_not_drop_total_authority():
  normal = _command(_path(0.006), 0.006)
  transition = _command(_path(0.0061), 0.0061)
  assert transition.curvature <= normal.curvature
  assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
 def test_low_speed_still_uses_available_model_pose():
  command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
  assert command.path_offset > 0.0
  assert command.path_angle > 0.0
 def test_higher_speed_advances_predicted_pose_and_extends_heading_horizon():
  model = _changing_path(0.0, 0.015, speed=20.0)
  slow = _command(model, 0.012, v_ego=7.0)
  fast = _command(model, 0.012, v_ego=20.0)
  assert fast.path_offset > slow.path_offset
  assert fast.path_angle > slow.path_angle
 def test_short_model_uses_available_endpoint():
  model = _path(0.04, speed=1.0)
  command = _command(model, 0.04, v_ego=1.0)
  assert command.valid
  assert command.path_offset > 0.0
  assert command.path_angle > 0.0
 def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
  controller = FordPathController(dt=0.01)
  for _ in range(20):
    assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
  outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
  assert 0.0 < outputs[0].curvature < 0.004
  assert outputs[0].path_offset > 0.0
  assert outputs[0].path_angle > 0.0
  assert outputs[-1].curvature == 0.0
 def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
  controller = FordPathController(dt=0.01)
  for _ in range(20):
    controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
  outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
  assert 0.0 < outputs[0].curvature < 0.004
  assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
  assert outputs[-1].path_offset == 0.0
  assert outputs[-1].path_angle == 0.0
 def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
  controller = FordPathController(dt=0.01)
  normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
  entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
  entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
  assert np.all(np.diff(entry_authority) >= -1e-9)
  assert entry_authority[0] >= _equivalent_curvature(normal)
  exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
  exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
  assert np.all(np.diff(exit_authority) <= 1e-9)
  assert np.all(exit_authority >= 0.004 - 1e-9)
 def test_measured_tracking_error_closes_bidirectionally_without_abandoning_the_turn():
  model = _path(0.04)
  under = _command(model, 0.04, current_curvature=0.005)
  on_target = _command(model, 0.04, current_curvature=0.04)
  over = _command(model, 0.04, current_curvature=0.05)
  assert under.path_offset > on_target.path_offset
  assert under.path_angle > on_target.path_angle
  assert 0.0 < over.path_offset < on_target.path_offset
  assert 0.0 < over.path_angle < on_target.path_angle
 def test_gentle_curve_does_not_add_fast_tracking_trim():
  model = _path(0.004)
  under = _command(model, 0.004, current_curvature=0.002)
  on_target = _command(model, 0.004, current_curvature=0.004)
  over = _command(model, 0.004, current_curvature=0.006)
  assert under.path_offset == on_target.path_offset == over.path_offset == 0.0
  assert under.path_angle == on_target.path_angle == over.path_angle == 0.0
  assert np.allclose([under.curvature, on_target.curvature, over.curvature], 0.004, atol=2e-6)
 def test_overshoot_trim_cannot_erase_a_modeled_turn():
  model = _path(0.04)
  on_target = _command(model, 0.04, current_curvature=0.04)
  over = _command(model, 0.04, current_curvature=0.06)
  assert over.path_offset > 0.95 * on_target.path_offset
  assert over.path_angle > 0.9 * on_target.path_angle
 def test_corrupt_measured_curvature_cannot_reverse_a_modeled_turn():
  command = _command(_path(0.04), 0.04, current_curvature=0.5)
  assert command.path_offset > 0.0
  assert command.path_angle > 0.0
  assert command.curvature == 0.0
 def test_feedback_preserves_half_lsb_feedforward_direction():
  for feedforward, resolution in ((0.006, 0.01), (0.0004, 0.0005)):
    result = feedforward + _bounded_feedback(feedforward, -1.0, resolution, 1.0)
    assert result >= 0.5 * resolution
 def test_recent_curvature_trend_advances_vehicle_pose_without_a_response_gain():
  model = _model_path(_path(0.04))
  assert model is not None
  constant = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.0, v_ego=8.0)
  rising = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.01, v_ego=8.0)
  assert 0.0 < rising.path_offset < constant.path_offset
  assert 0.0 < rising.path_angle < constant.path_angle
 def test_model_path_exit_zeros_lingering_c2_and_countersteers():
  command = _command(_path(0.0), 0.004, current_curvature=0.006)
  assert command.path_offset <= 0.0
  assert command.path_angle < 0.0
  assert command.curvature == 0.0
 def test_model_path_reversal_zeros_opposing_lingering_c2():
  command = _command(_path(-0.004), 0.004, current_curvature=0.002)
  assert command.path_offset < 0.0
  assert command.path_angle < 0.0
  assert command.curvature == 0.0
 def test_s_turn_reverses_model_pose_without_slow_c2():
  controller = FordPathController(dt=0.05)
  for _ in range(10):
    controller.update(_path(0.04), 0.04, v_ego=8.0)
  outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)]
  assert all(command.curvature == 0.0 for command in outputs)
  assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0)
  assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0)
  assert outputs[-1].path_offset < 0.0
  assert outputs[-1].path_angle < 0.0
 def test_output_limits_and_rates_are_bounded():
  controller = FordPathController()
  outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)]
  assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs)
  assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs)
  assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs)
  assert np.max(np.abs(np.diff([command.path_offset for command in outputs]))) <= 0.04 + 1e-9
  assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
 def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
  horizon = 7.0
  for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
    model = _path(curvature)
    command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
    path = _model_path(model)
    assert path is not None
    advance = 0.1 * horizon
    model_offset, model_angle = _relative_pose(advance + horizon, path,
                                                _predicted_pose(advance, curvature, 0.0))
    assert command.path_angle == angle_limit
    assert np.isclose(command.path_offset + horizon * command.path_angle,
                      model_offset + horizon * model_angle)
 def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
  controller = FordPathController(dt=0.01)
  for _ in range(20):
    active = controller.update(_path(0.04), 0.04, v_ego=8.0)
  invalid = controller.update(None, 0.0, v_ego=8.0)
  assert invalid.valid
  assert abs(invalid.path_offset) < abs(active.path_offset)
  assert abs(invalid.path_angle) < abs(active.path_angle)
  assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid
 def test_sunnypilot_path_message_round_trip():
  message = custom.CarControlSP.new_message()
  message.fordLateralPath.pathOffset = 0.3
  message.fordLateralPath.pathAngle = -0.2
  message.fordLateralPath.curvature = 0.008
  message.fordLateralPath.curvatureRate = -0.0004
  message.fordLateralPath.valid = True
  path = convert_carControlSP(message.as_reader()).fordLateralPath
  assert np.isclose(path.pathOffset, 0.3)
  assert np.isclose(path.pathAngle, -0.2)
  assert np.isclose(path.curvature, 0.008)
  assert np.isclose(path.curvatureRate, -0.0004)
  assert path.valid
 def test_pscm_observer_mirrors_exact_250hz_slew_and_c3_target():
  observer = FordPscmObserver()
  observer.set_command(FordPath(True, 1.0, 0.5, 0.0, 0.001))
  observer.advance(1.0)
  assert np.isclose(observer.state.path_offset, 1.0)
  assert np.isclose(observer.state.path_angle, 0.100006103515625)
  assert np.isclose(observer.state.curvature, 0.0030059814453125)
 def test_pscm_observer_tracks_wire_quantized_commands():
  observer = FordPscmObserver()
  observer.set_command(FordPath(True, 0.006, 0.0004, 0.000011, 0.0))
  assert observer.command.path_offset == 0.01
  assert observer.command.path_angle == 0.0005
  assert observer.command.curvature == 0.00002
 def test_pscm_c2_contribution_is_speed_scheduled():
  state = FordPscmObserver().state
  state = type(state)(curvature=0.004)
  low = _pscm_contributions(state, 5.0)[2]
  high = _pscm_contributions(state, 20.0)[2]
  assert high > low * 10.0
 def test_pscm_observer_fills_missing_gentle_c2_with_fast_fields():
  controller = FordPscmObserverPathController(dt=0.01)
  command = controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
                              v_ego=20.0, v_ego_raw=20.0)
  assert command.path_offset > 0.0
  assert command.path_angle > 0.0
  assert command.curvature > 0.0
 def test_pscm_observer_uses_c0_only_after_c1_reaches_its_effective_limit():
  controller = FordPscmObserverPathController(dt=0.01)
  small = controller._command_for_state(FordPath(True, 0.2, 0.0, 0.0, 0.0), 8.0)
  large = controller._command_for_state(FordPath(True, 1.0, 0.5, 0.0, 0.0), 8.0)
  assert small.path_offset == 0.0
  assert small.path_angle > 0.0
  assert large.path_offset > 0.0
  assert large.path_angle == 0.349609375 / 10.0
 def test_pscm_observer_preserves_c2_residual_across_c0_c1_headroom():
  controller = FordPscmObserverPathController(dt=0.01)
  target = FordPath(True, 0.0, 0.0, 0.004, 0.0)
  command = controller._command_for_state(target, 20.0)
  target_contribution = sum(_pscm_contributions(FordPscmState(curvature=target.curvature), 20.0))
  command_contributions = _pscm_contributions(FordPscmState(command.path_offset, command.path_angle), 20.0)
  assert np.isclose(sum(command_contributions), target_contribution)
  controller.observer.state = FordPscmState(curvature=0.004)
  unwind = controller._command_for_state(FordPath(valid=True), 20.0)
  unwind_contributions = _pscm_contributions(FordPscmState(unwind.path_offset, unwind.path_angle), 20.0)
  lingering_c2 = _pscm_contributions(controller.observer.state, 20.0)[2]
  assert np.isclose(sum(unwind_contributions) + lingering_c2, 0.0)
 def test_pscm_observer_unloads_fast_residual_as_c2_loads():
  controller = FordPscmObserverPathController(dt=0.01)
  outputs = [controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
                               v_ego=20.0, v_ego_raw=20.0) for _ in range(200)]
  assert outputs[0].path_angle > outputs[-1].path_angle >= 0.0
  assert controller.observer.state.curvature > 0.003
 def test_pscm_observer_counters_lingering_c2_during_model_exit():
  controller = FordPscmObserverPathController(dt=0.01)
  for _ in range(200):
    controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
                      v_ego=20.0, v_ego_raw=20.0)
  command = controller.update(_path(0.0, speed=20.0), 0.0, current_curvature=0.004,
                              v_ego=20.0, v_ego_raw=20.0)
  assert command.path_angle < 0.0
  assert command.curvature < controller.observer.state.curvature
 def test_pscm_observer_avoids_ineffective_c0_c1_windup():
  controller = FordPscmObserverPathController(dt=1.0)
  command = controller.update(_path(0.2), 0.2, v_ego=8.0, v_ego_raw=8.0)
  assert abs(command.path_offset) <= 1.0
  assert abs(command.path_angle) <= 0.349609375 / 10.0
@@ -32,7 +32,14 @@ from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
 from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
 from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
 from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
  AssistCategory,
  AssistedDrivingMilestones,
  MilestoneEvent,
  MilestoneStore,
 )
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.system.statsd import statlog
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -88,7 +95,8 @@ class SelfdriveD(CruiseHelper):
    self.big_model_ready_t = 0.
    # Setup sockets
-    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
                                  ['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
    self.gps_location_service = get_gps_location_service(self.params)
    self.gps_packets = [self.gps_location_service]
@@ -127,6 +135,7 @@ class SelfdriveD(CruiseHelper):
      self.params.remove("ExperimentalMode")
    self.CS_prev = car.CarState.new_message()
    self.car_state_log_mono_time = 0
    self.AM = AlertManager()
    self.events = Events()
@@ -137,6 +146,11 @@ class SelfdriveD(CruiseHelper):
    self.cruise_mismatch_counter = 0
    self.last_steering_pressed_frame = 0
    self.distance_traveled = 0
    self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
    self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
    self.assisted_driving_milestone_drive_id = ""
    self._milestone_event: MilestoneEvent | None = None
    self._milestone_event_expires_ns = 0
    self.last_functional_fan_frame = 0
    self.events_prev = []
    self.logged_comm_issue = None
@@ -528,6 +542,8 @@ class SelfdriveD(CruiseHelper):
  def data_sample(self):
    _car_state = messaging.recv_one(self.car_state_sock)
    CS = _car_state.carState if _car_state else self.CS_prev
    if _car_state is not None:
      self.car_state_log_mono_time = _car_state.logMonoTime
    self.sm.update(0)
@@ -646,6 +662,31 @@ class SelfdriveD(CruiseHelper):
      self.pm.send('onroadEventsSP', ce_send_sp)
    self.events_sp_prev = self.events_sp.names.copy()
  def publish_assisted_driving_milestones(self, now_ns: int, event: MilestoneEvent | None) -> None:
    if event is not None:
      self._milestone_event = event
      self._milestone_event_expires_ns = now_ns + 1_000_000_000
    elif now_ns >= self._milestone_event_expires_ns:
      self._milestone_event = None
    if event is None and self.sm.frame % 10 != 0:
      return
    snapshot = self.assisted_driving_milestones.snapshot()
    msg = messaging.new_message("assistedDrivingMilestoneState")
    msg.valid = True
    state = msg.assistedDrivingMilestoneState
    state.enabled = self.assisted_driving_milestones_enabled
    state.madsDistanceMeters = snapshot.distances_meters[AssistCategory.MADS]
    state.fullAssistDistanceMeters = snapshot.distances_meters[AssistCategory.FULL_ASSIST]
    if self._milestone_event is not None:
      state.event.id = self._milestone_event.event_id
      state.event.category = self._milestone_event.category.value
      state.event.distanceMeters = self._milestone_event.distance_meters
      state.event.previousDistanceMeters = self._milestone_event.previous_distance_meters
      state.event.unit = self._milestone_event.unit.value
    self.pm.send("assistedDrivingMilestoneState", msg)
  def step(self):
    CS = self.data_sample()
    self.update_events(CS)
@@ -655,6 +696,28 @@ class SelfdriveD(CruiseHelper):
      self.mads.update(CS)
    self.update_alerts(CS)
    now_ns = time.monotonic_ns()
    if not self.assisted_driving_milestone_drive_id:
      self.assisted_driving_milestone_drive_id = self.params.get("CurrentRoute") or ""
      self.assisted_driving_milestones.set_drive_id(self.assisted_driving_milestone_drive_id)
    car_control = self.sm['carControl']
    milestone_event = self.assisted_driving_milestones.update(
      self.car_state_log_mono_time,
      CS.vEgo,
      lat_active=car_control.latActive,
      long_active=car_control.longActive,
      is_metric=self.is_metric,
      enabled=self.assisted_driving_milestones_enabled,
    )
    if milestone_event is not None:
      cloudlog.event("assisted_driving_milestone_reached",
                     event_id=milestone_event.event_id,
                     category=milestone_event.category.value,
                     distance_meters=milestone_event.distance_meters)
      statlog.gauge(f"assisted_driving_milestone.{milestone_event.category.value}.meters",
                    milestone_event.distance_meters)
    self.publish_assisted_driving_milestones(now_ns, milestone_event)
    self.button_state_tracker.update(CS)
    self.publish_selfdriveState(CS)
@@ -667,6 +730,7 @@ class SelfdriveD(CruiseHelper):
      self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
      self.personality = self.params.get("LongitudinalPersonality", return_default=True)
      self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
      self.mads.read_params()
      time.sleep(0.1)
@@ -680,6 +744,7 @@ class SelfdriveD(CruiseHelper):
        self.step()
        self.rk.monitor_time()
    finally:
      self.assisted_driving_milestones.close()
      e.set()
      t.join()
@@ -1,5 +1,8 @@
 import os
 import pyray as rl
 import openpilot.cereal.messaging as messaging
 from openpilot.common.hardware import PC
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
@@ -61,7 +64,8 @@ class MiciMainLayout(Scroller):
    # Start onboarding if terms or training not completed, make sure to push after self
    self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
-    if not self._onboarding_window.completed:
+    skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
    if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
      gui_app.push_widget(self._onboarding_window)
    # initialize correct onroad layout
@@ -119,6 +123,8 @@ class MiciMainLayout(Scroller):
        self._onroad_time_delay = rl.get_time()
      else:
        self._scroll_to(self._home_layout)
        if hasattr(self._home_layout, "request_drive_summary"):
          self._home_layout.request_drive_summary()
    # FIXME: these two pops can interrupt user interacting in the settings
    if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
@@ -47,6 +47,7 @@ class TogglesLayoutMici(NavScroller):
    is_metric_toggle = BigParamControl("use metric units", "IsMetric")
    ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
    always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
    milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
    record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
    record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
    enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
@@ -57,6 +58,7 @@ class TogglesLayoutMici(NavScroller):
      is_metric_toggle,
      ldw_toggle,
      always_on_dm_toggle,
      milestone_celebrations_toggle,
      record_front,
      record_mic,
      enable_openpilot,
@@ -68,6 +70,7 @@ class TogglesLayoutMici(NavScroller):
      ("IsMetric", is_metric_toggle),
      ("IsLdwEnabled", ldw_toggle),
      ("AlwaysOnDM", always_on_dm_toggle),
      ("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
      ("RecordFront", record_front),
      ("RecordAudio", record_mic),
      ("OpenpilotEnabledToggle", enable_openpilot),
@@ -20,6 +20,7 @@ AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus
 ALERT_MARGIN = 18
 ALERT_BACKGROUND_OPACITY = 0.90
 ALERT_FONT_SMALL = 66 - 50
 ALERT_FONT_BIG = 88 - 40
@@ -279,7 +280,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
  def _draw_background(self, alert: Alert) -> None:
    # draw top gradient for alert text at top
    color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
-    color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
+    color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
    translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
    small_alert_height = round(self._rect.height * 0.583) # 140px at mici height
@@ -19,10 +19,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
 from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum
 MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
 if gui_app.sunnypilot_ui():
  from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
  from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
  if MILESTONE_CELEBRATION_ENABLED:
    from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
 OpState = log.SelfdriveState.OpenpilotState
 CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
 NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
@@ -156,6 +161,7 @@ class AugmentedRoadView(CameraView):
    self._alert_renderer = AlertRenderer()
    self._driver_state_renderer = DriverStateRenderer()
    self._confidence_ball = ConfidenceBall()
    self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
    self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                       text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
                                       alignment=TextAlignment.CENTER,
@@ -223,6 +229,12 @@ class AugmentedRoadView(CameraView):
    alert_to_render, not_animating_out = self._alert_renderer.will_render()
    if self._milestone_celebration is not None:
      if alert_to_render is not None:
        self._milestone_celebration.cancel_for_alert()
      else:
        self._milestone_celebration.render(self._content_rect)
    # Hide DMoji when disengaged unless AlwaysOnDM is enabled
    should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
                         (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
@@ -247,7 +259,6 @@ class AugmentedRoadView(CameraView):
    self._confidence_ball.render(self.rect)
    self._bookmark_icon.render(self.rect)
  def _switch_stream_if_needed(self, sm):
    if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
      v_ego = sm['carState'].vEgo
@@ -355,10 +366,12 @@ class AugmentedRoadView(CameraView):
    return self._cached_matrix
  def show_event(self):
    super().show_event()
    if gui_app.sunnypilot_ui():
      ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
  def hide_event(self):
    super().hide_event()
    if gui_app.sunnypilot_ui():
      ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
@@ -24,14 +24,8 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
 SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
 FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)
 AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
 DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
 VOLUME_BASE = 20
 if HARDWARE.get_device_type() == "tizi":
  AMBIENT_DB = 30
  VOLUME_BASE = 10
 AudibleAlert = log.SelfdriveState.AudibleAlert
 AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
@@ -53,6 +47,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
  AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),
  AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
  AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),
  AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
  AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
@@ -60,6 +55,14 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
  **sound_list_sp,
 }
 def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
  ambient_db = 30 if device_type in ("mici", "tizi") else 26
  volume_base = 10 if device_type in ("mici", "tizi") else 20
  volume_boost = 1.5 if device_type == "mici" else 1.0
  volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
  return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
 def check_selfdrive_timeout_alert(sm):
  ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
@@ -74,6 +77,7 @@ class Soundd(QuietMode):
  def __init__(self):
    super().__init__()
    self.device_type = HARDWARE.get_device_type()
    self.load_sounds()
    self.current_alert = AudibleAlert.none
@@ -85,6 +89,7 @@ class Soundd(QuietMode):
    self.selfdrive_timeout_alert = False
    self.pending_stop = False
    self.last_milestone_event_id = 0
    self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
@@ -164,9 +169,19 @@ class Soundd(QuietMode):
      self.update_alert(AudibleAlert.none)
      self.selfdrive_timeout_alert = False
  def update_milestone_alert(self, sm):
    if not sm.updated['assistedDrivingMilestoneState']:
      return
    milestone_state = sm['assistedDrivingMilestoneState']
    event_id = milestone_state.event.id
    if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
      return
    self.last_milestone_event_id = event_id
    if self.current_alert == AudibleAlert.none and not self.enabled:
      self.update_alert(AudibleAlert.complete)
  def calculate_volume(self, weighted_db):
-    volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
+    return calculate_volume_for_device(weighted_db, self.device_type)
    return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
  @retry(attempts=10, delay=3)
  def get_stream(self, sd):
@@ -180,7 +195,7 @@ class Soundd(QuietMode):
    import sounddevice as sd
    micd.patch_sounddevice(sd)
-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
    with self.get_stream(sd) as stream:
      rk = Ratekeeper(20)
@@ -198,6 +213,7 @@ class Soundd(QuietMode):
            self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
        self.get_audible_alert(sm)
        self.update_milestone_alert(sm)
        # Ramp up immediate warning sound over 4s
        if self.current_alert == AudibleAlert.warningImmediate:
@@ -5,19 +5,79 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import math
 import time
 import pyray as rl
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
-from openpilot.system.ui.lib.application import FontWeight
+from openpilot.system.ui.lib.application import FontWeight, TextAlignment
-from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
 METERS_PER_MILE = 1609.344
 METERS_PER_KILOMETER = 1000.0
 SUMMARY_DURATION_SECONDS = 10.0
 SUMMARY_WAIT_SECONDS = 3.0
 def _nonnegative_float(value) -> float:
  try:
    return max(0.0, float(value))
  except (TypeError, ValueError):
    return 0.0
 class MiciHomeLayoutSP(MiciHomeLayout):
  def __init__(self):
    super().__init__()
    self._openpilot_label = UnifiedLabel("sunnypilot", font_size=88, font_weight=FontWeight.AUDIOWIDE, max_width=480, wrap_text=False)
    initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
    self._last_summary_id = initial_summary.get("id", 0)
    self._summary_wait_until = 0.0
    self._summary_visible_until = 0.0
    self._drive_summary = {}
  def request_drive_summary(self) -> None:
    self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
  def _render(self, _: rl.Rectangle) -> None:
    super()._render(_)
    now = time.monotonic()
    if now < self._summary_wait_until:
      summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
      summary_id = summary.get("id", 0)
      if summary_id and summary_id != self._last_summary_id:
        self._last_summary_id = summary_id
        distances = summary.get("distancesMeters", {})
        enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
        if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
          self._drive_summary = summary
          self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
        self._summary_wait_until = 0.0
    if now < self._summary_visible_until:
      self._draw_drive_summary(_)
  def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
    distances = self._drive_summary.get("distancesMeters", {})
    metric = self._drive_summary.get("unit") == "metric"
    meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
    unit = "KM" if metric else "MI"
    mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
    full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
    rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
    gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
              font_weight=FontWeight.SEMI_BOLD, alignment=TextAlignment.CENTER)
    gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
              color=rl.Color(255, 255, 255, 184), alignment=TextAlignment.CENTER)
    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
              color=rl.Color(255, 255, 255, 184), alignment=TextAlignment.CENTER)
    gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
              font_weight=FontWeight.DISPLAY, alignment=TextAlignment.CENTER)
    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
              font_weight=FontWeight.DISPLAY, alignment=TextAlignment.CENTER)
  def _set_chestnut_visibility(self):
    usb_connected = ui_state.usb_connected
@@ -0,0 +1,228 @@
 """Render assisted-driving milestone celebrations over the on-road view."""
 import math
 import random
 import time
 from collections import deque
 from dataclasses import dataclass
 import pyray as rl
 from openpilot.cereal import custom
 from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
 from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.lib.application import FontWeight, gui_app
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.widgets import Widget
 CELEBRATION_DURATION = 4.5
 PARTICLE_COUNT = 150
 METERS_PER_MILE = 1609.344
 METERS_PER_KILOMETER = 1000.0
 CONFETTI_COLORS = (
  rl.Color(255, 55, 95, 255),
  rl.Color(255, 183, 3, 255),
  rl.Color(48, 209, 88, 255),
  rl.Color(36, 179, 255, 255),
  rl.Color(112, 72, 232, 255),
  rl.Color(255, 45, 196, 255),
 )
@dataclass(frozen=True)
 class ConfettiParticle:
  x: float
  y: float
  width: float
  height: float
  speed: float
  drift: float
  angle: float
  spin: float
  phase: float
  color: rl.Color
@dataclass(frozen=True)
 class CelebrationMilestone:
  event_id: int
  full_assist: bool
  distance_meters: float
  previous_distance_meters: float
  metric: bool
 class MilestoneCelebration(Widget):
  """Pure renderer for typed assisted-driving milestone events."""
  def __init__(self):
    super().__init__()
    self._drive_started_time = -1.0
    self._celebration_started_time: float | None = None
    self._current_milestone: CelebrationMilestone | None = None
    self._pending_milestones: deque[CelebrationMilestone] = deque()
    self._last_event_id = 0
    self._particles = self._make_particles()
  @staticmethod
  def _make_particles() -> list[ConfettiParticle]:
    rng = random.Random(20260828)
    return [
      ConfettiParticle(
        x=rng.random(),
        y=rng.uniform(-0.25, 0.95),
        width=rng.uniform(10, 24),
        height=rng.uniform(24, 58),
        speed=rng.uniform(0.12, 0.34),
        drift=rng.uniform(-0.035, 0.035),
        angle=rng.uniform(0, 360),
        spin=rng.uniform(-150, 150),
        phase=rng.uniform(0, math.tau),
        color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
      )
      for _ in range(PARTICLE_COUNT)
    ]
  def _render(self, rect: rl.Rectangle, /) -> None:
    now = time.monotonic()
    if ui_state.started_time != self._drive_started_time:
      self._drive_started_time = ui_state.started_time
      self._celebration_started_time = None
      self._current_milestone = None
      self._pending_milestones.clear()
    self._consume_event(suppress=False)
    if self._current_milestone is None and self._pending_milestones:
      self._current_milestone = self._pending_milestones.popleft()
      self._celebration_started_time = now
    if self._celebration_started_time is None or self._current_milestone is None:
      return
    elapsed = now - self._celebration_started_time
    if elapsed >= CELEBRATION_DURATION:
      self._celebration_started_time = None
      self._current_milestone = None
      return
    alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
    self._draw_background_scrim(rect, alpha)
    self._draw_confetti(rect, elapsed, alpha)
    self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
  def cancel_for_alert(self) -> None:
    self._consume_event(suppress=True)
    self._celebration_started_time = None
    self._current_milestone = None
    self._pending_milestones.clear()
  def _consume_event(self, suppress: bool) -> None:
    if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
      return
    state = ui_state.sm["assistedDrivingMilestoneState"]
    event = state.event
    if not state.enabled:
      self._celebration_started_time = None
      self._current_milestone = None
      self._pending_milestones.clear()
      return
    if event.id == 0 or event.id == self._last_event_id:
      return
    self._last_event_id = event.id
    if suppress:
      return
    self._pending_milestones.append(CelebrationMilestone(
      event_id=event.id,
      full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
      distance_meters=event.distanceMeters,
      previous_distance_meters=event.previousDistanceMeters,
      metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
    ))
  def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
    travel_height = rect.height * 1.45
    compact = rect.height <= 300
    particle_scale = rect.height / 1080.0
    particles = self._particles[:100] if compact else self._particles
    for particle in particles:
      x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
      y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
      flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
      particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
      origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
      color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
      rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
  @staticmethod
  def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
    # Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
    scale = rect.height / 240.0
    pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
    number_size = int(FONT_SIZES.set_speed * scale * pulse)
    milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
    category_size = int(22 * scale * pulse)
    unit_size = category_size
    display_font = gui_app.font(FontWeight.DISPLAY)
    semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
    tween_progress = min(elapsed / 0.85, 1.0)
    tween_progress = 1.0 - (1.0 - tween_progress) ** 3
    meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
    previous_distance = milestone.previous_distance_meters / meters_per_unit
    milestone_distance = milestone.distance_meters / meters_per_unit
    displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
    if tween_progress >= 1.0:
      number = f"{round(milestone_distance):,}"
    else:
      number = f"{displayed_distance:,.1f}"
    unit = tr("KM") if milestone.metric else tr("MI")
    category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
    milestone_label = tr("MILESTONE")
    unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
    number_bounds = measure_text_cached(display_font, number, number_size)
    max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
    if number_bounds.x > max_number_width:
      number_size = max(1, int(number_size * max_number_width / number_bounds.x))
      number_bounds = measure_text_cached(display_font, number, number_size)
    category_bounds = measure_text_cached(semibold_font, category, category_size)
    milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
    center_x = rect.x + rect.width / 2
    center_y = rect.y + rect.height / 2
    text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
    secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
    number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
    number_x = center_x - number_line_width / 2
    number_y = center_y - 76 * scale
    unit_y = center_y + 14 * scale
    category_y = center_y - 91 * scale
    milestone_y = center_y + 50 * scale
    rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
                    category_size, 0, secondary_color)
    rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
    rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
                    unit_size, 0, secondary_color)
    rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
                    milestone_size, 0, text_color)
  @staticmethod
  def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
    # Match the alert background: a mostly opaque black core fading to transparent.
    fade_height = round(rect.height * 0.25)
    solid_height = round(rect.height * 0.50)
    solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
    transparent = rl.Color(0, 0, 0, 0)
    x = int(rect.x)
    y = int(rect.y)
    width = int(rect.width)
    rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
    rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
    rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)
@@ -35,7 +35,8 @@ class UIStateSP:
    self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
    self.sm_services_ext = [
      "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
+      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
      "assistedDrivingMilestoneState",
    ]
    self.sunnylink_state = SunnylinkState()
@@ -0,0 +1,45 @@
 #!/usr/bin/env python3
 """Generate the assisted-driving milestone celebration chime."""
 import math
 import wave
 from array import array
 from pathlib import Path
 SAMPLE_RATE = 48_000
 DURATION_SECONDS = 0.82
 NOTES = (
  (0.00, 523.25),
  (0.11, 659.25),
  (0.22, 783.99),
 )
 def note_sample(age: float, frequency: float) -> float:
  if not 0 <= age <= 0.58:
    return 0.0
  attack = min(age / 0.008, 1.0)
  release = min((0.58 - age) / 0.15, 1.0)
  envelope = attack * release * math.exp(-3.8 * age)
  tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
  return envelope * tone
 def main() -> None:
  output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
  samples = array('h')
  for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
    t = frame / SAMPLE_RATE
    value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
    samples.append(round(max(-1.0, min(1.0, value)) * 32767))
  with wave.open(str(output), "wb") as wav:
    wav.setnchannels(1)
    wav.setsampwidth(2)
    wav.setframerate(SAMPLE_RATE)
    wav.writeframes(samples.tobytes())
 if __name__ == "__main__":
  main()
@@ -0,0 +1,38 @@
 #!/usr/bin/env python3
 """Publish deterministic milestone events for the local comma-four UI preview."""
 import itertools
 import time
 from openpilot.cereal import messaging
 def main() -> None:
  pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
  milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
  event_id = 0
  milestone, previous_milestone, category = 0, 0, "mads"
  next_event_time = time.monotonic() + 1.0
  while True:
    now = time.monotonic()
    if now >= next_event_time:
      event_id += 1
      milestone, previous_milestone, category = next(milestones)
      next_event_time = now + 6.0
    msg = messaging.new_message("assistedDrivingMilestoneState")
    state = msg.assistedDrivingMilestoneState
    state.enabled = True
    if event_id:
      state.event.id = event_id
      state.event.category = category
      state.event.distanceMeters = milestone * 1609.344
      state.event.previousDistanceMeters = previous_milestone * 1609.344
      state.event.unit = "imperial"
    pm.send("assistedDrivingMilestoneState", msg)
    time.sleep(0.1)
 if __name__ == "__main__":
  main()
@@ -0,0 +1,27 @@
 #!/usr/bin/env bash
 set -e
 repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
 replay_pid=""
 preview_pid=""
 cleanup() {
  for pid in "$preview_pid" "$replay_pid"; do
    if [[ -n "$pid" ]]; then
      kill "$pid" 2>/dev/null || true
      wait "$pid" 2>/dev/null || true
    fi
  done
 }
 trap cleanup EXIT INT TERM
 export PATH="$repo_root/.venv/bin:$PATH"
 export SP_MILESTONE_PREVIEW=1
 playback="${SP_MILESTONE_PLAYBACK:-1}"
 "$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
 replay_pid=$!
 "$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
 preview_pid=$!
 "$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
@@ -4,12 +4,63 @@ import time
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import log, messaging
 from openpilot.cereal.messaging import SubMaster, PubMaster
-from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
+from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
 AudibleAlert = log.SelfdriveState.AudibleAlert
 class TestSoundd(OpenpilotTestCase):
  @staticmethod
  def milestone_submaster(event_id=42):
    class SubMasterStub:
      def __init__(self):
        self.updated = {'assistedDrivingMilestoneState': True}
        msg = messaging.new_message('assistedDrivingMilestoneState')
        msg.assistedDrivingMilestoneState.enabled = True
        msg.assistedDrivingMilestoneState.event.id = event_id
        self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
      def __getitem__(self, service):
        return self.data[service]
    return SubMasterStub()
  def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
    for weighted_db in (20.0, 30.0, 40.0, 50.0):
      with self.subTest(weighted_db=weighted_db):
        comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
        comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
        assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
  def test_milestone_chime_uses_typed_milestone_event_once(self):
    soundd = Soundd()
    sm = self.milestone_submaster()
    soundd.update_milestone_alert(sm)
    assert soundd.current_alert == AudibleAlert.complete
    soundd.current_alert = AudibleAlert.none
    soundd.update_milestone_alert(sm)
    assert soundd.current_alert == AudibleAlert.none
  def test_safety_alert_consumes_milestone_without_replaying_it(self):
    soundd = Soundd()
    sm = self.milestone_submaster()
    soundd.current_alert = AudibleAlert.warningImmediate
    soundd.update_milestone_alert(sm)
    soundd.current_alert = AudibleAlert.none
    soundd.update_milestone_alert(sm)
    assert soundd.current_alert == AudibleAlert.none
  def test_quiet_mode_consumes_milestone_without_playing_it(self):
    soundd = Soundd()
    soundd.enabled = True
    soundd.update_milestone_alert(self.milestone_submaster())
    assert soundd.current_alert == AudibleAlert.none
  def test_check_selfdrive_timeout_alert(self, mocker):
    sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
    pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
@@ -104,6 +104,14 @@ class ControlsExt(ModelStateBase):
    CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
    CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
    ford_path = getattr(self, 'ford_path', None)
    if ford_path is not None:
      CC_SP.fordLateralPath.valid = ford_path.valid
      CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
      CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
      CC_SP.fordLateralPath.curvature = ford_path.curvature
      CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
    return CC_SP
  @staticmethod
@@ -0,0 +1,260 @@
 """Authoritative assisted-driving distance and milestone tracking."""
 import math
 from collections.abc import Mapping
 from dataclasses import dataclass
 from enum import StrEnum
 from openpilot.common.params import Params
 METERS_PER_MILE = 1609.344
 METERS_PER_KILOMETER = 1000.0
 MAX_SAMPLE_INTERVAL_SECONDS = 0.5
 PERSIST_INTERVAL_NS = 10_000_000_000
 STATE_VERSION = 1
 STATE_PARAM = "AssistedDrivingMilestoneState"
 LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
 class AssistCategory(StrEnum):
  MADS = "mads"
  FULL_ASSIST = "fullAssist"
 class MilestoneUnit(StrEnum):
  IMPERIAL = "imperial"
  METRIC = "metric"
@dataclass(frozen=True)
 class MilestoneEvent:
  event_id: int
  category: AssistCategory
  distance_meters: float
  previous_distance_meters: float
  unit: MilestoneUnit
@dataclass(frozen=True)
 class MilestoneSnapshot:
  distances_meters: dict[AssistCategory, float]
  drive_start_distances_meters: dict[AssistCategory, float]
  next_event_id: int
  next_summary_id: int
  unit: MilestoneUnit
  active_drive_id: str
 def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
  if not lat_active:
    return None
  return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
 def _meters_per_unit(unit: MilestoneUnit) -> float:
  return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
 def _next_ladder_value(value: float) -> float:
  value = max(0.0, value)
  magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
  for multiplier in (1.0, 2.0, 5.0):
    candidate = multiplier * magnitude
    if candidate > value + 1e-9:
      return candidate
  return 10.0 * magnitude
 def _previous_ladder_value(value: float) -> float:
  if value <= 1.0:
    return 0.0
  magnitude = 10.0 ** math.floor(math.log10(value))
  normalized = value / magnitude
  if normalized <= 1.0 + 1e-9:
    return 5.0 * magnitude / 10.0
  if normalized <= 2.0 + 1e-9:
    return magnitude
  return 2.0 * magnitude
 def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
  meters_per_unit = _meters_per_unit(unit)
  return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
 class MilestoneStore:
  def __init__(self, params: Params | None = None):
    self._params = params or Params()
  def load(self) -> MilestoneSnapshot:
    raw = self._params.get(STATE_PARAM, return_default=True)
    raw = raw if isinstance(raw, dict) else {}
    raw_distances = raw.get("distancesMeters", {})
    raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
    try:
      unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
    except ValueError:
      unit = MilestoneUnit.IMPERIAL
    def distance(category: AssistCategory) -> float:
      try:
        return max(0.0, float(raw_distances.get(category.value, 0.0)))
      except (TypeError, ValueError):
        return 0.0
    distances = {category: distance(category) for category in AssistCategory}
    raw_drive_start = raw.get("driveStartDistancesMeters", {})
    raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
    def drive_start_distance(category: AssistCategory) -> float:
      try:
        return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
      except (TypeError, ValueError):
        return distances[category]
    try:
      next_event_id = max(1, int(raw.get("nextEventId", 1)))
    except (TypeError, ValueError):
      next_event_id = 1
    try:
      next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
    except (TypeError, ValueError):
      next_summary_id = 1
    return MilestoneSnapshot(
      distances_meters=distances,
      drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
      next_event_id=next_event_id,
      next_summary_id=next_summary_id,
      unit=unit,
      active_drive_id=str(raw.get("activeDriveId", "")),
    )
  def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
    if block:
      self._params.flush()
    self._params.put(STATE_PARAM, {
      "version": STATE_VERSION,
      "distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
      "driveStartDistancesMeters": {
        category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
      },
      "nextEventId": max(1, snapshot.next_event_id),
      "nextSummaryId": max(1, snapshot.next_summary_id),
      "unit": snapshot.unit.value,
      "activeDriveId": snapshot.active_drive_id,
    }, block=block)
  def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
    self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
      "version": STATE_VERSION,
      "id": summary_id,
      "distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
      "unit": unit.value,
    }, block=True)
 class AssistedDrivingMilestones:
  """Tracks, persists, and emits milestones through one small interface."""
  def __init__(self, store: MilestoneStore | None = None):
    self._store = store or MilestoneStore()
    snapshot = self._store.load()
    self._distances_meters = snapshot.distances_meters
    self._drive_start_distances_meters = snapshot.drive_start_distances_meters
    self._next_event_id = snapshot.next_event_id
    self._next_summary_id = snapshot.next_summary_id
    self._unit = snapshot.unit
    self._active_drive_id = snapshot.active_drive_id
    self._next_milestone_meters = {
      category: next_milestone_meters(distance, self._unit)
      for category, distance in self._distances_meters.items()
    }
    self._last_timestamp_ns: int | None = None
    self._last_persist_timestamp_ns: int | None = None
    self._last_speed_mps = 0.0
    self._last_category: AssistCategory | None = None
    self._enabled = False
    self._closed = False
  def snapshot(self) -> MilestoneSnapshot:
    return MilestoneSnapshot(
      self._distances_meters.copy(),
      self._drive_start_distances_meters.copy(),
      self._next_event_id,
      self._next_summary_id,
      self._unit,
      self._active_drive_id,
    )
  def set_drive_id(self, drive_id: str) -> None:
    if not drive_id or drive_id == self._active_drive_id:
      return
    self._active_drive_id = drive_id
    self._drive_start_distances_meters = self._distances_meters.copy()
    self._persist()
  def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
             is_metric: bool, enabled: bool) -> MilestoneEvent | None:
    self._enabled = enabled
    unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
    if unit != self._unit:
      self._unit = unit
      self._next_milestone_meters = {
        category: next_milestone_meters(distance, unit)
        for category, distance in self._distances_meters.items()
      }
    speed_mps = max(0.0, speed_mps)
    category = assist_category(lat_active, long_active) if enabled else None
    event = None
    if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
      dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
      if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
        active_category = self._last_category
        self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
        threshold_meters = self._next_milestone_meters[active_category]
        if self._distances_meters[active_category] >= threshold_meters:
          meters_per_unit = _meters_per_unit(self._unit)
          threshold_units = threshold_meters / meters_per_unit
          event = MilestoneEvent(
            event_id=self._next_event_id,
            category=active_category,
            distance_meters=threshold_meters,
            previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
            unit=self._unit,
          )
          self._next_event_id += 1
          self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
          self._persist(timestamp_ns=timestamp_ns)
    self._last_timestamp_ns = timestamp_ns
    self._last_speed_mps = speed_mps
    self._last_category = category
    if self._last_persist_timestamp_ns is None:
      self._last_persist_timestamp_ns = timestamp_ns
    elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
      self._persist(timestamp_ns=timestamp_ns)
    return event
  def close(self) -> None:
    if self._closed:
      return
    self._closed = True
    drive_distances = {
      category: self._distances_meters[category] - self._drive_start_distances_meters[category]
      for category in AssistCategory
    }
    summary_id = self._next_summary_id
    self._next_summary_id += 1
    self._persist(block=True)
    if self._enabled:
      self._store.save_drive_summary(summary_id, drive_distances, self._unit)
  def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
    self._store.save(self.snapshot(), block=block)
    self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns
@@ -0,0 +1,123 @@
 import unittest
 from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
  METERS_PER_MILE,
  AssistCategory,
  AssistedDrivingMilestones,
  MilestoneStore,
  MilestoneUnit,
 )
 class ParamsStub:
  def __init__(self, state=None):
    self.values = {"AssistedDrivingMilestoneState": state or {}}
    self.writes = []
  def get(self, key, return_default=False):
    return self.values.get(key, {} if return_default else None)
  def put(self, key, value, block=False):
    self.values[key] = value
    self.writes.append((key, value, block))
  def flush(self):
    pass
 class TestAssistedDrivingMilestones(unittest.TestCase):
  def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
    params = ParamsStub({
      "version": 1,
      "distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
      "nextEventId": 7,
      "unit": "imperial",
    })
    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
    event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    self.assertIsNotNone(event)
    assert event is not None
    self.assertEqual(event.event_id, 7)
    self.assertEqual(event.category, AssistCategory.MADS)
    self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
    self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
    self.assertFalse(params.writes[-1][2])
  def test_switching_units_schedules_only_a_future_milestone(self):
    params = ParamsStub({
      "version": 1,
      "distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
      "nextEventId": 2,
      "unit": "imperial",
    })
    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
    event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
    self.assertIsNotNone(event)
    assert event is not None
    self.assertEqual(event.unit, MilestoneUnit.METRIC)
    self.assertAlmostEqual(event.distance_meters, 10_000.0)
  def test_ignores_disabled_reverse_and_timestamp_gaps(self):
    params = ParamsStub()
    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
    milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
    milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
  def test_close_persists_totals_and_last_drive_summary(self):
    params = ParamsStub()
    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
    milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
    milestones.close()
    summary = params.values["LastDriveAssistedDrivingSummary"]
    self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
    self.assertTrue(params.writes[-1][2])
    write_count = len(params.writes)
    milestones.close()
    self.assertEqual(len(params.writes), write_count)
  def test_process_restart_preserves_the_current_drive_start(self):
    params = ParamsStub()
    first_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    first_process.set_drive_id("route-1")
    first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    first_process.close()
    second_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    second_process.set_drive_id("route-1")
    second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    second_process.close()
    summary = params.values["LastDriveAssistedDrivingSummary"]
    self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
  def test_disabled_feature_does_not_publish_drive_summary(self):
    params = ParamsStub()
    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
    milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
    milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
    milestones.close()
    self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
 if __name__ == "__main__":
  unittest.main()
@@ -1383,6 +1383,12 @@
              "title": "Steering Arc",
              "description": "Display steering arc on the driving screen when lateral control is enabled."
            },
            {
              "key": "AssistedDrivingMilestonesEnabled",
              "widget": "toggle",
              "title": "Assisted Driving Milestones",
              "description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
            },
            {
              "key": "ShowTurnSignals",
              "widget": "toggle",
@@ -2168,6 +2174,43 @@
    }
  ],
  "vehicle_settings": {
    "ford": {
      "title": "Ford Settings",
      "description": "",
      "items": [
        {
          "key": "FordModelActionController",
          "widget": "toggle",
          "needs_onroad_cycle": true,
          "title": "Selected-Action Path Tracking (Experimental)",
          "description": "Follow the selected steering plan 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"
            }
          ]
        },
        {
          "key": "FordPscmObserver",
          "widget": "toggle",
          "needs_onroad_cycle": true,
          "title": "PSCM Coefficient Observer (Experimental)",
          "description": "Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.",
          "details": "This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when Selected-Action Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.",
          "enablement": [
            {
              "type": "offroad_only"
            },
            {
              "type": "param",
              "key": "FordModelActionController",
              "equals": false
            }
          ]
        }
      ]
    },
    "hyundai": {
      "title": "Hyundai / Kia / Genesis Settings",
      "description": "",
@@ -6,6 +6,29 @@ icon: vehicle
 order: 99
 kind: vehicle
 sections:
 - id: ford
  title: Ford Settings
  description: ''
  items:
  - key: FordModelActionController
    widget: toggle
    needs_onroad_cycle: true
    title: Selected-Action Path Tracking (Experimental)
    description: Follow the selected steering plan 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
    widget: toggle
    needs_onroad_cycle: true
    title: PSCM Coefficient Observer (Experimental)
    description: Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.
    details: This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when Selected-Action Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.
    enablement:
    - $ref: '#/macros/offroad'
    - type: param
      key: FordModelActionController
      equals: false
 - id: hyundai
  title: Hyundai / Kia / Genesis Settings
  description: ''
@@ -20,6 +20,10 @@ sections:
    widget: toggle
    title: Steering Arc
    description: Display steering arc on the driving screen when lateral control is enabled.
  - key: AssistedDrivingMilestonesEnabled
    widget: toggle
    title: Assisted Driving Milestones
    description: Celebrate cumulative MADS and full-assist distance milestones while driving.
  - key: ShowTurnSignals
    widget: toggle
    title: Display Turn Signals
@@ -5,6 +5,7 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import json
 import tempfile
 from openpilot.common.params import Params
 from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import (
@@ -278,6 +279,34 @@ class TestKnownPanels(OpenpilotTestCase):
 class TestKnownVehicleSettings(OpenpilotTestCase):
  def test_ford_model_action_is_separate_default_off_cycle_only_toggle(self, schema):
    items = _brand_items(schema["vehicle_settings"].get("ford"))
    candidate = next(item for item in items if item["key"] == "FordModelActionController")
    assert candidate["title"] == "Selected-Action Path Tracking (Experimental)"
    assert candidate["widget"] == "toggle"
    assert candidate["needs_onroad_cycle"] is True
    assert candidate["enablement"] == [{"type": "offroad_only"}]
    assert "takes priority over PSCM Coefficient Observer" in candidate["details"]
    assert "Turning it off restores PSCM Coefficient Observer if selected" in candidate["details"]
    assert "not road-validated" in candidate["details"]
    with tempfile.TemporaryDirectory() as path:
      assert Params(path).get_default_value("FordModelActionController") is False
  def test_retired_ford_toggles_are_not_exposed(self, schema):
    items = _brand_items(schema["vehicle_settings"].get("ford"))
    keys = {item["key"] for item in items}
    assert "FordVirtualAngleController" not in keys
    assert "FordSharedPathController" not in keys
  def test_ford_has_pscm_observer(self, schema):
    items = _brand_items(schema["vehicle_settings"].get("ford"))
    observer = next(item for item in items if item["key"] == "FordPscmObserver")
    assert observer["needs_onroad_cycle"] is True
    assert observer["enablement"] == [
      {"type": "offroad_only"},
      {"type": "param", "key": "FordModelActionController", "equals": False},
    ]
  def test_hyundai_has_longitudinal_tuning(self, schema):
    keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
    assert "HyundaiLongitudinalTuning" in keys
@@ -103,6 +103,32 @@ def _migrate_model_bundle_slots(_params):
    cloudlog.exception(f"Error migrating model bundle slots: {e}")
 def _migrate_assisted_driving_milestones(_params):
  try:
    state = _params.get("AssistedDrivingMilestoneState", return_default=True)
    if isinstance(state, dict) and state.get("version") == 1:
      return
    _params.put("AssistedDrivingMilestoneState", {
      "version": 1,
      "distancesMeters": {
        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
      },
      "driveStartDistancesMeters": {
        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
      },
      "nextEventId": 1,
      "nextSummaryId": 1,
      "unit": "metric" if _params.get_bool("IsMetric") else "imperial",
      "activeDriveId": "",
    }, block=True)
    cloudlog.info("params_migration: migrated assisted-driving milestone state")
  except Exception as e:
    cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
 def run_migration(_params):
  # migrate OnroadScreenOffBrightness
  if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -142,3 +168,5 @@ def run_migration(_params):
  # seed the chestnut model slot from the pre-split single slot
  _migrate_model_bundle_slots(_params)
  _migrate_assisted_driving_milestones(_params)
@@ -7,7 +7,44 @@ See the LICENSE.md file in the root directory for more details.
 from openpilot.common.params import Params
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
+from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
 class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
  def test_preserves_prototype_distances_once(self):
    class ParamsStub:
      def __init__(self):
        self.values = {
          "MadsDrivenDistanceMeters": 123.0,
          "FullAssistDrivenDistanceMeters": 456.0,
          "OnroadScreenOffBrightness": 0,
          "OnroadScreenOffTimer": 15,
          "AssistedDrivingMilestoneState": {},
          "IsMetric": False,
        }
      def get(self, key, return_default=False):
        return self.values.get(key)
      def put(self, key, value, block=False):
        self.values[key] = value
      def get_bool(self, key):
        return bool(self.values.get(key, False))
    params = ParamsStub()
    run_migration(params)
    state = params.get("AssistedDrivingMilestoneState")
    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
    params.put("MadsDrivenDistanceMeters", 12.0, block=True)
    params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
    run_migration(params)
    state = params.get("AssistedDrivingMilestoneState")
    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
 class TestModelBundleSlotMigration(OpenpilotTestCase):
@@ -0,0 +1,498 @@
 #!/usr/bin/env python3
 """Offline evaluation of Ford's native four-field path polynomial.
 The experiment deliberately does not alter the live controller. It rebases the
 model path into the vehicle pose expected at actuation time, fits one cubic over
 the remaining short path, and converts the cubic into the LMC2 C0/C1/C2/C3
 signals. A first-order C2 response envelope is included to expose commands that
 would look good only if the PSCM curvature channel were instantaneous.
 """
 import argparse
 from collections import defaultdict
 from dataclasses import dataclass
 import glob
 import math
 from pathlib import Path
 import numpy as np
 from openpilot.tools.lib.logreader import LogReader
 DBC_OFFSET = (-5.12, 5.11)
 DBC_ANGLE = (-0.5, 0.5235)
 DBC_CURVATURE = (-0.02, 0.02)
 DBC_CURVATURE_RATE = (-0.001024, 0.001023)
 MAX_LATERAL_ACCEL = 3.0 + 9.81 * 0.06
 MAX_LATERAL_JERK = 3.0 + 9.81 * 0.06
@dataclass(frozen=True)
 class ModelPath:
  x: np.ndarray
  y: np.ndarray
  heading: np.ndarray
  distance: np.ndarray
@dataclass(frozen=True)
 class Sample:
  route: str
  time: float
  speed: float
  curvature: float
  steering_pressed: bool
  path: ModelPath
  sent_c0: float
  sent_c1: float
  sent_c2: float
  sent_c3: float
@dataclass(frozen=True)
 class NativePath:
  c0: float
  c1: float
  c2: float
  c3: float
  fit_rmse: float
  path_rms: float
 def _model_path(model) -> ModelPath | None:
  try:
    x = np.asarray(model.position.x, dtype=float)
    y = np.asarray(model.position.y, dtype=float)
    heading = np.unwrap(np.asarray(model.orientation.z, dtype=float))
  except (AttributeError, TypeError, ValueError):
    return None
  if len(x) < 4 or len(x) != len(y) or len(x) != len(heading):
    return None
  if not np.isfinite(np.concatenate((x, y, heading))).all():
    return None
  distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
  unique_distance, unique = np.unique(distance, return_index=True)
  if len(unique_distance) < 4 or unique_distance[-1] <= 0.0:
    return None
  return ModelPath(x[unique], y[unique], heading[unique], unique_distance)
 def _arc_pose(distance: float, curvature: float) -> tuple[float, float, float]:
  heading = curvature * distance
  if abs(curvature) < 1e-9:
    return distance, 0.0, 0.0
  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
 def _relative_points(path: ModelPath, vehicle_pose: tuple[float, float, float], start: float,
                     horizon: float, count: int = 25) -> tuple[np.ndarray, np.ndarray]:
  sample_distance = np.linspace(start, min(start + horizon, path.distance[-1]), count)
  desired_x = np.interp(sample_distance, path.distance, path.x)
  desired_y = np.interp(sample_distance, path.distance, path.y)
  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
  dx = desired_x - vehicle_x
  dy = desired_y - vehicle_y
  cosine = math.cos(vehicle_heading)
  sine = math.sin(vehicle_heading)
  return cosine * dx + sine * dy, -sine * dx + cosine * dy
 def _fit_points(path: ModelPath, speed: float, current_curvature: float, delay: float,
                horizon: float) -> tuple[np.ndarray, np.ndarray] | None:
  advance = min(max(speed, 0.0) * delay, path.distance[-1])
  available = min(horizon, path.distance[-1] - advance)
  if available <= 0.25:
    return None
  x, y = _relative_points(path, _arc_pose(advance, current_curvature), advance, available)
  forward = (x >= -0.25) & (x <= horizon)
  x = x[forward]
  y = y[forward]
  if len(x) < 4 or np.ptp(x) <= 0.25:
    return None
  return x, y
 def _wire_coefficients(c0: float, c1: float, c2: float, c3: float) -> tuple[float, float, float, float]:
  slope = math.tan(c1)
  slope_norm = 1.0 + slope ** 2
  a2 = 0.5 * c2 * slope_norm ** 1.5
  a3 = (c3 + 12.0 * slope * a2 ** 2 / slope_norm ** 3) * slope_norm ** 2 / 6.0
  return c0, slope, a2, a3
 def _wire_rmse(command: tuple[float, float, float, float], x: np.ndarray, y: np.ndarray) -> float:
  a0, a1, a2, a3 = _wire_coefficients(*command)
  reconstructed = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3
  return float(np.sqrt(np.mean((reconstructed - y) ** 2)))
 def fit_native_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
                    horizon: float) -> NativePath:
  """Fit the delay-aligned path and return physical LMC2 fields.
  C2 and C3 are curvature and curvature rate at the vehicle-frame origin, not
  the raw quadratic and cubic polynomial coefficients.
  """
  points = _fit_points(path, speed, current_curvature, delay, horizon)
  if points is None:
    return NativePath(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
  x, y = points
  # Scaling x before the least-squares solve keeps tight-turn fits well
  # conditioned while preserving an ordinary cubic in vehicle coordinates.
  scale = max(float(np.max(np.abs(x))), 1.0)
  normalized_x = x / scale
  design = np.column_stack((np.ones(len(x)), normalized_x, normalized_x ** 2, normalized_x ** 3))
  scaled, *_ = np.linalg.lstsq(design, y, rcond=None)
  a0, a1, a2, a3 = (float(scaled[index] / scale ** index) for index in range(4))
  slope = a1
  slope_norm = 1.0 + slope ** 2
  curvature = 2.0 * a2 / slope_norm ** 1.5
  curvature_rate = 6.0 * a3 / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
  command = (float(np.clip(a0, *DBC_OFFSET)),
             float(np.clip(math.atan(slope), *DBC_ANGLE)),
             float(np.clip(curvature, *DBC_CURVATURE)),
             float(np.clip(curvature_rate, *DBC_CURVATURE_RATE)))
  return NativePath(
    *command,
    _wire_rmse(command, x, y),
    float(np.sqrt(np.mean(y ** 2))),
  )
 def fit_c2_aware_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
                      horizon: float, target_c2: float, effective_c2: float,
                      use_c3: bool = True) -> NativePath:
  """Fit fast fields around the C2 curvature the PSCM is expected to realize."""
  points = _fit_points(path, speed, current_curvature, delay, horizon)
  if points is None:
    return NativePath(0.0, 0.0, target_c2, 0.0, 0.0, 0.0)
  x, y = points
  slope = 0.0
  a0 = a1 = a3 = 0.0
  for _ in range(3):
    a2 = 0.5 * effective_c2 * (1.0 + slope ** 2) ** 1.5
    design = np.column_stack((np.ones(len(x)), x, x ** 3))
    (a0, a1, a3), *_ = np.linalg.lstsq(design, y - a2 * x ** 2, rcond=None)
    slope = float(a1)
  slope_norm = 1.0 + slope ** 2
  c3 = 6.0 * float(a3) / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
  c3 = float(np.clip(c3, *DBC_CURVATURE_RATE)) if use_c3 else 0.0
  # Once C2 and C3 are fixed to what the hardware can realize, refit C0/C1 so
  # their fast feedback preserves as much of the same path as possible.
  _, _, fixed_a2, fixed_a3 = _wire_coefficients(0.0, math.atan(slope), effective_c2, c3)
  (a0, a1), *_ = np.linalg.lstsq(np.column_stack((np.ones(len(x)), x)),
                                  y - fixed_a2 * x ** 2 - fixed_a3 * x ** 3, rcond=None)
  c0 = float(np.clip(a0, *DBC_OFFSET))
  c1 = float(np.clip(math.atan(float(a1)), *DBC_ANGLE))
  effective_command = (c0, c1, effective_c2, c3)
  return NativePath(c0, c1, target_c2, c3, _wire_rmse(effective_command, x, y),
                    float(np.sqrt(np.mean(y ** 2))))
 def _route(path: str) -> str:
  return Path(path).name.split("--", 1)[0]
 def load_samples(paths: list[str], stride: int = 2) -> list[Sample]:
  grouped: dict[str, list[str]] = defaultdict(list)
  for path in paths:
    grouped[_route(path)].append(path)
  samples = []
  for route, route_paths in sorted(grouped.items()):
    events = []
    for path in sorted(route_paths):
      events.extend(LogReader(path))
    events.sort(key=lambda event: event.logMonoTime)
    if not events:
      continue
    start_time = events[0].logMonoTime
    model_path = None
    curvature = 0.0
    lat_active = path_valid = False
    sent = (0.0, 0.0, 0.0, 0.0)
    car_state_count = 0
    for event in events:
      which = event.which()
      if which == "modelV2":
        model_path = _model_path(event.modelV2)
      elif which == "controlsState":
        curvature = float(event.controlsState.curvature)
      elif which == "carControl":
        lat_active = bool(event.carControl.latActive)
      elif which == "carControlSP":
        command = event.carControlSP.fordLateralPath
        path_valid = bool(command.valid)
        sent = (float(command.pathOffset), float(command.pathAngle),
                float(command.curvature), float(command.curvatureRate))
      elif which == "carState" and lat_active and path_valid and model_path is not None:
        car_state_count += 1
        if car_state_count % stride:
          continue
        samples.append(Sample(
          route, (event.logMonoTime - start_time) * 1e-9, float(event.carState.vEgo), curvature,
          bool(event.carState.steeringPressed), model_path, *sent,
        ))
  return samples
 def _percentile(values: np.ndarray, percentile: float, mask: np.ndarray | None = None) -> float:
  selected = values if mask is None else values[mask]
  return float(np.percentile(np.abs(selected), percentile)) if len(selected) else math.nan
 def _route_rate(samples: list[Sample], values: np.ndarray) -> np.ndarray:
  rate = np.zeros(len(values))
  for index in range(1, len(values)):
    dt = samples[index].time - samples[index - 1].time
    if samples[index].route == samples[index - 1].route and 0.005 <= dt <= 0.2:
      rate[index] = (values[index] - values[index - 1]) / dt
  return rate
 def _c2_response(samples: list[Sample], target: np.ndarray, tau_load: float,
                 tau_unload: float) -> np.ndarray:
  effective = np.zeros(len(target))
  previous_route = None
  previous_time = 0.0
  state = 0.0
  for index, sample in enumerate(samples):
    if sample.route != previous_route:
      state = 0.0
      previous_time = sample.time
    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
    loading = target[index] * state >= 0.0 and abs(target[index]) > abs(state)
    tau = tau_load if loading else tau_unload
    state += (1.0 - math.exp(-dt / tau)) * (target[index] - state)
    effective[index] = state
    previous_route, previous_time = sample.route, sample.time
  return effective
 def _limit_c2_command(samples: list[Sample], target: np.ndarray) -> np.ndarray:
  """Mirror the CAN-FD Ford curvature acceleration/jerk limiter."""
  limited = np.zeros(len(target))
  previous_route = None
  previous_time = 0.0
  previous = 0.0
  for index, sample in enumerate(samples):
    if sample.route != previous_route:
      previous = 0.0
      previous_time = sample.time
    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
    speed = max(sample.speed, 1.0)
    value = float(np.clip(target[index], -MAX_LATERAL_ACCEL / speed ** 2,
                          MAX_LATERAL_ACCEL / speed ** 2))
    step = MAX_LATERAL_JERK / speed ** 2 * dt
    value = float(np.clip(value, previous - step, previous + step))
    limited[index] = float(np.clip(value, *DBC_CURVATURE))
    previous = limited[index]
    previous_route, previous_time = sample.route, sample.time
  return limited
 def _limit_fast_fields(samples: list[Sample], c0_target: np.ndarray,
                       c1_target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
  c0 = np.zeros(len(samples))
  c1 = np.zeros(len(samples))
  previous_route = None
  previous_time = 0.0
  previous_c0 = previous_c1 = 0.0
  for index, sample in enumerate(samples):
    if sample.route != previous_route:
      previous_c0 = previous_c1 = 0.0
      previous_time = sample.time
    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
    c0[index] = np.clip(c0_target[index], previous_c0 - 4.0 * dt, previous_c0 + 4.0 * dt)
    c1[index] = np.clip(c1_target[index], previous_c1 - 1.0 * dt, previous_c1 + 1.0 * dt)
    previous_c0, previous_c1 = c0[index], c1[index]
    previous_route, previous_time = sample.route, sample.time
  return c0, c1
 def evaluate(samples: list[Sample], *, delay: float, horizon: float,
             tau_load: float, tau_unload: float, horizon_time: float = 0.0,
             assumed_tau_load: float | None = None, assumed_tau_unload: float | None = None,
             use_c3: bool = True, c2_limit: float = DBC_CURVATURE[1]) -> dict[str, float]:
  horizons = np.asarray([float(np.clip(sample.speed * horizon_time, 1.0, horizon))
                         if horizon_time > 0.0 else horizon for sample in samples])
  commands = [fit_native_path(sample.path, sample.speed, sample.curvature,
                              delay=delay, horizon=sample_horizon)
              for sample, sample_horizon in zip(samples, horizons, strict=True)]
  c0 = np.asarray([command.c0 for command in commands])
  c1 = np.asarray([command.c1 for command in commands])
  raw_c2 = np.asarray([command.c2 for command in commands])
  c2 = np.clip(raw_c2, -c2_limit, c2_limit)
  c3 = np.asarray([command.c3 for command in commands])
  fit_rmse = np.asarray([command.fit_rmse for command in commands])
  path_rms = np.asarray([command.path_rms for command in commands])
  transmitted_c2 = _limit_c2_command(samples, c2)
  effective_c2 = _c2_response(samples, transmitted_c2, tau_load, tau_unload)
  estimated_c2 = _c2_response(samples, transmitted_c2,
                              tau_load if assumed_tau_load is None else assumed_tau_load,
                              tau_unload if assumed_tau_unload is None else assumed_tau_unload)
  compensated = [fit_c2_aware_path(sample.path, sample.speed, sample.curvature,
                                   delay=delay, horizon=sample_horizon, target_c2=target,
                                   effective_c2=estimated, use_c3=use_c3)
                 for sample, sample_horizon, target, estimated in
                 zip(samples, horizons, c2, estimated_c2, strict=True)]
  compensated_c0 = np.asarray([command.c0 for command in compensated])
  compensated_c1 = np.asarray([command.c1 for command in compensated])
  compensated_c3 = np.asarray([command.c3 for command in compensated])
  limited_c0, limited_c1 = _limit_fast_fields(samples, compensated_c0, compensated_c1)
  estimated_compensated_rmse = np.asarray([command.fit_rmse for command in compensated])
  compensated_rmse = []
  for sample, sample_horizon, command, effective in zip(samples, horizons, compensated, effective_c2, strict=True):
    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
    compensated_rmse.append(0.0 if points is None else _wire_rmse(
      (command.c0, command.c1, effective, command.c3), *points))
  compensated_rmse = np.asarray(compensated_rmse)
  limited_compensated_rmse = []
  for sample, sample_horizon, c0_value, c1_value, c3_value, effective in \
      zip(samples, horizons, limited_c0, limited_c1, compensated_c3, effective_c2, strict=True):
    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
    limited_compensated_rmse.append(0.0 if points is None else _wire_rmse(
      (c0_value, c1_value, effective, c3_value), *points))
  limited_compensated_rmse = np.asarray(limited_compensated_rmse)
  missing_c2 = transmitted_c2 - effective_c2
  # Compare channels by their lateral contribution at the fit horizon. This
  # includes C3: treating it as zero would incorrectly blame C0/C1 for a
  # curvature transition the native polynomial assigns to curvature rate.
  fast = (2.0 * compensated_c0 / horizons ** 2 +
          2.0 * np.tan(compensated_c1) / horizons +
          compensated_c3 * horizons / 3.0)
  lagging = np.abs(missing_c2) > 0.0005
  unloading = lagging & (np.abs(c2) < 0.75 * np.abs(effective_c2))
  pressed = np.asarray([sample.steering_pressed for sample in samples])
  speed = np.asarray([sample.speed for sample in samples])
  sent_c2 = np.asarray([sample.sent_c2 for sample in samples])
  sent_transmitted_c2 = _limit_c2_command(samples, sent_c2)
  sent_effective_c2 = _c2_response(samples, sent_transmitted_c2, tau_load, tau_unload)
  sent_lpf_rmse = []
  for sample, sample_horizon, effective in zip(samples, horizons, sent_effective_c2, strict=True):
    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
    sent_lpf_rmse.append(0.0 if points is None else _wire_rmse(
      (sample.sent_c0, sample.sent_c1, effective, sample.sent_c3), *points))
  sent_lpf_rmse = np.asarray(sent_lpf_rmse)
  raw_c2_rate = _route_rate(samples, c2)
  c2_rate = _route_rate(samples, transmitted_c2)
  sent_c2_rate = _route_rate(samples, sent_c2)
  compensated_c0_rate = _route_rate(samples, compensated_c0)
  compensated_c1_rate = _route_rate(samples, compensated_c1)
  compensated_c3_rate = _route_rate(samples, compensated_c3)
  normalized_fit = np.divide(fit_rmse, path_rms, out=np.zeros_like(fit_rmse), where=path_rms > 1e-4)
  return {
    "samples": float(len(samples)),
    "delay": delay,
    "horizon": horizon,
    "horizon_time": horizon_time,
    "assumed_tau_load": tau_load if assumed_tau_load is None else assumed_tau_load,
    "assumed_tau_unload": tau_unload if assumed_tau_unload is None else assumed_tau_unload,
    "use_c3": float(use_c3),
    "c2_limit": c2_limit,
    "actual_horizon_p50": _percentile(horizons, 50),
    "actual_horizon_p95": _percentile(horizons, 95),
    "fit_rmse_p50": _percentile(fit_rmse, 50),
    "fit_rmse_p95": _percentile(fit_rmse, 95),
    "normalized_fit_p95": _percentile(normalized_fit, 95),
    "c2_aware_rmse_p50": _percentile(compensated_rmse, 50),
    "c2_aware_rmse_p95": _percentile(compensated_rmse, 95),
    "c2_aware_estimated_rmse_p95": _percentile(estimated_compensated_rmse, 95),
    "c2_aware_limited_rmse_p95": _percentile(limited_compensated_rmse, 95),
    "sent_lpf_rmse_p50": _percentile(sent_lpf_rmse, 50),
    "sent_lpf_rmse_p95": _percentile(sent_lpf_rmse, 95),
    "c0_p95": _percentile(c0, 95),
    "c1_p95": _percentile(c1, 95),
    "c2_p95": _percentile(c2, 95),
    "c3_p95": _percentile(c3, 95),
    "c0_clip_rate": float(np.mean((c0 <= DBC_OFFSET[0]) | (c0 >= DBC_OFFSET[1]))),
    "c1_clip_rate": float(np.mean((c1 <= DBC_ANGLE[0]) | (c1 >= DBC_ANGLE[1]))),
    "c2_clip_rate": float(np.mean((c2 <= DBC_CURVATURE[0]) | (c2 >= DBC_CURVATURE[1]))),
    "c3_clip_rate": float(np.mean((c3 <= DBC_CURVATURE_RATE[0]) | (c3 >= DBC_CURVATURE_RATE[1]))),
    "c2_aware_c0_p95": _percentile(compensated_c0, 95),
    "c2_aware_c1_p95": _percentile(compensated_c1, 95),
    "c2_aware_c3_p95": _percentile(compensated_c3, 95),
    "c2_aware_c0_rate_p95": _percentile(compensated_c0_rate, 95),
    "c2_aware_c1_rate_p95": _percentile(compensated_c1_rate, 95),
    "c2_aware_c0_rate_limit_rate": float(np.mean(np.abs(compensated_c0_rate) > 4.0)),
    "c2_aware_c1_rate_limit_rate": float(np.mean(np.abs(compensated_c1_rate) > 1.0)),
    "c2_aware_c3_rate_p95": _percentile(compensated_c3_rate, 95),
    "raw_c2_rate_p95": _percentile(raw_c2_rate, 95),
    "c2_rate_p95": _percentile(c2_rate, 95),
    "sent_c2_rate_p95": _percentile(sent_c2_rate, 95),
    "c2_lag_p95": _percentile(missing_c2, 95),
    "lag_samples": float(np.count_nonzero(lagging)),
    "lag_fast_support_rate": float(np.mean(fast[lagging] * missing_c2[lagging] > 0.0)) if np.any(lagging) else math.nan,
    "lag_fast_coverage_p50": _percentile(np.divide(fast, missing_c2, out=np.zeros_like(fast),
                                                    where=np.abs(missing_c2) > 1e-6), 50, lagging),
    "unload_samples": float(np.count_nonzero(unloading)),
    "unload_fast_counter_rate": float(np.mean(fast[unloading] * effective_c2[unloading] < 0.0)) if np.any(unloading) else math.nan,
    "unload_residual_c2_p95": _percentile(effective_c2 - c2, 95, unloading),
    "pressed_c0_p95": _percentile(c0, 95, pressed),
    "pressed_c1_p95": _percentile(c1, 95, pressed),
    "low_speed_fit_p95": _percentile(fit_rmse, 95, speed < 5.0),
    "road_speed_fit_p95": _percentile(fit_rmse, 95, speed >= 15.0),
  }
 def _expand(patterns: list[str]) -> list[str]:
  return sorted({path for pattern in patterns for path in glob.glob(pattern)})
 def _self_test() -> None:
  distance = np.linspace(0.0, 20.0, 81)
  coefficients = (0.2, 0.03, 0.004, -0.00005)
  y = sum(coefficient * distance ** power for power, coefficient in enumerate(coefficients))
  slope = coefficients[1] + 2.0 * coefficients[2] * distance + 3.0 * coefficients[3] * distance ** 2
  heading = np.arctan(slope)
  path = ModelPath(distance, y, heading, np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(distance), np.diff(y))))))
  command = fit_native_path(path, 0.0, 0.0, delay=0.1, horizon=7.0)
  assert abs(command.c0 - coefficients[0]) < 2e-3
  assert abs(command.c1 - math.atan(coefficients[1])) < 2e-3
  expected_c2 = 2.0 * coefficients[2] / (1.0 + coefficients[1] ** 2) ** 1.5
  assert abs(command.c2 - expected_c2) < 2e-4
  assert command.fit_rmse < 1e-4
 def main() -> int:
  parser = argparse.ArgumentParser(description=__doc__)
  parser.add_argument("--logs", action="append", help="rlog glob", default=[])
  parser.add_argument("--delay", type=float, default=0.1)
  parser.add_argument("--horizon", type=float, action="append")
  parser.add_argument("--time-horizon", type=float, default=0.0,
                      help="if nonzero, use clamp(speed * seconds, 1 m, --horizon)")
  parser.add_argument("--tau-load", type=float, default=0.75)
  parser.add_argument("--tau-unload", type=float, default=1.3)
  parser.add_argument("--assumed-tau-load", type=float)
  parser.add_argument("--assumed-tau-unload", type=float)
  parser.add_argument("--zero-c3", action="store_true")
  parser.add_argument("--c2-limit", type=float, action="append",
                      help="C2 cap to test; defaults to gentle 0.006 and full 0.02")
  parser.add_argument("--self-test", action="store_true")
  args = parser.parse_args()
  if args.self_test:
    _self_test()
  paths = _expand(args.logs)
  if not paths:
    if args.self_test:
      return 0
    parser.error("at least one usable --logs glob is required")
  samples = load_samples(paths)
  if not samples:
    parser.error("logs contain no active Ford path samples")
  print(f"loaded_logs={len(paths)} samples={len(samples)} tau_load={args.tau_load} tau_unload={args.tau_unload}")
  for horizon in args.horizon or [3.5, 5.0, 7.0, 10.0]:
    for c2_limit in args.c2_limit or [0.006, DBC_CURVATURE[1]]:
      result = evaluate(samples, delay=args.delay, horizon=horizon,
                        tau_load=args.tau_load, tau_unload=args.tau_unload,
                        horizon_time=args.time_horizon,
                        assumed_tau_load=args.assumed_tau_load,
                        assumed_tau_unload=args.assumed_tau_unload,
                        use_c3=not args.zero_c3,
                        c2_limit=c2_limit)
      print(" ".join(f"{key}={value:.8g}" for key, value in result.items()))
  return 0
 if __name__ == "__main__":
  raise SystemExit(main())
@@ -0,0 +1 @@
 """Offline Ford candidate verification. No hardware or CAN transmission."""
@@ -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)
@@ -0,0 +1,224 @@
 """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
 from service records, never from candidate/baseline output validity. Neither
 pass scores counterfactual motion. Source extracts and archived reports are
 read-only; --output selects a separate destination.
 """
 import argparse
 from collections import Counter
 import hashlib
 import json
 from pathlib import Path
 import subprocess
 from types import SimpleNamespace
 import numpy as np
 import opendbc
 from opendbc.can import CANPacker, CANParser
 from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
 from openpilot.cereal import custom
 from openpilot.selfdrive.controls.lib import ford_model_action
 from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController
 from openpilot.selfdrive.controls.lib.ford_path import _model_path
 PINNED_OPENDBC = '72a775d35e54c21ff5c5798acef22016eedcc0a7'
 def revision(directory):
  return subprocess.check_output(['git', '-C', str(directory), 'rev-parse', 'HEAD'], text=True).strip()
 def verify_dependency(expected=PINNED_OPENDBC):
  directory = Path(opendbc.__file__).resolve().parent.parent
  actual = revision(directory)
  if actual != expected:
    raise ValueError(f'Expected opendbc {expected}; imported {directory} at {actual}')
  return directory
 def table(raw, name):
  return dict(zip(raw[name+'_names'], raw[name].T, strict=True))
 def sample(stream, query, *, nearest=False):
  if len(stream['t']) == 0 or np.any(np.diff(stream['t']) < 0):
    raise ValueError('Replay requires nonempty streams in original timestamp order')
  if nearest:
    right = np.clip(np.searchsorted(stream['t'], query), 0, len(stream['t'])-1)
    left = np.maximum(right-1, 0)
    index = np.where(abs(stream['t'][right]-query) < abs(stream['t'][left]-query), right, left)
  else:
    index = np.clip(np.searchsorted(stream['t'], query, side='right')-1, 0, len(stream['t'])-1)
  return {key: values[index] for key, values in stream.items()}
 def field_checks(command, valid, t):
  assert np.isfinite(command).all()
  assert (abs(command[:, :2]) <= [5.1100001, .5000001]).all()
  assert (command[:, 2:] == 0.).all()
  assert (command[~valid] == 0.).all()
  dt = np.r_[.01, np.diff(t)]
  consecutive = valid[1:] & valid[:-1]
  assert (abs(np.diff(command[:, :2], axis=0))[consecutive] <=
          (dt[1:, None]*[4., .5]+[.0100001, .0005001])[consecutive]).all()
  return True
 class WireCheck:
  """Real Float32 publication and in-memory CAN round trips on every cycle."""
  def __init__(self):
    self.packer = CANPacker('ford_lincoln_base_pt')
    self.parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
    self.bus = CanBus(fingerprint={0: {}})
    self.count = 0
  def check(self, path):
    msg = custom.CarControlSP.new_message()
    msg.fordLateralPath.valid = path.valid
    msg.fordLateralPath.pathOffset = path.path_offset
    msg.fordLateralPath.pathAngle = path.path_angle
    msg.fordLateralPath.curvature = path.curvature
    msg.fordLateralPath.curvatureRate = path.curvature_rate
    p = msg.fordLateralPath
    counter = self.count % 16
    packet = create_lat_ctl2_msg(self.packer, self.bus, 2 if path.valid else 0,
                                -p.pathOffset, -p.pathAngle, -p.curvature, -p.curvatureRate, counter)
    self.count += 1
    # Synthetic parser clock only; input times and gaps are never resampled.
    self.parser.update([self.count*10_000_000, [packet]])
    decoded = self.parser.vl['LateralMotionControl2']
    assert abs(decoded['LatCtlPathOffst_L_Actl']+path.path_offset) < 1e-9
    assert abs(decoded['LatCtlPath_An_Actl']+path.path_angle) < 1e-9
    assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
    assert decoded['LatCtl_D2_Rq'] == (2 if path.valid else 0)
    assert decoded['LatCtlPath_No_Cnt'] == counter
 def run(directory, output):
  directory, output = directory.resolve(), output.resolve()
  if output == directory or directory in output.parents:
    raise ValueError('Output must be outside the source route directory')
  dependency = verify_dependency()
  with np.load(directory/'route.npz', allow_pickle=False) as raw:
    streams = {name: table(raw, name) for name in ('controls', 'cs', 'cc', 'model', 'params', 'path')}
    if len(raw['maneuver']):
      raise ValueError('This extract cannot identify the selected maneuver service per cycle; use the integration tests for that source')
    models = [SimpleNamespace(position=SimpleNamespace(x=p[1], y=p[2]), orientation=SimpleNamespace(z=p[3])) for p in raw['model_paths']]
  with np.load(directory/'encoder_comparison.npz', allow_pickle=False) as archive:
    baseline = {key: archive[key] for key in ('t', 'valid', 'action_heading')}
  with np.load(directory/'pose_candidate/pose_replay.npz', allow_pickle=False) as pose:
    np.testing.assert_array_equal(pose['t'], baseline['t'])
    clean = pose['clean']
  controls = streams['controls']
  t = controls['t']
  np.testing.assert_array_equal(t, baseline['t'])
  cs, params = (sample(streams[name], t) for name in ('cs', 'params'))
  cc = sample(streams['cc'], t, nearest=True)  # same-cycle publication match, never a motion sample
  mi = np.clip(np.searchsorted(streams['model']['ns'], controls['model_ns']), 0, len(models)-1)
  model = {key: values[mi] for key, values in streams['model'].items()}
  exact = model['ns'] == controls['model_ns']
  services_valid = ((controls['valid'] == 1) & (cc['valid'] == 1) & (abs(cc['t']-t) < .005) &
                    (cs['valid'] == 1) & (cs['can_valid'] == 1) & (params['valid'] == 1) &
                    (t-params['t'] >= 0.) & (t-params['t'] <= .15) & exact & (model['valid'] == 1))
  dt = np.r_[.01, np.diff(t)]
  core, entry_clock_core, adapter, wire = ModelActionController(), ModelActionController(), FordModelActionController(), WireCheck()
  commands = np.zeros((len(t), 4))
  adapted = np.zeros_like(commands)
  valid = np.zeros(len(t), bool)
  adapter_valid = np.zeros_like(valid)
  reasons = Counter()
  for i, now in enumerate(t):
    selected_model = models[mi[i]] if exact[i] else None
    old_gate = core.update(selected_model, controls['desired'][i], 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], 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
    adapter_valid[i] = new_gate.valid
    reasons[adapter.diagnostics['status']] += 1
    wire.check(new_gate)
    # 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], 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'])
  np.testing.assert_array_equal(commands[:, :2], baseline['action_heading'])
  field_checks(commands, valid, t)
  field_checks(adapted, adapter_valid, t)
  # Recompute the archived cohorts from actual commands, retaining the original
  # interval-clean driver mask and time weights. No recorded yaw performance score.
  v = cs['speed']
  demand = abs(controls['desired'])*v**2
  masks = {'small_request': clean & (demand < .15), 'turn': clean & (demand >= .5)}
  for low, high in ((2, 8), (8, 15), (15, 55)):
    for name in ('small', 'turn'):
      masks[f'{name}_speed_{low}_{high}'] = masks['small_request' if name == 'small' else name] & (v >= low) & (v < high)
  paths = [_model_path(m) for m in models]
  y10 = np.array([np.interp(10., p[0], p[2]) if p is not None else np.nan for p in paths])
  h10 = np.array([np.interp(10., p[0], p[3]) if p is not None else np.nan for p in paths])
  coverage = np.array([p[0][-1] if p is not None else 0. for p in paths])[mi]
  masks['pose_quiet'] = masks['small_request'] & (v >= 8) & (abs(y10[mi]) <= .1) & (abs(h10[mi]) <= np.radians(.5))
  recorded = sample(streams['path'], t, nearest=True)
  recorded = np.column_stack((recorded['c0'], recorded['c1']))
  weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
  cohorts = {}
  previous_report = json.loads((directory/'encoder_comparison.json').read_text())
  for name, mask in masks.items():
    def rms(values, selected=mask):
      return np.sqrt(np.average(values[selected, :2]**2, weights=weight[selected], axis=0)).tolist() if selected.any() else None
    cohorts[name] = {'seconds': float(weight[mask].sum()), 'core_c0_c1_rms': rms(commands), 'recorded_v8_c0_c1_rms': rms(recorded),
                     'adapter_eligible_seconds': float(weight[mask & adapter_valid].sum()),
                     'adapter_c0_c1_rms': rms(adapted, mask & adapter_valid)}
    expected = previous_report['cohorts'][name]
    np.testing.assert_allclose(cohorts[name]['seconds'], expected['seconds'], rtol=0., atol=1e-8)
    np.testing.assert_allclose(cohorts[name]['core_c0_c1_rms'], expected['candidates']['action_heading']['c0_c1_rms'], rtol=0., atol=1e-12)
    np.testing.assert_allclose(cohorts[name]['recorded_v8_c0_c1_rms'], expected['v8_c0_c1_rms'], rtol=0., atol=1e-12)
  root = Path(__file__).resolve().parents[2]
  sources = [Path(__file__), Path(ford_model_action.__file__),
             root/'openpilot/selfdrive/controls/lib/ford_path.py', directory/'route.npz', directory/'metadata.json',
             directory/'encoder_comparison.npz', directory/'encoder_comparison.json', directory/'pose_candidate/pose_replay.npz']
  report = {'scope': '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),
            'adapter_exact_match_with_fresh_engagement_dt': True,
            'core_active_path_shorter_than_7m_cycles': int(np.sum(valid & (coverage < 7.))),
            'adapter_validity_differs_from_archive_cycles': int(np.sum(adapter_valid != valid)),
            'adapter_command_differs_from_archive_cycles': int(np.any(abs(adapted-commands) > 1e-9, axis=1).sum()),
            'adapter_max_absolute_command_difference_c0_c1': np.max(abs(adapted[:, :2]-commands[:, :2]), axis=0).tolist(),
            'field_slew_zero_c2_c3_pass': True, 'float32_can_round_trips': wire.count,
            'timing': 'Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.',
            'eligibility': 'Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.',
            'reference': 'Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.',
            'host_yaw': 'Extract cs.yaw already equals -carState.yawRate. Used only for inherited finite/range gate, never feedback.',
            'cohorts': cohorts, 'workspace_head': revision(root), 'opendbc_import_head': revision(dependency),
            'opendbc_import_path': str(dependency),
            'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}}
  output.mkdir(parents=True, exist_ok=True)
  np.savez_compressed(output/'commands.npz', t=t, core=commands, core_valid=valid, adapter=adapted, adapter_valid=adapter_valid)
  (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
  print(json.dumps({k: v for k, v in report.items() if k not in ('cohorts', 'source_sha256')}, indent=2))
 if __name__ == '__main__':
  parser = argparse.ArgumentParser(description=__doc__)
  parser.add_argument('route_directory', type=Path)
  parser.add_argument('--output', type=Path, required=True)
  args = parser.parse_args()
  run(args.route_directory, args.output)
@@ -0,0 +1,129 @@
 """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
 Float32/CAN packing of the continuous state, independently of host _packed.
 No synthetic plant is fitted or used to claim vehicle tracking performance.
 """
 import argparse
 import hashlib
 import json
 import math
 from pathlib import Path
 from types import SimpleNamespace
 import numpy as np
 from openpilot.cereal import custom
 from openpilot.selfdrive.controls.lib import ford_model_action
 from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
 from opendbc.car.ford.fordcan import create_lat_ctl2_msg
 from tools.ford_pscm_lab.model_action_replay import PINNED_OPENDBC, WireCheck, verify_dependency, revision
 def line(offset, heading=0.):
  s = np.linspace(0., 30., 33)
  return SimpleNamespace(position=SimpleNamespace(x=s*math.cos(heading), y=offset+s*math.sin(heading)),
                         orientation=SimpleNamespace(z=np.full_like(s, heading)))
 def check_raw_packing(wire, controller, path):
  # Set a raw Float32 publication independently of the host quantization helper.
  msg = custom.CarControlSP.new_message()
  msg.fordLateralPath.pathOffset = controller.c0
  msg.fordLateralPath.pathAngle = controller.c1
  raw = create_lat_ctl2_msg(wire.packer, wire.bus, 2 if path.valid else 0, -msg.fordLateralPath.pathOffset,
                            -msg.fordLateralPath.pathAngle, 0., 0., wire.count % 16)
  wire.check(path)
  wire.parser.update([wire.count*10_000_000, [raw]])
  decoded = wire.parser.vl['LateralMotionControl2']
  assert abs(decoded['LatCtlPathOffst_L_Actl']+path.path_offset) < 1e-9
  assert abs(decoded['LatCtlPath_An_Actl']+path.path_angle) < 1e-9
 def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
  dependency = verify_dependency(opendbc_revision)
  if cycles < 1:
    raise ValueError('cycles must be positive')
  rng = np.random.default_rng(seed)
  controller, mirrored, wire = ModelActionController(), ModelActionController(), WireCheck()
  c0 = c1 = 0.
  resets = 0
  rates = (4., .5)
  dt_values = (.002, .003, .01, .013, .05, .1)
  max_continuous_step = np.zeros(2)
  for i in range(cycles):
    offset, heading = float(rng.uniform(-8., 8.)), float(rng.uniform(-1.2, 1.2))
    speed = float(rng.uniform(.3, 55.))
    desired = float(rng.uniform(-.15, .15))
    dt = dt_values[i % len(dt_values)]
    active = i % 137 != 0
    valid = i % 211 != 0
    if i % 307 == 0:
      dt = .101
    model, mirror = line(offset, heading), line(-offset, -heading)
    if i % 401 == 0:
      model.position.y[4] = mirror.position.y[4] = math.nan
    out = controller.update(model, desired, 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])
    if expected_valid:
      target = (max(-5.11, min(5.11, offset+7.*math.sin(heading))), max(-.5, min(.5, max(7., speed)*desired)))
      c0 += max(-4.*dt, min(4.*dt, target[0]-c0))
      c1 += max(-.5*dt, min(.5*dt, target[1]-c1))
      step = abs(np.array([controller.c0, controller.c1])-previous)
      assert (step <= np.array(rates)*dt+1e-10).all()
      max_continuous_step = np.maximum(max_continuous_step, step)
    else:
      c0 = c1 = 0.
      resets += 1
      assert out.path_offset == out.path_angle == 0.
    assert abs(controller.c0-c0) < 1e-10 and abs(controller.c1-c1) < 1e-10
    assert abs(controller.c0+mirrored.c0) < 1e-10 and abs(controller.c1+mirrored.c1) < 1e-10
    assert abs(out.path_offset+other.path_offset) <= .0100001
    assert abs(out.path_angle+other.path_angle) <= .0005001
    assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0.
    check_raw_packing(wire, controller, out)
  # Every representable host C0/C1 value, plus the float32 immediately below,
  # at, and above each half-quantum transition. Seed the slew positions only
  # here to isolate packing from slew; the sequence above checks actual slew.
  boundary_cases = 0
  for field, resolution, low, high in ((0, .01, -5.11, 5.11), (1, .0005, -.5, .5)):
    grid = np.arange(round(low/resolution), round(high/resolution)+1)*resolution
    for value in np.r_[grid, (grid[:-1]+grid[1:])/2.]:
      raw = np.float32(value)
      for scalar in (np.nextafter(raw, np.float32(-np.inf)), raw, np.nextafter(raw, np.float32(np.inf))):
        selected = float(np.clip(scalar, low, high))
        offset, heading = (selected, 0.) if field == 0 else (0., selected)
        controller.c0, controller.c1 = offset, heading
        out = controller.update(line(offset), heading/20., 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': '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'))}}
  output.parent.mkdir(parents=True, exist_ok=True)
  output.write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
  print(json.dumps({k: v for k, v in report.items() if k != 'source_sha256'}, indent=2))
 if __name__ == '__main__':
  parser = argparse.ArgumentParser(description=__doc__)
  parser.add_argument('--cycles', type=int, default=200_000)
  parser.add_argument('--seed', type=int, default=20260907)
  parser.add_argument('--output', type=Path, required=True)
  parser.add_argument('--opendbc-revision', default=PINNED_OPENDBC,
                      help='Exact required dependency commit; defaults to the historical replay pin.')
  args = parser.parse_args()
  run(args.cycles, args.seed, args.output, args.opendbc_revision)
@@ -0,0 +1,62 @@
 import numpy as np
 import pytest
 from openpilot.selfdrive.controls.lib.ford_path import FordPath
 from tools.ford_pscm_lab import model_action_replay as replay
 def test_service_sampling_keeps_original_gaps_and_never_pulls_future_inputs():
  stream = {'t': np.array([1., 1.01, 2.]), 'value': np.array([3., 4., 5.])}
  sampled = replay.sample(stream, np.array([1.005, 1.5, 2.]))
  np.testing.assert_array_equal(sampled['t'], [1., 1.01, 2.])
  np.testing.assert_array_equal(sampled['value'], [3., 4., 5.])
  assert 1.5-sampled['t'][1] > .15  # The adapter sees the gap, not a resampled fresh input.
@pytest.mark.parametrize('times', [[], [1., .9]])
 def test_replay_does_not_sort_away_backward_time_or_invent_missing_streams(times):
  with pytest.raises(ValueError):
    replay.sample({'t': np.array(times)}, np.array([1.]))
 def test_dependency_mismatch_fails_before_replaying(monkeypatch):
  monkeypatch.setattr(replay, 'revision', lambda _: 'wrong_revision')
  with pytest.raises(ValueError, match='Expected opendbc'):
    replay.verify_dependency()
 def test_explicit_stress_dependency_still_requires_an_exact_match(monkeypatch):
  monkeypatch.setattr(replay, 'revision', lambda _: 'deployment_commit')
  replay.verify_dependency('deployment_commit')
  with pytest.raises(ValueError, match='Expected opendbc'):
    replay.verify_dependency('different_commit')
  with pytest.raises(ValueError, match='Expected opendbc'):
    replay.verify_dependency()  # Historical replay never silently follows the local checkout.
 def test_source_route_directory_cannot_be_overwritten(tmp_path):
  with pytest.raises(ValueError, match='outside the source'):
    replay.run(tmp_path, tmp_path/'selected_controller')
@pytest.mark.parametrize('field,value', [(0, 5.12), (1, .501), (2, .00002), (3, .000001), (0, np.nan)])
 def test_field_validation_catches_range_and_zero_c2_c3_violations(field, value):
  command = np.zeros((2, 4))
  command[0, field] = value
  with pytest.raises(AssertionError):
    replay.field_checks(command, np.array([True, True]), np.array([1., 1.01]))
 def test_invalid_cycles_cannot_publish_a_retained_command():
  with pytest.raises(AssertionError):
    replay.field_checks(np.array([[.01, 0., 0., 0.]]), np.array([False]), np.array([1.]))
 def test_field_validation_catches_excessive_slew_with_original_dt():
  with pytest.raises(AssertionError):
    replay.field_checks(np.array([[0., 0., 0., 0.], [.08, 0., 0., 0.]]), np.array([True, True]), np.array([1., 1.01]))
 def test_wire_validator_checks_real_curvature_fields_instead_of_filling_them_with_zero():
  with pytest.raises(AssertionError):
    replay.WireCheck().check(FordPath(True, 0., 0., .001, 0.))
		`@@ -0,0 +1 @@`
							`"""Offline Ford candidate verification. No hardware or CAN transmission."""`