mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-10 13:33:43 +08:00
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
This commit is contained in:
@@ -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`
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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.
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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'])
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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.',
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user