mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-18 10:23:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford: increase immediate C1 error correction to P=0.75

Browse Source
This commit is contained in:
Isaac Barham
2026-09-15 01:11:15 -04:00
parent b720e9f1bb
commit 31cdfa12a0
10 changed files with 982 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/ford_c1_p75_trial.md
+98
View File
@@ -0,0 +1,98 @@
# Ford C1 proportional trial: P=0.75
Route 149 contains large steering shortfalls before driver intervention while
the selected curvature matches the model and neither C1's bound nor the PSCM
reached-limit flag explains the shortfall. This trial raises the immediate C1
error correction from P=0.50 to P=0.75. I remains 0.25. Runtime changes are the
gain constant and the diagnostic version, `model-action-curvature-c0-distance-pi-v13`.
The intended effect is more correction while behind and more release correction
when measured steering exceeds the request. Increasing P does not establish a
faster physical response: it can also amplify measurement fluctuations and
produce oscillation. This is a trial coefficient, not a learned calibration.
The selected model action, +0.40 s low-speed preview, C0 distance setting and
formula, integral arithmetic, PSCM arbitration, field bounds, and 100 Hz sender
remain unchanged. C2/C3 stay zero. The existing default-off Sunnylink toggle
still selects the experiment on Ford CAN FD; toggle-off selects upstream Ford.
## Paired production replay
Compared explicit P=0.50 and P=0.75 production adapters with I=0.25 and fixed-7 m
C0 across 21 route extracts: 112–117, 119, 11a, 120, 124, 125, 146, 149, a0, a2,
a5, a9, b8, b9, ca, and Raptor 02. The passes cover 2,275,248 source cycles and
4,550,496 real Float32-to-CAN encode/decode round trips.
Both passes use the same recorded selected curvature, measured motion, model
geometry, input timestamps, and driver/PSCM flags. Older routes retain their
original model requests; their neural inference is not rerun with the new delay.
This compares commands, not predicted wheel motion or tracking accuracy.
Checks passed on every cycle: identical C0, eligibility, feedforward, overflow,
and feedback/driver/PSCM gates; finite and bounded output; inactive zero output;
zero C2/C3; exact decoded fields, mode and counter; and the expected 1.5 ratio
between proportional terms. Integration tests separately check the selected
defaults, downstream checksums, reference selection, reversals, driver override,
reached-limit behavior, duplicate measurements, and toggle-off upstream behavior.
On route 149, the P=0.50 replay agrees with the recorded path commands over the
clean scoring cohort to Float32 precision: maximum C0 difference 5.8e-8 m and C1
difference 1.5e-8 rad. Full SubMaster health and exact control execution clocks
are not in the extract; publication timestamps approximate them. Historical
versions used different command laws, so their recorded commands are not
expected to match this baseline.
Clean scoring excludes driver steering, unavailable feedback, inactive/invalid
control, the following second, and speed below 3 mph. It contains 11,128.80 s.
Durations use original timestamps, clipping gaps to 30 ms. Percentiles are
sample-based. Request-angle categories do not identify road geometry.
| Recorded request magnitude | Scored seconds | Mean absolute C1 change | P95 C1 change |
| --- | ---: | ---: | ---: |
| Under 10 degrees | 9,051.37 | 0.00088 rad | 0.00250 rad |
| 10–45 degrees | 1,631.68 | 0.00250 rad | 0.00900 rad |
| At least 45 degrees | 445.75 | 0.00829 rad | 0.03100 rad |
C1 bound exposure increases from 21.30 to 22.67 s over the clean cohort. Mean
absolute stored I changes from 0.005987 to 0.005984 rad; a larger P term changes
the remaining accumulation headroom even though I's gain is unchanged.
The largest small-request C1 difference is 0.093 rad on route 117, where the
recorded request is +4.6 degrees and the wheel is still at -210 degrees. This is
a large release error, not ordinary centering. Restricting both requested and
actual wheel angle to within 10 degrees leaves 8,788.38 s: mean absolute C1
change 0.00073 rad, P95 0.00200 rad, maximum 0.01250 rad. These measurements do not
establish preserved centering or closed-loop stability.
In route 149, the candidate increases the C1 request at the reviewed entry
misses and reduces the remaining turn command during the clean segment-14
release. Its clean C1 bound exposure rises from 0.23 to 0.61 s. The paired I
traces remain nearly identical. The local report includes five entry, reversal,
and exit comparisons with the recorded wheel trace clearly distinguished from
replayed command traces.
## Validation and reproduction
455 tests and 25 subtests pass, including Ford controller/adapter/selection,
C0 distance settings, diagnostic logging, delay helpers, and Ford CAN tests.
The actual controlsd-to-CAN integration test failed at the old proportional
output before changing the default, then passed at the new setting. Ruff and
`git diff --check` pass. A device build, installation, and physical evaluation
are not part of these offline checks.
The compact evidence record is [ford_c1_p75_validation.json](ford_c1_p75_validation.json).
Full command arrays and per-route reports are in `.cache/ford_p75_trial` locally.
Reproduce one route using the built cereal/opendbc environment:
```sh
PYTHONPATH=.:opendbc_repo PYTHONDONTWRITEBYTECODE=1 python \
tools/ford_pscm_lab/proportional_replay.py \
--routes 149=.cache/ford_route149/full \
--output .cache/ford_p75_recheck --workers 1
```
Additional `label=extract-directory` pairs replay independently. Each directory
must contain `route.npz`, `model_paths.npz`, and `metadata.json`; injection routes
are rejected. The input hashes are recorded in each result. The new test is
worth evaluating as a bounded change, but improved entry and preserved smooth
release still require measured vehicle response.
docs/ford_c1_p75_validation.json
+707
View File
@@ -0,0 +1,707 @@
{
"routes": 21,
"cycles": 2275248,
"can_round_trips": 4550496,
"scope": "Fixed-motion production controller and CAN replay. No predicted wheel motion or stability claim.",
"cohorts": {
"all": {
"seconds": 11128.798286258982,
"mean_abs_c1_change": 0.0014142689534451223,
"p95_abs_c1_change": 0.004500000000000004,
"max_abs_c1_change": 0.09299999999999997,
"c1_bound_seconds": [
21.296853938993763,
22.66821589999995
],
"mean_abs_integral": [
0.00598675347437431,
0.005984439210041702
]
},
"small": {
"seconds": 9051.37332302302,
"mean_abs_c1_change": 0.0008801267467942443,
"p95_abs_c1_change": 0.0025000000000000022,
"max_abs_c1_change": 0.09299999999999997,
"c1_bound_seconds": [
0.0,
0.0
],
"mean_abs_integral": [
0.005708567029833155,
0.005708567029833155
]
},
"bend": {
"seconds": 1631.677856559975,
"mean_abs_c1_change": 0.0024991973101179256,
"p95_abs_c1_change": 0.008999999999999897,
"max_abs_c1_change": 0.08899999999999997,
"c1_bound_seconds": [
0.0,
0.0
],
"mean_abs_integral": [
0.006396022651557497,
0.006396128425459091
]
},
"turn": {
"seconds": 445.74710667598833,
"mean_abs_c1_change": 0.008289169314090508,
"p95_abs_c1_change": 0.031000000000000028,
"max_abs_c1_change": 0.08000000000000002,
"c1_bound_seconds": [
21.296853938993763,
22.66821589999995
],
"mean_abs_integral": [
0.01013747903479613,
0.010079312488152544
]
},
"releasing": {
"seconds": 106.96494071107061,
"mean_abs_c1_change": 0.005058356779798081,
"p95_abs_c1_change": 0.025500000000000023,
"max_abs_c1_change": 0.08899999999999997,
"c1_bound_seconds": [
0.009879735000026812,
0.0
],
"mean_abs_integral": [
0.006354435594257913,
0.006350676023352573
]
}
},
"baseline_commit": "b720e9f1bb805c69e5560f13dd86034d4bf50260",
"gains": [
0.5,
0.75
],
"integral_gain": 0.25,
"c0_time_based": false,
"hypothesis": "model-action-curvature-c0-distance-pi-v13",
"tests": {
"passed": 455,
"subtests_passed": 25,
"ruff": "passed",
"diff_check": "passed"
},
"replay_command_invariants_passed": true,
"physical_tracking_or_stability_validated": false,
"route_results": [
{
"route": "112",
"cycles": 108971,
"can_round_trips": 217942,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.0679107408662434,
"p95": 0.26000000476837126,
"max": 3.119999904632568
},
"c1": {
"mean": 0.005686226432883118,
"p95": 0.014999999850988377,
"max": 0.18949999547004703
}
},
"clean_seconds": 732.0411244650002,
"c1_bound_seconds": [
0.0,
0.021201738000058867
],
"source_sha256": {
".cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e",
".cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a",
".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "113",
"cycles": 49614,
"can_round_trips": 99228,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.1281940820409913,
"p95": 0.897999979019158,
"max": 3.260000095367432
},
"c1": {
"mean": 0.006052000019791209,
"p95": 0.033500000965595265,
"max": 0.13200000381469723
}
},
"clean_seconds": 212.64164745499875,
"c1_bound_seconds": [
1.2653679959998954,
1.2653679959998954
],
"source_sha256": {
".cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327",
".cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f",
".cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "114",
"cycles": 61027,
"can_round_trips": 122054,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.11075714097529091,
"p95": 0.3999999904632565,
"max": 3.500000133514404
},
"c1": {
"mean": 0.006464089004288734,
"p95": 0.01899999927878381,
"max": 0.17200000238418578
}
},
"clean_seconds": 336.37317285400025,
"c1_bound_seconds": [
0.26486993000000325,
0.26486993000000325
],
"source_sha256": {
".cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e",
".cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07",
".cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "115",
"cycles": 40037,
"can_round_trips": 80074,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.08961574629129629,
"p95": 0.3500000071525573,
"max": 2.7699999141693112
},
"c1": {
"mean": 0.006259665157786696,
"p95": 0.0225000038146973,
"max": 0.10550000000000004
}
},
"clean_seconds": 213.9659020539998,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23",
".cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0",
".cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "116",
"cycles": 68793,
"can_round_trips": 137586,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.1229211861673407,
"p95": 0.40999998092651335,
"max": 3.539999895095825
},
"c1": {
"mean": 0.00492455679344663,
"p95": 0.01600000357627862,
"max": 0.12550000327825545
}
},
"clean_seconds": 286.72210779700015,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394",
".cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d",
".cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "117",
"cycles": 27301,
"can_round_trips": 54602,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.1466742382755,
"p95": 0.6379999847412033,
"max": 4.270000057220459
},
"c1": {
"mean": 0.00582792986837905,
"p95": 0.024499997889995594,
"max": 0.05100000405311583
}
},
"clean_seconds": 97.48647941900026,
"c1_bound_seconds": [
0.9928339700004472,
1.080442829000276
],
"source_sha256": {
".cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e",
".cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7",
".cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "119",
"cycles": 171423,
"can_round_trips": 342846,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.06276063585086782,
"p95": 0.22999999165535012,
"max": 4.129999866485596
},
"c1": {
"mean": 0.00042448693421451755,
"p95": 0.0024999996423721305,
"max": 0.035999999776482605
}
},
"clean_seconds": 616.4493080259994,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_route119/route.npz": "415a099935fef152123b7422870442d6cfe6302bab9b8983ce3b3ffc71a7702b",
".cache/ford_route119/model_paths.npz": "dfd41383bea486ccd0a476e94e612921a44a099a0c1213ff132ab81df3aa94d7",
".cache/ford_route119/metadata.json": "d61bd3351912f2f1a3c51f6061beffe89b430ebc50178c35abff8d1ecc1fc04d",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "11a",
"cycles": 235771,
"can_round_trips": 471542,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.060649023224496834,
"p95": 0.17000000953674288,
"max": 3.7
},
"c1": {
"mean": 0.00018421321567653392,
"p95": 0.0005000000949949013,
"max": 0.045000003814697265
}
},
"clean_seconds": 1085.65972405699,
"c1_bound_seconds": [
0.0811529149996204,
0.0811529149996204
],
"source_sha256": {
".cache/ford_route11a/route.npz": "428c203d090731386f06bf9fdeefe608f1999c43c52ff39cab7ee86ec0ac804f",
".cache/ford_route11a/model_paths.npz": "e2b827fd7c3dfbf66d7872a56b57eaec13c600a9a48e12dae59be81c31b0978f",
".cache/ford_route11a/metadata.json": "b7b10ccb1c0fa740779a1cf977faa4ae6714c9f9d7a0ffc39f3684680c16ae2a",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "120",
"cycles": 56704,
"can_round_trips": 113408,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.07882976760905562,
"p95": 0.3099999999999996,
"max": 3.3499999046325684
},
"c1": {
"mean": 6.576444779324904e-10,
"p95": 3.218650845848714e-09,
"max": 1.478195188475695e-08
}
},
"clean_seconds": 325.62925950599976,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_route120/route.npz": "ca40b52257af313ed0abd1109a33115a0a3227bd3438367faf282467e807a34a",
".cache/ford_route120/model_paths.npz": "9b4a21357de4076c555e8776ed36b1d5728da1307d739268a3f84e84593e12b4",
".cache/ford_route120/metadata.json": "110fe2016561722743064f2619e32f1b69164cc86015ef01c0a9fd2dd7f24bba",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "124",
"cycles": 5820,
"can_round_trips": 11640,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 2.909556870453723e-10,
"p95": 6.70552502413102e-10,
"max": 4.76837147544984e-09
},
"c1": {
"mean": 2.2964119221745972e-10,
"p95": 8.344650526126429e-10,
"max": 3.3974647539736225e-09
}
},
"clean_seconds": 22.732139372000063,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_c0_response_124_125/route124/route.npz": "26f8cf284e54c5f80bc7414c1302c429d4c10d54147e137fc5b8a348249e4fe5",
".cache/ford_c0_response_124_125/route124/model_paths.npz": "898d75cc80f29fc50a818c29614961ca8fdf56d1e8908993662c6e92e54ebf6f",
".cache/ford_c0_response_124_125/route124/metadata.json": "8f93b186178c89a95e56177dda7f0aba740bc57dfb832cde6d5c3fd3f43a8126",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "125",
"cycles": 41717,
"can_round_trips": 83434,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 3.143796501787139e-10,
"p95": 1.3411041166477844e-09,
"max": 4.76837147544984e-09
},
"c1": {
"mean": 0.001590841778388488,
"p95": 0.004000000216066835,
"max": 0.0040000033974647575
}
},
"clean_seconds": 400.6387703520005,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_c0_response_124_125/route125/route.npz": "8dcf304871bf5c6bf1ce206c788d7034bf5f056522faded0aa7948ff1cccbb75",
".cache/ford_c0_response_124_125/route125/model_paths.npz": "4be9c7809dabbcb52296ba93b4743146dc941e49a32aca07f008b75757703e49",
".cache/ford_c0_response_124_125/route125/metadata.json": "1f2b812d3c2cc8e5a2246098756940d6f8989a2591daf4d74b1ef4c8555e2d57",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "146",
"cycles": 53229,
"can_round_trips": 106458,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 3.47655752720815e-10,
"p95": 1.3411041166477844e-09,
"max": 2.8610229740877458e-08
},
"c1": {
"mean": 2.782228297198912e-10,
"p95": 1.19209286886246e-09,
"max": 1.3351440397713077e-08
}
},
"clean_seconds": 388.4780471659999,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_model_replay_146/full/route.npz": "b01db5bdd1c3b9fb83418a298571c98e60b5eb0509800d86cdaf07bafcdc6d63",
".cache/ford_model_replay_146/full/model_paths.npz": "11e0e9c8ce2959aac72d2f8a9cc4f6a025480034d5cd8b59b68a41b2ae3cf4a8",
".cache/ford_model_replay_146/full/metadata.json": "2be1254dd1eee422f3b2c2011faa91a21ab431751998525105381c2886df15ca",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "149",
"cycles": 132334,
"can_round_trips": 264668,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 1.604709350643841e-09,
"p95": 7.152557657263969e-09,
"max": 5.7220459037665705e-08
},
"c1": {
"mean": 6.774543859790247e-10,
"p95": 3.0994415922691587e-09,
"max": 1.478195188475695e-08
}
},
"clean_seconds": 868.9707798709996,
"c1_bound_seconds": [
0.2315784860001031,
0.6131428530001415
],
"source_sha256": {
".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "a0",
"cycles": 114582,
"can_round_trips": 229164,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.02570244338167474,
"p95": 0.08000000059604684,
"max": 1.4500001144409183
},
"c1": {
"mean": 0.01759150615156611,
"p95": 0.0444999985605478,
"max": 0.1095000026226044
}
},
"clean_seconds": 926.827608547,
"c1_bound_seconds": [
7.083908395000037,
7.184738287000073
],
"source_sha256": {
".cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
".cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b",
".cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "a2",
"cycles": 71111,
"can_round_trips": 142222,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.04191847719972587,
"p95": 0.11000001072883592,
"max": 2.5599999904632567
},
"c1": {
"mean": 0.018344418527189366,
"p95": 0.06350000252127648,
"max": 0.10950000333786014
}
},
"clean_seconds": 362.6477764930001,
"c1_bound_seconds": [
4.459983808000175,
4.459983808000175
],
"source_sha256": {
".cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
".cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717",
".cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "a5",
"cycles": 38961,
"can_round_trips": 77922,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.0678814615486646,
"p95": 0.23999999046325726,
"max": 2.549999809265137
},
"c1": {
"mean": 0.010050927104249109,
"p95": 0.02549999953061341,
"max": 0.11449999988079068
}
},
"clean_seconds": 281.78537111799994,
"c1_bound_seconds": [
3.117821645999925,
3.1221818889999327
],
"source_sha256": {
".cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
".cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4",
".cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "a9",
"cycles": 286319,
"can_round_trips": 572638,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.0574873679806263,
"p95": 0.24000002861022907,
"max": 1.8600000095367433
},
"c1": {
"mean": 0.012787646152435335,
"p95": 0.04100000113248825,
"max": 0.20099999380111694
}
},
"clean_seconds": 1231.1594245750002,
"c1_bound_seconds": [
0.945385510000051,
1.1985475469998619
],
"source_sha256": {
".cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975",
".cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240",
".cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "b8",
"cycles": 160431,
"can_round_trips": 320862,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.09075655656311847,
"p95": 0.3199999809265135,
"max": 3.8600001049041746
},
"c1": {
"mean": 0.014587774875948546,
"p95": 0.047500000029802314,
"max": 0.2005000000223518
}
},
"clean_seconds": 465.3759713629988,
"c1_bound_seconds": [
1.233373995994043,
1.536421163000341
],
"source_sha256": {
".cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b",
".cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615",
".cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "b9",
"cycles": 90774,
"can_round_trips": 181548,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.0779309579021297,
"p95": 0.3100000107288361,
"max": 4.5400001907348635
},
"c1": {
"mean": 0.007145982184135228,
"p95": 0.020500003129243882,
"max": 0.14649999999999996
}
},
"clean_seconds": 680.5921192759997,
"c1_bound_seconds": [
0.7494592899999759,
0.9690469479999706
],
"source_sha256": {
".cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c",
".cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861",
".cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "ca",
"cycles": 327448,
"can_round_trips": 654896,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.015992715429034462,
"p95": 0.05000000029802365,
"max": 1.0699999809265135
},
"c1": {
"mean": 0.0030197859214003767,
"p95": 0.00799999958276748,
"max": 0.10800000715255731
}
},
"clean_seconds": 999.6971771089911,
"c1_bound_seconds": [
0.871117996999601,
0.871117996999601
],
"source_sha256": {
".cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9",
".cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96",
".cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
},
{
"route": "raptor02",
"cycles": 132881,
"can_round_trips": 265762,
"baseline_difference_from_recorded_path": {
"c0": {
"mean": 0.01159572992270899,
"p95": 0.06918421261012586,
"max": 0.39572428941726656
},
"c1": {
"mean": 0.017327589689457763,
"p95": 0.04501086750242392,
"max": 0.10240508272871374
}
},
"clean_seconds": 592.9243753840037,
"c1_bound_seconds": [
0.0,
0.0
],
"source_sha256": {
".cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980",
".cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5",
".cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a",
"tools/ford_pscm_lab/proportional_replay.py": "0296fedbc134c3e11022c51e0b94cc16e1135b620fd278bf381ce040e953a7ed",
"openpilot/selfdrive/controls/lib/ford_model_action.py": "0c07b85db9c5cb0f42433116aea5d270fb244d8840119b4d7e8a526ec5b090d1"
}
}
],
"selected_runtime_source_sha256": "a653b9c16ff8e5ac91e7c1751eaeb7a77b50701d185ce426fe1074d1a58a07b4",
"opendbc_commit": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a"
}
docs/ford_model_action_drive_test.md
+10 -9
View File
@@ -1,8 +1,8 @@
# Ford selected-action drive-test branch
This v12 controller restores [curvature-derived C0](ford_curvature_c0_v8.md) and retains direct C0/C1 requests
and [continuous C1 PI feedback](ford_c1_minimal_pi.md)
with **P=0.50 and I=0.25**.
This v13 trial increases C1's proportional gain from 0.50 to **0.75**, retaining
I=0.25, [curvature-derived C0](ford_curvature_c0_v8.md), direct C0/C1 requests,
and [continuous C1 PI feedback](ford_c1_minimal_pi.md).
Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 defaults to a 7 m circular arc from selected desired curvature. An on-device toggle can instead use max(7 m, speed × 1 second).
[Base C1 overflow allocation to C0](ford_c1_overflow.md) remains.
It is selectable on **any Ford CAN FD vehicle**
@@ -10,10 +10,11 @@ through the existing persistent, default-off Sunnylink
toggle. Offline checks establish software behavior; physical tracking,
turn-exit behavior and closed-loop stability remain unvalidated.
V12 retains the v11/v9 command law by default after the model-path C0 trial in `5db3e3c9a`.
Both base commands use selected, upstream-limited desired curvature. The gains remain
P=0.50 and I=0.25, and PSCM `LimitReached` handling is unchanged. The separate
offline experiment that ignores the reached-limit integration block is not included.
Both base commands use selected, upstream-limited desired curvature. The +0.40 s
low-speed model preview from `b720e9f1b` remains: full offset at 15 mph and below,
tapering to zero at 30 mph. The trial changes only the immediate error correction;
PSCM `LimitReached` handling, integral gain, field bounds, and selection are retained.
See [P=0.75 replay results](ford_c1_p75_trial.md) for scope, tradeoffs, and reproduction.
## Select and restore
@@ -27,9 +28,9 @@ offline experiment that ignores the reached-limit integration block is not inclu
The startup event `Ford path controller selected` should report
`FordModelActionController`. Periodic `Ford C2-free path tracking` events
identify **`hypothesis=model-action-curvature-c0-distance-pi-v12`**. They report desired and measured
identify **`hypothesis=model-action-curvature-c0-distance-pi-v13`**. They report desired and measured
curvature, base heading, proportional and accumulated correction, applied heading,
feedback timing and driver/PSCM gating. `proportional_gain=0.5` and
feedback timing and driver/PSCM gating. `proportional_gain=0.75` and
`integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0
target in meters before C0 amplitude limits. `calibration_approved=false`
remains. The retired request/unwind/reversal diagnostic fields are removed.
openpilot/selfdrive/controls/lib/ford_model_action.py
+2 -2
View File
@@ -17,7 +17,7 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
OFFSET_STATION_M = 7.0
HEADING_TIME_S = 1.0
C1_PROPORTIONAL_GAIN = 0.50 # Drive-trial gains, not a learned calibration.
C1_PROPORTIONAL_GAIN = 0.75 # Drive-trial gains, not a learned calibration.
C1_INTEGRAL_GAIN = 0.25
CALIBRATION_APPROVED = False
@@ -131,7 +131,7 @@ class FordModelActionController:
"""
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False):
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based)
self.hypothesis = 'model-action-curvature-c0-distance-pi-v12'
self.hypothesis = 'model-action-curvature-c0-distance-pi-v13'
self.reset()
def set_c0_time_based(self, enabled, *, lateral_engaged):
openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py
+1 -1
View File
@@ -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-distance-pi-v12')
self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v13')
self.assertIs(record['calibration_approved'], False)
self.assertEqual(record['command'][2:], [0., 0.])
self.assertEqual(record['status'], controller.diagnostics['status'])
openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py
+5 -5
View File
@@ -187,9 +187,9 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
exec(call, environment)
expected_curvature = (-1 if maneuver else 1)*.000125
assert controls.desired_curvature == pytest.approx(expected_curvature)
assert controller.core.proportional == pytest.approx(.5*20.*expected_curvature)
assert controller.core.proportional == pytest.approx(.75*20.*expected_curvature)
assert controller.core.correction == 0. # First measurement has no elapsed feedback time.
assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0035)
assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0045)
assert controls.ford_path.path_offset == pytest.approx(0.) # Limited curvature arc is below one C0 step.
assert cc.latActive and cc.actuators.curvature == 0.
assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
@@ -275,7 +275,7 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
frame += 1
core = controls.ford_path_controller.core
expected_p = .5*20.*(sign*.004-measured) if torque == 0. else 0.
expected_p = .75*20.*(sign*.004-measured) if torque == 0. else 0.
assert core.proportional == pytest.approx(expected_p)
assert core.correction == pytest.approx(expected)
assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
@@ -303,7 +303,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s
'time': SimpleNamespace(monotonic=lambda now=now: now)})
controller = controls.ford_path_controller
assert controller.diagnostics['pscm_limited'] is service_valid
assert controller.core.proportional == pytest.approx(.01)
assert controller.core.proportional == pytest.approx(.015)
# All three fresh samples may integrate unless the valid PSCM limit blocks it.
assert controller.core.correction == pytest.approx(0. if service_valid else .00015)
assert cc.latActive and controls.ford_path.valid
@@ -355,7 +355,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-distance-pi-v12'
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
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.
openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py
+4 -3
View File
@@ -8,7 +8,7 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
@pytest.mark.parametrize('gain', [.1, .25, .5])
@pytest.mark.parametrize('gain', [.1, .25, .5, .75])
@pytest.mark.parametrize('sign', [-1., 1.])
def test_p_responds_without_waiting_for_integral_and_disappears_at_catchup(gain, sign):
controller = ModelActionController(proportional_gain=gain)
@@ -42,8 +42,9 @@ def test_aligned_feedback_does_not_replace_current_feedforward_or_path(sign):
@pytest.mark.parametrize('sign', [-1., 1.])
def test_pi_combined_request_obeys_amplitude_and_does_not_wind_up_behind_p(sign):
controller = ModelActionController(proportional_gain=.5)
@pytest.mark.parametrize('gain', [.5, .75])
def test_pi_combined_request_obeys_amplitude_and_does_not_wind_up_behind_p(sign, gain):
controller = ModelActionController(proportional_gain=gain)
for _ in range(200):
out = controller.update(straight(), sign*.01, current_curvature=-sign*.1, speed=20., dt=.01)
assert controller.c1 == pytest.approx(sign*.5)
openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py
+2 -2
View File
@@ -46,9 +46,9 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
if candidate:
assert type(selected.ford_path_controller) is FordModelActionController
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .50
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v12'
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v13'
else:
assert selected.ford_path_controller is None
assert selected.ford_model_action == candidate
openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py
+5 -4
View File
@@ -68,11 +68,12 @@ def test_centering_cannot_gate_continuous_heading_correction(sign):
@pytest.mark.parametrize('sign', [-1., 1.])
@pytest.mark.parametrize('limited', [False, True])
def test_duplicate_measurements_cannot_retire_integral(sign, limited):
core = ModelActionController()
@pytest.mark.parametrize('gain', [.5, .75])
def test_duplicate_measurements_cannot_retire_integral(sign, limited, gain):
core = ModelActionController(gain, .25)
core.c1, core.correction = sign*.07, sign*.03
core.update(straight(), -sign*.002, current_curvature=sign*.004, speed=20., dt=.01,
feedback_dt=0., pscm_limited=limited)
assert core.correction == sign*.03
assert core.proportional == pytest.approx(-sign*.06)
assert core.c1 == pytest.approx(-sign*.07)
assert core.proportional == pytest.approx(-sign*.12*gain)
assert core.c1 == pytest.approx(-sign*(.01+.12*gain))
tools/ford_pscm_lab/proportional_replay.py
+148
View File
@@ -0,0 +1,148 @@
"""Compare P gains using production adapters and fixed recorded motion.
This checks command behavior, not counterfactual tracking or stability. Original
selected curvature is retained, including each route's original model/delay.
Service publication times proxy control execution time; full SubMaster state is
not logged. In particular, this is not exact process replay of historical cars.
"""
import argparse
from collections import Counter
from concurrent.futures import ProcessPoolExecutor, as_completed
import hashlib
import json
from pathlib import Path
from types import SimpleNamespace
import numpy as np
from openpilot.selfdrive.controls.lib import ford_model_action
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
GAINS = (.50, .75)
DIAGNOSTICS = ('heading_feedforward', 'heading_proportional', 'heading_correction',
'feedback_enabled', 'pscm_limited', 'driver_override', 'offset_overflow')
def describe(values, mask):
values = values[mask]
return {'mean': float(np.mean(values)), 'p95': float(np.quantile(values, .95)),
'max': float(np.max(values))} if len(values) else None
def replay(route, output):
label, source = route.split('=', 1)
directory = Path(source).resolve()
destination = output.resolve()/label
if destination == directory or directory in destination.parents:
raise ValueError('Output must preserve source extracts')
raw = np.load(directory/'route.npz', allow_pickle=False)
r = {key: table(raw, key) for key in ('controls', 'cs', 'cc', 'model', 'params', 'pscm', 'path')}
metadata = json.loads((directory/'metadata.json').read_text())
if ('maneuver' in raw and len(raw['maneuver'])) or any(
f['counts'].get(service, 0) for f in metadata['files'] for service in ('lateralManeuverPlan', 'testJoystick')
):
raise ValueError('This replay requires the recorded model-selected reference, without maneuver or joystick injection')
geometry = np.load(directory/'model_paths.npz', allow_pickle=False)
np.testing.assert_array_equal(geometry['ns'], r['model']['ns'])
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2]))
for p in geometry['paths']]
c, t = r['controls'], r['controls']['t']
cs, pa, ps = (sample(r[key], t) for key in ('cs', 'params', 'pscm'))
# carControl is the same-cycle publication, never a future motion sample.
cc = sample(r['cc'], t, nearest=True)
mi = np.clip(np.searchsorted(r['model']['ns'], c['model_ns']), 0, len(models)-1)
exact = r['model']['ns'][mi] == c['model_ns']
health = (c['valid'].astype(bool) & cc['valid'].astype(bool) & cs['valid'].astype(bool)
& cs['can_valid'].astype(bool) & pa['valid'].astype(bool) & exact
& r['model']['valid'][mi].astype(bool) & (abs(cc['t']-t) < .005)
& (t-pa['t'] >= 0.) & (t-pa['t'] <= .15))
controllers = [FordModelActionController(proportional_gain=kp, integral_gain=.25, c0_time_based=False) for kp in GAINS]
commands = np.zeros((2, len(t), 4))
valid = np.zeros((2, len(t)), bool)
diagnostics = np.zeros((2, len(t), len(DIAGNOSTICS)))
reasons = [Counter(), Counter()]
wire = WireCheck()
for i, now in enumerate(t):
status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
kwargs = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now,
'measurement_time': cs['t'][i], 'model_time': r['model']['t'][mi[i]],
'reference_time': r['model']['t'][mi[i]], 'active': bool(cc['active'][i]), 'valid': bool(health[i]),
'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]),
'driver_torque': cs['torque'][i], 'pscm_status': status}
for k, controller in enumerate(controllers):
command = controller.update(models[mi[i]] if exact[i] else None, c['desired'][i], **kwargs)
commands[k, i] = command.path_offset, command.path_angle, command.curvature, command.curvature_rate
valid[k, i] = command.valid
diagnostics[k, i] = [controller.diagnostics.get(name, 0.) for name in DIAGNOSTICS]
reasons[k][controller.diagnostics['status']] += 1
wire.check(command)
assert np.isfinite(commands).all() and np.isfinite(diagnostics).all()
assert (abs(commands[:, :, :2]) <= [5.1100001, .5000001]).all()
assert (commands[:, :, 2:] == 0.).all() and (commands[~valid] == 0.).all()
np.testing.assert_array_equal(valid[0], valid[1])
np.testing.assert_array_equal(commands[0, :, 0], commands[1, :, 0])
np.testing.assert_array_equal(diagnostics[0, :, [0, 3, 4, 5, 6]], diagnostics[1, :, [0, 3, 4, 5, 6]])
np.testing.assert_allclose(diagnostics[1, :, 1], 1.5*diagnostics[0, :, 1], rtol=1e-12, atol=1e-12)
assert reasons[0] == reasons[1]
driver = (cs['pressed'] > 0.) | (abs(cs['torque']) > 1.) | ((ps['status_valid'] > 0) & (ps['limit'] == 3))
bad = driver | ~valid[0] | (diagnostics[0, :, 3] == 0.)
last_bad = np.maximum.accumulate(np.where(bad, t, -1e6))
clean = ~bad & (t-last_bad >= 1.) & (cs['speed'] >= 3.*.44704)
weights = np.minimum(np.diff(t, append=t[-1]+.01), .03)
angle = abs(c['desired_angle'])
# These labels describe the recorded error, not the candidate's response.
behind = c['desired']*(c['desired']-c['measured']) > 0.
derivative = np.r_[0., np.diff(abs(c['desired']))/np.maximum(np.diff(t), .002)]
masks = {'all': clean, 'small_under_10deg': clean & (angle < 10.),
'bend_10_to_45deg': clean & (angle >= 10.) & (angle < 45.),
'turn_over_45deg': clean & (angle >= 45.),
'turn_entry_behind': clean & (angle >= 45.) & behind & (derivative > 0.),
'turn_releasing_excess_steering': clean & (angle >= 10.) & ~behind & (derivative < 0.)}
change = commands[1, :, 1]-commands[0, :, 1]
cohorts = {}
for name, mask in masks.items():
cohorts[name] = {'seconds': float(weights[mask].sum()), 'abs_c1_change': describe(abs(change), mask),
'settings': [{'kp': kp, 'c1_bound_seconds': float(weights[mask & (abs(commands[k, :, 1]) >= .4995)].sum()),
'abs_c1': describe(abs(commands[k, :, 1]), mask),
'abs_integral': describe(abs(diagnostics[k, :, 2]), mask)} for k, kp in enumerate(GAINS)]}
paired = clean[1:] & clean[:-1]
step = np.diff(commands[:, :, 1], axis=1)
step_metrics = [{'kp': kp, 'abs_c1_per_cycle_change': describe(abs(step[k]), paired)} for k, kp in enumerate(GAINS)]
rec = sample(r['path'], t, nearest=True)
rec_mask = clean & (rec['valid'] > 0) & (abs(rec['t']-t) < .005)
baseline_comparison = {name: describe(abs(commands[0, :, idx]-rec[name]), rec_mask) for idx, name in enumerate(('c0', 'c1'))}
sources = (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json',
Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())
report = {'scope': __doc__, 'route': label, 'cycles': len(t), 'gains': list(GAINS), 'ki': .25,
'can_round_trips': wire.count, 'validity_and_c0_and_gates_identical': True,
'status_counts': dict(reasons[0]), 'cohorts': cohorts, 'per_cycle_changes': step_metrics,
'baseline_difference_from_recorded_path': baseline_comparison,
'baseline_comparison_note': 'Only meaningful for matching historical mapping/gains; adapter clock/health reconstruction is approximate.',
'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}}
destination.mkdir(parents=True, exist_ok=True)
(destination/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
np.savez_compressed(destination/'commands.npz', t=t-metadata['t0'], commands=commands, valid=valid,
diagnostics=diagnostics, diagnostic_names=DIAGNOSTICS, clean=clean,
driver=driver, speed=cs['speed'], desired=c['desired'], measured=c['measured'])
return report
if __name__ == '__main__':
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--routes', nargs='+', required=True, help='label=extract-directory pairs')
parser.add_argument('--output', type=Path, required=True)
parser.add_argument('--workers', type=int, default=4)
args = parser.parse_args()
labels = [route.split('=', 1)[0] for route in args.routes]
if len(set(labels)) != len(labels) or any(Path(label).name != label or label in ('.', '..') for label in labels):
parser.error('Route labels must be unique directory names')
with ProcessPoolExecutor(max_workers=args.workers) as pool:
jobs = {pool.submit(replay, route, args.output): route for route in args.routes}
for job in as_completed(jobs):
result = job.result()
print(json.dumps({'route': result['route'], 'cycles': result['cycles'], 'can_round_trips': result['can_round_trips'],
'clean_seconds': result['cohorts']['all']['seconds']}), flush=True)