diff --git a/docs/ford_c1_feedback.md b/docs/ford_c1_feedback.md new file mode 100644 index 0000000000..33ca2b439d --- /dev/null +++ b/docs/ford_c1_feedback.md @@ -0,0 +1,124 @@ +# Ford C1 feedback experiment + +The restored original v1 can leave a steering error while C0 and C1 still have +room. Its command law does not directly correct measured steering error. This +experiment keeps that mapping and adds one accumulated C1 correction: + +```text +error = selected_limited_desired_curvature - measured_curvature +correction += error * speed * elapsed_measurement_time +C1_target = original_model_C1 + correction +``` + +Curvature (1/m) multiplied by traveled distance (m) gives heading mismatch in +radians. Applying that mismatch to C1 at **1:1 is an explicit feedback-strength +choice**. Dimensional consistency does not prove that every PSCM responds +correctly to that strength. There is no fitted PSCM response model or new +tunable multiplier. + +For example, at 20 m/s, a constant curvature shortfall of 0.001/m adds 0.02 rad +to C1 over one second when the output can accept it. When measured curvature +matches the request, the correction holds. If the vehicle turns more than +requested, the correction moves in the unwind direction. Changing the model +request still changes the base immediately, subject to the existing slew. + +## Preserved mapping and limits + +- C0 is the current model path's lateral offset at 7 m of arc distance, holding + the available endpoint for shorter paths; its limits remain ±5.11 m and 4 m/s. +- Base C1 is `max(7 m, speed × 1 s) × selected_limited_desired_curvature`, clipped + to ±0.5 rad. Final C1 uses the same ±0.5 rad and 0.5 rad/s limits as v1. +- C2 and C3 are zero. Sign conversion, Float32/CAN rounding, upstream curvature + limiting and the 100 Hz sender retain their existing behavior. + +The core holds three values: unquantized C0, unquantized C1 and the correction. +Zero error from a reset leaves the correction at zero and preserves the old +command arithmetic exactly. There is no separate percentage or distance cap +on the correction. + +## Feedback measurement, timing and limits + +The measurement is `controlsd.curvature`, computed from measured steering +angle with the existing live vehicle parameters. It matches the curvature +used for the desired-versus-actual steering comparison. It is not an independent +measurement of tire slip or the vehicle's actual ground path. CAN yaw remains +an input-health gate and does not drive this feedback. + +The adapter integrates only elapsed time between fresh `carState` publications. +The first publication after reset integrates zero time. Duplicate timestamps +integrate zero; a fresh timestamp accounts for the elapsed measurement interval. +Output slew continues on valid control cycles. Existing service-age, speed, +model-geometry and clock-order gates remain, with the same finite/range check +also applied to measured curvature. Disengagement or invalid input clears all +three core states. + +The correction cannot accumulate farther into an unavailable C1 amplitude or +slew request. Increments that move back toward the available output remain +allowed. Moving the base request does not itself rewrite the correction. + +Fresh PSCM status means a valid message whose original CAN receipt timestamp +is within the existing −5 to +150 ms age allowance. Reached-limit status (2) +prevents extra accumulation in the measured turn direction. An old correction +opposing that direction can return to zero; it cannot be trapped below the +base request by the limit flag. Unwind and base model changes remain available. +Close-to-limit status (1) does not block feedback. Missing or stale status +does not gate it; local amplitude and slew anti-windup still apply. + +Driver steering-pressed, torque above the existing 1 Nm allowance, nonfinite +torque, or fresh driver-limit status (3) clears the correction. Fresh denied +or inactive PSCM status also clears it. The base model request continues +through existing engagement and driver arbitration; clearing the correction +does not bypass the final output slew. + +## Offline evidence and reproduction + +`ford_c1_feedback_validation.json` records the source hashes and completed +checks. Tests exercise build, hold, unwind, saturation, limit flags, immediate +driver input, stale and repeated measurements, invalid inputs and both signs. +Integration tests execute actual controlsd selection and limiting, Float32 +publication, CarControlSP conversion and the Ford CarController CAN builder. +Randomized runs check feedback invariants separately from zero-error +compatibility with the original independent scalar oracle. + +The combined suite passes **511 tests and 9,146 subtests**. Its 178 skips are +in inherited safety base classes or unsupported safety-test variants. Ruff, +the controller's Ty check and settings compilation pass. Feedback stress, +zero-error stress and the b8 replay total **578,569 Float32/CAN round trips**; +the integration test separately verifies 1,010 transmitted packet constructions, +including every counter and checksum. No packets are sent to hardware. + +The b8 replay retains recorded desired/measured curvature, model publications, +driver input and PSCM flags. It compares candidate commands with the restored +v1 at `a7d70e2b0890184636827351e4789d866f2a7c97`. All 160,431 reconstructed +activation decisions and C0 commands match. C1 changes on 58,106 cycles. +At 4:12.493, for example, reconstructed host C1 changes from −0.1625 to +−0.2035 rad; at 3:56.250 it changes from −0.1280 to −0.1080 rad. These are +changes to commands on frozen measurements, not predicted wheel angles. + +Controls publication time proxies the unlogged computation clock, and the +full SubMaster health state cannot be reconstructed. This route uses the +consumed model publication as its reference and has no maneuver-plan messages. +Replay cannot show whether this feedback fixes weak turns, hanging turns or +oscillation. A new drive is needed to measure those outcomes. + +Use the branch's native dependencies and pinned opendbc revision +`c21a9013700734dd20b09e05aa68329ad8cc20f9`: + +```sh +export PYTHONDONTWRITEBYTECODE=1 +export PYTHONPATH=.:opendbc_repo +python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py opendbc_repo/opendbc/safety/tests/test_ford.py +python openpilot/sunnypilot/sunnylink/tools/compile_settings_ui.py --check +python -m tools.ford_pscm_lab.feedback_replay stress --cycles 200000 --output .cache/ford_c1_feedback/feedback_stress.json +python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_c1_feedback/zero_error_stress.json +python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_routeb8 --output .cache/ford_c1_feedback/routeb8 +``` + +The last command requires the existing full-rlog b8 extract (`route.npz`, +`model_paths.npz`, `metadata.json`), identified by hashes in the validation +record. The historical route90/95 replay deliberately sets measured curvature +equal to requested curvature to check zero-error compatibility; it does not +exercise recorded steering feedback. + +Enable using the [existing Sunnylink toggle](ford_model_action_drive_test.md). +The diagnostic identity is `model-action-c1-feedback-v1`. diff --git a/docs/ford_c1_feedback_validation.json b/docs/ford_c1_feedback_validation.json new file mode 100644 index 0000000000..cd80adb03b --- /dev/null +++ b/docs/ford_c1_feedback_validation.json @@ -0,0 +1,258 @@ +{ + "created_at_utc": "2026-09-09T14:45:33.853345+00:00", + "baseline_commit": "a7d70e2b0890184636827351e4789d866f2a7c97", + "deployment_target": { + "repository": "sunnypilot/sunnypilot", + "branch": "hiimisaac-dev" + }, + "scope": "C1 measured-curvature feedback on restored original v1. Offline software validation only; no predicted or measured physical improvement.", + "calibration_approved": false, + "toggle": { + "key": "FordModelActionController", + "default_enabled": false, + "activation": "Existing startup selection after offroad-to-onroad cycle" + }, + "feedback_law": "correction += (desired_curvature - measured_curvature) * speed * elapsed_measurement_time, subject to output and PSCM anti-windup", + "feedback_strength": "Explicit 1:1 heading-error-to-C1 choice; no fitted PSCM plant or new tunable multiplier", + "preserved": [ + "C0 mapping and limits", + "C2=C3=0", + "C1 final amplitude and slew limits", + "100 Hz sender", + "upstream selection and limiting" + ], + "panda_safety_changed": false, + "opendbc_submodule_changed": false, + "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9", + "controller_size": { + "total_lines": 181, + "code_lines_excluding_blanks_comments_docstrings": 123, + "core_persistent_values": 3, + "adapter_timestamps": 3 + }, + "tests": { + "combined_suite": "511 passed, 178 skipped, 9146 subtests passed in 5.14s", + "suite_log_sha256": "001ef6633b22513317593dd8debc160a0ca8aaf78ea53418c5f7a50c370cc818", + "ruff_changed_python": "pass", + "ty_controller": "pass", + "settings_compiler_check": "pass", + "safety_skip_reasons": [ + "SKIPPED [145] ../../../../dev/sunnypilot/.venv/lib/python3.12/site-packages/_pytest/unittest.py:523: Skipped", + "SKIPPED [9] opendbc_repo/opendbc/safety/tests/common.py:64: Safety mode implements no _user_regen_msg", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:51: Skipping test because MADS button is not supported", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:254: Skipping test because MADS button is not supported", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:67: Skipping test because _acc_state_msg is not implemented for this car", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:165: Skipping test because MADS button is not supported", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:165: Skipping test because ACC main is not supported", + "SKIPPED [3] opendbc_repo/opendbc/safety/tests/mads_common.py:411: MADS button not supported", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:378: CAN FD only", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:361: CAN FD only", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:351: CAN FD only", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:327: CAN FD only", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:341: CAN FD only", + "SKIPPED [1] opendbc_repo/opendbc/safety/tests/test_ford.py:320: CAN FD only" + ], + "safety_native_build": "Pinned safety C source is compiled locally by libsafety_py before testing.", + "controlsd_to_can_feedback_integration_frames": 1010, + "integration_checks": "Both signs: build, hold, unwind, rebuild, immediate driver override; actual 100 Hz sender, counter, checksum, fields and publication. Separate integration tests validate PSCM service forwarding.", + "regression_test_evidence": [ + "Nonzero-error integration failed with zero correction before implementing feedback.", + "Both sign tests failed when a reached limit trapped an old opposing correction; they pass after allowing return to zero." + ] + }, + "route_b8": { + "baseline_revision": "a7d70e2b0890184636827351e4789d866f2a7c97", + "baseline_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a", + "cycles": 160431, + "active_cycles": 68217, + "validity_and_c0_match_original_v1_exactly": true, + "status_counts": { + "inactive": 92214, + "active": 68217 + }, + "feedback_enabled_seconds": 598.256888772994, + "pscm_limit_2_seconds": 12.139079590997426, + "c1_changed_cycles": 58106, + "max_abs_c1_change_rad": 0.29800000000000004, + "max_abs_correction_rad": 0.29816844327770786, + "can_round_trips": 160431, + "timing_limit": "Controls publication time proxies the computation clock; full SubMaster checks are unavailable.", + "reference_limit": "Uses exact consumed model publication as reference; the b8 route has no maneuver-plan messages.", + "example_points": [ + { + "time_s": 130.9368894940053, + "old_c0_c1": [ + -0.7400000000000002, + -0.18700000000000006 + ], + "candidate_c0_c1": [ + -0.7400000000000002, + -0.22899999999999998 + ], + "correction_rad": -0.042171663052515254, + "feedback_enabled": true, + "pscm_limited": false + }, + { + "time_s": 235.3960996990063, + "old_c0_c1": [ + -2.04, + -0.40449999999999997 + ], + "candidate_c0_c1": [ + -2.04, + -0.4145 + ], + "correction_rad": -0.00989648519895422, + "feedback_enabled": true, + "pscm_limited": true + }, + { + "time_s": 236.25034470800165, + "old_c0_c1": [ + -1.46, + -0.128 + ], + "candidate_c0_c1": [ + -1.46, + -0.10799999999999998 + ], + "correction_rad": 0.01990758350705991, + "feedback_enabled": true, + "pscm_limited": false + }, + { + "time_s": 252.49320156300382, + "old_c0_c1": [ + -0.6699999999999999, + -0.16249999999999998 + ], + "candidate_c0_c1": [ + -0.6699999999999999, + -0.20350000000000001 + ], + "correction_rad": -0.040907632902654506, + "feedback_enabled": true, + "pscm_limited": false + }, + { + "time_s": 674.430371745002, + "old_c0_c1": [ + 0.4299999999999997, + 0.128 + ], + "candidate_c0_c1": [ + 0.4299999999999997, + 0.1345 + ], + "correction_rad": 0.00639271291315417, + "feedback_enabled": true, + "pscm_limited": false + }, + { + "time_s": 1534.5190040400048, + "old_c0_c1": [ + 2.62, + 0.5 + ], + "candidate_c0_c1": [ + 2.62, + 0.5 + ], + "correction_rad": 0.0, + "feedback_enabled": true, + "pscm_limited": true + }, + { + "time_s": 1562.5074677500015, + "old_c0_c1": [ + -0.1200000000000001, + -0.051000000000000045 + ], + "candidate_c0_c1": [ + -0.1200000000000001, + -0.046499999999999986 + ], + "correction_rad": 0.004453988923883501, + "feedback_enabled": true, + "pscm_limited": false + } + ] + }, + "route_input_sha256": { + "route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b", + "model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615", + "metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c" + }, + "feedback_stress": { + "cycles": 200000, + "mirrored_updates": 200000, + "can_round_trips": 200000, + "checks": "Mirror symmetry, reset/override, amplitude, slew, correction bounds, integration direction/size, PSCM anti-windup, CAN fields.", + "scope": "Numerical software invariants only; no model of vehicle motion.", + "calibration_approved": false, + "controller_sha256": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34" + }, + "zero_error_stress": { + "seed": 20260907, + "random_cycles": 200000, + "mirrored_core_updates": 200000, + "invalid_or_inactive_resets": 3537, + "field_boundary_cases": 18138, + "float32_can_round_trips": 218138, + "analytic_targets_scalar_slew_and_mirror_checks_pass": true, + "direct_raw_float32_packing_matches_host_output": true, + "max_continuous_step_c0_c1": [ + 0.40000000000000147, + 0.05000000000000002 + ], + "calibration_approved": false, + "scope": "Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.", + "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9" + }, + "total_lab_float32_can_round_trips": 578569, + "artifact_sha256": { + ".cache/ford_c1_feedback/routeb8/report.json": "648887eebb76260a60f7c0f0d9aaac83d0f1c06b443e28bc6fdf296bad4526c0", + ".cache/ford_c1_feedback/routeb8/commands.npz": "1aef98365572b3fb3cb8ba2a93cf30041ff3be133718d469145b710f2c94dc32", + ".cache/ford_c1_feedback/feedback_stress.json": "abf4e7bccc1e460008cc7450fcd92e9b2a6108bd71e53a01bdc24e31e5b5ad32", + ".cache/ford_c1_feedback/zero_error_stress.json": "2a3f284e10e5054205a788cce59bcf57bd837e13afff327457244141ba5522f0", + ".cache/ford_c1_feedback/safety_skip_reasons.txt": "5384c82b07b7cc20c6b22b8e94246cb104d53f8866af02b28fda7d4138cf377f" + }, + "native_params": { + "library_sha256": "270bf43241cf7c02cc432cf78ec9411a62d7653ca445695efe785ae82241aa09", + "sources_match_original_rebuild_record": true, + "provenance": "Same locally rebuilt native library and source hashes recorded in ford_model_action_drive_test_validation.json; verified for this run." + }, + "test_environment": { + "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python", + "PYTHONPATH": ".:opendbc_repo:.cache/ford_v6/test_deps", + "LOG_ROOT": "/private/tmp/ford-feedback-logs", + "PARAMS_ROOT": "/private/tmp/ford-feedback-params", + "PYTHONDONTWRITEBYTECODE": "1" + }, + "source_sha256": { + "docs/ford_c1_feedback.md": "c1bc7f24c5ebe28679b4a04d09085d7b937926e63a38d43a3abfac93dfcfa0f9", + "docs/ford_model_action_candidate.md": "20cd8d10008cd796cc8719f5795ee80f50d8133d6e7684fb057a78fb05323fbe", + "docs/ford_model_action_drive_test.md": "7860ae26a61682aff86743ba302eb23c8f271d5700a2e616da1b6b38d438b57d", + "opendbc_repo/opendbc/car/vehicle_model.py": "ddc2a93d9c2b2ef6c9a913a5aef4c51e2bc387db1f7640473657e5ade4e50fac", + "openpilot/selfdrive/controls/controlsd.py": "2b7e246f00bccce3a2bb9f6f44009ca77690cadb8527cd2bdfe855e9ad72ad1e", + "openpilot/selfdrive/controls/lib/drive_helpers.py": "916bcd83c2a909a89795da58c7c43d7b168c9b82e1a6d281484bae45c667c01e", + "openpilot/selfdrive/controls/lib/ford_model_action.py": "4499defbb7fc5ddf5029ca42c549f0935b0758b08818c5bf0490fb52221f9a34", + "openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a", + "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "84113b1b7800c868117af0034278f53a1a6153c7bb5fadc1ea45958e62c4f0d0", + "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "cbe1b2aa1961deba3a42e1d82f5f75ae0c3d7a219428dea5f50cb70e1b27fd11", + "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "c7f5ffd650e804e6e02fa12d435e0867b56b13a49c3d9fa511993188d5cb625a", + "openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py": "f7a956c082a246d9506e21adbf348cbdc7f94d5342d832841058c71f7e264eeb", + "openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py": "7a13dc5ce49b40e27e05e62cdb9ef1bb764de8ed8167f7e982d54a4dffe97ed4", + "openpilot/sunnypilot/sunnylink/settings_ui.json": "7d38f315a7c5ce6d46d01a06f7eaddd4933f85639e5325ff71fdce22866ef401", + "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "410e306958ece12e49fc114707741c57a2dd927c6ba3410e160834e52a759ea9", + "tools/ford_pscm_lab/feedback_replay.py": "ca552217953f3cce35da0b1666252fd43b6f8ab6c067e5c9102da8ea8d97c2f3", + "tools/ford_pscm_lab/model_action_replay.py": "af97c665f342c66b1be2502e188c63e6f3ee106d0a0d5e80997bc3040373ff9f", + "tools/ford_pscm_lab/stress_model_action.py": "0b25188edf2b248ebe741173ce02ce75bd59f1f39fd5bd909d41a3dca2294aa8" + }, + "limitations": [ + "Frozen route replay changes commands only; it cannot establish tracking, unwind response or closed-loop stability.", + "Measured curvature uses the existing steering-angle vehicle model; it is not an independent ground-path measurement.", + "No full device build, device boot, installation or road validation was performed." + ] +} diff --git a/docs/ford_model_action_candidate.md b/docs/ford_model_action_candidate.md index 2f47fa4b10..62b6e09e6a 100644 --- a/docs/ford_model_action_candidate.md +++ b/docs/ford_model_action_candidate.md @@ -5,7 +5,9 @@ stage committed as `7ca3c6e3b`. The candidate is now available behind a separate default-off Sunnylink toggle; see [drive-test setup and validation](ford_model_action_drive_test.md). The counts, source hashes and selector status below describe that earlier -stage, not the subsequent wiring change. +stage. The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md) +to this original mapping; the historical no-feedback description below is +not the current controller specification. The decision is `C0 = current model y(7 m)`, `C1 = max(7 m, speed × 1 s) × selected upstream-limited desiredCurvature`, diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 70cf6875a3..0a785b5575 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -1,83 +1,53 @@ # Ford selected-action drive-test branch -The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind -its own persistent, default-off Sunnylink toggle. The command law and input -gates from the [offline candidate](ford_model_action_candidate.md) are unchanged. -`calibration_approved=false`: offline checks do not establish physical tracking, -turn-exit behavior or closed-loop stability. +The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md) +to the restored original v1 mapping. It is selectable on the **Ford CAN FD +F-150 Lightning** through the existing persistent, default-off Sunnylink +toggle. Offline checks establish software behavior; physical tracking, +turn-exit behavior and closed-loop stability remain unvalidated. ## Select and restore -1. Install branch `hiimisaac-dev` from - `sunnypilot/sunnypilot` on the device using your normal branch-switch process. - Allow its build to finish before changing the setting. +1. Install branch `hiimisaac-dev` from `sunnypilot/sunnypilot` using the device's + normal branch-switch process and allow its build to finish. 2. While offroad, open Sunnylink device settings → Vehicle → Ford and enable **Selected-Action Path Tracking (Experimental)** (`FordModelActionController`). 3. Complete a real offroad-to-onroad cycle. Selection occurs when `controlsd` - starts; changing a stored toggle or disengaging alone cannot swap an active + starts; a stored toggle change or disengagement alone cannot swap an active controller. Initial physical evaluation remains controlled testing. -The startup log event `Ford path controller selected` should report +The startup event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify `hypothesis=model-action-c0-c1-v1` and report the command tuple. +identify **`hypothesis=model-action-c1-feedback-v1`**. They report desired and +measured curvature, base heading, accumulated correction, applied heading, +feedback timing and driver/PSCM gating. `calibration_approved=false` remains. -Turning the new toggle off and completing another offroad-to-onroad cycle -restores **PSCM Coefficient Observer** if selected, otherwise the original -Ford path controller. The stored observer selection is preserved. The candidate -takes priority on the supported vehicle, independently of EPS firmware query -results. Other vehicles retain their existing selection. - -The v8 implementation, its Sunnylink toggle and its dedicated tests are removed. -A leftover `FordVirtualAngleController=1` file cannot enable the new controller. -The shared Float32/CAN rounding helper now lives in `ford_model_action.py`; -unused v8 PSCM-feedback plumbing is removed. Historical v8 route evidence remains -in Git history and the archived validation documents. +Turning the toggle off and completing another offroad-to-onroad cycle restores +**PSCM Coefficient Observer** if selected, otherwise the original Ford path +controller. The stored observer selection is preserved. The experiment takes +priority on the supported vehicle; other vehicles retain their existing +controller. A leftover `FordVirtualAngleController` parameter has no effect. ## Wiring and validation -`Controls.__init__` selects the candidate once at startup. It shares the -existing Ford call path, selected upstream-limited curvature, service gates, -invalid-output disengagement, Float32 publication and downstream CAN builder. -C2 and C3 stay zero. No opendbc pointer or Panda safety change is included. +`controlsd` supplies the selected, upstream-limited desired curvature and the +measured steering-derived curvature already used in its tracking diagnostics. +Fresh steering publications advance C1 feedback. Repeated publications may +advance output slew but cannot integrate the same elapsed interval twice. +Driver override clears the correction. A fresh PSCM reached-limit flag stops +extra outward accumulation while preserving unwind and base model changes. -Sunnylink publishes the toggle through its generated settings schema and -writes the registered Boolean through the existing parameter endpoint. The -offroad UI rule and `needs_onroad_cycle` metadata describe when it can be -changed and when it takes effect. An onroad backend write changes storage -only; the controller continues using its startup selection. +C0 retains the original 7 m model-path mapping. C2 and C3 remain zero. The +existing output limits, 100 Hz sender, Float32 publication and CAN builder +remain in place. No opendbc submodule or Panda safety change is required. -Native validation also exposed a pre-existing `params_keys_by_flag` bug: -every returned buffer referenced the same reusable string. Sunnylink backup -key enumeration could therefore return corrupted names. The bridge now -returns separate strings owned by the parameter handle. Regression tests -check distinct registered keys across flags, and toggle tests check its -persistence and backup registration using the rebuilt native library. - -The current validation record is `ford_model_action_drive_test_validation.json`. -The final combined Ford, Params and Sunnylink suite passes **284 tests and -26 subtests**, with no skips. The candidate has **100% statement and branch -coverage** (87 statements, 26 branches). Ruff, Ty, settings compilation and -both review axes pass. The fresh route/stress runs check **485,238 Float32/CAN -round trips**, including 200,000 randomized and 200,000 mirrored core updates. -The controller is 145 total lines, including 95 code lines excluding comments, -blanks and docstrings; v8's 469-line module is removed. - -The previous 133,550-cycle route reconstruction, 485,238 packing round trips -and mutation probes remain recorded separately in -`ford_model_action_validation.json` at the offline-stage source hashes. - -## Reproduce deployment checks - -Initialize the branch's exact opendbc submodule (`c21a9013700734dd20b09e05aa68329ad8cc20f9`) -and build the native Params library from this branch before testing. - -```sh -export PYTHONDONTWRITEBYTECODE=1 -export PYTHONPATH=.:opendbc_repo -python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py -python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output .cache/ford_model_action_drive_test/stress.json -``` +See [the feedback specification and validation](ford_c1_feedback.md) and +`ford_c1_feedback_validation.json` for the current evidence and reproduction +commands. `ford_model_action_validation.json` and +`ford_model_action_drive_test_validation.json` are historical records for the +original offline candidate and its first wiring, respectively; their counts +and coverage are not claims about the feedback version. The full hardware build and device boot are not performed by these offline -tests. Installing the branch and enabling the toggle are separate actions; -pushing the branch does not change a device's selected software or settings. +checks. Pushing the branch does not install it on the device or change its +stored toggle. diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 5ab125d59a..066d65458e 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -173,11 +173,13 @@ class Controls(ControlsExt): if self.ford_model_action: reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2' self.ford_path = self.ford_path_controller.update( - ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(), + ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(), measurement_time=self.sm.logMonoTime['carState'] * 1e-9, model_time=self.sm.logMonoTime['modelV2'] * 1e-9, reference_time=self.sm.logMonoTime[reference_service] * 1e-9, active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]), + driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque, + pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None, ) if not self.ford_path.valid: CC.latActive = False diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 9f01b43b3f..7e0a36b7d0 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,14 +1,15 @@ -"""Experimental Ford C2-free controller: nearby offset and selected-action heading. +"""Experimental Ford C2-free controller with measured-curvature C1 feedback. Selected only by its explicit toggle. The 7 m station and one-second scale are -engineering choices, not identified PSCM gains or physical calibration. +engineering choices. Feeding integrated heading mismatch into C1 at 1:1 is an +explicit feedback-strength choice, not an identified PSCM model or calibration. """ import math import struct import numpy as np -from opendbc.car.ford.values import FordFlags +from opendbc.car.ford.values import CarControllerParams, FordFlags from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path @@ -51,21 +52,24 @@ def encode_model_action(model, desired_curvature, speed): class ModelActionController: - """Only two control states: unquantized, independently slewed C0 and C1. + """Unquantized C0/C1 slew positions and one C1 feedback correction. - Freshness and engagement belong to the caller. No measured yaw, model - history, heading integral, blending or release modes enter the law. + Feedback integrates requested minus measured curvature over traveled distance. + Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. """ - __slots__ = ('c0', 'c1') + __slots__ = ('c0', 'c1', 'correction') def __init__(self): self.reset() def reset(self): - self.c0 = self.c1 = 0. + self.c0 = self.c1 = self.correction = 0. - def update(self, model, desired_curvature, *, speed, dt, active=True, valid=True): - if not active or not valid or not _finite(dt) or not .002 <= dt <= .1: + def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, + feedback_dt=None, feedback_enabled=True, pscm_limited=False): + feedback_dt = dt if feedback_dt is None else feedback_dt + if (not active or not valid or not _finite(dt, feedback_dt, current_curvature) or not .002 <= dt <= .1 + or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1.): self.reset() return FordPath() target = encode_model_action(model, desired_curvature, speed) @@ -73,8 +77,27 @@ class ModelActionController: self.reset() return FordPath() c0 = float(np.clip(target.path_offset, -5.11, 5.11)) - c1 = float(np.clip(target.path_angle, -.5, .5)) + base_c1 = float(np.clip(target.path_angle, -.5, .5)) + lower = max(-.5, self.c1-.5*dt) + upper = min(.5, self.c1+.5*dt) + if not feedback_enabled: + self.correction = 0. + else: + increment = (desired_curvature-current_curvature)*speed*feedback_dt + # LimitReached inhibits only extra demand in the measured turn direction. + # Opposing correction and changes to the model request remain available. + direction = current_curvature if current_curvature else self.c1 + if pscm_limited and increment*direction > 0.: + # An old opposing correction may return to zero; don't trap it below + # the base request just because the PSCM now reports a limit. + increment = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.))) + # Integrate only as far as this cycle's amplitude/slew envelope permits. + # If the base moved outside that envelope, allow increments toward it; + # never rewrite existing correction merely because the base changed. + request = base_c1+self.correction + self.correction += float(np.clip(increment, min(lower-request, 0.), max(upper-request, 0.))) self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt)) + c1 = float(np.clip(base_c1+self.correction, -.5, .5)) self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt)) return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.) @@ -83,13 +106,13 @@ class FordModelActionController: """Input adapter for the opt-in selected-action controller. controlsd owns upstream selection/limiting and service health. This adapter - checks ages and clock order, then supplies elapsed time to the two-state - core. Its timestamps and diagnostics never affect the targets. Raw model - geometry is checked on every cycle, even at a repeated model timestamp. + checks ages and clock order, then supplies elapsed time to the three-state + core. Feedback advances once per fresh steering measurement; repeated samples + can still advance output slew. Raw model geometry is checked on every cycle. - Yaw is checked only for the inherited finite/range input gate. Engagement - and downstream driver arbitration still apply. This controller does not use - PSCM status or driver torque as control-law inputs. + CAN yaw remains a health gate, not the feedback measurement. Driver override + clears the correction. Fresh PSCM limits only inhibit outward integration; + neither a limit nor a repeated measurement freezes the model request. """ def __init__(self): self.core = ModelActionController() @@ -98,41 +121,54 @@ class FordModelActionController: def reset(self, status='inactive'): self.core.reset() self.last_time = self.last_measurement_time = self.last_model_time = None - self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-v1', + self.diagnostics = {'status': status, 'hypothesis': 'model-action-c1-feedback-v1', 'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)} - def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time, - active, valid=True): + def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time, + reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None): reason = None if not active: reason = 'inactive' elif not valid: reason = 'invalid_service' - elif not _finite(desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time): + elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time): reason = 'nonfinite' elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)): reason = 'stale_input' - elif not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1: + elif not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1 or abs(current_curvature) > 1: reason = 'input_range' if reason is not None: self.reset(reason) return FordPath() dt = .01 if self.last_time is None else now - self.last_time - if not .002 <= dt <= .1 or (self.last_measurement_time is not None and measurement_time < self.last_measurement_time) or ( + feedback_dt = 0. if self.last_measurement_time is None else measurement_time-self.last_measurement_time + if not .002 <= dt <= .1 or not 0. <= feedback_dt <= .15 or ( self.last_model_time is not None and model_time < self.last_model_time ): self.reset('timing_reset') return FordPath() - command = self.core.update(model, desired_curvature, speed=speed, dt=dt) + status_fresh = (pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0 + and -.005 <= now-pscm_status.canMonoTime*1e-9 <= .15) + pscm_limited = bool(status_fresh and pscm_status.limit == 2) + driver_override = bool(driver_pressed or not _finite(driver_torque) + or abs(driver_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + or (status_fresh and pscm_status.limit == 3)) + feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2))) + command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt, + feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited) if not command.valid: self.reset('invalid_path') return command self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time - self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-v1', + self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v1', 'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature, 'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time, 'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1, + 'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt, + 'heading_feedforward': float(np.clip(max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature, -.5, .5)), + 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, + 'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh), 'command': (command.path_offset, command.path_angle, 0., 0.)} return command diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index 1e5fdfbf3c..8b82e271e0 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -48,12 +48,12 @@ class TestFordControlsLogging(unittest.TestCase): def test_candidate_diagnostics_identify_the_experiment_and_do_not_claim_calibration(self): controller = FordModelActionController() for active, valid in ((False, True), (True, True), (True, False)): - controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1., + controller.update(circle(.01), .005, current_curvature=.0025, yaw_rate=.05, speed=20., now=1., measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid) controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025, sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000})) record = self.emit_controls_event('Ford C2-free path tracking', controls) - self.assertEqual(record['hypothesis'], 'model-action-c0-c1-v1') + self.assertEqual(record['hypothesis'], 'model-action-c1-feedback-v1') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action.py b/openpilot/selfdrive/controls/tests/test_ford_model_action.py index b5466b542a..653d5b68ec 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action.py @@ -42,27 +42,27 @@ def test_centering_information_is_independent_of_action_and_not_scaled_with_spee assert target.path_angle == pytest.approx(sign*.2) # No 10 m cap at highway speed. -def test_two_actuator_positions_are_sufficient_for_every_next_output(): +def test_three_control_states_are_sufficient_for_every_next_output(): controller = ModelActionController() assert not hasattr(controller, '__dict__') for i in range(300): copied = ModelActionController() - copied.c0, copied.c1 = controller.c0, controller.c1 + copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction model = straight(.2*math.sin(i*.1)) kwargs = {'speed': 20., 'dt': .01} desired = .005*math.cos(i*.03) - assert controller.update(model, desired, **kwargs) == copied.update(model, desired, **kwargs) + assert controller.update(model, desired, current_curvature=0., **kwargs) == copied.update(model, desired, current_curvature=0., **kwargs) def test_held_turn_releases_without_a_bias_tail_or_sign_reversal(): for sign in (-1., 1.): controller = ModelActionController() for _ in range(400): - out = controller.update(circle(sign*.01), sign*.01, speed=20., dt=.01) + out = controller.update(circle(sign*.01), sign*.01, current_curvature=sign*.01, speed=20., dt=.01) assert out.path_angle == pytest.approx(sign*.2) previous = np.array([out.path_offset, out.path_angle]) for desired in sign*np.linspace(.01, 0., 101): - out = controller.update(straight(), desired, speed=20., dt=.01) + out = controller.update(straight(), desired, current_curvature=desired, speed=20., dt=.01) values = np.array([out.path_offset, out.path_angle]) assert (abs(values) <= abs(previous)+1e-8).all() assert (sign*values >= -1e-8).all() @@ -73,13 +73,13 @@ def test_held_turn_releases_without_a_bias_tail_or_sign_reversal(): def test_current_model_replacement_leaves_only_independent_actuator_slew(): controller = ModelActionController() for _ in range(150): - controller.update(straight(1.), .04, speed=20., dt=.01) + controller.update(straight(1.), .04, current_curvature=.04, speed=20., dt=.01) for _ in range(25): - out = controller.update(straight(), 0., speed=20., dt=.01) + out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01) assert out.path_offset == pytest.approx(0.) assert out.path_angle > 0. # C1 cannot hold C0 during its longer release. for _ in range(75): - out = controller.update(straight(), 0., speed=20., dt=.01) + out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01) assert out == FordPath(True, 0., 0., 0., 0.) @@ -87,12 +87,12 @@ def test_current_model_replacement_leaves_only_independent_actuator_slew(): def test_invalid_or_inactive_input_clears_state_before_reengagement(overrides): controller = ModelActionController() for _ in range(100): - controller.update(straight(.5), .01, speed=20., dt=.01) + controller.update(straight(.5), .01, current_curvature=.01, speed=20., dt=.01) kwargs = {'speed': 20., 'dt': .01, 'active': True, 'valid': True} kwargs.update(overrides) - assert controller.update(straight(), 0., **kwargs) == FordPath() + assert controller.update(straight(), 0., current_curvature=0., **kwargs) == FordPath() assert (controller.c0, controller.c1) == (0., 0.) - assert controller.update(straight(), 0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.) + assert controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.) def test_malformed_geometry_and_nonfinite_action_never_create_an_active_command(): @@ -108,7 +108,7 @@ def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2( previous = np.zeros(2) for i in range(600): sign = 1. if i < 300 else -1. - out = controller.update(straight(sign*8.), sign*.1, speed=30., dt=.01) + out = controller.update(straight(sign*8.), sign*.1, current_curvature=sign*.1, speed=30., dt=.01) fields = np.array([out.path_offset, out.path_angle]) assert (abs(fields) <= [5.1100001, .5000001]).all() assert (abs(fields-previous) <= [.0500001, .0055001]).all() @@ -133,8 +133,8 @@ def test_short_path_holds_available_endpoint_without_extrapolation(): def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry(): model = make_model([0., 1e308, -1e308], [0., 0., 0.], [0., 0., 0.]) controller = ModelActionController() - controller.update(straight(.4), .01, speed=20., dt=.01) - assert controller.update(model, .01, speed=20., dt=.01) == FordPath() + controller.update(straight(.4), .01, current_curvature=.01, speed=20., dt=.01) + assert controller.update(model, .01, current_curvature=.01, speed=20., dt=.01) == FordPath() assert (controller.c0, controller.c1) == (0., 0.) @@ -143,9 +143,9 @@ def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry(): def test_malformed_numeric_input_resets_without_throwing(field, value): controller = ModelActionController() kwargs = {'speed': 20., 'dt': .01, 'desired_curvature': .01} - controller.update(straight(.4), **kwargs) + controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs) kwargs[field] = value - assert controller.update(straight(.4), **kwargs) == FordPath() + assert controller.update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs) == FordPath() assert (controller.c0, controller.c1) == (0., 0.) @@ -160,8 +160,8 @@ def test_malformed_numeric_input_resets_without_throwing(field, value): ]) def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model): controller = ModelActionController() - controller.update(straight(.4), .01, speed=20., dt=.01) - assert controller.update(model, .01, speed=20., dt=.01) == FordPath() + controller.update(straight(.4), .01, current_curvature=.01, speed=20., dt=.01) + assert controller.update(model, .01, current_curvature=.01, speed=20., dt=.01) == FordPath() assert (controller.c0, controller.c1) == (0., 0.) @@ -173,7 +173,7 @@ def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model): def test_domain_and_elapsed_time_boundaries(field, value, valid): kwargs = {'speed': 20., 'desired_curvature': .01, 'dt': .01} kwargs[field] = value - assert ModelActionController().update(straight(.4), **kwargs).valid == valid + assert ModelActionController().update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs).valid == valid def test_arc_station_not_forward_x_or_model_heading_determines_offset(): @@ -188,6 +188,6 @@ def test_arc_station_not_forward_x_or_model_heading_determines_offset(): def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew(): model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.]) assert encode_model_action(model, .01, 20.) == FordPath(True, .4, .2, 0., 0.) - out = ModelActionController().update(model, .01, speed=20., dt=.002) + out = ModelActionController().update(model, .01, current_curvature=.01, speed=20., dt=.002) assert out.path_offset == pytest.approx(.01) assert out.path_angle == pytest.approx(.001) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index d705fb627c..ab05f2b27d 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -15,6 +15,7 @@ import pytest from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.ford.carcontroller import CarController +from opendbc.car.ford.fordcan import calculate_lat_ctl2_checksum from opendbc.car.ford.values import FordFlags from openpilot.cereal import custom from openpilot.selfdrive.car.helpers import convert_carControlSP @@ -29,6 +30,7 @@ def update(controller, now=1., **overrides): kwargs = {'model': straight(.4), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now, 'model_time': now, 'measurement_time': now, 'reference_time': now, 'active': True} kwargs.update(overrides) + kwargs.setdefault('current_curvature', kwargs['desired_curvature']) # Preserve feedforward-only compatibility probes. return controller.update(**kwargs) @@ -149,7 +151,7 @@ class Subscriptions: frame = 1 def __init__(self, maneuver): - self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True} + self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True, 'carStateSP': True} self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000} self.failed = set() self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)} @@ -217,3 +219,73 @@ def test_actual_controlsd_service_gates(pipeline, maneuver, failed): exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}) assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline, sign): + call, publication = pipeline + controls, sm = startup(), Subscriptions(False) + controls.sm, controls.desired_curvature = sm, sign*.004 + model = straight(.4) + model.action = SimpleNamespace(desiredCurvature=sign*.004) + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=20., yawRate=.2, canValid=True, steeringPressed=False, steeringTorque=0.) + cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1') + downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP()) + vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int), + lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) + parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main) + frame = 0 + for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02), + (sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.), + (sign*.003, 0., 100, sign*.02), (0., 1.0625, 5, 0.)]: + for _ in range(count): + now = 1.+frame*.01 + controls.curvature, cs.steeringTorque = measured, torque + sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) + environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, + 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, + 'time': SimpleNamespace(monotonic=lambda now=now: now)} + exec(call, environment) + msg = custom.CarControlSP.new_message() + exec(publication, {'self': controls, 'CC_SP': msg}) + _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9)) + received = parser.update([round(now*1e9), packets]) + assert parser.dbc.name_to_msg['LateralMotionControl2'].address in received + wire = parser.vl['LateralMotionControl2'] + assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle) + assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset) + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + assert wire['LatCtl_D2_Rq'] == 2 + assert wire['LatCtlPath_No_Cnt'] == frame % 16 + address = parser.dbc.name_to_msg['LateralMotionControl2'].address + packet = next(packet for packet in packets if packet[0] == address) + assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) + frame += 1 + assert controls.ford_path_controller.core.correction == pytest.approx(expected) + assert controls.ford_path.path_angle == pytest.approx(sign*.08+expected) + assert controls.ford_path.path_offset == pytest.approx(.4) + + +@pytest.mark.parametrize('service_valid', [False, True]) +def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, service_valid): + controls, sm = startup(), Subscriptions(False) + controls.sm, controls.desired_curvature = sm, .004 + model = straight(.4) + model.action = SimpleNamespace(desiredCurvature=.004) + cc = structs.CarControl(latActive=True) + cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.) + for frame in range(101): + now = 1.+frame*.01 + controls.curvature = .004 if frame < 100 else .003 + sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) + sm.valid['carStateSP'] = service_valid + status = sm['carStateSP'].fordPscmStatus + status.valid, status.canMonoTime, status.limit, status.lateralState = True, round(now*1e9), 2, 2 + exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, + 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, + 'time': SimpleNamespace(monotonic=lambda now=now: now)}) + controller = controls.ford_path_controller + assert controller.diagnostics['pscm_limited'] is service_valid + assert controller.core.correction == pytest.approx(0. if service_valid else .0002) + assert cc.latActive and controls.ford_path.valid diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py new file mode 100644 index 0000000000..ad282de6f2 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py @@ -0,0 +1,182 @@ +"""C1 feedback behavior; these tests do not simulate a Ford steering plant.""" +import math +from types import SimpleNamespace + +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +def tick(controller, desired, measured, **overrides): + kwargs = {'current_curvature': measured, 'speed': 20., 'dt': .01} + kwargs.update(overrides) + return controller.update(straight(.4), desired, **kwargs) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign): + controller, matched = ModelActionController(), ModelActionController() + for _ in range(100): + tick(controller, sign*.004, sign*.004) + for _ in range(100): + out = tick(controller, sign*.004, sign*.003) + baseline = tick(matched, sign*.004, sign*.004) + assert controller.correction == pytest.approx(sign*.02) + assert out.path_angle == pytest.approx(sign*.1) + assert out.path_offset == baseline.path_offset == pytest.approx(.4) + for _ in range(100): + out = tick(controller, sign*.004, sign*.004) + assert controller.correction == pytest.approx(sign*.02) + assert out.path_angle == pytest.approx(sign*.1) + for _ in range(200): + out = tick(controller, sign*.004, sign*.005) + assert controller.correction == pytest.approx(-sign*.02) + assert out.path_angle == pytest.approx(sign*.06) + assert out.curvature == out.curvature_rate == 0. + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_amplitude_and_slew_limits_do_not_store_unavailable_feedback(sign): + controller = ModelActionController() + # The unchanged model request is already ahead of the output slew. + for _ in range(10): + tick(controller, sign*.01, 0.) + assert controller.correction == 0. + for _ in range(1000): + before = controller.c1 + tick(controller, sign*.01, -sign*.9) + assert abs(controller.c1-before) <= .0050000001 + assert abs(controller.correction) <= .3000000001 + assert controller.c1 == pytest.approx(sign*.5) + assert controller.correction == pytest.approx(sign*.3) + for _ in range(200): + tick(controller, sign*.01, 0.) + assert controller.correction == pytest.approx(sign*.3) + tick(controller, sign*.01, sign*.02) + assert sign*controller.correction < .3 # Unwind is allowed at the cap. + assert sign*controller.c1 < .5 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign): + controller = ModelActionController() + for _ in range(100): + tick(controller, sign*.004, sign*.004) + for _ in range(100): + tick(controller, sign*.004, sign*.003, pscm_limited=True) + assert controller.correction == 0. + out = tick(controller, sign*.004, sign*.005, pscm_limited=True) + assert sign*controller.correction < 0. + # A limit cannot stall the new model request itself or its unwind slew. + for _ in range(100): + out = tick(controller, 0., 0., pscm_limited=True) + assert abs(out.path_angle) < .001 + assert out.path_offset == pytest.approx(.4) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign): + controller = ModelActionController() + controller.correction = -sign*.02 + controller.c1 = sign*.06 + for _ in range(200): + out = tick(controller, sign*.004, sign*.003, pscm_limited=True) + assert controller.correction == pytest.approx(0.) + assert out.path_angle == pytest.approx(sign*.08) + + +def test_driver_intervention_clears_feedback_through_existing_output_slew(): + controller = ModelActionController() + for _ in range(100): + tick(controller, .004, .004) + for _ in range(100): + tick(controller, .004, .003) + assert controller.correction > 0. + previous = controller.c1 + tick(controller, .004, -.01, feedback_enabled=False) + assert controller.correction == 0. + assert abs(controller.c1-previous) <= .0050000001 + for _ in range(100): + out = tick(controller, .004, -.01, feedback_enabled=False) + assert controller.correction == 0. + assert out.path_angle == pytest.approx(.08) + + +@pytest.mark.parametrize('field,value', [('current_curvature', math.nan), ('current_curvature', None), + ('current_curvature', 1.01), ('feedback_dt', math.nan), + ('feedback_dt', -.001), ('feedback_dt', .151), ('active', False)]) +def test_bad_feedback_inputs_and_disengagement_clear_every_control_state(field, value): + controller = ModelActionController() + controller.correction = .03 + out = tick(controller, .004, .003, **{field: value}) + assert out == FordPath() + assert (controller.c0, controller.c1, controller.correction) == (0., 0., 0.) + + +def adapter_tick(controller, now, **overrides): + kwargs = {'current_curvature': .003, 'speed': 20., 'yaw_rate': 0., 'now': now, + 'measurement_time': now, 'model_time': now, 'reference_time': now, 'active': True} + kwargs.update(overrides) + return controller.update(straight(.4), .004, **kwargs) + + +def status(now, **overrides): + fields = {'valid': True, 'canMonoTime': round(now*1e9), 'limit': 0, 'lateralState': 2, 'denied': False} + fields.update(overrides) + return SimpleNamespace(**fields) + + +def test_repeated_steering_samples_only_advance_output_slew(): + controller = FordModelActionController() + for i in range(100): + adapter_tick(controller, 1.+i*.01, current_curvature=.004) + before = controller.core.correction + for i in range(1, 6): + adapter_tick(controller, 1.99+i*.01, measurement_time=1.99) + assert controller.core.correction == before + adapter_tick(controller, 2.05) + assert controller.core.correction == pytest.approx(.02*.06) + assert controller.diagnostics['feedback_dt'] == pytest.approx(.06) + + +@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': 1.01}, + {'driver_torque': -1.01}, {'driver_torque': math.nan}, + {'pscm_status': status(2.01, limit=3)}, + {'pscm_status': status(2.01, denied=True)}, + {'pscm_status': status(2.01, lateralState=1)}]) +def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides): + controller = FordModelActionController() + for i in range(101): + adapter_tick(controller, 1.+i*.01) + assert controller.core.correction > 0. + assert adapter_tick(controller, 2.01, **overrides).valid + assert controller.core.correction == 0. + assert not controller.diagnostics['feedback_enabled'] + + +@pytest.mark.parametrize('overrides,limited', [({}, True), ({'valid': False}, False), + ({'canMonoTime': 0}, False), ({'canMonoTime': 1_800_000_000}, False), + ({'canMonoTime': 2_020_000_000}, False), ({'limit': 1}, False)]) +def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, limited): + controller = FordModelActionController() + for i in range(100): + adapter_tick(controller, 1.+i*.01, current_curvature=.004) + adapter_tick(controller, 2., pscm_status=status(2., **{'limit': 2, **overrides})) + assert controller.diagnostics['pscm_limited'] is limited + assert (controller.core.correction == 0.) is limited + + +def test_measurement_cadence_preserves_elapsed_distance_integration(): + results = [] + for period in (1, 2, 5): + controller = FordModelActionController() + for i in range(101): + now = 1.+i*.01 + adapter_tick(controller, now, current_curvature=.004) + for i in range(1, 101): + now = 2.+i*.01 + adapter_tick(controller, now, measurement_time=2.+(i//period)*period*.01) + results.append(controller.core.correction) + assert results == pytest.approx([.02, .02, .02]) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 8903610d1b..709a7fde6d 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2183,8 +2183,8 @@ "widget": "toggle", "needs_onroad_cycle": true, "title": "Selected-Action Path Tracking (Experimental)", - "description": "Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.", - "details": "Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", + "description": "Follow the selected steering plan using model-path centering and measured steering feedback on the Ford CAN FD F-150 Lightning.", + "details": "Keeps the nearby model-path centering request and adjusts the heading request using the difference between requested and measured steering. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", "enablement": [ { "type": "offroad_only" diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index b4c1bf9e7d..ae7933fae0 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -14,8 +14,8 @@ sections: widget: toggle needs_onroad_cycle: true title: Selected-Action Path Tracking (Experimental) - description: Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning. - details: Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. + description: Follow the selected steering plan using model-path centering and measured steering feedback on the Ford CAN FD F-150 Lightning. + details: Keeps the nearby model-path centering request and adjusts the heading request using the difference between requested and measured steering. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. enablement: - $ref: '#/macros/offroad' - key: FordPscmObserver diff --git a/tools/ford_pscm_lab/feedback_replay.py b/tools/ford_pscm_lab/feedback_replay.py new file mode 100644 index 0000000000..74cba950b2 --- /dev/null +++ b/tools/ford_pscm_lab/feedback_replay.py @@ -0,0 +1,181 @@ +"""Inspect C1 feedback commands on frozen route measurements, never vehicle motion. + +Input: route.npz, model_paths.npz and metadata.json from the full-rlog extractor. +The recorded desired/actual curvature, clocks, driver input and PSCM flags stay +fixed. This verifies software behavior, not counterfactual physical tracking. +""" +import argparse +from collections import Counter +import hashlib +import json +from pathlib import Path +import subprocess +from types import ModuleType, SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib import ford_model_action +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController +from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency + + +BASELINE = 'a7d70e2b0890184636827351e4789d866f2a7c97' +OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9' + + +def original_controller(): + root = Path(__file__).resolve().parents[2] + source = subprocess.check_output(['git', '-C', str(root), 'show', + f'{BASELINE}:openpilot/selfdrive/controls/lib/ford_model_action.py'], text=True) + module = ModuleType('ford_original_v1') + exec(compile(source, f'{BASELINE}:ford_model_action.py', 'exec'), module.__dict__) + return module.FordModelActionController(), hashlib.sha256(source.encode()).hexdigest() + + +def replay(directory, output): + directory, output = directory.resolve(), output.resolve() + if output == directory or directory in output.parents: + raise ValueError('Output must be outside the source route directory') + verify_dependency(OPENDBC) + metadata = json.loads((directory/'metadata.json').read_text()) + with np.load(directory/'route.npz', allow_pickle=False) as z: + r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')} + with np.load(directory/'model_paths.npz', allow_pickle=False) as z: + model_ns, paths = z['ns'], z['paths'] + for stream in r.values(): + if np.any(np.diff(stream['t']) < 0): + raise ValueError('Source stream contains a backward clock') + c = r['controls'] + t = c['t'] + cs, pa, ps = (sample(r[k], t) for k in ('cs', 'params', 'pscm')) + cc = sample(r['cc'], t, nearest=True) + mi = np.clip(np.searchsorted(r['model']['ns'], c['model_ns']), 0, len(r['model']['ns'])-1) + exact = r['model']['ns'][mi] == c['model_ns'] + np.testing.assert_array_equal(model_ns, r['model']['ns']) + models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths] + old, baseline_hash = original_controller() + controller, wire_check = FordModelActionController(), WireCheck() + baseline = np.zeros((len(t), 4)) + commands = np.zeros_like(baseline) + old_valid = np.zeros(len(t), bool) + valid = np.zeros(len(t), bool) + correction = np.zeros(len(t)) + feedback_dt = np.zeros(len(t)) + feedback_enabled = np.zeros(len(t), bool) + pscm_limited = np.zeros(len(t), bool) + reasons = Counter() + for i, now in enumerate(t): + model_time = r['model']['t'][mi[i]] + service_valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] + and pa['valid'][i] and r['model']['valid'][mi[i]] and exact[i] + and abs(cc['t'][i]-now) < .005 and 0. <= now-pa['t'][i] <= .15) + common = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now, + 'measurement_time': cs['t'][i], 'model_time': model_time, 'reference_time': model_time, + 'active': bool(cc['active'][i]), 'valid': service_valid} + model = models[mi[i]] if exact[i] else None + status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9), + limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i])) + previous = old.update(model, c['desired'][i], **common) + command = controller.update(model, c['desired'][i], current_curvature=c['measured'][i], + driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i], pscm_status=status, **common) + for destination, result in ((baseline, previous), (commands, command)): + destination[i] = result.path_offset, result.path_angle, result.curvature, result.curvature_rate + old_valid[i], valid[i] = previous.valid, command.valid + d = controller.diagnostics + reasons[d['status']] += 1 + correction[i] = controller.core.correction + feedback_dt[i] = d.get('feedback_dt', 0.) + feedback_enabled[i] = d.get('feedback_enabled', False) + pscm_limited[i] = d.get('pscm_limited', False) + wire_check.check(command) + field_checks(commands, valid, t) + np.testing.assert_array_equal(valid, old_valid) + np.testing.assert_array_equal(commands[:, 0], baseline[:, 0]) + assert np.all(correction[~feedback_enabled] == 0.) + assert np.all(abs(correction) <= 1.+1e-10) + weight = np.minimum(np.diff(t, append=t[-1]+.01), .03) + report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': baseline_hash, + 'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()), + 'validity_and_c0_match_original_v1_exactly': True, 'status_counts': dict(reasons), + 'feedback_enabled_seconds': float(weight[feedback_enabled].sum()), + 'pscm_limit_2_seconds': float(weight[pscm_limited & valid].sum()), + 'c1_changed_cycles': int((abs(commands[:, 1]-baseline[:, 1]) > 1e-8).sum()), + 'max_abs_c1_change_rad': float(abs(commands[:, 1]-baseline[:, 1]).max()), + 'max_abs_correction_rad': float(abs(correction).max()), 'can_round_trips': wire_check.count, + 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in + (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json', + Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}, + 'timing_limit': 'Controls publication time proxies the computation clock; full SubMaster checks are unavailable.', + 'reference_limit': 'Uses exact consumed model publication as reference; the b8 route has no maneuver-plan messages.'} + report['example_points'] = [] + for seconds in (130.937, 235.396, 236.250, 252.493, 674.430, 1534.519, 1562.507): + i = int(np.argmin(abs(t-metadata['t0']-seconds))) + report['example_points'].append({'time_s': float(t[i]-metadata['t0']), 'old_c0_c1': baseline[i, :2].tolist(), + 'candidate_c0_c1': commands[i, :2].tolist(), 'correction_rad': float(correction[i]), + 'feedback_enabled': bool(feedback_enabled[i]), 'pscm_limited': bool(pscm_limited[i])}) + output.mkdir(parents=True, exist_ok=True) + np.savez_compressed(output/'commands.npz', t=t-metadata['t0'], baseline=baseline, candidate=commands, valid=valid, + correction=correction, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited) + (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n') + print(json.dumps({k: v for k, v in report.items() if k != 'source_sha256'}, indent=2)) + + +def stress(cycles, output): + verify_dependency(OPENDBC) + rng = np.random.default_rng(20260909) + controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck() + for i in range(cycles): + desired, measured = rng.uniform(-.1, .1, 2) + speed, dt, offset = rng.uniform(.3, 55.), rng.uniform(.002, .1), rng.uniform(-8., 8.) + active, enabled, limited = i % 211 != 0, i % 97 != 0, i % 7 == 0 + feedback_dt = 0. if i % 5 == 0 else rng.uniform(.002, .15) + previous = controller.c0, controller.c1, controller.correction + args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active, + 'feedback_enabled': enabled, 'pscm_limited': limited} + def model(y): + return SimpleNamespace(position=SimpleNamespace(x=[0., 20.], y=[y, y]), orientation=SimpleNamespace(z=[0., 0.])) + out = controller.update(model(offset), desired, current_curvature=measured, **args) + other = mirror.update(model(-offset), -desired, current_curvature=-measured, **args) + state = controller.c0, controller.c1, controller.correction + mirrored = mirror.c0, mirror.c1, mirror.correction + np.testing.assert_allclose(state, -np.array(mirrored), rtol=0., atol=1e-10) + assert abs(controller.c0) <= 5.11+1e-10 and abs(controller.c1) <= .5+1e-10 and abs(controller.correction) <= 1.+1e-10 + if active: + assert abs(controller.c0-previous[0]) <= 4.*dt+1e-10 + assert abs(controller.c1-previous[1]) <= .5*dt+1e-10 + if enabled: + delta = controller.correction-previous[2] + request = (desired-measured)*speed*feedback_dt + assert delta*request >= -1e-10 and abs(delta) <= abs(request)+1e-10 + if limited and request*(measured if measured else previous[1]) > 0.: + assert abs(controller.correction) <= abs(previous[2])+1e-10 + assert controller.correction*previous[2] >= -1e-10 + else: + assert controller.correction == 0. + else: + assert state == (0., 0., 0.) + assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0. + wire.check(out) + report = {'cycles': cycles, 'mirrored_updates': cycles, 'can_round_trips': wire.count, + 'checks': 'Mirror symmetry, reset/override, amplitude, slew, correction bounds, integration direction/size, PSCM anti-windup, CAN fields.', + 'scope': 'Numerical software invariants only; no model of vehicle motion.', 'calibration_approved': False, + 'controller_sha256': hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()} + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(json.dumps(report, indent=2)+'\n') + print(json.dumps(report, indent=2)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + sub = parser.add_subparsers(dest='mode', required=True) + route = sub.add_parser('route') + route.add_argument('directory', type=Path) + route.add_argument('--output', type=Path, required=True) + random = sub.add_parser('stress') + random.add_argument('--cycles', type=int, default=200_000) + random.add_argument('--output', type=Path, required=True) + args = parser.parse_args() + if args.mode == 'route': + replay(args.directory, args.output) + else: + stress(args.cycles, args.output) diff --git a/tools/ford_pscm_lab/model_action_replay.py b/tools/ford_pscm_lab/model_action_replay.py index 4dca9d6759..2bde01e9b8 100644 --- a/tools/ford_pscm_lab/model_action_replay.py +++ b/tools/ford_pscm_lab/model_action_replay.py @@ -1,4 +1,8 @@ -"""Replay the selected core and its adapter on route90/95 original-time extracts. +"""Check zero-error compatibility on route90/95 original-time extracts. + +Measured curvature is deliberately set to the requested curvature in this +historical compatibility check, so feedback stays zero. This is not a replay +of the new feedback controller's response to recorded steering measurements. The historical pass deliberately uses the archived eligibility mask to check command compatibility. The separate adapter pass reconstructs input eligibility @@ -132,11 +136,13 @@ def run(directory, output): reasons = Counter() for i, now in enumerate(t): selected_model = models[mi[i]] if exact[i] else None - old_gate = core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=dt[i], active=bool(baseline['valid'][i])) + old_gate = core.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i], + speed=cs['speed'][i], dt=dt[i], active=bool(baseline['valid'][i])) commands[i] = old_gate.path_offset, old_gate.path_angle, old_gate.curvature, old_gate.curvature_rate valid[i] = old_gate.valid wire.check(old_gate) - new_gate = adapter.update(selected_model, controls['desired'][i], speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, + new_gate = adapter.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i], + speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, measurement_time=cs['t'][i], model_time=model['t'][i], reference_time=model['t'][i], active=bool(cc['active'][i]), valid=bool(services_valid[i])) adapted[i] = new_gate.path_offset, new_gate.path_angle, new_gate.curvature, new_gate.curvature_rate @@ -146,7 +152,8 @@ def run(directory, output): # Isolate the adapter's fresh 10 ms engagement tick from the archived # harness, which used the preceding publication interval even on engage. entry_dt = dt[i] if i > 0 and baseline['valid'][i-1] else .01 - expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=entry_dt, + expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], current_curvature=controls['desired'][i], + speed=cs['speed'][i], dt=entry_dt, active=bool(baseline['valid'][i])) assert new_gate == expected_adapter, f'Unexplained adapter difference at cycle {i}' np.testing.assert_array_equal(valid, baseline['valid']) @@ -186,7 +193,7 @@ def run(directory, output): sources = [Path(__file__), Path(ford_model_action.__file__), root/'openpilot/selfdrive/controls/lib/ford_path.py', directory/'route.npz', directory/'metadata.json', directory/'encoder_comparison.npz', directory/'encoder_comparison.json', directory/'pose_candidate/pose_replay.npz'] - report = {'scope': 'Command construction and adapter reconstruction only; no counterfactual closed-loop score.', + report = {'scope': 'Zero-error compatibility only: measured curvature is set to requested curvature. No physical tracking score.', 'calibration_approved': False, 'executes_live_selector': False, 'cycles': len(t), 'core_active_cycles': int(valid.sum()), 'core_exact_archived_match': True, 'cohorts_reproduced': True, 'adapter_active_cycles': int(adapter_valid.sum()), 'adapter_status_counts': dict(reasons), diff --git a/tools/ford_pscm_lab/stress_model_action.py b/tools/ford_pscm_lab/stress_model_action.py index 4cce9cd6f2..2722dfb4e4 100644 --- a/tools/ford_pscm_lab/stress_model_action.py +++ b/tools/ford_pscm_lab/stress_model_action.py @@ -1,4 +1,7 @@ -"""Deterministic numerical stress and exhaustive field-boundary CAN checks. +"""Zero-error numerical stress and exhaustive field-boundary CAN checks. + +Measured curvature equals requested curvature here to preserve the independent +feedforward oracle. Feedback behavior is covered by its own tests and replay. Analytic straight/rotated paths supply an independent y(7) oracle. The reference slew uses scalar arithmetic. Packing is checked against direct @@ -64,8 +67,8 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC): model, mirror = line(offset, heading), line(-offset, -heading) if i % 401 == 0: model.position.y[4] = mirror.position.y[4] = math.nan - out = controller.update(model, desired, speed=speed, dt=dt, active=active, valid=valid) - other = mirrored.update(mirror, -desired, speed=speed, dt=dt, active=active, valid=valid) + out = controller.update(model, desired, current_curvature=desired, speed=speed, dt=dt, active=active, valid=valid) + other = mirrored.update(mirror, -desired, current_curvature=-desired, speed=speed, dt=dt, active=active, valid=valid) expected_valid = active and valid and dt <= .1 and i % 401 != 0 assert out.valid == other.valid == expected_valid previous = np.array([c0, c1]) @@ -99,14 +102,14 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC): selected = float(np.clip(scalar, low, high)) offset, heading = (selected, 0.) if field == 0 else (0., selected) controller.c0, controller.c1 = offset, heading - out = controller.update(line(offset), heading/20., speed=20., dt=.01) + out = controller.update(line(offset), heading/20., current_curvature=heading/20., speed=20., dt=.01) check_raw_packing(wire, controller, out) boundary_cases += 1 report = {'seed': seed, 'random_cycles': cycles, 'mirrored_core_updates': cycles, 'invalid_or_inactive_resets': resets, 'field_boundary_cases': boundary_cases, 'float32_can_round_trips': wire.count, 'analytic_targets_scalar_slew_and_mirror_checks_pass': True, 'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(), - 'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.', + 'calibration_approved': False, 'scope': 'Zero-error numerical construction: measured equals requested curvature. No PSCM response claims.', 'opendbc_import_head': revision(dependency), 'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in (Path(__file__), Path(ford_model_action.__file__), Path(__file__).with_name('model_action_replay.py'))}}