From bf00bc691def830e1beb15363d05416714c1dc42 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sun, 13 Sep 2026 08:18:42 -0400 Subject: [PATCH] Ford: add explicit proportional C1 feedback trial --- docs/ford_c1_pi.md | 151 +++++++ docs/ford_c1_pi_validation.json | 373 ++++++++++++++++++ docs/ford_model_action_drive_test.md | 22 +- .../controls/lib/ford_model_action.py | 53 ++- .../tests/test_ford_model_action_adapter.py | 29 +- .../tests/test_ford_model_action_pi.py | 102 +++++ .../tests/test_ford_model_action_selection.py | 4 +- tools/ford_pscm_lab/feedback_replay.py | 9 +- tools/ford_pscm_lab/pi_replay.py | 124 ++++++ tools/ford_pscm_lab/pi_stress.py | 95 +++++ 10 files changed, 925 insertions(+), 37 deletions(-) create mode 100644 docs/ford_c1_pi.md create mode 100644 docs/ford_c1_pi_validation.json create mode 100644 openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py create mode 100644 tools/ford_pscm_lab/pi_replay.py create mode 100644 tools/ford_pscm_lab/pi_stress.py diff --git a/docs/ford_c1_pi.md b/docs/ford_c1_pi.md new file mode 100644 index 0000000000..64fd961c9e --- /dev/null +++ b/docs/ford_c1_pi.md @@ -0,0 +1,151 @@ +# Ford C1 proportional feedback trial + +V6 adds **P = 0.25** to the selected-action controller. It responds to a steering +shortfall immediately and subtracts demand immediately when the wheel exceeds +the selected request. V5 accumulated correction over traveled distance. P has +no stored correction to release when its error disappears. + +This is an initial drive-trial gain, not an identified PSCM calibration or a +claim of improved physical tracking. Six Lightning routes establish command +behavior across recorded scenarios. They cannot identify the best stable gain +without observing the vehicle responding to the changed commands. + +## Command law + +With curvature in inverse meters, speed in meters per second and heading in radians: + +```text +D = max(7 m, speed × 1 s) +error = selected_limited_curvature - measured_steering_curvature +base_C1 = clip(D × selected_limited_curvature, -0.5, +0.5) +P = 0.25 × D × error +I_increment = speed × error × fresh_measurement_elapsed_time +C1 = amplitude_and_slew_limit(base_C1 + P + I) +``` + +P is 25% of the heading-equivalent tracking error, not a 25% multiplier on the +model request. At matched curvature it is zero. It is stateless and can change +with a new request even if a steering publication repeats; repeated steering +publications still cannot integrate I twice. Driver override and fresh PSCM +denied/inactive states clear both feedback terms. Fresh `limit=2` inhibits +outward I accumulation while permitting unwind; P remains available inside the +existing combined output envelope. + +The existing C1 amplitude limit (±0.5 rad) and slew (0.5 rad/s) apply to the sum. +Anti-windup includes P when calculating I's available headroom. P can consume +a slew interval that previously allowed I accumulation. The conditional I +release rules, including [completed-unwind release](ford_unwind_catchup.md), +remain. C0 retains the same 7 m mapping, base-heading overflow, cap and slew; +neither P nor I spills into C0. C2/C3 stay zero. No plant, gain schedule or +automatic gain learning is introduced. + +Onroad selection explicitly supplies `C1_PROPORTIONAL_GAIN = 0.25`. Direct +`FordModelActionController()` and `ModelActionController()` construction defaults +to zero P for v5 reference/replay compatibility. The existing default-off +Sunnylink toggle selects v6 on any Ford CAN FD. Toggle off still selects +upstream Ford control. See [installation and selection](ford_model_action_drive_test.md). + +## Lightning replay findings + +Routes `112`, `113`, `114`, `115`, `b9` and `ca` supplied 677,871 control cycles. +Each was replayed with P gains 0, 0.1, 0.25 and 0.5, paired with diagnostic +feedback delays 0, 0.2 and 0.4 s: 12 combinations and 8,134,452 candidate updates. +Recorded model, driver, steering and PSCM inputs stayed fixed. + +Both command columns are replayed C1 in radians with left positive. The angle +pair is the single recorded desired/actual wheel measurement, not a predicted +outcome for either candidate. + +| Example | Desired / actual angle | V5 C1 | P=0.25 C1 | +| --- | ---: | ---: | ---: | +| 115, 207.908 s: late left entry | 94.2° / 51.2° | +0.1685 | +0.1825 | +| 115, 208.099 s: entry continues | 111.0° / 72.8° | +0.2105 | +0.2245 | +| 114, 473.086 s: well-tracked bend | 57.3° / 56.3° | +0.1855 | +0.1850 | +| 113, 481.567 s: hanging right exit | −7.6° / −94.4° | +0.0645 | +0.0875 | +| 115, 133.595 s: completed unwind | 6.0° / 29.9° | +0.0105 | 0.0000 | + +The completed-unwind example is excluded by the original quality/driver clean +mask. It is useful for checking command release, not autonomous tracking +attribution. Large-turn windows often contain interventions and require review +of driver input before assigning a tracking result to the controller. + +Across 2,740.44 seconds of valid, feedback-enabled, clean samples with desired +wheel angle below 30°, the duration-weighted mean absolute C1 change is +0.001001 rad at P=0.25, versus 0.001961 at P=0.5. Per-route 95th-percentile +changes at P=0.25 are 0.0025–0.0070 rad; the largest ordinary-cohort change is +0.0350 rad. Small average command changes do not establish unchanged centering +or stability. + +P=0.25 is an engineering choice between the tested smaller and larger responses, +not an optimization result. At the late-entry example, adding a fixed 0.4 s +feedback delay instead gives C1 +0.1420 rad. Across the ordinary cohort, that +delayed P=0.25 variant changes C1 by 0.011255 rad on average. V6 therefore +retains v5's feedback timing to isolate P. This does not identify or disprove +the vehicle's physical delay. The diagnostic delay variants change only P and +I integration targets; request-release decisions still use the current request. + +## Tuning and next-drive evidence + +Comma's [torque controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_torque.py) +separates feedforward, P and I and aligns its torque feedback reference with +steering delay. Its [angle PID controller](https://github.com/commaai/openpilot/blob/master/openpilot/selfdrive/controls/lib/latcontrol_pid.py) +uses desired-minus-measured steering angle directly. These different paths do +not imply one delay setting should be copied into Ford C1. + +Use the same discipline: explicit parameters, separate term logging, fixed +request conditions and measured response. Comma's +[lateral maneuver report](https://blog.comma.ai/0111release/#lateral-maneuver-report) +uses repeatable step/sine maneuvers to assess response. C1 is a path-heading +request to another controller, not normalized steering torque; numerical torque +gains and torque calibration cannot be copied across. + +For the next controlled evaluation, compare similar speeds and model requests: +entry delay/shortfall, overshoot as the request relaxes, correction after catch-up, +ordinary-bend centering and oscillation. Keep desired/actual tracking on original +timestamps. Check C0/C1 caps, slew, driver input and fresh PSCM flags separately. +More gain cannot remove hardware limits and can introduce oscillation. These +logs all come from a Lightning; the gain is not yet validated across other +PSCMs. No scripted maneuver mode is enabled by this change. + +Periodic `Ford C2-free path tracking` events identify +`hypothesis=model-action-c1-pi-v6` and expose `heading_proportional`, +`proportional_gain`, `feedback_curvature` and `feedback_error` alongside +`heading_feedforward`, `heading_correction`, command and release diagnostics. +`calibration_approved=false` remains. + +## Validation and reproduction + +The final selected path exactly matches the sweep's P=0.25, zero-delay variant +on all six routes, including C0/C1, P, I and activation. Zero-P/zero-delay matches +v5 exactly on every cycle. All variants preserve C0 and activation. Another +200,000 seeded stress cycles check PI arithmetic, anti-windup, mirror symmetry, +driver/PSCM arbitration, resets, amplitude/slew and zero-P parity. Sweep, +selected replay and stress total **9,012,323 Float32/CAN round trips**. +Encoding checks do not test vehicle motion. + +**776 tests and 9,145 subtests passed; 178 were skipped.** Coverage includes +actual startup selection, controlsd request source/limiting, Float32 publication, +100 Hz CAN encoding, checksums, both turn signs, integral release, Sunnylink +persistence, toggle-off upstream behavior and Ford safety tests. Ruff, +controller Ty and settings compilation passed. A hardware build/device boot +and physical response tests have not been performed. + +Use the project's Python environment and built cereal/opendbc dependencies: + +```sh +export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps +export PYTHONDONTWRITEBYTECODE=1 +export PARAMS_ROOT=/tmp/ford-pi-test-params +export LOG_ROOT=/tmp/ford-pi-test-logs +python -m tools.ford_pscm_lab.pi_replay .cache/ford_route115 --output .cache/ford_pi_sweep/route115 +python -m tools.ford_pscm_lab.feedback_replay route .cache/ford_route115 --baseline 22d188776cb557acea459a1fca70812bdb2df46c --output .cache/ford_pi_sweep/selected115 +python -m tools.ford_pscm_lab.pi_stress --cycles 200000 --gain .25 --output .cache/ford_pi_sweep/stress.json +python -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/sunnypilot/sunnylink/tests openpilot/common/tests/test_params.py opendbc_repo/opendbc/car/ford/tests/test_ford.py opendbc_repo/opendbc/safety/tests/test_ford.py +``` + +Repeat both replay commands for the other five extracts. The machine-readable +[validation record](ford_c1_pi_validation.json) records counts, source hashes, +cohort definitions and sampled command changes. Publication time proxies the +computation clock; full SubMaster state and selected maneuver-plan publications +are not reconstructed. Real maneuver source selection is exercised in integration +tests. Historical controller reports retain their original version scope. diff --git a/docs/ford_c1_pi_validation.json b/docs/ford_c1_pi_validation.json new file mode 100644 index 0000000000..49fb3b64d5 --- /dev/null +++ b/docs/ford_c1_pi_validation.json @@ -0,0 +1,373 @@ +{ + "hypothesis": "model-action-c1-pi-v6", + "baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c", + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "selected_gain": 0.25, + "selected_feedback_delay_s": 0.0, + "calibration_approved": false, + "scope": "Frozen recorded steering, model, driver and PSCM inputs. Command checks only; no predicted wheel response or physical tracking improvement score.", + "controller_sha256": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "routes": { + "112": { + "route": "84865544361f55cb_00000112--ec2edd4afc", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 108971, + "candidate_updates": 1307652, + "sweep_can_round_trips": 1307652, + "selected_can_round_trips": 108971, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 655.2228206790003, + "ordinary_mean_abs_c1_change_rad": 0.0008885702173989647, + "ordinary_p95_abs_c1_change_rad": 0.0025000000000000022, + "ordinary_max_abs_c1_change_rad": 0.02400000000000002, + "all_valid_max_abs_c1_change_rad": 0.054500000000000104, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/angles.npz": "9429813237b4988dd99a1461d1a9bd6f6d3056693a93a5dd1ec63567d19f2aa1" + }, + "sweep_report_sha256": "413e844d20d7591d32d95b1d48a14db11fa089800315cb899e9f5a32a3e630f5", + "selected_report_sha256": "ab4ced7ec6b628ac4a941fb968e6e18d06f9c616f8704f1a6abd0081a4fb0e65" + }, + "113": { + "route": "84865544361f55cb_00000113--3947b0487c", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 49614, + "candidate_updates": 595368, + "sweep_can_round_trips": 595368, + "selected_can_round_trips": 49614, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 187.8263240149995, + "ordinary_mean_abs_c1_change_rad": 0.0015004664599222238, + "ordinary_p95_abs_c1_change_rad": 0.007000000000000006, + "ordinary_max_abs_c1_change_rad": 0.025000000000000022, + "all_valid_max_abs_c1_change_rad": 0.08250000000000002, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/angles.npz": "2f280a1b5973901d878f1e36564d6e7ae6d817cac176149f2cb898ab7591d65b" + }, + "sweep_report_sha256": "a712fdbe0602404ade075b0ccb540d5f571e5a55d37723fc2674c79b5f62252f", + "selected_report_sha256": "0098ef5b1dd0b8b1f0b742cfb5f8e80211eb60718a08e471927c78b9a7c740b3" + }, + "114": { + "route": "84865544361f55cb_00000114--03902c6e04", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 61027, + "candidate_updates": 732324, + "sweep_can_round_trips": 732324, + "selected_can_round_trips": 61027, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 263.004525574, + "ordinary_mean_abs_c1_change_rad": 0.0010481495632417845, + "ordinary_p95_abs_c1_change_rad": 0.003500000000000003, + "ordinary_max_abs_c1_change_rad": 0.010999999999999954, + "all_valid_max_abs_c1_change_rad": 0.0655, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/angles.npz": "63aa2ff6a21e8613519bda476690e2eda6f42cb5dcf108606bc6072c9e29408d" + }, + "sweep_report_sha256": "138c522c4278d6827eb6192acc2052ba4832f652c3194e2ca330d3a4458a2f85", + "selected_report_sha256": "52cdc595248505301501d815d7f6219c47e8cdcac2a059dc345498b3ef707875" + }, + "115": { + "route": "84865544361f55cb_00000115--899b9bf91d", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 40037, + "candidate_updates": 480444, + "sweep_can_round_trips": 480444, + "selected_can_round_trips": 40037, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 165.0304544679998, + "ordinary_mean_abs_c1_change_rad": 0.0009869321832509364, + "ordinary_p95_abs_c1_change_rad": 0.0030000000000000027, + "ordinary_max_abs_c1_change_rad": 0.034999999999999976, + "all_valid_max_abs_c1_change_rad": 0.09900000000000003, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/angles.npz": "71415b0f49efbfeda495ae52bde4beb7e16f31a1f5570af515a365e756056d3b" + }, + "sweep_report_sha256": "cdb08fede1f86427174c1291e40f63a3ac18044dfb6276e98e0bc184bbff7dc5", + "selected_report_sha256": "92d3e720d12018599a5883ecfb3c2dd1330e22235e07b3cd0dcc9bbc0aa7c3dd" + }, + "b9": { + "route": "84865544361f55cb_000000b9--5da7fe66ad", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 90774, + "candidate_updates": 1089288, + "sweep_can_round_trips": 1089288, + "selected_can_round_trips": 90774, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 577.3471252719997, + "ordinary_mean_abs_c1_change_rad": 0.001220959786299961, + "ordinary_p95_abs_c1_change_rad": 0.0050000000000000044, + "ordinary_max_abs_c1_change_rad": 0.02350000000000002, + "all_valid_max_abs_c1_change_rad": 0.10250000000000004, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/angles.npz": "ce16212d9b13e41993d2c8df3246ddfe21324ad0cb213310e62eee1d685b197b" + }, + "sweep_report_sha256": "13c9b2c4c0f89ec580e9c7a70c7556b4e40873d4d98561c9edfa1c12a0855669", + "selected_report_sha256": "733c3eb657cbae29fcb809ca42b85ed6fc03ab44665c5fe31020e266717ce53c" + }, + "ca": { + "route": "84865544361f55cb_000000ca--1f70b49ec6", + "fingerprint": "FORD_F_150_LIGHTNING_MK1", + "cycles": 327448, + "candidate_updates": 3929376, + "sweep_can_round_trips": 3929376, + "selected_can_round_trips": 327448, + "selected_matches_sweep_commands_p_i_valid_exactly": true, + "zero_gain_zero_delay_matches_v5_exactly": true, + "all_c0_and_activation_match_exactly": true, + "ordinary_clean_seconds": 892.0115341569954, + "ordinary_mean_abs_c1_change_rad": 0.0008244438245156879, + "ordinary_p95_abs_c1_change_rad": 0.0025000000000000022, + "ordinary_max_abs_c1_change_rad": 0.01200000000000001, + "all_valid_max_abs_c1_change_rad": 0.05149999999999999, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/feedback_replay.py": "c7de9d9d7aefb27221ad6d964df723fbf9f2b0f820cdd7de4799c83984d05485", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/angles.npz": "1681bda6db7dac332abf627d97e7452acd7e0faef0c196b11e55e22291cad073" + }, + "sweep_report_sha256": "4ced4be50569d4025c9de7fb8085714deebfb6098238ab63e5a2e4c88460a94b", + "selected_report_sha256": "db0edb05105600f211e7ff833a324887f4402dc64ddceb44ee8ddec911de7533" + } + }, + "examples": [ + { + "description": "Late left entry", + "route": "115", + "time_s": 207.908066963, + "desired_angle_deg": 94.1796646118164, + "actual_angle_deg": 51.20000076293945, + "speed_mph": 12.099885821086458, + "strict_clean": true, + "c0_m_left_positive": 0.96, + "c1_rad_left_positive": { + "v5": 0.16849999999999998, + "p_025": 0.1825, + "p_025_delay_04": 0.14200000000000002 + }, + "p_rad_left_positive": 0.018101743422448635, + "i_rad_left_positive": 0.015372994845796784 + }, + { + "description": "Entry continues", + "route": "115", + "time_s": 208.09921410900006, + "desired_angle_deg": 111.01296997070312, + "actual_angle_deg": 72.80000305175781, + "speed_mph": 12.297558204224886, + "strict_clean": true, + "c0_m_left_positive": 1.13, + "c1_rad_left_positive": { + "v5": 0.21050000000000002, + "p_025": 0.22450000000000003, + "p_025_delay_04": 0.18100000000000005 + }, + "p_rad_left_positive": 0.016087211202830076, + "i_rad_left_positive": 0.02176835685651445 + }, + { + "description": "Completed right-turn unwind", + "route": "115", + "time_s": 133.59461515599992, + "desired_angle_deg": 5.989600658416748, + "actual_angle_deg": 29.899999618530273, + "speed_mph": 5.890273288393662, + "strict_clean": false, + "c0_m_left_positive": 0.1900000000000004, + "c1_rad_left_positive": { + "v5": 0.010500000000000065, + "p_025": 0.0, + "p_025_delay_04": -0.0030000000000000027 + }, + "p_rad_left_positive": -0.010158478980883956, + "i_rad_left_positive": -0.0015653052344988395 + }, + { + "description": "Large left overshoot; nearby driver input", + "route": "114", + "time_s": 168.957705716, + "desired_angle_deg": 278.3920593261719, + "actual_angle_deg": 450.20001220703125, + "speed_mph": 7.027778014508665, + "strict_clean": true, + "c0_m_left_positive": 5.11, + "c1_rad_left_positive": { + "v5": 0.34099999999999997, + "p_025": 0.2875, + "p_025_delay_04": 0.3125 + }, + "p_rad_left_positive": -0.07201755233108997, + "i_rad_left_positive": -0.13527950258838367 + }, + { + "description": "Well-tracked left bend", + "route": "114", + "time_s": 473.085523785, + "desired_angle_deg": 57.32655334472656, + "actual_angle_deg": 56.29999923706055, + "speed_mph": 27.33261651794824, + "strict_clean": true, + "c0_m_left_positive": 0.6699999999999999, + "c1_rad_left_positive": { + "v5": 0.1855, + "p_025": 0.18500000000000005, + "p_025_delay_04": 0.15900000000000003 + }, + "p_rad_left_positive": 0.0007143015310955292, + "i_rad_left_positive": 0.022011912629614844 + }, + { + "description": "Hanging right exit", + "route": "113", + "time_s": 481.56693996600006, + "desired_angle_deg": -7.566320896148682, + "actual_angle_deg": -94.4000015258789, + "speed_mph": 11.959057581279636, + "strict_clean": true, + "c0_m_left_positive": -0.5800000000000001, + "c1_rad_left_positive": { + "v5": 0.0645, + "p_025": 0.08750000000000002, + "p_025_delay_04": 0.034499999999999975 + }, + "p_rad_left_positive": 0.03597008844371885, + "i_rad_left_positive": 0.06765882642510383 + } + ], + "ordinary_cohort": "Existing interval-clean angle mask, valid replay and feedback enabled, absolute desired wheel angle <30 degrees.", + "weighting": "Extracted interval duration weights for seconds and mean absolute command changes; percentiles are cycle-weighted.", + "ordinary_clean_seconds": 2740.4427841649945, + "ordinary_mean_abs_c1_change_by_setting_rad": [ + { + "kp": 0.0, + "delay_s": 0.0, + "mean_abs_delta_rad": 0.0 + }, + { + "kp": 0.1, + "delay_s": 0.0, + "mean_abs_delta_rad": 0.0003946011107110312 + }, + { + "kp": 0.25, + "delay_s": 0.0, + "mean_abs_delta_rad": 0.0010009008647461168 + }, + { + "kp": 0.5, + "delay_s": 0.0, + "mean_abs_delta_rad": 0.001961192031401653 + }, + { + "kp": 0.0, + "delay_s": 0.2, + "mean_abs_delta_rad": 0.006131012841938232 + }, + { + "kp": 0.1, + "delay_s": 0.2, + "mean_abs_delta_rad": 0.006179941022213607 + }, + { + "kp": 0.25, + "delay_s": 0.2, + "mean_abs_delta_rad": 0.006305419932002006 + }, + { + "kp": 0.5, + "delay_s": 0.2, + "mean_abs_delta_rad": 0.006643212063186747 + }, + { + "kp": 0.0, + "delay_s": 0.4, + "mean_abs_delta_rad": 0.010964264260175235 + }, + { + "kp": 0.1, + "delay_s": 0.4, + "mean_abs_delta_rad": 0.011070256870936306 + }, + { + "kp": 0.25, + "delay_s": 0.4, + "mean_abs_delta_rad": 0.011255438766715855 + }, + { + "kp": 0.5, + "delay_s": 0.4, + "mean_abs_delta_rad": 0.01162257561022028 + } + ], + "totals": { + "cycles": 677871, + "candidate_updates": 8134452, + "sweep_can_round_trips": 8134452, + "selected_can_round_trips": 677871, + "can_round_trips_including_stress": 9012323 + }, + "stress": { + "cycles": 200000, + "gain": 0.25, + "seed": 20260913, + "mirrored_updates": 200000, + "zero_gain_exact_v5_comparisons": 200000, + "can_round_trips": 200000, + "baseline_revision": "22d188776cb557acea459a1fca70812bdb2df46c", + "baseline_source_sha256": "2ceb4cd8717bb3325f9b22189c78605ad5d42a1061dec9ca2e4dbb90fd256d1e", + "release_cycles": 91, + "calibration_approved": false, + "checks": "Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.", + "scope": "Check PI arithmetic and CAN invariants without a model of vehicle response.", + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/pi_stress.py": "07f3bd99c56e2184ba3402d7b2f324506ac0b786c7a15e422e9e2392c934544a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8eaf242a8627c398b732e9c185527640b2ae39b4726cbb138c0a10490a4b890f" + }, + "formatting_only_final_source_ast_identical": true, + "final_script_sha256": "cff014beda81b0bfbaabd7219b8e2848464c5c45ac779669f80084d5be347a06" + }, + "tests": { + "passed": 776, + "skipped": 178, + "subtests_passed": 9145, + "log_sha256": "5b9274e2557f41b630ad285b0426c916835c2997fc7f5c4fcad91aa4c6f64243", + "ruff": "passed", + "controller_ty": "passed", + "settings_compiler": "passed" + } +} diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 40003e41ba..c9032d46cb 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -4,6 +4,8 @@ The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md and [conditional correction release](ford_c1_carryover.md) to the restored original v1 mapping, with [base C1 overflow allocated to C0](ford_c1_overflow.md) and [completed-unwind correction release](ford_unwind_catchup.md). +V6 adds [explicit proportional C1 feedback with P=0.25](ford_c1_pi.md), retaining +the existing feedback timing. This is an initial drive-trial gain. It is selectable on **any Ford CAN FD vehicle** through the existing persistent, default-off Sunnylink toggle. Offline checks establish software behavior; physical tracking, @@ -21,8 +23,8 @@ turn-exit behavior and closed-loop stability remain unvalidated. The startup event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify **`hypothesis=model-action-c1-feedback-v5`**. They report desired and -measured curvature, base heading, accumulated correction, applied heading, +identify **`hypothesis=model-action-c1-pi-v6`**. They report desired and +measured curvature, base heading, proportional and accumulated correction, applied heading, feedback timing and driver/PSCM gating. `carryover_release_count` counts conditional releases since the last controller reset; it does not control steering. `offset_overflow` reports the extra C0 target in meters before C0 @@ -32,6 +34,8 @@ request and sufficiently large measured error agree. Periodic logs can miss individual retirement cycles. `unwind_direction` remembers an unfinished unwind; `unwind_release` reports correction retired when a confirmed unwind catches the selected curvature. +`heading_proportional`, `proportional_gain`, `feedback_curvature` and +`feedback_error` separate the new P contribution and its reference from I. Turning the toggle off and completing another offroad-to-onroad cycle restores **upstream Ford curvature control**: 20 Hz steering messages, limited mode on @@ -45,9 +49,10 @@ See [toggle-off validation](ford_upstream_fallback.md). `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 +Fresh steering publications advance C1 integration. P responds to the current +error without accumulating. Repeated publications may advance P and output slew +but cannot integrate the same elapsed interval twice. +Driver override clears P and I. A fresh PSCM reached-limit flag stops extra outward accumulation while preserving unwind and base model changes. With fresh feedback, the controller can discard an opposing correction when it prevents C1 from following the direction shared by original model C0, applied C0 @@ -69,9 +74,10 @@ place. An explicit selection flag distinguishes upstream mode from an invalid experimental command; invalid experimental input cannot switch to upstream. The opendbc sender restores upstream behavior when that flag is false. -See [completed-unwind release and validation](ford_unwind_catchup.md) and -`ford_unwind_catchup_validation.json` for current evidence and reproduction -commands. [Changed-request release](ford_c1_request_release.md) and its +See [proportional feedback trial and validation](ford_c1_pi.md) and +`ford_c1_pi_validation.json` for current evidence and reproduction commands. +[Completed-unwind release](ford_unwind_catchup.md) and +`ford_unwind_catchup_validation.json` record v5. [Changed-request release](ford_c1_request_release.md) and its validation JSON record v4. The [overflow specification](ford_c1_overflow.md) and `ford_c1_overflow_validation.json` record v3. The carryover specification and `ford_c1_carryover_validation.json` record the previous experiment. `ford_c1_feedback_validation.json` records the initial feedback diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 173891fb8c..649e2e1230 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,8 +1,9 @@ -"""Experimental Ford C2-free controller with measured-curvature C1 feedback. +"""Experimental Ford C2-free controller with measured-curvature C1 PI 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. +The selected experiment adds an explicit proportional heading-error term. Opposed correction may be released when both path commands confirm the turn. Changed requests retire opposing correction only while measured error agrees. Completed, direction-confirmed unwinds release dominant old correction. @@ -19,6 +20,7 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path OFFSET_STATION_M = 7.0 HEADING_TIME_S = 1.0 +C1_PROPORTIONAL_GAIN = 0.25 # Initial drive-trial gain, not a learned calibration. CALIBRATION_APPROVED = False @@ -60,11 +62,15 @@ class ModelActionController: Feedback integrates requested minus measured curvature over traveled distance. Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. + Zero P is the v5 reference; onroad selection supplies the explicit trial gain. """ __slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release', - 'unwind_direction', 'unwind_release') + 'unwind_direction', 'unwind_release', 'proportional_gain', 'proportional', 'feedback_curvature') - def __init__(self): + def __init__(self, proportional_gain=0.): + if not _finite(proportional_gain) or proportional_gain < 0.: + raise ValueError('Proportional gain must be finite and nonnegative') + self.proportional_gain = float(proportional_gain) self.reset() def reset(self): @@ -74,18 +80,26 @@ class ModelActionController: self.request_release = 0. # Diagnostic radians retired on this cycle. self.unwind_direction = 0. self.unwind_release = 0. # Diagnostic only; final output still obeys slew. + self.proportional = self.feedback_curvature = 0. def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, - feedback_dt=None, feedback_enabled=True, pscm_limited=False): + feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None): 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.): + feedback_curvature = desired_curvature if feedback_curvature is None else feedback_curvature + if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, feedback_curvature) or not .002 <= dt <= .1 + or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(feedback_curvature) > 1.): self.reset() return FordPath() target = encode_model_action(model, desired_curvature, speed) if not target.valid: self.reset() return FordPath() + self.feedback_curvature = feedback_curvature + feedback_error = feedback_curvature-current_curvature + self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*feedback_error if feedback_enabled else 0. + if not _finite(self.proportional): + self.reset() + return FordPath() base_c1 = float(np.clip(target.path_angle, -.5, .5)) # Preserve the linear path reference at 7 m when the base heading clips. # This is instantaneous geometry, not stored error or C1 feedback spill. @@ -145,7 +159,7 @@ class ModelActionController: and (base_c1+self.correction)*direction <= 0.): self.correction = 0. self.carryover_release_count += 1 - increment = (desired_curvature-current_curvature)*speed*feedback_dt + increment = feedback_error*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 @@ -153,16 +167,17 @@ class ModelActionController: # 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.))) + # Include P in the available headroom so I cannot wind up behind it. # 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 + request = base_c1+self.proportional+self.correction self.correction += float(np.clip(increment, min(lower-request, 0.), max(upper-request, 0.))) if self.correction*self.unwind_direction < 0.: self.unwind_direction = 0. if self.last_feedback_desired is None or feedback_dt > 0. or not feedback_enabled: self.last_feedback_desired = desired_curvature - c1 = float(np.clip(base_c1+self.correction, -.5, .5)) + c1 = float(np.clip(base_c1+self.proportional+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.) @@ -179,18 +194,20 @@ class FordModelActionController: 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() + def __init__(self, proportional_gain=0.): + self.core = ModelActionController(proportional_gain=proportional_gain) + self.hypothesis = 'model-action-c1-pi-v6' if proportional_gain else 'model-action-c1-feedback-v5' 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-v5', + self.diagnostics = {'status': status, 'hypothesis': self.hypothesis, '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): + reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None, + feedback_curvature=None): reason = None if not active: reason = 'inactive' @@ -221,14 +238,15 @@ class FordModelActionController: or (status_fresh and pscm_status.limit == 3)) feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2))) command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt, - feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited) + feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, + feedback_curvature=feedback_curvature) if not command.valid: self.reset('invalid_path') return command self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time raw_heading = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature base_heading = float(np.clip(raw_heading, -.5, .5)) - self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c1-feedback-v5', + self.diagnostics = {'status': 'active', 'hypothesis': self.hypothesis, '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, @@ -236,6 +254,9 @@ class FordModelActionController: 'heading_feedforward': base_heading, 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, + 'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain, + 'feedback_curvature': self.core.feedback_curvature, + 'feedback_error': self.core.feedback_curvature-current_curvature, 'request_release': self.core.request_release, 'unwind_direction': self.core.unwind_direction, 'unwind_release': self.core.unwind_release, 'carryover_release_count': self.core.carryover_release_count, @@ -248,5 +269,5 @@ def select_model_action_controller(CP, enabled): """Only opt-in Ford CAN FD vehicles override upstream curvature control.""" compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD if enabled and compatible: - return FordModelActionController() + return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN) return None 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 38b704f92b..5c7ad81ecb 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -180,7 +180,9 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe 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 controller.core.proportional == pytest.approx(.25*20.*expected_curvature) + assert controller.core.correction == 0. # First measurement has no elapsed feedback time. + assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.003) 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) @@ -237,9 +239,11 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline 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.)]: + # A .005 rad P step consumes one slew interval before I can accumulate; + # going from -.005 to +.005 consumes two. Matched steering removes P only. + for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.0198), + (sign*.004, 0., 100, sign*.0198), (sign*.005, 0., 100, 0.), + (sign*.003, 0., 100, sign*.0196), (0., 1.0625, 5, 0.)]: for _ in range(count): now = 1.+frame*.01 controls.curvature, cs.steeringTorque = measured, torque @@ -263,8 +267,12 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline 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) + core = controls.ford_path_controller.core + expected_p = .25*20.*(sign*.004-measured) if torque == 0. else 0. + assert core.proportional == pytest.approx(expected_p) + assert core.correction == pytest.approx(expected) + assert core.c1 == pytest.approx(sign*.08+expected_p+expected) + assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025) assert controls.ford_path.path_offset == pytest.approx(.4) @@ -276,7 +284,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s 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): + for frame in range(102): now = 1.+frame*.01 controls.curvature = .004 if frame < 100 else .003 sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) @@ -288,6 +296,8 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s 'time': SimpleNamespace(monotonic=lambda now=now: now)}) controller = controls.ford_path_controller assert controller.diagnostics['pscm_limited'] is service_valid + assert controller.core.proportional == pytest.approx(.005) + # First error sample spends the slew allowance on P; the second may add I. assert controller.core.correction == pytest.approx(0. if service_valid else .0002) assert cc.latActive and controls.ford_path.valid @@ -337,11 +347,12 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel packet = next(packet for packet in packets if packet[0] == address) assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) if frame == 199: - assert core.correction == pytest.approx(sign*(-speed*.002 if same_turn else .06)) + # P spends one or three slew intervals before the remaining I updates. + assert core.correction == pytest.approx(sign*(-.0198 if same_turn else .0582)) assert core.carryover_release_count == 0 assert core.carryover_release_count == (0 if same_turn else 1) assert controls.ford_path_controller.diagnostics['carryover_release_count'] == core.carryover_release_count - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-feedback-v5' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6' if same_turn: assert request_releases > 0 assert controls.desired_curvature == pytest.approx(sign*.01) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py new file mode 100644 index 0000000000..96f412a11f --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py @@ -0,0 +1,102 @@ +"""Explicit PI experiment semantics; no simulated PSCM response.""" +import math + +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +@pytest.mark.parametrize('gain', [.1, .25, .5]) +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_p_responds_without_waiting_for_integral_and_disappears_at_catchup(gain, sign): + controller = ModelActionController(proportional_gain=gain) + controller.c1 = sign*.2 + out = controller.update(straight(), sign*.01, current_curvature=sign*.009, + speed=20., dt=.1, feedback_dt=0.) + assert controller.proportional == pytest.approx(sign*gain*.02) + assert controller.correction == 0. + assert out.path_angle == pytest.approx(sign*(.2+gain*.02)) + out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.1) + assert controller.proportional == controller.correction == 0. + assert out.path_angle == pytest.approx(sign*.2) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_aligned_feedback_does_not_replace_current_feedforward_or_path(sign): + controller = ModelActionController(proportional_gain=.25) + controller.c0, controller.c1 = sign*.4, sign*.2 + out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008, + feedback_curvature=sign*.008, speed=20., dt=.1) + assert controller.proportional == controller.correction == 0. + assert out.path_offset == pytest.approx(sign*.4) + assert out.path_angle == pytest.approx(sign*.2) + # Latest request is ahead of measured steering, but the delay-aligned target + # has already been exceeded. P and I must use the explicit feedback target. + out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008, + feedback_curvature=sign*.006, speed=20., dt=.1) + assert controller.proportional == pytest.approx(-sign*.01) + assert sign*controller.correction < 0. + assert sign*out.path_angle < .2 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_pi_combined_request_obeys_slew_and_does_not_wind_up_behind_p(sign): + controller = ModelActionController(proportional_gain=.5) + for _ in range(200): + before = controller.c1 + out = controller.update(straight(), sign*.01, current_curvature=-sign*.1, speed=20., dt=.01) + assert abs(controller.c1-before) <= .0050000001 + assert abs(out.path_angle) <= .50000001 + assert controller.correction == 0. # Feedforward + P alone exceeds the cap. + assert out.path_angle == pytest.approx(sign*.5) + for _ in range(60): + out = controller.update(straight(), sign*.01, current_curvature=sign*.01, speed=20., dt=.01) + assert out.path_angle == pytest.approx(sign*.2) + assert controller.correction == controller.proportional == 0. + + +@pytest.mark.parametrize('gain', [-.1, math.nan, math.inf, None, 'bad']) +def test_invalid_gain_is_rejected(gain): + with pytest.raises(ValueError): + ModelActionController(proportional_gain=gain) + + +@pytest.mark.parametrize('feedback', [math.nan, math.inf, 'bad', 1.001]) +def test_invalid_feedback_target_clears_all_output(feedback): + controller = ModelActionController(proportional_gain=.25) + controller.c1, controller.correction = .2, .01 + out = controller.update(straight(), .01, current_curvature=.008, feedback_curvature=feedback, speed=20., dt=.01) + assert out == FordPath() + assert controller.proportional == controller.correction == controller.c1 == 0. + + +def test_overflowing_p_cannot_escape_as_an_active_command(): + controller = ModelActionController(proportional_gain=1e308) + out = controller.update(straight(), 1., current_curvature=-1., speed=55., dt=.01) + assert out == FordPath() + + +@pytest.mark.parametrize('limited', [False, True]) +def test_driver_override_clears_both_p_and_i(limited): + controller = ModelActionController(proportional_gain=.25) + controller.c1, controller.correction = .2, .01 + controller.update(straight(), .01, current_curvature=.008, speed=20., dt=.01, + feedback_enabled=False, pscm_limited=limited) + assert controller.proportional == controller.correction == 0. + + +def test_adapter_logs_separate_feedforward_p_i_and_explicit_feedback_target(): + controller = FordModelActionController(proportional_gain=.25) + for i in range(30): + now = 1.+i*.01 + controller.update(straight(), .004, current_curvature=.002, feedback_curvature=.003, + speed=20., yaw_rate=0., now=now, measurement_time=now, model_time=now, + reference_time=now, active=True) + d = controller.diagnostics + assert d['heading_feedforward'] == pytest.approx(.08) + assert d['heading_proportional'] == pytest.approx(.005) + assert d['proportional_gain'] == .25 and d['feedback_curvature'] == .003 + assert d['heading_correction'] > 0. + assert d['heading_request'] == pytest.approx(d['heading_feedforward']+d['heading_proportional']+d['heading_correction']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 6cfd97716b..724d129979 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -9,7 +9,7 @@ import pytest from opendbc.car.ford.values import CAR, FordFlags from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType -from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller +from openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller from openpilot.selfdrive.controls.lib.ford_path import FordPath @@ -46,6 +46,8 @@ def test_actual_startup_priority(candidate, observer, fingerprint): selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=settings.__getitem__)) if candidate: assert type(selected.ford_path_controller) is FordModelActionController + assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .25 + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6' else: assert selected.ford_path_controller is None assert selected.ford_model_action == candidate diff --git a/tools/ford_pscm_lab/feedback_replay.py b/tools/ford_pscm_lab/feedback_replay.py index 7f1896b24f..3c39f9fced 100644 --- a/tools/ford_pscm_lab/feedback_replay.py +++ b/tools/ford_pscm_lab/feedback_replay.py @@ -16,7 +16,7 @@ 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 openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, ModelActionController from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency @@ -57,12 +57,13 @@ def replay(directory, output, baseline_revision=BASELINE): models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths] old, baseline_hash = original_controller(baseline_revision) old_has_feedback = 'current_curvature' in inspect.signature(old.update).parameters - controller, wire_check = FordModelActionController(), WireCheck() + controller, wire_check = FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN), 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)) + proportional = np.zeros(len(t)) baseline_correction = np.zeros(len(t)) feedback_dt = np.zeros(len(t)) feedback_enabled = np.zeros(len(t), bool) @@ -96,6 +97,7 @@ def replay(directory, output, baseline_revision=BASELINE): d = controller.diagnostics reasons[d['status']] += 1 correction[i] = controller.core.correction + proportional[i] = controller.core.proportional baseline_correction[i] = getattr(old.core, 'correction', 0.) feedback_dt[i] = d.get('feedback_dt', 0.) feedback_enabled[i] = d.get('feedback_enabled', False) @@ -122,6 +124,7 @@ def replay(directory, output, baseline_revision=BASELINE): 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_revision, 'baseline_source_sha256': baseline_hash, + 'proportional_gain': C1_PROPORTIONAL_GAIN, 'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()), 'validity_matches_baseline_exactly': True, 'status_counts': dict(reasons), 'c0_matches_baseline_exactly': bool(np.array_equal(commands[:, 0], baseline[:, 0])), @@ -156,7 +159,7 @@ def replay(directory, output, baseline_revision=BASELINE): '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, baseline_correction=baseline_correction, feedback_dt=feedback_dt, + correction=correction, proportional=proportional, baseline_correction=baseline_correction, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, offset_overflow=offset_overflow, request_release=request_release, unwind_release=unwind_release, unwind_direction=unwind_direction) (output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n') diff --git a/tools/ford_pscm_lab/pi_replay.py b/tools/ford_pscm_lab/pi_replay.py new file mode 100644 index 0000000000..2d75a1e34f --- /dev/null +++ b/tools/ford_pscm_lab/pi_replay.py @@ -0,0 +1,124 @@ +"""Compare explicit C1 P gains and feedback delays on fixed route measurements. + +No PSCM plant is fitted. A change in command is not a predicted change in wheel +angle. Feedforward uses the current selected request. Only P and elapsed-distance +integration use the delayed request; v5's conditional I retirement is preserved. +""" +import argparse +import hashlib +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib import ford_model_action +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController +from tools.ford_pscm_lab.feedback_replay import OPENDBC, original_controller +from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency + + +BASELINE = '22d188776cb557acea459a1fca70812bdb2df46c' +GAINS = (0., .1, .25, .5) +DELAYS = (0., .2, .4) + + +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'] + c, t = r['controls'], r['controls']['t'] + assert all(np.all(np.diff(stream['t']) >= 0.) for stream in r.values()) + 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] + settings = [(gain, delay) for delay in DELAYS for gain in GAINS] + controllers = [FordModelActionController(proportional_gain=gain) for gain, _ in settings] + reference, reference_hash = original_controller(BASELINE) + delayed = {} + for delay in DELAYS: + # Causal history lookup at the steering measurement's time. Zero delay is + # the original v5 same-cycle request, kept exact for baseline comparison. + ix = np.searchsorted(t, cs['t']-delay, side='right')-1 + delayed[delay] = np.where(ix >= 0, c['desired'][np.maximum(ix, 0)], c['measured']) if delay else c['desired'] + shape = (len(settings), len(t)) + commands = np.zeros((*shape, 4)) + proportional, integral, feedforward, feedback_error = (np.zeros(shape) for _ in range(4)) + enabled = np.zeros(shape, bool) + valid = np.zeros(shape, bool) + wire = WireCheck() + 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) + 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])) + kwargs = {'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, + 'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]), + 'driver_torque': cs['torque'][i], 'pscm_status': status} + old = reference.update(model, c['desired'][i], **kwargs) + for k, (controller, (_, delay)) in enumerate(zip(controllers, settings, strict=True)): + command = controller.update(model, c['desired'][i], feedback_curvature=delayed[delay][i], **kwargs) + if k == 0: + assert command == old + assert controller.core.correction == reference.core.correction + commands[k, i] = command.path_offset, command.path_angle, command.curvature, command.curvature_rate + valid[k, i] = command.valid + d = controller.diagnostics + proportional[k, i] = d.get('heading_proportional', 0.) + integral[k, i] = d.get('heading_correction', 0.) + feedforward[k, i] = d.get('heading_feedforward', 0.) + feedback_error[k, i] = delayed[delay][i]-c['measured'][i] + enabled[k, i] = d.get('feedback_enabled', False) + wire.check(command) + for k in range(len(settings)): + field_checks(commands[k], valid[k], t) + np.testing.assert_array_equal(valid[k], valid[0]) + np.testing.assert_array_equal(commands[k, :, 0], commands[0, :, 0]) + assert np.all(proportional[k, ~enabled[k]] == 0.) + assert np.all(integral[k, ~enabled[k]] == 0.) + assert np.all(abs(integral[k]) <= 1.+1e-10) + report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': reference_hash, + 'cycles': len(t), 'candidate_updates': len(settings)*len(t), 'can_round_trips': wire.count, + 'zero_gain_zero_delay_matches_v5_exactly': True, 'all_c0_and_activation_match_exactly': True, + 'calibration_approved': False, 'settings': [], + '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())}, + 'limitations': ['Recorded model, steering, driver and PSCM inputs stay fixed; no tracking improvement score.', + '0.2/0.4 s are diagnostic timing alternatives, not fitted or validated PSCM delays.', + 'Gain sweep does not identify a physically optimal or stable gain.', + 'Publication clock proxies computation time; maneuver references are not reconstructed.']} + for k, (gain, delay) in enumerate(settings): + report['settings'].append({'index': k, 'kp': gain, 'delay_s': delay, + 'max_abs_c1_change_rad': float(abs(commands[k, :, 1]-commands[0, :, 1]).max()), + 'max_abs_p_rad': float(abs(proportional[k]).max()), + 'max_abs_i_rad': float(abs(integral[k]).max())}) + output.mkdir(parents=True, exist_ok=True) + np.savez_compressed(output/'commands.npz', t=t-metadata['t0'], settings=np.array(settings), commands=commands, + valid=valid, proportional=proportional, integral=integral, feedforward=feedforward, + feedback_error=feedback_error, feedback_enabled=enabled, + desired_curvature=c['desired'], measured_curvature=c['measured'], + desired_angle=c['desired_angle'], actual_angle=c['actual_angle'], speed=cs['speed']) + (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)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('directory', type=Path) + parser.add_argument('--output', type=Path, required=True) + args = parser.parse_args() + replay(args.directory, args.output) diff --git a/tools/ford_pscm_lab/pi_stress.py b/tools/ford_pscm_lab/pi_stress.py new file mode 100644 index 0000000000..c1d127e0b0 --- /dev/null +++ b/tools/ford_pscm_lab/pi_stress.py @@ -0,0 +1,95 @@ +"""Check PI arithmetic and CAN invariants without a model of vehicle response.""" +import argparse +import hashlib +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib import ford_model_action +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController +from tools.ford_pscm_lab.feedback_replay import OPENDBC, original_controller +from tools.ford_pscm_lab.model_action_replay import WireCheck, verify_dependency +from tools.ford_pscm_lab.pi_replay import BASELINE + + +def stress(cycles, gain, output): + verify_dependency(OPENDBC) + rng = np.random.default_rng(20260913) + controller = ModelActionController(proportional_gain=gain) + mirror = ModelActionController(proportional_gain=gain) + zero = ModelActionController() + original, original_hash = original_controller(BASELINE) + wire = WireCheck() + release_count = 0 + 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 + count = controller.carryover_release_count + 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) + z = zero.update(model(offset), desired, current_curvature=measured, **args) + old = original.core.update(model(offset), desired, current_curvature=measured, **args) + assert z == old and zero.correction == original.core.correction + state = np.array([controller.c0, controller.c1, controller.correction, controller.proportional]) + np.testing.assert_allclose(state, -np.array([mirror.c0, mirror.c1, mirror.correction, mirror.proportional]), atol=1e-10, rtol=0.) + assert controller.carryover_release_count == mirror.carryover_release_count + assert abs(controller.request_release+mirror.request_release) <= 1e-10 + assert controller.unwind_release == -mirror.unwind_release + assert abs(controller.c0) <= 5.11+1e-10 and abs(controller.c1) <= .5+1e-10 and abs(controller.correction) <= 1.+1e-10 + if active: + distance = max(7., speed) + base = min(.5, max(-.5, distance*desired)) + p = gain*distance*(desired-measured) if enabled else 0. + assert controller.proportional == p + assert abs(controller.c0-previous[0]) <= 4.*dt+1e-10 + assert abs(controller.c1-previous[1]) <= .5*dt+1e-10 + if enabled: + released = controller.carryover_release_count > count + release_count += released or bool(controller.unwind_release) + remaining = 0. if released else previous[2]-controller.unwind_release+controller.request_release + increment = (desired-measured)*speed*feedback_dt + direction = measured if measured else previous[1] + if limited and increment*direction > 0.: + increment = min(max(increment, min(-remaining, 0.)), max(-remaining, 0.)) + lower, upper = max(-.5, previous[1]-.5*dt), min(.5, previous[1]+.5*dt) + target = base+p+remaining + increment = min(max(increment, min(lower-target, 0.)), max(upper-target, 0.)) + assert abs(controller.correction-(remaining+increment)) <= 1e-10 + else: + assert controller.correction == 0. + target = min(.5, max(-.5, base+p+controller.correction)) + expected = previous[1]+min(.5*dt, max(-.5*dt, target-previous[1])) + assert abs(controller.c1-expected) <= 1e-10 + else: + assert np.all(state == 0.) + assert out.curvature == out.curvature_rate == other.curvature == other.curvature_rate == 0. + wire.check(out) + report = {'cycles': cycles, 'gain': gain, 'seed': 20260913, 'mirrored_updates': cycles, + 'zero_gain_exact_v5_comparisons': cycles, 'can_round_trips': wire.count, + 'baseline_revision': BASELINE, 'baseline_source_sha256': original_hash, + 'release_cycles': release_count, 'calibration_approved': False, + 'checks': + 'Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.', + 'scope': __doc__, 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in + (Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}} + 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__) + parser.add_argument('--cycles', type=int, default=200_000) + parser.add_argument('--gain', type=float, default=.25) + parser.add_argument('--output', type=Path, required=True) + args = parser.parse_args() + stress(args.cycles, args.gain, args.output)