diff --git a/docs/ford_c1_minimal_pi.md b/docs/ford_c1_minimal_pi.md new file mode 100644 index 0000000000..8e863325e2 --- /dev/null +++ b/docs/ford_c1_minimal_pi.md @@ -0,0 +1,82 @@ +# Ford continuous PI drive trial (v7) + +The selected controller follows the current, upstream-limited desired curvature +with P=0.50 and I=0.25. It replaces v6's request-change, C0-confirmation and +unwind-catchup release rules. C0 mapping, heading overflow allocation, input +health gates, driver/PSCM arbitration and final output limits remain unchanged. +Only C0, C1 and I carry control history. C2/C3 stay zero. + +```text +D = max(7 m, speed × 1 s) +error = selected desired curvature − measured steering-derived curvature +base = clip(D × desired curvature, −0.5, +0.5) +P = 0.50 × D × error +increment = 0.25 × speed × error × fresh steering measurement interval +C1 target = clip(base + P + I, −0.5, +0.5) +``` + +After PSCM outward-accumulation arbitration, the increment first cancels up to +its own magnitude of opposing I. It cannot cross zero in that step. Any remainder +is bounded by the combined command's amplitude/slew headroom. C1 output still +slews at 0.5 rad/s. This allows old I to unwind even when the output is already +slewing. It adds no timer, request history, turn detection or position threshold. + +P doubles relative to v6, and I builds at one quarter the previous rate for an +identical error and fresh measurement interval. Lower I also unwinds more slowly +for an identical existing state and error; its replay benefit comes mainly from +storing less correction. Zero error removes P and holds I. Persistent tracking +bias may need that holding correction. There is no claim that all I is unwanted. + +## Evidence and limitations + +Six offline settings were compared across fourteen routes and 1,578,250 source +cycles before selecting this candidate. The production selector and adapter now +exactly reproduce the selected P=0.50/I=0.25 archived commands, validity, P, I, +feedforward, C0 overflow and feedback/PSCM gates on every cycle of all fourteen +routes. These comprise Lightning 112–117, a0, a2, a5, a9, b8, b9, ca and Raptor 02. +The integration replay performs 1,578,250 actual Float32/CAN round trips. + +A further 20,000 randomized cycles with mirrored and independent C0 paths perform +60,000 CAN checks on production, checking independent scalar arithmetic, +C0-independent feedback, symmetry, reset, amplitude and slew behavior. +The Ford, Sunnylink, params, sender and safety suite passes 679 tests and +9,145 subtests; 178 are skipped by the platform test suite. Removed maneuver +heuristic tests are replaced with continuous-error, cancellation, freshness, +three-state reproduction and actual controlsd-to-CAN entry/exit checks. +Historical untracked offline experiment tests are outside this deployment suite. + +At a previously reviewed route-115 exit (133.595 s), the original small PI +controller requested +0.1090 rad C1; this trial requests +0.0175. That sample +includes driver context. Reviewed large entries remain similar, but commands +are not identical everywhere. In a previously well-tracked route-116 bend +(112 s), C1 falls from +0.1530 to +0.1355 rad. Lower I could weaken a persistent +bend, while higher P can increase response to measurement fluctuations. + +Recorded wheel motion remains fixed in replay. These checks establish software +behavior and exact integration of the candidate; they do not establish improved +physical tracking or stability. Gains remain experimental, not an identified +universal PSCM calibration. No device build, boot or new drive is claimed. + +## Selection and reproduction + +Use the existing default-off Sunnylink **Selected-Action Path Tracking +(Experimental)** toggle on any Ford CAN FD, followed by a real offroad-to-onroad +cycle. Logs identify `model-action-c1-pi-v7`, `proportional_gain=0.5`, +`integral_gain=0.25`. Toggle-off selects upstream Ford control. See the +[drive instructions](ford_model_action_drive_test.md). + +With the built cereal/opendbc environment and archived local extracts: + +```sh +export PYTHONPATH=.:opendbc_repo:.cache/ford_v6/test_deps +export PYTHONDONTWRITEBYTECODE=1 +export PARAMS_ROOT=/tmp/ford-v7-params +export LOG_ROOT=/tmp/ford-v7-logs +python -m tools.ford_pscm_lab.minimal_pi_validate --output .cache/ford_minimal_tuning/production --workers 4 +python -m tools.ford_pscm_lab.minimal_pi_production_stress --cycles 20000 --output .cache/ford_minimal_tuning/production_stress.json +``` + +The validation JSON records route and source hashes. The archived six-setting +sweep is local evidence, not a checked-in dataset. Historical v5/v6 lab tools +load their pinned controller revisions so their baseline comparisons retain +their original meaning after production changes. diff --git a/docs/ford_c1_minimal_pi_validation.json b/docs/ford_c1_minimal_pi_validation.json new file mode 100644 index 0000000000..430834c460 --- /dev/null +++ b/docs/ford_c1_minimal_pi_validation.json @@ -0,0 +1,332 @@ +{ + "scope": "Production integration of the selected continuous PI drive trial; fixed recorded motion, not a physical tracking prediction.", + "parent_revision": "5e6993aab", + "previous_controller_revision": "bf00bc691def830e1beb15363d05416714c1dc42", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "control_history": [ + "c0", + "c1", + "correction" + ], + "physical_lines": { + "module": 201, + "core_class": 58, + "core_update": 42 + }, + "production_source_sha256": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8", + "route_cycles": 1578250, + "production_can_round_trips": 1638250, + "tests": { + "passed": 679, + "skipped": 178, + "subtests_passed": 9145, + "log_sha256": "2bc3dfd7ba08d4985d3dea98f235eb1fc99b23e5bbb3927e823a2439c8e9a481", + "scope": "Ford controls, tracked PSCM lab tests, PSCM status, Sunnylink, params, Ford car and safety suites; historical untracked experiments excluded." + }, + "ruff": "pass", + "ty_production": "pass", + "routes": [ + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "112", + "cycles": 108971, + "can_round_trips": 108971, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/route.npz": "2b08a2fb636f7d14556d7df4035eafc1d1b97932237955528562a16db2b31d3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/model_paths.npz": "9837afe78aab4cad288cad98a595a5777fa8a66bb235986b1272a7f7c54e559a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route112/metadata.json": "726d78a7e7aa45307dcfe27cb00775c20ecc9d54538eb7f26a0166fc216226ce", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/112/report.json": "d8cbcf8b676a67a90310390611da31783b4e025a0a965239ef4672caa47aae56", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/112/commands.npz": "8d91f449c7f8741ce465ec4326949a9d9527ed6b5de7a526ad7d30212f16c469", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "113", + "cycles": 49614, + "can_round_trips": 49614, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/route.npz": "774ca4a21b7113c2706d6130bc180c3216ea4833155300ab01e75b3486e36327", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/model_paths.npz": "93c41761eb85263f534f5371b905482cf7c948582eb1e9149966594be1d3768f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route113/metadata.json": "1c0ca74dd48b90ab9d5444c5ca7f8aa9361700bbdf98bd5e853be50ad2895d7f", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/113/report.json": "e2dfeba5d3fe8a1147f82bce9ddf2ad3f14271bda35fac1639d383f2a0189228", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/113/commands.npz": "3890c802072dd2d38b4cded5f9f80310d1f86eb8a8f3598138235a0e2cc16460", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "114", + "cycles": 61027, + "can_round_trips": 61027, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/route.npz": "83524f07d61104b84b004ddc46bb751ca7f61da67798aa47db56d307765f5b3e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/model_paths.npz": "14d14362d1a3e46398edd8e22b7cc4e36277a7596a73f546192d0e14c6642b07", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route114/metadata.json": "ec677b9275c1707e477ebd0ffa235d49b5ec63a1ee717b16040c3acdbcd3bdc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/114/report.json": "eb630bb5f35a43c8902e15a97b53d83db3dc3cfa843a14d09cb8a1e6dab89289", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/114/commands.npz": "899033d0c9eeffe7929aa1289bfbc0e7da5608833b03989ce10a87d975344d3d", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "115", + "cycles": 40037, + "can_round_trips": 40037, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/route.npz": "e0df32d9e80f1c6b7d56327b37cf07af60070ffc212b8d111e343306148bff23", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/model_paths.npz": "52f3ed9e188e4947618a57a3ed872fd1966a887fe1014999df741553b6c13bc0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route115/metadata.json": "d3c73035bad8eb5059e07962fd274c19cb70c8952b6f1514835d312d08b87a16", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/115/report.json": "58cee499da3191808aedbd79a9497d9c1553f49007f24543c44bc2b58ff971fa", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/115/commands.npz": "09c2da33dabbc0072c61578224cd5ab27ed44690394315350c1c33708b0a67bc", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "116", + "cycles": 68793, + "can_round_trips": 68793, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/route.npz": "13df1d49edd0c92bd4a4236c766ee76baa6dad477824b9de4b79927c98410394", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/model_paths.npz": "85d1112b5e89340e8a955b7fe4040b3135cf1c91a54ff756826c6119cc9cc01d", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route116/metadata.json": "d52fe63d013fa546524ad9522a26943d9372730d397d92d75dfd9d75c63ce766", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/report.json": "01d2e608a53f9fd805baee853d18acc63b5506319e0f0ceeeb3e59a92bae0385", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/116/commands.npz": "2186a1a1fea2ea98390fb90c9df1288d321cd165f70e3aef0c268cbbd2fd8c95", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "117", + "cycles": 27301, + "can_round_trips": 27301, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/route.npz": "65e4ea6c76dce73018b636479b3777116f1e58bb9813e6c1a7b79018b219140e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/model_paths.npz": "079f6003968287530e1ed6f2243c727e80c2ef3d16fb8a7d945d7eca744d54e7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_route117/metadata.json": "348396b3059c5f54d7938cd3acf2bb268a813b47ff47561da7a94cb75c3f3114", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/report.json": "80efcf48f018d3a887e8b9fdb36f67d9c5d9a57b09f117c8fdecd4d5556ff80b", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/117/commands.npz": "c2056b5d9faf765cb4713f73e9c4937bb24c9eeb1d4dac0d21e141322d52609e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "a0", + "cycles": 114582, + "can_round_trips": 114582, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/model_paths.npz": "5d0c69d10a6d13882b47d950ae64c4d98f7ca2b59eab27e4e9557eef3bc4917b", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea0/metadata.json": "00496a11afbee475e9b5d66838a4d1663a3c0063cd562b4d1408cce6d71e586e", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/report.json": "5f3ca84b700e019cc72201d9e4ff85c42fbd15e9a3850434caf1bac688b332d8", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a0/commands.npz": "5c52d587ba45446117677686aabc6af81d5d2eacab5c04b02b53fe6fb95e36d0", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "a2", + "cycles": 71111, + "can_round_trips": 71111, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/model_paths.npz": "5a5a7c4fc30c7b3bc7f2413daa8b11fe86b69dd3249e8df2d35b0a0636a49717", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea2/metadata.json": "cfecd240216e409a1e2591c6c2e6f0bad002ecd2a50c8f6dbf22a8f9174708da", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/report.json": "de61021f0c13af16e98688b17aadae9d61c99d20be82329b7cc235836defa41a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a2/commands.npz": "fd263ab667b97905d840751577179ad75472421104cf20663e3ee51bf32dd2ee", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "a5", + "cycles": 38961, + "can_round_trips": 38961, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/model_paths.npz": "72e014b844322df6318f6af588c27175ce68d9a37bda62b70eafca5bdfe6dfe4", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea5/metadata.json": "c533b06d1b7c598330b36b117bdb3624221bc4193ded8354246d5c86f36b2628", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/report.json": "dc6c05043c5caef6bf12b12eff69ef53d213fe8606f59865859d2c475e0aa3ee", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a5/commands.npz": "45f551b32a354a65bff585b81a89091abc5edad406b6ca100a8dcabc50c5dbe3", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "a9", + "cycles": 286319, + "can_round_trips": 286319, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/model_paths.npz": "7f3646ad24aa52de5982c65600813de5bbc5f7664d92a2516bbba35da6e7b240", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/report.json": "17c41415b9fb9fcd8d72835347a205113c32d32d955c0de68850a9764968b3e9", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/a9/commands.npz": "27af5f6fb8f6e08f31d54e24c90430d30c72d31b1448bc1fbcb31f35804afe8a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "b8", + "cycles": 160431, + "can_round_trips": 160431, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/route.npz": "6f5dd369b70eaed4b95b28c8b25c9f2e9b830fa07a334881a185505481667c8b", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/model_paths.npz": "939af6cf7e74251d8842581cc078d26d9fbfd22a0d7817cb0e368697d419b615", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb8/metadata.json": "73b439132d1de37ec187b544c04d2b05c80965065515a4b7dec29ba57ae37e7c", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/report.json": "5c6b52222abbb95147dc756a34e62ada97c42aa550b952fd6ae7b26d8d9639fa", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b8/commands.npz": "9c158eff98009bcb039a4e7c29b97b0d3a9c67d61160a4e4766a176174de52d7", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "b9", + "cycles": 90774, + "can_round_trips": 90774, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/route.npz": "b07c789d8155335f5d120d0262fced6e4d5803fe767b0ff49b6413dce4140b5c", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/model_paths.npz": "6b1f87897c050273fdc05af051307a049b6fc3a93072e7cda1721195ce7c3861", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeb9/metadata.json": "9ce452220cab61b81883f32fc2fcaf5db6c78a674cb255a49cc77d5029580fee", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b9/report.json": "9eeda0c24ede7a55ef8a924e0ebe0bcb0dea7a9612457af0c5aff898a828c195", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/b9/commands.npz": "2dcc0865591c988d8a08671311a9631f3c4aa3ded4e90ee6f76d28b1654809ad", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "ca", + "cycles": 327448, + "can_round_trips": 327448, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/route.npz": "ae9d46770eaf0dbbac6af86aebc926320eed0cf114eb43d5f78b0676e8e0dbf9", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/model_paths.npz": "bf17deb442383aaa79432566cd382df24a1bbbbd0521d0cafab956618f5bdd96", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_routeca/metadata.json": "a759d5cdf878df8b05d91db637b1935b6b4bdd87af96f0f256b67e7d809b3525", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/ca/report.json": "025f2f49f68ec2164a10474a4c6ec0eb1b6022990e135fc38134a052a8710dff", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/ca/commands.npz": "cbe9f63955a409495f57466dc5811d3eed65af2cf096ee060d924437488c3f76", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + { + "scope": "Verify production commands against the archived P=.50/I=.25 offline candidate.\n\nFixed recorded motion verifies integration parity, not physical tracking.\n", + "route": "raptor02", + "cycles": 132881, + "can_round_trips": 132881, + "production_matches_archived_trial_exactly": true, + "opendbc_revision": "64aa61b9b3fd26e70a7caa915acab207ff3cd64a", + "hypothesis": "model-action-c1-pi-v7", + "kp": 0.5, + "ki": 0.25, + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/route.npz": "8a4cfe53994988d053b47d9caadf21ebd064fd20e598fa05ddc2da245bd0e980", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/model_paths.npz": "d5dc6f72d91472b8f2fb1add1680cdaea1e122774ddd6d8519fd7673daf71bb5", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_raptor_route02/metadata.json": "9602e08efc2bd784133837c4156c075bb89bad3bedd6b309f15e884ed20e338a", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/report.json": "2c2dda2a5e4e8a764a10852cca3e5caf319b62e3d1d841ef9e1b8c11be77ebf6", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_minimal_tuning/sweep/raptor02/commands.npz": "e0440d0a4035b975a9c4a47d85335b5b0f0903fcd00dd7e7c858856deffd1249", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_validate.py": "35b5334f52eb6069b79fc3ed3be71ceaa81a3db8e1f76e8b1ab6e363cd4f18ae", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + } + ], + "production_stress": { + "cycles": 20000, + "kp": 0.5, + "ki": 0.25, + "controller_updates": 60000, + "can_round_trips": 60000, + "seed": 20260913, + "independent_c0_comparisons": 20000, + "checks": "Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.", + "source_sha256": { + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/minimal_pi_production_stress.py": "fa9058826512498be1938797efba25666df832080fb7ad6ff18788ee4bda333d", + "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "a899b8595e903d6fc4401392a8d123ef77725f6d6d7fa10392a7b3fe72e732b8" + } + }, + "limits": [ + "No hardware build, device boot or new physical drive performed.", + "Matched offline commands do not establish improved tracking or closed-loop stability.", + "Higher P can amplify measurement fluctuations; lower I can take longer to correct persistent error.", + "Toggle defaults off and off selects upstream Ford control; on applies to any Ford CAN FD." + ] +} diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index c9032d46cb..a0a6c827d6 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -1,11 +1,9 @@ # Ford selected-action drive-test branch -The current experiment adds [measured-curvature C1 feedback](ford_c1_feedback.md) -and [conditional correction release](ford_c1_carryover.md) to the restored -original v1 mapping, with [base C1 overflow allocated to C0](ford_c1_overflow.md) -and [completed-unwind correction release](ford_unwind_catchup.md). -V6 adds [explicit proportional C1 feedback with P=0.25](ford_c1_pi.md), retaining -the existing feedback timing. This is an initial drive-trial gain. +The current v7 experiment uses [continuous C1 PI feedback](ford_c1_minimal_pi.md) +with **P=0.50 and I=0.25**, replacing the earlier conditional-release rules. +Only C0, C1 and accumulated error carry control history. The restored model +mapping and [base C1 overflow allocation to C0](ford_c1_overflow.md) remain. It is selectable on **any Ford CAN FD vehicle** through the existing persistent, default-off Sunnylink toggle. Offline checks establish software behavior; physical tracking, @@ -23,19 +21,12 @@ turn-exit behavior and closed-loop stability remain unvalidated. The startup event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify **`hypothesis=model-action-c1-pi-v6`**. They report desired and -measured curvature, base heading, proportional and accumulated correction, applied heading, -feedback timing and driver/PSCM gating. `carryover_release_count` counts -conditional releases since the last controller reset; it does not control -steering. `offset_overflow` reports the extra C0 target in meters before C0 -amplitude and slew limits. `calibration_approved=false` remains. -`request_release` reports correction retired on the current cycle when a changed -request and sufficiently large measured error agree. Periodic logs can miss -individual retirement cycles. -`unwind_direction` remembers an unfinished unwind; `unwind_release` reports -correction retired when a confirmed unwind catches the selected curvature. -`heading_proportional`, `proportional_gain`, `feedback_curvature` and -`feedback_error` separate the new P contribution and its reference from I. +identify **`hypothesis=model-action-c1-pi-v7`**. 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 +`integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0 +target in meters before C0 amplitude and slew limits. `calibration_approved=false` +remains. The retired request/unwind/reversal diagnostic fields are removed. Turning the toggle off and completing another offroad-to-onroad cycle restores **upstream Ford curvature control**: 20 Hz steering messages, limited mode on @@ -54,15 +45,11 @@ error without accumulating. Repeated publications may advance P and output slew but cannot integrate the same elapsed interval twice. Driver override clears P and I. A fresh PSCM reached-limit flag stops extra outward accumulation while preserving unwind and base model changes. -With fresh feedback, the controller can discard an opposing correction when -it prevents C1 from following the direction shared by original model C0, applied C0 -and base C1, while measured curvature is still opposite. A separate bounded -release now handles changed requests within the same turn when measured error -also opposes the old correction. V5 additionally retires dominant unwind memory -at catch-up when the selected request reaches zero/new-side curvature and both -C0 requests confirm that side. Steady requests cannot arm this release, and -catching an old-side bend retains correction. Final output slew still applies. -See [completed-unwind release](ford_unwind_catchup.md). +With fresh feedback, the part of the error increment that cancels existing I +is applied before the ordinary accumulation clamp. Any remainder must fit the +combined feedforward/P/I amplitude and slew envelope. There is no C0 confirmation +threshold or remembered turn direction. Zero error removes P and holds I; it +does not trigger a release. Final command limits still apply. C0 starts with the original 7 m model-path mapping. When the raw base heading exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading. @@ -74,8 +61,9 @@ place. An explicit selection flag distinguishes upstream mode from an invalid experimental command; invalid experimental input cannot switch to upstream. The opendbc sender restores upstream behavior when that flag is false. -See [proportional feedback trial and validation](ford_c1_pi.md) and -`ford_c1_pi_validation.json` for current evidence and reproduction commands. +See [continuous PI trial and validation](ford_c1_minimal_pi.md) and +`ford_c1_minimal_pi_validation.json` for current evidence and reproduction commands. +[Proportional feedback](ford_c1_pi.md) and its validation JSON record v6. [Completed-unwind release](ford_unwind_catchup.md) and `ford_unwind_catchup_validation.json` record v5. [Changed-request release](ford_c1_request_release.md) and its validation JSON record v4. The [overflow specification](ford_c1_overflow.md) and diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 649e2e1230..cf965a357c 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,13 +1,8 @@ -"""Experimental Ford C2-free controller with measured-curvature C1 PI feedback. +"""Opt-in Ford C2-free model mapping with measured-curvature PI feedback. -Selected only by its explicit toggle. The 7 m station and one-second scale are -engineering choices. Feeding integrated heading mismatch into C1 at 1:1 is an -explicit feedback-strength choice, not an identified PSCM model or calibration. -The selected experiment adds an explicit proportional heading-error term. -Opposed correction may be released when both path commands confirm the turn. -Changed requests retire opposing correction only while measured error agrees. -Completed, direction-confirmed unwinds release dominant old correction. -Base heading clipped by C1 is allocated to C0 at the existing 7 m reference. +C0 samples the model at 7 m, including clipped base-heading overflow. C1 +combines the selected curvature's heading with proportional and integrated +tracking error. Reference distance and gains are explicit trial choices. """ import math import struct @@ -20,7 +15,8 @@ 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.25 # Initial drive-trial gain, not a learned calibration. +C1_PROPORTIONAL_GAIN = 0.50 # Drive-trial gains, not a learned calibration. +C1_INTEGRAL_GAIN = 0.25 CALIBRATION_APPROVED = False @@ -58,127 +54,62 @@ def encode_model_action(model, desired_curvature, speed): class ModelActionController: - """C0/C1 slew, C1 correction, last feedback request and unwind direction. + """Only C0 slew, C1 slew and integrated tracking error carry control history. - Feedback integrates requested minus measured curvature over traveled distance. Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. - Zero P is the v5 reference; onroad selection supplies the explicit trial gain. """ - __slots__ = ('c0', 'c1', 'correction', 'carryover_release_count', 'last_feedback_desired', 'request_release', - 'unwind_direction', 'unwind_release', 'proportional_gain', 'proportional', 'feedback_curvature') + __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature') - def __init__(self, proportional_gain=0.): - if not _finite(proportional_gain) or proportional_gain < 0.: - raise ValueError('Proportional gain must be finite and nonnegative') - self.proportional_gain = float(proportional_gain) + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN): + if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.: + raise ValueError('PI gains must be finite and nonnegative') + self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain) self.reset() def reset(self): - self.c0 = self.c1 = self.correction = 0. - self.carryover_release_count = 0 # Diagnostic only; never feeds the command law. - self.last_feedback_desired = None - self.request_release = 0. # Diagnostic radians retired on this cycle. - self.unwind_direction = 0. - self.unwind_release = 0. # Diagnostic only; final output still obeys slew. - self.proportional = self.feedback_curvature = 0. + self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 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 = dt if feedback_dt is None else feedback_dt - feedback_curvature = desired_curvature if feedback_curvature is None else feedback_curvature - if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, feedback_curvature) or not .002 <= dt <= .1 - or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(feedback_curvature) > 1.): + 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 + or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.): self.reset() return FordPath() target = encode_model_action(model, desired_curvature, speed) if not target.valid: self.reset() return FordPath() - self.feedback_curvature = feedback_curvature - feedback_error = feedback_curvature-current_curvature - self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*feedback_error if feedback_enabled else 0. + 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): self.reset() return FordPath() - base_c1 = float(np.clip(target.path_angle, -.5, .5)) - # Preserve the linear path reference at 7 m when the base heading clips. - # This is instantaneous geometry, not stored error or C1 feedback spill. - c0 = float(np.clip(target.path_offset + OFFSET_STATION_M*(target.path_angle-base_c1), -5.11, 5.11)) - self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt)) - lower = max(-.5, self.c1-.5*dt) - upper = min(.5, self.c1+.5*dt) - self.request_release = 0. - self.unwind_release = 0. - if not feedback_enabled: - self.correction = 0. - self.unwind_direction = 0. - else: - error = desired_curvature-current_curvature - if feedback_dt > 0.: - # Remember an unwind only when a relaxing request and excess measured - # steering call for leaving the old turn. This may precede accumulation - # while the output is still slewing. A steady request cannot arm it. - previous = self.last_feedback_desired - if (previous is not None and (desired_curvature-previous)*previous < 0. - and current_curvature*previous > 0. and error*previous < 0.): - self.unwind_direction = math.copysign(1., error) - direction = self.unwind_direction - if direction and error*direction <= 0.: - # Steering has caught the request. Retire dominant unwind memory only - # after the selected request crosses zero and both path offsets also - # confirm the new side. Catching a steady old-side bend retains I. - if (desired_curvature*direction >= 0. and self.correction*direction > abs(base_c1) - and min(target.path_offset*direction, self.c0*direction) >= .01): - self.unwind_release = self.correction - self.correction = 0. - self.unwind_direction = 0. - # A changed target can make old correction counterproductive before - # steering reverses. Retire at most the heading change, and only when - # fresh measured error calls for that same change, by at least a C1 DBC - # step. Never cross zero or discard a steady tracking correction just - # because error changes sign. - # Require the heading mismatch over the existing reference distance to - # cover the correction: small tracking noise must not erode a useful I. - # Evaluate both requests at today's speed so speed changes alone do not - # release anything. Duplicate measurements leave this history untouched. - if feedback_dt > 0. and self.last_feedback_desired is not None: - distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S) - previous_base = float(np.clip(distance*self.last_feedback_desired, -.5, .5)) - change = base_c1-previous_base - if (abs(change) >= .0005 and change*error > 0. and change*self.correction < 0. - and abs(distance*error) >= abs(self.correction)): - self.request_release = float(np.clip(change, min(-self.correction, 0.), max(-self.correction, 0.))) - self.correction += self.request_release - direction = math.copysign(1., base_c1) - # Release only correction that prevents C1 from requesting the direction - # shared by model C0, slewed C0 and base C1, while measured steering is - # still opposite. One DBC step confirms each request is nonzero. Matched - # steering, neutral/conflicting centering and duplicate samples retain I. - if (feedback_dt > 0. and abs(base_c1) >= .0005 and current_curvature*direction < 0. - and min(target.path_offset*direction, self.c0*direction) >= .01 - and (base_c1+self.correction)*direction <= 0.): - self.correction = 0. - self.carryover_release_count += 1 - increment = feedback_error*speed*feedback_dt - # LimitReached inhibits only extra demand in the measured turn direction. - # Opposing correction and changes to the model request remain available. + 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 += float(np.clip(offset-self.c0, -4.*dt, 4.*dt)) + lower, upper = max(-.5, self.c1-.5*dt), min(.5, self.c1+.5*dt) + if feedback_enabled: + increment = self.integral_gain*error*speed*feedback_dt + if not _finite(increment): + self.reset() + return FordPath() direction = current_curvature if current_curvature else self.c1 if pscm_limited and increment*direction > 0.: - # An old opposing correction may return to zero; don't trap it below - # the base request just because the PSCM now reports a limit. increment = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.))) - # Include P in the available headroom so I cannot wind up behind it. - # Integrate only as far as this cycle's amplitude/slew envelope permits. - # If the base moved outside that envelope, allow increments toward it; - # never rewrite existing correction merely because the base changed. - request = base_c1+self.proportional+self.correction + # Retire existing I before limiting new accumulation; never cross zero + # through this step. The final command still obeys amplitude and slew. + relief = float(np.clip(increment, min(-self.correction, 0.), max(-self.correction, 0.))) + self.correction += relief + increment -= relief + request = base+self.proportional+self.correction self.correction += float(np.clip(increment, min(lower-request, 0.), max(upper-request, 0.))) - if self.correction*self.unwind_direction < 0.: - self.unwind_direction = 0. - if self.last_feedback_desired is None or feedback_dt > 0. or not feedback_enabled: - self.last_feedback_desired = desired_curvature - c1 = float(np.clip(base_c1+self.proportional+self.correction, -.5, .5)) - self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt)) + else: + self.correction = 0. + request = float(np.clip(base+self.proportional+self.correction, -.5, .5)) + self.c1 += float(np.clip(request-self.c1, -.5*dt, .5*dt)) return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.) @@ -194,9 +125,9 @@ 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=0.): - self.core = ModelActionController(proportional_gain=proportional_gain) - self.hypothesis = 'model-action-c1-pi-v6' if proportional_gain else 'model-action-c1-feedback-v5' + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN): + self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain) + self.hypothesis = 'model-action-c1-pi-v7' self.reset() def reset(self, status='inactive'): @@ -255,11 +186,8 @@ class FordModelActionController: 'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading), 'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled, 'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain, - 'feedback_curvature': self.core.feedback_curvature, + 'integral_gain': self.core.integral_gain, 'feedback_curvature': self.core.feedback_curvature, 'feedback_error': self.core.feedback_curvature-current_curvature, - 'request_release': self.core.request_release, - 'unwind_direction': self.core.unwind_direction, 'unwind_release': self.core.unwind_release, - 'carryover_release_count': self.core.carryover_release_count, 'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh), 'command': (command.path_offset, command.path_angle, 0., 0.)} return command @@ -269,5 +197,5 @@ def select_model_action_controller(CP, enabled): """Only opt-in Ford CAN FD vehicles override upstream curvature control.""" compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD if enabled and compatible: - return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN) + return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN) return None diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index f462a6e571..13fdd506d9 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase): controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.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-c1-feedback-v5') + self.assertEqual(record['hypothesis'], 'model-action-c1-pi-v7') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action.py b/openpilot/selfdrive/controls/tests/test_ford_model_action.py index a0111bf211..1637154608 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action.py @@ -42,14 +42,12 @@ def test_centering_information_is_independent_of_action_and_not_scaled_with_spee assert target.path_angle == pytest.approx(sign*.2) # No 10 m cap at highway speed. -def test_five_control_states_are_sufficient_for_every_next_output(): +def test_three_control_states_are_sufficient_for_every_next_output(): controller = ModelActionController() assert not hasattr(controller, '__dict__') for i in range(300): copied = ModelActionController() copied.c0, copied.c1, copied.correction = controller.c0, controller.c1, controller.correction - copied.last_feedback_desired = controller.last_feedback_desired - copied.unwind_direction = controller.unwind_direction model = straight(.2*math.sin(i*.1)) kwargs = {'speed': 20., 'dt': .01} desired = .005*math.cos(i*.03) 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 5c7ad81ecb..af66742d1d 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -180,9 +180,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(.25*20.*expected_curvature) + assert controller.core.proportional == pytest.approx(.5*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)*.003) + assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0035) assert controls.ford_path.path_offset == pytest.approx(.04) assert cc.latActive and cc.actuators.curvature == 0. assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02) @@ -239,11 +239,11 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main) frame = 0 - # A .005 rad P step consumes one slew interval before I can accumulate; - # going from -.005 to +.005 consumes two. Matched steering removes P only. - for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.0198), - (sign*.004, 0., 100, sign*.0198), (sign*.005, 0., 100, 0.), - (sign*.003, 0., 100, sign*.0196), (0., 1.0625, 5, 0.)]: + # P spends output slew headroom before new I accumulates. Existing I can + # unwind throughout that slew. Matched steering removes P and preserves I. + for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.0049), + (sign*.004, 0., 100, sign*.0049), (sign*.005, 0., 100, -sign*.0001), + (sign*.003, 0., 100, sign*.0048), (0., 1.0625, 5, 0.)]: for _ in range(count): now = 1.+frame*.01 controls.curvature, cs.steeringTorque = measured, torque @@ -268,7 +268,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 = .25*20.*(sign*.004-measured) if torque == 0. else 0. + expected_p = .5*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) @@ -284,7 +284,7 @@ def test_actual_controlsd_passes_only_valid_pscm_service_to_feedback(pipeline, s model.action = SimpleNamespace(desiredCurvature=.004) cc = structs.CarControl(latActive=True) cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.) - for frame in range(102): + for frame in range(103): now = 1.+frame*.01 controls.curvature = .004 if frame < 100 else .003 sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) @@ -296,16 +296,16 @@ 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(.005) - # First error sample spends the slew allowance on P; the second may add I. - assert controller.core.correction == pytest.approx(0. if service_valid else .0002) + assert controller.core.proportional == pytest.approx(.01) + # Two error samples spend the slew allowance on P; the third may add I. + assert controller.core.correction == pytest.approx(0. if service_valid else .00005) assert cc.latActive and controls.ford_path.valid @pytest.mark.parametrize('sign', [-1., 1.]) @pytest.mark.parametrize('maneuver', [False, True]) @pytest.mark.parametrize('same_turn', [False, True]) -def test_carryover_release_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn): +def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(pipeline, sign, maneuver, same_turn): call, publication = pipeline controls, sm = startup(), Subscriptions(maneuver) controls.sm, controls.desired_curvature = sm, sign*.004 @@ -318,7 +318,6 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel vehicle = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed), acc_tja_status_stock_values=defaultdict(int), lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main) - request_releases = 0 for frame in range(280): now = 1.+frame*.01 desired = sign*(.004 if frame < 200 else .01 if same_turn else -.001) @@ -327,12 +326,13 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel sm.messages['lateralManeuverPlan'].desiredCurvature = desired controls.curvature = sign*(.004 if frame < 100 else .006 if same_turn else .001 if frame < 200 else .003) sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9), lateralManeuverPlan=round(now*1e9)) - before = core.c0, core.c1 + before = core.c0, core.c1, core.correction exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) assert abs(core.c0-before[0]) <= .0400000001 and abs(core.c1-before[1]) <= .0050000001 - request_releases += core.request_release != 0. + increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01 + assert abs(core.correction-before[2]) <= abs(increment)+1e-10 msg = custom.CarControlSP.new_message() exec(publication, {'self': controls, 'CC_SP': msg}) _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9)) @@ -347,26 +347,21 @@ def test_carryover_release_through_selected_limited_request_and_actual_can(pipel packet = next(packet for packet in packets if packet[0] == address) assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) if frame == 199: - # P spends one or three slew intervals before the remaining I updates. - assert core.correction == pytest.approx(sign*(-.0198 if same_turn else .0582)) - assert core.carryover_release_count == 0 - assert core.carryover_release_count == (0 if same_turn else 1) - assert controls.ford_path_controller.diagnostics['carryover_release_count'] == core.carryover_release_count - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6' + assert sign*core.correction < 0. if same_turn else sign*core.correction > 0. + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v7' if same_turn: - assert request_releases > 0 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*(.2 if same_turn else -.2)) controls.ford_path_controller.reset() - assert core.carryover_release_count == 0 + assert core.c0 == core.c1 == core.correction == 0. @pytest.mark.parametrize('sign', [-1., 1.]) @pytest.mark.parametrize('maneuver', [False, True]) -def test_completed_unwind_release_through_selected_request_and_actual_can(pipeline, sign, maneuver): +def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, sign, maneuver): call, publication = pipeline controls, sm = startup(), Subscriptions(maneuver) controls.sm, controls.desired_curvature = sm, -sign*.02 @@ -378,7 +373,6 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli vehicle = SimpleNamespace(out=structs.CarState(vEgo=4., vEgoRaw=4.), acc_tja_status_stock_values=defaultdict(int), lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], downstream.CAN.main) - release_count = 0 for frame in range(180): now = 1.+frame*.01 desired = -sign*(.02 if frame < 30 else .01 if frame < 80 else 0.) @@ -392,12 +386,12 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) assert abs(core.c0-before[0]) <= .0400000001 and abs(core.c1-before[1]) <= .0050000001 - release_count += core.unwind_release != 0. if frame == 129: - assert sign*core.correction > .04 and core.unwind_direction == sign + assert 0. < sign*core.correction < .02 if frame == 130: - assert core.unwind_release == before[2] and core.correction == 0. - assert core.c1 == pytest.approx(before[1]-sign*.005) + # Zero measured error removes P, but does not arbitrarily erase I. + assert core.correction == before[2] + assert core.proportional == 0. msg = custom.CarControlSP.new_message() exec(publication, {'self': controls, 'CC_SP': msg}) _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, round(now*1e9)) @@ -411,8 +405,8 @@ def test_completed_unwind_release_through_selected_request_and_actual_can(pipeli assert wire['LatCtl_D2_Rq'] == 2 and wire['LatCtlPath_No_Cnt'] == frame % 16 packet = next(packet for packet in packets if packet[0] == address) assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) - assert release_count == 1 - assert controls.ford_path.path_angle == core.correction == 0. + assert controls.ford_path.path_angle == pytest.approx(core.correction, abs=.00025) + assert 0. < sign*core.correction < .02 assert controls.ford_path.path_offset == pytest.approx(sign*.05) @@ -438,7 +432,7 @@ def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerp # Match the selected request after its real upstream limiter, isolating base allocation. controls.curvature = clip_curvature(cs.vEgo, controls.desired_curvature, desired, 0.)[0] sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9)) - before = core.c0, core.c1 + before = core.c0, core.c1, core.correction exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda now=now: now)}) 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 1a64ab6f1e..0d0f7068b9 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py @@ -15,117 +15,9 @@ def tick(controller, desired, measured, **overrides): return controller.update(straight(.4), desired, **kwargs) -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_old_turn_correction_does_not_keep_c1_in_old_direction_after_reversal(sign): - controller = ModelActionController() - # Build an actual correction through feedback, rather than injecting a state. - for _ in range(100): - controller.update(straight(), sign*.004, current_curvature=sign*.004, speed=20., dt=.01) - for _ in range(100): - controller.update(straight(), sign*.004, current_curvature=sign*.001, speed=20., dt=.01) - assert controller.correction == pytest.approx(sign*.06) - # The request has reversed, but measured steering still points into the old - # turn. C0 also confirms the new direction. The existing slew permits - # crossing zero within these 0.4 seconds. - for _ in range(40): - before = controller.c1 - out = controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01) - assert abs(controller.c1-before) <= .0050000001 - assert sign*out.path_angle < 0., 'Stored old-turn correction still overrides the new C1 direction' - assert out.path_offset == pytest.approx(-sign*.2) - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('limited', [False, True]) -def test_carryover_release_uses_fresh_feedback_and_preserves_final_slew(sign, limited): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05 - kwargs = {'speed': 20., 'dt': .01, 'current_curvature': sign*.003, 'pscm_limited': limited} - controller.update(straight(-sign*.2), -sign*.001, feedback_dt=0., **kwargs) - assert controller.correction == pytest.approx(sign*.05) - before = controller.c1 - out = controller.update(straight(-sign*.2), -sign*.001, **kwargs) - assert controller.correction == 0. - assert controller.carryover_release_count == 1 - assert out.path_angle == pytest.approx(before-sign*.005) - for _ in range(30): - controller.update(straight(-sign*.2), -sign*.001, **kwargs) - assert controller.carryover_release_count == 1 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('case', ['matched', 'same_turn', 'already_turning_new_way', 'c0_opposes', 'c0_neutral', - 'c0_sub_resolution', 'target_c0_opposes', 'c1_sub_resolution', - 'neutral_request', 'neutral_measurement', 'correction_helps', 'correction_not_dominant']) -def test_carryover_release_preserves_steady_correction_and_ambiguous_requests(sign, case): - controller = ModelActionController() - offset, desired, measured, correction = -.2, -.001, .003, .05 - if case == 'matched': - measured = desired - elif case == 'same_turn': - measured = -.0005 - elif case == 'already_turning_new_way': - measured = -.003 - elif case == 'c0_opposes': - offset = .2 - elif case == 'c0_neutral': - offset = 0. - elif case == 'c0_sub_resolution': - offset = -.001 - elif case == 'c1_sub_resolution': - desired = -.000001 - elif case == 'neutral_request': - desired = 0. - elif case == 'neutral_measurement': - measured = 0. - elif case == 'correction_helps': - correction = -.05 - elif case == 'correction_not_dominant': - correction = .01 - controller.c0, controller.c1, controller.correction = sign*offset, sign*(20.*desired+correction), sign*correction - if case == 'target_c0_opposes': - offset = .2 # The old slewed C0 alone is not enough to confirm the request. - controller.update(straight(sign*offset), sign*desired, current_curvature=sign*measured, speed=20., dt=.01) - assert controller.carryover_release_count == 0 - assert abs(controller.correction-sign*correction) <= abs(desired-measured)*20.*.01+1e-10 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_carryover_release_waits_for_c0_to_finish_opposing_the_new_request(sign): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = sign*.2, sign*.03, sign*.05 - for _ in range(4): - controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01) - assert controller.carryover_release_count == 0 - for _ in range(3): - controller.update(straight(-sign*.2), -sign*.001, current_curvature=sign*.003, speed=20., dt=.01) - assert controller.carryover_release_count == 1 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_steady_opposing_correction_is_preserved_through_small_error_crossings(sign): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = -sign*.2, sign*.03, sign*.05 - for i in range(200): - measured = sign*(-.001+(-1 if i % 2 else 1)*.000001) - controller.update(straight(-sign*.2), -sign*.001, current_curvature=measured, speed=20., dt=.01) - assert controller.carryover_release_count == 0 - assert controller.correction == pytest.approx(sign*.05) - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('offset', [.009999, .01, .010001]) -@pytest.mark.parametrize('heading', [.0004999, .0005, .0005001]) -def test_carryover_direction_confirmation_uses_existing_dbc_steps(sign, offset, heading): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = -sign*offset, sign*.03, sign*.05 - controller.update(straight(-sign*offset), -sign*heading/20., current_curvature=sign*.003, speed=20., dt=.01) - assert controller.carryover_release_count == int(offset >= .01 and heading >= .0005) - - @pytest.mark.parametrize('sign', [-1., 1.]) def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign): - controller, matched = ModelActionController(), ModelActionController() + controller, matched = ModelActionController(proportional_gain=0., integral_gain=1.), ModelActionController(proportional_gain=0., integral_gain=1.) for _ in range(100): tick(controller, sign*.004, sign*.004) for _ in range(100): @@ -147,7 +39,7 @@ def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign): @pytest.mark.parametrize('sign', [-1., 1.]) def test_amplitude_and_slew_limits_do_not_store_unavailable_feedback(sign): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) # The unchanged model request is already ahead of the output slew. for _ in range(10): tick(controller, sign*.01, 0.) @@ -169,7 +61,7 @@ def test_amplitude_and_slew_limits_do_not_store_unavailable_feedback(sign): @pytest.mark.parametrize('sign', [-1., 1.]) def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) for _ in range(100): tick(controller, sign*.004, sign*.004) for _ in range(100): @@ -186,7 +78,7 @@ def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign): @pytest.mark.parametrize('sign', [-1., 1.]) def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) controller.correction = -sign*.02 controller.c1 = sign*.06 for _ in range(200): @@ -196,7 +88,7 @@ def test_pscm_limit_cannot_trap_old_correction_below_the_model_request(sign): def test_driver_intervention_clears_feedback_through_existing_output_slew(): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) for _ in range(100): tick(controller, .004, .004) for _ in range(100): @@ -216,7 +108,7 @@ def test_driver_intervention_clears_feedback_through_existing_output_slew(): ('current_curvature', 1.01), ('feedback_dt', math.nan), ('feedback_dt', -.001), ('feedback_dt', .151), ('active', False)]) def test_bad_feedback_inputs_and_disengagement_clear_every_control_state(field, value): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) controller.correction = .03 out = tick(controller, .004, .003, **{field: value}) assert out == FordPath() @@ -237,7 +129,7 @@ def status(now, **overrides): def test_repeated_steering_samples_only_advance_output_slew(): - controller = FordModelActionController() + controller = FordModelActionController(proportional_gain=0., integral_gain=1.) for i in range(100): adapter_tick(controller, 1.+i*.01, current_curvature=.004) before = controller.core.correction @@ -255,7 +147,7 @@ def test_repeated_steering_samples_only_advance_output_slew(): {'pscm_status': status(2.01, denied=True)}, {'pscm_status': status(2.01, lateralState=1)}]) def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides): - controller = FordModelActionController() + controller = FordModelActionController(proportional_gain=0., integral_gain=1.) for i in range(101): adapter_tick(controller, 1.+i*.01) assert controller.core.correction > 0. @@ -268,7 +160,7 @@ def test_adapter_clears_feedback_when_driver_or_pscm_overrides(overrides): ({'canMonoTime': 0}, False), ({'canMonoTime': 1_800_000_000}, False), ({'canMonoTime': 2_020_000_000}, False), ({'limit': 1}, False)]) def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, limited): - controller = FordModelActionController() + controller = FordModelActionController(proportional_gain=0., integral_gain=1.) for i in range(100): adapter_tick(controller, 1.+i*.01, current_curvature=.004) adapter_tick(controller, 2., pscm_status=status(2., **{'limit': 2, **overrides})) @@ -279,7 +171,7 @@ def test_only_fresh_reached_pscm_limit_blocks_outward_integration(overrides, lim def test_measurement_cadence_preserves_elapsed_distance_integration(): results = [] for period in (1, 2, 5): - controller = FordModelActionController() + controller = FordModelActionController(proportional_gain=0., integral_gain=1.) for i in range(101): now = 1.+i*.01 adapter_tick(controller, now, current_curvature=.004) 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 b0432f9f47..d83103ee86 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py @@ -59,7 +59,7 @@ def test_extra_offset_releases_at_existing_slew_without_stored_overflow(sign): @pytest.mark.parametrize('sign', [-1., 1.]) def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign): - controller = ModelActionController() + controller = ModelActionController(proportional_gain=0., integral_gain=1.) 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) @@ -77,14 +77,3 @@ def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, l assert out.path_offset == pytest.approx(sign*.9) assert out.path_angle == pytest.approx(sign*.5) assert controller.correction == 0. - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_overflow_cannot_replace_model_centering_confirmation_for_carryover_release(sign): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = sign*.6, -sign*.1, -sign*.6 - for _ in range(20): - controller.update(straight(-sign*.1), sign*.03, current_curvature=-sign*.002, speed=20., dt=.01) - assert sign*controller.c0 > 0. # Overflow agrees with heading; model centering still opposes it. - assert controller.carryover_release_count == 0 - assert sign*controller.correction < -.4 diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py deleted file mode 100644 index b6a811f78c..0000000000 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_request_release.py +++ /dev/null @@ -1,129 +0,0 @@ -"""Changed requests should not wait for obsolete C1 correction to integrate away. - -These exercise command delay, not a simulated PSCM or predicted wheel angle. -""" -import pytest - -from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController -from openpilot.selfdrive.controls.tests.test_ford_model_action import straight - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('unwind', [False, True]) -def test_changed_request_retires_opposing_correction_within_output_slew(sign, unwind): - controller = ModelActionController() - desired = sign*.035 - model = straight(sign*.4) - for _ in range(100): - controller.update(model, desired, current_curvature=desired, speed=4., dt=.01) - # Build old unwind (or turn-in) correction while retaining the same request. - old_error = sign*(.01 if unwind else -.01) - for _ in range(50): - controller.update(model, desired, current_curvature=desired-old_error, speed=4., dt=.01) - assert controller.correction == pytest.approx(old_error*2.) - # The model now moves in the opposite direction to the stored correction. - # Actual steering is on the other side of the new target: the old correction - # is delaying exactly the response now needed. Both remain in the same turn. - changed = desired-old_error - before = controller.c1 - for _ in range(20): - out = controller.update(model, changed, current_curvature=desired, speed=4., dt=.01) - assert abs(controller.c1-before) <= .100000001 - assert out.path_offset == pytest.approx(sign*.4) - assert (out.path_angle-7.*changed)*old_error <= 1e-10, 'Old correction still opposes the changed model request' - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('case', ['unchanged', 'matched', 'still_short', 'helpful', 'small_error', 'small_change', 'speed_only', 'clipped_base']) -def test_request_release_preserves_correction_without_confirming_evidence(sign, case): - controller = ModelActionController() - previous, desired, measured, correction, speed = .035, .025, .035, .02, 4. - if case == 'unchanged': - desired = previous - measured = .05 - elif case == 'matched': - measured = desired - elif case == 'still_short': - measured = .015 - elif case == 'helpful': - correction = -.02 - elif case == 'small_error': - measured = desired+.00001 - elif case == 'small_change': - desired, measured = previous-.00001, .05 - elif case == 'speed_only': - desired, measured, speed = previous, .05, 10. - elif case == 'clipped_base': - previous, desired, measured = .1, .09, .11 - controller.c0, controller.c1, controller.correction = sign*.4, sign*.2, sign*correction - controller.last_feedback_desired = sign*previous - controller.update(straight(sign*.4), sign*desired, current_curvature=sign*measured, speed=speed, dt=.01) - assert controller.request_release == 0. - assert abs(controller.correction-sign*correction) <= abs(desired-measured)*speed*.01+1e-10 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_small_request_change_cannot_erase_the_entire_correction(sign): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02 - controller.last_feedback_desired = sign*.035 - controller.update(straight(sign*.4), sign*.034, current_curvature=sign*.05, speed=4., dt=.01) - assert sign*controller.correction > .01 # Most of the original 0.02 rad must survive. - assert 0. < -sign*controller.request_release <= .0070000001 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -def test_small_target_and_measurement_noise_cannot_erase_steady_correction(sign): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = sign*.4, sign*.265, sign*.02 - controller.last_feedback_desired = sign*.035 - for i in range(1000): - desired = sign*(.035+(.00001 if i % 2 else -.00001)) - measured = sign*.035 - controller.update(straight(sign*.4), desired, current_curvature=measured, speed=4., dt=.01) - assert controller.request_release == 0. - assert controller.correction == pytest.approx(sign*.02) - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('limited', [False, True]) -def test_retiring_correction_at_pscm_limit_never_builds_extra_turn_demand(sign, limited): - controller = ModelActionController() - controller.c0, controller.c1, controller.correction = sign*.4, sign*.225, -sign*.02 - controller.last_feedback_desired = sign*.035 - controller.update(straight(sign*.4), sign*.045, current_curvature=sign*.035, speed=4., dt=.01, pscm_limited=limited) - assert controller.correction == 0. # Slew/limit still prevents a new outward increment here. - assert controller.request_release == pytest.approx(sign*.02) - assert controller.c1 == pytest.approx(sign*.23) - - -def test_adapter_retires_request_once_per_fresh_measurement_and_reset_clears_history(): - controller = FordModelActionController() - def tick(now, desired, measured, stamp, **kwargs): - return controller.update(straight(.4), desired, current_curvature=measured, speed=4., yaw_rate=0., now=now, - measurement_time=stamp, model_time=now, reference_time=now, active=True, **kwargs) - for i in range(150): - now = 1.+i*.01 - tick(now, .035, .035 if i < 100 else .025, now) - before = controller.core.correction - tick(2.50, .025, .035, 2.49) - assert controller.core.correction == before - assert controller.core.last_feedback_desired == .035 - assert controller.diagnostics['request_release'] == 0. - tick(2.51, .025, .035, 2.51) - assert controller.diagnostics['request_release'] == pytest.approx(-before) - assert controller.core.last_feedback_desired == .025 - tick(2.52, .025, .035, 2.51) - assert controller.diagnostics['request_release'] == 0. - tick(2.53, .025, .035, 2.53, valid=False) - assert controller.core.last_feedback_desired is None - assert controller.core.request_release == 0. - - -def test_driver_override_clears_correction_and_cannot_leave_a_pending_release(): - controller = ModelActionController() - controller.correction, controller.last_feedback_desired = .02, .035 - controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01, feedback_enabled=False) - assert controller.correction == controller.request_release == 0. - controller.update(straight(.4), .025, current_curvature=.035, speed=4., dt=.01) - assert controller.request_release == 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 724d129979..09185dfcc2 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -9,7 +9,7 @@ import pytest from opendbc.car.ford.values import CAR, FordFlags from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType -from openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, select_model_action_controller +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 @@ -46,8 +46,9 @@ def test_actual_startup_priority(candidate, observer, fingerprint): selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=settings.__getitem__)) if candidate: assert type(selected.ford_path_controller) is FordModelActionController - assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .25 - assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v6' + assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .50 + assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25 + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-c1-pi-v7' else: assert selected.ford_path_controller is None assert selected.ford_model_action == candidate diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py index 1417a03266..45b048a071 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_unwind.py @@ -1,127 +1,78 @@ -"""Unwind correction must not become a new turn after a confirmed catch-up. +import math -These reproduce controller memory, not the PSCM's physical steering response. -""" import pytest -from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController from openpilot.selfdrive.controls.tests.test_ford_model_action import straight +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController -def unwind(controller, sign): - for _ in range(30): - controller.update(straight(-sign*.5), -sign*.02, current_curvature=-sign*.02, speed=4., dt=.01) - # Selected curvature relaxes, but the wheel is still too far into the old - # turn. Feedback builds the same opposite-direction correction seen in 115. +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_existing_integral_can_unwind_while_output_slews(sign): + core = ModelActionController(.5, .25) + core.c1, core.correction = sign*.07, sign*.03 + core.update(straight(), sign*.002, current_curvature=sign*.004, speed=20., dt=.01) + assert core.correction == pytest.approx(sign*.0299) + assert core.c1 == pytest.approx(sign*.065) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_unwinding_cannot_charge_opposite_correction_through_a_slew_limit(sign): + core = ModelActionController(.5, .25) + core.c1, core.correction = sign*.07, sign*.03 + core.update(straight(), 0., current_curvature=sign*.5, speed=20., dt=.01, feedback_dt=.15) + assert core.correction == 0. + assert core.c1 == pytest.approx(sign*.065) + + +@pytest.mark.parametrize('ki', [0., .25, .5, 1.]) +def test_integral_gain_scales_fresh_error_only(ki): + core = ModelActionController(0., ki) + core.c1 = .04 + core.update(straight(), .002, current_curvature=.001, speed=20., dt=.01) + assert core.correction == pytest.approx(ki*.0002) + before = core.correction + core.update(straight(), 0., current_curvature=.001, speed=20., dt=.01, feedback_dt=0.) + assert core.correction == before + + +def test_zero_error_does_not_erase_holding_correction(): + core = ModelActionController(.5, .25) + core.c1, core.correction = .14, .1 for _ in range(50): - controller.update(straight(-sign*.5), -sign*.01, current_curvature=-sign*.04, speed=4., dt=.01) - assert sign*controller.correction > .04 - assert controller.unwind_direction == sign - for _ in range(30): - controller.update(straight(sign*.05), 0., current_curvature=-sign*.01, speed=4., dt=.01) - assert sign*controller.correction > .04 + core.update(straight(), .002, current_curvature=.002, speed=20., dt=.01) + assert core.correction == .1 + + +@pytest.mark.parametrize('ki', [-1., math.inf, math.nan]) +def test_invalid_integral_gain_is_rejected(ki): + with pytest.raises(ValueError): + ModelActionController(.25, ki) + + +def test_overflowing_integral_increment_resets(): + core = ModelActionController(.25, 1e308) + assert not core.update(straight(), 1., current_curvature=0., speed=55., dt=.01).valid + assert core.c0 == core.c1 == core.correction == 0. @pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('requested', [0., .0001]) -def test_completed_unwind_does_not_keep_requesting_a_new_turn(sign, requested): - controller = ModelActionController() - unwind(controller, sign) - before = controller.c1 - # The model path now confirms the unwind direction and actual steering has - # caught the near-zero/new-direction request. Allow the original output slew - # to finish; the old unwind correction must no longer prop up C1. - for _ in range(20): - out = controller.update(straight(sign*.05), sign*requested, current_curvature=sign*requested, speed=4., dt=.01) - assert abs(controller.c1-before) <= .100000001 - assert out.path_offset == pytest.approx(sign*.05) - assert controller.correction == 0., 'Stored unwind correction remains after catch-up' - assert abs(out.path_angle-7.*sign*requested) <= .000500001 - assert controller.unwind_direction == 0. +def test_centering_cannot_gate_continuous_heading_correction(sign): + commands = [] + for offset in (-.1, -.010001, -.01, -.009999, 0., .009999, .01, .010001, .1): + core = ModelActionController() + core.c0, core.c1, core.correction = offset, sign*.07, sign*.03 + out = core.update(straight(offset), sign*.002, current_curvature=sign*.004, speed=20., dt=.01) + commands.append((out.path_angle, core.correction)) + assert len(set(commands)) == 1 @pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('case', ['not_caught', 'old_side_request', 'model_c0_old', 'model_c0_neutral', 'slewed_c0_old', - 'correction_not_dominant', 'opposite_correction', 'no_unwind', 'duplicate']) -def test_unwind_release_requires_catchup_and_confirmed_new_direction(sign, case): - controller = ModelActionController() - unwind(controller, sign) - desired = measured = 0. - offset, feedback_dt = sign*.05, .01 - if case == 'not_caught': - measured = -sign*.005 - elif case == 'old_side_request': - desired = measured = -sign*.001 - elif case == 'model_c0_old': - offset = -sign*.05 - elif case == 'model_c0_neutral': - offset = 0. - elif case == 'slewed_c0_old': - controller.c0 = -sign*.5 - elif case == 'correction_not_dominant': - desired = measured = sign*.02 - elif case == 'opposite_correction': - controller.correction = -sign*.01 - elif case == 'no_unwind': - controller.unwind_direction = 0. - elif case == 'duplicate': - feedback_dt = 0. - before = controller.correction - controller.update(straight(offset), desired, current_curvature=measured, speed=4., dt=.01, feedback_dt=feedback_dt) - assert controller.unwind_release == 0. - assert abs(controller.correction-before) <= abs(desired-measured)*4.*feedback_dt+1e-10 - - -@pytest.mark.parametrize('sign', [-1., 1.]) -@pytest.mark.parametrize('desired', [0., .004]) -def test_steady_tracking_correction_survives_matched_steering_and_noise(sign, desired): - controller = ModelActionController() - controller.correction, controller.c1, controller.c0 = sign*.05, sign*(7.*desired+.05), sign*.05 - for i in range(300): - measured = sign*(desired+(1 if i % 2 else -1)*.000001) - controller.update(straight(sign*.05), sign*desired, current_curvature=measured, speed=4., dt=.01) - assert controller.unwind_direction == controller.unwind_release == 0. - assert controller.correction == pytest.approx(sign*.05) - - @pytest.mark.parametrize('limited', [False, True]) -def test_catchup_release_uses_existing_slew_and_is_consumed_once(limited): - controller = ModelActionController() - unwind(controller, 1.) - before, correction = controller.c1, controller.correction - controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01, pscm_limited=limited) - assert controller.unwind_release == correction - assert controller.c1 == pytest.approx(before-.005) - assert controller.correction <= 0. # Remaining feedback can only correct the overshoot. - assert controller.unwind_direction == 0. - controller.update(straight(.05), 0., current_curvature=.001, speed=4., dt=.01) - assert controller.unwind_release == 0. - - -@pytest.mark.parametrize('override', ['driver', 'invalid', 'inactive']) -def test_unwind_history_is_cleared_by_override_and_reset(override): - controller = ModelActionController() - unwind(controller, 1.) - kwargs = {'driver': {'feedback_enabled': False}, 'invalid': {'valid': False}, 'inactive': {'active': False}}[override] - controller.update(straight(.05), 0., current_curvature=0., speed=4., dt=.01, **kwargs) - assert controller.unwind_direction == controller.unwind_release == controller.correction == 0. - - -def test_duplicate_adapter_measurement_cannot_consume_unwind_until_fresh_catchup(): - controller = FordModelActionController() - for frame in range(130): - now = 1.+frame*.01 - desired = -.02 if frame < 30 else -.01 if frame < 80 else 0. - measured = -.02 if frame < 30 else -.04 if frame < 80 else -.01 - controller.update(straight(-.5 if frame < 80 else .05), desired, current_curvature=measured, - speed=4., yaw_rate=0., now=now, measurement_time=now, model_time=now, reference_time=now, active=True) - assert controller.core.unwind_direction == 1. - before = controller.core.correction - for now, stamp in [(2.30, 2.29), (2.31, 2.31), (2.32, 2.31)]: - controller.update(straight(.05), 0., current_curvature=0., speed=4., yaw_rate=0., now=now, - measurement_time=stamp, model_time=now, reference_time=now, active=True) - if now == 2.30: - assert controller.core.correction == before and controller.core.unwind_direction == 1. - else: - assert controller.core.correction == controller.core.unwind_direction == 0. - assert controller.diagnostics['unwind_release'] == (before if now == 2.31 else 0.) +def test_duplicate_measurements_cannot_retire_integral(sign, limited): + core = ModelActionController() + 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*.065) diff --git a/tools/ford_pscm_lab/feedback_replay.py b/tools/ford_pscm_lab/feedback_replay.py index 3c39f9fced..b70abae2c9 100644 --- a/tools/ford_pscm_lab/feedback_replay.py +++ b/tools/ford_pscm_lab/feedback_replay.py @@ -16,7 +16,7 @@ from types import ModuleType, SimpleNamespace import numpy as np from openpilot.selfdrive.controls.lib import ford_model_action -from openpilot.selfdrive.controls.lib.ford_model_action import C1_PROPORTIONAL_GAIN, FordModelActionController, ModelActionController +from openpilot.selfdrive.controls.lib.ford_model_action import C1_INTEGRAL_GAIN, C1_PROPORTIONAL_GAIN, FordModelActionController from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency @@ -89,7 +89,7 @@ def replay(directory, output, baseline_revision=BASELINE): feedback = {'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i], 'pscm_status': status} previous = old.update(model, c['desired'][i], **common, **(feedback if old_has_feedback else {})) - previous_count, previous_correction = controller.core.carryover_release_count, controller.core.correction + previous_count, previous_correction = getattr(controller.core, 'carryover_release_count', 0), controller.core.correction command = controller.update(model, c['desired'][i], **feedback, **common) for destination, result in ((baseline, previous), (commands, command)): destination[i] = result.path_offset, result.path_angle, result.curvature, result.curvature_rate @@ -111,7 +111,7 @@ def replay(directory, output, baseline_revision=BASELINE): 'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']), 'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]), 'baseline_c0_c1': baseline[i, :2].tolist(), 'candidate_c0_c1': commands[i, :2].tolist()}) - if controller.core.carryover_release_count > previous_count: + if getattr(controller.core, 'carryover_release_count', 0) > previous_count: releases.append({'time_s': float(now-metadata['t0']), 'correction_before_rad': float(previous_correction), 'correction_after_rad': float(correction[i]), 'base_c1_rad': float(d['heading_feedforward']), 'desired_angle_deg': float(c['desired_angle'][i]), 'actual_angle_deg': float(c['actual_angle'][i]), @@ -124,7 +124,8 @@ def replay(directory, output, baseline_revision=BASELINE): assert np.all(abs(correction) <= 1.+1e-10) weight = np.minimum(np.diff(t, append=t[-1]+.01), .03) report = {'scope': __doc__, 'baseline_revision': baseline_revision, 'baseline_source_sha256': baseline_hash, - 'proportional_gain': C1_PROPORTIONAL_GAIN, + 'proportional_gain': C1_PROPORTIONAL_GAIN, 'integral_gain': C1_INTEGRAL_GAIN, + 'hypothesis': controller.hypothesis, 'calibration_approved': False, 'cycles': len(t), 'active_cycles': int(valid.sum()), 'validity_matches_baseline_exactly': True, 'status_counts': dict(reasons), 'c0_matches_baseline_exactly': bool(np.array_equal(commands[:, 0], baseline[:, 0])), @@ -170,7 +171,10 @@ def replay(directory, output, baseline_revision=BASELINE): def stress(cycles, output): verify_dependency(OPENDBC) rng = np.random.default_rng(20260913) - controller, mirror, wire = ModelActionController(), ModelActionController(), WireCheck() + # This historical stress compares v5's conditional release with v4. + candidate_revision = '22d188776cb557acea459a1fca70812bdb2df46c' + candidate, candidate_hash = original_controller(candidate_revision) + controller, mirror, wire = candidate.core, type(candidate.core)(), WireCheck() old, baseline_hash = original_controller(FEEDBACK_V4) releases = 0 request_releases = 0 @@ -263,10 +267,11 @@ def stress(cycles, output): 'request_release_cycles': request_releases, 'unwind_release_cycles': unwind_releases, 'exact_unchanged_state_and_commands_without_unwind_release': unchanged_without_unwind_release, + 'candidate_revision': candidate_revision, 'candidate_source_sha256': candidate_hash, 'exact_v4_match_after_only_declared_retirement': cycles, 'checks': 'Symmetry, resets, amplitude/slew, bounded retirement, carryover confirmation, integration, PSCM limits, CAN.', 'scope': 'Numerical software invariants only; no model of vehicle motion.', 'calibration_approved': False, - 'controller_sha256': hashlib.sha256(Path(ford_model_action.__file__).read_bytes()).hexdigest()} + 'controller_sha256': candidate_hash} output.parent.mkdir(parents=True, exist_ok=True) output.write_text(json.dumps(report, indent=2)+'\n') print(json.dumps(report, indent=2)) diff --git a/tools/ford_pscm_lab/minimal_pi_production_stress.py b/tools/ford_pscm_lab/minimal_pi_production_stress.py new file mode 100644 index 0000000000..5a92a86204 --- /dev/null +++ b/tools/ford_pscm_lab/minimal_pi_production_stress.py @@ -0,0 +1,83 @@ +"""Independent arithmetic and C0-independence stress for production PI.""" +import argparse +import hashlib +import json +import math +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib import ford_model_action +from tools.ford_pscm_lab.model_action_replay import WireCheck +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController + + +def stress(cycles, output): + wire = WireCheck() + kp, ki = .5, .25 + rng = np.random.default_rng(20260913) + core, mirror, other = (ModelActionController() for _ in range(3)) + for i in range(cycles): + desired, measured = rng.uniform(-.1, .1, 2) + speed, dt, offset, alternate = rng.uniform(.3, 55.), rng.uniform(.002, .1), *rng.uniform(-8., 8., 2) + active, enabled, limited = i % 211 != 0, i % 97 != 0, i % 7 == 0 + feedback_dt = 0. if i % 5 == 0 else rng.uniform(.002, .15) + before = core.c0, core.c1, core.correction + args = {'speed': speed, 'dt': dt, 'feedback_dt': feedback_dt, 'active': active, + 'feedback_enabled': enabled, 'pscm_limited': limited} + + def model(y): + return SimpleNamespace(position=SimpleNamespace(x=[0., 20.], y=[y, y]), orientation=SimpleNamespace(z=[0., 0.])) + + outputs = [core.update(model(offset), desired, current_curvature=measured, **args), + mirror.update(model(-offset), -desired, current_curvature=-measured, **args), + other.update(model(alternate), desired, current_curvature=measured, **args)] + state = np.array([core.c0, core.c1, core.correction, core.proportional]) + np.testing.assert_allclose(state, -np.array([mirror.c0, mirror.c1, mirror.correction, mirror.proportional]), atol=1e-10, rtol=0.) + assert (core.c1, core.correction, core.proportional) == (other.c1, other.correction, other.proportional) + if active: + distance = max(7., speed) + base = min(.5, max(-.5, distance*desired)) + target_c0 = min(5.11, max(-5.11, offset+7.*(distance*desired-base))) + assert abs(core.c0-(before[0]+min(4.*dt, max(-4.*dt, target_c0-before[0])))) <= 1e-10 + p = kp*distance*(desired-measured) if enabled else 0. + integral = 0. + if enabled: + increment = ki*(desired-measured)*speed*feedback_dt + direction = measured if measured else before[1] + if limited and increment*direction > 0.: + increment = min(max(increment, min(-before[2], 0.)), max(-before[2], 0.)) + integral = before[2] + if increment*integral < 0.: + cancel = math.copysign(min(abs(increment), abs(integral)), increment) + integral += cancel + increment -= cancel + lower, upper = max(-.5, before[1]-.5*dt), min(.5, before[1]+.5*dt) + request = base+p+integral + integral += min(max(increment, min(lower-request, 0.)), max(upper-request, 0.)) + assert core.proportional == p and abs(core.correction-integral) <= 1e-10 + request = min(.5, max(-.5, base+p+integral)) + assert abs(core.c1-(before[1]+min(.5*dt, max(-.5*dt, request-before[1])))) <= 1e-10 + assert abs(core.c0) <= 5.11+1e-10 and abs(core.c1) <= .5+1e-10 and abs(core.correction) <= 1.+1e-10 + else: + assert np.all(state == 0.) + for command in outputs: + assert command.curvature == command.curvature_rate == 0. + wire.check(command) + result = {'cycles': cycles, 'kp': kp, 'ki': ki, 'controller_updates': 3*cycles, + 'can_round_trips': wire.count, 'seed': 20260913, 'independent_c0_comparisons': cycles, + 'checks': 'Independent scalar PI/unwind-first/anti-windup arithmetic, mirror symmetry, exact C0 independence, limits, slew, resets, CAN.', + 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in + (Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}} + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(json.dumps(result, indent=2)+'\n') + print(json.dumps(result)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--cycles', type=int, default=20_000) + parser.add_argument('--output', type=Path, required=True) + args = parser.parse_args() + stress(args.cycles, args.output) diff --git a/tools/ford_pscm_lab/minimal_pi_validate.py b/tools/ford_pscm_lab/minimal_pi_validate.py new file mode 100644 index 0000000000..23606418ff --- /dev/null +++ b/tools/ford_pscm_lab/minimal_pi_validate.py @@ -0,0 +1,109 @@ +"""Verify production commands against the archived P=.50/I=.25 offline candidate. + +Fixed recorded motion verifies integration parity, not physical tracking. +""" +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 openpilot.selfdrive.controls.lib import ford_model_action +from openpilot.selfdrive.controls.lib.ford_model_action import select_model_action_controller +from opendbc.car.ford.values import FordFlags +from tools.ford_pscm_lab.feedback_replay import OPENDBC +from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency + + +ROUTES = ('112', '113', '114', '115', '116', '117', 'a0', 'a2', 'a5', 'a9', 'b8', 'b9', 'ca', 'raptor02') +SELECTED = 4 + + +def replay(label, output): + directory = Path('.cache/ford_raptor_route02' if label == 'raptor02' else f'.cache/ford_route{label}').resolve() + previous = Path('.cache/ford_minimal_tuning/sweep')/label + output = (output/label).resolve() + if output == directory or directory in output.parents or output == previous.resolve(): + raise ValueError('Output must preserve previous experiments and input extracts') + verify_dependency(OPENDBC) + archived_report = json.loads((previous/'report.json').read_text()) + assert archived_report['settings'][SELECTED] == ['unwind_p50_i25', .5, .25, True] + for filename in ('route.npz', 'model_paths.npz', 'metadata.json'): + path = directory/filename + assert hashlib.sha256(path.read_bytes()).hexdigest() == archived_report['source_sha256'][str(path)] + archived = dict(np.load(previous/'commands.npz', allow_pickle=False)) + metadata = json.loads((directory/'metadata.json').read_text()) + with np.load(directory/'route.npz', allow_pickle=False) as raw: + r = {key: table(raw, key) for key in ('controls', 'cs', 'cc', 'model', 'params', 'pscm')} + if 'maneuver' in raw and len(raw['maneuver']): + raise ValueError('Maneuver reference reconstruction is not available in this sweep') + with np.load(directory/'model_paths.npz', allow_pickle=False) as raw: + model_ns, paths = raw['ns'], raw['paths'] + c, t = r['controls'], r['controls']['t'] + assert all(np.all(np.diff(stream['t']) >= 0.) for stream in r.values()) + np.testing.assert_allclose(archived['t'], t-metadata['t0'], rtol=0., atol=1e-8) + cs, pa, ps = (sample(r[key], t) for key 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] + cp = SimpleNamespace(brand='ford', flags=int(FordFlags.CANFD)) + assert select_model_action_controller(cp, False) is None + controller = select_model_action_controller(cp, True) + assert controller is not None + assert (controller.core.proportional_gain, controller.core.integral_gain) == (.5, .25) + shape = (len(t),) + commands = np.zeros((*shape, 4)) + valid = np.zeros(shape, bool) + names = ('heading_proportional', 'heading_correction', 'heading_feedforward', 'feedback_enabled', 'offset_overflow', 'pscm_limited') + diagnostics = {name: np.zeros(shape) for name in names} + wire = WireCheck() + for i, now in enumerate(t): + model_time = r['model']['t'][mi[i]] + services_valid = bool(c['valid'][i] and cc['valid'][i] and cs['valid'][i] and cs['can_valid'][i] + 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) + model = models[mi[i]] if exact[i] else None + 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': model_time, 'reference_time': model_time, 'active': bool(cc['active'][i]), 'valid': services_valid, + 'current_curvature': c['measured'][i], 'driver_pressed': bool(cs['pressed'][i]), + 'driver_torque': cs['torque'][i], 'pscm_status': status} + command = controller.update(model, c['desired'][i], **kwargs) + commands[i] = command.path_offset, command.path_angle, command.curvature, command.curvature_rate + valid[i] = command.valid + for name in names: + diagnostics[name][i] = controller.diagnostics.get(name, 0.) + wire.check(command) + field_checks(commands, valid, t) + np.testing.assert_array_equal(commands, archived['commands'][SELECTED]) + np.testing.assert_array_equal(valid, archived['valid'][SELECTED]) + for name in names: + np.testing.assert_array_equal(diagnostics[name], archived[name][SELECTED]) + output.mkdir(parents=True, exist_ok=True) + sources = (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json', + previous/'report.json', previous/'commands.npz', Path(__file__).resolve(), Path(ford_model_action.__file__).resolve()) + result = {'scope': __doc__, 'route': label, 'cycles': len(t), 'can_round_trips': wire.count, + 'production_matches_archived_trial_exactly': True, 'opendbc_revision': OPENDBC, + 'hypothesis': controller.hypothesis, 'kp': .5, 'ki': .25, + 'source_sha256': {str(path.resolve()): hashlib.sha256(path.read_bytes()).hexdigest() for path in sources}} + (output/'report.json').write_text(json.dumps(result, indent=2, allow_nan=False)+'\n') + return result + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--routes', nargs='+', choices=ROUTES, default=ROUTES) + parser.add_argument('--output', type=Path, required=True) + 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} + for job in as_completed(jobs): + result = job.result() + print(json.dumps({'completed': jobs[job], 'cycles': result['cycles'], 'can_checks': result['can_round_trips']}), flush=True) diff --git a/tools/ford_pscm_lab/pi_replay.py b/tools/ford_pscm_lab/pi_replay.py index 2d75a1e34f..130e2e4ae5 100644 --- a/tools/ford_pscm_lab/pi_replay.py +++ b/tools/ford_pscm_lab/pi_replay.py @@ -12,13 +12,12 @@ 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.feedback_replay import OPENDBC, original_controller from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, table, verify_dependency BASELINE = '22d188776cb557acea459a1fca70812bdb2df46c' +CANDIDATE = 'bf00bc691def830e1beb15363d05416714c1dc42' GAINS = (0., .1, .25, .5) DELAYS = (0., .2, .4) @@ -42,7 +41,8 @@ def replay(directory, output): 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] settings = [(gain, delay) for delay in DELAYS for gain in GAINS] - controllers = [FordModelActionController(proportional_gain=gain) for gain, _ in settings] + candidate, candidate_hash = original_controller(CANDIDATE) + controllers = [type(candidate)(proportional_gain=gain) for gain, _ in settings] reference, reference_hash = original_controller(BASELINE) delayed = {} for delay in DELAYS: @@ -91,12 +91,13 @@ def replay(directory, output): assert np.all(integral[k, ~enabled[k]] == 0.) assert np.all(abs(integral[k]) <= 1.+1e-10) report = {'scope': __doc__, 'baseline_revision': BASELINE, 'baseline_source_sha256': reference_hash, + 'candidate_revision': CANDIDATE, 'candidate_source_sha256': candidate_hash, 'cycles': len(t), 'candidate_updates': len(settings)*len(t), 'can_round_trips': wire.count, 'zero_gain_zero_delay_matches_v5_exactly': True, 'all_c0_and_activation_match_exactly': True, 'calibration_approved': False, 'settings': [], 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in (directory/'route.npz', directory/'model_paths.npz', directory/'metadata.json', - Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}, + Path(__file__).resolve())}, 'limitations': ['Recorded model, steering, driver and PSCM inputs stay fixed; no tracking improvement score.', '0.2/0.4 s are diagnostic timing alternatives, not fitted or validated PSCM delays.', 'Gain sweep does not identify a physically optimal or stable gain.', diff --git a/tools/ford_pscm_lab/pi_stress.py b/tools/ford_pscm_lab/pi_stress.py index c1d127e0b0..bf312087a7 100644 --- a/tools/ford_pscm_lab/pi_stress.py +++ b/tools/ford_pscm_lab/pi_stress.py @@ -7,19 +7,18 @@ 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 ModelActionController from tools.ford_pscm_lab.feedback_replay import OPENDBC, original_controller from tools.ford_pscm_lab.model_action_replay import WireCheck, verify_dependency -from tools.ford_pscm_lab.pi_replay import BASELINE +from tools.ford_pscm_lab.pi_replay import BASELINE, CANDIDATE def stress(cycles, gain, output): verify_dependency(OPENDBC) rng = np.random.default_rng(20260913) - controller = ModelActionController(proportional_gain=gain) - mirror = ModelActionController(proportional_gain=gain) - zero = ModelActionController() + candidate, candidate_hash = original_controller(CANDIDATE) + controller = type(candidate.core)(proportional_gain=gain) + mirror = type(candidate.core)(proportional_gain=gain) + zero = type(candidate.core)() original, original_hash = original_controller(BASELINE) wire = WireCheck() release_count = 0 @@ -75,12 +74,13 @@ def stress(cycles, gain, output): wire.check(out) report = {'cycles': cycles, 'gain': gain, 'seed': 20260913, 'mirrored_updates': cycles, 'zero_gain_exact_v5_comparisons': cycles, 'can_round_trips': wire.count, + 'candidate_revision': CANDIDATE, 'candidate_source_sha256': candidate_hash, 'baseline_revision': BASELINE, 'baseline_source_sha256': original_hash, 'release_cycles': release_count, 'calibration_approved': False, 'checks': 'Independent scalar PI arithmetic, combined anti-windup, mirror symmetry, slew/amplitude, driver/PSCM gates, resets, zero-P v5 parity and CAN.', 'scope': __doc__, 'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in - (Path(__file__).resolve(), Path(ford_model_action.__file__).resolve())}} + (Path(__file__).resolve(),)}} output.parent.mkdir(parents=True, exist_ok=True) output.write_text(json.dumps(report, indent=2)+'\n') print(json.dumps(report, indent=2))