mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-18 10:23:43 +08:00
Ford: align feedback with upstream request-history timing
This commit is contained in:
Isaac Barham
@@ -0,0 +1,98 @@
|
||||
# Ford feedback timing follows comma's request-history pattern
|
||||
|
||||
The base C0/C1 mapping continues to use the latest selected curvature. C0 P,
|
||||
C1 P and C1 I now calculate error from an earlier selected curvature, using
|
||||
the same one-second request buffer and frame-index expression as comma's torque
|
||||
controller. The buffer advances at the 100 Hz controls rate, including while
|
||||
disengaged. The outgoing base request is never buffered for later transmission.
|
||||
|
||||
`controlsd` passes the same `lat_delay` value it passes to the upstream lateral
|
||||
controller: `lateralDelay.lateralDelay + LAT_SMOOTH_SECONDS`. The existing extra
|
||||
Ford model preview remains model-side; this change does not add it to feedback
|
||||
delay or infer hardware delay from a turn's angle crossing. In the four replayed
|
||||
routes the published delay was 0.1689463705 s and `LAT_SMOOTH_SECONDS` was zero.
|
||||
Upstream's integer indexing selects the request 16 control cycles earlier,
|
||||
nominally 0.16 s. The maximum lookback is 99 cycles, nominally 0.99 s.
|
||||
|
||||
The history is in curvature units because that is this controller's feedback
|
||||
quantity. This adopts upstream's timing pattern, not its torque conversion,
|
||||
acceleration gain schedule, friction/jerk feedforward or low-speed integral
|
||||
freeze. Ford gains, formulas, anti-windup, integration cadence, field bounds,
|
||||
upstream curvature limiting and driver/PSCM arbitration remain unchanged.
|
||||
Action C0 P remains 1.0; direct-path C0 P remains 0.5; C1 P=0.75 and I=1.0.
|
||||
Both Ford reference modes use the new timing.
|
||||
|
||||
The history starts at zero, like upstream. Ordinary disengagement clears P/I
|
||||
while continuing to record requests. Existing invalid-input, timing and path
|
||||
resets also clear request history, preventing invalid retained references from
|
||||
surviving recovery. A nonfinite delay rejects the active command; finite delay
|
||||
values use upstream's bounded frame selection. An explicit zero delay recovers
|
||||
the previous command law exactly. Existing offline feedback-reference overrides
|
||||
remain available; production supplies delay rather than an override.
|
||||
|
||||
Diagnostics identify `v21-delayed-feedback` and report the requested delay,
|
||||
nominal frame delay, delayed feedback curvature/error and latest requested
|
||||
curvature separately. The reported frame delay is not a measured physical delay.
|
||||
|
||||
## Verification and limitations
|
||||
|
||||
The delay-tracking regression failed against the old feedback behavior: a wheel
|
||||
following a request with the configured delay still received correction. It
|
||||
passes with delayed feedback through entry, reversal and release, in both turn
|
||||
directions and both reference modes. A separate test executes the actual
|
||||
upstream torque controller's buffer-selection expressions as an oracle.
|
||||
|
||||
Integration tests execute controlsd's actual delay wiring, source selection,
|
||||
limiter, adapter and Float32 publication through the Ford CAN sender. A
|
||||
delay-matched wheel trace generates zero P/I while the current base commands
|
||||
reach CAN immediately. Persistent error still integrates; repeated measurements
|
||||
do not integrate twice; driver and fresh PSCM override still clear correction
|
||||
immediately. Exact suite counts are in the validation JSON.
|
||||
|
||||
Four native-time route replays cover 583,599 control cycles and 58,362 CAN
|
||||
pack/decode checks. The baseline is the filtered-driver controller at
|
||||
`baeabfaa807c7a07baf183968e570f2c1d3fd665`. Baseline commands match the archived
|
||||
v20 replay exactly on all four routes. A third controller with zero delay
|
||||
matches that baseline's commands and integral on every cycle. With delay,
|
||||
command validity, base heading/overflow and feedback arbitration remain equal.
|
||||
All commands remain bounded with C2=C3=0.
|
||||
|
||||
| Route | Low-speed C0 changes >0.25 m, v20 → v21 | Low-speed C1 changes >0.05 rad, v20 → v21 | C1 at field bound, v20 → v21 |
|
||||
| --- | --- | --- | --- |
|
||||
| 162 | 70 → 68 | 50 → 44 | 1.06 → 0.92 s |
|
||||
| 157 | 137 → 110 | 59 → 51 | 6.52 → 5.94 s |
|
||||
| 151 | 77 → 65 | 42 → 36 | 4.04 → 3.89 s |
|
||||
| 149 | 197 → 159 | 111 → 95 | 16.08 → 13.23 s |
|
||||
|
||||
Low speed means below 15 mph; these counts include valid driver-interaction
|
||||
periods and are command-continuity measurements, not autonomous tracking scores.
|
||||
Neither baseline nor candidate was driven in these recordings; both command
|
||||
streams use the same recorded requests and vehicle motion.
|
||||
|
||||
The change reduces transient correction. At route-162 time 302.649 s, requesting
|
||||
141.4 degrees with the wheel at 87.3 degrees, replay C0 changes from -2.19 to
|
||||
-1.73 m and C1 from -0.3695 to -0.3135 rad. At 307.502 s, after driver input,
|
||||
opposite-direction release C0 falls from +1.52 to +1.04 m and C1 from +0.0655
|
||||
to +0.0355 rad. The base request is unchanged in each example.
|
||||
|
||||
This can mean less correction during both entry and unwind. Fewer large command
|
||||
steps do not prove reduced physical oscillation, maintained turn authority or
|
||||
better release. These are frozen-motion replays, not a validated PSCM simulator.
|
||||
The supplied delay has not been identified specifically for the combined C0/C1
|
||||
response. The next physical evaluation must distinguish these effects; this is
|
||||
not a demonstrated death-wobble fix.
|
||||
|
||||
Reproduce with the project's Python environment and built dependencies:
|
||||
|
||||
```sh
|
||||
PYTHONPATH=.:opendbc_repo:.cache/ford_geometry_deps python \
|
||||
tools/ford_pscm_lab/filtered_driver_replay.py \
|
||||
--source .cache/ford_route162/full \
|
||||
--delay-intake .cache/ford_route162/intake.npz \
|
||||
--baseline baeabfaa807c7a07baf183968e570f2c1d3fd665 \
|
||||
--output .cache/ford_feedback_delay_v21/162
|
||||
```
|
||||
|
||||
Pull and restart the software while offroad. The existing master toggle still
|
||||
selects upstream Ford control when disabled. For the current action trial,
|
||||
leave Model Geometry Reference and C0 one-second distance disabled.
|
||||
@@ -0,0 +1,391 @@
|
||||
{
|
||||
"baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665",
|
||||
"tests": {
|
||||
"passed": 722,
|
||||
"subtests_passed": 2
|
||||
},
|
||||
"total_cycles": 583599,
|
||||
"total_wire_checks": 58362,
|
||||
"code_sha256": {
|
||||
"openpilot/selfdrive/controls/lib/ford_model_action.py": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d",
|
||||
"openpilot/selfdrive/controls/controlsd.py": "3427212538fcb1cf4d8a44f6be62c54b31ab5ba1a3eb16d958bb58f9c52abecf",
|
||||
"openpilot/selfdrive/controls/lib/latcontrol_torque.py": "83ea33d37c349b10eca994150dae2a5841ec56b7038402432205a74054c917ee",
|
||||
"tools/ford_pscm_lab/filtered_driver_replay.py": "9c210b31df62cb6f41e6cf10d76e2e7ba15eefbbfcb30a8ea0244e095365543a"
|
||||
},
|
||||
"method": "Same frozen recorded inputs for v20, v21 using logged lateralDelay, and v21 with zero delay. The latter must exactly match v20 at every cycle. No counterfactual wheel response is predicted.",
|
||||
"limitations": "Supplied delay is not an identified C0/C1-specific response model. Lower command steps do not establish physical stability. Feedback demand is lower on rising and falling requests; entry and unwind can weaken.",
|
||||
"points": [
|
||||
{
|
||||
"meaning": "Clean entry shortfall",
|
||||
"t": 302.64934647200005,
|
||||
"desired_angle": 141.37510681152344,
|
||||
"old_c0": -2.1900000000000004,
|
||||
"new_c0": -1.7300000000000004,
|
||||
"old_c1": -0.36950000000000005,
|
||||
"new_c1": -0.3135,
|
||||
"old_i": -0.05458394920016066,
|
||||
"new_i": -0.02249363859059498,
|
||||
"old_reference": -0.03498964384198189,
|
||||
"new_reference": -0.030434442684054375
|
||||
},
|
||||
{
|
||||
"meaning": "Release following driver input; command comparison only",
|
||||
"t": 307.502298498,
|
||||
"desired_angle": 22.443912506103516,
|
||||
"old_c0": 1.52,
|
||||
"new_c0": 1.04,
|
||||
"old_c1": 0.0655,
|
||||
"new_c1": 0.035499999999999976,
|
||||
"old_i": 0.018708751669361232,
|
||||
"new_i": 0.01363232660438634,
|
||||
"old_reference": -0.005546241067349911,
|
||||
"new_reference": -0.010331000201404095
|
||||
}
|
||||
],
|
||||
"routes": {
|
||||
"162": {
|
||||
"cycles": 49003,
|
||||
"wire_checks": 4901,
|
||||
"zero_delay_matches_baseline": true,
|
||||
"feedback_delay_requested_min_median_max": [
|
||||
0.16894637048244476,
|
||||
0.16894637048244476,
|
||||
0.16894637048244476
|
||||
],
|
||||
"feedback_delay_used_min_median_max": [
|
||||
0.16,
|
||||
0.16,
|
||||
0.16
|
||||
],
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
2.3841861818141297e-09,
|
||||
5.2452087118126656e-08,
|
||||
0.39000000458657785,
|
||||
2.5800000023841863
|
||||
],
|
||||
"c1": [
|
||||
0.001499999597668611,
|
||||
0.07550000331401825,
|
||||
0.11149999844551088,
|
||||
0.24500000381469733
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.019999999999999574,
|
||||
0.22000000000000064,
|
||||
0.4300000000000006,
|
||||
0.839999999999999
|
||||
],
|
||||
"c1": [
|
||||
0.003500000000000003,
|
||||
0.02899999999999997,
|
||||
0.04049999999999998,
|
||||
0.0635
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 218,
|
||||
"feedback_switches_low_speed": 95,
|
||||
"low_speed_c0_steps_over_025m": 70,
|
||||
"low_speed_c1_steps_over_005rad": 50,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.16000000000000014,
|
||||
"c1": 0.019949999999999843
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 1.0580744659999937
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 218,
|
||||
"feedback_switches_low_speed": 95,
|
||||
"low_speed_c0_steps_over_025m": 68,
|
||||
"low_speed_c1_steps_over_005rad": 44,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.13999999999999968,
|
||||
"c1": 0.018449999999999786
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 0.9192208439999945
|
||||
}
|
||||
},
|
||||
"method": "Compare Ford feedback variants on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665",
|
||||
"baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route162/full/route.npz": "a725f9ca309b90a2d750d51a9b9d5b459d99119a54ab048631bc0fce07348af8",
|
||||
".cache/ford_route162/full/model_paths.npz": "cffe310b122d9ae26a53ee504608156b4242a6bba5f681898d5b661951ac5ce5",
|
||||
".cache/ford_route162/full/metadata.json": "ef3ac1caca0eaec31f7140a0743bd3b98885ce143cb0f97a8eee615b33943d7f",
|
||||
".cache/ford_route162/intake.npz": "9455b01e1d1d3dcba08276b382a8c6cd54ff6fb19cb417b72d0ac89cf193034b"
|
||||
},
|
||||
"baseline_matches_archived_v20_commands_and_integral": true
|
||||
},
|
||||
"157": {
|
||||
"cycles": 76554,
|
||||
"wire_checks": 7656,
|
||||
"zero_delay_matches_baseline": true,
|
||||
"feedback_delay_requested_min_median_max": [
|
||||
0.0,
|
||||
0.16894637048244476,
|
||||
0.16894637048244476
|
||||
],
|
||||
"feedback_delay_used_min_median_max": [
|
||||
0.16,
|
||||
0.16,
|
||||
0.16
|
||||
],
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.009999999999999787,
|
||||
0.41999999010562905,
|
||||
1.3956999619007082,
|
||||
5.39999999165535
|
||||
],
|
||||
"c1": [
|
||||
4.291534405620467e-09,
|
||||
0.03549999962002037,
|
||||
0.06600000168576836,
|
||||
0.4085000002384186
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.009999999999999787,
|
||||
0.21999999999999975,
|
||||
0.4900000000000002,
|
||||
1.5100000000000007
|
||||
],
|
||||
"c1": [
|
||||
0.0030000000000000027,
|
||||
0.028999999999999915,
|
||||
0.039000000000000035,
|
||||
0.09049999999999997
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 224,
|
||||
"feedback_switches_low_speed": 148,
|
||||
"low_speed_c0_steps_over_025m": 137,
|
||||
"low_speed_c1_steps_over_005rad": 59,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.20000000000000018,
|
||||
"c1": 0.022499999999999964
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.2999506119999751,
|
||||
"c1_bound_active_s": 6.521285587999955
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 224,
|
||||
"feedback_switches_low_speed": 148,
|
||||
"low_speed_c0_steps_over_025m": 110,
|
||||
"low_speed_c1_steps_over_005rad": 51,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.16000000000000014,
|
||||
"c1": 0.01750000000000007
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.2999506119999751,
|
||||
"c1_bound_active_s": 5.943273473999852
|
||||
}
|
||||
},
|
||||
"method": "Compare Ford feedback variants on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665",
|
||||
"baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb",
|
||||
".cache/ford_route157/full/model_paths.npz": "25fc2526e092fde5ceb5aab63a5aa4f73a2c989a3263cedb0a26faf8da61c47b",
|
||||
".cache/ford_route157/full/metadata.json": "f2d72c9a877ed6694e4da6831841113dc0c05987e2c76cca51f358d12411b235",
|
||||
".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4"
|
||||
},
|
||||
"baseline_matches_archived_v20_commands_and_integral": true
|
||||
},
|
||||
"151": {
|
||||
"cycles": 325708,
|
||||
"wire_checks": 32571,
|
||||
"zero_delay_matches_baseline": true,
|
||||
"feedback_delay_requested_min_median_max": [
|
||||
0.16894637048244476,
|
||||
0.16894637048244476,
|
||||
0.16894637048244476
|
||||
],
|
||||
"feedback_delay_used_min_median_max": [
|
||||
0.16,
|
||||
0.16,
|
||||
0.16
|
||||
],
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.010000000223516992,
|
||||
0.2000000013411043,
|
||||
1.010000009727478,
|
||||
4.550000023841858
|
||||
],
|
||||
"c1": [
|
||||
0.004999999240040742,
|
||||
0.02900000113248826,
|
||||
0.07250000010803342,
|
||||
0.28449999523162844
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.009999999999999787,
|
||||
0.07999999999999999,
|
||||
0.2900000000000002,
|
||||
1.2599999999999998
|
||||
],
|
||||
"c1": [
|
||||
0.0025000000000000022,
|
||||
0.02250000000000002,
|
||||
0.038500000000000034,
|
||||
0.07650000000000001
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 259,
|
||||
"feedback_switches_low_speed": 101,
|
||||
"low_speed_c0_steps_over_025m": 77,
|
||||
"low_speed_c1_steps_over_005rad": 42,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.13999999999999968,
|
||||
"c1": 0.014499999999999957
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.029249633000290487,
|
||||
"c1_bound_active_s": 4.042117826999856
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 259,
|
||||
"feedback_switches_low_speed": 101,
|
||||
"low_speed_c0_steps_over_025m": 65,
|
||||
"low_speed_c1_steps_over_005rad": 36,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.10000000000000053,
|
||||
"c1": 0.01050000000000001
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 3.8897953380001127
|
||||
}
|
||||
},
|
||||
"method": "Compare Ford feedback variants on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665",
|
||||
"baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1",
|
||||
".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b",
|
||||
".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1",
|
||||
".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9"
|
||||
},
|
||||
"baseline_matches_archived_v20_commands_and_integral": true
|
||||
},
|
||||
"149": {
|
||||
"cycles": 132334,
|
||||
"wire_checks": 13234,
|
||||
"zero_delay_matches_baseline": true,
|
||||
"feedback_delay_requested_min_median_max": [
|
||||
0.16894637048244476,
|
||||
0.16894637048244476,
|
||||
0.16894637048244476
|
||||
],
|
||||
"feedback_delay_used_min_median_max": [
|
||||
0.16,
|
||||
0.16,
|
||||
0.16
|
||||
],
|
||||
"baseline_recorded_error_p50_p95_p99_max": {
|
||||
"c0": [
|
||||
0.02999999910593054,
|
||||
0.8400000008046626,
|
||||
2.1400000276565554,
|
||||
4.560000002980233
|
||||
],
|
||||
"c1": [
|
||||
0.015500000141560999,
|
||||
0.10499999982118613,
|
||||
0.32499999746203423,
|
||||
0.43149999833107
|
||||
]
|
||||
},
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.009999999999999787,
|
||||
0.1999999999999993,
|
||||
0.4900000000000002,
|
||||
1.3100000000000005
|
||||
],
|
||||
"c1": [
|
||||
0.0040000000000000036,
|
||||
0.03200000000000003,
|
||||
0.04999999999999999,
|
||||
0.08850000000000002
|
||||
]
|
||||
},
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 354,
|
||||
"feedback_switches_low_speed": 171,
|
||||
"low_speed_c0_steps_over_025m": 197,
|
||||
"low_speed_c1_steps_over_005rad": 111,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.1999999999999993,
|
||||
"c1": 0.02150000000000002
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 16.08094779300012
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 354,
|
||||
"feedback_switches_low_speed": 171,
|
||||
"low_speed_c0_steps_over_025m": 159,
|
||||
"low_speed_c1_steps_over_005rad": 95,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.16000000000000014,
|
||||
"c1": 0.017500000000000016
|
||||
},
|
||||
"raw_crossing_c0_steps_over_025m": 0,
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 13.22902962499984
|
||||
}
|
||||
},
|
||||
"method": "Compare Ford feedback variants on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\nBoth controllers receive the logged selected curvature. Recorded-command agreement\nis expected only for routes driven with the selected baseline and fixed 7 m C0.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "baeabfaa807c7a07baf183968e570f2c1d3fd665",
|
||||
"baseline_controller_sha256": "b34a55f84fdbf6c432ce89cdc378aaaf9bbd765ca4e6934845059f586e6f0773",
|
||||
"candidate_controller_sha256": "d1730a79307297b32b44ba18e9792df216a1961a3f32927c5e6acf28014d288d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
|
||||
".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
|
||||
".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32",
|
||||
".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d"
|
||||
},
|
||||
"baseline_matches_archived_v20_commands_and_integral": true
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -180,6 +180,7 @@ class Controls(ControlsExt):
|
||||
model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
|
||||
reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
|
||||
active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
|
||||
lat_delay=lat_delay,
|
||||
driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
|
||||
reference_source=reference_service, roll=lp.roll,
|
||||
pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
|
||||
|
||||
@@ -8,12 +8,14 @@ Commands use the current bounded request without an additional C0/C1 slew.
|
||||
The direct-path trial samples model position and heading separately, retains
|
||||
independent upstream request limits, and uses heading-equivalent feedback.
|
||||
"""
|
||||
from collections import deque
|
||||
import math
|
||||
import struct
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
|
||||
@@ -28,6 +30,7 @@ DIRECT_PATH_C0_PROPORTIONAL_GAIN = 0.5
|
||||
C0_RESPONSE_CONSTANT = 0.010717679293424373
|
||||
C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647
|
||||
CALIBRATION_APPROVED = False
|
||||
CURVATURE_REQUEST_BUFFER_SECONDS = 1.0
|
||||
|
||||
|
||||
def _packed(value, resolution, offset):
|
||||
@@ -177,8 +180,9 @@ class FordModelActionController:
|
||||
|
||||
controlsd owns upstream selection/limiting and service health. This adapter
|
||||
checks ages and clock order, then supplies elapsed time to the core.
|
||||
Feedback advances once per fresh steering measurement; repeated samples
|
||||
still use the current request. Raw model geometry is checked on every cycle.
|
||||
Base commands use the latest request. Like comma's torque controller, feedback
|
||||
uses a one-second request buffer indexed by lateralDelay at the 100 Hz control
|
||||
cadence. Only integration waits for fresh steering measurements.
|
||||
|
||||
CAN yaw remains a health gate, not the feedback measurement. Ford's filtered
|
||||
steeringPressed and fresh PSCM driver overrides clear the correction.
|
||||
@@ -192,8 +196,10 @@ class FordModelActionController:
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
|
||||
c0_proportional_gain=c0_proportional_gain)
|
||||
self.direct_path = bool(direct_path)
|
||||
self.hypothesis = ('model-path-direct-feedback-v20-filtered-driver' if self.direct_path else
|
||||
'model-action-curvature-c0-feedback-v20-filtered-driver')
|
||||
self.request_buffer_size = int(CURVATURE_REQUEST_BUFFER_SECONDS / DT_CTRL)
|
||||
self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size)
|
||||
self.hypothesis = ('model-path-direct-feedback-v21-delayed-feedback' if self.direct_path else
|
||||
'model-action-curvature-c0-feedback-v21-delayed-feedback')
|
||||
self.reset()
|
||||
|
||||
def path_curvature(self, model, speed):
|
||||
@@ -211,6 +217,8 @@ class FordModelActionController:
|
||||
|
||||
def reset(self, status='inactive'):
|
||||
self.core.reset()
|
||||
if status != 'inactive':
|
||||
self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size)
|
||||
self.last_time = self.last_measurement_time = self.last_model_time = None
|
||||
self.diagnostics = {'status': status, 'hypothesis': self.hypothesis,
|
||||
'c0_time_based': self.core.c0_time_based,
|
||||
@@ -218,13 +226,22 @@ class FordModelActionController:
|
||||
|
||||
def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time,
|
||||
reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None,
|
||||
feedback_curvature=None, curvature_scale=1., reference_source='modelV2', roll=0.):
|
||||
feedback_curvature=None, curvature_scale=1., reference_source='modelV2', roll=0., lat_delay=0.):
|
||||
# Record on every control cycle, including disengagement, as upstream does.
|
||||
# This buffer changes feedback only; it never queues the outgoing base path.
|
||||
feedback_delay = 0.
|
||||
if _finite(desired_curvature, lat_delay) and abs(desired_curvature) <= 1.:
|
||||
self.request_buffer.append(desired_curvature)
|
||||
delay_frames = int(np.clip(lat_delay / DT_CTRL + 1, 1, self.request_buffer_size))
|
||||
if feedback_curvature is None:
|
||||
feedback_curvature = self.request_buffer[-delay_frames]
|
||||
feedback_delay = (delay_frames - 1) * DT_CTRL
|
||||
reason = None
|
||||
if not active:
|
||||
reason = 'inactive'
|
||||
elif not valid:
|
||||
reason = 'invalid_service'
|
||||
elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
|
||||
elif not _finite(desired_curvature, current_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time, lat_delay):
|
||||
reason = 'nonfinite'
|
||||
elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)):
|
||||
reason = 'stale_input'
|
||||
@@ -267,6 +284,7 @@ class FordModelActionController:
|
||||
'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
|
||||
'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
|
||||
'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt,
|
||||
'feedback_delay_requested': lat_delay, 'feedback_delay': feedback_delay,
|
||||
'heading_feedforward': base_heading,
|
||||
'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading),
|
||||
'offset_proportional': self.core.offset_proportional, 'c0_proportional_gain': self.core.c0_proportional_gain,
|
||||
|
||||
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015,
|
||||
sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
|
||||
record = self.emit_controls_event('Ford C2-free path tracking', controls)
|
||||
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v20-filtered-driver')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v21-delayed-feedback')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -0,0 +1,127 @@
|
||||
"""Feedforward remains current while feedback follows comma's request history."""
|
||||
import ast
|
||||
from collections import deque
|
||||
import math
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, encode_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_feedback import status
|
||||
|
||||
|
||||
def step(controller, frame, desired, measured=0., **overrides):
|
||||
now = 1.+frame*.01
|
||||
kwargs = {'current_curvature': measured, 'speed': 5., 'yaw_rate': 0., 'now': now,
|
||||
'measurement_time': now, 'model_time': now, 'reference_time': now,
|
||||
'active': True, 'lat_delay': .2}
|
||||
kwargs.update(overrides)
|
||||
return controller.update(straight(), desired, **kwargs)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('direct_path', [False, True])
|
||||
def test_perfectly_delayed_tracking_does_not_generate_phantom_feedback(sign, direct_path):
|
||||
controller = FordModelActionController(direct_path=direct_path)
|
||||
requests = [0.]*100+[sign*.004]*100+[-sign*.003]*100+[0.]*100
|
||||
for i, desired in enumerate(requests):
|
||||
measured = requests[max(0, i-20)]
|
||||
command = step(controller, i, desired, measured)
|
||||
assert command.valid
|
||||
assert controller.core.feedback_curvature == pytest.approx(measured)
|
||||
assert controller.core.proportional == controller.core.offset_proportional == controller.core.correction == 0.
|
||||
# Current heading reverses/releases immediately even while feedback uses the old request.
|
||||
assert command.path_angle == pytest.approx(7.*desired, abs=.00025)
|
||||
if not direct_path:
|
||||
assert command.path_offset == pytest.approx(encode_model_action(straight(), desired, 5.).path_offset, abs=.005)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('delay', [-1., 0., .009, .16894637048244476, .2, .4, .99, 2.])
|
||||
def test_same_buffer_index_as_comma_torque_controller(delay):
|
||||
controller = FordModelActionController()
|
||||
upstream = deque([0.]*100, maxlen=100)
|
||||
# Execute the actual upstream selection expressions, not a second copy of our formula.
|
||||
source_path = Path(__file__).resolve().parents[1]/'lib/latcontrol_torque.py'
|
||||
tree = ast.parse(source_path.read_text())
|
||||
cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'LatControlTorque')
|
||||
method = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == 'update')
|
||||
nodes = [n for n in method.body if isinstance(n, ast.Assign) and
|
||||
ast.unparse(n.targets[0]) in ('delay_frames', 'expected_lateral_accel')]
|
||||
assert len(nodes) == 2
|
||||
oracle = compile(ast.Module(body=nodes, type_ignores=[]), str(source_path), 'exec')
|
||||
torque = SimpleNamespace(dt=.01, lat_accel_request_buffer=upstream, lat_accel_request_buffer_len=100)
|
||||
for i in range(230):
|
||||
desired = .005*math.sin(i*.023)
|
||||
upstream.append(desired)
|
||||
context = {'self': torque, 'np': np, 'lat_delay': delay}
|
||||
exec(oracle, context)
|
||||
expected = context['expected_lateral_accel']
|
||||
step(controller, i, desired, lat_delay=delay)
|
||||
assert controller.core.feedback_curvature == expected
|
||||
assert controller.diagnostics['feedback_delay'] == pytest.approx((context['delay_frames']-1)*.01)
|
||||
|
||||
|
||||
def test_history_keeps_up_while_disengaged_and_delay_changes_without_another_queue():
|
||||
controller = FordModelActionController()
|
||||
requests = []
|
||||
for i in range(170):
|
||||
desired = .00001*i
|
||||
requests.append(desired)
|
||||
if i < 130:
|
||||
assert step(controller, i, desired, active=False) == FordPath()
|
||||
else:
|
||||
delay = .1 if i < 150 else .3
|
||||
measured = requests[i-round(delay/.01)]
|
||||
step(controller, i, desired, measured, lat_delay=delay)
|
||||
assert controller.core.feedback_curvature == pytest.approx(measured)
|
||||
assert controller.core.correction == 0.
|
||||
|
||||
|
||||
def test_persistent_error_still_builds_correction_and_repeated_measurements_do_not_double_integrate():
|
||||
controller = FordModelActionController()
|
||||
for i in range(150):
|
||||
step(controller, i, .004, active=False)
|
||||
step(controller, 150, .004, .003)
|
||||
for i in range(151, 251):
|
||||
step(controller, i, .004, .003)
|
||||
assert controller.core.proportional > 0. and controller.core.offset_proportional > 0.
|
||||
assert controller.core.correction == pytest.approx(.005)
|
||||
for i in range(251, 256):
|
||||
step(controller, i, .004, .003, measurement_time=3.5)
|
||||
assert controller.core.correction == pytest.approx(.005)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('override', ['driver', 'pscm'])
|
||||
def test_driver_override_still_clears_feedback_immediately_with_delayed_reference(override):
|
||||
controller = FordModelActionController()
|
||||
for i in range(150):
|
||||
step(controller, i, .004)
|
||||
assert controller.core.correction > 0.
|
||||
kwargs = {'driver_pressed': True} if override == 'driver' else {'pscm_status': status(2.5, limit=3)}
|
||||
command = step(controller, 150, -.004, **kwargs)
|
||||
assert controller.core.proportional == controller.core.offset_proportional == controller.core.correction == 0.
|
||||
assert command.path_angle < 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('delay', [math.nan, math.inf, None])
|
||||
def test_invalid_delay_rejects_command_without_raising(delay):
|
||||
controller = FordModelActionController()
|
||||
assert step(controller, 0, .004, lat_delay=delay) == FordPath()
|
||||
assert controller.diagnostics['status'] == 'nonfinite'
|
||||
|
||||
|
||||
@pytest.mark.parametrize('invalid', [{'valid': False}, {'desired': math.nan}, {'lat_delay': math.nan}, {'now': 5.}])
|
||||
def test_bad_input_or_timing_resets_request_history(invalid):
|
||||
controller = FordModelActionController()
|
||||
for i in range(150):
|
||||
step(controller, i, .004)
|
||||
options = dict(invalid)
|
||||
desired = options.pop('desired', .004)
|
||||
assert step(controller, 150, desired, **options) == FordPath()
|
||||
assert all(value == 0. for value in controller.request_buffer)
|
||||
assert step(controller, 151, 0.).valid
|
||||
assert controller.core.feedback_curvature == 0.
|
||||
@@ -145,8 +145,11 @@ def pipeline():
|
||||
selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']")
|
||||
limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and
|
||||
isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature')
|
||||
delay = next(n for n in body if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'lat_delay')
|
||||
model_source = ast.parse((root/'selfdrive/modeld/modeld.py').read_text())
|
||||
smoothing = next(n for n in model_source.body if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'LAT_SMOOTH_SECONDS')
|
||||
branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
|
||||
call = compile(ast.Module(body=[selection, limiter, branch], type_ignores=[]), str(controls_file), 'exec')
|
||||
call = compile(ast.Module(body=[smoothing, selection, limiter, delay, branch], type_ignores=[]), str(controls_file), 'exec')
|
||||
publication_file = root/'sunnypilot/selfdrive/controls/controlsd_ext.py'
|
||||
body = _method(publication_file, 'ControlsExt', 'state_control_ext').body
|
||||
publish = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'ford_path') or
|
||||
@@ -163,7 +166,8 @@ class Subscriptions:
|
||||
self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True, 'carStateSP': True}
|
||||
self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000}
|
||||
self.failed = set()
|
||||
self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1)}
|
||||
self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1),
|
||||
'lateralDelay': SimpleNamespace(lateralDelay=0.)}
|
||||
|
||||
def __getitem__(self, service):
|
||||
return self.messages[service]
|
||||
@@ -291,6 +295,51 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
assert (core.offset_proportional == 0.) == (cs.steeringPressed or measured == sign*.004)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('delay', [.16894637048244476, .2])
|
||||
def test_controlsd_uses_lateral_delay_for_feedback_and_current_base_on_can(pipeline, sign, delay):
|
||||
call, publication = pipeline
|
||||
controls, sm = startup(), Subscriptions(False)
|
||||
controls.sm, controls.desired_curvature = sm, 0.
|
||||
sm.messages['lateralDelay'].lateralDelay = delay
|
||||
model = straight()
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
|
||||
downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=5., vEgoRaw=5.), acc_tja_status_stock_values=defaultdict(int),
|
||||
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
|
||||
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main)
|
||||
history = [0.]*100
|
||||
for frame, desired in enumerate([0.]*100+[sign*.004]*100+[-sign*.003]*100+[0.]*100):
|
||||
now = 1.+frame*.01
|
||||
model.action = SimpleNamespace(desiredCurvature=desired)
|
||||
selected, _ = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.)
|
||||
history.append(selected)
|
||||
controls.curvature = history[-int(delay/.01+1)]
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
'time': SimpleNamespace(monotonic=lambda now=now: now)})
|
||||
core = controls.ford_path_controller.core
|
||||
assert core.feedback_curvature == controls.curvature
|
||||
assert core.proportional == core.offset_proportional == core.correction == 0.
|
||||
assert_current_request(core, selected, cs.vEgo)
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(publication, {'self': controls, 'CC_SP': msg})
|
||||
_, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9))
|
||||
received = parser.update([round(now*1e9), packets])
|
||||
address = parser.dbc.name_to_msg['LateralMotionControl2'].address
|
||||
assert address in received
|
||||
wire = parser.vl['LateralMotionControl2']
|
||||
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-encode_model_action(model, selected, cs.vEgo).path_offset, abs=.005)
|
||||
assert wire['LatCtlPath_An_Actl'] == pytest.approx(-7.*selected, abs=.00025)
|
||||
assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
|
||||
assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16
|
||||
packet = next(packet for packet in packets if packet[0] == address)
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
|
||||
|
||||
@pytest.mark.parametrize('service_valid', [False, True])
|
||||
def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, service_valid):
|
||||
controls, sm = startup(), Subscriptions(False)
|
||||
@@ -363,7 +412,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
if frame == 199:
|
||||
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v20-filtered-driver'
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback'
|
||||
if same_turn:
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
|
||||
|
||||
@@ -53,7 +53,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
|
||||
assert selected.ford_path_controller.core.c0_proportional_gain == 1.
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v20-filtered-driver'
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
"""Compare driver-input arbitration on recorded references and frozen vehicle motion.
|
||||
"""Compare Ford feedback variants on recorded references and frozen vehicle motion.
|
||||
|
||||
Input: extract.py route.npz/model_paths.npz/metadata.json directories. This checks
|
||||
command continuity and overrides; it does not predict a changed wheel response.
|
||||
@@ -6,6 +6,7 @@ Both controllers receive the logged selected curvature. Recorded-command agreeme
|
||||
is expected only for routes driven with the selected baseline and fixed 7 m C0.
|
||||
"""
|
||||
import argparse
|
||||
import ast
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
@@ -19,7 +20,7 @@ from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionCo
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
|
||||
|
||||
|
||||
def replay(source, destination, baseline_class, provenance):
|
||||
def replay(source, destination, baseline_class, provenance, *, delay_intake=None):
|
||||
meta = json.loads((source/'metadata.json').read_text())
|
||||
with np.load(source/'route.npz') as z:
|
||||
r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'path', 'params', 'pscm', 'model')}
|
||||
@@ -28,6 +29,16 @@ def replay(source, destination, baseline_class, provenance):
|
||||
path_ns = z['ns']
|
||||
c = r['controls']
|
||||
t = c['t']
|
||||
delays = np.zeros(len(t))
|
||||
if delay_intake is not None:
|
||||
with np.load(delay_intake) as z:
|
||||
stream = z['delay']
|
||||
# Match controlsd's lateralDelay message plus its imported modeld constant.
|
||||
source_tree = ast.parse(Path('openpilot/selfdrive/modeld/modeld.py').read_text())
|
||||
smoothing = next(ast.literal_eval(n.value) for n in source_tree.body
|
||||
if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'LAT_SMOOTH_SECONDS')
|
||||
delays = sample({'t': stream[:, 0], 'value': stream[:, 3]}, t)['value'] + smoothing
|
||||
delays[t < stream[0, 0]] = smoothing
|
||||
cs, pa, ps = [sample(r[k], t) for k in ('cs', 'params', 'pscm')]
|
||||
cc, sent = [sample(r[k], t, nearest=True) for k in ('cc', 'path')]
|
||||
mi = np.clip(np.searchsorted(path_ns, c['model_ns']), 0, len(path_ns)-1)
|
||||
@@ -38,10 +49,12 @@ def replay(source, destination, baseline_class, provenance):
|
||||
steerRatio=car['steer_ratio'], rotationalInertia=0.)
|
||||
vm = VehicleModel(cp)
|
||||
cores = [baseline_class(), FordModelActionController()]
|
||||
zero_delay = FordModelActionController() if delay_intake is not None else None
|
||||
wire = WireCheck()
|
||||
names = ['t', 'active', 'valid', 'speed', 'pressed', 'raw_torque', 'pscm_override', 'limit',
|
||||
'old_c0', 'new_c0', 'old_c1', 'new_c1', 'old_i', 'new_i', 'old_feedback', 'new_feedback',
|
||||
'recorded_c0', 'recorded_c1', 'latched_angle_error', 'model_ns', 'desired_angle']
|
||||
'recorded_c0', 'recorded_c1', 'latched_angle_error', 'model_ns', 'desired_angle',
|
||||
'desired', 'measured', 'old_reference', 'new_reference', 'feedback_delay', 'old_c0_p', 'new_c0_p', 'old_c1_p', 'new_c1_p']
|
||||
rows = np.zeros((len(t), len(names)))
|
||||
for i, now in enumerate(t):
|
||||
vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
|
||||
@@ -54,12 +67,13 @@ def replay(source, destination, baseline_class, provenance):
|
||||
active = bool(cc['active'][i])
|
||||
valid = bool(c['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and pa['valid'][i] and exact[i])
|
||||
commands = []
|
||||
for core in cores:
|
||||
command = core.update(models[mi[i]], c['desired'][i], current_curvature=c['measured'][i],
|
||||
speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, measurement_time=cs['t'][i],
|
||||
model_time=c['model_ns'][i]*1e-9, reference_time=c['model_ns'][i]*1e-9,
|
||||
active=active, valid=valid, driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i],
|
||||
pscm_status=status, curvature_scale=scale)
|
||||
inputs = {'current_curvature': c['measured'][i], 'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now, 'measurement_time': cs['t'][i],
|
||||
'model_time': c['model_ns'][i]*1e-9, 'reference_time': c['model_ns'][i]*1e-9,
|
||||
'active': active, 'valid': valid, 'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i],
|
||||
'pscm_status': status, 'curvature_scale': scale}
|
||||
for variant, core in enumerate(cores):
|
||||
delay_args = {'lat_delay': delays[i]} if variant == 1 and delay_intake is not None else {}
|
||||
command = core.update(models[mi[i]], c['desired'][i], **inputs, **delay_args)
|
||||
assert abs(command.path_offset) <= 5.1100001 and abs(command.path_angle) <= .5000001
|
||||
assert command.curvature == command.curvature_rate == 0.
|
||||
if command.valid and (cs['pressed'][i] or (status.valid and -.005 <= now-ps['stamp'][i] <= .15 and status.limit == 3)):
|
||||
@@ -67,13 +81,21 @@ def replay(source, destination, baseline_class, provenance):
|
||||
assert core.core.correction == core.core.proportional == core.core.offset_proportional == 0.
|
||||
commands.append(command)
|
||||
assert commands[0].valid == commands[1].valid
|
||||
if zero_delay is not None:
|
||||
assert zero_delay.update(models[mi[i]], c['desired'][i], **inputs, lat_delay=0.) == commands[0]
|
||||
assert zero_delay.core.correction == cores[0].core.correction
|
||||
for key in ('heading_feedforward', 'offset_overflow', 'driver_override', 'feedback_enabled', 'pscm_limited'):
|
||||
assert cores[0].diagnostics.get(key) == cores[1].diagnostics.get(key)
|
||||
if i % 10 == 0:
|
||||
wire.check(commands[1])
|
||||
old, new = commands
|
||||
rows[i] = [now-meta['t0'], active, new.valid, cs['speed'][i], cs['pressed'][i], cs['torque'][i], status.limit == 3, status.limit == 2,
|
||||
old.path_offset, new.path_offset, old.path_angle, new.path_angle, cores[0].core.correction, cores[1].core.correction,
|
||||
cores[0].diagnostics.get('feedback_enabled', False), cores[1].diagnostics.get('feedback_enabled', False),
|
||||
sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i]), c['model_ns'][i], c['desired_angle'][i]]
|
||||
sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i]), c['model_ns'][i], c['desired_angle'][i],
|
||||
c['desired'][i], c['measured'][i], cores[0].core.feedback_curvature, cores[1].core.feedback_curvature,
|
||||
cores[1].diagnostics.get('feedback_delay', 0.), cores[0].core.offset_proportional, cores[1].core.offset_proportional,
|
||||
cores[0].core.proportional, cores[1].core.proportional]
|
||||
a = dict(zip(names, rows.T, strict=True))
|
||||
live = a['valid'].astype(bool)
|
||||
consecutive = live[1:] & live[:-1] & (np.diff(t) < .03)
|
||||
@@ -83,6 +105,9 @@ def replay(source, destination, baseline_class, provenance):
|
||||
& ~a['pscm_override'][:-1].astype(bool) & ~a['pscm_override'][1:].astype(bool)
|
||||
& (np.diff(a['model_ns']) == 0) & (abs(np.diff(a['desired_angle'])) < .1))
|
||||
metrics = {'cycles': len(t), 'wire_checks': wire.count,
|
||||
'zero_delay_matches_baseline': True if zero_delay is not None else None,
|
||||
'feedback_delay_requested_min_median_max': np.quantile(delays, [0., .5, 1.]).tolist(),
|
||||
'feedback_delay_used_min_median_max': np.quantile(a['feedback_delay'][live], [0., .5, 1.]).tolist(),
|
||||
'baseline_recorded_error_p50_p95_p99_max': {
|
||||
k: np.quantile(abs(a['old_'+k][live]-a['recorded_'+k][live]), [.5, .95, .99, 1]).tolist() for k in ('c0', 'c1')},
|
||||
'latched_angle_match_fraction': float(np.mean(a['latched_angle_error'][live] < 1e-4)),
|
||||
@@ -104,6 +129,8 @@ def replay(source, destination, baseline_class, provenance):
|
||||
metrics['method'] = __doc__
|
||||
metrics['provenance'] = provenance
|
||||
metrics['sources_sha256'] = {str(source/k): hashlib.sha256((source/k).read_bytes()).hexdigest() for k in ('route.npz', 'model_paths.npz', 'metadata.json')}
|
||||
if delay_intake is not None:
|
||||
metrics['sources_sha256'][str(delay_intake)] = hashlib.sha256(delay_intake.read_bytes()).hexdigest()
|
||||
destination.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(destination/'commands.npz', names=names, rows=rows)
|
||||
(destination/'report.json').write_text(json.dumps(metrics, indent=2)+'\n')
|
||||
@@ -115,6 +142,7 @@ if __name__ == '__main__':
|
||||
parser.add_argument('--source', type=Path, required=True)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
parser.add_argument('--baseline', default='4900c0a40c87c72000b7168a6cf4fe6dd98ea6d0')
|
||||
parser.add_argument('--delay-intake', type=Path, help='intake.npz with logged lateralDelay; requires a baseline with identical gains and driver gating')
|
||||
args = parser.parse_args()
|
||||
revision = subprocess.check_output(['git', 'rev-parse', f'{args.baseline}^{{commit}}'], text=True).strip()
|
||||
controller_path = 'openpilot/selfdrive/controls/lib/ford_model_action.py'
|
||||
@@ -124,5 +152,5 @@ if __name__ == '__main__':
|
||||
provenance = {'baseline_commit': revision, 'baseline_controller_sha256': hashlib.sha256(code.encode()).hexdigest(),
|
||||
'candidate_controller_sha256': hashlib.sha256(Path(controller_path).read_bytes()).hexdigest(),
|
||||
'fixed_c0_distance_m': 7.}
|
||||
result = replay(args.source, args.output, namespace['FordModelActionController'], provenance)
|
||||
result = replay(args.source, args.output, namespace['FordModelActionController'], provenance, delay_intake=args.delay_intake)
|
||||
print(json.dumps({k: v for k, v in result.items() if k != 'sources_sha256'}, indent=2))
|
||||
|
||||
Reference in New Issue
Block a user