diff --git a/docs/ford_c0_feedback_v15.md b/docs/ford_c0_feedback_v15.md new file mode 100644 index 0000000000..c39e09e407 --- /dev/null +++ b/docs/ford_c0_feedback_v15.md @@ -0,0 +1,105 @@ +# Ford C0 proportional feedback trial + +The existing controller sends its tracking correction through C1 only. Offline +identification on the Lightning suggests that stronger C1 requests can stop +producing faster wheel movement while additional C0 may still help. This trial +adds proportional correction to C0. It does not change the selected curvature, +C0 base geometry, C1 PI calculation, transmission rate or existing opt-in selection. + +## Command + +With host-sign road curvature error `e = reference - measured`: + +``` +scale = VM.get_steer_from_curvature(1, speed, 0) / (CP.steerRatio * CP.wheelbase) +C0_response = 0.010717679293424373 + 0.018122981795212647 / max(speed, 1.34)^2 +C0_P = 0.5 * e * scale / C0_response +C0 = clip(existing_bounded_C0_base + C0_P, -5.11, 5.11) +``` + +The response coefficients are an offline fit to isolated C0 commands on route +`84865544361f55cb/00000145--5d9f02fee7`. They express **geometric steering curvature +per metre of C0**, not road curvature or an instantaneous wheel response. The +existing vehicle model converts the error into those units; its road-roll and +steering-angle offsets cancel in the error. No plant identification, observer or +dynamic plant simulation runs on the device. + +`0.5` is an explicit trial feedback gain, not a fitted optimum. Before field +clipping, the extra C0 has a fitted steady effect equal to half the current +wheel-angle error. This does not mean 50% of the C0 field. The 1.34 m/s term +holds the fitted conversion below the identification dataset's minimum speed; +it does not disable steering or add a maneuver state. + +C0 correction is recomputed every update, including updates that have no fresh +integration interval. It has no integrator, request-change state, deadband, or +additional slew limit. It becomes zero at zero error and changes sign on +overshoot. Existing driver override and PSCM arbitration disable it together +with other feedback. PSCM limit-reached continues to inhibit outward C1 +integration; it does not freeze either proportional command. C1 remains +P=0.75, I=1.0. C2/C3 remain zero. + +The existing `FordModelActionController` Sunnylink toggle still selects this +controller for CAN FD. With the toggle off, startup selects upstream Ford +control. No new UI or lateral-maneuver changes are included. + +## Offline validation + +The native-time replay compares the candidate against production commit +`d425b3260b0785b22096d130702b54a2e0761c36`, with recorded measurements, requests, +model health, driver input, service timestamps and PSCM arbitration held fixed. + +- Ten Lightning routes: a9, b9, 112, 113, 117, 11c, 125, 146, 149 and 151. +- 1,170,113 cycles; 117,016 in-memory Float32/CAN encode/decode checks. +- Identical C1 output, C1 integral and command validity on every compared cycle. +- All outputs finite, inside field bounds, with C2/C3 zero. +- C0 exactly matches the old command whenever its new correction is zero or + feedback is disabled. +- Across eligible samples with at least 30 degrees of wheel-angle error, the + median C0 change is 0.65 m and the 95th percentile is 1.47 m. +- Across eligible requests below 5 degrees, the median change is 0.01 m and + the 95th percentile is 0.05 m. This cohort includes turn exits with a still + turned wheel; its largest correction is consequently much larger. +- At route 151 time 306.89 s, the replay changes C0 from 0.75 to 1.68 m for + approximately 76 degrees of tracking error. Both replays send the same C1. + +The test suite covers immediate correction, no accumulation, reversal, catch-up, +invalid inputs, real vehicle-model units across stiffness/speed/roll changes, +driver/PSCM arbitration, upstream fallback, and actual controlsd publication +through the 100 Hz CAN sender. The machine-readable route replay summary is +`ford_c0_feedback_v15_validation.json`. + +``` +PYTHONPATH=.:opendbc_repo python -m pytest -q \ + openpilot/selfdrive/controls/tests/test_ford_model_action*.py \ + openpilot/selfdrive/controls/tests/test_ford_path.py + +PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/c0_feedback_validate.py \ + --output .cache/ford_c0_feedback_v15 --workers 4 +``` + +The replay requires the existing local rlog extracts. Input hashes are recorded +in its validation JSON. It does not assume the truck follows modified commands. + +## What is still experimental + +The earlier fitted plant overpredicted one second of wheel movement by about +16 degrees in the route 151 example. It also misses the phase of a low-speed +oscillation in route 125. Independent C0/C1 response contributions are an +approximation; a shared PSCM limit could prevent the extra movement predicted +from C0. + +An additional four-second fitted-plant simulation, with controller feedback +recomputed against simulated wheel angle, showed no regression for the tested +0.5 gain in its turn, unwind and near-straight cohorts. Route 149 had 27 eligible +turn windows; route 151 had no four-second turn windows surviving the strict +intervention/status mask. Future reference and speed were frozen, and the model +does not reproduce the route 125 failure faithfully. These results are a +sanity check, not validation of road tracking or an optimized gain. + +Actual acceptance is better desired-versus-actual wheel tracking on turn entry, +without added oscillation, overshoot or delayed unwind. The software behavior +is validated; the physical improvement remains to be measured. + +Diagnostics identify `model-action-curvature-c0-feedback-v15` and record +`offset_proportional`, `c0_proportional_gain`, and `curvature_scale` alongside +the existing heading/feedforward/integral signals. diff --git a/docs/ford_c0_feedback_v15_validation.json b/docs/ford_c0_feedback_v15_validation.json new file mode 100644 index 0000000000..4980c64645 --- /dev/null +++ b/docs/ford_c0_feedback_v15_validation.json @@ -0,0 +1,856 @@ +{ + "scope": "Replay C0 feedback against v14 using frozen native-time Lightning measurements.\n\nThis checks software/CAN behavior, not the physical response to new commands.\nUse cached route.npz, model_paths.npz and metadata.json from the rlog extractor.\n", + "routes": [ + { + "route": "112", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 108971, + "wire_round_trips": 10898, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 83590, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.28000000000000025, + 1.7300000000000004 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 43982, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.04999999999999982, + 1.4699999999999998 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 9939, + "delta_c0_m_quantiles": [ + 0.15999999999999925, + 1.1199999999999997, + 1.7300000000000004 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 3604, + "delta_c0_m_quantiles": [ + 0.73, + 1.4299999999999997, + 1.7300000000000004 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + ".cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "113", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 49614, + "wire_round_trips": 4962, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 24171, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.9400000000000004, + 2.2200000000000006 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 16815, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.05999999999999961, + 1.0499999999999998 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 2920, + "delta_c0_m_quantiles": [ + 0.6600000000000001, + 1.8704999999999974, + 2.2200000000000006 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 2831, + "delta_c0_m_quantiles": [ + 0.8699999999999997, + 1.9399999999999995, + 2.2200000000000006 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327", + ".cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f", + ".cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "117", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 27301, + "wire_round_trips": 2731, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 13659, + "delta_c0_m_quantiles": [ + 0.03000000000000025, + 0.5, + 1.5900000000000007 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 5001, + "delta_c0_m_quantiles": [ + 0.010000000000000675, + 0.3000000000000007, + 1.5899999999999999 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 2891, + "delta_c0_m_quantiles": [ + 0.1200000000000001, + 0.7400000000000002, + 1.38 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 1195, + "delta_c0_m_quantiles": [ + 0.54, + 1.5599999999999996, + 1.5900000000000007 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e", + ".cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114", + ".cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "11c", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 54146, + "wire_round_trips": 5415, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 36780, + "delta_c0_m_quantiles": [ + 0.020000000000000462, + 0.3700000000000001, + 2.1799999999999997 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 16750, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.09999999999999964, + 0.5 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 2461, + "delta_c0_m_quantiles": [ + 0.1899999999999995, + 0.8899999999999997, + 2.1799999999999997 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 1827, + "delta_c0_m_quantiles": [ + 0.5599999999999996, + 0.9569999999999982, + 2.1799999999999997 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_maneuver_11c/analysis/route.npz": "1f3c37a7c7ae85ba69e9958435a9568f8244fa862f43ad1ee2a0192d1df13966", + ".cache/ford_maneuver_11c/analysis/metadata.json": "2d0051d23059145988ca905e5386704def24b8080ce92541cec5e4ab501bcd0c", + ".cache/ford_maneuver_11c/analysis/model_paths.npz": "0ff433da3dde1d4d7f10596cd53558ec5de9d56fbc49aec3e8afb4c9872377d5", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "125", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 41717, + "wire_round_trips": 4172, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 41107, + "delta_c0_m_quantiles": [ + 0.0, + 0.05999999999999961, + 0.40000000000000036 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 33135, + "delta_c0_m_quantiles": [ + 0.0, + 0.010000000000000675, + 0.28000000000000025 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 192, + "delta_c0_m_quantiles": [ + 0.3200000000000003, + 0.3799999999999999, + 0.40000000000000036 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_c0_response_124_125/route125/route.npz": "8dcf304871bf5c6bf1ce206c788d7034bf5f056522faded0aa7948ff1cccbb75", + ".cache/ford_c0_response_124_125/route125/metadata.json": "1f2b812d3c2cc8e5a2246098756940d6f8989a2591daf4d74b1ef4c8555e2d57", + ".cache/ford_c0_response_124_125/route125/model_paths.npz": "4be9c7809dabbcb52296ba93b4743146dc941e49a32aca07f008b75757703e49", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "146", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 53229, + "wire_round_trips": 5323, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 40717, + "delta_c0_m_quantiles": [ + 0.0, + 0.04999999999999982, + 1.1300000000000003 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 30796, + "delta_c0_m_quantiles": [ + 0.0, + 0.02999999999999936, + 0.35000000000000053 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 585, + "delta_c0_m_quantiles": [ + 0.3700000000000001, + 1.0300000000000002, + 1.1300000000000003 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 298, + "delta_c0_m_quantiles": [ + 0.4500000000000002, + 1.0614999999999999, + 1.1300000000000003 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_model_replay_146/full/route.npz": "b01db5bdd1c3b9fb83418a298571c98e60b5eb0509800d86cdaf07bafcdc6d63", + ".cache/ford_model_replay_146/full/metadata.json": "2be1254dd1eee422f3b2c2011faa91a21ab431751998525105381c2886df15ca", + ".cache/ford_model_replay_146/full/model_paths.npz": "11e0e9c8ce2959aac72d2f8a9cc4f6a025480034d5cd8b59b68a41b2ae3cf4a8", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "149", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 132334, + "wire_round_trips": 13234, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 109224, + "delta_c0_m_quantiles": [ + 0.019999999999999574, + 0.43999999999999995, + 2.2800000000000002 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 54914, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.07000000000000028, + 2.04 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 15161, + "delta_c0_m_quantiles": [ + 0.20999999999999996, + 1.0300000000000002, + 2.25 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 6659, + "delta_c0_m_quantiles": [ + 0.6399999999999997, + 1.4100000000000001, + 2.2800000000000002 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade", + ".cache/ford_route149/full/metadata.json": "624fff03c25eb298661cb7b25f3dbe6d214d863f799d93635c0f4f05fc0d2b32", + ".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "151", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 325708, + "wire_round_trips": 32571, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 124806, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.10000000000000053, + 2.21 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "straight": { + "n": 85546, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.03999999999999915, + 0.6300000000000008 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 3715, + "delta_c0_m_quantiles": [ + 0.25, + 1.12, + 2.21 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "large_error": { + "n": 1839, + "delta_c0_m_quantiles": [ + 0.6200000000000001, + 1.35, + 2.21 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + } + }, + "sources": { + ".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1", + ".cache/ford_route151/full/metadata.json": "937825317a0edd470c54647240b922be8f79dda5b3365ffdd61281f0aca877a1", + ".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "a9", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 286319, + "wire_round_trips": 28632, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 142471, + "delta_c0_m_quantiles": [ + 0.019999999999999574, + 0.15999999999999925, + 1.5699999999999994 + ], + "c0_old_capped": 25, + "c0_new_capped": 128 + }, + "straight": { + "n": 99221, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.05999999999999961, + 1.4800000000000004 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 6893, + "delta_c0_m_quantiles": [ + 0.23000000000000043, + 0.9699999999999998, + 1.5600000000000005 + ], + "c0_old_capped": 25, + "c0_new_capped": 128 + }, + "large_error": { + "n": 3224, + "delta_c0_m_quantiles": [ + 0.6600000000000001, + 1.2599999999999998, + 1.5699999999999994 + ], + "c0_old_capped": 25, + "c0_new_capped": 128 + } + }, + "sources": { + ".cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975", + ".cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93", + ".cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + }, + { + "route": "b9", + "baseline": "d425b3260b0785b22096d130702b54a2e0761c36", + "baseline_sha256": "c55464ecfbe7fad9f51905cf5f226be2676440ac5afec470b05515e32b065cc8", + "cycles": 90774, + "wire_round_trips": 9078, + "c1_identical": true, + "c0_gain": 0.5, + "metrics": { + "all": { + "n": 81066, + "delta_c0_m_quantiles": [ + 0.010000000000000675, + 0.3100000000000005, + 2.12 + ], + "c0_old_capped": 0, + "c0_new_capped": 10 + }, + "straight": { + "n": 42647, + "delta_c0_m_quantiles": [ + 0.009999999999999787, + 0.07000000000000028, + 1.63 + ], + "c0_old_capped": 0, + "c0_new_capped": 0 + }, + "turn": { + "n": 9816, + "delta_c0_m_quantiles": [ + 0.1200000000000001, + 0.8300000000000001, + 2.12 + ], + "c0_old_capped": 0, + "c0_new_capped": 10 + }, + "large_error": { + "n": 3486, + "delta_c0_m_quantiles": [ + 0.6600000000000001, + 1.5100000000000005, + 2.12 + ], + "c0_old_capped": 0, + "c0_new_capped": 10 + } + }, + "sources": { + ".cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c", + ".cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee", + ".cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861", + ".cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/c0_feedback_validate.py": "12b843f024b671f8b4282e1981f6d68de879a9cdff20fee74fd5228b6e4168ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca" + } + } + ], + "test_summary": { + "pytest_passed": 439, + "ruff": "passed", + "git_diff_check": "passed" + }, + "fit_source_sha256": "536ea20ee1bfa9366453c6d01fae6c7dd7b83388f2a71662cc12b43311767c6f", + "fitted_plant_sanity_check": { + "scope": "Approximate fitted-plant simulation with frozen future reference/speed, recomputed controller feedback, 20 Hz, four-second clean windows. Not physical tracking validation.", + "routes": { + "149": { + "0.0": { + "all": { + "windows": 515, + "tracking_rmse_deg": 1.9599623978328702, + "p95_abs_error_deg": 3.6108132362595673 + }, + "turn": { + "windows": 27, + "tracking_rmse_deg": 5.648897344766248, + "p95_abs_error_deg": 10.154937282895988 + }, + "unwind": { + "windows": 16, + "tracking_rmse_deg": 4.56296777072208, + "p95_abs_error_deg": 5.8047028901317175 + }, + "near_straight": { + "windows": 280, + "tracking_rmse_deg": 0.5053540034793153, + "p95_abs_error_deg": 0.9099971698933058 + } + }, + "0.25": { + "all": { + "windows": 515, + "tracking_rmse_deg": 1.868833791306516, + "p95_abs_error_deg": 3.4194303642888486 + }, + "turn": { + "windows": 27, + "tracking_rmse_deg": 5.3063969129788715, + "p95_abs_error_deg": 9.669302705935252 + }, + "unwind": { + "windows": 16, + "tracking_rmse_deg": 4.307554036590833, + "p95_abs_error_deg": 5.549381163721539 + }, + "near_straight": { + "windows": 280, + "tracking_rmse_deg": 0.4868309463403626, + "p95_abs_error_deg": 0.8696197439962221 + } + }, + "0.5": { + "all": { + "windows": 515, + "tracking_rmse_deg": 1.7898071883511764, + "p95_abs_error_deg": 3.260221243224674 + }, + "turn": { + "windows": 27, + "tracking_rmse_deg": 5.006440079078192, + "p95_abs_error_deg": 9.002861391131468 + }, + "unwind": { + "windows": 16, + "tracking_rmse_deg": 4.078042889798365, + "p95_abs_error_deg": 5.24331028633435 + }, + "near_straight": { + "windows": 280, + "tracking_rmse_deg": 0.4694670576005347, + "p95_abs_error_deg": 0.8331203606609966 + } + }, + "0.75": { + "all": { + "windows": 515, + "tracking_rmse_deg": 1.7203138728888623, + "p95_abs_error_deg": 3.1149598999491044 + }, + "turn": { + "windows": 27, + "tracking_rmse_deg": 4.743912352245749, + "p95_abs_error_deg": 8.664577551257613 + }, + "unwind": { + "windows": 16, + "tracking_rmse_deg": 3.8750067743533827, + "p95_abs_error_deg": 4.9966209407120425 + }, + "near_straight": { + "windows": 280, + "tracking_rmse_deg": 0.45412470534078503, + "p95_abs_error_deg": 0.7964604020331516 + } + }, + "1.0": { + "all": { + "windows": 515, + "tracking_rmse_deg": 1.6590272653809652, + "p95_abs_error_deg": 2.990976062659628 + }, + "turn": { + "windows": 27, + "tracking_rmse_deg": 4.507245023760021, + "p95_abs_error_deg": 8.107112731535002 + }, + "unwind": { + "windows": 16, + "tracking_rmse_deg": 3.693644190831072, + "p95_abs_error_deg": 4.749308802498215 + }, + "near_straight": { + "windows": 280, + "tracking_rmse_deg": 0.4404620597734029, + "p95_abs_error_deg": 0.7694584025732562 + } + } + }, + "151": { + "0.0": { + "all": { + "windows": 794, + "tracking_rmse_deg": 0.9049924838460381, + "p95_abs_error_deg": 1.496850675938314 + }, + "unwind": { + "windows": 7, + "tracking_rmse_deg": 1.3568942395552432, + "p95_abs_error_deg": 2.4717442023707914 + }, + "near_straight": { + "windows": 525, + "tracking_rmse_deg": 0.47746381422226297, + "p95_abs_error_deg": 0.9045192063209317 + } + }, + "0.25": { + "all": { + "windows": 794, + "tracking_rmse_deg": 0.8744021810097441, + "p95_abs_error_deg": 1.4250449905919713 + }, + "unwind": { + "windows": 7, + "tracking_rmse_deg": 1.2734874215389322, + "p95_abs_error_deg": 2.3213325000763567 + }, + "near_straight": { + "windows": 525, + "tracking_rmse_deg": 0.45648429088650827, + "p95_abs_error_deg": 0.8575351438117865 + } + }, + "0.5": { + "all": { + "windows": 794, + "tracking_rmse_deg": 0.8487165771252436, + "p95_abs_error_deg": 1.3633464420801624 + }, + "unwind": { + "windows": 7, + "tracking_rmse_deg": 1.1986282107421773, + "p95_abs_error_deg": 2.163514655425466 + }, + "near_straight": { + "windows": 525, + "tracking_rmse_deg": 0.4395196212749708, + "p95_abs_error_deg": 0.82723861668119 + } + }, + "0.75": { + "all": { + "windows": 794, + "tracking_rmse_deg": 0.8256116631456035, + "p95_abs_error_deg": 1.309241916169811 + }, + "unwind": { + "windows": 7, + "tracking_rmse_deg": 1.1341202040172427, + "p95_abs_error_deg": 2.0707107084230465 + }, + "near_straight": { + "windows": 525, + "tracking_rmse_deg": 0.42406706004679057, + "p95_abs_error_deg": 0.7946947170401512 + } + }, + "1.0": { + "all": { + "windows": 794, + "tracking_rmse_deg": 0.8052189413919172, + "p95_abs_error_deg": 1.2575495536727697 + }, + "unwind": { + "windows": 7, + "tracking_rmse_deg": 1.0739985410343649, + "p95_abs_error_deg": 1.9599553503928666 + }, + "near_straight": { + "windows": 525, + "tracking_rmse_deg": 0.41043500769646296, + "p95_abs_error_deg": 0.7683540262306987 + } + } + }, + "125": { + "0.0": { + "all": { + "windows": 346, + "tracking_rmse_deg": 1.5101438591368561, + "p95_abs_error_deg": 1.3433430124202965 + }, + "near_straight": { + "windows": 246, + "tracking_rmse_deg": 0.3997814518183382, + "p95_abs_error_deg": 0.8420670524669589 + } + }, + "0.25": { + "all": { + "windows": 346, + "tracking_rmse_deg": 1.460719401448126, + "p95_abs_error_deg": 1.2614591679946765 + }, + "near_straight": { + "windows": 246, + "tracking_rmse_deg": 0.377498378600397, + "p95_abs_error_deg": 0.7868969957812859 + } + }, + "0.5": { + "all": { + "windows": 346, + "tracking_rmse_deg": 1.4162750202728391, + "p95_abs_error_deg": 1.188472341120946 + }, + "near_straight": { + "windows": 246, + "tracking_rmse_deg": 0.35896062148345687, + "p95_abs_error_deg": 0.7523514432810818 + } + }, + "0.75": { + "all": { + "windows": 346, + "tracking_rmse_deg": 1.3766402449201502, + "p95_abs_error_deg": 1.1248966870109076 + }, + "near_straight": { + "windows": 246, + "tracking_rmse_deg": 0.34130621657460447, + "p95_abs_error_deg": 0.7141102879383627 + } + }, + "1.0": { + "all": { + "windows": 346, + "tracking_rmse_deg": 1.3414563262275077, + "p95_abs_error_deg": 1.0712299506410858 + }, + "near_straight": { + "windows": 246, + "tracking_rmse_deg": 0.3255065971687205, + "p95_abs_error_deg": 0.6807731116817476 + } + } + } + } + } +} diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 62316f943f..00622c4a2f 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -171,6 +171,8 @@ class Controls(ControlsExt): reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2' self.ford_path = self.ford_path_controller.update( ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(), + # Roll/angle offset cancel in the error; retain the normal steering-angle conversion's speed and stiffness effects. + curvature_scale=self.VM.get_steer_from_curvature(1., CS.vEgo, 0.) / (self.CP.steerRatio*self.CP.wheelbase), measurement_time=self.sm.logMonoTime['carState'] * 1e-9, model_time=self.sm.logMonoTime['modelV2'] * 1e-9, reference_time=self.sm.logMonoTime[reference_service] * 1e-9, diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index abf0221ceb..a62365456a 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,7 +1,7 @@ """Opt-in Ford C2-free model mapping with measured-curvature PI feedback. C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s), -including base-heading overflow. C1 +including base-heading overflow, plus proportional tracking correction. C1 combines the selected curvature's heading with proportional and integrated tracking error. Reference distance and gains are explicit trial choices. Commands use the current bounded request without an additional C0/C1 slew. @@ -19,6 +19,10 @@ OFFSET_STATION_M = 7.0 HEADING_TIME_S = 1.0 C1_PROPORTIONAL_GAIN = 0.75 # Drive-trial gains, not a learned calibration. C1_INTEGRAL_GAIN = 1.0 +C0_PROPORTIONAL_GAIN = 0.5 # Trial fraction of the measured wheel-angle error. +# Offline Lightning fit: geometric curvature per metre of C0, with v in m/s. +C0_RESPONSE_CONSTANT = 0.010717679293424373 +C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647 CALIBRATION_APPROVED = False @@ -63,26 +67,33 @@ class ModelActionController: Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. """ - __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based') + __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based', + 'c0_proportional_gain', 'offset_proportional') - def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False): - if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.: + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False, + c0_proportional_gain=C0_PROPORTIONAL_GAIN): + if not _finite(proportional_gain, integral_gain, c0_proportional_gain) or min(proportional_gain, integral_gain, c0_proportional_gain) < 0.: raise ValueError('PI gains must be finite and nonnegative') self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain) + self.c0_proportional_gain = float(c0_proportional_gain) self.c0_time_based = bool(c0_time_based) self.reset() def reset(self): self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0. + self.offset_proportional = 0. def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True, - feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None): + feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None, curvature_scale=1.): feedback_dt = dt if feedback_dt is None else feedback_dt reference = desired_curvature if feedback_curvature is None else feedback_curvature - if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference) or not .002 <= dt <= .1 + if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference, curvature_scale) or not .002 <= dt <= .1 or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.): self.reset() return FordPath() + if curvature_scale <= 0.: + self.reset() + return FordPath() target = encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based) if not target.valid: self.reset() @@ -90,12 +101,16 @@ class ModelActionController: self.feedback_curvature = reference error = reference-current_curvature self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*error if feedback_enabled else 0. - if not _finite(self.proportional): + # Road-curvature error -> geometric steering error -> metres of C0. + # This is proportional only: nothing is accumulated or carried into a release. + response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/max(speed, 1.34)**2 + self.offset_proportional = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0. + if not _finite(self.proportional, self.offset_proportional): self.reset() return FordPath() base = float(np.clip(target.path_angle, -.5, .5)) offset = float(np.clip(target.path_offset+OFFSET_STATION_M*(target.path_angle-base), -5.11, 5.11)) - self.c0 = offset + self.c0 = float(np.clip(offset+self.offset_proportional, -5.11, 5.11)) if feedback_enabled: increment = self.integral_gain*error*speed*feedback_dt if not _finite(increment): @@ -129,9 +144,11 @@ class FordModelActionController: clears the correction. Fresh PSCM limits only inhibit outward integration; neither a limit nor a repeated measurement freezes the model request. """ - def __init__(self, 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-v14' + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False, + c0_proportional_gain=C0_PROPORTIONAL_GAIN): + self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based, + c0_proportional_gain=c0_proportional_gain) + self.hypothesis = 'model-action-curvature-c0-feedback-v15' self.reset() def set_c0_time_based(self, enabled, *, lateral_engaged): @@ -151,7 +168,7 @@ 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): + feedback_curvature=None, curvature_scale=1.): reason = None if not active: reason = 'inactive' @@ -183,7 +200,7 @@ class FordModelActionController: feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2))) command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt, feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited, - feedback_curvature=feedback_curvature) + feedback_curvature=feedback_curvature, curvature_scale=curvature_scale) if not command.valid: self.reset('invalid_path') return command @@ -199,6 +216,8 @@ class FordModelActionController: 'curvature_error': desired_curvature-current_curvature, 'feedback_dt': feedback_dt, '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, + 'curvature_scale': curvature_scale, 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, 'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain, 'integral_gain': self.core.integral_gain, 'feedback_curvature': self.core.feedback_curvature, diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index b8fb16a62c..d6205c960f 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -29,7 +29,8 @@ from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import def assert_current_request(core, desired, speed): target = encode_model_action(straight(), desired, speed) base = min(.5, max(-.5, target.path_angle)) - assert core.c0 == pytest.approx(min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base)))) + offset = min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base))) + assert core.c0 == pytest.approx(min(5.11, max(-5.11, offset+core.offset_proportional))) assert core.c1 == pytest.approx(min(.5, max(-.5, base+core.proportional+core.correction))) @@ -190,7 +191,8 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe 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)*.0045) - assert controls.ford_path.path_offset == pytest.approx(0.) # Limited curvature arc is below one C0 step. + assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.01) + assert controller.core.offset_proportional*expected_curvature > 0. assert cc.latActive and cc.actuators.curvature == 0. assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02) @@ -280,7 +282,9 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline assert core.correction == pytest.approx(expected) assert core.c1 == pytest.approx(sign*.08+expected_p+expected) assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025) - assert controls.ford_path.path_offset == pytest.approx(sign*.1) + base = encode_model_action(straight(), sign*.004, cs.vEgo).path_offset + assert controls.ford_path.path_offset == pytest.approx(base+core.offset_proportional, abs=.005) + assert (core.offset_proportional == 0.) == (torque != 0. or measured == sign*.004) @pytest.mark.parametrize('service_valid', [False, True]) @@ -355,13 +359,16 @@ 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-v14' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15' 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. else: assert sign*controls.ford_path.path_angle < 0. - assert controls.ford_path.path_offset == pytest.approx(sign*(.24 if same_turn else -.02)) + if same_turn: + assert sign*controls.ford_path.path_offset > .24 + else: + assert sign*controls.ford_path.path_offset < -.02 controls.ford_path_controller.reset() assert core.c0 == core.c1 == core.correction == 0. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py new file mode 100644 index 0000000000..e992f85785 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py @@ -0,0 +1,104 @@ +"""C0 proportional correction semantics, independent of simulated PSCM motion.""" +import math + +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_c0_helps_entry_then_releases_at_catchup_without_accumulation(sign): + controller = ModelActionController(c0_proportional_gain=.5) + matched = ModelActionController(c0_proportional_gain=0.) + kwargs = {'speed': 5., 'dt': .01, 'feedback_dt': 0., 'curvature_scale': 1.2} + base = matched.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs) + first = controller.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs) + assert sign*(first.path_offset-base.path_offset) > .4 + assert controller.offset_proportional*sign > 0. + for _ in range(500): + held = controller.update(straight(), sign*.02, current_curvature=sign*.01, **kwargs) + assert held == first + caught = controller.update(straight(), sign*.02, current_curvature=sign*.02, **kwargs) + assert controller.offset_proportional == 0. + assert caught.path_offset == base.path_offset + overshot = controller.update(straight(), sign*.02, current_curvature=sign*.03, **kwargs) + assert sign*(overshot.path_offset-base.path_offset) < -.4 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_zero_target_commands_opposite_c0_and_does_not_retain_old_correction(sign): + core = ModelActionController(c0_proportional_gain=.5) + core.update(straight(), sign*.1, current_curvature=0., speed=5., dt=.01) + out = core.update(straight(), 0., current_curvature=sign*.03, speed=5., dt=.01, feedback_dt=0.) + assert sign*out.path_offset < 0. + out = core.update(straight(), 0., current_curvature=0., speed=5., dt=.01) + assert out.path_offset == core.offset_proportional == 0. + + +def test_new_c0_does_not_change_c1_or_its_integral_for_identical_measurements(): + old, new = ModelActionController(c0_proportional_gain=0.), ModelActionController(c0_proportional_gain=.5) + for i in range(200): + desired, measured = .03*math.sin(i/13), .025*math.sin((i-5)/13) + kwargs = {'current_curvature': measured, 'speed': 12., 'dt': .01, 'feedback_enabled': i % 7 != 0, 'pscm_limited': i % 9 == 0} + a, b = [c.update(straight(), desired, **kwargs) for c in (old, new)] + assert a.path_angle == b.path_angle + assert (old.correction, old.proportional) == (new.correction, new.proportional) + assert b.curvature == b.curvature_rate == 0. + + +def test_c0_is_disabled_by_existing_feedback_arbitration_and_reset(): + core = ModelActionController(c0_proportional_gain=.5) + core.update(straight(), .02, current_curvature=0., speed=5., dt=.01) + assert core.offset_proportional > 0. + disabled = core.update(straight(), .02, current_curvature=0., speed=5., dt=.01, feedback_enabled=False) + assert core.offset_proportional == 0. + core.reset() + assert core.c0 == core.offset_proportional == 0. + assert disabled.path_offset > 0. # Existing feedforward remains available. + + +@pytest.mark.parametrize('scale', [0., -1., math.nan, math.inf, None]) +def test_bad_curvature_conversion_cannot_send_a_command(scale): + core = ModelActionController() + assert core.update(straight(), .02, current_curvature=0., speed=5., dt=.01, curvature_scale=scale) == FordPath() + assert core.c0 == core.offset_proportional == 0. + + +@pytest.mark.parametrize('gain', [-1., math.nan, math.inf, None]) +def test_bad_c0_gain_is_rejected(gain): + with pytest.raises(ValueError): + ModelActionController(c0_proportional_gain=gain) + + +def test_c0_uses_feedback_reference_and_stays_inside_field_bounds(): + core = ModelActionController(c0_proportional_gain=.5) + core.update(straight(), .03, current_curvature=.02, feedback_curvature=.02, speed=5., dt=.01) + assert core.offset_proportional == 0. + for sign in (-1., 1.): + out = core.update(straight(), sign*.9, current_curvature=-sign*.9, speed=55., dt=.01, curvature_scale=3.) + assert out.path_offset == pytest.approx(sign*5.11) + + +def test_c0_response_units_and_explicit_gain(): + core = ModelActionController(c0_proportional_gain=.5) + core.update(straight(), .01, current_curvature=0., speed=5., dt=.01, curvature_scale=1.2) + # Fitted C0 gain is geometric curvature per metre of command, not road curvature. + response = .010717679293424373+.018122981795212647/25. + assert core.offset_proportional == pytest.approx(.5*.01*1.2/response) + + +@pytest.mark.parametrize('speed', [1., 5., 20., 40.]) +@pytest.mark.parametrize('stiffness,ratio,roll', [(.3, 14., -.1), (1., 16.9, 0.), (2., 20., .1)]) +def test_curvature_error_conversion_matches_normal_desired_wheel_angle(speed, stiffness, ratio, roll): + controls = startup() + vm, cp = controls.VM, controls.CP + vm.update_params(stiffness, ratio) + angle, angle_offset, desired = 15., 1.5, .002 + measured = -vm.calc_curvature(math.radians(angle-angle_offset), speed, roll) + target = math.degrees(vm.get_steer_from_curvature(-desired, speed, roll))+angle_offset + scale = vm.get_steer_from_curvature(1., speed, 0.)/(cp.steerRatio*cp.wheelbase) + converted_error = -(desired-measured)*scale*cp.steerRatio*cp.wheelbase*180/math.pi + assert converted_error == pytest.approx(target-angle) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py index 0560709aa6..a6ba145dda 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py @@ -17,7 +17,7 @@ def tick(controller, desired, measured, **overrides): @pytest.mark.parametrize('sign', [-1., 1.]) def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign): - controller, matched = ModelActionController(proportional_gain=0., integral_gain=1.), ModelActionController(proportional_gain=0., integral_gain=1.) + controller, matched = (ModelActionController(proportional_gain=0., integral_gain=1., c0_proportional_gain=0.) for _ in range(2)) for _ in range(100): tick(controller, sign*.004, sign*.004) for _ in range(100): diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py index 2f62be0119..0c6836e85f 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py @@ -63,7 +63,7 @@ def test_extra_offset_releases_immediately_without_stored_overflow(sign): @pytest.mark.parametrize('sign', [-1., 1.]) def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign): - controller = ModelActionController(proportional_gain=0., integral_gain=1.) + controller = ModelActionController(proportional_gain=0., integral_gain=1., c0_proportional_gain=0.) for _ in range(200): out = controller.update(straight(sign*.2), sign*.02, current_curvature=0., speed=20., dt=.01) assert out.path_angle == pytest.approx(sign*.5) @@ -78,6 +78,7 @@ def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, l for _ in range(150): out = controller.update(straight(sign*.2), sign*.03, current_curvature=sign*.02, speed=20., dt=.01, feedback_enabled=enabled, pscm_limited=limited) - assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005) + expected = sign*((1-math.cos(.21))/.03+.7)+controller.offset_proportional + assert out.path_offset == pytest.approx(expected, abs=.005) assert out.path_angle == pytest.approx(sign*.5) assert controller.correction == 0. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index d2a0b707f0..9dfb36c5b4 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -8,6 +8,7 @@ from types import SimpleNamespace import pytest from opendbc.car.ford.values import CAR, FordFlags +from opendbc.car.vehicle_model import VehicleModel from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller from openpilot.selfdrive.controls.lib.ford_path import FordPath @@ -18,7 +19,9 @@ CANFD_CARS = [car for car in CAR if car.config.flags & FordFlags.CANFD] def car_params(**overrides): return SimpleNamespace(**({'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1', - 'carFw': []} | overrides)) + 'carFw': [], 'steerRatio': 16.9, 'wheelbase': 3.7, 'mass': 3084., 'rotationalInertia': 5000., + 'centerToFront': 1.628, 'steerRatioRear': 0., 'tireStiffnessFront': 378306.8125, + 'tireStiffnessRear': 469877.5625} | overrides)) def startup(cp=None, params=None): @@ -36,6 +39,7 @@ def startup(cp=None, params=None): 'select_model_action_controller': select_model_action_controller, 'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)} exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment) + controls.VM = VehicleModel(controls.CP) return controls @@ -48,7 +52,8 @@ def test_actual_startup_priority(candidate, observer, fingerprint): assert type(selected.ford_path_controller) is FordModelActionController 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.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v14' + assert selected.ford_path_controller.core.c0_proportional_gain == .5 + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15' else: assert selected.ford_path_controller is None assert selected.ford_model_action == candidate diff --git a/tools/ford_pscm_lab/c0_feedback_validate.py b/tools/ford_pscm_lab/c0_feedback_validate.py new file mode 100644 index 0000000000..19d4e0021b --- /dev/null +++ b/tools/ford_pscm_lab/c0_feedback_validate.py @@ -0,0 +1,122 @@ +"""Replay C0 feedback against v14 using frozen native-time Lightning measurements. + +This checks software/CAN behavior, not the physical response to new commands. +Use cached route.npz, model_paths.npz and metadata.json from the rlog extractor. +""" +import argparse +from concurrent.futures import ProcessPoolExecutor, as_completed +import hashlib +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib import ford_model_action +from tools.ford_pscm_lab.feedback_replay import original_controller +from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table + + +BASELINE = 'd425b3260b0785b22096d130702b54a2e0761c36' +SPECIAL = {'11c': 'ford_maneuver_11c/analysis', '125': 'ford_c0_response_124_125/route125', + '146': 'ford_model_replay_146/full', '149': 'ford_route149/full', '151': 'ford_route151/full'} +ROUTES = ['a9', 'b9', '112', '113', '117', '11c', '125', '146', '149', '151'] + + +def replay(label, output): + source = Path('.cache')/SPECIAL.get(label, 'ford_route'+label) + 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', 'model', 'params', 'pscm')} + with np.load(source/'model_paths.npz') as z: + model_ns, paths = z['ns'], z['paths'] + reference = Path('.cache/ford_route112/metadata.json') + car = {**json.loads(reference.read_text())['car'][0], **meta['car'][0]} + assert car['fingerprint'] == 'FORD_F_150_LIGHTNING_MK1' + cp = SimpleNamespace(**{k: car[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', + 'tireStiffnessFront', 'tireStiffnessRear')}, + steerRatio=car['steer_ratio'], rotationalInertia=0.) + vm = VehicleModel(cp) + c, t = r['controls'], r['controls']['t'] + assert all(np.all(np.diff(s['t']) >= 0) for s in r.values()) + cs, pa, ps = [sample(r[k], t) for k in ('cs', 'params', 'pscm')] + cc = sample(r['cc'], t, nearest=True) + mi = np.clip(np.searchsorted(r['model']['ns'], c['model_ns']), 0, len(r['model']['ns'])-1) + exact = r['model']['ns'][mi] == c['model_ns'] + np.testing.assert_array_equal(model_ns, r['model']['ns']) + models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths] + old, old_hash = original_controller(BASELINE) + new = ford_model_action.FordModelActionController() + wire = WireCheck() + names = ['t', 'valid', 'c0_old', 'c0_new', 'c1', 'offset_p', 'enabled', 'error_deg', 'target_deg', 'speed', 'curvature_scale'] + rows = np.zeros((len(t), len(names))) + for i, now in enumerate(t): + model_time = r['model']['t'][mi[i]] + good_params = np.isfinite([pa['stiffness'][i], pa['steer_ratio'][i]]).all() and min(pa['stiffness'][i], pa['steer_ratio'][i]) > 0 + if good_params: + vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1)) + scale = vm.get_steer_from_curvature(1., cs['speed'][i], 0.)/(cp.steerRatio*cp.wheelbase) + valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and good_params + and pa['valid'][i] and r['model']['valid'][mi[i]] and exact[i] and abs(cc['t'][i]-now) < .005 + and 0 <= now-pa['t'][i] <= .15) + 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])) + args = {'current_curvature': c['measured'][i], 'yaw_rate': cs['yaw'][i], 'speed': cs['speed'][i], 'now': now, + 'measurement_time': cs['t'][i], 'model_time': model_time, 'reference_time': model_time, 'active': bool(cc['active'][i]), + 'valid': valid, 'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i], 'pscm_status': status} + model = models[mi[i]] if exact[i] else None + a = old.update(model, c['desired'][i], **args) + b = new.update(model, c['desired'][i], curvature_scale=scale, **args) + assert a.valid == b.valid + assert a.path_angle == b.path_angle + assert old.core.correction == new.core.correction + assert abs(b.path_offset) <= 5.1100001 and abs(b.path_angle) <= .5000001 + assert b.curvature == b.curvature_rate == 0. + assert np.isfinite([b.path_offset, b.path_angle, new.core.offset_proportional]).all() + if not new.diagnostics.get('feedback_enabled', False): + assert new.core.offset_proportional == 0. and a.path_offset == b.path_offset + if new.core.offset_proportional == 0.: + assert a.path_offset == b.path_offset + error = -(c['desired'][i]-c['measured'][i])*scale*cp.steerRatio*cp.wheelbase*180/np.pi + target = vm.get_steer_from_curvature(-c['desired'][i], cs['speed'][i], pa['roll'][i])*180/np.pi+pa['angle_offset'][i] + rows[i] = [now-meta['t0'], b.valid, a.path_offset, b.path_offset, b.path_angle, new.core.offset_proportional, + new.diagnostics.get('feedback_enabled', False), error, target, cs['speed'][i], scale] + if i % 10 == 0: # Native-time integration above; CAN round trip on every tenth frame. + wire.check(b) + dest = output/label + dest.mkdir(parents=True, exist_ok=True) + np.savez_compressed(dest/'commands.npz', names=names, rows=rows) + eligible = rows[:, 1].astype(bool) & rows[:, 6].astype(bool) + groups = {'all': eligible, 'straight': eligible & (abs(rows[:, 8]) < 5), + 'turn': eligible & (abs(rows[:, 8]) >= 45), 'large_error': eligible & (abs(rows[:, 7]) >= 30)} + metrics = {} + for name, mask in groups.items(): + if mask.any(): + delta = abs(rows[mask, 3]-rows[mask, 2]) + metrics[name] = {'n': int(mask.sum()), 'delta_c0_m_quantiles': np.quantile(delta, [.5, .95, 1.]).tolist(), + 'c0_old_capped': int((abs(rows[mask, 2]) >= 5.105).sum()), + 'c0_new_capped': int((abs(rows[mask, 3]) >= 5.105).sum())} + files = [source/n for n in ('route.npz', 'metadata.json', 'model_paths.npz')] + files += [reference, Path(__file__), Path(ford_model_action.__file__)] + report = {'route': label, 'baseline': BASELINE, 'baseline_sha256': old_hash, 'cycles': len(t), 'wire_round_trips': wire.count, + 'c1_identical': True, 'c0_gain': new.core.c0_proportional_gain, 'metrics': metrics, + 'sources': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in files}} + (dest/'report.json').write_text(json.dumps(report, indent=2)+'\n') + return report + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--routes', nargs='+', default=ROUTES, choices=ROUTES) + parser.add_argument('--workers', type=int, default=4) + args = parser.parse_args() + with ProcessPoolExecutor(max_workers=args.workers) as pool: + jobs = {pool.submit(replay, label, args.output): label for label in args.routes} + results = [] + for job in as_completed(jobs): + result = job.result() + results.append(result) + print(json.dumps({k: result[k] for k in ('route', 'cycles', 'wire_round_trips', 'metrics')}), flush=True) + (args.output/'validation.json').write_text(json.dumps({'scope': __doc__, 'routes': results}, indent=2)+'\n')