From 72e9d94f62487d07e9b7c5cb17d5988d5ddfbedb Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Tue, 8 Sep 2026 04:33:06 -0400 Subject: [PATCH] Ford: remove yaw damping from selected-action controller Remove excess-yaw C0 attenuation while retaining full path prediction, command limits, slew and input-health gates. Valid measured yaw no longer changes path demand. Update diagnostics and Sunnylink help for v5. Validate with 356 tests and 26 subtests, 100% controller coverage, 280,636 recorded route cycles and 779,410 Float32/CAN round trips. These are command checks; physical tracking improvement is not established. Assisted-by: OpenAI Codex --- docs/ford_model_action_drive_test.md | 12 +- docs/ford_model_action_full_prediction.md | 3 + docs/ford_model_action_no_yaw_damping.md | 67 ++++ ...odel_action_no_yaw_damping_validation.json | 304 ++++++++++++++++++ .../controls/lib/ford_model_action.py | 31 +- .../tests/test_ford_controlsd_logging.py | 2 +- .../tests/test_ford_model_action_adapter.py | 4 +- .../tests/test_ford_model_action_damping.py | 75 ----- .../tests/test_ford_model_action_yaw.py | 29 ++ .../sunnypilot/sunnylink/settings_ui.json | 2 +- .../settings_ui_src/pages/vehicle.yaml | 2 +- tools/ford_pscm_lab/damping_replay.py | 10 +- tools/ford_pscm_lab/model_action_replay.py | 5 +- tools/ford_pscm_lab/stress_model_action.py | 12 +- .../ford_pscm_lab/test_model_action_replay.py | 4 +- 15 files changed, 435 insertions(+), 127 deletions(-) create mode 100644 docs/ford_model_action_no_yaw_damping.md create mode 100644 docs/ford_model_action_no_yaw_damping_validation.json delete mode 100644 openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py create mode 100644 openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 71677048c9..852c2a4ad7 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -1,9 +1,9 @@ # 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. Version 4 uses -[full geometric path prediction](ford_model_action_full_prediction.md) while retaining -[excess-yaw offset damping](ford_model_action_damping.md). Input gates are unchanged. +its own persistent, default-off Sunnylink toggle. Version 5 retains full geometric +path prediction and [removes yaw damping](ford_model_action_no_yaw_damping.md). +Measured yaw is used only for input-health checks and diagnostics. Input gates are unchanged. `calibration_approved=false`: offline checks do not establish physical tracking, turn-exit behavior or closed-loop stability. @@ -20,7 +20,7 @@ turn-exit behavior or closed-loop stability. The startup log event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify `hypothesis=model-action-c0-c1-prediction-v4` and report host yaw and the command tuple. +identify `hypothesis=model-action-c0-c1-prediction-v5` and report host yaw and the command tuple. Turning the new toggle off and completing another offroad-to-onroad cycle restores **PSCM Coefficient Observer** if selected, otherwise the original @@ -54,7 +54,9 @@ 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_full_prediction_validation.json`; +The current validation record is `ford_model_action_no_yaw_damping_validation.json`; +the [v5 notes](ford_model_action_no_yaw_damping.md) explain damping removal and its +scope. `ford_model_action_full_prediction_validation.json` archives v4 checks; the [full-prediction notes](ford_model_action_full_prediction.md) explain cap removal and remaining physical uncertainty. `ford_model_action_prediction_validation.json` archives the capped v3 evaluation. `ford_model_action_damping_validation.json` diff --git a/docs/ford_model_action_full_prediction.md b/docs/ford_model_action_full_prediction.md index f5d87bfb64..c402714688 100644 --- a/docs/ford_model_action_full_prediction.md +++ b/docs/ford_model_action_full_prediction.md @@ -1,5 +1,8 @@ # Experimental Ford full path prediction, v4 +This document archives v4. The [current v5 controller](ford_model_action_no_yaw_damping.md) +retains this prediction and removes yaw damping. + V4 removes the extra 15 cm / 25% limit on the geometric prediction introduced in [v3](ford_model_action_prediction.md). Those numbers were hand-chosen tuning bounds, not identified Ford response limits. The current user request is to diff --git a/docs/ford_model_action_no_yaw_damping.md b/docs/ford_model_action_no_yaw_damping.md new file mode 100644 index 0000000000..cd42dc5881 --- /dev/null +++ b/docs/ford_model_action_no_yaw_damping.md @@ -0,0 +1,67 @@ +# Experimental Ford selected-action controller, v5 + +V5 removes the excess-yaw C0 attenuation at the user's request. The damping +function, 0.02 rad/s deadband and 0.2 s reduction scale are deleted. Valid measured +yaw no longer changes either command target. Existing yaw input-health checks +and logging remain. No replacement gain, increment gate or controller state is added. + +The full 150 ms geometric prediction introduced in v4 remains, along with the +7 m offset station and one-second heading scale. This is not a return to v1: +v1 did not predict the offset. C0/C1 bounds, slew, quantization, service gates, +engagement, downstream driver arbitration and zero C2/C3 are unchanged. + +The latest supplied route a0 ran v2, not v4. Prior same-input comparisons found +identical v1/v2 commands during its driver-clean minor-bend warning intervals +and the preceding five seconds. That evidence does not identify damping as the +cause of those misses. Removing damping can restore C0 demand where the damper +was active, including turn exits; it is not evidence of improved tracking or +reduced oversteer. + +## Offline validation + +The regression suite checks yaw-independent commands through mirrored turn +entry, release and reversal, including valid yaw extremes and small yaw offsets. +Six cases fail with v4 damping present and pass after removal. Invalid yaw still +resets the controller. Actual controlsd selection, upstream limiting, Float32 +publication and downstream CAN tests cover both model and maneuver references. + +Full-rlog comparisons run pinned v4 against production v5 on routes9b, 9e and a0. +They preserve original clocks, exact consumed model frames and causal carState; +publication times proxy computation time, and complete SubMaster health is +unavailable. The numerical stress run checks independent geometric targets, +scalar slew, mirrored turns and Float32/CAN packing. Recorded vehicle motion +stays fixed: none of these checks establishes counterfactual steering response, +closed-loop stability or a physical tracking improvement. + +Results and source hashes are recorded in +`ford_model_action_no_yaw_damping_validation.json`. Earlier validation documents +remain archives of their specified controller versions. + +Validation passes 356 tests and 26 subtests with 100% controller statement and +branch coverage, 280,636 recorded route cycles and 779,410 Float32/CAN round trips, +including 200,000 random stress cycles. C1 and input eligibility match v4 exactly +on all three routes. Commands during all 1,578 driver-clean ordinary-bend warning +cycles on route a0 also remain identical to v4. At the earlier right-turn exit, +removing damping increases C0 magnitude by a mean 0.079 m, maximum 0.13 m; these +are command offsets, not measured vehicle displacement. + +The module is 171 total lines, or 111 code lines excluding blanks, comments and +docstrings, with two control states. No hardware build or device boot was performed. + +## Reproduce and select + +Use the dependency setup and combined suite in the +[drive-test guide](ford_model_action_drive_test.md). Replay and stress commands: + +```sh +python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v4 --candidate current --window right_entry 637 640 --window right_exit 642.7 643.852 --output /path/to/separate/route9b-results +python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v4 --candidate current --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --output /path/to/separate/route9e-results +python -m tools.ford_pscm_lab.damping_replay /path/to/routea0/rlogs --baseline v4 --candidate current --window bends_5min 298 338 --window bend_7min 449 458 --window bend_9min 579 588 --output /path/to/separate/routea0-results +python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json +``` + +The same default-off **Selected-Action Path Tracking (Experimental)** Sunnylink +toggle selects v5 on the CAN FD F-150 Lightning. Deployment remains +`sunnypilot/sunnypilot`, branch `hiimisaac-dev`. After updating, restart controlsd +through a real offroad-to-onroad cycle. Diagnostics identify +`model-action-c0-c1-prediction-v5`; `calibration_approved=false` remains explicit. diff --git a/docs/ford_model_action_no_yaw_damping_validation.json b/docs/ford_model_action_no_yaw_damping_validation.json new file mode 100644 index 0000000000..1d53666480 --- /dev/null +++ b/docs/ford_model_action_no_yaw_damping_validation.json @@ -0,0 +1,304 @@ +{ + "date": "2026-09-08", + "baseline_commit": "7e63449749d112f096c56cb848dd289054e5f85b", + "hypothesis": "model-action-c0-c1-prediction-v5", + "scope": "Remove yaw damping only; retain full path prediction and existing input-health gates. Fixed-input command checks, not physical tracking or stability evidence.", + "deployment_target": { + "repository": "sunnypilot/sunnypilot", + "branch": "hiimisaac-dev" + }, + "calibration_approved": false, + "hardware_build_and_device_boot": "not performed", + "opendbc_revision": "c21a9013700734dd20b09e05aa68329ad8cc20f9", + "controller_size": { + "total_lines": 171, + "code_lines_excluding_blanks_comments_docstrings": 111, + "core_persistent_values": 2, + "adapter_timestamps": 3 + }, + "checks": { + "combined_ford_params_sunnylink_suite": "356 passed, 26 subtests passed; no skips", + "coverage": { + "covered_lines": 102, + "num_statements": 102, + "percent_covered": 100.0, + "percent_covered_display": "100", + "missing_lines": 0, + "excluded_lines": 0, + "percent_statements_covered": 100.0, + "percent_statements_covered_display": "100", + "num_branches": 30, + "num_partial_branches": 0, + "covered_branches": 30, + "missing_branches": 0, + "percent_branches_covered": 100.0, + "percent_branches_covered_display": "100" + }, + "ruff": "pass", + "ty_controller_and_lab": "pass", + "settings_compiler_check": "pass", + "removal_regression_probe": "6 cases fail with v4 damping present; all 21 yaw tests pass after removal.", + "reviews": { + "standards": { + "remaining_findings": 0 + }, + "spec": { + "remaining_findings": 0, + "independent_focused_tests_passed": 107 + }, + "corrected_findings": [ + "Removed stale damping claim from Sunnylink YAML and regenerated JSON.", + "Corrected replay yaw-use metadata to input-health checks and diagnostics." + ] + } + }, + "total_original_route_cycles": 280636, + "total_float32_can_round_trips": 779410, + "stress": { + "seed": 20260908, + "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, + "valid_yaw_does_not_affect_targets_checked": true, + "full_geometric_prediction_checked": true, + "direct_raw_float32_packing_matches_host_output": true, + "max_continuous_step_c0_c1": [ + 0.4000000000003041, + 0.05000000000000002 + ], + "calibration_approved": false, + "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.", + "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9" + }, + "routes": { + "route9b": { + "cycles": 82386, + "eligible_cycles": 70703, + "same_validity": true, + "c1_exactly_unchanged": true, + "field_slew_zero_c2_c3_pass": true, + "float32_can_round_trips": 164772, + "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b", + "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21", + "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8", + "source_rlog_sha256": { + "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f", + "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d", + "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f", + "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3", + "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e", + "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553", + "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184", + "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03", + "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff", + "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc", + "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932", + "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45", + "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4", + "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4" + }, + "focus_cohorts": { + "right_entry": { + "cycles": 298, + "seconds": 2.9932484459999387, + "changed_c0_cycles": 100, + "mean_absolute_c0_change_m": 0.007683282515599457, + "max_absolute_c0_change_m": 0.04999999999999982, + "increased_absolute_c0_cycles": 100, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 20.395198628296086, + "baseline_peak_absolute_c0_m": 3.0300000000000002, + "candidate_peak_absolute_c0_m": 3.0300000000000002 + }, + "right_exit": { + "cycles": 115, + "seconds": 1.156770048999988, + "changed_c0_cycles": 115, + "mean_absolute_c0_change_m": 0.07934493768172877, + "max_absolute_c0_change_m": 0.13000000000000078, + "increased_absolute_c0_cycles": 115, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 0.0, + "baseline_peak_absolute_c0_m": 0.7199999999999998, + "candidate_peak_absolute_c0_m": 0.7800000000000002 + } + } + }, + "route9e": { + "cycles": 83668, + "eligible_cycles": 74669, + "same_validity": true, + "c1_exactly_unchanged": true, + "field_slew_zero_c2_c3_pass": true, + "float32_can_round_trips": 167336, + "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b", + "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21", + "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8", + "source_rlog_sha256": { + "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7", + "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b", + "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2", + "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc", + "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223", + "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540", + "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d", + "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538", + "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940", + "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf", + "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb", + "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2", + "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1", + "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754", + "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f" + }, + "focus_cohorts": { + "left_entry": { + "cycles": 238, + "seconds": 2.4048367620016506, + "changed_c0_cycles": 0, + "mean_absolute_c0_change_m": 0.0, + "max_absolute_c0_change_m": 0.0, + "increased_absolute_c0_cycles": 0, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 2.403816920672437, + "baseline_peak_absolute_c0_m": 2.6399999999999997, + "candidate_peak_absolute_c0_m": 2.6399999999999997 + }, + "left_peak": { + "cycles": 288, + "seconds": 2.891819394000777, + "changed_c0_cycles": 0, + "mean_absolute_c0_change_m": 0.0, + "max_absolute_c0_change_m": 0.0, + "increased_absolute_c0_cycles": 0, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 6.41034524444729, + "baseline_peak_absolute_c0_m": 2.95, + "candidate_peak_absolute_c0_m": 2.95 + }, + "left_exit": { + "cycles": 219, + "seconds": 2.203320298998733, + "changed_c0_cycles": 129, + "mean_absolute_c0_change_m": 0.037433836245882854, + "max_absolute_c0_change_m": 0.11000000000000032, + "increased_absolute_c0_cycles": 128, + "decreased_absolute_c0_cycles": 1, + "driver_input_percent": 9.936619705158035, + "baseline_peak_absolute_c0_m": 2.51, + "candidate_peak_absolute_c0_m": 2.51 + } + } + }, + "routea0": { + "cycles": 114582, + "eligible_cycles": 105382, + "same_validity": true, + "c1_exactly_unchanged": true, + "field_slew_zero_c2_c3_pass": true, + "float32_can_round_trips": 229164, + "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b", + "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21", + "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8", + "source_rlog_sha256": { + "84865544361f55cb_000000a0--5e86c30dae--0--rlog.zst": "57b8113783f70f9176e1f2703e70185f64df07b3283f739a6fd7b182a2c92417", + "84865544361f55cb_000000a0--5e86c30dae--1--rlog.zst": "a821ce7df80c6466110a6f6433b2c9482f5f3a5b4227e8459182c3ee9fb8c7a7", + "84865544361f55cb_000000a0--5e86c30dae--2--rlog.zst": "984cca8e311be8a61444ca2bc23bbf3937c6f00264303faefb7efb8f0a3e8aa2", + "84865544361f55cb_000000a0--5e86c30dae--3--rlog.zst": "c6ea25b226aa875a15a8f8356522ab3b12501d280f63ebe0438a9d50b2116279", + "84865544361f55cb_000000a0--5e86c30dae--4--rlog.zst": "99b129e2965674fb3406855d1b022c0876083984adf2f2ef51c07f1e699974ea", + "84865544361f55cb_000000a0--5e86c30dae--5--rlog.zst": "428e2ce9a1ac0ca7135c00221590c286ab32b4ce5cf24d376de1519b39b25fec", + "84865544361f55cb_000000a0--5e86c30dae--6--rlog.zst": "efca7fedd0aaed4be4500fd1a0c67f01d8f805cfdd681a0a9c0439061ec2f36f", + "84865544361f55cb_000000a0--5e86c30dae--7--rlog.zst": "1da238f57b63efbbf7dfd2b147637c1b846736e198999741817fc0646d4f60b7", + "84865544361f55cb_000000a0--5e86c30dae--8--rlog.zst": "b61209325da26751b98da85fe166dab65ab9eaaef129b859a59fe99e4de5ab65", + "84865544361f55cb_000000a0--5e86c30dae--9--rlog.zst": "cf555f1c57a9afdbed702b5934892acb5b2174ef48f1fcf494422aba9bd01321", + "84865544361f55cb_000000a0--5e86c30dae--10--rlog.zst": "13ff83c236cca64b57c11ffa05cb74511af1258cde3cf0554395922769a39683", + "84865544361f55cb_000000a0--5e86c30dae--11--rlog.zst": "848a1d26fd72bbe4e119339bd405aedb0b9d7e6b96329d0b868a27bd01b6b128", + "84865544361f55cb_000000a0--5e86c30dae--12--rlog.zst": "9e8bf8597942cd74d8d04c783148b8201f63f56e55b766f7887f27465de276f2", + "84865544361f55cb_000000a0--5e86c30dae--13--rlog.zst": "e37575ed1424d574015c20a3066754bcca6fdacbb104db2bedce782428fd48ef", + "84865544361f55cb_000000a0--5e86c30dae--14--rlog.zst": "b67c7ed7ddd957bf5dcbcdb0604ddc9c52cd7af9d6aa70c1eae3e7622cee160d", + "84865544361f55cb_000000a0--5e86c30dae--15--rlog.zst": "6baaf9fdf64c72a9be70d261580914e2489f363230d95a1291b8479bcc249ba2", + "84865544361f55cb_000000a0--5e86c30dae--16--rlog.zst": "f84803fa2a615caf59a6bd4e9b2848059dd9f4a7f0c01ac4ec7e2347e317b5e7", + "84865544361f55cb_000000a0--5e86c30dae--17--rlog.zst": "f0f5f36cb155c18f8f9f90f700e1e91e169ef0442fd70670323be4a1c658040e", + "84865544361f55cb_000000a0--5e86c30dae--18--rlog.zst": "8674d5822949bafaefc0d0f30b0e91603354b9981de7566419a2e5b780054e64", + "84865544361f55cb_000000a0--5e86c30dae--19--rlog.zst": "b5df55701d5bcfbfd750608a1dfbcba9b931a39fba7a82e87bc7915b8ca2333f" + }, + "focus_cohorts": { + "bends_5min": { + "cycles": 3966, + "seconds": 39.99197329400005, + "changed_c0_cycles": 8, + "mean_absolute_c0_change_m": 1.950586094527418e-05, + "max_absolute_c0_change_m": 0.009999999999999787, + "increased_absolute_c0_cycles": 8, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 14.825647085260368, + "baseline_peak_absolute_c0_m": 0.3099999999999996, + "candidate_peak_absolute_c0_m": 0.3099999999999996 + }, + "bend_7min": { + "cycles": 893, + "seconds": 8.995078784999919, + "changed_c0_cycles": 0, + "mean_absolute_c0_change_m": 0.0, + "max_absolute_c0_change_m": 0.0, + "increased_absolute_c0_cycles": 0, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 0.0, + "baseline_peak_absolute_c0_m": 0.20000000000000018, + "candidate_peak_absolute_c0_m": 0.20000000000000018 + }, + "bend_9min": { + "cycles": 895, + "seconds": 8.998970718999999, + "changed_c0_cycles": 0, + "mean_absolute_c0_change_m": 0.0, + "max_absolute_c0_change_m": 0.0, + "increased_absolute_c0_cycles": 0, + "decreased_absolute_c0_cycles": 0, + "driver_input_percent": 0.22147398432951632, + "baseline_peak_absolute_c0_m": 0.2400000000000002, + "candidate_peak_absolute_c0_m": 0.2400000000000002 + } + } + } + }, + "routea0_ordinary_bend_warnings_v4_vs_v5": { + "definition": "Driver-clean +/-1s, speed>=8m/s, absolute desired wheel angle 3 to30deg. Proxy for ordinary bends, not map geometry.", + "cycles": 1578, + "seconds": 15.92979767199978, + "changed_c0_cycles": 0, + "changed_c1_cycles": 0, + "max_c0_change_m": 0.0 + }, + "source_sha256": { + "openpilot/selfdrive/controls/lib/ford_model_action.py": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8", + "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "be610c09e4bbd93e84d1920cfbb3e9609b3fbb7dca8eb229b8e67befb4b2ca84", + "openpilot/sunnypilot/sunnylink/settings_ui.json": "4d2dffdfd81190a871bd41e28e5832502aa95d8aacf91daa7e68382085588aca", + "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "30cfffb86fcd3830320e9b2ec1f65cdfd219684a55d0bc0bb13f77966cc18b53", + "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "7f8b13c4d85217cbb6bd32ec193ef18bdb30bdf1f2b59fa7fa9d40988cb20bea", + "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "eb50e1d6acdb5e7332fadc3dcfcfa1b3809545e607c47c54b0ba3de80b2df208", + "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd", + "openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py": "d646f89e2c4d4e112d03e77c5c34b7541b0bdba800419d472a4a69753637c515", + "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "9697db696d5a01838ea5cdcf4f1771d813a647f5038cabe9f0460778a2936929", + "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e", + "tools/ford_pscm_lab/damping_replay.py": "d21565a29b9c2b4668fafe07f995c15e113888a1f4787f37d72d02d36998f9cf", + "tools/ford_pscm_lab/model_action_replay.py": "a90caed1c46c6f964fecb50bd0f531ae355505f1e84be9a1e37243d910b292b4", + "tools/ford_pscm_lab/stress_model_action.py": "123c6c0c9f53a5af2ab50bb568e47c0f8960b7d49fad2fb6ea74de5c63e03ad5", + "tools/ford_pscm_lab/test_model_action_replay.py": "bf1a1612474cb7308b3640cba535ee534999fdecdf055ae6d7bc83b9a8e66f63" + }, + "artifact_sha256": { + ".cache/ford_no_yaw_damping/removal_red.txt": "4aa79e5af1066d66db65853faad3923f8645f5dff8383fa3106489a3e53cae18", + ".cache/ford_no_yaw_damping/suite_run.txt": "434d80389bc7d2859070c9f9df0cac1ac0f854151b5888dcedc2ce38df23d519", + ".cache/ford_no_yaw_damping/coverage.json": "d99baeb8729a1394b41d9a4739b538372aad27b56d379f4b9cefe682ed99d662", + ".cache/ford_no_yaw_damping/stress.json": "3e2c49dcc473d90ea03c95dc12905c2216ee573a691891b8a1f56ba82fd78251", + ".cache/ford_no_yaw_damping/route9b/report.json": "446371173fed1ff632ece4286e53d0d941491be590d467af960d1931431212ca", + ".cache/ford_no_yaw_damping/route9b/commands.npz": "d543a92991f06b56e20d0ed58daf6e4ef6a0021b79c027570b8d997658336b9d", + ".cache/ford_no_yaw_damping/route9e/report.json": "fb8ddd8d2c242a14a63993593c96604e7306dcd97219d073bbcfcbeba68af45b", + ".cache/ford_no_yaw_damping/route9e/commands.npz": "375a94227f3cb1a161a1295496908baa428d5ef0e0b79134a728f876477dbd6b", + ".cache/ford_no_yaw_damping/routea0/report.json": "7959b77af715016b4da45b9bc8f095872702f662950324f64a0f4d332c814eb0", + ".cache/ford_no_yaw_damping/routea0/commands.npz": "4f9dcde39ad9e20958ad04e53dbb32830c6a933dc7f743fdffe5a612252cb16b" + } +} diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 78782d0735..55263c3d35 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -14,8 +14,6 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path OFFSET_STATION_M = 7.0 HEADING_TIME_S = 1.0 -EXCESS_YAW_DEADBAND = .02 # rad/s; above the observed approximately .008 rad/s Ford yaw offset -EXCESS_YAW_LOOKAHEAD_S = .2 # engineering choice, not an identified PSCM delay CALIBRATION_APPROVED = False PREDICTION_TIME_S = .15 # geometric preview, not an identified actuator delay @@ -76,27 +74,11 @@ def encode_model_action(model, desired_curvature, speed): return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath() -def damp_offset(c0, desired_curvature, speed, yaw_rate): - """Attenuate same-direction C0 demand when yaw exceeds the requested turn. - - Inputs are finite and range-checked by the caller. The deadband avoids - chasing small yaw offsets. Opposing centering demand is left intact. - """ - if c0*yaw_rate <= 0.: - return c0 - direction = math.copysign(1., c0) - # An opposed plan must not amplify near-zero yaw bias into a large correction. - requested_yaw = max(0., direction*speed*desired_curvature) - excess = max(0., direction*yaw_rate-requested_yaw-EXCESS_YAW_DEADBAND) - reduction = OFFSET_STATION_M*EXCESS_YAW_LOOKAHEAD_S*excess - return direction*max(0., abs(c0)-reduction) - - class ModelActionController: """Only two control states: unquantized, independently slewed C0 and C1. - Freshness and engagement belong to the caller. Excess yaw attenuates the - offset target without model history, an integral or release modes. + Freshness and engagement belong to the caller. Measured yaw is checked for + input health only; it never changes valid offset or heading targets. """ __slots__ = ('c0', 'c1') @@ -107,7 +89,7 @@ class ModelActionController: self.c0 = self.c1 = 0. def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True): - # The production adapter always supplies validated measured yaw. + # Retain the existing input-health gate without yaw feedback. if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3: self.reset() return FordPath() @@ -116,7 +98,6 @@ class ModelActionController: self.reset() return FordPath() c0 = float(np.clip(target.path_offset, -5.11, 5.11)) - c0 = damp_offset(c0, desired_curvature, speed, yaw_rate) c1 = float(np.clip(target.path_angle, -.5, .5)) self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt)) self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt)) @@ -131,7 +112,7 @@ class FordModelActionController: core. Its timestamps and diagnostics never affect the targets. Raw model geometry is checked on every cycle, even at a repeated model timestamp. - Validated host-coordinate yaw supplies stateless offset damping. Engagement + Host-coordinate yaw supplies diagnostics and input-health checks. Engagement and downstream driver arbitration still apply. PSCM status and driver torque are not control-law inputs. """ @@ -142,7 +123,7 @@ 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-prediction-v4', + self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-prediction-v5', '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, @@ -173,7 +154,7 @@ class FordModelActionController: 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-prediction-v4', + self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-prediction-v5', 'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature, 'yaw_rate': yaw_rate, 'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time, diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index a6d2933abd..ac41c3a849 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase): 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-prediction-v4') + self.assertEqual(record['hypothesis'], 'model-action-c0-c1-prediction-v5') 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_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index e2eaba3e56..ce9d5b42cc 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -184,9 +184,9 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe expected_curvature = initial_curvature+(-1 if maneuver else 1)*.000125 assert controls.desired_curvature == pytest.approx(expected_curvature) assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature) - expected_offset = .04 if host_yaw < .02 else .01 + expected_offset = .04 if initial_curvature: - expected_offset = .44 if maneuver and host_yaw < .02 else .36 + expected_offset = .44 if maneuver else .36 assert controls.ford_path.path_offset == pytest.approx(expected_offset) assert controller.diagnostics['yaw_rate'] == host_yaw assert cc.latActive and cc.actuators.curvature == 0. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py deleted file mode 100644 index 85435e844e..0000000000 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py +++ /dev/null @@ -1,75 +0,0 @@ -"""Bounded excess-yaw damping: remove offset demand without integral or modes.""" -import math -import pytest - -from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, damp_offset -from openpilot.selfdrive.controls.lib.ford_path import FordPath -from openpilot.selfdrive.controls.tests.test_ford_model_action import straight - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_recorded_turn_exit_reduces_same_direction_offset_before_driver_intervention(sign): - # Route9b, segment10, about643.0s; fresh Ford yaw in host coordinates. - c0, desired, speed, yaw = sign*.5, sign*.0117238564, 9.71, sign*.180 - reduced = damp_offset(c0, desired, speed, yaw) - assert reduced == pytest.approx(sign*(.5-1.4*(.180-9.71*.0117238564-.02))) - assert 0. < sign*reduced < .45 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_recorded_late_exit_removes_remaining_offset_without_creating_countersteer(sign): - # About643.9s. C1 is already slightly opposite; C0 still points into the turn. - assert damp_offset(sign*.12, sign*-.0004078, 10.77, sign*.1002) == pytest.approx(sign*.00772) - assert damp_offset(sign*.12, 0., 10.77, sign*.2) == 0. - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013]) -def test_matched_turn_and_straight_bias_cannot_reduce_offset(sign, bias): - for desired in (0., sign*.01, sign*.05): - assert damp_offset(sign*.4, desired, 10., 10.*desired+bias) == sign*.4 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013, .02]) -def test_opposed_plan_cannot_amplify_small_yaw_bias(sign, bias): - for desired in (-sign*.01, -sign*.1): - assert damp_offset(sign*.4, desired, 20., sign*bias) == sign*.4 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_damping_begins_continuously_above_the_yaw_deadband(sign): - assert damp_offset(sign*.4, -sign*.1, 20., sign*.020001) == pytest.approx(sign*(.4-1.4e-6)) - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_entry_deficit_opposing_centering_and_zero_offset_are_preserved(sign): - assert damp_offset(sign*2.75, sign*.053889, 5.283, sign*.272) == sign*2.75 - assert damp_offset(sign*-.25, sign*.001, 10., sign*.2) == sign*-.25 - assert damp_offset(0., sign*.001, 10., sign*.2) == 0. - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_core_keeps_heading_unchanged_and_slews_offset_independently(sign): - baseline, damped = ModelActionController(), ModelActionController() - for i in range(300): - desired = sign*(.02 if i < 100 else .001) - a = baseline.update(straight(sign*.4), desired, speed=10., dt=.01) - b = damped.update(straight(sign*.4), desired, speed=10., dt=.01, yaw_rate=sign*.2) - assert a.path_angle == b.path_angle - assert a.curvature == b.curvature == a.curvature_rate == b.curvature_rate == 0. - assert a.path_offset == pytest.approx(sign*.39) # Geometric prediction slightly reduces the .4m offset. - assert b.path_offset == pytest.approx(sign*.15) - # No damping memory: a fresh copied pair of actuator states behaves identically. - copied = ModelActionController() - copied.c0, copied.c1 = damped.c0, damped.c1 - assert copied.update(straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.) == damped.update( - straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.) - - -@pytest.mark.parametrize('yaw', [math.nan, math.inf, -math.inf, None, 'bad', 3.001, -3.001]) -def test_invalid_yaw_resets_core(yaw): - c = ModelActionController() - c.update(straight(.4), .01, speed=10., dt=.01) - assert c.update(straight(.4), .01, speed=10., dt=.01, yaw_rate=yaw) == FordPath() - assert c.c0 == c.c1 == 0. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py new file mode 100644 index 0000000000..4079a74d73 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py @@ -0,0 +1,29 @@ +"""Measured yaw gates input health but cannot attenuate path demand.""" +import math + +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('yaw', [-3., -.2, -.008, 0., .008, .2, 3.]) +def test_valid_yaw_cannot_change_commands_during_entry_release_or_reversal(sign, yaw): + reference, measured = ModelActionController(), ModelActionController() + for i in range(400): + offset, desired = ((.4, .02), (.4, .001), (.12, -.0004078), (-.4, -.02))[i//100] + model = straight(sign*offset) + expected = reference.update(model, sign*desired, speed=10., dt=.01) + actual = measured.update(model, sign*desired, speed=10., dt=.01, yaw_rate=yaw) + assert actual == expected + assert actual.curvature == actual.curvature_rate == 0. + + +@pytest.mark.parametrize('yaw', [math.nan, math.inf, -math.inf, None, 'bad', 3.001, -3.001]) +def test_invalid_yaw_still_resets_core(yaw): + controller = ModelActionController() + controller.update(straight(.4), .01, speed=10., dt=.01) + assert controller.update(straight(.4), .01, speed=10., dt=.01, yaw_rate=yaw) == FordPath() + assert controller.c0 == controller.c1 == 0. diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 09866c698e..f99c100c8a 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2184,7 +2184,7 @@ "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 a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Reduces same-direction offset demand when measured turning exceeds the requested turn. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", + "details": "Uses a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", "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 93df92910d..e6c5d7e5b2 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -15,7 +15,7 @@ sections: 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 a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Reduces same-direction offset demand when measured turning exceeds the requested turn. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. + details: Uses a short prediction of the nearby model path to respond as bends develop, plus a heading request based on selected planned curvature. The predicted offset uses the full geometric request within the existing command limits and rate limits. Default off; this revised turn-entry and exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. enablement: - $ref: '#/macros/offroad' - key: FordPscmObserver diff --git a/tools/ford_pscm_lab/damping_replay.py b/tools/ford_pscm_lab/damping_replay.py index 6f237753df..beab494ccd 100644 --- a/tools/ford_pscm_lab/damping_replay.py +++ b/tools/ford_pscm_lab/damping_replay.py @@ -16,7 +16,9 @@ import zstandard from openpilot.cereal import log from openpilot.selfdrive.controls.lib import ford_model_action -from tools.ford_pscm_lab.model_action_replay import V1_REVISION, V2_REVISION, V3_REVISION, WireCheck, field_checks, load_controller, sample, verify_dependency +from tools.ford_pscm_lab.model_action_replay import ( + V1_REVISION, V2_REVISION, V3_REVISION, V4_REVISION, WireCheck, field_checks, load_controller, sample, verify_dependency, +) DEPLOYMENT_OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9' @@ -73,7 +75,7 @@ def run(directory, output, baseline_version='v1', candidate_version='v2', window if output == directory or directory in output.parents: raise ValueError('Output must be outside the source route directory') verify_dependency(DEPLOYMENT_OPENDBC) - revisions = {'v1': V1_REVISION, 'v2': V2_REVISION, 'v3': V3_REVISION} + revisions = {'v1': V1_REVISION, 'v2': V2_REVISION, 'v3': V3_REVISION, 'v4': V4_REVISION} baseline_source = load_controller(revisions[baseline_version]) candidate_source = ford_model_action if candidate_version == 'current' else load_controller(revisions[candidate_version]) streams, models, sources, t0 = extract(directory) @@ -162,8 +164,8 @@ if __name__ == '__main__': parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('rlog_directory', type=Path) parser.add_argument('--output', type=Path, required=True) - parser.add_argument('--baseline', choices=['v1', 'v2', 'v3'], default='v1') - parser.add_argument('--candidate', choices=['v2', 'v3', 'current'], default='v2') + parser.add_argument('--baseline', choices=['v1', 'v2', 'v3', 'v4'], default='v1') + parser.add_argument('--candidate', choices=['v2', 'v3', 'v4', 'current'], default='v2') parser.add_argument('--window', action='append', nargs=3, metavar=('LABEL', 'START_SECONDS', 'END_SECONDS'), default=[]) args = parser.parse_args() run(args.rlog_directory, args.output, args.baseline, args.candidate, args.window) diff --git a/tools/ford_pscm_lab/model_action_replay.py b/tools/ford_pscm_lab/model_action_replay.py index ada8cde662..31583c8c01 100644 --- a/tools/ford_pscm_lab/model_action_replay.py +++ b/tools/ford_pscm_lab/model_action_replay.py @@ -29,9 +29,10 @@ PINNED_OPENDBC = '72a775d35e54c21ff5c5798acef22016eedcc0a7' V1_REVISION = '5fc16abc7662020706e29f57d31a6d5e2bc1293a' V2_REVISION = '744a97d9bc08d8743b250eceff7c88585b5480de' V3_REVISION = '01f8d51c82b3e863f1012d383b5994813ef01b81' +V4_REVISION = '7e63449749d112f096c56cb848dd289054e5f85b' -@lru_cache(maxsize=3) +@lru_cache(maxsize=4) def load_controller(commit): """Load exact archived Python source for offline comparisons, never production.""" if len(commit) != 40 or any(c not in '0123456789abcdef' for c in commit): @@ -217,7 +218,7 @@ def run(directory, output): 'timing': 'Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.', 'eligibility': 'Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.', 'reference': 'Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.', - 'host_yaw': 'Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.', + 'host_yaw': 'Extract cs.yaw equals -carState.yawRate; current adapter uses it for input-health checks and diagnostics only.', 'archived_core_revision': V1_REVISION, 'archived_core_source_sha256': archived.source_sha256, 'cohorts': cohorts, 'workspace_head': revision(root), 'opendbc_import_head': revision(dependency), 'opendbc_import_path': str(dependency), diff --git a/tools/ford_pscm_lab/stress_model_action.py b/tools/ford_pscm_lab/stress_model_action.py index 29f23a4d55..e8aad4127e 100644 --- a/tools/ford_pscm_lab/stress_model_action.py +++ b/tools/ford_pscm_lab/stress_model_action.py @@ -81,14 +81,8 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC): future_x, future_y = (7.+distance)*math.cos(heading), offset+(7.+distance)*math.sin(heading) predicted = math.cos(rotation)*(future_y-ego_y)-math.sin(rotation)*(future_x-ego_x) target = (max(-5.11, min(5.11, predicted)), max(-.5, min(.5, max(7., speed)*desired))) - # Independent piecewise scalar oracle; do not call the production helper. - offset_target = target[0] - if offset_target > 0. and yaw > 0.: - offset_target = max(0., offset_target-1.4*max(0., yaw-max(0., speed*desired)-.02)) - elif offset_target < 0. and yaw < 0.: - offset_target = min(0., offset_target+1.4*max(0., -yaw-max(0., -speed*desired)-.02)) - assert abs(offset_target) <= abs(target[0]) and offset_target*target[0] >= 0. - c0 += max(-4.*dt, min(4.*dt, offset_target-c0)) + # Independent scalar slew oracle. Valid measured yaw cannot alter demand. + c0 += max(-4.*dt, min(4.*dt, target[0]-c0)) c1 += max(-.5*dt, min(.5*dt, target[1]-c1)) step = abs(np.array([controller.c0, controller.c1])-previous) assert (step <= np.array(rates)*dt+1e-10).all() @@ -122,7 +116,7 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC): 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, - 'bounded_excess_yaw_damping_checked': True, + 'valid_yaw_does_not_affect_targets_checked': True, 'full_geometric_prediction_checked': True, 'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(), 'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.', diff --git a/tools/ford_pscm_lab/test_model_action_replay.py b/tools/ford_pscm_lab/test_model_action_replay.py index 507f14dc50..70af7ff93b 100644 --- a/tools/ford_pscm_lab/test_model_action_replay.py +++ b/tools/ford_pscm_lab/test_model_action_replay.py @@ -45,13 +45,13 @@ def test_archived_loader_uses_exact_source_and_records_its_hash(monkeypatch): monkeypatch.setattr(replay.subprocess, 'check_output', read_source) replay.load_controller.cache_clear() try: - for commit in (replay.V1_REVISION, replay.V2_REVISION, replay.V3_REVISION): + for commit in (replay.V1_REVISION, replay.V2_REVISION, replay.V3_REVISION, replay.V4_REVISION): module = replay.load_controller(commit) assert module.archived_value == 42 assert module.source_sha256 == hashlib.sha256(source).hexdigest() assert calls[-1][-2:] == ['show', f'{commit}:openpilot/selfdrive/controls/lib/ford_model_action.py'] assert replay.load_controller(commit) is module - assert len(calls) == 3 + assert len(calls) == 4 finally: replay.load_controller.cache_clear()