diff --git a/CREDITS.md b/CREDITS.md index 3ad41cef66..c7d9dc78b9 100644 --- a/CREDITS.md +++ b/CREDITS.md @@ -12,15 +12,9 @@ The copied MRR35 generator source and DBC definition are at `opendbc_repo/opendb ## Ford support adapted from BluePilot -The Ford lateral strategies, extended CAN constructors, camera-radar filtering and native checks are adapted from StarPilot `2efe6017bfe14cee4807833e05f9a7f6ba255cdb`. Their lineage includes BluePilot's `bp-7.0` Ford work, principally by Alan Polk, with contributions from John Christman, Jacob Neulight, tonesto7, Haibin Wen and other sunnypilot and BluePilot contributors. The reference snapshot is [`e1d051d7ba270261b4455068bd68f1a58db15a4a`](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a); the exact checkout used for the earlier StarPilot import was not recorded. +The Ford CAN FD camera-lead radar filtering is adapted from StarPilot `2efe6017bfe14cee4807833e05f9a7f6ba255cdb`. Its lineage includes BluePilot's `bp-7.0` Ford work, principally by Alan Polk, with contributions from John Christman, Jacob Neulight, tonesto7, Haibin Wen and other sunnypilot and BluePilot contributors. The reference snapshot is [`e1d051d7ba270261b4455068bd68f1a58db15a4a`](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a); the exact checkout used for the earlier StarPilot import was not recorded. -Relevant lineage includes the controller and integration work in [`db2bdff05`](https://github.com/BluePilotDev/bluepilot/commit/db2bdff05df103d71df62f45c2a3cb5211aba6e6) and [`d0aac605f`](https://github.com/BluePilotDev/bluepilot/commit/d0aac605f99d37e9da205e419f7989c1e9eaa386), extended CAN and safety work in [`8f8d6d15f`](https://github.com/BluePilotDev/bluepilot/commit/8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d), and manual-turn detection in [`97867c1eb`](https://github.com/BluePilotDev/bluepilot/commit/97867c1eb57b7472f6fc3de62f0fef576e5a5497). - -The vehicle strategy and CAN/native owners preserve that attribution while adapting to the current controller interfaces and shared safety limits. Upstream contributors do not maintain or endorse this adaptation. Applicable notices are retained in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md). - -## Optional Ford angle controller - -The vendored `opendbc_repo/opendbc/bluepilot_lateral` angle core, codecs and Ford-only safety hooks are adapted directly from [BluePilot bp-7.0 e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a), principally by Alan Polk, with John Christman, Jacob Neulight, Nathan Ingraham, Praeuner and other BluePilot, sunnypilot and comma contributors. Exact source paths and intentionally stricter safety checks are recorded in that folder. Both upstream `LICENSE` and `LICENSE.md` notices are retained. Its layout follows Alan Polk's September 2026 library proposal; this is a StarPilot adapter, not a published BluePilot library release. +The radar interface preserves that attribution while adapting to the current interfaces. Upstream contributors do not maintain or endorse this adaptation. Applicable notices are retained in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md). ## RAV4 Continental ARS510 receiver @@ -34,10 +28,6 @@ Spotify's Bluetooth output binds the comma's BlueALSA A2DP source to the selecte The Pre-AP configuration, pedal codec/feedback, VirtualDAS feedforward table, grade compensation, torque-zero estimator and vehicle-specific gains preserve firestar5683's intentional Dom work, including `7837a3c1aab`, `aed0b7fdb9e9` and pinned `b220a37f4bd119e402225a7aa16a7cd84878d0af`. Original author attribution: firestar5683 <168790843+firestar5683@users.noreply.github.com>. Startup/calibration ownership, current upstream button/PID interfaces and native command bounds are adapted to the current StarPilot architecture; the old permissive native gateway is not transplanted. Existing source/license notices remain intact. -## Ford optional pinion measurement - -The default-off Use Pinion Yaw Sensor measurement option is adapted from jrneulight’s BluePilot PR #145 (https://github.com/BluePilotDev/bluepilot/pull/145), merged as `fe5b019568c8c13fb894dc6d8d779d60985051a8`. Its per-platform geometry, native pinion measurement, integrity requirements and host vehicle-model compensation are retained. StarPilot adapts the option to its existing exact safety-profile word, modern curvature checks, AOL authority and Galaxy/native settings; the upstream maintainers do not maintain this integration. Existing upstream copyright and license notices remain in THIRD_PARTY_NOTICES.md. - ## Subaru angle steering from Jacob Waller's fork Subaru angle support includes implementation maintained and integrated by **Jacob Waller (jacobw)** in [jacobwaller/openpilot, jul-angle-based](https://github.com/jacobwaller/openpilot/tree/542816196a18dba5f6f0af093c1ece4f415b5a22), with its [opendbc implementation at 8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0](https://github.com/jacobwaller/opendbc/tree/8acbeccdab45a47aa7af170d2d17b61f0eb5e4f0). Jacob authored the VehicleModel angle-limit type correction and integrated the Crosstrek support. The branch also retains the original angle implementation and follow-up work by **lukasloetkolben and elkoled**, alongside comma contributors; their authorship remains theirs. diff --git a/THIRD_PARTY_NOTICES.md b/THIRD_PARTY_NOTICES.md index 36c6b73a13..5f140d6ae2 100644 --- a/THIRD_PARTY_NOTICES.md +++ b/THIRD_PARTY_NOTICES.md @@ -73,17 +73,7 @@ the exclusion of the other. ## BluePilot Ford support -The Ford lateral strategies, CAN constructors, camera-radar filtering and native checks include work adapted from BluePilot's `bp-7.0` source, through StarPilot `2efe6017bfe14cee4807833e05f9a7f6ba255cdb`. See [CREDITS.md](CREDITS.md#ford-support-adapted-from-bluepilot) for contributors and source revisions. The upstream extension-file and `LICENSE.md` notices reproduced above also apply to the Ford extension sources; they are retained without resolving the difference between their published license descriptions. - -## Optional BluePilot angle library adapter - -The angle-only adapter in `opendbc_repo/opendbc/bluepilot_lateral` is sourced directly from BluePilot bp-7.0 commit e1d051d7ba270261b4455068bd68f1a58db15a4a. The complete upstream MIT notice is retained in that folder's `LICENSE`, and the simultaneously published Custom MIT notice is retained verbatim in `LICENSE.md`. The applicable SUNNYPILOT acknowledgments reproduced above are also retained in its `CREDITS.md`. This preserves the published terms without resolving their scope or representing the adapted extension as unambiguously plain MIT. - -The optional, default-disabled angle lane-centering trim additionally derives from -BluePilot bp-dev e22afa6be9b881fa784c92ebb316db47728a3d81. Its exact source and -contributors are recorded in the library CREDITS.md; the same root/opendbc MIT -and Custom MIT notices coexist unchanged at that revision. This adds attribution, -not a license-scope decision or permission grant. +The Ford CAN FD camera-radar filtering includes work adapted from BluePilot's `bp-7.0` source, through StarPilot `2efe6017bfe14cee4807833e05f9a7f6ba255cdb`. See [CREDITS.md](CREDITS.md#ford-support-adapted-from-bluepilot) for contributors and source revisions. The upstream extension-file and `LICENSE.md` notices reproduced above also apply to that source; they are retained without resolving the difference between their published license descriptions. ## Continental ARS510 radar diff --git a/opendbc_repo/opendbc/bluepilot_lateral/.gitattributes b/opendbc_repo/opendbc/bluepilot_lateral/.gitattributes new file mode 100644 index 0000000000..790302d680 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/.gitattributes @@ -0,0 +1 @@ +.github export-ignore diff --git a/opendbc_repo/opendbc/bluepilot_lateral/.gitignore b/opendbc_repo/opendbc/bluepilot_lateral/.gitignore new file mode 100644 index 0000000000..c18dd8d83c --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md b/opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md index 347892a413..233bb151f6 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md +++ b/opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md @@ -1,35 +1,53 @@ -# BluePilot angle source +# Credits -Adapted from BluePilotDev/bluepilot bp-7.0 at -[e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a), -principally Alan Polk, with John Christman, Jacob Neulight, Nathan Ingraham, -Praeuner, tonesto7, Haibin Wen and other BluePilot, sunnypilot and comma contributors. -Source paths: opendbc/sunnypilot/car/ford/lateral_angle_ext.py, -fordcan_ext.py, human_turn.py and opendbc/safety/modes/ford.h. -The simultaneous upstream LICENSE and LICENSE.md notices are retained. -This software is licensed under a custom license requiring permission for use. -This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use. +The BluePilot Ford lateral library: Ford's four-signal lateral control in curvature and angle +modes, the CAN messages that carry it, and the panda safety that checks it. Everything here is +under the MIT terms in [LICENSE](LICENSE). -This is a StarPilot adapter to the September 2026 proposed library shape. -It is not a released BluePilot library, tag, or claim of BluePilot validation. -The existing curvature controller remains StarPilot-owned. Pinion curvature, -curvature-primary settings and the unsafe native reset bypass are not imported. +## Contributors -The exact upstream root notices are additionally retained as upstream-root-LICENSE -and upstream-root-LICENSE.md; LICENSE/ LICENSE.md retain the opendbc notices. -ORIGIN_HISTORY.tsv records the inspected path history with commit IDs/authors/dates. -The private compact upstream root clone preserves its reachable root history. -Pre-conversion opendbc submodule objects are not included in that root history. +- **Alan Polk (`alan-polk`)**: principal author of the curvature and angle controllers, the + human-turn behavior, the angle-mode side channel and its panda safety, and the lane centering + trim. +- **tonesto7**: the CAN message builders and BluePilot's early releases. +- **John Christman**: lateral integration, platform data, and the angle-mode anti-stall work. +- **Jacob Neulight**: the steering-pinion curvature measurement and its panda safety, and the + shadow-curvature measurement. +- **Blue Genie (`blue-genie`)**: the split of BluePilot's lateral control into opendbc that this + library grew from. +- **ghbarker**: the lane centering trim's confidence blend, fallback, filter and rate limit. +- **Nathan Ingraham**: the angle-mode high-speed damping adjustment. +- **Praeuner**: the angle-mode damping range. +- **OneFast440**: the early Ford safety declarations. +- **nitrobass24**: the library's structure, its inputs and settings plumbing, and the safety tests + and CI. -Angle lane-centering trim is separately adapted from BluePilot bp-dev -[e22afa6be9b881fa784c92ebb316db47728a3d81](https://github.com/BluePilotDev/bluepilot/commit/e22afa6be9b881fa784c92ebb316db47728a3d81), -`opendbc/sunnypilot/car/ford/lane_center_trim.py` (blob -`5eedebd79e99493624f9d6af0750d1dd5713306c`). Alan Polk authored the initial -trim, model-position fallback, correction slew and default-strength changes; -ghbarker contributed reference/NaN guards and tests. The source explicitly credits -StarPilot/u/jc01rho for curvature-domain placement/geometry. The core adaptation -adds immutable host projection and conservative stale/driver-pressure suppression. -The pinned bp-7.0 controller baseline and default-disabled policy are unchanged. -Both simultaneous root/opendbc MIT and Custom MIT notices also exist unchanged at -this trim revision; all retained notices and permission acknowledgments apply -without assigning an exclusive license or claiming a new permission grant. +## Third-party code + +### comma.ai's opendbc + +MIT, Copyright (c) 2020, Comma.ai, Inc. + +- `can/fordcan_ext.py`: `create_lat_ctl_msg` and `create_lat_ctl2_msg`, comma's Ford lateral + message builders from `opendbc/car/ford/fordcan.py`, with ramp-type and precision parameters + added. +- `safety/lateral.h`: the measured-table rate check, curvature-error band and lateral-accel cap, + from comma's earlier checks in `opendbc/safety/lateral.h`, and `ford_bp_curvature_cmd_checks`, + adapted from comma's `steer_curvature_cmd_checks`. +- `safety/ford.h`: the rate-of-change shape of the path angle and path offset checks, the + curvature-error band in the shadow-curvature check, and the message-rate formula. +- `core/lateral_curv_ext.py`: `apply_ford_curvature_limits_ext`, adapted from comma's + `apply_ford_curvature_limits`, with its road-roll and lateral-accel constants. +- Small pieces: the message packing and the yaw-rate curvature measurement in + `core/carcontroller_ext.py`, and the model time-index formula in `core/values_ext.py`. +- The BluePilot safety tests start from comma's Ford safety test skeletons. + +### StarPilot + +MIT, Copyright (c) 2026, firestar5683 and StarPilot contributors + +- `core/lane_center_trim.py`: the lane-centering core (the correction formula, lookahead clip, + lane-line probability, std and width gates, the 0.004 correction limit and the lane-line + handling), ported from StarPilot's `selfdrive/controls/lib/lane_centering.py` + (`LaneCenteringController`, originally by jc01rho). The confidence blend, fallback, filter and + rate limit are BluePilot's. diff --git a/opendbc_repo/opendbc/bluepilot_lateral/FORKS.md b/opendbc_repo/opendbc/bluepilot_lateral/FORKS.md new file mode 100644 index 0000000000..4dc88b88e8 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/FORKS.md @@ -0,0 +1,83 @@ +# Bringing BluePilot lateral to a fork + +How an openpilot fork takes BluePilot's Ford lateral control. There are two ways to do it: + +1. **Use a release on your own** (preferred). Pin a release of this library and write your own hooks and settings against [HOSTS.md](HOSTS.md). BluePilot isn't in the loop and nothing waits on BluePilot: you consume lateral code that has been driven on Fords. +2. **Work with BluePilot.** BluePilot helps develop your hooks, keeps them in this repo, and tests the library on your fork in its CI, every PR and every night. + +Either way the library is the same, and the same rules apply to it (below). + +**This is a modified Ford safety model.** The panda safety allows signals and ranges that upstream's Ford safety blocks. Your fork decides whether to ship it, and whether BluePilot lateral is on or off by default for your users. + +## The rules for either way + +- **Pin a release, unmodified.** The library sits at `/bluepilot_lateral/` in your opendbc (`` is its package name) as one of: + - a **git submodule** pointing at a release tag, if your fork uses submodules; updating is moving the pointer; + - a **stamped copy** made with `tools/vendor.sh`, if it doesn't (see "The stamp tools"). +- **Don't edit inside the library folder**, carry it as a git subtree with its history, or copy parts of it elsewhere. A fix to the library is a PR to this repo; once it's released, every fork gets it. +- **Everything else is yours:** the hooks in your opendbc, the code in your openpilot that fills the library's inputs every frame, and your settings UI. + +## Option 1: use a release on your own + +1. Pick a release from this repo's releases. Each one says what changed in the lateral control, the CAN messages and the panda safety, and what was validated on a car. +2. Mount it (submodule or stamped copy, above). +3. Write the hooks in your opendbc. [HOSTS.md](HOSTS.md) has the contract: the hook sites, the per-frame inputs and settings, the startup safety bits, and the host bindings to define if your safety function names differ from upstream opendbc's. Each release attaches the hook patch for sunnypilot/opendbc as a worked example. +4. Write your adapter: the small module that hands the library your fork's types (`hosts/sunnypilot/` is the example to copy). It lives in your fork, outside the library folder. +5. Run the library's tests in your tree ([HOSTS.md](HOSTS.md), "Running the tests") and drive it before you ship it. +6. To update, move to a newer release and rerun your tests. + +BluePilot doesn't review or approve any of it. Questions and bugs in the library are welcome as issues in this repo. + +## Option 2: work with BluePilot + +For a fork that wants BluePilot to help with the hooks and keep them working. Open an issue in this repo, "Host: ": where the fork's source and integration branch are, how it carries its opendbc, and how it takes outside changes. From there, this repo keeps for that fork: + +- **its adapter**, `hosts//`, plus host bindings if its safety names differ from upstream's; +- **its hooks**, `hosts//hooks.patch`, until the fork carries them; +- **`hosts//host.sh`**: `tree ` builds the fork's opendbc at a pinned commit with the hooks and the library, placed by `vendor.sh`; `test ` runs the library tests, the fork's safety suite with the BluePilot safety tests, and coverage; `make ` writes `hooks.patch` back after a hook is edited in `/host`; +- **a CI job** that runs `host.sh tree` and `test` on every PR to this repo, so a library change that would break the fork fails here first; +- **a nightly check** that tries the fork's latest integration branch: a pass moves the pinned commit, a failure opens one issue saying which step broke. Once the fork carries the hooks it tests the fork's tree as it is, and runs `check_vendored.sh` on its copy. + +The steps: +1. Develop and test the hooks and the fork's side in the fork, together. +2. Add the adapter, `hooks.patch`, `host.sh`, the CI job and the nightly check here, in one PR. +3. Release the library with them. +4. BluePilot opens the PR to the fork: the hooks, its settings UI and the library at that release. Later releases arrive the same way, as PRs the fork takes when it chooses. + +An adapter lives in this repo only if this repo's CI tests the library on that fork. + +## The stamp tools + +For a fork that takes no submodules. Both run from a clone of this repo that has the release tag (`git fetch --tags`). + +**`tools/vendor.sh `** makes `` exactly the library at ``: +- it empties the folder first, so files a release removed don't linger; +- it copies the release's files, without the library's own CI (`.github/`); +- it writes `/VERSION`: the tag and the commit, e.g. `v1.2.3 0a1b2c3d...`. + +```bash +git clone https://github.com/BluePilotDev/bluepilot-lateral.git +cd bluepilot-lateral +tools/vendor.sh //bluepilot_lateral +``` + +Commit the folder in the fork. To update, run it again with the new tag: the diff shows exactly what changed between the two releases. + +**`tools/check_vendored.sh `** reads `/VERSION` and compares the folder with that release, file by file (Python caches ignored). It lists every added, removed or changed file, and exits non-zero if there is one: + +```bash +tools/check_vendored.sh //bluepilot_lateral +``` + +Run it in your CI to catch an edit to the copy. For a fork working with BluePilot, this repo's nightly check runs it too. + +## Installer note: nested submodule + +A fork that embeds the library as a *nested* submodule (Option 2 — e.g. `fork → opendbc → opendbc/bluepilot_lateral`) can end up with the library missing after an end-user install. Some installers run a non-recursive `git submodule update --init`, which checks out the first-level submodule but not the library nested inside it, and the build then fails to import it. Installs that use `git submodule update --init --recursive` are unaffected. + +If your fork is installed that way, self-heal on launch: before the build imports the library, when its directory is empty, run a recursive submodule update scoped to the embedding submodule — guard on the library's `__init__.py`, retry, and fail soft so an offline boot is no worse than before. + +## How BluePilot releases + +- Lateral changes and changes to the library's safety headers are driven on a car before they're released. +- Every release says what changed in the lateral control, the CAN messages and the panda safety, and what was validated. diff --git a/opendbc_repo/opendbc/bluepilot_lateral/HOSTS.md b/opendbc_repo/opendbc/bluepilot_lateral/HOSTS.md new file mode 100644 index 0000000000..d211deb45a --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/HOSTS.md @@ -0,0 +1,129 @@ +# Hosting the BluePilot lateral + +A host is an opendbc fork with this library at `/bluepilot_lateral/` and the hook patch applied, plus the openpilot on top of it that fills the library's inputs each frame. `` is the fork's opendbc package: `opendbc` on sunnypilot/opendbc (ford-op/opendbc, under bp-sunnypilot), `iqdbc` on IQ.Pilot, `opendbc` on StarPilot. This page is what a host provides and what it gets back. + +## Mounting the library + +A host mounts this repo at `/bluepilot_lateral/`, directly inside its opendbc package, as a git submodule (ford-op/opendbc) or, in a host that takes no submodules (IQPilot, at `artifacts/package_sources/iqdbc/iqdbc/bluepilot_lateral/`), as a stamped copy of a release. Updating is a bump of the submodule's commit, or vendoring the new release (below). The hook patch below carries every file ford-op/opendbc adds or changes outside the library, so since it mounts the library it also carries the mount's `.gitmodules` entry. + +The library never names its host's package. Python imports are relative: the library's own modules by `.`/`..`, the host's opendbc by one more level (`from ...car.ford.values import CarControllerParams` in `core/`), and `tests/test_core_imports.py` fails on any absolute `opendbc`/`iqdbc`/`openpilot` import. Safety headers include each other as `../safety/.h` and the host's declarations as `../../safety/declarations.h`, relative to the including file. The host includes the library's safety as `"/bluepilot_lateral/safety/ford.h"`. + +### Without submodules: a stamped copy + +From a clone of this repo, `tools/vendor.sh //bluepilot_lateral` makes the folder exactly the library at that tag (files it no longer has are removed), without `.github/` (`.gitattributes`), plus a `VERSION` file: the tag and the commit. Nothing inside the folder is ever edited: a change goes into this repo, gets released, and the host vendors the new tag. `tools/check_vendored.sh ` lists anything in the folder that differs from the release in its `VERSION` and fails if there is something; a host can run it in its CI, and this repo's IQPilot checks run it on IQPilot's copy. + +### Host bindings + +The safety headers are written against upstream opendbc's names. A host whose names differ defines these before it includes `safety/ford.h` (`safety/lateral_declarations.h` has the defaults): + +| macro | default (upstream, sunnypilot) | IQ.Pilot | +|---|---|---| +| `BP_HOST_CURVATURE_LIMITS` | `CurvatureSteeringLimits` | `NativeCurvatureSteeringLimits` | +| `BP_HOST_CURVATURE_STATE` | `curvature_state` | `native_curvature_state` | +| `BP_HOST_RT_CURVATURE_RATE_CHECK` | `rt_curvature_rate_limit_check` | `rt_native_curvature_rate_limit_check` | +| `BP_HOST_LAT_ALLOWED()` | `(controls_allowed \|\| controls_allowed_lateral)` | `is_lat_active()` | +| `BP_HOST_SPEED_MISMATCH_CHECK()` | `speed_mismatch_check(vehicle_speed_2 ...)` | `((void)0)`: IQ's `ford_rx_hook` runs it | + +The limits struct needs only `max_curvature`, `curvature_to_can`, `frequency`, `max_curvature_error` and `curvature_error_min_speed`. + +## The hook patch + +`tools/make_hook_patch.sh` prints it: every file outside the library that ford-op/opendbc changes from `master`, minus its own `.github/` (its CI). It fails unless the edited files are exactly the non-`.github` entries of `.github/bluepilot-hooks.txt`. + +| file | what the hook does | +|---|---| +| `opendbc/car/ford/carcontroller.py` | Builds one `BluePilotLateral(CP, CP_SP, packer, CAN)`, the sunnypilot adapter (`hosts/sunnypilot/lateral.py`, below). Each frame `bp.update(CC_SP, frame)` says whether BluePilot lateral is active; if it is, `bp.step_lateral` sends the steering message at `STEER_STEP` (and returns the curvature to pass back next time) and `bp.step_lka` sends Lane_Assist_Data1 at `LKA_STEP`. Otherwise the stock path runs. | +| `opendbc/car/structs.py` | Imports `LateralInputs` from `host_types.py` and adds `CarControlSP.lateralInputs` and `CarParamsSP.lateralInputsRequired`. | +| `opendbc/sunnypilot/car/interfaces.py` | Calls `init_ford_safety_param_sp(CP, CP_SP, params_dict)` (`hosts/sunnypilot/safety_param.py`) at car init: the startup safety bits below, and `lateralInputsRequired` for every Ford. | +| `opendbc/safety/modes/ford.h` | Includes `safety/ford.h` and makes one call per hook site: counter and quality checks for the pinion message, the yaw measurement skipped while the pinion source is on, `ford_bp_rx_hook` (pinion measurement), `ford_bp_lka_tx` (angle-mode state from Lane_Assist_Data1), `ford_bp_lmc_tx` in place of the stock steering check when `BP_LATERAL` is set (the stock check runs unchanged when it isn't), and the pinion RX check list from `ford_bp_init`. | +| `opendbc/safety/tests/test_bluepilot_lateral.py` | New: hands the library's `safety/tests/` to upstream's test discovery, so upstream's `test.sh` and `mutation.py` run the BluePilot safety tests. | + +## Every frame: inputs + +A host fills these and the settings below for a car whose `CarParamsSP.lateralInputsRequired` is set: every Ford, BluePilot lateral on or off (`init_ford_safety_param_sp`). + +`CarControlSP.lateralInputs` (`host_types.LateralInputs`): + +| fields | from | +|---|---| +| `pathYawRate`, `pathX`, `pathY` | `modelV2.orientationRate.z`, `modelV2.position.x`, `.y` (33 points) | +| `laneLineLeftX/Y`, `laneLineRightX/Y` | `modelV2.laneLines[1]`, `[2]` | +| `laneLineProbs`, `laneLineStds` | `modelV2.laneLineProbs`, `.laneLineStds` | +| `laneChangeState`, `laneChangeDirection` | `modelV2.meta` | +| `steerRatio`, `stiffnessFactor`, `roll`, `angleOffsetDeg` | the learned vehicle parameters (`vehicleParameters`, paramsd) | +| `lateralDelay` | `lateralDelay.lateralDelay` | +| `valid` | true only when both the model and the vehicle parameters have been seen and every field above is filled | + +Each path array has `host_types.MODEL_PATH_POINTS` (33) entries at the times in `host_types.MODEL_T_IDXS` (openpilot's `ModelConstants.T_IDXS`): the core interpolates over that grid. On a frame that isn't valid, or whose arrays aren't the full shape or contain a non-finite value, the library keeps the previous inputs. + +## Every frame: settings + +Every key in `opendbc.bluepilot_lateral.core.values_ext.BP_LATERAL_PARAMS` (the per-frame settings in `params/manifest.py`; hosts import the list from there), every frame, as (key, value) with the value as bytes (sunnypilot: `CarControlSP.params` entries): bools `"1"`/`"0"`, numbers as text. A missing, empty or unparsable value gives the manifest default. A host publishes its own registry default for a key nobody has set, so a host can carry its own defaults (BP7 uses 3.0 for `LC_PID_gain_UI_curv`). `python -m .bluepilot_lateral.params.manifest` prints the keys as `params_keys.h` lines (IQ.Pilot drops `BACKUP`, which its params don't have). + +## At startup: the safety bits + +`values_ext.ford_safety_param(CP, params_dict)` reads the startup settings (`disable_BP_lat_UI`, `FordPrefSteerAngleCurvature`) and returns the bits. The host sends them to the panda in its second safety param (sunnypilot: the adapter ORs them into `CP_SP.safetyParam`). StarPilot has no second param: its adapter shifts them into bits 9-14 of its Ford `safetyParam` (`hosts/starpilot/safety_param.py`, clear of the bits StarPilot's Ford safety reads), and its `ford_init` hands `(param >> 9) & 0x3F` to `ford_bp_init`: + +| bit | | +|---|---| +| 5 `BP_LATERAL` | the panda runs the BluePilot steering checks; the car controller sends the 4-signal messages only when it's set | +| 0 `STEER_ANGLE_CURVATURE`, 1-4 geometry index | the pinion angle replaces the yaw rate as the curvature measurement, on platforms with a row in the pinion table | + +The panda keeps what it was given at init: changing `disable_BP_lat_UI` while driving takes full effect at the next init. + +## Reading status + +`BluePilotLateral.status()` returns `host_types.LateralStatus`, this frame's values for a BluePilot UI, under the names BP7's `controllerStateBP` publisher reads off its car controller: + +| field | | +|---|---| +| `lateralUncertainty` | from whichever strategy ran | +| `angleRateLimited` | angle mode: the path angle soft rate limit bit this frame | +| `curvatureRateLimited` | angle mode: the equivalent curvature would have been rate-limited | +| `curvatureDeviationLimited` | either mode: the current-curvature error clip constrained the command | +| `humanTurnLateralPaused` | angle mode: lateral sent inactive during a manual turn | +| `stallBlipActive` | angle mode: the brief mode-0 pulse after a post-override stall | +| `disable_BP_lat_UI` | BluePilot lateral off: the setting, or the panda bit not set at init | +| `primary_lateral_control` | 0 = curvature, 1 = angle | + +The four angle-mode values read false outside angle mode. bp-sunnypilot doesn't read status; it has no BluePilot UI. + +## What the library uses from the host's opendbc + +Every host is an opendbc fork, so the library uses these directly: + +- `.car`: `structs`, `DT_CTRL`, `ACCELERATION_DUE_TO_GRAVITY`, `scale_rot_inertia_per_mass` and `scale_tire_stiffness_per_mass` (or, on an opendbc from before the per-mass vehicle model, `scale_rot_inertia` and `scale_tire_stiffness`, at unit mass), `common.pid.PIDController`, `vehicle_model.VehicleModel` (either signature), `lateral` (`AngleSteeringLimits`, `apply_std_steer_angle_limits`, `ISO_LATERAL_ACCEL`), `ford.fordcan.CanBus`, `ford.values` (`CarControllerParams`, `FordFlags`). +- `can/fordcan_ext.py` sets LateralMotionControl2's checksum itself: upstream's packer fills Ford checksums (and overwrites it with the same value), IQ.Pilot's doesn't. +- The safety headers use `modes/ford.h`'s Ford constants and upstream's safety helpers, through the bindings above. `safety/lateral.h` and `can/fordcan_ext.py` copy four upstream functions, pinned per host package by `tests/test_bluepilot_upstream_copies.py`, so a host where one changed fails that test (a host with no pins skips it). +- The safety tests use the host's test harness: `.safety.tests.test_ford`, `common` and `libsafety.libsafety_py`. `tests/host.py` names the host's own structs, adapter and libsafety setters for the tests that touch them. + +## The core's API, and the host adapters + +The core takes only the library's types: + +- `core.carcontroller_ext.BluePilotLateral(CP, safety_param, packer, CAN)`: `safety_param` is the startup bits (`ford_safety_param`). +- `update(inputs, params, frame)`: `inputs` is a `host_types.LateralInputs`, `params` a `params.param_store.ParamStore` built from the frame's (key, value) settings. Then `step_lateral`, `step_lka` and `status()` as above. + +Nothing under `core/`, `can/`, `params/` or `safety/` imports a host (`tests/test_core_imports.py`). `hosts/sunnypilot/`: + +- `lateral.py`: a `BluePilotLateral` subclass with sunnypilot's types, `(CP, CP_SP, packer, CAN)` and `update(CC_SP, frame)`. It passes `CP_SP.safetyParam`, `CC_SP.lateralInputs` and the `CC_SP.params` entries as (key, value) pairs. +- `safety_param.py`: `init_ford_safety_param_sp(CP, CP_SP, params_dict)` ORs `ford_safety_param` into `CP_SP.safetyParam`. Its own module, so car init imports only `values_ext`. + +`hosts/iqpilot/`, the same for IQ.Pilot's types: + +- `lateral.py`: `BluePilotLateral(CP, CP_IQ, packer, CAN)` and `update(CC_IQ, frame)`, passing `CP_IQ.iqSafetyFlags`, `CC_IQ.lateralInputs` and the `CC_IQ.params` entries. +- `safety_param.py`: `init_ford_safety_param_iq(CP, CP_IQ, params_dict)` ORs `ford_safety_param` into `CP_IQ.iqSafetyFlags` (the panda's `current_safety_param_iq`) and sets `CP_IQ.lateralInputsRequired`. + +`hosts/starpilot/`, for StarPilot, which has neither a second safety param nor CarControl / CarParams extension structs: + +- `lateral.py`: `BluePilotLateral(CP, packer, CAN)`, reading the startup bits back out of the Ford `safetyParam`, and `update(frame_inputs, frame)`. `FrameInputs` carries `lateralInputs` and the `params` entries; StarPilot's openpilot side fills one each frame and hands it to the Ford CarController, in place of a CarControl extension. +- `safety_param.py`: `init_ford_safety_param_starpilot(CP, params_dict)` ORs `ford_safety_param << 9` into `CP.safetyConfigs[-1].safetyParam`; `bp_safety_param(CP)` reads them back; `lateral_inputs_required(CP)` says whether to fill `FrameInputs`. StarPilot's Transit (`FordFlags.LKA_STEERING`, steering through Lane_Assist_Data1 alone) gets neither. + +Another host writes its own adapter against the same API. + +## Running the tests + +- Python tests: `python -m unittest discover -s /bluepilot_lateral/tests -t .`, from the directory that holds ``. +- Safety tests: the host's `/safety/tests/test.sh` runs them, through a loader (ford-op/opendbc's `test_bluepilot_lateral.py`) or by naming `../../bluepilot_lateral/safety/tests` (IQ.Pilot: its cross-mode TX sweep reads `TX_MSGS` off every `Test*` class in its directory, so a loader there breaks it). Coverage of the BluePilot headers and `modes/ford.h`: `safety/tests/bluepilot_coverage.sh`, which runs the library's tests itself where the host has no loader; mutation: `safety/tests/bluepilot_mutation.py` (needs the host's `mutation.py`, which IQ.Pilot doesn't have). +- On a clean sunnypilot/opendbc checkout: `tools/test_on_host.sh `, from a ford-op/opendbc checkout. It copies the library as committed and applies the hook patch to a temporary clone, then runs both. +- On IQPilot: `hosts/iqpilot/host.sh tree ` builds iqdbc from IQ.Lvbs's repo at the pinned commit with `hosts/iqpilot/hooks.patch` (IQPilot's hooks, kept here until IQPilot carries them) and the library, vendored; `host.sh test ` runs the library tests, IQPilot's safety suite with the BluePilot ones (minus `known_failures.txt`, IQPilot's own) and coverage; `host.sh make ` writes the hook patch back after editing the hooks in `/host`. CI's `iqpilot` job runs tree and test. Every night `iqpilot pin check` tries IQ.Lvbs's latest `beta`: green moves the pin in a PR it merges itself (dropping `hooks.patch` once IQPilot carries the hooks), red keeps one issue open saying which step failed. diff --git a/opendbc_repo/opendbc/bluepilot_lateral/LICENSE b/opendbc_repo/opendbc/bluepilot_lateral/LICENSE index f1fd199c63..d04886ea68 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/LICENSE +++ b/opendbc_repo/opendbc/bluepilot_lateral/LICENSE @@ -1,4 +1,6 @@ Copyright (c) 2020, Comma.ai, Inc. +Copyright (c) 2026, firestar5683 and StarPilot contributors +Copyright (c) 2024-2026, BluePilot contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: diff --git a/opendbc_repo/opendbc/bluepilot_lateral/LICENSE.md b/opendbc_repo/opendbc/bluepilot_lateral/LICENSE.md deleted file mode 100644 index 650b6f55b3..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/LICENSE.md +++ /dev/null @@ -1,21 +0,0 @@ -# Custom MIT License - -Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions: - -1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s). - -2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment: - "This software is licensed under a custom license requiring permission for use." - -3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment: - "This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use." - -4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - -Contact sunnypilot Support for permission requests. - ---- - -Haibin Wen, SUNNYPILOT LLC diff --git a/opendbc_repo/opendbc/bluepilot_lateral/ORIGIN_HISTORY.tsv b/opendbc_repo/opendbc/bluepilot_lateral/ORIGIN_HISTORY.tsv deleted file mode 100644 index 55105a47d7..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/ORIGIN_HISTORY.tsv +++ /dev/null @@ -1,38 +0,0 @@ -9d190417bf74563abfa6f36996f1e673e9ee0a1b Praeuner 2026-08-13T22:26:15-05:00 Update dampening factor to .25 -a881646ea5edc21c0db4c4e74076e0dc14671a4e alan-polk 2026-08-10T09:04:52-05:00 Merge pull request #176 from BluePilotDev/bp-no-stall -9012f76666a5c90764fcaec40832b9a607488c27 John Christman 2026-08-05T17:49:59-04:00 anti-stall -3610e3f18a0ad37d793d7ada61dbc739ab1077a3 Nathan Ingraham 2026-07-23T22:26:29-05:00 self.path_angle_gain_lowC_highV remains the platform-specific base gain. The toggle now controls separate self.user_dampening_factor. l ow_gain_calc uses: self.path_angle_gain_lowC_highV * self.user_dampening_factor UI placement, description, default, and 0.75–1.25 range remain unchanged. -1db52bc79e607ddb00214254dd4d732615e27fe4 Nathan Ingraham 2026-07-23T00:00:51-05:00 -Added “High Speed Dampening” directly below “High Speed Adjustment Factor”, range 0.75–1.25, step 0.01, and default 1.0. -5d994c24edf63f3b9004b2963f18ef4199e5d368 Jacob Neulight 2026-07-18T20:17:04-06:00 Ford safety: optional pinion-sourced angle_meas behind STEER_ANGLE_CURVATURE SP flag -699c17d9fde62c690eed9d68eed7d731744d5137 Jacob Neulight 2026-07-18T19:53:46-06:00 Ford: publish angle-mode shadow curvature from measured curvature during overrides -b7428fee4b116d4832f91c850d54cac2730de2ef alan-polk 2026-07-18T08:52:24-05:00 restore curvature safety checks to bp-6.0 logic. -97867c1eb57b7472f6fc3de62f0fef576e5a5497 alan-polk 2026-07-16T19:18:05-05:00 Human Turn Reset for Angle (#118) -7efee67e3e643d52b973895d6c6765c5cc12e314 alan-polk 2026-07-16T08:19:43-05:00 angle mode: rescale exit-detection threshold for 20Hz cadence -1958a3501ead10ae1f496f0d11e1e8cda6e2dbcc alan-polk 2026-07-14T15:22:38-05:00 Bp 7.0 variable cleanup (#111) -8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d alan-polk 2026-07-14T08:53:28-05:00 Bp 7.0 panda2 (#109) -11ac3b760671b489ca449d522f1edf240a232884 alan-polk 2026-07-09T07:53:21-05:00 angle mode: fix ROC values tuned for 100Hz cadence running at actual 20Hz -dbdcea747e65f574d326a55c9bf1a9d6c9485e22 alan-polk 2026-07-08T09:21:32-05:00 Revert "ford: add current-curvature deviation check to angle mode (panda-side)" -69a9714563622729ba304ac75354ba3fd37b0b39 alan-polk 2026-07-08T09:17:09-05:00 ford: add current-curvature deviation check to angle mode (panda-side) -be7f86a5d0dc1ea3b3f0052d4b54ff0b8a70f843 John Christman 2026-07-06T21:53:55-04:00 cast on debug -5c1fbb07dd4d7d950a01cb2cd1faddd90d41e632 alan-polk 2026-07-06T14:20:49-05:00 controllerStateBP: add angleRateLimited + curvatureRateLimited diagnostics -06d654e5f48ea8fcffce6e98e48e3a30450b5097 alan-polk 2026-06-25T23:28:51-05:00 Revert "temp blend ratio change" — restore predicted-curvature blend to 0.50 -540e69fcf4861a2ce5d7ca41467effc231638e27 alan-polk 2026-06-22T08:35:44-05:00 temp blend ratio change -e3819c7e630d85cfb44858840124fe7f72f1762d alan-polk 2026-06-13T20:47:09-05:00 ford.h: don't block angle mode on the inactive curvature signal -5d343738e30267688d0ffa6eebe2ec3ca57b10ee alan-polk 2026-06-13T18:28:30-05:00 ford.h: widen path_angle ROC to 2% looser than lateral_angle_ext _soft_roc -d0aac605f99d37e9da205e419f7989c1e9eaa386 alan-polk 2026-06-13T11:15:50-05:00 BluePilot: add angle-primary lateral control mode -51d5de8991508c862e7c51333e5055425b2b8d23 alan-polk 2026-04-11T12:00:10-05:00 fixing missing punctation in ford.h -8194ceb7e0bbc112c73d393a8f0add3261f6592d alan-polk 2026-04-11T11:51:49-05:00 make safety limits symetrical but control limits asymetrical. Add 1.5s duration to detect human turn -db2bdff05df103d71df62f45c2a3cb5211aba6e6 alan-polk 2026-04-09T23:47:30-05:00 move all external logic to lateral_curv_ext.py. Apply some order of operations fixes to try and eleminate panda blocking messages. -1a8ac23b5bd5d7952c15eb913c2b1e39f14ad92e Blue Genie 2026-04-08T15:10:24-05:00 Adjust panda limits -892be076daf898d806983f847c72357accff7312 Blue Genie 2026-04-08T11:45:28-05:00 Add logging functionality -ce7d27230657159332f64a253a0faea6cf8dcc95 alan-polk 2026-04-08T11:40:02-05:00 fix rate of change limits -021b5146e80ac58fe2e81dbd848bc7cd409ef08e alan-polk 2026-04-07T16:24:38-05:00 align ford.h limits with dbc and carcontroller -7f9212f7f8dc22c4a0a22443158554f4d3652a3c tonesto7 2026-04-05T21:28:37-04:00 Merge bp-6.1 upstream changes into bp-6.0-dev (#124) -dd1a9d3fd8f60462d3d8ac6de3e7860e70e1f667 alan-polk 2025-12-12T12:22:31-06:00 convert opendbc from a submodule to a subfolder -10101012d8a4cc37951a557023824cdd995443a6 alan-polk 2026-08-10T17:23:04Z BluePilot: angle-mode lane centering trim (advanced lane positioning) -560e80e9d593badca08aa60e8ddb692e58033425 alan-polk 2026-08-11T12:50:00Z BluePilot: angle-mode lane centering falls back to model position, not zero -94bf8144d4a683e9f3408f3babf892878f3b81f4 ghbarker 2026-08-12T01:31:33Z ford ALP: restore StarPilot guards (narrow lane, laneline std, NaN params) and cap applied correction at 0.0015 -80825b81568ad684fe4ec37a797d3cbaa004fdcf ghbarker 2026-08-13T01:04:02Z split applied-correction ceiling into its own PR per review -e5b92cc48b34ea43ac756a96a96687c9851a188d ghbarker 2026-08-13T01:05:07Z add tests for the restored guards -af4bc410c9ef417ba723212ceaa2a77758dd8ecb alan-polk 2026-08-14T03:24:28Z BluePilot: rate-limit the lane centering correction, not just filter it -8eea9eb68e0c4126ee1c52f251e274d5865b6743 alan-polk 2026-08-19T13:33:00Z BluePilot: lower angle-mode lane centering default strength to 0.25 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/README.md b/opendbc_repo/opendbc/bluepilot_lateral/README.md index 69a561b84a..434970deca 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/README.md +++ b/opendbc_repo/opendbc/bluepilot_lateral/README.md @@ -1,81 +1,14 @@ -# BluePilot Ford angle control +# BluePilot lateral -StarPilot adaptation of BluePilotDev/bluepilot `bp-7.0`, pinned to -[e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a). -The layout follows Alan Polk's September 2026 library proposal. This is an -adaptation, not an upstream library release. +BluePilot's Ford lateral control as a library an opendbc fork mounts at `opendbc/bluepilot_lateral/`: both modes (curvature and angle), the CAN messages that carry them, the panda safety that checks them, and a settings manifest. -Existing curvature control remains the default. Explicit `FordLateralMode=1` -selects angle control next drive; the saved choice persists when unavailable. -Only this selector is exposed. Fixed defaults and bounds live in -`params/manifest.json`; pinion-yaw correction and curvature-primary tuning are -outside this library. Angle lane-centering trim is available as a default-disabled -core capability, separately sourced from BluePilot bp-dev -`e22afa6be9b881fa784c92ebb316db47728a3d81`; it adds no public settings. +**This is a modified Ford safety model.** The panda safety allows signals and ranges that upstream's Ford safety blocks. A fork chooses whether to ship it. -`core/` accepts scalar/model inputs without openpilot, Params or messaging -imports. `can/` owns encoding, `safety/ford_bp.h` owns native enforcement, and -`hosts/starpilot.py` connects the host. `params/protocol.json` generates the wire -constants. The folder is vendored without submodules, LFS or an external runtime. +**Status:** `v0.2.0`. It runs on bp-sunnypilot, a sunnypilot fork, through ford-op/opendbc (sunnypilot's opendbc plus the hooks). -## Admission +- [FORKS.md](FORKS.md): how a fork takes BluePilot lateral and works with BluePilot: carrying the library (a submodule or a stamped copy), the stamp tools, and adding a fork. +- [HOSTS.md](HOSTS.md): what a host provides, the hook patch, and how to run the tests. +- [can/LANE_ASSIST_DATA1.md](can/LANE_ASSIST_DATA1.md): the Lane_Assist_Data1 bits angle mode uses. +- [CREDITS.md](CREDITS.md) and [LICENSE](LICENSE) (MIT). -Classic: Bronco Sport Mk1, Escape Mk4, Focus Mk4, Maverick Mk1, Explorer Mk6. -CAN FD: Escape Mk4.5, Expedition Mk4, F150 Mk14, F150 Lightning Mk1, Ranger Mk2, -Mustang Mach-E Mk1. - -Exact classic safety parameters 32/33 map to angle 128/129; CAN FD 18/19 and -66/67 map to 130/131. Parameter 131 requires DEBUG; RELEASE retains stock 130. -Longitudinal, radar, fingerprint and parser policy remain host-owned. Admission -uses the final CarParams contract, not brand or steering-control enum alone. - -AOL, alternative experiences, Transit, Edge, Mondeo, SecOC, passive, dashcam, -notCar and unsupported static flags are excluded. Auxiliary noOutput requires -parameter zero; production deployments with two Pandas remain unqualified until -actual startup, bus routing and public acknowledgments are verified. - -## Safety design - -The extension uses small Ford hooks without changing shared safety headers or -existing curvature namespaces. Active steering requires ordinary controls -permission, healthy physical RX and a fresh matching public Panda acknowledgment -of safety parameter, alternative experience, controls, RX validity, heartbeat and -faults. Cruise, gas/brake, checksum, counter, quality and freshness checks apply. -Neutral mode cannot grant permission. - -The BluePilot reset-bypass latch is omitted. Rejected frames preserve accepted -actuator history and withdraw the shadow lease; accepted neutral commands or -permission withdrawal reset history. Lost counters recover through neutral -resynchronization. Version or checksum failure cannot fall through to curvature -control. - -Active frames carry neutral curvature, offset and curvature rate, with checked -mode, ramp, precision, path angle, counter, checksum and cadence. Native enforcement -checks the shadow against the actual path across gains 0.95–1.3, bounds both by -`min(0.02, 2.4114/v²)`, and bounds the full actual-curvature interval against -measured curvature above 10 m/s. Actual path also obeys the 3.5886 m/s³ equivalent -jerk limit and rolling message-rate limit. Core quantization and timing margins -keep packed commands inside these bounds; an impossible intersection is neutral. - -The versioned 0x3CA announcement preserves StarPilot's existing byte 4 bit 1 -curvature flag and requires it clear in angle mode. It carries a counter, signed -shadow curvature, protocol version and checksum; its lease expires after 60 ms. -The shadow derives from the final path after rate limiting and saturation. - -Control computes at 20 Hz, announces through 33 Hz LKA, then sends the latest -announced result at 20 Hz, adding at most one steering tick. Permission withdrawal -sends neutral immediately. Human-turn recovery uses bounded neutral blips without -a native bypass. The retained Dom factory steering delay is 0.22 s; pinned -BluePilot uses 0.20 s. - -These stricter native limits and versioned metadata intentionally differ from -BluePilot 7.0. Identical bytes or safety decisions are not claimed for affected -commands. Runtime and physical ECU qualification remain pending. - -## Attribution and notices - -See `CREDITS.md`, `ORIGIN_HISTORY.tsv` and the project `THIRD_PARTY_NOTICES.md`. -Both upstream MIT and Custom MIT notices are retained verbatim in `LICENSE`, -`LICENSE.md`, `upstream-root-LICENSE` and `upstream-root-LICENSE.md`. The maintainer -reports author permission to use this adaptation under MIT. Both published notices -remain intact. +History before 2026-09-15 lives in [BluePilotDev/bluepilot](https://github.com/BluePilotDev/bluepilot). diff --git a/opendbc_repo/opendbc/bluepilot_lateral/VERSION b/opendbc_repo/opendbc/bluepilot_lateral/VERSION new file mode 100644 index 0000000000..8404b87693 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/VERSION @@ -0,0 +1 @@ +v0.2.1 2b9dc6cc784faac47a1885d0bda0561f7b76f74d diff --git a/opendbc_repo/opendbc/bluepilot_lateral/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/__init__.py index 5cea0d35c7..e69de29bb2 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/__init__.py +++ b/opendbc_repo/opendbc/bluepilot_lateral/__init__.py @@ -1,116 +0,0 @@ -"""Adapter to the proposed BluePilot library; angle only, exact source pinned.""" -from opendbc.bluepilot_lateral.can.ford import lateral, lka -from opendbc.bluepilot_lateral.core.angle import Command -from opendbc.bluepilot_lateral.hosts.starpilot import Host, qualified - - -class BluePilotLateral: - def __init__(self, CP, *, host=None): - if not qualified(CP): - raise ValueError("Angle mode is not configured") - self.host = host or Host(CP) - self.canfd = bool(CP.flags & 1) - self.pending = Command() - self.announced = Command() - self.announced_ns = 0 - self.lka_counter = 0 - self.control_counter = 0 - self.control_sent_ns = 0 - self.shadow_sent_ns = 0 - self.control_echo_ns = 0 - self.shadow_echo_ns = 0 - self.control_returned_ns = 0 - self.shadow_returned_ns = 0 - self.control_echo_pending = False - self.shadow_echo_pending = False - self.shadow_rejection_pending = False - self.control_return_payload = None - self.shadow_return_payload = None - self.pending_generation = 0 - self.announced_generation = -1 - self.sent_generation = -2 - self.steering_rejection_ns = 0 - self.apply_curvature_last = 0.0 - - def observe_can(self, can_packets, main_bus): - # Forwarded stock traffic also returns with src|0x80. Consume one matching - # return for our actual emitted payload; unrelated stock echoes cannot pace us. - # Preserve return ordering here; - # start the delay when consumed, in the controller's own clock domain. - control_address = 0x3D6 if self.canfd else 0x3D3 - for timestamp, frames in can_packets: - if timestamp <= 0: - continue - for address, data, source in frames: - if len(data) != 8: - continue - if source == (main_bus | 0xC0) and address in (control_address, 0x3CA): - if timestamp > self.steering_rejection_ns: - self.steering_rejection_ns = timestamp - self.host.steering_rejected = True - if address == 0x3CA and data == self.shadow_return_payload: - self.lka_counter = (data[4] >> 2) & 7 - self.shadow_return_payload = None - self.shadow_rejection_pending = True - continue - if source != (main_bus | 0x80): - continue - if address == control_address and data == self.control_return_payload and timestamp > self.control_returned_ns: - self.control_return_payload = None - self.control_returned_ns = timestamp - self.control_echo_pending = True - elif address == 0x3CA and data == self.shadow_return_payload and timestamp > self.shadow_returned_ns: - self.shadow_return_payload = None - self.shadow_returned_ns = timestamp - self.shadow_echo_pending = True - - def step_lateral(self, CC, CS, packer, CAN, frame, now_ns): - if now_ns <= 0: - return [] - if self.control_echo_pending: - self.control_echo_ns = now_ns - self.control_echo_pending = False - if ((self.control_sent_ns and now_ns - self.control_sent_ns < 50_000_000) or - (self.control_echo_ns and now_ns - self.control_echo_ns < 45_000_000) or - self.announced_generation <= self.sent_generation): - return [] - self.pending = self.host.update(CC, CS) - self.pending_generation += 1 - command = self.announced - # A pending permission withdrawal is immediate; an increase waits until - # the 33Hz side channel has announced the matching final actuator. - fresh = 0 < self.announced_ns <= now_ns <= self.announced_ns + 60_000_000 - if not self.pending.active or not fresh: - command = Command() - self.announced = command - if not fresh: - # Native records the neutral actuator as zero. Do not resume an old - # nonzero path after a new announcement; restart through the normal limits. - self.host.controller.reset_actuator() - self.pending = Command() - self.control_sent_ns = now_ns - self.sent_generation = self.announced_generation - message = lateral(packer, CAN.main, command, canfd=self.canfd, counter=self.control_counter) - self.control_return_payload = message[1] - self.control_counter = (self.control_counter + 1) % 16 - return [message] - - def step_lka(self, packer, CAN, now_ns): - if now_ns <= 0: - return [] - if self.shadow_echo_pending or self.shadow_rejection_pending: - self.shadow_echo_ns = now_ns - self.shadow_echo_pending = False - self.shadow_rejection_pending = False - if (self.shadow_return_payload is not None or - (self.shadow_sent_ns and now_ns - self.shadow_sent_ns < 30_000_000) or - (self.shadow_echo_ns and now_ns - self.shadow_echo_ns < 25_000_000)): - return [] - self.announced = self.pending - self.announced_generation = self.pending_generation - self.announced_ns = now_ns - self.shadow_sent_ns = now_ns - message = lka(packer, CAN.main, self.announced.shadow_curvature, self.lka_counter) - self.shadow_return_payload = message[1] - self.lka_counter = (self.lka_counter + 1) % 8 - return [message] diff --git a/opendbc_repo/opendbc/bluepilot_lateral/can/LANE_ASSIST_DATA1.md b/opendbc_repo/opendbc/bluepilot_lateral/can/LANE_ASSIST_DATA1.md new file mode 100644 index 0000000000..30ba06e8c6 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/can/LANE_ASSIST_DATA1.md @@ -0,0 +1,60 @@ +# Lane_Assist_Data1: BluePilot's angle-mode side channel + +In angle mode the curvature signal in `LateralMotionControl2` is held at 0 and path angle does the +steering, so the panda has no commanded curvature to check against the car's measured curvature. +BluePilot sends that curvature, the *shadow curvature*, plus an "angle mode engaged" flag, in bits of +`Lane_Assist_Data1` (0x3CA) that carry no DBC signal. openpilot sends this message itself at 33 Hz, +and the panda reads it in the TX hook as it goes out. A separate message was tried first: the panda +never receives its own TX, and an RX check for it blocked controls car-wide (2026-07-09). + +Packed by `create_lka_msg` in `fordcan_ext.py`; read by `ford_bp_lka_tx` in `../safety/ford.h`. The +byte positions, mask and scale are named once on each side (`BP_LKA_*` in `fordcan_ext.py`, +`FORD_BP_LKA_*` in `../safety/ford_declarations.h`), and `../tests/test_lane_assist_data1.py` checks +that they agree with each other and with the DBC. + +## Layout + +Bit numbers are within the byte, 7 = most significant. Stock signals are from `ford_lincoln_base_pt.dbc`. + +| byte | bits | stock Ford signal | BluePilot | +|---|---|---|---| +| 0 | 7-5 | `LkaActvStats_D2_Req` | | +| 0 | 4-2 | `LdwActvStats_D_Req` | | +| 0 | 1-0 | `LdwActvIntns_D_Req` | | +| 1-2 | 12 bits | `LaCurvature_No_Calc` | | +| 2-3 | 12 bits | `LaRefAng_No_Req` | | +| 4 | 7 | `LaRampType_B_Req` | | +| 4 | 6-5 | `LkaDrvOvrrd_D_Rq` | | +| 4 | 4-1 | none | free | +| 4 | 0 | none | **angle mode engaged** | +| 5-6 | all | none | **shadow curvature** | +| 7 | all | none | free | + +## Fields + +- **Angle mode engaged** (byte 4 bit 0): 1 while angle mode is the selected lateral mode and + BluePilot lateral is on. +- **Shadow curvature** (bytes 5-6): int16, big-endian, 1e-6 1/m per unit, clamped to + -32768..32767 (about ±0.0328 1/m). It is the curvature angle mode's path angle was derived from, + after the same error clip curvature mode applies; while the driver is pressing, inactive, or + during a human-turn or stall-blip pulse, it is the car's measured curvature instead. It is sent + negated, in Ford's sign convention, like the curvature and path angle in `LateralMotionControl2`. + +## What the panda does + +- It latches both fields from every `Lane_Assist_Data1` frame that goes through its TX hook, + including one it blocks for another reason. +- **Angle mode engaged widens the path-angle range.** While the flag is set, the lateral command's + path angle may use the DBC's full range (-0.5 to 0.5235 rad); otherwise it is held to ±0.25 rad + (`ford_lmc_checks`). The flag alone does it: curvature 0 without the flag keeps the tight range. +- **The shadow curvature check.** For a lateral command (`LateralMotionControl` on CAN or + `LateralMotionControl2` on CAN FD) with curvature 0, steering enabled and the flag set, the + panda scales the shadow curvature by 0.05 to curvature CAN units (2e-5 1/m, truncated toward + zero) and, above 10 m/s, blocks the frame if it is outside the measured curvature ± the curvature + error band + 1 (`ford_shadow_curvature_error_check`). It has no rate limit: path angle has its own. +- `ford_bp_init` clears the flag. + +## Not here yet + +A protocol-version byte and a shared bit allocation for other forks (StarPilot) are Phase 3 (the StarPilot host issue, #16): +they change bytes on the wire. diff --git a/opendbc_repo/opendbc/bluepilot_lateral/can/ford.py b/opendbc_repo/opendbc/bluepilot_lateral/can/ford.py deleted file mode 100644 index e12aa99c9d..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/can/ford.py +++ /dev/null @@ -1,37 +0,0 @@ -"""Ford angle codecs, adapted from BluePilot fordcan_ext.py at e1d051d7. - -Core commands use planner signs; path and shadow are negated on the wire. -The v1 side channel retains byte4 bit1 for StarPilot's curvature announcement. -""" -from opendbc.bluepilot_lateral.params import protocol as p - - -def checksum4(dat): - return (15 - (sum(dat[:7]) & 15)) & 15 - - -def lka(packer, bus, shadow, counter): - addr, original, _ = packer.make_can_msg("Lane_Assist_Data1", bus, {"LaRefAng_No_Req": 0., "LaCurvature_No_Calc": 0.}) - dat = bytearray(original) - raw = max(-20000, min(20000, round(-shadow / p.SHADOW_SCALE))) & 0xFFFF - dat[4] |= p.ANGLE_MASK | ((counter % p.COUNTER_MODULUS) << p.COUNTER_SHIFT) - dat[5], dat[6] = raw >> 8, raw & 255 - dat[7] = p.VERSION | (checksum4(dat) << p.CHECKSUM_SHIFT) - return addr, bytes(dat), bus - - -def lateral(packer, bus, command, *, canfd, counter): - values = {"HandsOffCnfm_B_Rq": 0, "LatCtlRampType_D_Rq": 2 if command.active else 0, - "LatCtlPrecision_D_Rq": command.precision, "LatCtlPathOffst_L_Actl": 0.0, - "LatCtlPath_An_Actl": -command.path_angle, "LatCtlCurv_No_Actl": 0.0} - if canfd: - values.update({"LatCtl_D2_Rq": int(command.active), "LatCtlCrv_NoRate2_Actl": 0.0, - "LatCtlPath_No_Cnt": counter % 16, "LatCtlPath_No_Cs": 0}) - dat = packer.make_can_msg("LateralMotionControl2", bus, values)[1] - fields = ((dat[2] << 3) | (dat[3] >> 5), (dat[6] << 3) | (dat[7] >> 5), - ((dat[3] & 31) << 6) | (dat[4] >> 2), ((dat[4] & 3) << 8) | dat[5]) - total = int(command.active) + counter % 16 + sum(v + (v >> 8) for v in fields) - values["LatCtlPath_No_Cs"] = 255 - (total & 255) - return packer.make_can_msg("LateralMotionControl2", bus, values) - values.update({"LatCtl_D_Rq": int(command.active), "LatCtlRng_L_Max": 0, "LatCtlCurv_NoRate_Actl": 0.0}) - return packer.make_can_msg("LateralMotionControl", bus, values) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/can/fordcan_ext.py b/opendbc_repo/opendbc/bluepilot_lateral/can/fordcan_ext.py new file mode 100644 index 0000000000..25d2b8a4c5 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/can/fordcan_ext.py @@ -0,0 +1,128 @@ +""" +BluePilot Ford CAN message builder extensions (lateral only). + +Extended versions of the stock fordcan.py lateral builders: + - Dynamic ramp_type and precision_type for the 4-signal lateral commands + - Lane_Assist_Data1 carries the angle-mode side channel read by bluepilot_lateral/safety/ford.h + +Trimmed for the lateral proof-of-concept: the longitudinal, HUD and ICBM builders from +BluePilot's fordcan_ext.py are out of scope here. +""" + +from ...car.ford.fordcan import CanBus + + +# BluePilot: the Lane_Assist_Data1 side channel, see LANE_ASSIST_DATA1.md. ford_declarations.h has the +# same values (tests/test_lane_assist_data1.py). +BP_LKA_ANGLE_MODE_BYTE = 4 +BP_LKA_ANGLE_MODE_MASK = 0x1 +BP_LKA_SHADOW_MSB_BYTE = 5 # shadow curvature: int16, big-endian, bytes 5-6 +BP_LKA_SHADOW_LSB_BYTE = 6 +BP_LKA_SHADOW_CURVATURE_SCALE = 1e-6 # 1/meter per raw unit + + +def create_lka_msg(packer, CAN: CanBus, lat_active: bool, hud_control, + angle_mode_engaged: bool = False, shadow_curvature: float = 0.0): + """ + Creates a CAN message for the Ford LKA Command. + + BluePilot extension: accepts lat_active and hud_control parameters for future + lane departure warning integration (unused so far). + + Also carries angle_mode_engaged + shadow_curvature (Ford sign) in bits with no DBC signal: the + layout and what the panda does with them are in LANE_ASSIST_DATA1.md. + + Frequency is 33Hz. + """ + addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) + dat = bytearray(dat) + + shadow_curvature_raw = int(round(shadow_curvature / BP_LKA_SHADOW_CURVATURE_SCALE)) + shadow_curvature_raw = max(-32768, min(32767, shadow_curvature_raw)) & 0xFFFF + + dat[BP_LKA_ANGLE_MODE_BYTE] |= BP_LKA_ANGLE_MODE_MASK if angle_mode_engaged else 0 + dat[BP_LKA_SHADOW_MSB_BYTE] = (shadow_curvature_raw >> 8) & 0xFF + dat[BP_LKA_SHADOW_LSB_BYTE] = shadow_curvature_raw & 0xFF + + return addr, bytes(dat), bus + + +def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, ramp_type: int, precision_type: int, + path_offset: float, path_angle: float, curvature: float, curvature_rate: float): + """ + Creates a CAN message for the Ford TJA/LCA Command (non-CAN FD). + + BluePilot extension: dynamic ramp_type and precision_type parameters. + Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise). + + Ford lane centering uses a third-order polynomial to describe the road centerline: + c0 (path_offset): lateral offset between vehicle and centerline (positive is right) + c1 (path_angle): heading angle between vehicle and centerline (positive is right) + c2 (curvature): curvature of the centerline (positive is left) + c3 (curvature_rate): rate of change of curvature + + Frequency is 20Hz. + """ + values = { + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, + # 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": precision_type, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return packer.make_can_msg("LateralMotionControl", CAN.main, values) + + +def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int, + path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + """ + Creates a CAN message for the Ford Lane Centering command (CAN FD). + + BluePilot extension: dynamic ramp_type and precision_type parameters. + Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise). + + This message replaces LateralMotionControl on CAN FD platforms and includes + counter and checksum fields. + + Frequency is 20Hz. + """ + values = { + "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, + # 3=SafeRampOut, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": ramp_type, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": precision_type, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians + "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter + "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 + "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] + "LatCtlPath_No_Cnt": counter, # [0|15] + "LatCtlPath_No_Cs": 0, # [0|255] + } + + # Computed here, not left to the packer: upstream's packer fills Ford checksums itself (and overwrites + # this with the same value), but a host's packer may not (IQ.Pilot's doesn't), and the PSCM drops a + # frame whose checksum is wrong. + dat = packer.make_can_msg("LateralMotionControl2", CAN.main, values)[1] + values["LatCtlPath_No_Cs"] = lat_ctl2_checksum(mode, counter, dat) + return packer.make_can_msg("LateralMotionControl2", CAN.main, values) + + +def lat_ctl2_checksum(mode: int, counter: int, dat: bytes) -> int: + """LatCtlPath_No_Cs of a packed LateralMotionControl2: upstream's ford_checksum over the message's + checksum fields (mode, counter, curvature, curvature rate, path angle, path offset).""" + curvature = (dat[2] << 3) | (dat[3] >> 5) + curvature_rate = (dat[6] << 3) | (dat[7] >> 5) + path_angle = ((dat[3] & 0x1F) << 6) | (dat[4] >> 2) + path_offset = ((dat[4] & 0x3) << 8) | dat[5] + + checksum = mode + counter + for sig_val in (curvature, curvature_rate, path_angle, path_offset): + checksum += sig_val + (sig_val >> 8) + return 0xFF - (checksum & 0xFF) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py b/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py deleted file mode 100644 index 700a859b56..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py +++ /dev/null @@ -1,212 +0,0 @@ -"""Angle-primary Ford steering adapted from Alan Polk's BluePilot bp-7.0. - -Source e1d051d7ba270261b4455068bd68f1a58db15a4a lateral_angle_ext.py. -See ../LICENSE, ../LICENSE.md and ../CREDITS.md. This core has no vehicle, -openpilot, Params, messaging or model-constant imports. -""" -from dataclasses import dataclass -import math -from ..params import limits as bounds -from .trim import LaneCenterTrim, LaneGeometry, TrimConfig - - -def clip(value, low, high): - return max(low, min(high, value)) - - -def interp(value, xs, ys): - if value <= xs[0]: - return ys[0] - for i in range(1, len(xs)): - if value < xs[i]: - return ys[i - 1] + (ys[i] - ys[i - 1]) * (value - xs[i - 1]) / (xs[i] - xs[i - 1]) - return ys[-1] - - -@dataclass(frozen=True) -class Inputs: - active: bool - speed: float - measured_curvature: float - desired_curvature: float - steering_pressed: bool = False - steering_angle: float = 0.0 - yaw_predictions: tuple = () - time_indices: tuple = () - lateral_delay: float = 0.15 - lane_change: bool = False - lane_direction: int = 0 - limit_status: int = 0 - geometry: LaneGeometry | None = None - speed_min: float | None = None - speed_max: float | None = None - - -@dataclass(frozen=True) -class Command: - active: bool = False - path_angle: float = 0.0 - shadow_curvature: float = 0.0 - precision: int = 1 - - -class AngleController: - def __init__(self, *, low_gain=1.0, high_gain=1.15, trim_config=None, recovery_blips=True): - self.trim_config = TrimConfig() if trim_config is None else trim_config - self.recovery_blips = recovery_blips - self.trim = LaneCenterTrim() - self.low_gain = low_gain - self.high_gain = high_gain - self.path_last = 0.0 - self.speed_last = 0.1 - self.desired_last = 0.0 - self.human_timer = 0.0 - self.press_last = False - self.preturned = False - self.press_timer = 0.0 - self.blip_frames = 0 - self.blip_cooldown = 0.0 - self.blip_hold = 0.0 - self.blip_count = 0 - - def reset_actuator(self): - self.trim.reset() - self.path_last = 0.0 - self.speed_last = 0.1 - - def reset(self): - self.reset_actuator() - self.desired_last = 0.0 - self.human_timer = 0.0 - self.press_last = False - self.preturned = False - self.press_timer = 0.0 - self.blip_frames = 0 - self.blip_cooldown = 0.0 - self.blip_hold = 0.0 - self.blip_count = 0 - - def step(self, i: Inputs) -> Command: - values = (i.speed, i.measured_curvature, i.desired_curvature, i.steering_angle, i.lateral_delay) - if not i.active or not all(math.isfinite(v) for v in values) or i.speed < 0.0: - self.reset() - return Command() - speed_min = max(i.speed, 0.1) if i.speed_min is None else i.speed_min - speed_max = max(i.speed, 0.1) if i.speed_max is None else i.speed_max - if (not all(math.isfinite(v) for v in (speed_min, speed_max)) or - not 0 < speed_min <= max(i.speed, 0.1) <= speed_max): - self.reset() - return Command() - if i.steering_pressed and not self.press_last: - self.preturned = abs(i.steering_angle) > 45.0 - self.press_last = i.steering_pressed - self.human_timer = self.human_timer + 0.05 if i.steering_pressed and abs(i.steering_angle) > 45.0 else 0.0 - if self.human_timer + 1e-9 >= (3.0 if self.preturned else 1.5): - self.trim.reset() - self.path_last = 0.0 - self.desired_last = i.desired_curvature - self.blip_frames = self.blip_count = 0 - self.blip_hold = self.blip_cooldown = self.press_timer = 0.0 - return Command() - if i.steering_pressed: - self.press_timer += 0.05 - else: - if self.recovery_blips and self.press_timer >= 0.5 and self.blip_cooldown <= 0.0 and self.blip_frames <= 0 and abs(self.path_last) < 0.1: - self.blip_frames = 6 - self.press_timer = 0.0 - if self.blip_frames > 0: - self.blip_frames -= 1 - self.trim.reset() - self.path_last = 0.0 - self.desired_last = i.desired_curvature - if self.blip_frames == 0: - self.blip_cooldown = 2.0 - return Command() - - base = clip(i.lateral_delay, 0.1, 0.15) + 0.05 - predictions_valid = (len(i.yaw_predictions) == len(i.time_indices) and len(i.time_indices) >= 17 and - all(math.isfinite(v) for v in (*i.yaw_predictions, *i.time_indices)) and - all(b > a for a, b in zip(i.time_indices, i.time_indices[1:], strict=False))) - curves = tuple(y / max(0.01, i.speed) for y in i.yaw_predictions) if predictions_valid else () - entering = bool(curves) and abs(interp(base, i.time_indices, curves)) > abs(i.desired_curvature) - factor = 1.0 if entering else interp(abs(i.desired_curvature), (0.005, 0.020), (1.0, 0.0)) - lookup = base + bounds.VLT_EXTRA * interp(i.speed, (11.176, 24.5872), (1.0, 0.0)) * factor - predicted = interp(lookup, i.time_indices, curves) if curves else 0.0 - saturated = i.limit_status >= 2 or self.path_last >= 0.5235 * 0.9 or self.path_last <= -0.5 * 0.9 - falling = abs(i.desired_curvature) < abs(self.desired_last) - 0.010 - blend = bounds.BLEND * 0.25 if not entering and (i.limit_status >= 1 or saturated or falling) else bounds.BLEND - requested = predicted * blend + i.desired_curvature * (1.0 - blend) - self.desired_last = i.desired_curvature - precision = 0 if i.lane_change and ((i.lane_direction == 1 and requested < 0) or - (i.lane_direction == 2 and requested > 0)) else 1 - # BluePilot's default lane-change factors are both 1.0. - # Trim requested curvature before every existing actuator/safety limiter. - requested = self.trim.update(requested, i.speed, i.geometry, self.trim_config, - active=i.active, lane_change=i.lane_change, steering_pressed=i.steering_pressed) - kappa = clip(requested, i.measured_curvature - 0.002, i.measured_curvature + 0.002) if i.speed > 9.0 else requested - low = interp(i.speed, (13.5, 26.82), (1.0, self.low_gain)) - high = interp(i.speed, (13.5, 26.82), (1.3, self.high_gain)) - gain = interp(abs(kappa), (0.0007, 0.001), (low, high)) - path = kappa * i.speed * gain - if saturated: - last_mag, magnitude = abs(self.path_last), abs(path) - if magnitude > last_mag: - path = self.path_last - elif last_mag - magnitude > 0.02: - path = math.copysign(last_mag - 0.02, self.path_last) - elif i.limit_status >= 1: - path = clip(path, -abs(self.path_last), abs(self.path_last)) - roc = interp(i.speed, (10.0, 15.0, 25.0), (0.055, 0.0425, 0.009)) - path = clip(clip(path, -0.5235, 0.5), self.path_last - roc, self.path_last + roc) - - # Tightening relative to bp-7.0: the shadow describes the final actuator, - # including ROC/saturation. A truthful derived curvature must fit both the - # measured-curvature interval and the modern ISO envelope. If those bands - # cannot meet the previous actuator ROC, send a neutral mode-0 frame. - effective_speed = max(i.speed, 0.1) - cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(speed_max, 1.0) ** 2) - low_kappa, high_kappa = -cap, cap - if i.speed > 9.0: - low_kappa = max(low_kappa, i.measured_curvature - 0.0018) - high_kappa = min(high_kappa, i.measured_curvature + 0.0018) - # Native cannot trust a host-selected feel gain. Intersect every admitted - # gain (0.95..1.3), so the worst equivalent curvature remains bounded. - lower_gain = bounds.GAIN_MAX if low_kappa >= 0.0 else bounds.GAIN_MIN - upper_gain = bounds.GAIN_MIN if high_kappa >= 0.0 else bounds.GAIN_MAX - lower_speed = speed_max if low_kappa >= 0.0 else speed_min - upper_speed = speed_min if high_kappa >= 0.0 else speed_max - lower = max(-bounds.PATH_MAX, low_kappa * lower_speed * lower_gain, self.path_last - roc) - upper = min(-bounds.PATH_MIN, high_kappa * upper_speed * upper_gain, self.path_last + roc) - # Bound the actual-angle equivalent derivative at the worst admitted gain. - # 45ms core margin remains inside the native elapsed-time/50ms cap. - jerk_delta = bounds.LATERAL_JERK / max(i.speed - 1.0, 1.0) ** 2 * bounds.CORE_STEER_PERIOD_S * bounds.GAIN_MIN * effective_speed - jerk_center = self.path_last / self.speed_last * effective_speed - lower = max(lower, jerk_center - jerk_delta) - upper = min(upper, jerk_center + jerk_delta) - lower += bounds.PATH_ROUNDING_MARGIN - upper -= bounds.PATH_ROUNDING_MARGIN - if lower > upper: - self.trim.reset() - self.path_last = 0.0 - return Command() - path = clip(path, lower, upper) - self.path_last = path - self.speed_last = effective_speed - shadow = path / (effective_speed * gain) - - self.blip_cooldown = max(0.0, self.blip_cooldown - 0.05) - gap = i.desired_curvature - i.measured_curvature - stalled = (not i.steering_pressed and not i.lane_change and i.speed > 9.0 and abs(gap) > 0.004 and - abs(i.desired_curvature) > abs(i.measured_curvature)) - if self.recovery_blips and stalled: - if abs(kappa - requested) > 1e-9 and self.blip_cooldown <= 0.0: - self.blip_hold += 0.05 - if self.blip_hold >= 0.5 and self.blip_count < 3 and abs(path) < 0.1: - self.blip_frames = 6 - self.blip_hold = 0.0 - self.blip_count += 1 - else: - self.blip_hold = 0.0 - if i.steering_pressed or abs(gap) < 0.002: - self.blip_count = 0 - return Command(True, path, shadow, precision) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/carcontroller_ext.py b/opendbc_repo/opendbc/bluepilot_lateral/core/carcontroller_ext.py new file mode 100644 index 0000000000..f94b2967e0 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/carcontroller_ext.py @@ -0,0 +1,234 @@ +""" +BluePilot Ford lateral: the one object the Ford CarController calls. + +Holds the two strategies, LateralCurvExt and LateralAngleExt, with the inputs and state they +share, and does the strategy dispatch and message assembly. Lives above both strategy modules +(rather than inside either) so it can call into both without the circular import that would +result from one depending on the other. + +The CarController owns apply_curvature_last, which the stock and BluePilot paths share: it is +passed into step_lateral and the result returned, so switching BluePilot lateral off mid-drive +continues from the last command, as before. +""" + +import math +from types import SimpleNamespace + +try: + from ...car import scale_rot_inertia_per_mass, scale_tire_stiffness_per_mass +except ImportError: + # a host on opendbc from before the per-mass vehicle model (IQ.Pilot): its mass-scaled helpers at unit + # mass are the per-mass values + from ...car import scale_rot_inertia, scale_tire_stiffness + + def scale_rot_inertia_per_mass(wheelbase): + return scale_rot_inertia(1.0, wheelbase) + + def scale_tire_stiffness_per_mass(wheelbase, center_to_front, tire_stiffness_factor): + return scale_tire_stiffness(1.0, wheelbase, center_to_front, tire_stiffness_factor) +from ...car.ford.values import CarControllerParams, FordFlags +from ...car.vehicle_model import VehicleModel +from ..can import fordcan_ext +from .lateral_angle_ext import LateralAngleExt +from .lateral_curv_ext import LateralCurvExt, PrimaryLateralControl +from ..params.manifest import value +from ..params.param_store import ParamStore +from .values_ext import FORD_PINION_WHEELBASE, FordSafetyFlagsSP +from .human_turn import HumanTurnDetector +from ..host_types import LateralInputs, LateralStatus +from .lateral_inputs import ModelView, VehicleParamsView, lateral_inputs_complete + + +def pinion_vehicle_model(CP): + """Vehicle model for the pinion-angle curvature measurement: CarSpecs, except where BluePilot's + pinion geometry uses another wheelbase (FORD_PINION_WHEELBASE, the same value as the C table), + keeping the platform's weight split and tire model.""" + wheelbase = FORD_PINION_WHEELBASE.get(CP.carFingerprint) + if wheelbase is None: + return VehicleModel(CP) + center_to_front = wheelbase * CP.centerToFront / CP.wheelbase + front, rear = scale_tire_stiffness_per_mass(wheelbase, center_to_front, CP.tireStiffnessFactor) + inertia = scale_rot_inertia_per_mass(wheelbase) + # Both VehicleModel signatures: upstream's per-mass fields, and the mass-scaled ones an older host's + # model reads, at unit mass (mass cancels out of the model, so the two are the same model). + return VehicleModel(SimpleNamespace( + rotationalInertiaPerMass=inertia, wheelbase=wheelbase, + centerToFront=center_to_front, steerRatio=CP.steerRatio, + tireStiffnessFrontPerMass=front, tireStiffnessRearPerMass=rear, + mass=1.0, rotationalInertia=inertia, tireStiffnessFront=front, tireStiffnessRear=rear, steerRatioRear=0.)) + + +class InputTracker: + """The model / vehicle-parameter inputs both strategies read, and the car's measured curvature. + Owned by BluePilotLateral and updated once per frame from the host's LateralInputs, before either + strategy runs.""" + + def __init__(self, CP, safety_param: int): + # Model / vehicle-parameter inputs arrive per frame from the host (see update) + self.VM = pinion_vehicle_model(CP) # used only for the pinion-angle curvature measurement + self.model = None + self.lp = None + self.lateral_delay = 0.0 + + # BluePilot: steering-angle curvature measurement (bad-yaw-sensor workaround). + # Mirrors the STEER_ANGLE_CURVATURE flag the safety firmware reads from + # current_safety_param_sp -- both layers must always agree, so this is init-time + # state from the startup safety bits (values_ext.ford_safety_param), never a live settings read: + # a live flip against stale firmware would fight the panda. + self.bp_pinion_curvature_enabled = bool(safety_param & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) + + def get_current_curvature(self, CS): + """Measured curvature of the car right now (OP sign convention). + + The single measurement source for every BluePilot lateral consumer: the deviation + clip, the stall detector, and the shadow curvature published to ford.h's angle-mode + deviation check. The default source is the RCM yaw rate -- the same family stock + ford.h derives its angle_meas from. The shadow value judged against that check must + always come from the same measurement as the check's own reference, so route all + reads through here. + + With the steering-angle curvature measurement enabled (FordPrefSteerAngleCurvature + toggle -> FordSafetyFlagsSP.STEER_ANGLE_CURVATURE), the pinion angle via the vehicle + model is used instead: some vehicles (e.g. a 2021 Explorer with a faulty RCM) + broadcast implausible VehYaw_W_Actl (sign-inverted vs IMU and steering geometry) + while its CAN quality flag still reads OK. The pinion angle (SteeringPinion_Data, + PSCM) is an equivalent measurement, independently validated against the comma IMU + (corr +0.99), and the panda safety angle_meas switches to the same source (see + bluepilot_lateral/safety/ford.h) -- the layers always agree. angleOffsetDeg/roll come from + liveParameters (paramsd, IMU-derived, not the car yaw sensor). + """ + if self.bp_pinion_curvature_enabled: + angle_offset_deg = self.lp.angleOffsetDeg if self.lp is not None else 0.0 + roll = self.lp.roll if self.lp is not None else 0.0 + return -self.VM.calc_curvature(math.radians(CS.out.steeringAngleDeg - angle_offset_deg), + CS.out.vEgoRaw, roll) + return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + + def update(self, li: LateralInputs): + """Take this frame's model / vehicle-parameter inputs from the host and update the vehicle model. + + A frame is used only if it has the full modelV2 shape this code indexes into (33 path points, + both inner lane lines, 3+ probs/stds); anything else is ignored and the previous inputs are + kept, exactly as a stale SubMaster message was. That also makes fuzzed or partial inputs + safe: the producer's valid flag is necessary but not trusted on its own.""" + if li.valid and lateral_inputs_complete(li): + self.model = ModelView(li) + self.lp = VehicleParamsView(li) + self.lateral_delay = li.lateralDelay + + if self.lp is not None: + x = max(self.lp.stiffnessFactor, 0.1) + sr = max(self.lp.steerRatio, 0.1) + self.VM.update_params(x, sr) + + +class SharedLateralState: + """The values both strategies read and write, so either mode sees what the other left, as when + the strategies were mixins on one object. bp_curvature_deviation_limited is the BluePilot UI's + deviation flag, from whichever mode ran.""" + + def __init__(self): + self.path_angle_last = 0.0 + self.precision_type = 1 # 1=Precise, 0=Comfortable + self.bp_curvature_deviation_limited = False + self.human_turn_detector = HumanTurnDetector() + + +class BluePilotLateral: + def __init__(self, CP, safety_param: int, packer, CAN): + """safety_param: the startup safety bits the panda was given (values_ext.ford_safety_param).""" + self.CP = CP + self.frame = 0 + self.inputs = InputTracker(CP, safety_param) + self.shared = SharedLateralState() + self.curv = LateralCurvExt(self.inputs, self.shared) + self.angle = LateralAngleExt(CP, self.inputs, self.shared) + self.packer = packer + self.CAN = CAN + + self.disable_BP_lat_UI = False + self.lateral_uncertainty = 0.0 # from whichever strategy ran last, for status() + # the panda only accepts 4-signal messages when this bit was set at init + self.bp_lateral_allowed = bool(safety_param & FordSafetyFlagsSP.BP_LATERAL) + + def update(self, inputs: LateralInputs, params: ParamStore, frame): + """Every frame: take this frame's model / vehicle-parameter inputs and settings, both from the host + (HOSTS.md). Returns whether BluePilot lateral is active.""" + self.frame = frame + self.inputs.update(inputs) + self.curv.update_lateral_params(params) + self.angle.update_angle_params(params) + # One flag for every BP consumer (LatCtl dispatch, LKA angle-mode bits, mode reporting): the UI + # toggle, and the panda bit that was set at init. No 4-signal traffic to a stock-configured panda. + self.disable_BP_lat_UI = value(params, "disable_BP_lat_UI") or not self.bp_lateral_allowed + return not self.disable_BP_lat_UI + + def step_lateral(self, CC, CS, actuators, apply_curvature_last): + """At STEER_STEP: the steer message(s). Returns (can_sends, apply_curvature): the CarController + keeps it and passes it back next time as apply_curvature_last.""" + # BluePilot: select the BP lateral strategy by primary control variable. + # angle -> LateralAngleExt: kappa -> path_angle (c1), apply_curvature held at 0. + # curvature -> LateralCurvExt: full 4-signal curvature-primary (default). + # Both return a LateralResult packed identically below. + # Do not run the stock curvature limits here or overwrite apply_curvature_last before the + # strategy runs. Panda rate-checks desired_curvature vs the last TX on the bus; that must match + # the prior frame's lat.apply_curvature only (not an intermediate stock-limited value). + angle_mode = self.curv.primary_lateral_control == PrimaryLateralControl.angle + if angle_mode: + lat = self.angle.update_angle_strategy(CC, CS, actuators) + else: + lat = self.curv.update(CC, CS, actuators, apply_curvature_last, self.CP) + apply_curvature = float(lat.apply_curvature) + self.lateral_uncertainty = float(lat.lateralUncertainty) + + # BluePilot: angle-mode human-turn override -- send lateral inactive (mode 0) while the + # driver manually turns, so the PSCM releases cleanly instead of stalling 2-3 s on + # re-engage (observed on Mach-E). Panda-clean: every ford.h check has a legitimate + # !steer_control_enabled branch for the zeroed frames; on release, path_angle ramps back + # from 0 through the soft ROC. Curvature mode keeps its own reset_steering path (zeroed + # signals with mode still active) in LateralCurvExt. + # The stall blip (lateral_angle_ext.py) rides the same mode-0 path: a short pulse that + # resets the PSCM's post-override attenuation when the deviation clip deadlocks hands-free. + lat_active = CC.latActive and not (angle_mode and (self.angle.angle_human_turn_active or self.angle.angle_stall_blip_active)) + + can_sends = [] + if self.CP.flags & FordFlags.CANFD: + mode = 1 if lat_active else 0 + counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 + can_sends.append(fordcan_ext.create_lat_ctl2_msg( + self.packer, self.CAN, mode, lat.ramp_type, lat.precision_type, + -lat.path_offset, -lat.path_angle, -lat.apply_curvature, -lat.curvature_rate, counter + )) + else: + can_sends.append(fordcan_ext.create_lat_ctl_msg( + self.packer, self.CAN, lat_active, lat.ramp_type, lat.precision_type, + -lat.path_offset, -lat.path_angle, -lat.apply_curvature, -lat.curvature_rate + )) + return can_sends, apply_curvature + + def status(self) -> LateralStatus: + """This frame's values for a BluePilot UI (HOSTS.md), as BP7 sets them on its car controller.""" + angle_mode = self.curv.primary_lateral_control == PrimaryLateralControl.angle + return LateralStatus( + lateralUncertainty=self.lateral_uncertainty, + angleRateLimited=bool(angle_mode and self.angle.bp_angle_rate_limited), + curvatureRateLimited=bool(angle_mode and self.angle.bp_curvature_rate_limited), + curvatureDeviationLimited=bool(self.shared.bp_curvature_deviation_limited), + humanTurnLateralPaused=bool(angle_mode and self.angle.angle_human_turn_active), + stallBlipActive=bool(angle_mode and self.angle.angle_stall_blip_active), + disable_BP_lat_UI=bool(self.disable_BP_lat_UI), + primary_lateral_control=int(self.curv.primary_lateral_control), + ) + + def step_lka(self, CC, hud_control): + """At LKA_STEP: the Lane_Assist_Data1 message.""" + # BluePilot: tell bluepilot_lateral/safety/ford.h whether angle mode is engaged, packed into + # Lane_Assist_Data1's unused bits. shadow_curvature is negated to match the + # path_angle/apply_curvature wire convention that ford.h's angle_meas is calibrated against. + # Only called while BluePilot lateral is active (update returned True this frame), so the mode + # alone decides. + angle_mode_engaged = self.curv.primary_lateral_control == PrimaryLateralControl.angle + shadow_curvature = -self.angle.bp_kappa_cmd if angle_mode_engaged else 0.0 + return fordcan_ext.create_lka_msg( + self.packer, self.CAN, CC.latActive, hud_control, angle_mode_engaged, shadow_curvature + ) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/human.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/core/human_turn.py similarity index 97% rename from opendbc_repo/opendbc/bluepilot_lateral/tests/reference/human.py.txt rename to opendbc_repo/opendbc/bluepilot_lateral/core/human_turn.py index 96f05760ee..12ff8a1b56 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/human.py.txt +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/human_turn.py @@ -7,8 +7,8 @@ each mode DOES with the signal differs: curvature-primary (``lateral_curv_ext``) (reset_steering + post-reset ramp); angle-primary (``lateral_angle_ext``) forces lateral inactive (mode 0 on the wire) and ramps path_angle back in from zero through its soft ROC on release. """ -from opendbc.car import DT_CTRL -from opendbc.car.ford.values import CarControllerParams +from ...car import DT_CTRL +from ...car.ford.values import CarControllerParams # Require sustained hands-on AND a large angle (avoids resetting on small wheel nudges in a curve). HUMAN_TURN_ANGLE_DEG = 45.0 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/bpdev_trim.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/core/lane_center_trim.py similarity index 92% rename from opendbc_repo/opendbc/bluepilot_lateral/tests/reference/bpdev_trim.py.txt rename to opendbc_repo/opendbc/bluepilot_lateral/core/lane_center_trim.py index 5eedebd79e..a8a310954c 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/bpdev_trim.py.txt +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/lane_center_trim.py @@ -14,10 +14,10 @@ for a lane-centering nudge, and it bypasses every limiter angle mode already app This class instead computes a small correction in the **curvature domain**, meant to be added to ``kappa_cmd`` before any of those limiters run -- so the correction automatically inherits all of -them, with no new safety-relevant code path. This mirrors where a GM-focused fork (StarPilot, -u/jc01rho -- ``selfdrive/controls/lib/drive_helpers.py::LaneCenteringController`` on -``feat/lane-centering-camera-offset``) places their own lane-centering trim: on the universal -``desired_curvature`` signal, upstream of any car-specific actuator conversion. The geometry here +them, with no new safety-relevant code path. The core is ported from StarPilot's +``selfdrive/controls/lib/lane_centering.py`` (``LaneCenteringController``, by jc01rho; see +CREDITS.md), which applies its trim the same way: on the universal ``desired_curvature`` signal, +upstream of any car-specific actuator conversion. The geometry here (``raw_correction = 2*error / lookahead**2``) is the same ``y ~= 1/2 * kappa * x**2`` relation BluePilot already uses for PSCM d_ref / path_angle elsewhere in this package, solved for the curvature that would eliminate a lateral position error at the lookahead distance. @@ -184,6 +184,12 @@ class LaneCenterTrim: return 0.0, 0.0 if not (np.all(np.diff(left_x) > 0) and np.all(np.diff(right_x) > 0)): return 0.0, 0.0 + if not all(np.isfinite(v) for v in (probs[1], probs[2], stds[1], stds[2])): + # Unlike the arrays above, a NaN here doesn't raise -- it flows straight into min()/ + # np.interp() and poisons confidence (and therefore the correction) with a NaN that + # survives every subsequent frame until a hard reset, instead of erroring or getting + # clipped like everything else in this function. + return 0.0, 0.0 left = float(np.interp(lookahead, left_x, left_y)) right = float(np.interp(lookahead, right_x, right_y)) @@ -194,7 +200,8 @@ class LaneCenterTrim: # probability via min() -- a single missing/unreliable line (e.g. no line on the curb # side, only a center stripe) drags confidence toward 0 on its own. width_tolerance = float(np.interp(width, _WIDTH_TOLERANCE_BP, _WIDTH_TOLERANCE_V)) - std_tolerance = float(np.interp(max(float(stds[1]), float(stds[2])), _STD_TOLERANCE_BP, _STD_TOLERANCE_V)) #StarPilot stopped at 0.3, this fades the std through the table + # StarPilot stopped at 0.3, this fades the std through the table + std_tolerance = float(np.interp(max(float(stds[1]), float(stds[2])), _STD_TOLERANCE_BP, _STD_TOLERANCE_V)) confidence = min(float(probs[1]), float(probs[2]), width_tolerance, std_tolerance) #confidence is the weakest signal scale = float(np.clip(np.interp(confidence, _CONFIDENCE_BP, _CONFIDENCE_V), 0.0, 1.0)) center_y = 0.5 * (left + right) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/angle.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_angle_ext.py similarity index 75% rename from opendbc_repo/opendbc/bluepilot_lateral/tests/reference/angle.py.txt rename to opendbc_repo/opendbc/bluepilot_lateral/core/lateral_angle_ext.py index 886ffca8a2..fce3b99c49 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/angle.py.txt +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_angle_ext.py @@ -2,55 +2,49 @@ BluePilot: Ford CAN-FD path-angle–primary lateral control (developer). Steering intent is c1 from κ → θ. Converts planner/model curvature into ``path_angle`` using -PSCM short lookahead d_ref and y ≈ ½κ x² ⇒ path_angle = ½ κ d_ref (see -``bluepilot/agent_info/20_FORD_PSCM_KNOWLEDGE_PACK.md``). Predicted curvature (modelV2) is -blended with ``actuators.curvature`` per ``FordPathAngleBlendRatio`` (0 = planner only, +PSCM short lookahead d_ref and y ≈ ½κ x² ⇒ path_angle = ½ κ d_ref. Predicted curvature +(modelV2) is blended with ``actuators.curvature`` by ``path_angle_blend_ratio`` (0 = planner only, 1 = model only). -**c0 (path_offset) is always zero on the wire, unconditionally.** Angle mode has no centering -trim -- an earlier port attempt piped a small additive trim onto path_angle through the -curv-mode ``LC_PID_controller``, but it never actually tracked lane center correctly in this -mode and was removed; only the DBC-required zero c0 remains. +**c0 (path_offset) is always zero on the wire, unconditionally.** An earlier port attempt piped +a small additive trim onto path_angle through the curv-mode ``LC_PID_controller``, but it never +actually tracked lane center correctly in this mode: path_angle here is a derived quantity +(``kappa_cmd * v_ego * curvature_factor``), so an additive trim in that domain has the wrong +(inverted) speed-dependence for a lane-centering nudge, and it bypassed every limiter this file +applies to ``kappa_cmd``. That attempt was removed; only the DBC-required zero c0 remains. + +**Lane centering trim (``lane_center_trim.py``)** replaces it: a small correction applied to +``kappa_cmd`` itself (see ``LaneCenterTrim``), before the deviation clip / gain table / PSCM +clamp / soft ROC below -- so it inherits every one of those limiters automatically instead of +bypassing them. Blends toward lane-line center by confidence (same formula as +``lateral_curv_ext``'s ``path_offset``) and falls back to the model's own predicted path -- not +to zero -- when lines are missing/unreliable, so the user's left/right offset still applies on +center-stripe-only roads. Disabled during lane changes, user-tunable (enable, offset, authority) +via ``enable_lane_positioning_ang`` / ``custom_path_offset_ang`` / ``lane_centering_strength_ang``. **Human-turn override**: while the driver manually turns (same sustained-press + angle criteria as ``lateral_curv_ext``, via the shared ``HumanTurnDetector``), lateral is forced inactive (mode 0, all-zero signals) instead of winding path_angle into a stale command the PSCM has to reconcile on release -- on the Mach-E's PSCM that reconciliation cost 2-3 s of dead time before control resumed. Mode 0 is panda-clean by construction: every ford.h check has a legitimate -!steer_control_enabled branch, so no reset-bypass latch involvement. On release, path_angle ramps -back in from zero through the soft ROC below (no jump seed) -- generous at human-turn speeds, and -admitted by ford.h's path_angle ROC check (2% looser) without any bypass. +!steer_control_enabled branch. On release, path_angle ramps back in from zero through the soft +ROC below (no jump seed) -- generous at human-turn speeds, and admitted by ford.h's path_angle +ROC check (2% looser). """ import numpy as np from numpy import clip, interp -from opendbc.car import DT_CTRL -from opendbc.car.lateral import apply_std_steer_angle_limits -from opendbc.car.ford.values import CAR, CarControllerParams -from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralResult -from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector -from opendbc.sunnypilot.car.ford.values_ext import BP_ANGLE_LIMITS -from selfdrive.modeld.constants import ModelConstants - -# Hard-coded per-platform gain defaults. -# CAN vehicles (Escape MK4, Bronco Sport, Explorer, Maverick, Edge) -_GAIN_CAN = (1.00, 1.15) -# CAN-FD body-on-frame trucks (F-150, Lightning, Expedition, Ranger) -_GAIN_CANFD_BOF = (0.95, 0.95) -# CAN-FD unibody SUVs (Mustang Mach-E, Escape MK4.5) -_GAIN_CANFD_SUV = (1.00, 1.05) - -_CANFD_BOF_CARS = frozenset({ - CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, - CAR.FORD_EXPEDITION_MK4, - CAR.FORD_RANGER_MK2, -}) -_CANFD_SUV_CARS = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, - CAR.FORD_ESCAPE_MK4_5, -}) - +from ...car import DT_CTRL +from ...car.lateral import apply_std_steer_angle_limits +from ...car.ford.values import CarControllerParams +from .lateral_curv_ext import LateralResult +from .lane_center_trim import LaneCenterTrim +from .values_ext import ( + ANGLE_GAIN_CAN, ANGLE_GAIN_CANFD_BOF, ANGLE_GAIN_CANFD_BOF_CARS, ANGLE_GAIN_CANFD_SUV, ANGLE_GAIN_CANFD_SUV_CARS, + BP_ANGLE_LIMITS, LANE_CHANGE_FACTOR_BP, LANE_CHANGE_FACTOR_LOW, +) +from .values_ext import MODEL_T_IDXS +from ..params.manifest import default, value # DBC ``LatCtlPath_An_Actl`` (rad) — panda safety uses the same in ``ford.h``; PSCM enforces in firmware. FORD_DBC_PATH_ANGLE_MIN = -0.5 @@ -125,49 +119,62 @@ def pscm_d_ref_m(v_ego_ms: float) -> float: return d +# the settings angle mode reads (params/manifest.py): (attribute, key) +ANGLE_SETTINGS = ( + ("low_speed_curv_factor", "FordLowSpeedFactor_ang"), + ("high_speed_curv_factor", "FordHighSpeedFactor_ang"), + ("user_dampening_factor", "FordHighSpeedDampening_ang"), + ("lane_change_factor_high_ang", "lane_change_factor_high_ang"), + ("enable_lane_positioning_ang", "enable_lane_positioning_ang"), # the lane centering trim (lane_center_trim.py) + ("custom_path_offset_ang", "custom_path_offset_ang"), + ("lane_centering_strength_ang", "lane_centering_strength_ang"), +) + + class LateralAngleExt: - def __init__(self, CP=None, CP_SP=None): - # Predicted-curvature blend for path_angle: pred * b + desired * (1-b); b from ``FordPathAngleBlendRatio`` + def __init__(self, CP, inputs, shared): + self.CP = CP + # the user settings (updated from the host's params each frame), at their defaults until then + for attr, key in ANGLE_SETTINGS: + setattr(self, attr, default(key)) + # BluePilotLateral's LateralInputs (read) and SharedLateralState (read and written), common to both strategies + self.inputs = inputs + self.shared = shared + # Predicted-curvature blend for path_angle: pred * b + desired * (1-b); b is this fixed default self.path_angle_blend_ratio = _FORD_PATH_ANGLE_BLEND_RATIO_DEFAULT - # Max extra VLT above t_base; from ``FordVLTExtraMax`` param + # Max extra VLT above t_base (fixed) self.vlt_extra_max = _VLT_T_EXTRA_MAX - # Telemetry: final path_angle (rad) after limits (see bp_card_publisher) + # Telemetry: final path_angle (rad) after limits (for the BluePilot UI's bp_card_publisher) self.bp_path_angle_final = 0.0 # High-speed gain factors: set per-platform via carFingerprint in update_angle_params. self.path_angle_gain_lowC_highV = 1.0 # dampening at high speed, low curvature self.path_angle_gain_highC_highV = 1.0 # gain at high speed, high curvature self.bp_path_angle_gain_lowC_highV = 1.0 self.bp_path_angle_gain_highC_highV = 1.0 - # User-tunable "feel" multipliers: read from the angle-tuning Params below. - self.low_speed_curv_factor = 1.0 - self.high_speed_curv_factor = 1.0 - self.user_dampening_factor = 1.0 + # Telemetry copies of the user "feel" multipliers (ANGLE_SETTINGS) self.bp_low_speed_curv_factor = 1.0 self.bp_high_speed_curv_factor = 1.0 - # BluePilot: angle mode's own lane-change scaling factor, independent of curvature mode's - # lane_change_factor_high_curv -- angle needs a boost (>1) where curvature needs a cut (<1). - self.lane_change_factor_high_ang = 1.0 + # BluePilot: angle-mode lane centering trim (advanced lane positioning) -- see + # lane_center_trim.py and the module docstring above. + self.lane_center_trim = LaneCenterTrim() # Telemetry: variable curvature lookup time used this frame (s) self.bp_curvature_lookup_time = _VLT_T_EXTRA_MAX + 0.3725 # warm start at ~0.5s - # BluePilot: error-clipped kappa path_angle was derived from -- carcontroller.py reads this as + # BluePilot: error-clipped kappa path_angle was derived from -- carcontroller_ext.py reads this as # shadow_curvature for ford.h's angle-mode deviation check. Actively consumed, not telemetry. self.bp_kappa_cmd = 0.0 - # BluePilot: rate-limit diagnostics (controllerStateBP) + # BluePilot: rate-limit diagnostics (the BluePilot UI's controllerStateBP) self.bp_angle_rate_limited = False # path_angle soft-ROC clip actually bit this frame self.bp_curvature_rate_limited = False # equivalent curvature would be rate-limited by curv-mode logic (sim) - self.bp_curvature_deviation_limited = False # current_curvature error-clip constrained kappa_cmd this frame self.sim_curvature_last = 0.0 # shadow curvature-mode last for the curvatureRateLimited sim # Exit detection: track previous desired curvature to sense when planner is actively reducing self._desired_curvature_last = 0.0 # Human-turn override: while the driver manually turns, lateral is forced inactive (mode 0, # all-zero signals) instead of winding path_angle into a stale command the PSCM can't cleanly # reconcile on release (2-3 s re-engage dead time observed on Mach-E). See module docstring. - # Note: in CarController this attribute is shared with LateralCurvExt (same mixin instance) -- - # only one lateral strategy runs per frame, so a single detector serves both. - self.human_turn_detector = HumanTurnDetector() - self.angle_human_turn_active = False # read by carcontroller to force mode 0 + # The detector itself is in BluePilotLateral's shared state: one detector serves both modes. + self.angle_human_turn_active = False # read by BluePilotLateral.step_lateral to force mode 0 # Post-override stall blip state (see module constants). angle_stall_blip_active is read by - # carcontroller to force mode 0, exactly like angle_human_turn_active. + # BluePilotLateral.step_lateral to force mode 0, exactly like angle_human_turn_active. self.stall_blip_hold_s = 0.0 # accumulated deviation-clip-binding time toward a pulse self.stall_blip_frames_left = 0 # remaining pulse frames; > 0 -> mode 0 on the wire self.stall_blip_cooldown_s = 0.0 # re-arm delay after a pulse @@ -177,47 +184,28 @@ class LateralAngleExt: def update_angle_params(self, params): """Sets per-platform gain defaults and reads user angle-tuning params.""" - self._ensure_lateral_curv_initialized(self.CP) fp = getattr(self.CP, 'carFingerprint', '') - if fp in _CANFD_BOF_CARS: - low, high = _GAIN_CANFD_BOF - elif fp in _CANFD_SUV_CARS: - low, high = _GAIN_CANFD_SUV + if fp in ANGLE_GAIN_CANFD_BOF_CARS: + low, high = ANGLE_GAIN_CANFD_BOF + elif fp in ANGLE_GAIN_CANFD_SUV_CARS: + low, high = ANGLE_GAIN_CANFD_SUV else: - low, high = _GAIN_CAN + low, high = ANGLE_GAIN_CAN self.path_angle_gain_lowC_highV = low self.path_angle_gain_highC_highV = high - if params is not None and hasattr(params, "get"): - for attr, key, min_value, max_value in ( - ("low_speed_curv_factor", "FordLowSpeedFactor_ang", 0.5, 1.5), - ("high_speed_curv_factor", "FordHighSpeedFactor_ang", 0.5, 1.5), - ("user_dampening_factor", "FordHighSpeedDampening_ang", 0.25, 1.25), - ): - try: - raw = params.get(key, return_default=True) - if raw is not None and raw != b"": - setattr(self, attr, float(clip( - float(raw.decode("utf-8", errors="replace") if isinstance(raw, bytes) else raw), min_value, max_value))) - except Exception: - pass - try: - raw = params.get("lane_change_factor_high_ang", return_default=True) - if raw is not None and raw != b"": - self.lane_change_factor_high_ang = float(clip( - float(raw.decode("utf-8", errors="replace") if isinstance(raw, bytes) else raw), 0.85, 1.50)) - except Exception: - pass + if params is not None: + for attr, key in ANGLE_SETTINGS: + setattr(self, attr, value(params, key)) - def update_angle_strategy(self, CC, CS, actuators, CP): + def update_angle_strategy(self, CC, CS, actuators): """ - Curvature from planner (+ optional predicted blend) → path_angle via ½·κ·d_ref. - c0 (path_offset) is always zero on the wire -- no centering trim in angle mode. c2 and c3 are zero. + Curvature from planner (+ optional predicted blend, + lane centering trim) → path_angle via + ½·κ·d_ref. c0 (path_offset) is always zero on the wire; the lane centering trim lives entirely + in the curvature domain (kappa_cmd), not on c0. c2 and c3 are zero. Blended κ is not passed through Ford c2 rate / DBC limits (those target the curvature actuator). """ - self._ensure_lateral_curv_initialized(CP) - v_ego = float(CS.out.vEgoRaw) - d_ref = pscm_d_ref_m(v_ego) + d_ref = pscm_d_ref_m(v_ego) # noqa: F841 # BluePilot: unused today; kept verbatim from bp-dev curvature_rate = 0.0 path_offset = 0.0 @@ -227,12 +215,11 @@ class LateralAngleExt: precision = 1 if not CC.latActive: - self.path_angle_last = 0.0 + self.shared.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False + self.shared.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 # Publish the shadow curvature from the measured curvature while inactive. LKA keeps # carrying angle_mode_engaged whenever angle mode is configured (independent of @@ -242,16 +229,17 @@ class LateralAngleExt: # ford.h's deviation check compares a zero shadow against real measured curvature. # (ford.h skips the check while steer_control_enabled is 0, so the value is free to # follow the measurement during the inactive period itself.) - self.bp_kappa_cmd = self.get_current_curvature(CS) - self.human_turn_detector.reset() + self.bp_kappa_cmd = self.inputs.get_current_curvature(CS) + self.shared.human_turn_detector.reset() self.angle_human_turn_active = False + self.lane_center_trim.reset() self.stall_blip_hold_s = 0.0 self.stall_blip_frames_left = 0 self.stall_blip_cooldown_s = 0.0 self.stall_blip_count = 0 self.angle_stall_blip_active = False self.press_timer_s = 0.0 - self.precision_type = 1 + self.shared.precision_type = 1 return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -263,28 +251,28 @@ class LateralAngleExt: ) # Human-turn override: sustained driver press + large wheel angle → force lateral inactive - # (carcontroller drops mode to 0; all signals are zero on the wire) so path_angle can't wind + # (BluePilotLateral.step_lateral drops mode to 0; all signals are zero on the wire) so path_angle can't wind # into a stale command while the driver turns. Always on in angle mode (no param gate) -- the # curv-suffixed human-turn toggle belongs to curvature mode's reset strategy, and the Mach-E # PSCM re-engage stall this prevents is not something a user should be able to opt out of. # On release, no jump seed: path_angle_last is 0, so the normal flow below ramps the command - # back in through the soft ROC -- generous at human-turn speeds, no panda bypass involved. - self.angle_human_turn_active = self.human_turn_detector.update( + # back in through the soft ROC -- generous at human-turn speeds. + self.angle_human_turn_active = self.shared.human_turn_detector.update( True, CS.out.steeringPressed, CS.out.steeringAngleDeg) if self.angle_human_turn_active: - self.path_angle_last = 0.0 + self.shared.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False + self.shared.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 # Truthful shadow during the override (mirrors the inactive path -- see the comment # there): the driver is steering, so the honest command is the car's actual curvature, # and the panda-latched shadow stays current for the re-engage frame. - self.bp_kappa_cmd = self.get_current_curvature(CS) + self.bp_kappa_cmd = self.inputs.get_current_curvature(CS) # Keep exit detection current so resume doesn't compare against a stale pre-turn value. self._desired_curvature_last = float(actuators.curvature) + self.lane_center_trim.reset() # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also # covers the press so far: only press time accumulated AFTER the latch releases should earn # a hand-off pulse. @@ -294,7 +282,7 @@ class LateralAngleExt: self.stall_blip_count = 0 self.angle_stall_blip_active = False self.press_timer_s = 0.0 - self.precision_type = 1 + self.shared.precision_type = 1 return LateralResult( apply_curvature=0.0, curvature_rate=0.0, @@ -314,7 +302,7 @@ class LateralAngleExt: else: if (self.press_timer_s >= _PRESS_BLIP_MIN_S and self.stall_blip_cooldown_s <= 0.0 and self.stall_blip_frames_left <= 0 - and abs(self.path_angle_last) < _BLIP_MAX_PATH_ANGLE): + and abs(self.shared.path_angle_last) < _BLIP_MAX_PATH_ANGLE): self.stall_blip_frames_left = _STALL_BLIP_FRAMES self.press_timer_s = 0.0 @@ -326,17 +314,17 @@ class LateralAngleExt: if self.stall_blip_frames_left > 0: self.stall_blip_frames_left -= 1 self.angle_stall_blip_active = True - self.path_angle_last = 0.0 + self.shared.path_angle_last = 0.0 self.bp_path_angle_final = 0.0 - self.apply_curvature_last = 0.0 self.bp_angle_rate_limited = False self.bp_curvature_rate_limited = False - self.bp_curvature_deviation_limited = False + self.shared.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 # Truthful shadow during the blip (see the inactive-path comment). - self.bp_kappa_cmd = self.get_current_curvature(CS) + self.bp_kappa_cmd = self.inputs.get_current_curvature(CS) self._desired_curvature_last = float(actuators.curvature) - self.precision_type = 1 + self.lane_center_trim.reset() + self.shared.precision_type = 1 if self.stall_blip_frames_left <= 0: self.stall_blip_cooldown_s = _STALL_COOLDOWN_S return LateralResult( @@ -350,9 +338,9 @@ class LateralAngleExt: ) self.angle_stall_blip_active = False - self.precision_type = 1 + self.shared.precision_type = 1 precision = 1 - LP = self.lp + LP = self.inputs.lp # noqa: F841 # BluePilot: unused today; kept verbatim from bp-dev desired_curvature = float(actuators.curvature) # Variable lookup time: t_base tracks planner pre-compensation; extra tapers on high speed and large curves. @@ -361,14 +349,14 @@ class LateralAngleExt: # curvature, kappa_entering stays True, and the exit-biased blend is permanently disabled — causing the car # to command max path_angle through the entire apex. 0.15s gives t_base ≤ 0.20s and VLT ≤ 0.33s, restoring # the 2.8m lookahead that kept kappa_entering False at the apex in successful earlier runs. - _t_base = float(clip(self.sm['liveDelay'].lateralDelay, 0.1, 0.15)) + _DT_MDL + _t_base = float(clip(self.inputs.lateral_delay, 0.1, 0.15)) + _DT_MDL _speed_factor = float(interp(v_ego, [_VLT_V_LOW_MS, _VLT_V_HIGH_MS], [1.0, 0.0])) # Direction-aware kappa factor: on curve ENTRY (model shows more curvature at t_base than planner now), # keep full lookahead so pre-steering begins early. On exit/apex, taper by magnitude to prevent unwind. _kappa_at_t_base = 0.0 - if self.model is not None and len(self.model.orientationRate.z) >= 17: - _curvatures_ref = np.array(self.model.orientationRate.z) / max(0.01, v_ego) - _kappa_at_t_base = abs(float(interp(_t_base, ModelConstants.T_IDXS, _curvatures_ref))) + if self.inputs.model is not None and len(self.inputs.model.orientationRate.z) >= 17: + _curvatures_ref = np.array(self.inputs.model.orientationRate.z) / max(0.01, v_ego) + _kappa_at_t_base = abs(float(interp(_t_base, MODEL_T_IDXS, _curvatures_ref))) _kappa_entering = _kappa_at_t_base > abs(desired_curvature) if _kappa_entering: _kappa_factor = 1.0 # curve deepening ahead: full extra lookahead for gradual entry @@ -378,10 +366,10 @@ class LateralAngleExt: self.bp_curvature_lookup_time = curvature_lookup_time predicted_curvature = 0.0 - if self.model is not None and len(self.model.orientationRate.z) >= 17: - curvatures = np.array(self.model.orientationRate.z) / max(0.01, v_ego) + if self.inputs.model is not None and len(self.inputs.model.orientationRate.z) >= 17: + curvatures = np.array(self.inputs.model.orientationRate.z) / max(0.01, v_ego) predicted_curvature = float( - interp(curvature_lookup_time, ModelConstants.T_IDXS, curvatures) + interp(curvature_lookup_time, MODEL_T_IDXS, curvatures) ) b = float(self.path_angle_blend_ratio) @@ -400,8 +388,8 @@ class LateralAngleExt: # That caused a positive-feedback flat-line: under-steer → CtrSat → path_angle frozen → more under-steer. # Use DBC-limit proximity instead: only block when path_angle is already near the ±0.5 rad CAN limits, # which is the only condition where the anti-snap unwind rate cap makes physical sense. - _dbc_sat = (self.path_angle_last >= FORD_DBC_PATH_ANGLE_MAX * 0.90 or - self.path_angle_last <= FORD_DBC_PATH_ANGLE_MIN * 0.90) + _dbc_sat = (self.shared.path_angle_last >= FORD_DBC_PATH_ANGLE_MAX * 0.90 or + self.shared.path_angle_last <= FORD_DBC_PATH_ANGLE_MIN * 0.90) _in_hard_sat = _pscm_lim >= 2 or _dbc_sat # BluePilot: per-call delta threshold. The original 0.002 was authored 2026-05-07 on # bp-sid-simple (9c3d000fd), which ran STEER_STEP=1 (true 100Hz, switched 2026-04-22) -- so it @@ -415,26 +403,36 @@ class LateralAngleExt: requested_curvature = predicted_curvature * b_blend + desired_curvature * (1.0 - b_blend) self._desired_curvature_last = desired_curvature - if self.model is not None: - self.lane_change = self.model.meta.laneChangeState in (1, 2, 3) + if self.inputs.model is not None: + self.lane_change = self.inputs.model.meta.laneChangeState in (1, 2, 3) else: self.lane_change = False lane_change_factor = interp( - v_ego, self.lane_change_factor_bp, [self.lane_change_factor_low, self.lane_change_factor_high_ang] + v_ego, LANE_CHANGE_FACTOR_BP, [LANE_CHANGE_FACTOR_LOW, self.lane_change_factor_high_ang] ) - if self.lane_change and self.model is not None: - if self.model.meta.laneChangeDirection == 1 and requested_curvature < 0: + if self.lane_change and self.inputs.model is not None: + if self.inputs.model.meta.laneChangeDirection == 1 and requested_curvature < 0: requested_curvature *= lane_change_factor precision = 0 - elif self.model.meta.laneChangeDirection == 2 and requested_curvature > 0: + elif self.inputs.model.meta.laneChangeDirection == 2 and requested_curvature > 0: requested_curvature *= lane_change_factor precision = 0 - self.precision_type = precision + self.shared.precision_type = precision # Use planner / predicted κ directly for the κ → path_angle map; we are not sending κ on CAN. kappa_cmd = float(requested_curvature) + # BluePilot: lane centering trim (advanced lane positioning) -- nudges kappa_cmd toward true + # lane-line center + user offset, gated on lane-line confidence and disabled during lane + # changes (see lane_center_trim.py). Applied here, before the deviation clip below, so the + # trimmed value inherits every limiter this file already applies to kappa_cmd instead of + # bypassing them. + kappa_cmd = self.lane_center_trim.update( + kappa_cmd, self.inputs.model, v_ego, self.enable_lane_positioning_ang, + self.custom_path_offset_ang, self.lane_centering_strength_ang, + CC.latActive, self.lane_change) + # BluePilot: clip kappa_cmd to current_curvature (measured, from yaw rate) +- CURVATURE_ERROR, # mirroring lateral_curv_ext.py's apply_ford_curvature_limits_ext exactly (same formula, same # v_ego > 9 gate, same CarControllerParams.CURVATURE_ERROR tolerance). Without this, kappa_cmd @@ -444,20 +442,18 @@ class LateralAngleExt: # routinely, not just on genuine pothole/override divergence. Curvature mode has always clipped # here; this brings angle mode's actual steering intent in line with that proven behavior rather # than only clipping the value reported to panda (which would make the check a no-op). - current_curvature = self.get_current_curvature(CS) - self.bp_curvature_deviation_limited = False + current_curvature = self.inputs.get_current_curvature(CS) + self.shared.bp_curvature_deviation_limited = False if v_ego > 9: _kappa_cmd_pre_error_clip = kappa_cmd kappa_cmd = float(clip(kappa_cmd, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR)) # BluePilot: did this clip actually constrain kappa_cmd this frame (deviation from measured, - # not rate-of-change -- see carcontroller.py)? - self.bp_curvature_deviation_limited = bool(abs(kappa_cmd - _kappa_cmd_pre_error_clip) > 1e-9) + # not rate-of-change)? The BluePilot UI's deviation flag. + self.shared.bp_curvature_deviation_limited = bool(abs(kappa_cmd - _kappa_cmd_pre_error_clip) > 1e-9) lateral_uncertainty = 0.0 # no curvature-limit ladder until angle-mode torque display is defined - - # Speed-interpolated gain: at low speed both curves use 1.0; at high speed the params take effect. self.low_gain_calc = interp( v_ego, [13.5, 26.82], [1.0, (self.path_angle_gain_lowC_highV * self.user_dampening_factor)] @@ -470,7 +466,6 @@ class LateralAngleExt: path_angle_calc = kappa_cmd * v_ego * self.curvature_factor path_angle = path_angle_calc - # PSCM authority limit clamp. # On CANFD Fords in angle mode, LatCtlLim_D_Stat does not fire, so _pscm_lim stays 0. # _in_hard_sat (computed above) combines _pscm_lim >= 2 with _dbc_sat (path_angle near ±0.5 rad limit). @@ -479,7 +474,7 @@ class LateralAngleExt: # Without the decrease cap, model+planner drop path_angle at ~0.36 rad/s at a sharp apex, # driving desired steering 30°+ ahead of actual while the PSCM is pinned, causing a snap when released. if _in_hard_sat: - _last = self.path_angle_last + _last = self.shared.path_angle_last _last_mag = abs(_last) _curr_mag = abs(path_angle) if _curr_mag > _last_mag: # magnitude growing — block @@ -488,7 +483,7 @@ class LateralAngleExt: _limited_mag = _last_mag - _PSCM_SAT_UNWIND_RATE path_angle = float(_limited_mag if _last >= 0 else -_limited_mag) elif _pscm_lim >= 1: # LimitClose (F150/non-angle-mode only): block increases only - path_angle = float(clip(path_angle, -abs(self.path_angle_last), abs(self.path_angle_last))) + path_angle = float(clip(path_angle, -abs(self.shared.path_angle_last), abs(self.shared.path_angle_last))) path_angle = min(FORD_DBC_PATH_ANGLE_MAX, max(FORD_DBC_PATH_ANGLE_MIN, path_angle)) @@ -504,12 +499,11 @@ class LateralAngleExt: _soft_roc = float(interp(v_ego, [9., 10., 15., 25.], [0.055, 0.055, 0.0425, 0.009])) _path_angle_pre_roc = path_angle path_angle = float(clip(path_angle, - self.path_angle_last - _soft_roc, - self.path_angle_last + _soft_roc)) + self.shared.path_angle_last - _soft_roc, + self.shared.path_angle_last + _soft_roc)) # BluePilot: did the soft ROC clip actually limit the path_angle we wanted to send this frame? self.bp_angle_rate_limited = bool(abs(path_angle - _path_angle_pre_roc) > 1e-9) - # c0 always zero -- no centering trim in angle mode. path_offset = 0.0 @@ -518,10 +512,9 @@ class LateralAngleExt: self.bp_path_angle_gain_highC_highV = self.path_angle_gain_highC_highV self.bp_low_speed_curv_factor = self.low_speed_curv_factor self.bp_high_speed_curv_factor = self.high_speed_curv_factor - self.path_angle_last = path_angle + self.shared.path_angle_last = path_angle self.bp_path_angle_final = path_angle - self.apply_curvature_last = 0.0 - # BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller.py reads this + # BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller_ext.py reads this # as shadow_curvature for ford.h's angle-mode deviation check (see fordcan_ext.create_lka_msg). # Not just telemetry: an actively-consumed value, unlike the removed *_kappa_cmd_raw stubs. # While the driver is pressing (before the human-turn override latches), the clipped planner @@ -530,7 +523,7 @@ class LateralAngleExt: # in-drive lateral safety block observed across ~3h of replayed road-test routes was exactly # this (driver fighting a sustained curve with the mode still enabled). The honest command # during a press is the driver's actual curvature. - self.bp_kappa_cmd = self.get_current_curvature(CS) if CS.out.steeringPressed else kappa_cmd + self.bp_kappa_cmd = self.inputs.get_current_curvature(CS) if CS.out.steeringPressed else kappa_cmd # BluePilot: would the equivalent curvature (kappa_cmd) have been rate-limited by curvature-mode's # ROC (apply_std_steer_angle_limits)? kappa_cmd is already error-clipped above (same clip @@ -551,10 +544,10 @@ class LateralAngleExt: and abs(_stall_gap) > _STALL_GAP_MIN and abs(desired_curvature) > abs(current_curvature)) if _stalled: - if self.bp_curvature_deviation_limited and self.stall_blip_cooldown_s <= 0.0: + if self.shared.bp_curvature_deviation_limited and self.stall_blip_cooldown_s <= 0.0: self.stall_blip_hold_s += _STEER_DT if (self.stall_blip_hold_s >= _STALL_HOLD_S and self.stall_blip_count < _STALL_MAX_BLIPS - and abs(self.path_angle_last) < _BLIP_MAX_PATH_ANGLE): + and abs(self.shared.path_angle_last) < _BLIP_MAX_PATH_ANGLE): self.stall_blip_frames_left = _STALL_BLIP_FRAMES self.stall_blip_hold_s = 0.0 self.stall_blip_count += 1 @@ -565,13 +558,12 @@ class LateralAngleExt: ramp_type = 2 - return LateralResult( apply_curvature=0.0, curvature_rate=curvature_rate, path_offset=path_offset, path_angle=path_angle, ramp_type=ramp_type, - precision_type=self.precision_type, + precision_type=self.shared.precision_type, lateralUncertainty=lateral_uncertainty, ) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_curv_ext.py b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_curv_ext.py new file mode 100644 index 0000000000..8bdd151db9 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_curv_ext.py @@ -0,0 +1,448 @@ +""" +BluePilot Ford lateral curvature extension. + +Implements Ford's 4-signal lateral control (curvature, curvature_rate, path_offset, path_angle) +using predicted curvature from modelV2, PID-based lane centering, and laneline-aware path offset. +Held by BluePilotLateral (carcontroller_ext.py) as LateralCurvExt. + +Ford uses four signals to control steering: + - curvature: primary steering command (also used in upstream, limited to 0.02 m^-1) + - curvature_rate: always 0, like upstream and stock Ford + - path_offset: lateral offset for lane centering (blends model + laneline data) + - path_angle: heading angle correction via PID controller + +A detailed explanation of Ford's control protocol: +https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706 +""" + +from collections import namedtuple, deque +from enum import IntEnum + +import numpy as np +from numpy import clip, interp + +from ...car.common.pid import PIDController +from ...car import ACCELERATION_DUE_TO_GRAVITY +from ...car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits +from ...car.ford.values import CarControllerParams, FordFlags +from .values_ext import BP_ANGLE_LIMITS, CURVATURE_MAX, LANE_CHANGE_FACTOR_BP, LANE_CHANGE_FACTOR_LOW +from .values_ext import MODEL_T_IDXS +from ..params.manifest import default, value + + +# the settings curvature mode reads (params/manifest.py), each kept as an attribute of the same name +CURV_SETTINGS = ("enable_human_turn_detection_curv", "lane_change_factor_high_curv", "pc_blend_ratio_high_C_UI_curv", + "pc_blend_ratio_low_C_UI_curv", "enable_lane_positioning_curv", "custom_path_offset_curv", + "enable_lane_full_mode_curv", "custom_profile_curv", "LC_PID_gain_UI_curv") + + +class PrimaryLateralControl(IntEnum): + curvature = 0 + angle = 1 + + +# CAN FD lateral-accel cap (match opendbc/car/ford/carcontroller.py apply_ford_curvature_limits) +AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation +MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL) + + +# Result namedtuple returned by LateralCurvExt.update() +LateralResult = namedtuple('LateralResult', [ + 'apply_curvature', + 'curvature_rate', + 'path_offset', + 'path_angle', + 'ramp_type', + 'precision_type', + 'lateralUncertainty', +]) + + +def apply_ford_curvature_limits_ext(apply_curvature, apply_curvature_last, current_curvature, + v_ego_raw, steering_angle, lat_active, CP): + """Extended version of apply_ford_curvature_limits that returns + (apply_curvature, max_curvature, curvature_deviation_limited). + + The max_curvature value is used by _calculate_lateral_uncertainty() for the steering + torque bar visualization. Stock version returns only apply_curvature. + + curvature_deviation_limited (BluePilot): True when the current_curvature +- CURVATURE_ERROR + clip below actually constrained the commanded curvature this frame -- i.e. the desired + maneuver was limited by deviation from measured current curvature, not by rate-of-change + (apply_std_steer_angle_limits below is a separate limiter with its own telemetry). + """ + max_curvature = 1 # large initial value + curvature_deviation_limited = False + + # No blending at low speed due to lack of torque wind-up and inaccurate current curvature + if v_ego_raw > 9: + apply_curvature_pre_error_clip = apply_curvature + apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, + current_curvature + CarControllerParams.CURVATURE_ERROR) + curvature_deviation_limited = bool(abs(apply_curvature - apply_curvature_pre_error_clip) > 1e-9) + max_curvature = abs(current_curvature) + CarControllerParams.CURVATURE_ERROR + + # Curvature rate limit after driver torque limit (same inputs/order as apply_ford_curvature_limits) + apply_curvature_before_std = apply_curvature + apply_curvature = apply_std_steer_angle_limits(apply_curvature_before_std, apply_curvature_last, v_ego_raw, + steering_angle, lat_active, BP_ANGLE_LIMITS) + + # Max one-step envelope for UI: steer_up must use pre-limit curvature (see apply_std_steer_angle_limits) + steer_up = (apply_curvature_last * apply_curvature_before_std >= 0.0 + and abs(apply_curvature_before_std) > abs(apply_curvature_last)) + rate_limits = BP_ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else BP_ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN + std_steer_angle_rate_limit = np.interp(v_ego_raw, rate_limits[0], rate_limits[1]) + std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit) + max_curvature = np.minimum(max_curvature, std_steer_angle_limit) + + # Ford CAN FD lateral acceleration limit (more torque available than CAN) + if CP.flags & FordFlags.CANFD: + curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) + apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) + max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) + + return apply_curvature, max_curvature, curvature_deviation_limited + + +class LateralCurvExt: + """ + BluePilot lateral control extension for Ford vehicles. + + Held by BluePilotLateral (carcontroller_ext.py), which calls + update() during the lateral control loop (20Hz) to get the full + 4-signal steering command instead of the upstream curvature-only approach. + """ + + def __init__(self, inputs, shared): + # BluePilotLateral's LateralInputs (read) and SharedLateralState (read and written), common to both strategies + self.inputs = inputs + self.shared = shared + + # Primary lateral control variable: consumed by BluePilotLateral's lateral dispatch. + self.primary_lateral_control = PrimaryLateralControl.curvature + + # Settings (updated from the host's params each frame), at their defaults until then + for key in CURV_SETTINGS: + setattr(self, key, default(key)) + + # Precision/ramp control (precision_type is in the shared state) + self.lateralUncertainty = 0.0 + + # Predicted curvature blending + self.curvature_lookup_time = 0.2 # seconds into the future for curvature extraction + self.pc_blend_ratio = 0.5 + self.pc_blend_ratio_bp = [0.0, 0.001] # curvature breakpoints (1/m) + self.pc_blend_ratio_low = 0.40 + self.pc_blend_ratio_high = 0.40 + self.pc_blend_ratio_low_C = 0.40 # from UI when custom_profile_curv == 1 + self.pc_blend_ratio_high_C = 0.40 # from UI when custom_profile_curv == 1 + + # Lane change smoothing + self.lane_change = False + self.lane_change_last = False + + # Post lane change transition + self.post_lane_change_timer = 0 + self.post_lane_change_active = False + self.pre_lane_change_values = {'path_angle': 0.0, 'path_offset': 0.0} + self.max_path_angle_change = 0.00125 + self.max_path_offset_change = 0.00125 + + # Human turn detection (the detector is in the shared state, used by both modes) + self.human_turn = False + self.post_reset_ramp_active = False + self.reset_steering_last = False + + # Path offset + self.path_offset_lookup_time = 0.2 # seconds + self.min_laneline_confidence_bp = [0.6, 0.8] + + # PID-based path angle for lane centering + self.path_angle_filter_samples = 3 + self.path_angle_deque = deque(maxlen=self.path_angle_filter_samples) + self.LC_PID_k_p = 0.25 + self.LC_PID_k_i = 0.05 + self.LC_PID_controller = PIDController(k_p=self.LC_PID_k_p, k_i=self.LC_PID_k_i, rate=20) + self.LC_PID_speed_bp = [0.0, 9.0, 15.0] # m/s + self.LC_PID_speed_v = [0.0, 0.0, 1.0] + self.LC_path_angle_ROC_bp = [5, 15, 25] # m/s + self.LC_path_angle_ROC_v = [0.003, 0.0015, 0.002] # match panda limits + self.LC_path_angle_reset_counter = 0 + self.LC_path_angle_reset_duration = 1.5 # seconds + + # DBC signal max limits + self.path_angle_max = 0.5 + self.path_offset_max = 2.0 + self.curvature_max = CURVATURE_MAX # 0.02 + + # Previous frame values + self.path_offset_last = 0.0 + + def update_lateral_params(self, params): + """Read lateral-related Params from the UI. Called each frame.""" + for key in CURV_SETTINGS: + setattr(self, key, value(params, key)) + + try: + self.primary_lateral_control = PrimaryLateralControl(value(params, "FordPrefLateralControl")) + except ValueError: # a mode other than 0 or 1: curvature mode, the default (#66) + self.primary_lateral_control = PrimaryLateralControl.curvature + + def update(self, CC, CS, actuators, apply_curvature_last, CP): + """ + Compute lateral steering signals for the current frame. + + Called at 20Hz from BluePilotLateral.step_lateral() when inside the STEER_STEP block. + + Args: + CC: CarControl with latActive, hudControl + CS: CarState with vEgoRaw, yawRate, steeringPressed, steeringAngleDeg + actuators: CC.actuators with desired curvature from planner + apply_curvature_last: previous frame's applied curvature + CP: CarParams with flags for CANFD etc. + + Returns: + LateralResult namedtuple with all signals needed for CAN message construction. + """ + apply_curvature = 0.0 + path_offset = 0.0 + path_angle = 0.0 + reset_steering = 0 + ramp_type = 2 + lateralUncertainty = 0.0 + curvature_deviation_limited = False + + if CC.latActive: + self.shared.precision_type = 1 + steeringPressed = CS.out.steeringPressed + steeringAngleDeg_PV = CS.out.steeringAngleDeg + + # Select tuning profile + if self.custom_profile_curv == 1: + self.pc_blend_ratio_low_C = self.pc_blend_ratio_low_C_UI_curv + self.pc_blend_ratio_high_C = self.pc_blend_ratio_high_C_UI_curv + + self.pc_blend_ratio_v = [self.pc_blend_ratio_low_C, self.pc_blend_ratio_high_C] + + # Current and desired curvature + current_curvature = self.inputs.get_current_curvature(CS) + desired_curvature = actuators.curvature + + # Extract predicted curvature from modelV2 + if self.inputs.model is not None and len(self.inputs.model.orientationRate.z) >= 17: + curvatures = np.array(self.inputs.model.orientationRate.z) / max(0.01, CS.out.vEgoRaw) + predicted_curvature = interp(self.curvature_lookup_time, MODEL_T_IDXS, curvatures) + else: + predicted_curvature = 0.0 + + # Blend predicted and desired curvature + self.pc_blend_ratio = interp(abs(desired_curvature), self.pc_blend_ratio_bp, self.pc_blend_ratio_v) + requested_curvature = (predicted_curvature * self.pc_blend_ratio) + (desired_curvature * (1 - self.pc_blend_ratio)) + + # Lane change detection + if self.inputs.model is not None: + self.lane_change = self.inputs.model.meta.laneChangeState in (1, 2, 3) + else: + self.lane_change = False + + # Lane change curvature smoothing + lane_change_factor = interp(CS.out.vEgoRaw, LANE_CHANGE_FACTOR_BP, + [LANE_CHANGE_FACTOR_LOW, self.lane_change_factor_high_curv]) + + if self.lane_change and self.inputs.model is not None: + if self.inputs.model.meta.laneChangeDirection == 1 and requested_curvature < 0: + requested_curvature *= lane_change_factor + self.shared.precision_type = 0 + elif self.inputs.model.meta.laneChangeDirection == 2 and requested_curvature > 0: + requested_curvature *= lane_change_factor + self.shared.precision_type = 0 + + # Human turn: sustained hands-on + large angle (not just a nudge) — see HumanTurnDetector + self.human_turn = self.shared.human_turn_detector.update( + self.enable_human_turn_detection_curv, steeringPressed, steeringAngleDeg_PV) + + # Steering reset logic + if self.human_turn or (CS.out.vEgoRaw < 0.1): + reset_steering = 1 + if reset_steering == 1: + requested_curvature = 0.0 + + # Apply curvature limits (extended version returning max_curvature) + apply_curvature, max_curvature, curvature_deviation_limited = apply_ford_curvature_limits_ext( + requested_curvature, apply_curvature_last, current_curvature, + CS.out.vEgoRaw, 0, CC.latActive, CP) + + # Lateral uncertainty for torque bar visualization + lateralUncertainty = self._calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) + + # Human turn / standstill reset: after apply_ford_curvature_limits_ext (for torque-bar max_curvature), + # force κ=0 and (below) path_angle=0 on the bus. ford.h accepts that frame (curvature 0 skips the + # curvature checks), but checks the frames after it as usual: a ramp back that exceeds the rate + # limit or sits outside the curvature error band is blocked. + if reset_steering == 1: + apply_curvature = 0.0 + self.post_reset_ramp_active = False + else: + # Detect transition from reset to normal + if self.reset_steering_last and not reset_steering: + self.post_reset_ramp_active = True + apply_curvature_last = 0.0 # Reset to ensure clean ramp from 0 + + # Post-reset ramp: gradually ramp from 0 to avoid safety limit trips + if self.post_reset_ramp_active: + apply_curvature = apply_std_steer_angle_limits( + requested_curvature, apply_curvature_last, + CS.out.vEgoRaw, 0, CC.latActive, BP_ANGLE_LIMITS) + + curvature_error = abs(requested_curvature - apply_curvature) + curvature_threshold = max(abs(requested_curvature) * 0.1, 0.001) + if curvature_error < curvature_threshold: + self.post_reset_ramp_active = False + + self.reset_steering_last = (reset_steering == 1) + + # Path offset: blend model position with laneline data + if self.inputs.model is not None: + path_offset_position = interp(self.path_offset_lookup_time, MODEL_T_IDXS, self.inputs.model.position.y) + path_offset_lanelines = (self.inputs.model.laneLines[1].y[0] + self.inputs.model.laneLines[2].y[0]) / 2 + + # Laneline width tolerance (prevents jumps when lanes merge/diverge) + laneline_width = self.inputs.model.laneLines[2].y[0] + (-self.inputs.model.laneLines[1].y[0]) + laneline_width_tolerance = interp(laneline_width, [3.75, 4.25], [0.81, 0.59]) + + # Laneline confidence + laneline_confidence = min(self.inputs.model.laneLineProbs[1], self.inputs.model.laneLineProbs[2], laneline_width_tolerance) + if not self.enable_lane_full_mode_curv: + laneline_confidence = 0.0 + + laneline_path_offset_scale = interp(laneline_confidence, self.min_laneline_confidence_bp, [0.0, 1.0]) + path_offset = ((path_offset_position * (1 - laneline_path_offset_scale)) + + (path_offset_lanelines * laneline_path_offset_scale)) + self.custom_path_offset_curv + + # No path offset during lane changes + if self.lane_change: + path_offset = 0 + + # PID-based path angle for lane centering + path_offset_error = path_offset * (self.LC_PID_gain_UI_curv / 100) + LC_PID_speed_factor = interp(CS.out.vEgoRaw, self.LC_PID_speed_bp, self.LC_PID_speed_v) + path_offset_error_adj = path_offset_error * LC_PID_speed_factor + + if not self.enable_lane_positioning_curv: + path_offset_error_adj = 0.0 + + path_angle_low_c = self.LC_PID_controller.update(path_offset_error_adj) + + if not self.enable_lane_positioning_curv: + path_angle_low_c = 0.0 + if reset_steering == 1: + path_angle_low_c = 0.0 + + # Rate limit path angle for comfort + path_angle_roc = interp(abs(CS.out.vEgoRaw), self.LC_path_angle_ROC_bp, self.LC_path_angle_ROC_v) + path_angle_low_c = clip(path_angle_low_c, self.shared.path_angle_last - path_angle_roc, self.shared.path_angle_last + path_angle_roc) + + # Reset PID if driver applies consistent steering pressure + if steeringPressed: + self.LC_path_angle_reset_counter += 1 + else: + self.LC_path_angle_reset_counter = 0 + if self.LC_path_angle_reset_counter > self.LC_path_angle_reset_duration * 20: + self.LC_PID_controller.reset() + + path_angle = path_angle_low_c # path_angle_high_c not used currently + + # Post lane change transition smoothing + path_angle, path_offset = self._handle_post_lane_change_transition(path_angle, path_offset) + + # Final reset handling + if reset_steering == 1: + path_angle = 0.0 + + # Clip all signals to DBC limits + # float() is required: numpy's clip returns np.float64, and capnp rejects that type when + # apply_curvature reaches actuators.curvature via apply_curvature_last in carcontroller.py. + # The cast is exact (both are IEEE 754 doubles), so no output value changes. + apply_curvature = float(clip(apply_curvature, -self.curvature_max, self.curvature_max)) + path_offset = float(clip(path_offset, -self.path_offset_max, self.path_offset_max)) + path_angle = float(clip(path_angle, -self.path_angle_max, self.path_angle_max)) + + # Zero path_offset before CAN send (path_offset and path_angle can conflict, causing discomfort) + path_offset = 0.0 + + # Ramp type selection + if reset_steering == 1: + ramp_type = 3 # Immediate + self.path_angle_deque.clear() + self.LC_PID_controller.reset() + else: + ramp_type = 2 # Fast + + else: + # Lateral control off — zero everything + apply_curvature = 0.0 + path_offset = 0.0 + path_angle = 0.0 + self.path_angle_deque.clear() + self.LC_PID_controller.reset() + ramp_type = 0 + lateralUncertainty = 0.0 + + # Update state for next frame + self.lateralUncertainty = lateralUncertainty + self.path_offset_last = path_offset + self.shared.path_angle_last = path_angle + # BluePilot: did the current_curvature +- CURVATURE_ERROR clip constrain the commanded + # curvature this frame (deviation from measured, not rate-of-change)? The BluePilot UI's deviation flag. + self.shared.bp_curvature_deviation_limited = curvature_deviation_limited + + return LateralResult( + apply_curvature=apply_curvature, + curvature_rate=0.0, # always 0, like stock Ford + path_offset=path_offset, + path_angle=path_angle, + ramp_type=ramp_type, + precision_type=self.shared.precision_type, + lateralUncertainty=lateralUncertainty, + ) + + def _handle_post_lane_change_transition(self, path_angle, path_offset): + """Smooth transition of control variables after lane change completes. + + Rate-limits path_angle and path_offset back to target values + over 160 frames (~8 seconds at 20Hz). + """ + # Detect lane change completion (True → False transition) + if self.lane_change_last and not self.lane_change: + self.post_lane_change_active = True + self.post_lane_change_timer = 0 + self.pre_lane_change_values = {'path_angle': 0.0, 'path_offset': 0.0} + + self.lane_change_last = self.lane_change + + if self.post_lane_change_active: + self.post_lane_change_timer += 1 + + new_path_angle = clip(path_angle, + self.pre_lane_change_values['path_angle'] - self.max_path_angle_change, + self.pre_lane_change_values['path_angle'] + self.max_path_angle_change) + new_path_offset = clip(path_offset, + self.pre_lane_change_values['path_offset'] - self.max_path_offset_change, + self.pre_lane_change_values['path_offset'] + self.max_path_offset_change) + + self.pre_lane_change_values = { + 'path_angle': new_path_angle, + 'path_offset': new_path_offset, + } + + if self.post_lane_change_timer >= 160: + self.post_lane_change_active = False + + return (new_path_angle, new_path_offset) + + return (path_angle, path_offset) + + def _calculate_lateral_uncertainty(self, requested_curvature, apply_curvature, max_curvature): + """Compute ratio of requested to max achievable curvature for the torque bar UI.""" + max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) + return float(requested_curvature / max_curvature) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_inputs.py b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_inputs.py new file mode 100644 index 0000000000..97d517b677 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/lateral_inputs.py @@ -0,0 +1,54 @@ +""" +BluePilot: views over the host's LateralInputs (host_types.py) in the modelV2 / vehicleParameters shape +the lateral code reads, and the check that a frame is complete. The host fills LateralInputs once per frame. +""" +import math +from itertools import chain +from types import SimpleNamespace + +from ..host_types import MODEL_PATH_POINTS + + +def lateral_inputs_complete(li) -> bool: + """True when every array the lateral indexes into has the shape modelV2 always provides, and + every value is finite. A NaN from the model doesn't raise in the lateral math; it reaches the + CAN encoders, which do, so the whole frame is dropped here and the previous inputs are kept.""" + return (len(li.pathYawRate) == MODEL_PATH_POINTS and len(li.pathX) == MODEL_PATH_POINTS and len(li.pathY) == MODEL_PATH_POINTS + and len(li.laneLineLeftX) == MODEL_PATH_POINTS and len(li.laneLineLeftY) == MODEL_PATH_POINTS + and len(li.laneLineRightX) == MODEL_PATH_POINTS and len(li.laneLineRightY) == MODEL_PATH_POINTS + and len(li.laneLineProbs) >= 3 and len(li.laneLineStds) >= 3 + and all(map(math.isfinite, chain(li.pathYawRate, li.pathX, li.pathY, li.laneLineLeftX, li.laneLineLeftY, + li.laneLineRightX, li.laneLineRightY, li.laneLineProbs, li.laneLineStds, + (li.steerRatio, li.stiffnessFactor, li.roll, li.angleOffsetDeg, li.lateralDelay))))) + + +class _LaneLine: + def __init__(self, x, y): + self.x = x + self.y = y + + +class ModelView: + """The subset of modelV2 the lateral code uses, with the same attribute paths.""" + + def __init__(self, li): + self.orientationRate = SimpleNamespace(z=li.pathYawRate) + self.position = SimpleNamespace(x=li.pathX, y=li.pathY) + # indices match modelV2.laneLines: 1 = left, 2 = right; 0 and 3 are not used + self.laneLines = [_LaneLine([], []), + _LaneLine(li.laneLineLeftX, li.laneLineLeftY), + _LaneLine(li.laneLineRightX, li.laneLineRightY), + _LaneLine([], [])] + self.laneLineProbs = li.laneLineProbs + self.laneLineStds = li.laneLineStds + self.meta = SimpleNamespace(laneChangeState=li.laneChangeState, laneChangeDirection=li.laneChangeDirection) + + +class VehicleParamsView: + """The subset of vehicleParameters the lateral code uses.""" + + def __init__(self, li): + self.steerRatio = li.steerRatio + self.stiffnessFactor = li.stiffnessFactor + self.roll = li.roll + self.angleOffsetDeg = li.angleOffsetDeg diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/trim.py b/opendbc_repo/opendbc/bluepilot_lateral/core/trim.py deleted file mode 100644 index 3d971982e6..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/core/trim.py +++ /dev/null @@ -1,104 +0,0 @@ -"""Pure angle lane-centering trim adapted from BluePilot bp-dev e22afa6b. - -Alan Polk and ghbarker; the source also credits StarPilot/u/jc01rho. -See ../CREDITS.md, ../LICENSE and ../LICENSE.md. Geometry/confidence/fallback, -filter and slew follow lane_center_trim.py; this module receives immutable arrays -instead of a host model. Stale geometry and driver pressure additionally suppress -trim, and nonfinite confidence cannot create correction. -""" -from dataclasses import dataclass -import math -from ..params.trim import DEFAULTS, BOUNDS - - -def clip(value, low, high): - return max(low, min(high, value)) - - -def interp(value, xs, ys): - if value <= xs[0]: - return ys[0] - for index in range(1, len(xs)): - if value < xs[index]: - return ys[index - 1] + (ys[index] - ys[index - 1]) * (value - xs[index - 1]) / (xs[index] - xs[index - 1]) - return ys[-1] - - -@dataclass(frozen=True) -class TrimConfig: - enabled: bool = DEFAULTS["enabled"] - offset_m: float = DEFAULTS["offset_m"] - gain: float = DEFAULTS["gain"] - - -@dataclass(frozen=True) -class LaneGeometry: - position_x: tuple = () - position_y: tuple = () - left_x: tuple = () - left_y: tuple = () - right_x: tuple = () - right_y: tuple = () - probabilities: tuple = () - standard_deviations: tuple = () - fresh: bool = False - - -def sample(xs, ys, distance): - if (not isinstance(xs, tuple) or not isinstance(ys, tuple) or len(xs) < 2 or len(xs) != len(ys) or - not all(math.isfinite(v) for v in (*xs, *ys)) or not all(b > a for a, b in zip(xs, xs[1:], strict=False))): - return None - return interp(distance, xs, ys) - - -class LaneCenterTrim: - def __init__(self): - self.correction = 0.0 - - def reset(self): - self.correction = 0.0 - - def update(self, requested, speed, geometry, config, *, active, lane_change, steering_pressed=False): - try: - valid = (isinstance(config, TrimConfig) and config.enabled is True and active and not lane_change and - not steering_pressed and isinstance(geometry, LaneGeometry) and geometry.fresh is True and - math.isfinite(requested) and math.isfinite(speed) and speed >= 0.0 and - math.isfinite(config.offset_m) and BOUNDS["offset_m"][0] <= config.offset_m <= BOUNDS["offset_m"][1] and - math.isfinite(config.gain) and BOUNDS["gain"][0] <= config.gain <= BOUNDS["gain"][1]) - authority = interp(speed, BOUNDS["speed_mps"], BOUNDS["speed_authority"]) if valid else 0.0 - if not valid or authority <= 0.0: - self.reset() - return requested - distance = clip(speed, *BOUNDS["lookahead_m"]) - model_y = sample(geometry.position_x, geometry.position_y, distance) - if model_y is None: - self.reset() - return requested - # Bad lane-only geometry retains the source's model-position/offset fallback. - scale, center = self.lane_blend(geometry, distance) - error = scale * (center - model_y) + config.offset_m - raw = 2.0 * error / distance ** 2 - target = clip(raw, -BOUNDS["raw_correction"], BOUNDS["raw_correction"]) * config.gain * authority - alpha = 1.0 - math.exp(-BOUNDS["period_s"] / BOUNDS["tau_s"]) - filtered = alpha * target + (1.0 - alpha) * self.correction - self.correction = clip(filtered, self.correction - BOUNDS["correction_delta"], self.correction + BOUNDS["correction_delta"]) - return requested + self.correction - except (AttributeError, IndexError, TypeError, ValueError, OverflowError): - self.reset() - return requested - - def lane_blend(self, geometry, distance): - try: - left = sample(geometry.left_x, geometry.left_y, distance) - right = sample(geometry.right_x, geometry.right_y, distance) - probs, stds = geometry.probabilities, geometry.standard_deviations - if (left is None or right is None or not isinstance(probs, tuple) or not isinstance(stds, tuple) or - len(probs) != 2 or len(stds) != 2 or not all(math.isfinite(p) and 0.0 <= p <= 1.0 for p in probs) or - not all(math.isfinite(s) and s >= 0.0 for s in stds)): - return 0.0, 0.0 - confidence = min(*probs, interp(right - left, BOUNDS["width_m"], BOUNDS["width_tolerance"]), - interp(max(stds), BOUNDS["std"], BOUNDS["std_tolerance"])) - scale = clip(interp(confidence, BOUNDS["confidence"], (0.0, 1.0)), 0.0, 1.0) - return scale, 0.5 * (left + right) - except (AttributeError, IndexError, TypeError, ValueError, OverflowError): - return 0.0, 0.0 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/values_ext.py b/opendbc_repo/opendbc/bluepilot_lateral/core/values_ext.py new file mode 100644 index 0000000000..2f7dd43cb2 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/values_ext.py @@ -0,0 +1,156 @@ +from ...car import structs +from ...car.lateral import AngleSteeringLimits +from .. import host_types +from ..params.manifest import PER_FRAME_KEYS, startup_value + + +class FordSafetyFlagsSP: + """BluePilot's Ford safety flags: the bits of the second safety param a host sends the panda + (current_safety_param_sp, USB control 0xdf), not the main safetyConfigs[].safetyParam (HOSTS.md). + ford_bp_init reads it with GET_FLAG(current_safety_param_sp, ...), same pattern as Subaru + STOP_AND_GO (subaru_common.h). Plain int constants, not IntFlag: the param must stay a plain int + through the host's serialization. + """ + # Bit layout of the SP safety param (mirrored in ford_bp_init, bluepilot_lateral/safety/ford.h; pinned by + # tests/test_safety_param_bits.py): + # bit 0 STEER_ANGLE_CURVATURE (pinion-sourced curvature measurement) + # bits 1-4 pinion geometry index (FORD_PINION_GEOMETRY_SHIFT / FORD_PINION_GEOMETRY_INDEX) + # bit 5 BP_LATERAL: the panda runs the 4-signal checks (ford_lmc_checks) instead of + # upstream's stock curvature-only checks. Clear = upstream safety, verbatim. + STEER_ANGLE_CURVATURE = 1 + BP_LATERAL = 1 << 5 + + +# Geometry-table index for the steering-angle curvature measurement, packed into +# the startup safety bits 1-4 when STEER_ANGLE_CURVATURE is set. Must match the +# ford_pinion_geometry table in bluepilot_lateral/safety/ford.h row for row. +# Index 0 is reserved as invalid: the firmware treats flag-set-but-no-index as feature +# off, so a half-configured param can never select the wrong geometry silently. +# Platforms with no row are in FORD_NO_PINION_PLATFORMS. Indices stay aligned with the C table. +# BluePilot: the per-platform tables key on the platform name (CP.carFingerprint), not the CAR enum, +# so a row for a platform some host lacks never breaks import there; it just never matches. +# FORD_HOST_ONLY_PLATFORMS lists the names with rows here that upstream opendbc's CAR doesn't have; any +# name a host's CAR lacks must be on it (tests/test_platform_tables.py). +FORD_HOST_ONLY_PLATFORMS: frozenset[str] = frozenset({ + "FORD_EDGE_MK2", # StarPilot + "FORD_MONDEO_MK5", # BP7, StarPilot + "FORD_TRANSIT_MK5", # StarPilot +}) +# No pinion geometry row, so no STEER_ANGLE_CURVATURE (the setting no-ops and the yaw source is kept): +FORD_NO_PINION_PLATFORMS: frozenset[str] = frozenset({ + # ALT_STEER_ANGLE: reads a RELATIVE pinion angle (SteeringPinion_Data_Alt + learned offset) and lacks the + # absolute measurement this needs + "FORD_EDGE_MK2", + # LKA steering through Lane_Assist_Data1 alone: no BluePilot lateral at all + "FORD_TRANSIT_MK5", +}) + +FORD_PINION_GEOMETRY_SHIFT = 1 +FORD_PINION_GEOMETRY_INDEX = { + "FORD_BRONCO_SPORT_MK1": 1, + "FORD_ESCAPE_MK4": 2, + "FORD_ESCAPE_MK4_5": 3, + "FORD_EXPEDITION_MK4": 4, + "FORD_EXPLORER_MK6": 5, + "FORD_FOCUS_MK4": 6, + "FORD_F_150_LIGHTNING_MK1": 7, + "FORD_F_150_MK14": 8, + "FORD_MAVERICK_MK1": 9, + "FORD_MONDEO_MK5": 10, + "FORD_MUSTANG_MACH_E_MK1": 11, + "FORD_RANGER_MK2": 12, +} + + +def ford_safety_param(CP: structs.CarParams, params_dict: dict[str, str]) -> int: + """The startup safety bits for this car, from the startup settings (HOSTS.md). A host ORs them into the + second safety param it sends the panda (current_safety_param_sp, USB 0xdf) at car init.""" + # Steering-angle curvature measurement (bad-yaw-sensor workaround): the STEER_ANGLE_CURVATURE flag + + # the platform geometry-table index; the control side mirrors the same flag + # (carcontroller_ext.InputTracker.get_current_curvature). Platforms without a geometry row silently + # keep stock yaw behavior -- the toggle no-ops rather than half-configuring. + bits = 0 + if CP.brand == 'ford': + # BluePilot 4-signal lateral: tell the panda to run the BP checks. Read once here, at init; + # the carcontroller sends 4-signal messages only when this bit is set. Changing the setting + # while driving does not change the panda: stock messages are then judged by the BP + # envelope (which is not identical to the stock one), so the setting takes full effect at + # the next init. Same property as openpilot longitudinal. + bp_lateral = not startup_value(params_dict, "disable_BP_lat_UI") + if bp_lateral: + bits |= FordSafetyFlagsSP.BP_LATERAL + + # The pinion-sourced measurement is a BP lateral feature: its wider error band and the + # Python side's yaw-vs-pinion handling only line up on the BP path, so it is off whenever + # BP lateral is off (ford_bp_init enforces the same on the panda). + steer_angle_curvature = bp_lateral and startup_value(params_dict, "FordPrefSteerAngleCurvature") + if steer_angle_curvature: + geometry_index = FORD_PINION_GEOMETRY_INDEX.get(CP.carFingerprint) + if geometry_index is not None: + bits |= FordSafetyFlagsSP.STEER_ANGLE_CURVATURE | (geometry_index << FORD_PINION_GEOMETRY_SHIFT) + return bits + + +# Pinion geometry wheelbase (m) where BluePilot deliberately differs from CarSpecs. Used by both the +# C table (ford_pinion_geometry in bluepilot_lateral/safety/ford.h) and the Python pinion path +# (carcontroller_ext.pinion_vehicle_model), so the two layers convert pinion angle the same way. +# FORD_F_150_MK14: the trucks with lane centering come in 3.68 and 3.99 m wheelbases and nothing on +# the car tells them apart; the average keeps either within about 4% (#21). +FORD_PINION_WHEELBASE = { + "FORD_F_150_MK14": 3.84, +} + + +# BluePilot: Max curvature for steering command (m^-1), from DBC file limits +CURVATURE_MAX = 0.02 + +# BluePilot: Curvature rate limits — 3-point breakpoints for smoother lateral control. +# Upstream opendbc uses 2-point ([5, 25]) with more conservative values. +# These allow higher rates at low speed for responsiveness, lower rates at mid-speed +# for comfort, and very low rates at highway speed for stability. +# +# Control (Python) uses stricter windup than unwind so OP stays inside panda when apply_std +# picks the wrong table vs ford_bp_curvature_cmd_checks. Safety firmware is looser than both: its +# up table is the control's down table, and its down table is 1% looser again — see +# bluepilot_lateral/safety/ford.h FORD_LIMITS. +_BP_ANGLE_RATE_UP = ([5, 16, 25], [0.0025, 0.0012, 0.00008]) +_BP_ANGLE_RATE_DOWN = ([5, 16, 25], [0.0025, 0.0014, 0.00018]) +BP_ANGLE_LIMITS = AngleSteeringLimits( + 0.02, # Max curvature for steering command, m^-1 + _BP_ANGLE_RATE_UP, + _BP_ANGLE_RATE_DOWN, +) + + +# The model's prediction times, part of the host contract (host_types.py); the strategies read it from here. +MODEL_T_IDXS = host_types.MODEL_T_IDXS + + +# The per-frame settings a host publishes every frame (HOSTS.md; hosts import this name from here). The +# settings, their defaults and how they are read are in params/manifest.py. +BP_LATERAL_PARAMS = PER_FRAME_KEYS + +# Lane change smoothing, both modes: speed breakpoints (m/s) and the low-speed factor +LANE_CHANGE_FACTOR_BP = (4.4, 40.23) +LANE_CHANGE_FACTOR_LOW = 0.95 + + +# Angle mode's per-platform high-speed gain defaults (low curvature, high curvature); any other +# platform gets ANGLE_GAIN_CAN. Read by lateral_angle_ext.update_angle_params. +# CAN vehicles (Escape MK4, Bronco Sport, Explorer, Maverick, Edge) +ANGLE_GAIN_CAN = (1.00, 1.15) +# CAN-FD body-on-frame trucks (F-150, Lightning, Expedition, Ranger) +ANGLE_GAIN_CANFD_BOF = (0.95, 0.95) +# CAN-FD unibody SUVs (Mustang Mach-E, Escape MK4.5) +ANGLE_GAIN_CANFD_SUV = (1.00, 1.05) + +ANGLE_GAIN_CANFD_BOF_CARS = frozenset({ + "FORD_F_150_MK14", + "FORD_F_150_LIGHTNING_MK1", + "FORD_EXPEDITION_MK4", + "FORD_RANGER_MK2", +}) +ANGLE_GAIN_CANFD_SUV_CARS = frozenset({ + "FORD_MUSTANG_MACH_E_MK1", + "FORD_ESCAPE_MK4_5", +}) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/host_types.py b/opendbc_repo/opendbc/bluepilot_lateral/host_types.py new file mode 100644 index 0000000000..7a470e58bf --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/host_types.py @@ -0,0 +1,48 @@ +"""BluePilot: the types a host hands the library and reads back from it (HOSTS.md). + +Standard library only: opendbc/car/structs.py imports LateralInputs for CarControlSP, so an import of +anything in opendbc here would be an import cycle.""" +from dataclasses import dataclass, field + +# The model's prediction grid: every path array in LateralInputs has MODEL_PATH_POINTS entries at these +# times, openpilot's ModelConstants.T_IDXS (t = 10 * (i / 32)^2). The core interpolates over it. +MODEL_PATH_POINTS = 33 +MODEL_T_IDXS = [10.0 * (i / 32) ** 2 for i in range(MODEL_PATH_POINTS)] + + +@dataclass +class LateralInputs: + """Per-frame model / vehicle inputs for the Ford 4-signal lateral control. The host fills them from + modelV2 / vehicleParameters / lateralDelay, so opendbc never subscribes to messaging.""" + valid: bool = False # producer saw model + vehicle params and filled every field below + pathYawRate: list[float] = field(default_factory=list) # modelV2.orientationRate.z, 33 points over T_IDXS + pathX: list[float] = field(default_factory=list) # modelV2.position.x + pathY: list[float] = field(default_factory=list) # modelV2.position.y + laneLineLeftX: list[float] = field(default_factory=list) # modelV2.laneLines[1] + laneLineLeftY: list[float] = field(default_factory=list) + laneLineRightX: list[float] = field(default_factory=list) # modelV2.laneLines[2] + laneLineRightY: list[float] = field(default_factory=list) + laneLineProbs: list[float] = field(default_factory=list) # 4 entries + laneLineStds: list[float] = field(default_factory=list) + laneChangeState: int = 0 # modelV2.meta.laneChangeState + laneChangeDirection: int = 0 # modelV2.meta.laneChangeDirection + steerRatio: float = 0.0 # vehicleParameters + stiffnessFactor: float = 0.0 + roll: float = 0.0 + angleOffsetDeg: float = 0.0 + lateralDelay: float = 0.0 # lateralDelay.lateralDelay + + +@dataclass +class LateralStatus: + """This frame's values for a BluePilot UI, from BluePilotLateral.status(). The names are the ones + BP7's controllerStateBP publisher reads off its car controller; the four angle-mode values read + False outside angle mode, as BP7 sets them.""" + lateralUncertainty: float = 0.0 # from whichever strategy ran last + angleRateLimited: bool = False # angle mode: the path_angle soft-ROC clip bit this frame + curvatureRateLimited: bool = False # angle mode: the equivalent curvature would have been rate-limited + curvatureDeviationLimited: bool = False # either mode: the current-curvature error clip constrained the command + humanTurnLateralPaused: bool = False # angle mode: lateral sent inactive during a manual turn + stallBlipActive: bool = False # angle mode: the brief mode-0 pulse after a post-override stall + disable_BP_lat_UI: bool = False # BluePilot lateral off: the UI toggle, or the panda bit not set at init + primary_lateral_control: int = 0 # 0 = curvature, 1 = angle diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/hooks.patch b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/hooks.patch new file mode 100644 index 0000000000..618e7d8ad4 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/hooks.patch @@ -0,0 +1,270 @@ +diff --git a/iqdbc/car/ford/carcontroller.py b/iqdbc/car/ford/carcontroller.py +index e00869f..cf8e4ba 100644 +--- a/iqdbc/car/ford/carcontroller.py ++++ b/iqdbc/car/ford/carcontroller.py +@@ -5,6 +5,7 @@ from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresi + from iqdbc.car.ford import fordcan + from iqdbc.car.ford.values import CarControllerParams, FordFlags, CAR + from iqdbc.car.interfaces import CarControllerBase, V_CRUISE_MAX ++from iqdbc.bluepilot_lateral.hosts.iqpilot.lateral import BluePilotLateral # BluePilot + + LongCtrlState = structs.CarControl.Actuators.LongControlState + VisualAlert = structs.CarControl.HUDControl.VisualAlert +@@ -38,6 +39,7 @@ class CarController(CarControllerBase): + super().__init__(dbc_names, CP, CP_IQ) + self.packer = CANPacker(dbc_names[Bus.pt]) + self.CAN = fordcan.CanBus(CP) ++ self.bp = BluePilotLateral(CP, CP_IQ, self.packer, self.CAN) # BluePilot + + self.apply_curvature_last = 0 + self.anti_overshoot_curvature_last = 0 +@@ -52,6 +54,7 @@ class CarController(CarControllerBase): + + def update(self, CC, CC_IQ, CS, now_nanos): + can_sends = [] ++ bp_active = self.bp.update(CC_IQ, self.frame) # BluePilot + + actuators = CC.actuators + hud_control = CC.hudControl +@@ -74,7 +77,10 @@ class CarController(CarControllerBase): + + ### lateral control ### + # send steer msg at 20Hz +- if (self.frame % CarControllerParams.STEER_STEP) == 0: ++ if bp_active and (self.frame % CarControllerParams.STEER_STEP) == 0: # BluePilot ++ lat_sends, self.apply_curvature_last = self.bp.step_lateral(CC, CS, actuators, self.apply_curvature_last) ++ can_sends.extend(lat_sends) ++ elif (self.frame % CarControllerParams.STEER_STEP) == 0: + # Bronco and some other cars consistently overshoot curv requests + # Apply some deadzone + smoothing convergence to avoid oscillations + if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): +@@ -108,7 +114,9 @@ class CarController(CarControllerBase): + can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.)) + + # send lka msg at 33Hz +- if (self.frame % CarControllerParams.LKA_STEP) == 0: ++ if bp_active and (self.frame % CarControllerParams.LKA_STEP) == 0: # BluePilot ++ can_sends.append(self.bp.step_lka(CC, hud_control)) ++ elif (self.frame % CarControllerParams.LKA_STEP) == 0: + can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) + + ### longitudinal control ### +diff --git a/iqdbc/car/structs.py b/iqdbc/car/structs.py +index 520c7fc..5f95bbd 100644 +--- a/iqdbc/car/structs.py ++++ b/iqdbc/car/structs.py +@@ -4,6 +4,7 @@ from typing import dataclass_transform, get_origin + import os + import capnp + from iqdbc.car.common.basedir import BASEDIR ++from iqdbc.bluepilot_lateral.host_types import LateralInputs # BluePilot: the per-frame inputs controlsd fills + try: + from iqpilot.cereal import car + except ImportError: +@@ -54,6 +55,7 @@ class IQCarParams: + longitudinalStoppingSpeedOverride: float = auto_field() + stoppingDecelRateOverride: float = auto_field() + longActiveWithGasOverride: bool = auto_field() ++ lateralInputsRequired: bool = auto_field() # BluePilot: controlsd fills IQCarControl.lateralInputs and params + + iqLateralNet: 'IQCarParams.LateralNet' = field(default_factory=lambda: IQCarParams.LateralNet()) + +@@ -120,6 +122,7 @@ class IQCarControl: + useRadarAccel: bool = auto_field() + radarSetSpeedKph: float = auto_field() + radarGapBars: int = auto_field() ++ lateralInputs: 'LateralInputs' = field(default_factory=lambda: LateralInputs()) # BluePilot + + @auto_dataclass + class Param: +diff --git a/iqdbc/lvbs/car/interfaces.py b/iqdbc/lvbs/car/interfaces.py +index 5ca575a..aafd822 100644 +--- a/iqdbc/lvbs/car/interfaces.py ++++ b/iqdbc/lvbs/car/interfaces.py +@@ -8,6 +8,7 @@ from collections.abc import Callable + + from iqdbc.car import structs + from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable ++from iqdbc.bluepilot_lateral.hosts.iqpilot.safety_param import init_ford_safety_param_iq # BluePilot + from iqdbc.car.subaru.values import SubaruFlags + from iqdbc.car.tesla.values import LEGACY_CARS + from iqdbc.lvbs.car.subaru.iq_values import SubaruFlagsIQ, SubaruSafetyFlagsIQ +@@ -80,6 +81,7 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, + _apply_tesla_options(CP, CP_IQ, params_dict) + _apply_creep_assist(CP, CP_IQ, params_dict) + _apply_toyota_options(CP, CP_IQ, params_dict) ++ init_ford_safety_param_iq(CP, CP_IQ, params_dict) # BluePilot + + + def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, +diff --git a/iqdbc/safety/modes/ford.h b/iqdbc/safety/modes/ford.h +index 26ec0af..5e5c0c4 100644 +--- a/iqdbc/safety/modes/ford.h ++++ b/iqdbc/safety/modes/ford.h +@@ -9,6 +9,7 @@ + #define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed + #define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed + #define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate ++#define FORD_SteeringPinion_Data 0x7EU // RX from PSCM, optional angle_meas source (BluePilot STEER_ANGLE_CURVATURE) + #define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons + #define FORD_ACCDATA 0x186U // TX by OP, ACC controls + #define FORD_ACCDATA_3 0x18AU // TX by OP, ACC/TJA user interface +@@ -29,6 +30,9 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) { + } else if (msg->addr == FORD_Yaw_Data_FD1) { + // Signal: VehRollYaw_No_Cnt + cnt = msg->data[5]; ++ } else if (msg->addr == FORD_SteeringPinion_Data) { ++ // Signal: StePinAn_No_Cnt (47|4@0+) ++ cnt = (msg->data[5] >> 4) & 0xFU; + } else { + } + return cnt; +@@ -74,6 +78,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { + valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf + } else if (msg->addr == FORD_Yaw_Data_FD1) { + valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf ++ } else if (msg->addr == FORD_SteeringPinion_Data) { ++ valid = ((msg->data[5] >> 2) & 0x3U) == 0x3U; // StePinCompAnEst_D_Qf (3=OK) + } else { + } + return valid; +@@ -94,6 +100,16 @@ static const NativeCurvatureSteeringLimits FORD_STEERING_LIMITS = { + .curvature_error_min_speed = 10.0, + }; + ++// BluePilot: the Ford 4-signal lateral checks, pinion angle_meas source and angle-mode state, from the ++// bluepilot_lateral subtree. The bindings point the library at IQ's native-curvature names. ++#define BP_HOST_CURVATURE_LIMITS NativeCurvatureSteeringLimits ++#define BP_HOST_CURVATURE_STATE native_curvature_state ++#define BP_HOST_RT_CURVATURE_RATE_CHECK rt_native_curvature_rate_limit_check ++#define BP_HOST_LAT_ALLOWED() is_lat_active() ++#define BP_HOST_SPEED_MISMATCH_CHECK() ((void)0) // ford_rx_hook runs it on FORD_EngVehicleSpThrottle2 ++// cppcheck-suppress misra-c2012-20.1; needs the Ford constants defined above ++#include "iqdbc/bluepilot_lateral/safety/ford.h" ++ + static void ford_rx_hook(const CANPacket_t *msg) { + if (msg->bus == FORD_MAIN_BUS) { + // Update in motion state from standstill signal +@@ -117,7 +133,7 @@ static void ford_rx_hook(const CANPacket_t *msg) { + } + + // Update vehicle yaw rate +- if (msg->addr == FORD_Yaw_Data_FD1) { ++ if ((msg->addr == FORD_Yaw_Data_FD1) && !ford_bp_pinion_curvature) { // BluePilot: pinion source replaces yaw + // Signal: VehYaw_W_Actl + // TODO: we should use the speed which results in the closest angle measurement to the desired angle + float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5; +@@ -149,6 +165,8 @@ static void ford_rx_hook(const CANPacket_t *msg) { + aol_button_press = GET_BIT(msg, 40U) ? AOL_BUTTON_PRESSED : AOL_BUTTON_NOT_PRESSED; + } + } ++ ++ ford_bp_rx_hook(msg); // BluePilot: pinion angle_meas source + } + + static bool ford_tx_hook(const CANPacket_t *msg) { +@@ -223,6 +241,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) { + if (action != 0U) { + tx = false; + } ++ ++ ford_bp_lka_tx(msg); // BluePilot: angle-mode state from the frame's unused bits + } + + // Safety check for LateralMotionControl action +@@ -234,12 +254,17 @@ static bool ford_tx_hook(const CANPacket_t *msg) { + unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5); + unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[6] >> 6); + +- // These signals are not yet tested with the current safety limits +- bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); +- +- // Check angle error and steer_control_enabled +- int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature +- violation |= steer_native_curvature_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); ++ bool violation = false; ++ if (ford_bp_lateral) { // BluePilot: 4-signal lateral ++ violation = ford_bp_lmc_tx(false, raw_curvature, raw_curvature_rate, raw_path_offset, raw_path_angle, steer_control_enabled); ++ } else { ++ // These signals are not yet tested with the current safety limits ++ violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); ++ ++ // Check angle error and steer_control_enabled ++ int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature ++ violation |= steer_native_curvature_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); ++ } + + if (violation) { + tx = false; +@@ -255,12 +280,17 @@ static bool ford_tx_hook(const CANPacket_t *msg) { + unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2); + unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[5]; + +- // These signals are not yet tested with the current safety limits +- bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); +- +- // Check angle error and steer_control_enabled +- int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature +- violation |= steer_native_curvature_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); ++ bool violation = false; ++ if (ford_bp_lateral) { // BluePilot: 4-signal lateral ++ violation = ford_bp_lmc_tx(true, raw_curvature, raw_curvature_rate, raw_path_offset, raw_path_angle, steer_control_enabled); ++ } else { ++ // These signals are not yet tested with the current safety limits ++ violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); ++ ++ // Check angle error and steer_control_enabled ++ int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature ++ violation |= steer_native_curvature_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); ++ } + + if (violation) { + tx = false; +@@ -287,6 +317,20 @@ static safety_config ford_init(uint16_t param) { + {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; + ++ // BluePilot: ford_rx_checks plus the pinion message, enforced only while the pinion angle_meas source is ++ // on (ford_bp_init). Written out rather than shared through a macro: cppcheck's MISRA 5.8 check misreads one. ++ static RxCheck ford_rx_checks_pinion[] = { ++ {.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, ++ {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, ++ // StePinAn_No_Cs matches no Ford checksum pattern on real frames: integrity via counter (0-15) + quality flag + 100 Hz ++ {.msg = {{FORD_SteeringPinion_Data, 0, 8, 100U, .max_counter = 15U, .ignore_checksum = true}, { 0 }, { 0 }}}, ++ }; ++ + #define FORD_COMMON_TX_MSGS \ + {FORD_Steering_Data_FD1, 0, 8, .check_relay = false}, \ + {FORD_Steering_Data_FD1, 2, 8, .check_relay = false}, \ +@@ -331,6 +375,11 @@ static safety_config ford_init(uint16_t param) { + } else { + ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS); + } ++ ++ // BluePilot: IQ safety flags (BP lateral, pinion source); the pinion RX check applies only when it is consumed ++ if (ford_bp_init(current_safety_param_iq)) { ++ SET_RX_CHECKS(ford_rx_checks_pinion, ret); ++ } + return ret; + } + +diff --git a/iqdbc/safety/tests/test.sh b/iqdbc/safety/tests/test.sh +index 1ff4ee8..9c8b425 100755 +--- a/iqdbc/safety/tests/test.sh ++++ b/iqdbc/safety/tests/test.sh +@@ -10,8 +10,8 @@ source ../../../setup.sh + rm -f ./libsafety/*.gcda + scons -j$(nproc) -D + +-# run safety tests and generate coverage data +-pytest -n8 --ignore-glob=misra/* ++# run safety tests and generate coverage data (and the BluePilot ones, from the bluepilot_lateral subtree) ++pytest -n8 --ignore-glob=misra/* . ../../bluepilot_lateral/safety/tests + + if [ "$(uname)" = "Darwin" ]; then + GCOV_EXEC="/opt/homebrew/opt/llvm@18/bin/llvm-cov gcov" diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/host.sh b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/host.sh new file mode 100755 index 0000000000..d9251e9036 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/host.sh @@ -0,0 +1,92 @@ +#!/usr/bin/env bash +# BluePilot: IQPilot as a test host for this library (#18). Its opendbc, iqdbc, comes from IQ.Lvbs's own repo at the +# commit pinned below, with IQPilot's hooks (hooks.patch, the files they change outside the library) and this +# library at iqdbc/bluepilot_lateral/. +# host.sh tree [ref] /iq: the IQPilot folders iqdbc's code imports; /host: iqdbc on its own, a git +# repo with IQPilot as committed (first commit) and the hooks plus this library, placed by +# tools/vendor.sh (second). Writes /state: "not adopted" (the patch applied), "adopted" +# (IQPilot already carries it, or there's no patch), or "drifted: ..." (and fails): the hooks +# or IQPilot's copy of the library differ from what this repo has +# host.sh test build /host and run the library tests, IQPilot's safety suite with the BluePilot +# safety tests, and 100% line coverage of modes/ford.h and the library's safety headers +# host.sh make write hooks.patch from /host: every change outside the library since IQPilot's +# commit, committed or not. Change a hook: tree, edit /host, test, make. +set -euo pipefail + +REPO=https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git +PIN=cb6a1c1145b18e36bf5180ab13a4e7beda39c40a # beta, 2026-10-29 +PROJECT=artifacts/package_sources/iqdbc # iqdbc's own project: setup.sh, pyproject.toml, uv.lock +IMPORTED="iqpilot/common iqpilot/cereal iqpilot/system iqpilot/selfdrive" # imported by iqdbc's code and tests +EXTRA_PACKAGES="pyserial setproctitle" # imported too, but only in IQPilot's top-level lock + +HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +LIB="$(cd "$HERE/../.." && pwd)" +DIR="$(mkdir -p "$2" && cd "$2" && pwd)" + +case "$1" in +tree) + REF=${3:-$PIN} + rm -rf "$DIR/iq" "$DIR/host" + GIT_LFS_SKIP_SMUDGE=1 git clone -q --filter=blob:none --no-checkout "$REPO" "$DIR/iq" + # shellcheck disable=SC2086 # a list of paths + git -C "$DIR/iq" sparse-checkout set $PROJECT $IMPORTED + GIT_LFS_SKIP_SMUDGE=1 git -C "$DIR/iq" checkout -q "$REF" + # iqdbc on its own: under IQPilot's root, uv takes it for a member of IQPilot's workspace + cp -R "$DIR/iq/$PROJECT" "$DIR/host" + cd "$DIR/host" + git init -q + git add -A + git -c user.name=ci -c user.email=ci@localhost commit -q -m "IQPilot $(git -C "$DIR/iq" rev-parse --short HEAD)" + state="adopted" + if [ -f "$HERE/hooks.patch" ]; then + if git apply --check "$HERE/hooks.patch" 2>/dev/null; then + git apply "$HERE/hooks.patch" + state="not adopted" + elif ! git apply --check --reverse "$HERE/hooks.patch" 2>/dev/null; then + state="drifted: hooks.patch neither applies nor is already in IQPilot (update it to what IQPilot carries: make)" + fi + fi + # IQPilot's own copy, once it carries one, has to be a release as vendored, before this library replaces it + if [ -f iqdbc/bluepilot_lateral/VERSION ] && ! "$LIB/tools/check_vendored.sh" iqdbc/bluepilot_lateral; then + state="drifted: IQPilot's copy of the library differs from the release in its VERSION" + fi + "$LIB/tools/vendor.sh" HEAD iqdbc/bluepilot_lateral + git add -A + git -c user.name=ci -c user.email=ci@localhost commit -q -m "hooks and library" + echo "$state" > "$DIR/state" + echo "IQPilot $(git -C "$DIR/iq" rev-parse --short HEAD), hooks: $state, library $(git -C "$LIB" rev-parse --short HEAD)" + [[ "$state" != drifted* ]] + ;; +test) + cd "$DIR/host" + set +u + # shellcheck disable=SC1091 # IQPilot's: tools (clang-18 and mull on Linux, for the coverage build) and the venv + source setup.sh + set -u + export PYTHONPATH="$DIR/host:$DIR/iq" # setup.sh sets it to iqdbc alone; its code imports iqpilot too + # shellcheck disable=SC2086 # a list of packages + uv pip install -q $EXTRA_PACKAGES + scons -j"$(nproc 2>/dev/null || sysctl -n hw.ncpu)" + python -m unittest discover -s iqdbc/bluepilot_lateral/tests -t . + # IQPilot's safety suite as its test.sh runs it (pytest: unittest also collects its base classes), the BluePilot + # tests in the same run (its cross-mode TX sweep reads every Test* class it collects), minus IQPilot's own failures + deselect=() + # pytest names a test from its rootdir (iqdbc's project), not from the directory it runs in + while read -r t; do deselect+=("--deselect=iqdbc/safety/tests/$t"); done < <(grep -v -e '^#' -e '^$' "$HERE/known_failures.txt") + cd iqdbc/safety/tests + rm -f libsafety/*.gcda + pytest -q -n8 --ignore-glob='misra/*' "${deselect[@]}" . ../../bluepilot_lateral/safety/tests + if [ "$(uname)" = "Darwin" ]; then GCOV_EXEC="/opt/homebrew/opt/llvm@18/bin/llvm-cov gcov"; else GCOV_EXEC="llvm-cov-18 gcov"; fi + # two reports: modes/ford.h and the library's safety/ford.h share a file name (see bluepilot_coverage.sh) + gcovr -r .. --gcov-executable "$GCOV_EXEC" --fail-under-line=100 -f "$DIR/host/iqdbc/safety/modes/ford.h" + gcovr -r "$DIR/host" --gcov-executable "$GCOV_EXEC" -d --fail-under-line=100 -f "$DIR/host/iqdbc/bluepilot_lateral/safety/" + ;; +make) + git -C "$DIR/host" diff HEAD~1 -- . ':(exclude)iqdbc/bluepilot_lateral' > "$HERE/hooks.patch" + echo "hooks.patch: $(grep -c '^diff --git' "$HERE/hooks.patch") files" + ;; +*) + echo "usage: $0 tree|test|make [ref]" >&2 + exit 2 + ;; +esac diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/known_failures.txt b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/known_failures.txt new file mode 100644 index 0000000000..4f041df48b --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/known_failures.txt @@ -0,0 +1,4 @@ +# IQPilot's own failing tests at the pinned commit, skipped by host.sh test so they don't fail this library's CI. +# Each one fails on IQPilot without the hooks or the library too. Remove a line once IQPilot fixes it. +test_tesla_legacy.py::TestTeslaHW2ExternalPandaSafety::test_enable_control_allowed_with_manual_aol_button_state +test_tesla_legacy.py::TestTeslaHW3ExternalPandaSafety::test_enable_control_allowed_with_manual_aol_button_state diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/lateral.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/lateral.py new file mode 100644 index 0000000000..5fd6987eca --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/lateral.py @@ -0,0 +1,21 @@ +""" +BluePilot: the IQ.Pilot host adapter for the lateral (HOSTS.md). The Ford CarController builds this +with IQ.Pilot's types; it hands the core the library's types and nothing IQ-specific. +""" +from ...core import carcontroller_ext +from ...params.param_store import ParamStore + + +def params_from_cc_iq(CC_IQ) -> ParamStore: + """The settings IQ.Pilot published this frame (IQCarControl.params entries) as (key, value) pairs.""" + return ParamStore((p.key, p.value) for p in CC_IQ.params) + + +class BluePilotLateral(carcontroller_ext.BluePilotLateral): + def __init__(self, CP, CP_IQ, packer, CAN): + super().__init__(CP, CP_IQ.iqSafetyFlags, packer, CAN) + + def update(self, CC_IQ, frame): + """Every frame: the inputs and settings IQ.Pilot's controlsd put in IQCarControl. Returns whether + BluePilot lateral is active.""" + return super().update(CC_IQ.lateralInputs, params_from_cc_iq(CC_IQ), frame) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/safety_param.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/safety_param.py new file mode 100644 index 0000000000..fd7b2b09f8 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/safety_param.py @@ -0,0 +1,14 @@ +""" +BluePilot: the startup safety bits onto IQ.Pilot's IQCarParams (HOSTS.md). Its own module, so car +init imports only values_ext, not the whole lateral. +""" +from ...core.values_ext import ford_safety_param + + +def init_ford_safety_param_iq(CP, CP_IQ, params_dict: dict[str, str]) -> None: + """Called from IQ.Pilot's apply_iq_car_config at car init. CP_IQ.iqSafetyFlags reaches the panda as + current_safety_param_iq (USB 0xdf); lateralInputsRequired tells the host to fill + IQCarControl.lateralInputs and the per-frame settings, for every Ford (BluePilot lateral on or off).""" + CP_IQ.iqSafetyFlags |= ford_safety_param(CP, params_dict) + if CP.brand == 'ford': + CP_IQ.lateralInputsRequired = True diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py deleted file mode 100644 index dd9419eaf0..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py +++ /dev/null @@ -1,138 +0,0 @@ -"""StarPilot's CP adapter; model transport and saved Params stay in the host. - -Only exact final CP words derived from the existing five classic/six CANFD -extended profiles can select angle. Combined AOL retains its original Ford profile identity. -""" -from opendbc.car import structs -from opendbc.car.ford.pinion import base_word, configured, MASK as PINION_MASK -from opendbc.car.ford.values import CAR, FordFlags -from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs -from opendbc.bluepilot_lateral.core.trim import LaneGeometry -from opendbc.bluepilot_lateral.params import limits as bounds - -ANGLE_FLAG = 128 -CLASSIC = frozenset({CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_FOCUS_MK4, - CAR.FORD_MAVERICK_MK1, CAR.FORD_EXPLORER_MK6}) -CANFD = frozenset({CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2, CAR.FORD_MUSTANG_MACH_E_MK1}) -TRUCKS = CANFD - {CAR.FORD_ESCAPE_MK4_5, CAR.FORD_MUSTANG_MACH_E_MK1} - - -def mode_word(cp, *, selected): - if (cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar or cp.alternativeExperience not in (0, 32) or - cp.steerControlType != structs.CarParams.SteerControlType.angle or - not cp.pcmCruise or len(cp.safetyConfigs) not in (1, 2)): - return None - if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - cp.safetyConfigs[0].safetyParam != 0): - return None - fd = cp.carFingerprint in CANFD - if ((fd and (not cp.flags & FordFlags.CANFD or cp.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM))) or - (not fd and (cp.carFingerprint not in CLASSIC or cp.flags & ~int(FordFlags.HAS_BSM)))): - return None - safety = cp.safetyConfigs[-1] - long = int(cp.openpilotLongitudinalControl) - original = (18 if cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 else 66) + long if fd else 32 + long - manual = cp.transmissionType == structs.CarParams.TransmissionType.manual - if cp.transmissionType not in (structs.CarParams.TransmissionType.automatic, structs.CarParams.TransmissionType.manual): - return None - if cp.alternativeExperience == 32: - expected = original | (256 if manual else 0) | (ANGLE_FLAG if selected else 0) - else: - expected = ANGLE_FLAG + (2 if fd else 0) + long if selected else original - valid = (safety.safetyModel == structs.CarParams.SafetyModel.ford and base_word(safety.safetyParam) == expected and - (safety.safetyParam == expected or configured(cp))) - return expected if valid else None - - -def supported(cp): - return mode_word(cp, selected=False) is not None or mode_word(cp, selected=True) is not None - - -def qualified(cp): - return mode_word(cp, selected=True) is not None - - -def select(cp, raw): - # Absence/invalid bytes stay at current curvature. Never rewrite saved bytes. - chosen = (type(raw) is int and raw == 1) or (type(raw) in (bytes, str) and raw in (b"1", "1")) - if not chosen or mode_word(cp, selected=False) is None: - return False - if cp.alternativeExperience == 32: - cp.safetyConfigs[-1].safetyParam |= ANGLE_FLAG - else: - metadata = cp.safetyConfigs[-1].safetyParam & PINION_MASK - cp.safetyConfigs[-1].safetyParam = (ANGLE_FLAG + (2 if cp.flags & FordFlags.CANFD else 0) + int(cp.openpilotLongitudinalControl)) | metadata - cp.steerActuatorDelay = 0.22 - return True - - -def model_geometry(model, *, fresh=False): - """Project a caller-qualified snapshot to immutable core arrays; no model imports.""" - if model is None or fresh is not True: - return None - try: - position_x, position_y = tuple(model.position.x), tuple(model.position.y) - except (AttributeError, TypeError, ValueError): - return None - try: - return LaneGeometry(position_x=position_x, position_y=position_y, fresh=True, left_x=tuple(model.laneLines[1].x), left_y=tuple(model.laneLines[1].y), - right_x=tuple(model.laneLines[2].x), right_y=tuple(model.laneLines[2].y), - probabilities=(model.laneLineProbs[1], model.laneLineProbs[2]), - standard_deviations=(model.laneLineStds[1], model.laneLineStds[2])) - except (AttributeError, IndexError, TypeError, ValueError): - return LaneGeometry(position_x=position_x, position_y=position_y, fresh=True) - - -class Host: - def __init__(self, cp): - if not qualified(cp): - raise ValueError("BluePilot angle requires an exact admitted Ford safety word") - low, high = (0.95, 0.95) if cp.carFingerprint in TRUCKS else ((1.0, 1.05) if cp.carFingerprint in CANFD else (1.0, 1.15)) - self.cp = cp - from opendbc.car.ford.pinion import Measurement - self.measurement = Measurement(cp) - self.controller = AngleController(low_gain=low, high_gain=high) - self.inputs = None - self.steering_rejected = False - - def update(self, CC, CS): - if self.inputs is not None: - self.inputs.update() - self.measurement.update_parameters(getattr(self.inputs, "vehicle_parameters", None)) - snapshot = self.inputs.lateral_snapshot(CS.out.vEgoRaw, angle=True) if self.inputs is not None else None - model, times, delay, _ = snapshot if snapshot is not None else (None, (), 0.15, True) - aol = self.cp.alternativeExperience == 32 - # The host is constructed before AOL marks the final CarParams. - # Retained AOL keeps EPS engaged through release and tracking error; - # physical human-turn pauses and fault recovery remain independent. - self.controller.recovery_blips = not aol - ordinary = CC.enabled and CS.out.cruiseState.enabled and not CS.out.gasPressed and not CS.out.brakePressed - ready = (self.inputs is not None and self.inputs.native_permission(self.cp, max_age_ns=bounds.PANDA_AGE_NS) and - (aol and CS.out.cruiseState.available or ordinary) and CC.latActive and CS.out.canValid and - not CS.out.canTimeout and - (not aol or CS.out.gearShifter in (structs.CarState.GearShifter.drive, - structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low)) and - not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent and not CS.out.vehicleSensorsInvalid) - # Only an actual steering/shadow rejection invalidates its accepted path - # and counter history. Unrelated ACC/UI rejections do not reset steering. - rejected, self.steering_rejected = self.steering_rejected, False - ready = ready and not rejected - speed_min = speed_max = None - speed_window = getattr(CS, "bluepilot_speed_window", None) - if speed_window is not None: - low, high, step = speed_window - # Allow source progress across the existing 100 ms CAN freshness window. - margin = max(0.05, 5 * step) - speed_min = max(0.1, min(low, CS.out.vEgoRaw) - margin) - speed_max = max(high, CS.out.vEgoRaw, 0.1) + margin - meta = model.meta if model is not None else None - return self.controller.step(Inputs( - active=ready, speed=CS.out.vEgoRaw, measured_curvature=self.measurement.curvature(CS.out), - desired_curvature=CC.actuators.curvature, steering_pressed=CS.out.steeringPressed, - steering_angle=CS.out.steeringAngleDeg, yaw_predictions=tuple(model.orientationRate.z) if model is not None else (), - time_indices=tuple(times), lateral_delay=delay, - lane_change=int(getattr(meta.laneChangeState, "raw", meta.laneChangeState)) in (1, 2, 3) if meta else False, - lane_direction=int(getattr(meta.laneChangeDirection, "raw", meta.laneChangeDirection)) if meta else 0, - limit_status=getattr(CS, "lat_ctl_lim_stat", 0), - geometry=model_geometry(model, fresh=snapshot is not None), speed_min=speed_min, speed_max=speed_max)) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/lateral.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/lateral.py new file mode 100644 index 0000000000..67256b32b4 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/lateral.py @@ -0,0 +1,40 @@ +""" +BluePilot: the StarPilot host adapter for the lateral (HOSTS.md). StarPilot's Ford CarController builds this +from CarParams alone (the startup bits ride its Ford safetyParam, safety_param.py). StarPilot has no +CarControl extension: its openpilot side hands the CarController a FrameInputs each frame instead, carrying +what sunnypilot and IQ.Pilot carry in theirs. +""" +from dataclasses import dataclass, field + +from ...core import carcontroller_ext +from ...host_types import LateralInputs +from ...params.param_store import ParamStore +from .safety_param import bp_safety_param + + +@dataclass +class FrameInputs: + """This frame's model / vehicle-parameter inputs and settings, filled by StarPilot's openpilot side.""" + + @dataclass + class Param: + key: str = "" + value: bytes = b"" + + lateralInputs: LateralInputs = field(default_factory=LateralInputs) + params: list = field(default_factory=list) + + +def params_from_frame(frame_inputs: FrameInputs) -> ParamStore: + """The settings StarPilot published this frame (FrameInputs.params entries) as (key, value) pairs.""" + return ParamStore((p.key, p.value) for p in frame_inputs.params) + + +class BluePilotLateral(carcontroller_ext.BluePilotLateral): + def __init__(self, CP, packer, CAN): + super().__init__(CP, bp_safety_param(CP), packer, CAN) + + def update(self, frame_inputs: FrameInputs, frame): + """Every frame: the inputs and settings StarPilot's openpilot side put in frame_inputs. Returns whether + BluePilot lateral is active.""" + return super().update(frame_inputs.lateralInputs, params_from_frame(frame_inputs), frame) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/safety_param.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/safety_param.py new file mode 100644 index 0000000000..1679fc1071 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/safety_param.py @@ -0,0 +1,37 @@ +""" +BluePilot: the startup safety bits onto StarPilot's Ford safetyParam (HOSTS.md). StarPilot has no second +safety param, so the bits ride its one Ford param at bits 9-14, clear of the bits StarPilot's Ford safety +reads (0-3, and 8 for its AOL's manual transmission). Its own module, so car init imports only values_ext, +not the whole lateral. +""" +from ....car import structs +from ....car.ford.values import FordFlags +from ...core.values_ext import ford_safety_param + +BP_SAFETY_PARAM_SHIFT = 9 +BP_SAFETY_PARAM_MASK = 0x3F # ford_safety_param's six bits, before the shift +# StarPilot's Transit steers through Lane_Assist_Data1 alone: no BluePilot lateral +LKA_STEERING = getattr(FordFlags, "LKA_STEERING", 0) + + +def init_ford_safety_param_starpilot(CP: structs.CarParams, params_dict: dict[str, str]) -> None: + """Called from StarPilot's Ford interface at car init, after its own safetyParam bits are set. The + shifted bits reach the panda with the Ford safety mode; StarPilot's ford_init passes them to ford_bp_init.""" + if CP.brand != 'ford' or CP.flags & LKA_STEERING: + return + bits = ford_safety_param(CP, params_dict) + if bits: + CP.safetyConfigs[-1].safetyParam |= bits << BP_SAFETY_PARAM_SHIFT + + +def bp_safety_param(CP: structs.CarParams) -> int: + """The startup bits as ford_safety_param returned them: what BluePilotLateral and ford_bp_init take.""" + if not CP.safetyConfigs: + return 0 + return (CP.safetyConfigs[-1].safetyParam >> BP_SAFETY_PARAM_SHIFT) & BP_SAFETY_PARAM_MASK + + +def lateral_inputs_required(CP: structs.CarParams) -> bool: + """Whether StarPilot's openpilot side fills the per-frame inputs and settings: every Ford the lateral can + run on, BluePilot lateral on or off.""" + return CP.brand == 'ford' and not CP.flags & LKA_STEERING diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/lateral.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/lateral.py new file mode 100644 index 0000000000..e96ca7e72a --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/lateral.py @@ -0,0 +1,21 @@ +""" +BluePilot: the sunnypilot host adapter for the lateral (HOSTS.md). The Ford CarController builds this +with sunnypilot's types; it hands the core the library's types and nothing sunnypilot-specific. +""" +from ...core import carcontroller_ext +from ...params.param_store import ParamStore + + +def params_from_cc_sp(CC_SP) -> ParamStore: + """The settings sunnypilot published this frame (CarControlSP.params entries) as (key, value) pairs.""" + return ParamStore((p.key, p.value) for p in CC_SP.params) + + +class BluePilotLateral(carcontroller_ext.BluePilotLateral): + def __init__(self, CP, CP_SP, packer, CAN): + super().__init__(CP, CP_SP.safetyParam, packer, CAN) + + def update(self, CC_SP, frame): + """Every frame: the inputs and settings sunnypilot's controlsd put in CarControlSP. Returns whether + BluePilot lateral is active.""" + return super().update(CC_SP.lateralInputs, params_from_cc_sp(CC_SP), frame) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/safety_param.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/safety_param.py new file mode 100644 index 0000000000..392ff82ca0 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/safety_param.py @@ -0,0 +1,15 @@ +""" +BluePilot: the startup safety bits onto sunnypilot's CarParamsSP (HOSTS.md). Its own module, so car +init imports only values_ext, not the whole lateral. +""" +from ....car import structs +from ...core.values_ext import ford_safety_param + + +def init_ford_safety_param_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None: + """Called from sunnypilot's setup_interfaces at car init. CP_SP.safetyParam reaches the panda as + current_safety_param_sp (USB 0xdf); lateralInputsRequired tells the host to fill + CarControlSP.lateralInputs and the per-frame settings, for every Ford (BluePilot lateral on or off).""" + CP_SP.safetyParam |= ford_safety_param(CP, params_dict) + if CP.brand == 'ford': + CP_SP.lateralInputsRequired = True diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/generate.py b/opendbc_repo/opendbc/bluepilot_lateral/params/generate.py deleted file mode 100644 index 50c6ed881e..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/generate.py +++ /dev/null @@ -1,41 +0,0 @@ -"""Generate protocol constants without loading a host or vehicle module.""" -import json -from pathlib import Path - -root = Path(__file__).parent -spec = json.loads((root / "protocol.json").read_text()) -(root / "protocol.py").write_text('"""Generated from protocol.json; layout preserves the existing curvature flag."""\n' + - "".join(f"{key.upper()} = {value!r}\n" for key, value in spec.items())) -(root / "protocol.h").write_text("#pragma once\n// Generated from protocol.json; do not allocate bit 1 to angle metadata.\n" + - "".join(f"#define FORD_BP_{key.upper()} {int(value)}U\n" for key, value in spec.items() if key != "shadow_scale")) - -manifest = json.loads((root / "manifest.json").read_text()) -defaults, bounds = manifest["defaults"], manifest["bounds"] -limits = {"BLEND": defaults["blend"], "VLT_EXTRA": defaults["vlt_extra_max"], - "PATH_MIN": bounds["path_angle_wire"][0], "PATH_MAX": bounds["path_angle_wire"][1], - "GAIN_MIN": bounds["gain"][0], "GAIN_MAX": bounds["gain"][1], - "CURVATURE_MAX": bounds["curvature"], "CURVATURE_ERROR": bounds["curvature_error"], - "LATERAL_ACCEL": bounds["lateral_accel"], "RX_AGE_US": bounds["rx_age_us"], - "SHADOW_AGE_US": bounds["shadow_age_us"], "PATH_ROUNDING_MARGIN": bounds["path_rounding_margin"], - "SHADOW_ROUNDING_MARGIN": bounds["shadow_rounding_margin"], "LATERAL_JERK": bounds["lateral_jerk"], - "RT_INTERVAL_US": bounds["rt_interval_us"], "STEER_PERIOD_US": bounds["steer_period_us"], - "CORE_STEER_PERIOD_S": bounds["core_steer_period_s"], "PANDA_AGE_NS": bounds["panda_age_ns"]} -(root / "limits.py").write_text('"""Generated from manifest.json; host and core use this manifest contract."""\n' + - "".join(f"{key} = {value!r}\n" for key, value in limits.items())) -(root / "limits.h").write_text("#pragma once\n// Generated from manifest.json; native and Python bounds share a source.\n" + - "".join(f"#define FORD_BP_{key} {str(value) + 'F' if isinstance(value, float) else str(value) + 'U'}\n" - for key, value in limits.items())) - -# Optional angle trim settings are Python-only; native safety limits stay unchanged. -trim = manifest["angle_lane_trim"] -types = {"enabled": "bool", "offset_m": "float", "gain": "float"} -defaults_fields = "".join(f" {key}: {types[key]}\n" for key in trim["defaults"]) -bounds_fields = "".join(f" {key}: {'list[float]' if isinstance(value, list) else 'float'}\n" for key, value in trim["bounds"].items()) -(root / "trim.py").write_text('"""Generated Python-only angle trim defaults; no native bound or registry changes."""\n' + - "from typing import TypedDict\n\n\n" + - "class TrimDefaults(TypedDict):\n" + defaults_fields + "\n\n" + - "class TrimBounds(TypedDict):\n" + bounds_fields + "\n\n" + - f"SOURCE_REVISION = {trim['source_revision']!r}\n" + - f"DEFAULTS: TrimDefaults = {trim['defaults']!r}\n" + - "BOUNDS: TrimBounds = {\n" + - "".join(f" {key!r}: {value!r},\n" for key, value in trim["bounds"].items()) + "}\n") diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/limits.h b/opendbc_repo/opendbc/bluepilot_lateral/params/limits.h deleted file mode 100644 index 5442ec8264..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/limits.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once -// Generated from manifest.json; native and Python bounds share a source. -#define FORD_BP_BLEND 0.5F -#define FORD_BP_VLT_EXTRA 0.1F -#define FORD_BP_PATH_MIN -0.5F -#define FORD_BP_PATH_MAX 0.5235F -#define FORD_BP_GAIN_MIN 0.95F -#define FORD_BP_GAIN_MAX 1.3F -#define FORD_BP_CURVATURE_MAX 0.02F -#define FORD_BP_CURVATURE_ERROR 0.002F -#define FORD_BP_LATERAL_ACCEL 2.4114F -#define FORD_BP_RX_AGE_US 100000U -#define FORD_BP_SHADOW_AGE_US 60000U -#define FORD_BP_PATH_ROUNDING_MARGIN 0.0005F -#define FORD_BP_SHADOW_ROUNDING_MARGIN 1e-06F -#define FORD_BP_LATERAL_JERK 3.5886F -#define FORD_BP_RT_INTERVAL_US 250000U -#define FORD_BP_STEER_PERIOD_US 50000U -#define FORD_BP_CORE_STEER_PERIOD_S 0.045F -#define FORD_BP_PANDA_AGE_NS 300000000U diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/limits.py b/opendbc_repo/opendbc/bluepilot_lateral/params/limits.py deleted file mode 100644 index a17e792ead..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/limits.py +++ /dev/null @@ -1,19 +0,0 @@ -"""Generated from manifest.json; host and core use this manifest contract.""" -BLEND = 0.5 -VLT_EXTRA = 0.1 -PATH_MIN = -0.5 -PATH_MAX = 0.5235 -GAIN_MIN = 0.95 -GAIN_MAX = 1.3 -CURVATURE_MAX = 0.02 -CURVATURE_ERROR = 0.002 -LATERAL_ACCEL = 2.4114 -RX_AGE_US = 100000 -SHADOW_AGE_US = 60000 -PATH_ROUNDING_MARGIN = 0.0005 -SHADOW_ROUNDING_MARGIN = 1e-06 -LATERAL_JERK = 3.5886 -RT_INTERVAL_US = 250000 -STEER_PERIOD_US = 50000 -CORE_STEER_PERIOD_S = 0.045 -PANDA_AGE_NS = 300000000 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.json b/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.json deleted file mode 100644 index 60b482f432..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.json +++ /dev/null @@ -1,109 +0,0 @@ -{ - "source_revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a", - "settings": [ - { - "key": "FordLateralMode", - "type": "enum", - "default": 0, - "values": { - "0": "Current curvature", - "1": "BluePilot angle" - }, - "label": "Ford steering controller", - "description": "Select the steering controller for supported Ford profiles. Takes effect on the next drive.", - "modes": [ - "angle" - ] - } - ], - "defaults": { - "blend": 0.5, - "vlt_extra_max": 0.1, - "low_speed_factor": 1.0, - "high_speed_factor": 1.0, - "dampening": 1.0, - "lane_change_factor": 1.0 - }, - "bounds": { - "path_angle_wire": [ - -0.5, - 0.5235 - ], - "gain": [ - 0.95, - 1.3 - ], - "curvature_error": 0.002, - "lateral_accel": 2.4114, - "rx_age_us": 100000, - "shadow_age_us": 60000, - "steer_period_us": 50000, - "curvature": 0.02, - "path_rounding_margin": 0.0005, - "shadow_rounding_margin": 1e-06, - "lateral_jerk": 3.5886, - "rt_interval_us": 250000, - "core_steer_period_s": 0.045, - "panda_age_ns": 300000000 - }, - "angle_lane_trim": { - "source_revision": "e22afa6be9b881fa784c92ebb316db47728a3d81", - "defaults": { - "enabled": false, - "offset_m": 0.0, - "gain": 0.25 - }, - "bounds": { - "offset_m": [ - -0.5, - 0.5 - ], - "gain": [ - 0.0, - 1.0 - ], - "width_m": [ - 2.4, - 2.8, - 3.75, - 4.25 - ], - "width_tolerance": [ - 0.0, - 0.81, - 0.81, - 0.59 - ], - "std": [ - 0.3, - 0.5 - ], - "std_tolerance": [ - 0.81, - 0.0 - ], - "confidence": [ - 0.6, - 0.8 - ], - "speed_mps": [ - 0.0, - 9.0, - 15.0 - ], - "speed_authority": [ - 0.0, - 0.0, - 1.0 - ], - "lookahead_m": [ - 8.0, - 35.0 - ], - "raw_correction": 0.004, - "tau_s": 0.4, - "period_s": 0.05, - "correction_delta": 0.00015 - } - } -} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.py b/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.py new file mode 100644 index 0000000000..ebdcba9409 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/params/manifest.py @@ -0,0 +1,137 @@ +""" +BluePilot: every setting the lateral library reads, in one place. + +Each Setting says what the host stores (key, type, default: its params_keys.h line), what the user +sees (label, description, range: the BluePilot UI's text and limits), and how the library reads it +(at startup, every frame or both; clamped to its range or not). The library reads every setting +through `value` (per frame, from the CarControlSP.params ParamStore) or `startup_value` (get_car's +params dict), with one rule: a published, parseable value is used; missing, empty or unparsable +gives the default. + +`python -m .bluepilot_lateral.params.manifest` prints the lines for a host's params_keys.h (: the host's opendbc package). +""" +from dataclasses import dataclass +from typing import Literal + +STARTUP, PER_FRAME, BOTH = "startup", "per-frame", "both" + + +@dataclass(frozen=True) +class Setting: + key: str + type: type + default: bool | int | float + read: Literal["startup", "per-frame", "both"] + label: str + description: str + range: tuple[float, float] | None = None # the UI's limits + clamp: bool = False # the library clamps to range (the angle-mode factors only, as BluePilot does) + + +SETTINGS: tuple[Setting, ...] = ( + Setting("disable_BP_lat_UI", bool, False, BOTH, "Disable BP Lateral Control", + "Disable BluePilot lateral control."), + Setting("enable_human_turn_detection_curv", bool, True, PER_FRAME, "Enable Human Turn Detection", + "Enable detection of human-initiated turns."), + Setting("enable_lane_positioning_curv", bool, False, PER_FRAME, "Enable Lane Positioning", + "Enable custom lane positioning controls."), + Setting("enable_lane_full_mode_curv", bool, False, PER_FRAME, "Enable Lanefull Mode", + "Enable lanefull mode for lane positioning."), + Setting("enable_lane_positioning_ang", bool, False, PER_FRAME, "Enable Lane Positioning", + "Nudge the vehicle toward true lane-line center (plus optional bias) while in angle-primary control."), + Setting("FordPrefLateralControl", int, 0, PER_FRAME, "Primary Control Variable", + "Curvature matches the existing strategy. Angle uses path_angle as the main actuator (in development).", + range=(0, 1)), + # an on/off toggle in the UI, stored as INT + Setting("custom_profile_curv", int, 0, PER_FRAME, "Use Custom Tuning Profile", + "Enable custom tuning profile settings.", range=(0, 1)), + Setting("lane_change_factor_high_curv", float, 0.85, PER_FRAME, "Lane Change Factor High", + "Scales steering during a lane change (curvature control). Values <1.0 reduce it.", range=(0.5, 1.0)), + Setting("pc_blend_ratio_high_C_UI_curv", float, 0.4, PER_FRAME, "Predicted Curvature Blend Ratio High", + "Adjust the high curvature blend ratio (0.0-1.0).", range=(0.0, 1.0)), + Setting("pc_blend_ratio_low_C_UI_curv", float, 0.4, PER_FRAME, "Predicted Curvature Blend Ratio Low", + "Adjust the low curvature blend ratio (0.0-1.0).", range=(0.0, 1.0)), + Setting("custom_path_offset_curv", float, 0.0, PER_FRAME, "In-Lane Offset", + "Adjust the in-lane offset (-0.5 to 0.5).", range=(-0.5, 0.5)), + Setting("LC_PID_gain_UI_curv", float, 1.0, PER_FRAME, "Centering PID gain", + "PID gain for the curvature-mode centering controller. Applies whenever lane positioning is on.", + range=(0.0, 50.0)), + Setting("FordLowSpeedFactor_ang", float, 1.0, PER_FRAME, "Low Speed Adjustment Factor", + "Scales the low-speed steering response in angle mode. Adjust for personal feel. Default 1.0.", + range=(0.5, 1.5), clamp=True), + Setting("FordHighSpeedFactor_ang", float, 1.0, PER_FRAME, "High Speed Adjustment Factor", + "Scales the high-speed steering response in angle mode. Adjust for personal feel. Default 1.0.", + range=(0.5, 1.5), clamp=True), + Setting("FordHighSpeedDampening_ang", float, 1.0, PER_FRAME, "High Speed Low Curve Adjustment Factor", + "Tune adjustment factor for low curve straightaways (highways) at high speeds. If oversteering, reduce. " + + "If understeering, increase", range=(0.25, 1.25), clamp=True), + Setting("lane_change_factor_high_ang", float, 1.0, PER_FRAME, "Lane Change Factor High", + "Scales steering during a lane change (angle control). Values >1.0 boost it.", range=(0.85, 1.5), clamp=True), + Setting("custom_path_offset_ang", float, 0.0, PER_FRAME, "In-Lane Offset", + "Adjust the in-lane offset (-0.5 to 0.5).", range=(-0.5, 0.5), clamp=True), + Setting("lane_centering_strength_ang", float, 0.25, PER_FRAME, "Lane Centering Strength", + "How much authority the lane centering trim has vs. the model's own path (0.0-1.0).", + range=(0.0, 1.0), clamp=True), + Setting("FordPrefSteerAngleCurvature", bool, False, STARTUP, "Use Pinion Yaw Sensor", + "Measures how the car is turning from the steering pinion angle sensor instead of a faulty RCM yaw sensor " + + '(symptoms: "Turn Exceeds Steering Limit" warnings, weak curve tracking, "Service AdvanceTrac"). Check with ' + + "tools/ford_yaw_health_check.py. Applies the next time the car starts. Not available on the Edge."), +) + +_BY_KEY = {s.key: s for s in SETTINGS} + +# what a host publishes in CarControlSP.params every frame (values_ext.BP_LATERAL_PARAMS) +PER_FRAME_KEYS = tuple(s.key for s in SETTINGS if s.read != STARTUP) + + +def _parse(setting: Setting, raw): + """The setting's value from what the host gave, or None if there is nothing usable.""" + if isinstance(raw, (bytes, bytearray)): + raw = raw.decode("utf-8", errors="replace") + if raw is None or raw == "": + return None + if setting.type is bool: + if isinstance(raw, (bool, int)): + return bool(raw) + text = str(raw).strip().lower() + return True if text in ("1", "true") else False if text in ("0", "false") else None + try: + v = setting.type(raw) + except (TypeError, ValueError): + return None + if setting.clamp and setting.range is not None: + v = setting.type(min(max(v, setting.range[0]), setting.range[1])) + return v + + +def default(key: str): + return _BY_KEY[key].default + + +def value(params, key: str): + """A per-frame setting from the host's ParamStore (built from this frame's (key, value) pairs).""" + setting = _BY_KEY[key] + assert setting.read != STARTUP, f"{key} is read at startup only: hosts don't publish it per frame" + v = _parse(setting, params.get(key)) + return setting.default if v is None else v + + +def startup_value(params_dict: dict, key: str): + """A startup setting from get_car's params dict (values as Params returned them).""" + setting = _BY_KEY[key] + assert setting.read != PER_FRAME, f"{key} is read per frame only: hosts don't pass it at startup" + v = _parse(setting, params_dict.get(key)) + return setting.default if v is None else v + + +def params_keys_lines() -> list[str]: + """A host's params_keys.h entries for every setting.""" + types = {bool: "BOOL", int: "INT", float: "FLOAT"} + + def text(s): + return ("1" if s.default else "0") if s.type is bool else str(s.default) + return [f'{{"{s.key}", {{PERSISTENT | BACKUP, {types[s.type]}, "{text(s)}"}}}},' for s in SETTINGS] + + +if __name__ == "__main__": + print("\n".join(params_keys_lines())) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/param_store.py b/opendbc_repo/opendbc/bluepilot_lateral/params/param_store.py new file mode 100644 index 0000000000..0bc7245673 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/params/param_store.py @@ -0,0 +1,17 @@ +""" +BluePilot: the settings a host published this frame, by key (HOSTS.md). The lateral code reads every +setting through params/manifest.py's value(), which parses the published text and takes the manifest +default for a key that is missing, empty or unparsable. A host publishes every per-frame key anyway, +using its own registry default for one never written. +""" + + +class ParamStore: + def __init__(self, pairs): + """pairs: the (key, value) settings a host published this frame, value as bytes or text.""" + self._values = dict(pairs) + + def get(self, key): + """The published value as text, or None if the host didn't publish the key.""" + v = self._values.get(key) + return v.decode("utf-8", errors="replace") if isinstance(v, (bytes, bytearray)) else v diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.h b/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.h deleted file mode 100644 index a7179eb9cf..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once -// Generated from protocol.json; do not allocate bit 1 to angle metadata. -#define FORD_BP_VERSION 1U -#define FORD_BP_ANGLE_MASK 1U -#define FORD_BP_EXTENDED_CURVATURE_MASK 2U -#define FORD_BP_COUNTER_SHIFT 2U -#define FORD_BP_COUNTER_MASK 28U -#define FORD_BP_COUNTER_MODULUS 8U -#define FORD_BP_VERSION_MASK 15U -#define FORD_BP_CHECKSUM_SHIFT 4U diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.json b/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.json deleted file mode 100644 index dc846eb0d6..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "version": 1, - "angle_mask": 1, - "extended_curvature_mask": 2, - "counter_shift": 2, - "counter_mask": 28, - "counter_modulus": 8, - "shadow_scale": 1e-06, - "version_mask": 15, - "checksum_shift": 4 -} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.py b/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.py deleted file mode 100644 index c13f2344c7..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/protocol.py +++ /dev/null @@ -1,10 +0,0 @@ -"""Generated from protocol.json; layout preserves the existing curvature flag.""" -VERSION = 1 -ANGLE_MASK = 1 -EXTENDED_CURVATURE_MASK = 2 -COUNTER_SHIFT = 2 -COUNTER_MASK = 28 -COUNTER_MODULUS = 8 -SHADOW_SCALE = 1e-06 -VERSION_MASK = 15 -CHECKSUM_SHIFT = 4 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/params/trim.py b/opendbc_repo/opendbc/bluepilot_lateral/params/trim.py deleted file mode 100644 index 51a13ca6fa..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/params/trim.py +++ /dev/null @@ -1,45 +0,0 @@ -"""Generated Python-only angle trim defaults; no native bound or registry changes.""" -from typing import TypedDict - - -class TrimDefaults(TypedDict): - enabled: bool - offset_m: float - gain: float - - -class TrimBounds(TypedDict): - offset_m: list[float] - gain: list[float] - width_m: list[float] - width_tolerance: list[float] - std: list[float] - std_tolerance: list[float] - confidence: list[float] - speed_mps: list[float] - speed_authority: list[float] - lookahead_m: list[float] - raw_correction: float - tau_s: float - period_s: float - correction_delta: float - - -SOURCE_REVISION = 'e22afa6be9b881fa784c92ebb316db47728a3d81' -DEFAULTS: TrimDefaults = {'enabled': False, 'offset_m': 0.0, 'gain': 0.25} -BOUNDS: TrimBounds = { - 'offset_m': [-0.5, 0.5], - 'gain': [0.0, 1.0], - 'width_m': [2.4, 2.8, 3.75, 4.25], - 'width_tolerance': [0.0, 0.81, 0.81, 0.59], - 'std': [0.3, 0.5], - 'std_tolerance': [0.81, 0.0], - 'confidence': [0.6, 0.8], - 'speed_mps': [0.0, 9.0, 15.0], - 'speed_authority': [0.0, 0.0, 1.0], - 'lookahead_m': [8.0, 35.0], - 'raw_correction': 0.004, - 'tau_s': 0.4, - 'period_s': 0.05, - 'correction_delta': 0.00015, -} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/ford.h b/opendbc_repo/opendbc/bluepilot_lateral/safety/ford.h new file mode 100644 index 0000000000..7958b3bf89 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/ford.h @@ -0,0 +1,383 @@ +/** + * BluePilot Ford lateral-control extensions: state and implementations. + * See ford_declarations.h in this directory for the declarations. + */ + +#pragma once + +#include "../safety/ford_declarations.h" +#include "../safety/lateral.h" + +// =============================== +// Global Variables +// =============================== + +bool ford_bp_lateral = false; // SP safety-param bit 5: BP 4-signal lateral checks instead of stock +bool ford_bp_pinion_curvature = false; +const AngleSteeringParams *ford_bp_pinion_params = &ford_pinion_geometry[0]; + +// BluePilot: per-platform geometry for pinion-angle -> curvature conversion (the optional +// angle_meas source), selected by the 4-bit geometry index in current_safety_param_sp +// bits 1-4. Row order and values must match FORD_PINION_GEOMETRY_INDEX in +// bluepilot_lateral/core/values_ext.py (enforced by test_ford_bluepilot_pinion.py's +// geometry test against CarSpecs + calc_slip_factor(VehicleModel(CP)), with the wheelbase +// overrides in FORD_PINION_WHEELBASE applied). +// Index 0 is reserved as invalid; ford_bp_init disables the feature outright on a zero or +// out-of-range index so a half-configured param can never select the wrong geometry. +// FORD_EDGE_MK2 (ALT_STEER_ANGLE: relative pinion angle + learned offset) is unsupported +// and deliberately absent. +const AngleSteeringParams ford_pinion_geometry[FORD_PINION_GEOMETRY_ROWS] = { + {.slip_factor = 0.0f, .steer_ratio = 1.0f, .wheelbase = 1.0f}, // 0: invalid + {.slip_factor = -0.00062819555f, .steer_ratio = 17.7f, .wheelbase = 2.670f}, // 1: FORD_BRONCO_SPORT_MK1 + {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 2: FORD_ESCAPE_MK4 + {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 3: FORD_ESCAPE_MK4_5 + {.slip_factor = -0.00045454798f, .steer_ratio = 17.0f, .wheelbase = 3.690f}, // 4: FORD_EXPEDITION_MK4 + {.slip_factor = -0.00055447339f, .steer_ratio = 16.8f, .wheelbase = 3.025f}, // 5: FORD_EXPLORER_MK6 + {.slip_factor = -0.00062121569f, .steer_ratio = 15.0f, .wheelbase = 2.700f}, // 6: FORD_FOCUS_MK4 + {.slip_factor = -0.00045331952f, .steer_ratio = 16.9f, .wheelbase = 3.700f}, // 7: FORD_F_150_LIGHTNING_MK1 + {.slip_factor = -0.00043679222f, .steer_ratio = 17.0f, .wheelbase = 3.840f}, // 8: FORD_F_150_MK14 (average wheelbase, #21) + {.slip_factor = -0.00054528036f, .steer_ratio = 17.0f, .wheelbase = 3.076f}, // 9: FORD_MAVERICK_MK1 + {.slip_factor = -0.00058852001f, .steer_ratio = 14.8f, .wheelbase = 2.850f}, // 10: FORD_MONDEO_MK5 + {.slip_factor = -0.00056209187f, .steer_ratio = 17.0f, .wheelbase = 2.984f}, // 11: FORD_MUSTANG_MACH_E_MK1 + {.slip_factor = -0.00051293030f, .steer_ratio = 17.0f, .wheelbase = 3.270f}, // 12: FORD_RANGER_MK2 +}; + +bool ford_bp_angle_mode_engaged = false; +int16_t ford_bp_shadow_curvature_raw = 0; // wire units, scale 1e-6 1/m (LANE_ASSIST_DATA1.md) + +int desired_path_angle_last = 0; +int desired_path_offset_last = 0; + + +// =============================== +// Function Implementations +// =============================== + +static inline bool path_angle_cmd_checks(int desired_path_angle, bool steer_control_enabled, const AngleSteeringLimits limits) { + bool violation = false; + + if (steer_control_enabled) { + float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.; + + int delta_path_angle_roc = (safety_interpolate(limits.angle_rate_up_lookup, speed) * limits.angle_deg_to_can) + 1.; + + int highest_desired_path_angle = desired_path_angle_last + delta_path_angle_roc; + int lowest_desired_path_angle = desired_path_angle_last - delta_path_angle_roc; + + violation |= safety_max_limit_check(desired_path_angle, highest_desired_path_angle, lowest_desired_path_angle); + FORD_BP_DBG("path_angle_cmd_checks 1: desired_path_angle: %d desired_path_angle_last: %d highest_desired_path_angle: %d lowest_desired_path_angle: %d violation: %d \n", + desired_path_angle, desired_path_angle_last, highest_desired_path_angle, lowest_desired_path_angle, (int)violation); + } + desired_path_angle_last = desired_path_angle; + + if (!steer_control_enabled) { + violation |= (desired_path_angle != 0); + } + FORD_BP_DBG("path_angle_cmd_checks 2: violation: %d \n", (int)violation); + + return violation; +} + +static inline bool path_offset_cmd_checks(int desired_path_offset, bool steer_control_enabled, const AngleSteeringLimits limits) { + bool violation = false; + + if (steer_control_enabled) { + float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.; + + int delta_path_offset_roc = (safety_interpolate(limits.angle_rate_up_lookup, speed) * limits.angle_deg_to_can) + 1.; + + int highest_desired_path_offset = desired_path_offset_last + delta_path_offset_roc; + int lowest_desired_path_offset = desired_path_offset_last - delta_path_offset_roc; + + violation |= safety_max_limit_check(desired_path_offset, highest_desired_path_offset, lowest_desired_path_offset); + FORD_BP_DBG("path_offset_cmd_checks 1: desired_path_offset: %d desired_path_offset_last: %d highest_desired_path_offset: %d lowest_desired_path_offset: %d violation: %d \n", + desired_path_offset, desired_path_offset_last, highest_desired_path_offset, lowest_desired_path_offset, (int)violation); + + } + desired_path_offset_last = desired_path_offset; + + if (!steer_control_enabled) { + violation |= (desired_path_offset != 0); + } + FORD_BP_DBG("path_offset_cmd_checks 2: violation: %d \n", (int)violation); + + return violation; +} + +// BluePilot: angle mode has no "current path_angle" measurement to check the command against, +// unlike curvature mode, which compares desired_curvature against angle_meas (measured curvature, +// from yaw rate). Without this, a large deviation between commanded path_angle and the car's ACTUAL +// curvature -- e.g. a pothole or driver override kicking the wheel -- would go unchecked: path_angle's +// own ROC only bounds how fast the *command* changes, not how far it may sit from reality. +// +// Deliberately narrower than steer_angle_cmd_checks: no rate-of-change enforcement here, and no +// shared state (desired_angle_last) with curvature mode. path_angle already has its own dedicated, +// tuned ROC (path_angle_cmd_checks / FORD_PATH_ANGLE_LIMITS); imposing a second, curvature-tuned ROC +// on shadow_curvature -- which isn't an actuator, just a cross-check value -- would risk spurious +// blocks unrelated to path_angle's actual behavior (confirmed on real hardware 2026-07-10: doing +// this via steer_angle_cmd_checks caused blocks at low speed from shadow_curvature jumping frame to +// frame with nothing driving it toward path_angle's own smooth ROC). This is a pure per-frame +// proximity check: does this frame's steering intent make physical sense given where the car is. +// BluePilot: enforce_angle_error is gone from the struct; the check is unconditional now because +// its only caller, ford_lmc_checks, passes the base of a BluePilot limit set (FORD_LIMITS, above +// ford_bp_lmc_tx), all of which set it true pre-sync. +static inline bool ford_shadow_curvature_error_check(int desired_curvature, bool steer_control_enabled, + const BP_HOST_CURVATURE_LIMITS limits) { + bool violation = false; + if (steer_control_enabled && + ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.curvature_error_min_speed)) { + int lowest_allowed = BP_HOST_CURVATURE_STATE.meas.min - limits.max_curvature_error - 1; + int highest_allowed = BP_HOST_CURVATURE_STATE.meas.max + limits.max_curvature_error + 1; + violation = safety_max_limit_check(desired_curvature, highest_allowed, lowest_allowed); + } + return violation; +} + +// BluePilot: shared curvature/curvature_rate/path_offset/path_angle command checks for +// LateralMotionControl (CAN) and LateralMotionControl2 (CAN FD), in signed CAN units. limits is the +// curvature limit set ford_bp_lmc_tx picked for the message and the angle_meas source. +// dbg_prefix labels the FORD_BP_DBG output ("CAN Out" / "CANFD Out"). +static inline bool ford_lmc_checks(int desired_curvature, int desired_curvature_rate, int desired_path_offset, int desired_path_angle, + bool steer_control_enabled, const FordBpCurvatureLimits *limits, const char *dbg_prefix) { + // dbg_prefix is only read inside FORD_BP_DBG, which expands to a no-op unless FORD_BP_DEBUG is defined for the + // debug build, making the parameter otherwise unused. + SAFETY_UNUSED(dbg_prefix); + + // PathAngle rate limits + static const AngleSteeringLimits FORD_PATH_ANGLE_LIMITS = { + .max_angle = 1000, + // 0.0005 + .angle_deg_to_can = 2000, // 1 / (2e-5) rad to can + // Mirror lateral_angle_ext.py _soft_roc: interp(v_ego, [9,10,15,25], [0.055,0.055,0.0425,0.009]) + // rad/call, scaled x1.02 so panda is 2% LOOSER than the Python control and never blocks LMC2. + // lookup_t is fixed at 3 points; Python's 9 & 10 m/s nodes are both 0.055 (flat top), so {10,15,25} + // reproduces the curve exactly and speeds <10 clamp to the first point. The +1 CAN unit and the + // speed-1 fudge in path_angle_cmd_checks add extra headroom on top of the 2%. + // BluePilot: LMC2 is only sent once per CarControllerParams.STEER_STEP (5) = 20Hz, not 100Hz -- + // _soft_roc's y-values (and this mirror) are per-call, not per-100Hz-tick; see lateral_angle_ext.py. + .angle_rate_up_lookup = { + .x = {10., 15., 25.}, + .y = {0.0561, 0.04335, 0.00918} + }, + .angle_rate_down_lookup = { + .x = {10., 15., 25.}, + .y = {0.0561, 0.04335, 0.00918} + }, + .frequency = 20U, // Hz -- LateralMotionControl/LateralMotionControl2 @ 20Hz (matches + // actual STEER_STEP=5 cadence; was 100U, a stale leftover from an + // abandoned 100Hz-cadence experiment. Currently unread by + // path_angle_cmd_checks (only angle_rate_up/down_lookup matter), + // but corrected for consistency/documentation and in case a future + // rt_angle_rate_limit_check() wiring starts consuming it. + }; + + // PathOffset rate limits + static const AngleSteeringLimits FORD_PATH_OFFSET_LIMITS = { + .max_angle = 100, // 1.0 meter in CAN units (100 * 0.01) + .angle_deg_to_can = 100, // 1 / (0.01) meter to can + .angle_rate_up_lookup = { + .x = {5., 15., 25.}, + .y = {0.05, 0.025, 0.01} // Slower rate limits for path offset + }, + .angle_rate_down_lookup = { + .x = {5., 15., 25.}, + .y = {0.05, 0.025, 0.01} // Slower rate limits for path offset + }, + .frequency = 20U, // Hz - 20Hz message rate + }; + + const BP_HOST_CURVATURE_LIMITS *base = &limits->base; + bool violation = false; + + // Check curvature value limits (already converted to signed CAN units by the caller). Note: + // curvature's wire scale (curvature_to_can) is the same for CAN and CAN FD, so this is safe to + // read off the (possibly pinion-widened) limits struct regardless of bus. + int curvature_min_can = (int)(FORD_CURVATURE_MIN * base->curvature_to_can); + int curvature_max_can = (int)(FORD_CURVATURE_MAX * base->curvature_to_can); + violation |= (desired_curvature < curvature_min_can) || (desired_curvature > curvature_max_can); + FORD_BP_DBG("%s: desired_curvature: %d, curvature_min_can: %d, curvature_max_can: %d, violation: %d\n", + dbg_prefix, desired_curvature, curvature_min_can, curvature_max_can, (int)violation); + + // Curvature rate must be 0, like stock Ford: BluePilot always sends 0 (#22) + violation |= (desired_curvature_rate != 0); + FORD_BP_DBG("%s: desired_curvature_rate: %d, violation: %d\n", dbg_prefix, desired_curvature_rate, (int)violation); + + // Check path offset value limits + int path_offset_min_can = (int)(FORD_PATH_OFFSET_MIN * FORD_PATH_OFFSET_LIMITS.angle_deg_to_can); + int path_offset_max_can = (int)(FORD_PATH_OFFSET_MAX * FORD_PATH_OFFSET_LIMITS.angle_deg_to_can); + violation |= (desired_path_offset < path_offset_min_can) || (desired_path_offset > path_offset_max_can); + FORD_BP_DBG("%s: desired_path_offset: %d, path_offset_min_can: %d, path_offset_max_can: %d, violation: %d\n", + dbg_prefix, desired_path_offset, path_offset_min_can, path_offset_max_can, (int)violation); + + // Check path angle value limits. Angle mode uses path_angle as the actuator and may swing to + // the full DBC range, corroborated by ford_bp_angle_mode_engaged so a frame can't unlock this + // wider range by merely setting curvature to 0. Curvature mode always keeps the tight cap, + // including at curvature == 0 (straight driving, or the reset/human-turn frame) -- path_angle + // there only trims and amplifies wound-up curvature, never needs the wide range. + float path_angle_min_phys = ford_bp_angle_mode_engaged ? FORD_DBC_PATH_ANGLE_MIN : FORD_PATH_ANGLE_MIN; + float path_angle_max_phys = ford_bp_angle_mode_engaged ? FORD_DBC_PATH_ANGLE_MAX : FORD_PATH_ANGLE_MAX; + const float path_angle_min_scaled = path_angle_min_phys * FORD_PATH_ANGLE_LIMITS.angle_deg_to_can; + const float path_angle_max_scaled = path_angle_max_phys * FORD_PATH_ANGLE_LIMITS.angle_deg_to_can; + int path_angle_min_can = (int)path_angle_min_scaled; + int path_angle_max_can = (int)path_angle_max_scaled; + violation |= (desired_path_angle < path_angle_min_can) || (desired_path_angle > path_angle_max_can); + FORD_BP_DBG("%s: desired_path_angle: %d, path_angle_min_can: %d, path_angle_max_can: %d, violation: %d\n", + dbg_prefix, desired_path_angle, path_angle_min_can, path_angle_max_can, (int)violation); + + // Check the curvature command and steer_control_enabled. At curvature 0 (angle mode, where + // path_angle does the steering, and curvature mode's reset frame) ford_bp_curvature_cmd_checks skips + // its value checks itself and keeps the speed cross-check, message-rate check and controls gate; + // path_angle keeps its own checks regardless. + violation |= ford_bp_curvature_cmd_checks(desired_curvature, steer_control_enabled, limits); + FORD_BP_DBG("%s: 1. desired_curvature violation: %d\n", dbg_prefix, (int)violation); + + // Angle mode's own deviation-only check against shadow_curvature, once angle mode is confirmed + // engaged. If desired_curvature == 0 but angle mode is NOT confirmed, this is skipped -- that's + // ordinary curvature mode at zero (straight driving or the reset/human-turn frame), which needs + // no shadow-curvature check; it's still bounded by the tight path_angle range above and + // steer_control_enabled's own checks. + if ((desired_curvature == 0) && ford_bp_angle_mode_engaged) { + const float shadow_curvature_scaled = (float)ford_bp_shadow_curvature_raw * FORD_BP_LKA_SHADOW_RAW_TO_CAN; + int shadow_curvature_can = (int)shadow_curvature_scaled; + violation |= ford_shadow_curvature_error_check(shadow_curvature_can, steer_control_enabled, *base); + } + + // Check path angle rate of change limits + violation |= path_angle_cmd_checks(desired_path_angle, steer_control_enabled, FORD_PATH_ANGLE_LIMITS); + FORD_BP_DBG("%s: 2. desired_path_angle violation: %d\n", dbg_prefix, (int)violation); + + // Check path offset rate of change limits + violation |= path_offset_cmd_checks(desired_path_offset, steer_control_enabled, FORD_PATH_OFFSET_LIMITS); + FORD_BP_DBG("%s: 3. desired_path_offset violation: %d\n", dbg_prefix, (int)violation); + + + return violation; +} + +// BluePilot: the curvature limit sets ford_bp_lmc_tx picks from: the host's BP_HOST_CURVATURE_LIMITS as +// .base plus the measured rate tables, checked by ford_bp_curvature_cmd_checks instead of upstream's +// ISO-jerk-derived delta. The values are the pre-sync macro's. max_curv_err is 100 (0.002) on the stock +// yaw-sourced angle_meas path and 150 (0.003) on the pinion-sourced path (STEER_ANGLE_CURVATURE): +// firmware uses the raw pinion angle, with no roll/alignment-offset compensation (the Python layer +// compensates via liveParameters). ford_bp_curvature_cmd_checks assumes max_steer_power 0 and +// inactive_curvature_is_zero true rather than reading them, and base sets only the fields the library +// reads, so it initializes upstream's CurvatureSteeringLimits and a host's narrower struct alike. +#define FORD_LIMITS(limit_lateral_accel, max_curv_err) { \ + .base = { \ + .max_curvature = 1000, /* 0.02 curvature */ \ + .curvature_to_can = 50000, /* 1 / (2e-5) rad to can */ \ + .frequency = 20U, /* LateralMotionControl / LateralMotionControl2 @ 20 Hz */ \ + .max_curvature_error = (max_curv_err), \ + /* no blending at low speed due to lack of torque wind-up and inaccurate current curvature */\ + .curvature_error_min_speed = 10.0, /* m/s */ \ + }, \ + /* Down table 1% looser than up (#20); both >= the Python control's tables (values_ext) */ \ + .curvature_rate_up_lookup = { \ + {5., 16., 25.}, \ + {0.0025f, 0.0014f, 0.00018f} \ + }, \ + .curvature_rate_down_lookup = { \ + {5., 16., 25.}, \ + {0.002525f, 0.001414f, 0.0001818f} \ + }, \ + .limit_lateral_acceleration = (limit_lateral_accel), \ +} + +// BluePilot: LateralMotionControl (CAN) or LateralMotionControl2 (CAN FD) with BP lateral on. Both +// messages carry the same four signals; ford_tx_hook decodes the raw values, and this converts them +// to signed CAN units (physical = raw * scale - offset), picks the limit set for the message and the +// angle_meas source, and runs ford_lmc_checks. +static inline bool ford_bp_lmc_tx(bool canfd, unsigned int raw_curvature, unsigned int raw_curvature_rate, + unsigned int raw_path_offset, unsigned int raw_path_angle, bool steer_control_enabled) { + // static inside the one function that reads them (MISRA 8.9) + static const FordBpCurvatureLimits FORD_BP_STEERING_LIMITS = FORD_LIMITS(false, 100); + static const FordBpCurvatureLimits FORD_BP_STEERING_LIMITS_PINION = FORD_LIMITS(false, 150); + static const FordBpCurvatureLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true, 100); + static const FordBpCurvatureLimits FORD_CANFD_STEERING_LIMITS_PINION = FORD_LIMITS(true, 150); + + const FordBpCurvatureLimits *limits; + unsigned int inactive_curvature_rate; + const char *dbg_prefix; + if (canfd) { + limits = ford_bp_pinion_curvature ? &FORD_CANFD_STEERING_LIMITS_PINION : &FORD_CANFD_STEERING_LIMITS; + inactive_curvature_rate = FORD_CANFD_INACTIVE_CURVATURE_RATE; + dbg_prefix = "CANFD Out"; + } else { + limits = ford_bp_pinion_curvature ? &FORD_BP_STEERING_LIMITS_PINION : &FORD_BP_STEERING_LIMITS; + inactive_curvature_rate = FORD_INACTIVE_CURVATURE_RATE; + dbg_prefix = "CAN Out"; + } + + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; + int desired_curvature_rate = raw_curvature_rate - inactive_curvature_rate; + int desired_path_offset = raw_path_offset - FORD_INACTIVE_PATH_OFFSET; + int desired_path_angle = raw_path_angle - FORD_INACTIVE_PATH_ANGLE; + return ford_lmc_checks(desired_curvature, desired_curvature_rate, desired_path_offset, desired_path_angle, + steer_control_enabled, limits, dbg_prefix); +} + +// BluePilot: angle_mode_engaged and shadow_curvature ride in Lane_Assist_Data1 bits with no DBC +// signal mapped to them (layout: bluepilot_lateral/can/LANE_ASSIST_DATA1.md), read out of +// the frame being sent. A separate CAN message (0x5F0) +// was tried first: the panda never receives its own TX, and an RX check for it tripped +// safety_tick's lagging check and blocked controls car-wide (2026-07-09). +static inline void ford_bp_lka_tx(const CANPacket_t *msg) { + ford_bp_angle_mode_engaged = (msg->data[FORD_BP_LKA_ANGLE_MODE_BYTE] & FORD_BP_LKA_ANGLE_MODE_MASK) != 0U; + unsigned int shadow_curvature_raw = (msg->data[FORD_BP_LKA_SHADOW_MSB_BYTE] << 8) | msg->data[FORD_BP_LKA_SHADOW_LSB_BYTE]; + ford_bp_shadow_curvature_raw = (int16_t)shadow_curvature_raw; +} + +// BluePilot: optional angle_meas source -- measured curvature from the steering pinion angle (PSCM) +// via the vehicle model, for vehicles whose RCM broadcasts implausible yaw (sign-inverted vs +// IMU/steering geometry) while its quality flag still reads OK. The pinion angle was validated +// against the comma IMU (corr +0.99 on real routes); the Python control layer measures from the +// same source when this is enabled (carcontroller_ext.InputTracker.get_current_curvature), so the layers always +// agree. ford_rx_hook skips its yaw block while this source is on. +static inline void ford_bp_rx_hook(const CANPacket_t *msg) { + // addr and bus compared directly, not with upstream's msg_matches, which not every host has + if ((msg->addr == FORD_SteeringPinion_Data) && (msg->bus == FORD_MAIN_BUS) && ford_bp_pinion_curvature) { + // Signal: StePinComp_An_Est, 22|15@0+ (0.1,-1600) deg + int angle_raw = ((msg->data[2] & 0x7FU) << 8) | msg->data[3]; + float pinion_angle_deg = ((float)angle_raw * 0.1f) - 1600.0f; + float pinion_angle_rad = pinion_angle_deg * 0.017453292519943295f; // DEG_TO_RAD + // angle -> curvature via vehicle model (matches VehicleModel.curvature_factor); + // sign: firmware angle_meas is Ford wire convention (the yaw block uses +yaw/v), and + // pinion angle correlates +0.97 with wire desired curvature on real frames -> positive. + float speed = SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); + float curvature_factor = get_curvature_factor(speed, *ford_bp_pinion_params); + float current_curvature = pinion_angle_rad * curvature_factor / ford_bp_pinion_params->steer_ratio; + // convert current curvature into units on CAN for comparison with desired curvature + update_sample(&BP_HOST_CURVATURE_STATE.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can)); + } +} + +// BluePilot: read the SP safety param and reset the BP lateral state. param_sp is +// current_safety_param_sp, delivered via USB 0xdf before the safety model is set (a separate uint16 +// from ford_init's param; pattern: subaru_common.h). Bits: 0 pinion-curvature enable, 1-4 pinion +// geometry index, 5 BP 4-signal lateral (clear = upstream's stock lateral safety, verbatim). A zero +// or out-of-range index disables the pinion source, so a half-configured param can never select the +// wrong geometry: the stock yaw path is kept. Returns whether the pinion source is on, i.e. whether +// ford_init applies the pinion RX check (ford_rx_checks_pinion). That check stays out of the stock +// config, so a pinion hiccup can't disable controls for users who never consume the message. +static inline bool ford_bp_init(uint16_t param_sp) { + const uint16_t FORD_PARAM_SP_BP_LATERAL = 32; + ford_bp_lateral = GET_FLAG(param_sp, FORD_PARAM_SP_BP_LATERAL); + const uint16_t FORD_PARAM_SP_STEER_ANGLE_CURVATURE = 1; + // Pinion measurement is only valid with the BP checks: the stock error band (100) was tuned for the + // yaw source and the stock Python path clips against yaw-derived curvature. + bool pinion_enabled = ford_bp_lateral && GET_FLAG(param_sp, FORD_PARAM_SP_STEER_ANGLE_CURVATURE); + const uint16_t pinion_geometry_index = (param_sp >> 1) & 0xFU; + if ((pinion_geometry_index == 0U) || (pinion_geometry_index > FORD_PINION_GEOMETRY_COUNT)) { + pinion_enabled = false; + } + ford_bp_pinion_curvature = pinion_enabled; + ford_bp_pinion_params = pinion_enabled ? &ford_pinion_geometry[pinion_geometry_index] : &ford_pinion_geometry[0]; + + // start from a clean BP lateral state, as set_safety_hooks does for upstream's (desired_angle_last + // etc.), so a re-init never checks against a previous session's commands. The angle-mode flag is + // set again by the next Lane_Assist_Data1 frame. + desired_path_angle_last = 0; + desired_path_offset_last = 0; + ford_bp_angle_mode_engaged = false; + return ford_bp_pinion_curvature; +} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_bp.h b/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_bp.h deleted file mode 100644 index 7c46d54890..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_bp.h +++ /dev/null @@ -1,249 +0,0 @@ -#pragma once -// Angle-primary Ford extension adapted from Alan Polk's bp-7.0 e1d051d7. -// See ../CREDITS.md, ../LICENSE and ../LICENSE.md. No reset bypass. -#include "opendbc/bluepilot_lateral/params/protocol.h" -#include "opendbc/bluepilot_lateral/params/limits.h" - -// Shared Ford stock-switch state is declared before the angle hooks below. -static bool ford_stock_switch = false; -static bool ford_bp_enabled = false; -static bool ford_bp_canfd = false; -static bool ford_bp_shadow_seen = false; -static bool ford_bp_lka_counter_seen = false; -static uint8_t ford_bp_lka_counter = 0U; -static int ford_bp_shadow_raw = 0; -static uint32_t ford_bp_shadow_us = 0U; -static int ford_bp_path_last = 0; -static bool ford_bp_control_counter_seen = false; -static uint8_t ford_bp_control_counter = 0U; -static bool ford_bp_control_seen = false; -static uint32_t ford_bp_control_us = 0U; -static float ford_bp_equivalent_last = 0.0F; -static unsigned int ford_bp_rt_msgs = 0U; -static unsigned int ford_bp_rt_msgs_prev = 0U; -static uint32_t ford_bp_rt_us = 0U; -static bool ford_bp_rx_seen[7]; -static uint32_t ford_bp_rx_us[7]; - -static void ford_bp_invalidate_shadow(void) { - ford_bp_shadow_seen = false; - ford_bp_shadow_raw = 0; -} - -static void ford_bp_withdraw(void) { - ford_bp_invalidate_shadow(); - ford_bp_path_last = 0; - ford_bp_equivalent_last = 0.0F; -} - -static bool ford_bp_configure(uint16_t param) { - ford_bp_enabled = ((param == 128U) || (param == 129U) || (param == 130U)) && (alternative_experience == 0); -#ifdef ALLOW_DEBUG - ford_bp_enabled |= (param == 131U) && (alternative_experience == 0); -#endif - const bool joint = ((unsigned int)alternative_experience == 32U) && - ((param == 146U) || (param == 147U) || (param == 160U) || (param == 161U) || (param == 194U) || (param == 195U)); - ford_bp_enabled |= joint; - ford_bp_canfd = (param & 2U) != 0U; - ford_bp_lka_counter_seen = false; - ford_bp_control_counter_seen = false; - ford_bp_control_seen = false; - ford_bp_lka_counter = 0U; - ford_bp_control_counter = 0U; - ford_bp_shadow_us = 0U; - ford_bp_control_us = 0U; - ford_bp_rt_msgs = 0U; - ford_bp_rt_msgs_prev = 0U; - ford_bp_rt_us = microsecond_timer_get(); - ford_bp_withdraw(); - for (unsigned int i = 0U; i < 7U; i++) { - ford_bp_rx_seen[i] = false; - ford_bp_rx_us[i] = 0U; - } - return ((param & 128U) == 0U) || ford_bp_enabled; -} - -static bool ford_bp_health(uint32_t now); - -static bool ford_bp_permission(void) { - bool permitted = false; - if (ford_aol_enabled) { - permitted = (ford_aol_permission_mask() & 1U) != 0U; - } else { - permitted = lateral_controls_allowed() && controls_allowed && !brake_pressed && !gas_pressed; - } - return permitted; -} - -static void ford_bp_rx(const CANPacket_t *msg) { - if (ford_bp_enabled && (msg->bus == 0U) && (GET_LEN(msg) == 8U)) { - const unsigned int ids[7] = {0x415U, 0x202U, 0x91U, 0x165U, 0x204U, 0x213U, 0x83U}; - for (unsigned int i = 0U; i < 7U; i++) { - if (msg->addr == ids[i]) { - ford_bp_rx_seen[i] = true; - ford_bp_rx_us[i] = microsecond_timer_get(); - } - } - // Angle-specific source freshness is stricter than the generic1s RX tick. - // Expose that withdrawal through the ordinary public controls acknowledgment. - if (controls_allowed && !ford_bp_health(microsecond_timer_get())) { - controls_allowed = false; - } - // Permission losses remove the old actuator history before any later arm. - if (!ford_bp_permission() || !ford_bp_health(microsecond_timer_get())) { - ford_bp_withdraw(); - } - } -} - -static bool ford_bp_health(uint32_t now) { - bool valid = !safety_rx_checks_invalid && !relay_malfunction; - for (int i = 0; i < current_safety_config.rx_checks_len; i++) { - const RxStatus *status = ¤t_safety_config.rx_checks[i].status; - valid &= status->msg_seen && !status->lagging && status->valid_checksum && - status->valid_quality_flag && (status->wrong_counters < MAX_WRONG_COUNTERS); - } - const unsigned int count = ford_stock_switch ? 7U : 6U; - for (unsigned int i = 0U; i < count; i++) { - const uint32_t age = ((i == 3U) || (i == 6U)) ? (2U * FORD_BP_RX_AGE_US) : FORD_BP_RX_AGE_US; - valid &= ford_bp_rx_seen[i] && (safety_get_ts_elapsed(now, ford_bp_rx_us[i]) <= age); - } - return valid; -} - -static bool ford_bp_lka(const CANPacket_t *msg) { - const uint32_t now = microsecond_timer_get(); - const unsigned int counter = (msg->data[4] & FORD_BP_COUNTER_MASK) >> FORD_BP_COUNTER_SHIFT; - unsigned int sum = 0U; - for (unsigned int i = 0U; i < 7U; i++) { sum += msg->data[i]; } - const unsigned int checksum = (15U - (sum & 15U)) & 15U; - // Preserve every DBC-owned bit of the packer's neutral LKA body. - const unsigned int angle = ((msg->data[2] & 0xFU) << 8) | msg->data[3]; - const unsigned int curvature = (msg->data[1] << 4) | (msg->data[2] >> 4); - bool valid = !relay_malfunction && (msg->bus == 0U) && (GET_LEN(msg) == 8U) && (msg->data[0] == 0U) && - (angle == 2048U) && (curvature == 2048U) && - ((msg->data[4] & 0xE3U) == FORD_BP_ANGLE_MASK) && - ((msg->data[7] & FORD_BP_VERSION_MASK) == FORD_BP_VERSION) && - ((msg->data[7] >> FORD_BP_CHECKSUM_SHIFT) == checksum); - const int shadow = to_signed((msg->data[5] << 8) | msg->data[6], 16); - const bool resync = (shadow == 0) && !ford_bp_permission(); - if (ford_bp_lka_counter_seen && !resync) { - valid &= counter == ((ford_bp_lka_counter + 1U) % FORD_BP_COUNTER_MODULUS); - valid &= safety_get_ts_elapsed(now, ford_bp_shadow_us) >= 20000U; - } - valid &= SAFETY_ABS(shadow) <= 20000; - if (valid) { - ford_bp_lka_counter = (uint8_t)counter; - ford_bp_lka_counter_seen = true; - ford_bp_shadow_raw = shadow; - ford_bp_shadow_seen = !resync; - ford_bp_shadow_us = now; - } else { - ford_bp_invalidate_shadow(); - } - return valid; -} - -static bool ford_bp_lateral(const CANPacket_t *msg) { - const bool fd = msg->addr == 0x3D6U; - const unsigned int mode = fd ? ((msg->data[0] >> 4) & 7U) : ((msg->data[4] >> 2) & 7U); - const unsigned int curvature = fd ? ((msg->data[2] << 3) | (msg->data[3] >> 5)) : - ((msg->data[0] << 3) | (msg->data[1] >> 5)); - const unsigned int rate = fd ? (((unsigned int)msg->data[6] << 3) | (msg->data[7] >> 5)) : - (((msg->data[1] & 31U) << 8) | msg->data[2]); - const unsigned int path_raw = fd ? (((msg->data[3] & 31U) << 6) | (msg->data[4] >> 2)) : - (((unsigned int)msg->data[3] << 3) | (msg->data[4] >> 5)); - const unsigned int offset = fd ? (((msg->data[4] & 3U) << 8) | msg->data[5]) : - (((unsigned int)msg->data[5] << 2) | (msg->data[6] >> 6)); - const unsigned int precision = fd ? ((msg->data[0] >> 2) & 3U) : (msg->data[4] & 3U); - const unsigned int ramp = fd ? (msg->data[0] & 3U) : ((msg->data[6] >> 4) & 3U); - const bool active = mode == 1U; - const int path = (int)path_raw - 1000; - const uint32_t now = microsecond_timer_get(); - bool valid = !relay_malfunction && (msg->bus == 0U) && (GET_LEN(msg) == 8U) && (fd == ford_bp_canfd) && (mode <= 1U) && - (curvature == 1000U) && (rate == (fd ? 1024U : 4096U)) && (offset == 512U) && - (precision <= 1U) && (ramp == (active ? 2U : 0U)); - const unsigned int counter = (msg->data[7] >> 1) & 15U; - if (fd) { - unsigned int checksum = mode + counter; - const unsigned int fields[4] = {curvature, rate, path_raw, offset}; - for (unsigned int i = 0U; i < 4U; i++) { checksum += fields[i] + (fields[i] >> 8); } - valid &= msg->data[1] == (0xFFU - (checksum & 0xFFU)); - valid &= ((msg->data[0] & 0x80U) == 0U) && ((msg->data[7] & 1U) == 0U); - if (ford_bp_control_counter_seen && active) { valid &= counter == ((ford_bp_control_counter + 1U) % 16U); } - } else { - valid &= ((msg->data[6] & 0xFU) == 0U) && (msg->data[7] == 0U); - } - const float speed_for_equivalent = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 0.1F); - const float equivalent = (float)path / (2000.0F * speed_for_equivalent); - // Time-window expiration is independent of packet acceptance: denied floods - // cannot latch the old count forever or erase accepted actuator history. - const uint32_t rt_elapsed = safety_get_ts_elapsed(now, ford_bp_rt_us); - if (rt_elapsed >= FORD_BP_RT_INTERVAL_US) { - ford_bp_rt_msgs = 0U; - ford_bp_rt_msgs_prev = 0U; - ford_bp_rt_us = now; - } else if (rt_elapsed >= (FORD_BP_RT_INTERVAL_US / 2U)) { - ford_bp_rt_msgs_prev = ford_bp_rt_msgs; - ford_bp_rt_msgs = 0U; - ford_bp_rt_us = now; - } else { - // Keep the current history while neither expiration boundary has elapsed. - } - const unsigned int rt_count = ford_bp_rt_msgs + ford_bp_rt_msgs_prev; - valid &= rt_count <= 7U; // Current20Hz curvature RT allowance; accepted frames own this count. - if (active) { - static const struct lookup_t roc = {{10.0F, 15.0F, 25.0F}, {0.0561F, 0.04335F, 0.00918F}}; - const float speed_min = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 0.1F); - const float speed_max = SAFETY_MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 0.1F); - const float delta_float = safety_interpolate(roc, speed_min - 1.0F) * 2000.0F; - const int delta = (int)delta_float + 1; - const float shadow = (float)ford_bp_shadow_raw * 0.000001F; - const float shadow_can = shadow * 50000.0F; - const float path_abs = SAFETY_ABS(path) / 2000.0F; - const float path_curvature_low = path_abs / (speed_max * FORD_BP_GAIN_MAX); - const float path_curvature_high = path_abs / (speed_min * FORD_BP_GAIN_MIN); - const float physical_cap = SAFETY_MIN(FORD_BP_CURVATURE_MAX, FORD_BP_LATERAL_ACCEL / (SAFETY_MAX(speed_max, 1.0F) * SAFETY_MAX(speed_max, 1.0F))); - const float relation_low = SAFETY_ABS(shadow) * speed_min * FORD_BP_GAIN_MIN * 2000.0F; - const float relation_high = SAFETY_ABS(shadow) * speed_max * FORD_BP_GAIN_MAX * 2000.0F; - valid &= ford_bp_permission() && ford_bp_health(now) && ford_bp_shadow_seen && - (safety_get_ts_elapsed(now, ford_bp_shadow_us) <= FORD_BP_SHADOW_AGE_US) && - (path >= -1000) && (path <= 1047) && (SAFETY_ABS(path - ford_bp_path_last) <= delta) && - ((path == 0) || ((ford_bp_shadow_raw != 0) && ((path > 0) == (ford_bp_shadow_raw > 0)))) && - (SAFETY_ABS(path) >= (relation_low - 2.0F)) && (SAFETY_ABS(path) <= (relation_high + 2.0F)) && - (SAFETY_ABS(shadow) <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN)) && (path_curvature_high <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN)); - if (speed_max > 10.0F) { - const int measured_min = curvature_state.meas.min - (ford_pinion_enabled ? 150 : 100); - const int measured_max = curvature_state.meas.max + (ford_pinion_enabled ? 150 : 100); - const float measured_min_float = (float)measured_min; - const float measured_max_float = (float)measured_max; - valid &= (shadow_can >= measured_min_float) && (shadow_can <= measured_max_float); - // Independent actual-actuator check; an invented shadow cannot launder path angle. - const float signed_low = (path < 0) ? (-path_curvature_high) : path_curvature_low; - const float signed_high = (path < 0) ? (-path_curvature_low) : path_curvature_high; - valid &= ((signed_low * 50000.0F) >= measured_min_float) && - ((signed_high * 50000.0F) <= measured_max_float); - } - const uint32_t elapsed = ford_bp_control_seen ? safety_get_ts_elapsed(now, ford_bp_control_us) : FORD_BP_STEER_PERIOD_US; - const float jerk_speed = SAFETY_MAX(speed_min - 1.0F, 1.0F); - const float jerk_delta = FORD_BP_LATERAL_JERK / (jerk_speed * jerk_speed) * - (SAFETY_MIN(elapsed, FORD_BP_STEER_PERIOD_US) * 0.000001F) * FORD_BP_GAIN_MIN; - valid &= SAFETY_ABS(equivalent - ford_bp_equivalent_last) <= (jerk_delta + FORD_BP_SHADOW_ROUNDING_MARGIN); - if (ford_bp_control_seen) { valid &= elapsed >= 40000U; } - } else { - valid &= path == 0; - } - if (valid) { - ford_bp_rt_msgs += 1U; - ford_bp_equivalent_last = active ? equivalent : 0.0F; - ford_bp_path_last = active ? path : 0; - ford_bp_control_counter = (uint8_t)counter; - ford_bp_control_counter_seen = true; - ford_bp_control_seen = true; - ford_bp_control_us = now; - if (!active) { ford_bp_shadow_seen = false; } - } else { - ford_bp_invalidate_shadow(); - } - return valid; -} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_declarations.h b/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_declarations.h new file mode 100644 index 0000000000..5565422ceb --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/ford_declarations.h @@ -0,0 +1,99 @@ +/** + * BluePilot Ford lateral-control extensions: type declarations, state, and prototypes. + * See ford.h in this directory for the implementations. + */ + +#pragma once + +#include "../../safety/declarations.h" +#include "../safety/lateral_declarations.h" + +// BluePilot: Ford safety debug prints. Compile-time switch: build with -DFORD_BP_DEBUG (bench / +// libsafety only) to turn every FORD_BP_DBG into a printf; otherwise it expands to nothing, so CI +// coverage and mutation see no code and the panda build carries no runtime flag. The printf +// branch is bench-only and is not part of the MISRA-checked configuration (test_misra.sh runs +// without FORD_BP_DEBUG). To use it on the bench, compile libsafety with +// cc ... -DALLOW_DEBUG -DFORD_BP_DEBUG -I . -c /safety/tests/libsafety/safety.c +// (the same flags /safety/tests/libsafety/libsafety_py.py uses, plus FORD_BP_DEBUG; is the +// host's opendbc package: opendbc, iqdbc). +#ifdef FORD_BP_DEBUG +#include +#define FORD_BP_DBG(...) printf(__VA_ARGS__) +#else +#define FORD_BP_DBG(...) ((void)0) +#endif + +// =============================== +// Constants and Defines +// =============================== + +// BluePilot: steering-angle curvature measurement (bad-yaw-sensor workaround). Index 0 of +// ford_pinion_geometry is reserved as invalid; see ford_bp_init in ford.h. +#define FORD_PINION_GEOMETRY_COUNT 12U +// Row count including the reserved index-0 entry. Kept as a plain literal: the mutation +// harness rewrites arithmetic in array sizes into runtime expressions, which cannot compile. +#define FORD_PINION_GEOMETRY_ROWS 13U + +// Control signal limits -- curvature magnitude must match MAX_CURVATURE; rate tables must match +// bluepilot_lateral/core/values_ext.py BP_ANGLE_LIMITS. +#define FORD_CURVATURE_MIN -0.02f +#define FORD_CURVATURE_MAX 0.02f +#define FORD_PATH_OFFSET_MIN -1.0f +#define FORD_PATH_OFFSET_MAX 1.0f +#define FORD_PATH_ANGLE_MIN -0.25f +#define FORD_PATH_ANGLE_MAX 0.25f +// BluePilot: full DBC signal range for path_angle (LatCtlPath_An, 0.0005 scale). Used as the +// value limit in ANGLE mode, where path_angle is the steering actuator (see ford_lmc_checks). +// Matches lateral_angle_ext.py FORD_DBC_PATH_ANGLE_MIN/MAX. In curvature mode the tight ±0.25 +// cap above applies instead (path_angle there only trims and amplifies wound-up curvature). +#define FORD_DBC_PATH_ANGLE_MIN -0.5f +#define FORD_DBC_PATH_ANGLE_MAX 0.5235f + +// =============================== +// Global Variables +// =============================== + +// BluePilot: optional angle_meas source, measured curvature from the steering pinion angle +// (PSCM) via the vehicle model, selected by the geometry index in current_safety_param_sp +// bits 1-4. Default off = stock yaw-sourced angle_meas. See ford_bp_init in ford.h. +extern bool ford_bp_lateral; +extern bool ford_bp_pinion_curvature; +extern const AngleSteeringParams *ford_bp_pinion_params; +extern const AngleSteeringParams ford_pinion_geometry[FORD_PINION_GEOMETRY_ROWS]; + +// BluePilot: angle_mode_engaged + shadow_curvature, read out of Lane_Assist_Data1's unused +// bits by ford_bp_lka_tx (no separate CAN message, no RX). The layout is in +// bluepilot_lateral/can/LANE_ASSIST_DATA1.md; fordcan_ext.py packs it with the same values +// (tests/test_lane_assist_data1.py). +#define FORD_BP_LKA_ANGLE_MODE_BYTE 4U +#define FORD_BP_LKA_ANGLE_MODE_MASK 0x1U +#define FORD_BP_LKA_SHADOW_MSB_BYTE 5U // shadow curvature: int16, big-endian, bytes 5-6 +#define FORD_BP_LKA_SHADOW_LSB_BYTE 6U +// shadow curvature raw units (1e-6 1/m) to curvature CAN units (2e-5 1/m) +#define FORD_BP_LKA_SHADOW_RAW_TO_CAN 0.05f +extern bool ford_bp_angle_mode_engaged; +extern int16_t ford_bp_shadow_curvature_raw; // wire units, scale 1e-6 1/m (LANE_ASSIST_DATA1.md) + +// last commanded values for the path_angle / path_offset rate-of-change checks +extern int desired_path_angle_last; +extern int desired_path_offset_last; + +// =============================== +// Function Declarations +// =============================== + +static inline bool path_angle_cmd_checks(int desired_path_angle, bool steer_control_enabled, AngleSteeringLimits limits); +static inline bool path_offset_cmd_checks(int desired_path_offset, bool steer_control_enabled, AngleSteeringLimits limits); +static inline bool ford_shadow_curvature_error_check(int desired_curvature, bool steer_control_enabled, BP_HOST_CURVATURE_LIMITS limits); + +// Shared curvature/curvature_rate/path_offset/path_angle command checks for LateralMotionControl +// (CAN) and LateralMotionControl2 (CAN FD) -- see ford_lmc_checks in ford.h. +static inline bool ford_lmc_checks(int desired_curvature, int desired_curvature_rate, int desired_path_offset, int desired_path_angle, + bool steer_control_enabled, const FordBpCurvatureLimits *limits, const char *dbg_prefix); + +// Called from modes/ford.h's hooks and ford_init -- see bluepilot_lateral/safety/ford.h. +static inline bool ford_bp_lmc_tx(bool canfd, unsigned int raw_curvature, unsigned int raw_curvature_rate, + unsigned int raw_path_offset, unsigned int raw_path_angle, bool steer_control_enabled); +static inline void ford_bp_lka_tx(const CANPacket_t *msg); +static inline void ford_bp_rx_hook(const CANPacket_t *msg); +static inline bool ford_bp_init(uint16_t param_sp); diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral.h b/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral.h new file mode 100644 index 0000000000..4a0150f2cc --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral.h @@ -0,0 +1,121 @@ +/** + * BluePilot lateral-control extensions shared across safety modes: implementation. + * See lateral_declarations.h in this directory for the declarations. + */ + +#pragma once + +#include "../safety/lateral_declarations.h" + +// BluePilot: measured-table rate limiting. This is the pre-sync steer_angle_cmd_checks logic, +// carried over verbatim in the curvature domain so Ford lateral behavior is unchanged by the +// move off AngleSteeringLimits. Differences from upstream's ISO-only path (steer_curvature_cmd_checks +// in opendbc/safety/lateral.h) that matter: +// - asymmetric up/down deltas selected by the sign of the last command +// - the error boundary uses relaxed deltas and CONVERGES toward measured, rather than +// hard-clamping, so a wound-up command unwinds instead of instantly faulting +// - the ISO lateral accel cap is applied only on buses that need it (Q4/CAN FD) +static inline bool ford_bp_curvature_rate_lookup_check(int desired_curvature, const FordBpCurvatureLimits *limits) { + const BP_HOST_CURVATURE_LIMITS *base = &limits->base; + bool violation = false; + + const float fudged_speed_lookup = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.; + const int delta_up = (safety_interpolate(limits->curvature_rate_up_lookup, fudged_speed_lookup) * base->curvature_to_can) + 1.; + const int delta_down = (safety_interpolate(limits->curvature_rate_down_lookup, fudged_speed_lookup) * base->curvature_to_can) + 1.; + + // allow down limits at zero since small floats from openpilot will be rounded to 0 + int highest_desired_curvature = BP_HOST_CURVATURE_STATE.desired_last + ((BP_HOST_CURVATURE_STATE.desired_last > 0) ? delta_up : delta_down); + int lowest_desired_curvature = BP_HOST_CURVATURE_STATE.desired_last - ((BP_HOST_CURVATURE_STATE.desired_last >= 0) ? delta_down : delta_up); + + if (base->max_curvature_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > base->curvature_error_min_speed)) { + // flipped fudge to avoid false positives + const float fudged_speed_error = (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.; + const int delta_up_relaxed = (safety_interpolate(limits->curvature_rate_up_lookup, fudged_speed_error) * base->curvature_to_can) - 1.; + const int delta_down_relaxed = (safety_interpolate(limits->curvature_rate_down_lookup, fudged_speed_error) * base->curvature_to_can) - 1.; + + const int lowest_desired_curvature_error = BP_HOST_CURVATURE_STATE.meas.min - base->max_curvature_error - 1; + const int highest_desired_curvature_error = BP_HOST_CURVATURE_STATE.meas.max + base->max_curvature_error + 1; + + // the MAX/MIN allow the desired value to reach the edge of the bounds without going under + if (BP_HOST_CURVATURE_STATE.desired_last > highest_desired_curvature_error) { + const int delta = (BP_HOST_CURVATURE_STATE.desired_last >= 0) ? delta_down_relaxed : delta_up_relaxed; + highest_desired_curvature = SAFETY_MAX(BP_HOST_CURVATURE_STATE.desired_last - delta, highest_desired_curvature_error); + + } else if (BP_HOST_CURVATURE_STATE.desired_last < lowest_desired_curvature_error) { + const int delta = (BP_HOST_CURVATURE_STATE.desired_last <= 0) ? delta_down_relaxed : delta_up_relaxed; + lowest_desired_curvature = SAFETY_MIN(BP_HOST_CURVATURE_STATE.desired_last + delta, lowest_desired_curvature_error); + + } else { + // already inside error boundary, don't allow commanding outside it + highest_desired_curvature = SAFETY_MIN(highest_desired_curvature, highest_desired_curvature_error); + lowest_desired_curvature = SAFETY_MAX(lowest_desired_curvature, lowest_desired_curvature_error); + } + + // don't enforce above the max curvature + lowest_desired_curvature = SAFETY_CLAMP(lowest_desired_curvature, -base->max_curvature, base->max_curvature); + highest_desired_curvature = SAFETY_CLAMP(highest_desired_curvature, -base->max_curvature, base->max_curvature); + } + + violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature, lowest_desired_curvature); + + if (limits->limit_lateral_acceleration) { + // Limit to average banked road since safety doesn't have the roll. NOTE: this is the + // pre-sync constant (ISO_LATERAL_ACCEL MINUS the roll term, ~2.4 m/s^2), not the + // MAX_LATERAL_ACCEL used by the ISO path in steer_curvature_cmd_checks (~3.6 m/s^2). Do not + // merge the two. + static const float BP_MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); + const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0); + const float max_curvature_accel = BP_MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed); + const int max_curvature_accel_can = (max_curvature_accel * base->curvature_to_can) + 1.; + violation |= safety_max_limit_check(desired_curvature, max_curvature_accel_can, -max_curvature_accel_can); + } + + return violation; +} + +// BluePilot: upstream's steer_curvature_cmd_checks (opendbc/safety/lateral.h) with +// ford_bp_curvature_rate_lookup_check in place of its ISO lateral accel and jerk checks. Ford has no +// steer power signal (max_steer_power 0) and resets to zero on a violation +// (inactive_curvature_is_zero), so only those sides of upstream's branches are kept. +// The other difference from upstream: curvature 0 (no curvature command: angle mode, curvature +// mode's reset frame) skips the cap and rate-table checks, keeping the message-rate check and the +// gates. Keep it when re-syncing against upstream (#60). +static inline bool ford_bp_curvature_cmd_checks(int desired_curvature, bool steer_control_enabled, + const FordBpCurvatureLimits *limits) { + bool violation = false; + + BP_HOST_SPEED_MISMATCH_CHECK(); + + if (BP_HOST_LAT_ALLOWED() && steer_control_enabled) { + // BluePilot: curvature 0 is the no-curvature-command sentinel (angle mode steers with path_angle + // and holds curvature at 0; curvature mode's reset frame sends 0), so the value checks don't + // apply to it: the error band would trip as the car turns while the command stays at 0. The + // message-rate check and the gates below still run. + if (desired_curvature != 0) { + // *** absolute curvature cap *** + violation |= safety_max_limit_check(desired_curvature, limits->base.max_curvature, -limits->base.max_curvature); + + // *** measured-table rate limits, error band and (CAN FD) lateral accel cap *** + violation |= ford_bp_curvature_rate_lookup_check(desired_curvature, limits); + } + + // *** real time rate limit check *** + violation |= BP_HOST_RT_CURVATURE_RATE_CHECK(limits->base); + } + BP_HOST_CURVATURE_STATE.desired_last = desired_curvature; + + // Curvature must be 0 while not steering + if (!steer_control_enabled) { + violation |= desired_curvature != 0; + } + + // No curvature control allowed when controls are not allowed + violation |= !BP_HOST_LAT_ALLOWED() && steer_control_enabled; + + // reset to zero (inactive_curvature_is_zero) + if (violation || !BP_HOST_LAT_ALLOWED()) { + BP_HOST_CURVATURE_STATE.desired_last = 0; + } + + return violation; +} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral_declarations.h b/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral_declarations.h new file mode 100644 index 0000000000..820e980884 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/lateral_declarations.h @@ -0,0 +1,60 @@ +/** + * BluePilot lateral-control extensions shared across safety modes: declarations. + * See lateral.h in this directory for the implementation. + */ + +#pragma once + +#include "../../safety/declarations.h" + +// BluePilot: the host bindings. The library is written against comma's opendbc safety names; a host +// whose names differ defines these before including safety/ford.h (HOSTS.md, "Host bindings"). The +// defaults are upstream opendbc's (sunnypilot/opendbc), so a host on those names defines nothing. +// Type of FordBpCurvatureLimits.base: the host's curvature limit struct. The library reads only its +// max_curvature, curvature_to_can, frequency, max_curvature_error and curvature_error_min_speed. +#ifndef BP_HOST_CURVATURE_LIMITS +#define BP_HOST_CURVATURE_LIMITS CurvatureSteeringLimits +#endif +// The host's curvature state (desired_last, meas), the one its curvature RX hook fills. +#ifndef BP_HOST_CURVATURE_STATE +#define BP_HOST_CURVATURE_STATE curvature_state +#endif +// The host's real-time message-rate check, taking a BP_HOST_CURVATURE_LIMITS by value. +#ifndef BP_HOST_RT_CURVATURE_RATE_CHECK +#define BP_HOST_RT_CURVATURE_RATE_CHECK rt_curvature_rate_limit_check +#endif +// Whether lateral control is allowed: controls_allowed or the host's lateral-only engagement (MADS). +#ifndef BP_HOST_LAT_ALLOWED +#define BP_HOST_LAT_ALLOWED() (controls_allowed || controls_allowed_lateral) +#endif +// The second-speed cross-check upstream runs inside its curvature check. A host that runs it in its +// RX hook instead defines this as ((void)0). +#ifndef BP_HOST_SPEED_MISMATCH_CHECK +#define BP_HOST_SPEED_MISMATCH_CHECK() speed_mismatch_check((float)vehicle_speed_2.values[0] / VEHICLE_SPEED_FACTOR) +#endif + +// BluePilot: curvature limits for the measured-table check. base holds upstream's fields +// (max_curvature, curvature_to_can, frequency, error band, ...); the rest are BluePilot's. +typedef struct { + BP_HOST_CURVATURE_LIMITS base; + // Explicit rate-of-change tables, mirrored from values_ext.py BP_ANGLE_LIMITS. Upstream derives + // the per-tick delta from the ISO lateral jerk limit alone, which is looser than Ford's EPS + // tolerates at low speed and tighter than it tolerates at high speed. + struct lookup_t curvature_rate_up_lookup; + struct lookup_t curvature_rate_down_lookup; + // Ford CAN (Q3) is NOT lateral-accel limited in safety -- the EPS already caps it, and applying + // the ISO cap here would clip commands the car accepts today. Ford CAN FD (Q4) has more torque + // available and IS limited. + bool limit_lateral_acceleration; +} FordBpCurvatureLimits; + +// BluePilot: measured-table curvature rate-of-change + error-band check, used by +// ford_bp_curvature_cmd_checks below. +static inline bool ford_bp_curvature_rate_lookup_check(int desired_curvature, const FordBpCurvatureLimits *limits); + +// BluePilot: curvature command checks for the BluePilot lateral (Ford only today, see the +// FORD_LIMITS macro in bluepilot_lateral/safety/ford.h). Upstream's steer_curvature_cmd_checks with +// the measured-table check in place of its ISO accel and jerk checks, and no value checks at +// curvature 0, the no-curvature-command sentinel (only the message-rate check and the gates). +static inline bool ford_bp_curvature_cmd_checks(int desired_curvature, bool steer_control_enabled, + const FordBpCurvatureLimits *limits); diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/__init__.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_coverage.sh b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_coverage.sh new file mode 100755 index 0000000000..804a0daad5 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_coverage.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash +# BluePilot: 100% line coverage of the BluePilot safety headers (#53). Upstream's test.sh only measures +# files under /safety/, and the headers live in /bluepilot_lateral/safety/. is the host's +# opendbc package, whatever it's named (opendbc, iqdbc): the one the library is mounted in. +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +ROOT="$(cd "$DIR/../../../.." && pwd)" +PKG="$(basename "$(cd "$DIR/../../.." && pwd)")" +cd "$ROOT/$PKG/safety/tests" + +source "$ROOT/setup.sh" + +# reset coverage data, then run upstream's safety tests. The BluePilot ones come in through the host's +# loader (test_bluepilot_lateral.py) where it has one; otherwise they run here on their own. +rm -f ./libsafety/*.gcda ./libsafety/*.gcno +SAFETY_COVERAGE=1 python -m unittest discover -s . +if [ ! -f test_bluepilot_lateral.py ]; then + SAFETY_COVERAGE=1 python -m unittest discover -s "$ROOT/$PKG/bluepilot_lateral/safety/tests" -t "$ROOT" +fi + +if [ "$(uname)" = "Darwin" ]; then + GCOV_EXEC="llvm-cov gcov" +else + GCOV_EXEC="gcov" +fi + +# Two reports: the library's safety/ford.h and the host's modes/ford.h share a file name, and one gcovr run +# over both can credit one's lines to the other. modes/ford.h is measured from /safety, as the host's +# test.sh does; the second run deletes the coverage data. +if ! OUT="$(gcovr -r "$ROOT/$PKG/safety" --gcov-executable "$GCOV_EXEC" --fail-under-line=100 \ + -f "$ROOT/$PKG/safety/modes/ford.h")"; then + echo -e "FAILED:\n$OUT" + exit 1 +fi +echo "$OUT" +if ! OUT="$(gcovr -r "$ROOT" --gcov-executable "$GCOV_EXEC" -d --fail-under-line=100 \ + -f "$ROOT/$PKG/bluepilot_lateral/safety/")"; then + echo -e "FAILED:\n$OUT" + exit 1 +fi +echo "$OUT" +echo "SUCCESS: the BluePilot safety headers and modes/ford.h have 100% coverage!" diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_mutation.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_mutation.py new file mode 100644 index 0000000000..87f7a1d866 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_mutation.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +"""BluePilot: run upstream's mutation.py on the BluePilot safety headers (#53). + +Upstream's run only mutates code from files under /safety/, and the BluePilot headers live in +the library (/bluepilot_lateral/safety/). This points the same run at them; the killer tests +are the ones mutation.py already discovers in /safety/tests/, the BluePilot ones through the +loader there (test_bluepilot_lateral.py). Same options as mutation.py. is the host's opendbc +package, whatever it's named (opendbc, iqdbc): the one the library is mounted in.""" +import importlib +import sys +from pathlib import Path + +PKG = Path(__file__).resolve().parents[3].name +mutation = importlib.import_module(f"{PKG}.safety.tests.mutation") + +mutation.SAFETY_DIR = mutation.ROOT / PKG / "bluepilot_lateral" / "safety" + +# Equivalent mutants: no test can kill them, so they are skipped (mutation.py keeps its own list inside +# main()). ford.h's +-0.02 range check in ford_lmc_checks is the same +-1000 CAN cap as lateral.h's +# max_curvature check, which blocks every frame it would (its steering-off and controls-not-allowed +# checks cover the rest), and the range check writes no state (only ford_bp_curvature_cmd_checks +# resets desired_last). It stays as the backstop if a platform's max_curvature ever differs, and a +# test can reach it then. +KNOWN_SURVIVORS = { + (f"{PKG}/bluepilot_lateral/safety/ford.h", 197, "|=", "&="), +} + +_enumerate_sites = mutation.enumerate_sites + + +def key(site): + return str(site.origin_file.relative_to(mutation.ROOT)), site.origin_line, site.original_op, site.mutated_op + + +def enumerate_sites(input_source, preprocessed_file): + sites, counts, build_incompatible_ids, source = _enumerate_sites(input_source, preprocessed_file) + skipped = [s for s in sites if key(s) in KNOWN_SURVIVORS] + assert len(skipped) == len(KNOWN_SURVIVORS), f"an equivalent mutant moved or is gone: {sorted(KNOWN_SURVIVORS - {key(s) for s in skipped})}" + for s in skipped: + print(f"skipping equivalent mutant: {key(s)[0]}:{s.origin_line} [{s.mutator}] {s.original_op}->{s.mutated_op}", flush=True) + return [s for s in sites if key(s) not in KNOWN_SURVIVORS], counts, build_incompatible_ids, source + + +mutation.enumerate_sites = enumerate_sites # main() looks it up by name + +if __name__ == "__main__": + sys.exit(mutation.main()) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/ford_bluepilot_common.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/ford_bluepilot_common.py new file mode 100644 index 0000000000..a313f97fa2 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/ford_bluepilot_common.py @@ -0,0 +1,268 @@ +""" +BluePilot: shared harness for the Ford 4-signal lateral safety tests. + +Not a test module (no test_ prefix). Provides: + * BPFordLateralModel: a Python mirror of the BluePilot curvature checks in + opendbc/bluepilot_lateral/safety/lateral.h (ford_bp_curvature_rate_lookup_check) and the lateral + acceleration cap, computed with the same float32 arithmetic and rounding as the C, so the + tests can assert exact CAN-unit boundaries on both x86 and ARM runners. + * BPFordTestCase: unittest base that borrows upstream's Ford test class for its message + builders (composition, so upstream's tests are not inherited) and sets the BP_LATERAL safety + param bit before ford_init. +""" +import re +import unittest +from pathlib import Path + +import numpy as np + +from ....car.structs import CarParams +from ....car.ford.fordcan import CanBus +from ....car.ford.values import FordSafetyFlags +from ...can import fordcan_ext +from ...core.values_ext import FordSafetyFlagsSP +from ....safety.tests.libsafety import libsafety_py +from ....safety.tests.common import CANPackerSafety +from ....safety.tests import test_ford # module import keeps its test classes out of discovery here +from ...tests import host + +f32 = np.float32 + + +def c_int(v) -> int: + """C (int) cast: truncation toward zero.""" + return int(np.trunc(float(v))) + + +# --- constants mirrored from the C (opendbc/safety/modes/ford.h, opendbc/bluepilot_lateral/safety/*.h, lateral.h) --- +CURVATURE_TO_CAN = 50000.0 # FORD_LIMITS .curvature_to_can +FORD_CURVATURE_MAX = 0.02 # rad/m, FORD_CURVATURE_MAX in ford_declarations.h +MAX_CURVATURE_CAN = c_int(f32(FORD_CURVATURE_MAX) * f32(CURVATURE_TO_CAN)) # FORD_LIMITS .max_curvature, 1000 +MAX_CURVATURE_ERROR_CAN = 100 # FORD_LIMITS(_, 100) +MAX_CURVATURE_ERROR_CAN_PINION = 150 # FORD_BP_STEERING_LIMITS_PINION / FORD_CANFD_STEERING_LIMITS_PINION +CURVATURE_ERROR_MIN_SPEED = 10.0 # m/s +VEHICLE_SPEED_FACTOR = 1000.0 +RATE_LOOKUP_X = (5., 16., 25.) # m/s, FORD_LIMITS curvature_rate_{up,down}_lookup +RATE_LOOKUP_UP_Y = (0.0025, 0.0014, 0.00018) # rad/m per frame, curvature_rate_up_lookup +RATE_LOOKUP_DOWN_Y = (0.002525, 0.001414, 0.0001818) # curvature_rate_down_lookup: 1% looser than up (#20) +ISO_LATERAL_ACCEL = 3.0 +EARTH_G = 9.81 +AVERAGE_ROAD_ROLL = 0.06 +BP_MAX_LATERAL_ACCEL = float(f32(ISO_LATERAL_ACCEL) - f32(EARTH_G) * f32(AVERAGE_ROAD_ROLL)) # lateral.h, ~2.41 m/s^2 + +PATH_ANGLE_TO_CAN = 2000.0 # FORD_PATH_ANGLE_LIMITS .angle_deg_to_can (1 / 5e-4 rad) +FORD_PATH_ANGLE_MAX = 0.25 # rad, curvature mode +FORD_DBC_PATH_ANGLE_MIN = -0.5 # rad, angle mode: the DBC's own range +FORD_DBC_PATH_ANGLE_MAX = 0.5235 +PATH_ANGLE_MAX_CAN = c_int(f32(FORD_PATH_ANGLE_MAX) * f32(PATH_ANGLE_TO_CAN)) # 500 +PATH_ANGLE_DBC_MIN_CAN = c_int(f32(FORD_DBC_PATH_ANGLE_MIN) * f32(PATH_ANGLE_TO_CAN)) # -1000 +PATH_ANGLE_DBC_MAX_CAN = c_int(f32(FORD_DBC_PATH_ANGLE_MAX) * f32(PATH_ANGLE_TO_CAN)) # 1047, truncated like the C's (int) cast +PATH_ANGLE_LOOKUP_X = (10., 15., 25.) +PATH_ANGLE_LOOKUP_Y = (0.0561, 0.04335, 0.00918) # rad per frame + +PATH_OFFSET_TO_CAN = 100.0 # FORD_PATH_OFFSET_LIMITS .angle_deg_to_can (1 / 0.01 m) +FORD_PATH_OFFSET_MAX = 1.0 # m +PATH_OFFSET_MAX_CAN = c_int(f32(FORD_PATH_OFFSET_MAX) * f32(PATH_OFFSET_TO_CAN)) # 100 +PATH_OFFSET_LOOKUP_X = (5., 15., 25.) +PATH_OFFSET_LOOKUP_Y = (0.05, 0.025, 0.01) # m per frame + +CURVATURE_RATE_TO_CAN_CANFD = 1000000.0 # LatCtlCrv_NoRate2_Actl wire resolution, 1e-6 1/m^2 +CURVATURE_RATE_TO_CAN_CAN = 4000000.0 # LatCtlCurv_NoRate_Actl wire resolution, 2.5e-7 1/m^2 + +LATCTL_CURVATURE_SIGNAL_MAX = 0.02094 # rad/m, LatCtlCurv_No_Actl range [-0.02|0.02094] in the DBC +CURVATURE_SIGNAL_MAX_CAN = c_int(f32(LATCTL_CURVATURE_SIGNAL_MAX) * f32(CURVATURE_TO_CAN)) # 1047: the packer cannot go further +FORMER_LATCH_WINDOW_FRAMES = 70 # > the 60 frames the removed reset-bypass latch covered (#9) +SMALL_ANGLE = 0.01 # rad: a small non-zero path_angle; probes hold it constant, so its ROC never decides + + +def c_interpolate(xs, ys, x) -> np.float32: + """safety_interpolate() from opendbc/safety/helpers.h, in float32.""" + x = f32(x) + xs = [f32(v) for v in xs] + ys = [f32(v) for v in ys] + ret = ys[-1] + if x <= xs[0]: + ret = ys[0] + else: + for i in range(len(xs) - 1): + if x < xs[i + 1]: + x0, y0 = xs[i], ys[i] + dx = f32(max(xs[i + 1] - x0, f32(0.0001))) + dy = f32(ys[i + 1] - y0) + ret = f32(f32(dy * f32(x - x0)) / dx) + y0 + break + return f32(ret) + + +def c_fudged_speed(speed_ms: float, offset: float) -> np.float32: + """(vehicle_speed.x / VEHICLE_SPEED_FACTOR) + offset, as the C computes it: the sample is an + int in mm/s, the division and the offset happen in double, and the result is rounded to + float once when passed to safety_interpolate. speed_ms comes from libsafety's getters, + which are the sample / 1000 rounded to float32; the integer is recovered exactly.""" + sample = round(float(speed_ms) * VEHICLE_SPEED_FACTOR) + return f32(sample / VEHICLE_SPEED_FACTOR + offset) + + +class BPFordLateralModel: + """Mirror of the BP curvature path in ford_bp_curvature_cmd_checks + ford_bp_curvature_rate_lookup_check. + + Speeds are what the C sees: vehicle_speed.min / max / values[0] in m/s (read back from + libsafety after the rx messages so packer rounding is included). + """ + + def __init__(self, canfd: bool, max_curvature_error: int = MAX_CURVATURE_ERROR_CAN): + self.limit_lateral_accel = canfd # FORD_LIMITS(true, …) only for CAN FD + self.max_curvature_error = max_curvature_error + + @staticmethod + def rate_delta(speed_min: float, up: bool = True) -> int: + # (safety_interpolate(lookup, (vehicle_speed.min / F) - 1.) * curvature_to_can) + 1. -> int + fudged = c_fudged_speed(speed_min, -1.0) + table = RATE_LOOKUP_UP_Y if up else RATE_LOOKUP_DOWN_Y + return c_int(float(f32(c_interpolate(RATE_LOOKUP_X, table, fudged) * f32(CURVATURE_TO_CAN))) + 1.0) + + @staticmethod + def rate_delta_relaxed(speed_max: float, up: bool = True) -> int: + fudged = c_fudged_speed(speed_max, 1.0) + table = RATE_LOOKUP_UP_Y if up else RATE_LOOKUP_DOWN_Y + return c_int(float(f32(c_interpolate(RATE_LOOKUP_X, table, fudged) * f32(CURVATURE_TO_CAN))) - 1.0) + + @staticmethod + def accel_cap(speed_min: float) -> int: + # BP_MAX_LATERAL_ACCEL / (fudged_speed^2) * curvature_to_can + 1, fudged = max(v.min - 1, 1) + fudged = f32(max(float(c_fudged_speed(speed_min, -1.0)), 1.0)) + max_curv = f32(f32(BP_MAX_LATERAL_ACCEL) / f32(fudged * fudged)) + return c_int(float(f32(max_curv * f32(CURVATURE_TO_CAN))) + 1.0) + + def bounds(self, desired_last: int, meas_min: int, meas_max: int, + speed_min: float, speed_max: float, speed_now: float) -> tuple[int, int]: + """(lowest, highest) allowed desired curvature in CAN units for the next frame.""" + delta_up = self.rate_delta(speed_min, up=True) + delta_down = self.rate_delta(speed_min, up=False) + highest = desired_last + (delta_up if desired_last > 0 else delta_down) + lowest = desired_last - (delta_down if desired_last >= 0 else delta_up) + if self.max_curvature_error and speed_now > CURVATURE_ERROR_MIN_SPEED: + relaxed_up = self.rate_delta_relaxed(speed_max, up=True) + relaxed_down = self.rate_delta_relaxed(speed_max, up=False) + lowest_err = meas_min - self.max_curvature_error - 1 + highest_err = meas_max + self.max_curvature_error + 1 + if desired_last > highest_err: + delta = relaxed_down if desired_last >= 0 else relaxed_up + highest = max(desired_last - delta, highest_err) + elif desired_last < lowest_err: + delta = relaxed_down if desired_last <= 0 else relaxed_up + lowest = min(desired_last + delta, lowest_err) + else: + highest = min(highest, highest_err) + lowest = max(lowest, lowest_err) + lowest = max(-MAX_CURVATURE_CAN, min(MAX_CURVATURE_CAN, lowest)) + highest = max(-MAX_CURVATURE_CAN, min(MAX_CURVATURE_CAN, highest)) + return lowest, highest + + def cap(self, speed_min: float) -> int: + """The largest |curvature| the checks accept at this speed: the lateral-accel cap on CAN FD, the + 0.02 hard limit otherwise (FORD_LIMITS .limit_lateral_acceleration).""" + return min(self.accel_cap(speed_min), MAX_CURVATURE_CAN) if self.limit_lateral_accel else MAX_CURVATURE_CAN + + def allowed(self, desired: int, desired_last: int, meas_min: int, meas_max: int, + speed_min: float, speed_max: float, speed_now: float) -> bool: + """Whether a non-zero desired curvature passes the BP curvature checks (controls allowed, steer enabled).""" + if abs(desired) > MAX_CURVATURE_CAN: + return False + lowest, highest = self.bounds(desired_last, meas_min, meas_max, speed_min, speed_max, speed_now) + if desired > highest or desired < lowest: + return False + if self.limit_lateral_accel: + cap = self.accel_cap(speed_min) + if desired > cap or desired < -cap: + return False + return True + + +_NUM = r"[-+0-9.eE]+" +_GEOMETRY_ROW = re.compile(rf"\{{\.slip_factor = (?P{_NUM})f, \.steer_ratio = (?P{_NUM})f, \.wheelbase = (?P{_NUM})f\}},\s*// (?P\d+):") + + +def pinion_geometry_table() -> dict[int, tuple[float, float, float]]: + """ford_pinion_geometry from opendbc/bluepilot_lateral/safety/ford.h as {index: (slip_factor, steer_ratio, + wheelbase)}. Read from source: libsafety exposes no getters for it and adding some would touch + upstream's harness.""" + bp = Path(__file__).resolve().parents[1] + src = (bp / "ford.h").read_text() + body = src[src.index("ford_pinion_geometry[FORD_PINION_GEOMETRY_ROWS] = {"):] + body = body[:body.index("};")] + matches = list(_GEOMETRY_ROW.finditer(body)) + # every initializer must parse (a row in a spelling the regex misses would otherwise vanish), + # the comment index must be the row's position, and the count must be the declared size + assert len(matches) == body.count("{.slip_factor"), "a geometry row did not parse" + rows = {} + for pos, m in enumerate(matches): + assert int(m["idx"]) == pos, f"row {pos} is labeled {m['idx']}" + rows[pos] = (float(m["slip"]), float(m["sr"]), float(m["wb"])) + declared = re.search(r"#define FORD_PINION_GEOMETRY_ROWS (\d+)U", (bp / "ford_declarations.h").read_text()) + assert declared and int(declared[1]) == len(rows), "FORD_PINION_GEOMETRY_ROWS does not match the table" + return rows + + +def angle_roc_delta(xs, ys, scale: float, speed_min: float) -> int: + """path_angle / path_offset per-frame ROC: interpolate(lookup, v.min - 1) * scale + 1.""" + fudged = c_fudged_speed(speed_min, -1.0) + return c_int(float(f32(c_interpolate(xs, ys, fudged) * f32(scale))) + 1.0) + + +class BPFordTestCase(unittest.TestCase): + """Base for BluePilot Ford lateral tests. Subclasses set CANFD and may set PARAM_SP.""" + + CANFD = True + PARAM_SP = FordSafetyFlagsSP.BP_LATERAL + + def setUp(self): + cls = test_ford.TestFordCANFDStockSafety if self.CANFD else test_ford.TestFordLongitudinalSafety + self.stock = cls("test_steer_allowed") # upstream's Ford TestCase, for its message builders only + self.stock.packer = CANPackerSafety("ford_lincoln_base_pt") + self.stock.safety = libsafety_py.libsafety + self.safety = self.stock.safety + self.reinit() + self.safety.init_tests() + self.safety.set_controls_allowed(True) + self.model = BPFordLateralModel(self.CANFD) + self.set_meas(0, 15.0) + + def tearDown(self): + # stock classes in the same process must not inherit the bit + host.set_safety_param(self.safety, 0) + super().tearDown() # the host's own resets (IQ.Pilot's AOL state) + + def reinit(self): + """A safety-mode init with the BP bit set (ford_init reads the bit).""" + host.set_safety_param(self.safety, self.PARAM_SP) + self.safety.set_safety_hooks(CarParams.SafetyModel.ford, host.ford_param(FordSafetyFlags.CANFD if self.CANFD else 0, self.PARAM_SP)) + + # --- wrappers over upstream's builders --- + def tx(self, msg) -> bool: + return self.stock._tx(msg) + + def rx(self, msg) -> bool: + return self.stock._rx(msg) + + def lat(self, enabled: bool, path_offset=0.0, path_angle=0.0, curvature=0.0, curvature_rate=0.0, increment_timer=True): + return self.stock._lat_ctl_msg(enabled, path_offset, path_angle, curvature, curvature_rate, increment_timer=increment_timer) + + def set_meas(self, curvature: float, speed: float): + self.stock._reset_curvature_measurement(curvature, speed) + + def set_prev_curvature_can(self, can: int): + self.safety.set_desired_curvature_last(can) + + def speeds(self) -> tuple[float, float]: + return float(self.safety.get_vehicle_speed_min()), float(self.safety.get_vehicle_speed_max()) + + def meas(self) -> tuple[int, int]: + return int(self.safety.get_curvature_meas_min()), int(self.safety.get_curvature_meas_max()) + + def lka_bp_status_msg(self, angle_mode_engaged: bool, shadow_curvature: float): + """Lane_Assist_Data1 with BluePilot's angle-mode side channel (can/LANE_ASSIST_DATA1.md), built by + the real packer, so every angle-mode test runs Python's packing through the panda's decode.""" + addr, dat, bus = fordcan_ext.create_lka_msg(self.stock.packer, CanBus(fingerprint={}), False, None, angle_mode_engaged, + shadow_curvature) + return libsafety_py.make_CANPacket(addr, bus, bytes(dat)) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot.py new file mode 100644 index 0000000000..51e2defcb4 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +""" +BluePilot: the BP_LATERAL safety-param bit selects which Ford lateral checks the panda runs. + +This pins the switch itself, not the BluePilot limits: with the bit set a modest non-zero +path_angle is accepted, and with it clear (upstream's stock checks) the same frame is rejected. +The BluePilot limits are covered separately. +""" +import unittest + +from ....car.structs import CarParams +from ....car.ford.values import FordSafetyFlags +from ...core.values_ext import FordSafetyFlagsSP +from ....safety.tests.libsafety import libsafety_py +from ....safety.tests.common import CANPackerSafety +from ....safety.tests import test_ford # module import: keeps its test classes out of this module's discovery +from ...tests import host + +SPEED = 15.0 # m/s, above the curvature-error gate, inside the path_angle ROC table +SMALL_PATH_ANGLE = 0.01 # rad, well inside the per-call ROC at this speed (~0.043) +LARGE_PATH_ANGLE = 0.30 # rad, outside both the ROC step and the ±0.25 curvature-mode range + + +class TestFordBPLateralBit(unittest.TestCase): + """Uses upstream's CAN FD stock test class only for its message helpers; its tests are not inherited.""" + + def setUp(self): + self.h = test_ford.TestFordCANFDStockSafety("test_steer_allowed") + self.h.packer = CANPackerSafety("ford_lincoln_base_pt") + self.h.safety = libsafety_py.libsafety + + def tearDown(self): + host.set_safety_param(self.h.safety, 0) + super().tearDown() + + def _init(self, param_sp: int): + h = self.h + host.set_safety_param(h.safety, param_sp) + h.safety.set_safety_hooks(CarParams.SafetyModel.ford, host.ford_param(FordSafetyFlags.CANFD, param_sp)) + h.safety.init_tests() + h.safety.set_controls_allowed(True) + h._reset_curvature_measurement(0, SPEED) + h._set_prev_desired_angle(0.001) + + def test_bit_set_runs_bluepilot_checks(self): + self._init(FordSafetyFlagsSP.BP_LATERAL) + self.assertTrue(self.h._tx(self.h._lat_ctl_msg(True, 0, SMALL_PATH_ANGLE, 0.001, 0))) + self.assertFalse(self.h._tx(self.h._lat_ctl_msg(True, 0, LARGE_PATH_ANGLE, 0.001, 0))) + + def test_bit_clear_runs_stock_checks(self): + self._init(0) + # upstream: any non-zero path_angle is a violation + self.assertFalse(self.h._tx(self.h._lat_ctl_msg(True, 0, SMALL_PATH_ANGLE, 0.001, 0))) + self.assertTrue(self.h._tx(self.h._lat_ctl_msg(True, 0, 0, 0.001, 0))) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_angle_mode.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_angle_mode.py new file mode 100644 index 0000000000..aeac172346 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_angle_mode.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +""" +BluePilot Ford lateral, angle mode: the shadow-curvature plausibility check. + +In angle mode the LatCtl curvature signal is held at 0 and the command rides on path_angle. +Lane_Assist_Data1 carries a side channel (angle mode engaged, and the curvature the controller +would have commanded, "shadow curvature"; layout in opendbc/bluepilot_lateral/can/LANE_ASSIST_DATA1.md) +that the panda checks against the yaw-measured curvature with the same error window as curvature mode +(ford_shadow_curvature_error_check, ford.h). Ported from BluePilot's earlier vendored tests. +""" +import unittest + +from .ford_bluepilot_common import ( + BPFordTestCase, SMALL_ANGLE, FORMER_LATCH_WINDOW_FRAMES, CURVATURE_TO_CAN, MAX_CURVATURE_ERROR_CAN, CURVATURE_ERROR_MIN_SPEED, +) + + +class TestFordBPAngleModeCANFD(BPFordTestCase): + CANFD = True + + def _angle_frame(self) -> bool: + return self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0)) + + def test_shadow_curvature_error_window(self): + """Above the speed gate the shadow curvature must be within max_curvature_error of measured; + below it the check is off. Covers ford_shadow_curvature_error_check and its call site.""" + measured_can = 250 # 0.005 rad/m + for speed in (CURVATURE_ERROR_MIN_SPEED - 1, CURVATURE_ERROR_MIN_SPEED + 1, 20.0): + enforced = speed > CURVATURE_ERROR_MIN_SPEED + self.set_meas(measured_can / CURVATURE_TO_CAN, speed) + m_min, m_max = self.meas() + hi_edge = m_max + MAX_CURVATURE_ERROR_CAN + 1 # highest_allowed in the C + lo_edge = m_min - MAX_CURVATURE_ERROR_CAN - 1 + # shadow arrives as int16 at 1e-6 rad/m; the C scales by 0.05 to CAN units and truncates, + # so pick shadow values that land exactly on the CAN-unit boundaries + for shadow_can, expect in ((m_max, True), (hi_edge, True), (hi_edge + 1, not enforced), + (lo_edge, True), (lo_edge - 1, not enforced), (0, not enforced)): + self.tx(self.lka_bp_status_msg(True, shadow_can / CURVATURE_TO_CAN)) + with self.subTest(speed=speed, shadow_can=shadow_can): + self.assertEqual(expect, self._angle_frame()) + + def test_shadow_curvature_has_no_rate_limit(self): + """Only the path_angle ROC applies in angle mode: a large frame-to-frame jump in the shadow + curvature that stays inside the window of a correspondingly moved measurement must pass.""" + speed = CURVATURE_ERROR_MIN_SPEED + 1 + for curvature in (0.015, -0.015, 0.018, -0.018, 0.001, -0.019): + self.set_meas(curvature, speed) + self.tx(self.lka_bp_status_msg(True, curvature)) + with self.subTest(curvature=curvature): + self.assertTrue(self._angle_frame()) + + def test_flag_clear_skips_shadow_check(self): + """Without the angle-mode bit a curvature-0 frame is not checked against the shadow value.""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + self.set_meas(0.005, speed) + self.tx(self.lka_bp_status_msg(False, 0.02)) # would fail the window if it were applied + self.assertTrue(self._angle_frame()) + + def test_curvature_mode_unaffected_by_angle_mode_flag(self): + """A real (non-zero) curvature command is curvature mode by its own content; the flag and the + shadow value must not gate it either way.""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + curvature_can = 500 + self.set_meas(curvature_can / CURVATURE_TO_CAN, speed) + for flag in (True, False): + self.tx(self.lka_bp_status_msg(flag, 0.02)) + self.set_prev_curvature_can(curvature_can) + with self.subTest(flag=flag): + self.assertTrue(self.tx(self.lat(True, 0, 0, curvature_can / CURVATURE_TO_CAN, 0))) + + def test_shadow_out_of_window_blocks_while_enabled(self): + """An out-of-window shadow value blocks the enabled frame; the disabled all-zero frame (mode 0) + is not checked against it.""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + self.set_meas(0.005, speed) + self.tx(self.lka_bp_status_msg(True, 0.02)) + self.assertFalse(self._angle_frame()) + self.assertTrue(self.tx(self.lat(False, 0, 0, 0, 0))) + + def test_mode0_frame_leaves_shadow_check_active(self): + """Angle mode's human-turn override sends mode 0 with every signal zero. The frames after it + are checked: an out-of-window shadow curvature blocks on every frame of the 60-frame window + the removed bypass latch used to cover (#9).""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + self.set_meas(0.005, speed) + self.tx(self.lka_bp_status_msg(True, 0.005)) + self.assertTrue(self.tx(self.lat(False, 0, 0, 0, 0))) + self.tx(self.lka_bp_status_msg(True, 0.02)) + for frame in range(FORMER_LATCH_WINDOW_FRAMES): + with self.subTest(frame=frame): + self.assertFalse(self._angle_frame()) + + +class TestFordBPAngleModeCAN(TestFordBPAngleModeCANFD): + CANFD = False + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_limits.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_limits.py new file mode 100644 index 0000000000..7562be9b9c --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_limits.py @@ -0,0 +1,463 @@ +#!/usr/bin/env python3 +""" +BluePilot Ford lateral: the 4-signal limits with the BP_LATERAL bit set. + +Every expectation comes from ford_bluepilot_common.BPFordLateralModel, a float32 mirror of the +C, so boundaries are asserted at the exact CAN unit on both x86 and ARM. +""" +import random +import unittest + +from ....safety.tests import common +from ....can.dbc import DBC + +from .ford_bluepilot_common import ( + BPFordTestCase, angle_roc_delta, SMALL_ANGLE, FORMER_LATCH_WINDOW_FRAMES, CURVATURE_SIGNAL_MAX_CAN, + CURVATURE_TO_CAN, MAX_CURVATURE_CAN, MAX_CURVATURE_ERROR_CAN, CURVATURE_ERROR_MIN_SPEED, + PATH_ANGLE_TO_CAN, PATH_ANGLE_MAX_CAN, PATH_ANGLE_DBC_MIN_CAN, PATH_ANGLE_DBC_MAX_CAN, + PATH_ANGLE_LOOKUP_X, PATH_ANGLE_LOOKUP_Y, + PATH_OFFSET_TO_CAN, PATH_OFFSET_MAX_CAN, PATH_OFFSET_LOOKUP_X, PATH_OFFSET_LOOKUP_Y, + CURVATURE_RATE_TO_CAN_CANFD, CURVATURE_RATE_TO_CAN_CAN, +) +from ...tests import host + +# speeds chosen to hit both sides of the error-band gate (10 m/s) and every segment of the +# {5, 16, 25} rate table, plus its clamped ends +SPEEDS = (3.0, 5.0, 8.0, 10.5, 12.0, 16.0, 20.0, 25.0, 30.0) + + +class TestFordBPLimitsCANFD(BPFordTestCase): + CANFD = True + + def _probe(self, desired_can: int, last_can: int) -> bool: + self.set_prev_curvature_can(last_can) + return self.tx(self.lat(True, 0, SMALL_ANGLE, desired_can / CURVATURE_TO_CAN, 0)) + + def _assert_matches_model(self, last_can: int, probes): + v_min, v_max = self.speeds() + m_min, m_max = self.meas() + for desired in probes: + if desired == 0 or abs(desired) > CURVATURE_SIGNAL_MAX_CAN: # 0 skips the value checks the model mirrors + continue + expected = self.model.allowed(desired, last_can, m_min, m_max, v_min, v_max, v_max) + with self.subTest(speed=v_max, meas=(m_min, m_max), last=last_can, desired=desired): + self.assertEqual(expected, self._probe(desired, last_can)) + + def test_curvature_rate_lookup_boundaries(self): + """Per-frame delta from the {5,16,25} m/s table (ford_bp_curvature_rate_lookup_check), both + directions, both signs, exact boundary: the speed fudge and the delta arithmetic.""" + for speed in SPEEDS: + self.set_meas(0, speed) + v_min, v_max = self.speeds() + delta = self.model.rate_delta(v_min) + for last in (0, 40, -40, 250, -250): + lowest, highest = self.model.bounds(last, *self.meas(), v_min, v_max, v_max) + self._assert_matches_model(last, (highest - 1, highest, highest + 1, lowest - 1, lowest, lowest + 1, + last + delta, last + delta + 1, last - delta, last - delta - 1)) + + def test_curvature_error_band_convergence(self): + """Above 10 m/s the command must stay within max_curvature_error of measured; when the last + command is already outside the band it must move back toward it by at least the relaxed + delta, and may not move further away. Below 10 m/s the band is off (the relaxed branches of + ford_bp_curvature_rate_lookup_check). Seen in the car: a 4-frame block at 26 mph with the driver + steering harder than the command, sitting exactly on this window's edge (#12).""" + # measured both signs, and last commands on both sides of zero, so both relaxed branches run + # with last commands of either sign. The C picks the up or down table by that sign, and the + # down table is 1% looser (#20), so each relaxed probe below uses the table the C picks. + for meas_can in (200, -200, 600, -600): + for speed in (8.0, 12.0, 16.0, 20.0): + self.set_meas(meas_can / CURVATURE_TO_CAN, speed) + v_min, v_max = self.speeds() + m_min, m_max = self.meas() + band_on = v_max > CURVATURE_ERROR_MIN_SPEED + delta = self.model.rate_delta(v_min) + lowest_err = m_min - MAX_CURVATURE_ERROR_CAN - 1 + highest_err = m_max + MAX_CURVATURE_ERROR_CAN + 1 + cap = self.model.cap(v_min) # the explicit asserts below stay under it + + # last command below the band (on either side of zero): must climb toward it + for last in (lowest_err - 150, 0, -1, 1): + if last >= lowest_err: + continue + relaxed = self.model.rate_delta_relaxed(v_max, up=last > 0) # climbing: down table while last <= 0 + self._assert_matches_model(last, (last + relaxed - 1, last + relaxed, last + delta, last + delta + 1, + last - 1, lowest_err)) + if band_on: + ctx = f"meas={meas_can} speed={speed} last={last} relaxed={relaxed} band=({lowest_err},{highest_err})" + self.assertFalse(self._probe(last + relaxed - 1, last), "must move toward the band " + ctx) + if abs(min(last + relaxed, lowest_err)) <= cap: + self.assertTrue(self._probe(min(last + relaxed, lowest_err), last), ctx) + if last - 1 != 0: # curvature 0 is angle mode's sentinel and skips the curvature checks + self.assertFalse(self._probe(last - 1, last), "may not move away from the band " + ctx) + + # last command above the band (on either side of zero): must descend toward it + for last in (highest_err + 150, 0, -1, 1): + if last <= highest_err: + continue + relaxed = self.model.rate_delta_relaxed(v_max, up=last < 0) # descending: down table while last >= 0 + self._assert_matches_model(last, (last - relaxed + 1, last - relaxed, last - delta, last - delta - 1, + last + 1, highest_err)) + if band_on: + ctx = f"meas={meas_can} speed={speed} last={last} relaxed={relaxed} band=({lowest_err},{highest_err})" + self.assertFalse(self._probe(last - relaxed + 1, last), "must move toward the band " + ctx) + if abs(max(last - relaxed, highest_err)) <= cap: + self.assertTrue(self._probe(max(last - relaxed, highest_err), last), ctx) + if last + 1 != 0: + self.assertFalse(self._probe(last + 1, last), "may not move away from the band " + ctx) + + # last command inside the band: may not leave it + last = highest_err - 1 + self._assert_matches_model(last, (highest_err, highest_err + 1, lowest_err, lowest_err - 1)) + if band_on and delta > 1 and abs(highest_err) <= cap: + self.assertTrue(self._probe(highest_err, last)) + self.assertFalse(self._probe(highest_err + 1, last), "inside the band, may not command outside it") + + def test_lateral_accel_cap(self): + """CAN FD only: |curvature| <= (ISO_LATERAL_ACCEL - g*roll) / v^2, with the speed fudged down 1 m/s.""" + for speed in (15.0, 20.0, 25.0, 30.0): + self.set_meas(0, speed) + v_min, _ = self.speeds() + cap = self.model.accel_cap(v_min) + self.assertLess(cap, MAX_CURVATURE_CAN, "cap only means something below the 0.02 hard limit") + for sign in (1, -1): + # measured curvature at the cap so the error band and rate limits allow it; last at the cap too + self.set_meas(sign * cap / CURVATURE_TO_CAN, speed) + with self.subTest(speed=speed, sign=sign): + self.assertTrue(self._probe(sign * cap, sign * cap)) + self.assertFalse(self._probe(sign * (cap + 1), sign * cap)) + + def test_max_curvature_hard_limit(self): + """+-0.02 rad/m (1000 CAN) regardless of speed. At 3 m/s the accel cap and the band are off. Only the + positive side has an over-cap frame: LatCtlCurv_No_Actl is 11 bits unsigned with offset -0.02, so its + minimum is exactly -0.02, and -1001 CAN units can't be sent (it wraps to +1047 on the wire).""" + self.set_meas(0.02, 3.0) + for sign in (1, -1): + self.assertTrue(self._probe(sign * MAX_CURVATURE_CAN, sign * MAX_CURVATURE_CAN)) + self.assertFalse(self._probe(MAX_CURVATURE_CAN + 1, MAX_CURVATURE_CAN)) + name = "LateralMotionControl2" if self.CANFD else "LateralMotionControl" + sig = next(m for m in DBC("ford_lincoln_base_pt").msgs.values() if m.name == name).sigs["LatCtlCurv_No_Actl"] + self.assertFalse(sig.is_signed) + self.assertEqual(round(sig.offset * CURVATURE_TO_CAN), -MAX_CURVATURE_CAN) + + def test_over_cap_frame_resets_last(self): + """A frame past the 0.02 cap resets desired_last to 0 (ford_bp_curvature_cmd_checks' own cap), so + the next frame at the cap is a jump from 0 past the rate limit. ford_lmc_checks' range check blocks + the over-cap frame either way; only the cap in ford_bp_curvature_cmd_checks resets. At 5 m/s the + band is off and the CAN FD accel cap is far above 0.02. Positive only: the signal's minimum is + -0.02 exactly (11 bits unsigned, offset -0.02), so -1001 wraps to +1047 on the wire.""" + self.set_meas(0.02, 5.0) + self.assertLess(self.model.rate_delta(self.speeds()[0]), MAX_CURVATURE_CAN) + self.assertFalse(self._probe(MAX_CURVATURE_CAN + 1, MAX_CURVATURE_CAN)) + self.assertFalse(self.tx(self.lat(True, 0, SMALL_ANGLE, MAX_CURVATURE_CAN / CURVATURE_TO_CAN, 0))) + + def test_signals_zero_when_steer_disabled(self): + """With LatCtl_D2_Rq = 0 every signal must be zero (path_angle/path_offset_cmd_checks, the + curvature-rate check and ford_bp_curvature_cmd_checks), each on its own and in combination.""" + self.set_meas(0, 15.0) + self.assertFalse(self.tx(self.lat(False, 0.1, 0, 0, 0))) + self.assertFalse(self.tx(self.lat(False, 0, 0, 0, 0.0001))) + self.assertFalse(self.tx(self.lat(False, 0, 0.01, 0, 0))) + self.assertFalse(self.tx(self.lat(False, 0.1, 0.01, 0, 0))) + self.assertFalse(self.tx(self.lat(False, 0, 0.01, 0, 0.0001))) + self.assertFalse(self.tx(self.lat(False, 0, 0, 0.001, 0))) + self.assertFalse(self.tx(self.lat(False, 0.1, 0, 0.001, 0))) + self.assertFalse(self.tx(self.lat(False, 0, 0, 0.001, 0.0001))) + # one CAN unit of curvature: the curvature-must-be-0 check is the only one that blocks it + for sign in (1, -1): + self.assertFalse(self.tx(self.lat(False, 0, 0, sign / CURVATURE_TO_CAN, 0))) + # the all-zero disabled frame is the one thing allowed + self.assertTrue(self.tx(self.lat(False, 0, 0, 0, 0))) + + def test_controls_not_allowed_blocks_steering(self): + """steer enabled with controls not allowed is blocked whatever the signals carry: a real + curvature command (ford_bp_curvature_cmd_checks), in curvature mode and with the angle-mode + flag set. Upstream's test_steer_allowed asserts this for the stock path; this is the BP path.""" + self.set_meas(0, 15.0) + for angle_mode in (False, True): + self.tx(self.lka_bp_status_msg(angle_mode, 0.0)) + self.safety.set_controls_allowed(True) + self.assertTrue(self._probe(20, 10)) + self.safety.set_controls_allowed(False) + host.set_controls_allowed_lat(self.safety, False) + with self.subTest(angle_mode=angle_mode): + self.assertFalse(self._probe(20, 10)) + self.assertFalse(self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0))) + self.safety.set_controls_allowed(True) + self.tx(self.lka_bp_status_msg(False, 0.0)) + + def test_newest_speed_sample_gates_the_band(self): + """The error band switches on vehicle_speed.values[0], the newest sample, not the window's + min or max, and only strictly above 10 m/s. Six samples at 9 m/s then one at 11: band on; + the reverse: band off; a newest sample of exactly 10.0: band off.""" + meas_can = 200 + last = meas_can + MAX_CURVATURE_ERROR_CAN # inside the band, so only the band decides + below, above, gate = CURVATURE_ERROR_MIN_SPEED - 1, CURVATURE_ERROR_MIN_SPEED + 1, CURVATURE_ERROR_MIN_SPEED + # outside the band (its edge is meas + band + 1), inside the rate delta from 'last' at the slowest window + out_of_band = last + self.model.rate_delta(below) // 2 + self.assertGreater(out_of_band, meas_can + MAX_CURVATURE_ERROR_CAN + 1) + for window, newest, expect_blocked in ((below, above, True), (above, below, False), (above, gate, False), (below, gate, False)): + self.set_meas(meas_can / CURVATURE_TO_CAN, window) + self.rx(self.stock._speed_msg(newest)) + v_min, v_max = self.speeds() + m_min, m_max = self.meas() + with self.subTest(window=window, newest=newest): + self.assertEqual(not expect_blocked, self.model.allowed(out_of_band, last, m_min, m_max, v_min, v_max, newest)) + self.assertEqual(not expect_blocked, self._probe(out_of_band, last)) + + def test_newest_speed_sample_gates_the_shadow_check(self): + """Angle mode's shadow-curvature check (ford_shadow_curvature_error_check) also switches on + vehicle_speed.values[0], the newest sample: older samples at 9.5 m/s and the newest at 10.5: on; + the reverse: off. 0.5 m/s either side keeps clear of speed_mismatch_check's 2 m/s limit.""" + below, above = CURVATURE_ERROR_MIN_SPEED - 0.5, CURVATURE_ERROR_MIN_SPEED + 0.5 + for window, newest, expect_blocked in ((below, above, True), (above, below, False)): + self.set_meas(0, window) + self.rx(self.stock._speed_msg(newest)) + _, m_max = self.meas() + out_of_band = m_max + MAX_CURVATURE_ERROR_CAN + 2 # one past highest_allowed in the C + self.tx(self.lka_bp_status_msg(True, out_of_band / CURVATURE_TO_CAN)) + with self.subTest(window=window, newest=newest): + self.assertEqual(not expect_blocked, self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0))) + self.tx(self.lka_bp_status_msg(False, 0.0)) + + def test_zero_curvature_skips_curvature_checks(self): + """Curvature == 0 (angle mode's sentinel) skips the curvature value checks (cap, rate tables, + error band); the controls-allowed gate and the message-rate check remain + (ford_bp_curvature_cmd_checks). Pins current behavior, see #10 and #60.""" + self.set_meas(0.01, 15.0) # measured 500 CAN: a real curvature command of 0 would be far outside the band + self.assertTrue(self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0))) + self.safety.set_controls_allowed(False) + host.set_controls_allowed_lat(self.safety, False) + self.assertFalse(self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0))) + self.safety.set_controls_allowed(True) + + # --- the reset frame: curvature 0 and path_angle 0 (#9) --- + + def test_reset_frame_accepted_mid_curve(self): + """Curvature mode's human-turn reset: steering enabled, curvature 0 and path_angle 0 while the + car is mid-curve. Curvature 0 skips the error band, so the frame is accepted; the + controller depends on this.""" + self.set_meas(0.01, 20.0) # measured 500 CAN: a real curvature command of 0 would be far outside the band + self.set_prev_curvature_can(500) + self.assertTrue(self.tx(self.lat(True, 0, 0, 0, 0))) + + def test_reset_frame_then_jump_blocked(self): + """The repro in #9, steps A-C: at 25 m/s a jump from 0.001 to 0.02 rad/m is blocked (A); a + curvature 0 / path_angle 0 frame is accepted (B); the same jump right after it is still + blocked (C). With the latch, C passed.""" + self.set_meas(0, 25.0) + jump, last = 0.02, int(0.001 * CURVATURE_TO_CAN) + self.set_prev_curvature_can(last) + self.assertFalse(self.tx(self.lat(True, 0, 0, jump, 0))) # A + self.assertTrue(self.tx(self.lat(True, 0, 0, 0, 0))) # B + self.set_prev_curvature_can(last) + self.assertFalse(self.tx(self.lat(True, 0, 0, jump, 0))) # C + + def _walk(self, reset_frame) -> list[bool]: + """Re-init, send reset_frame (if any), then a seeded random walk of curvature, path_angle and + path_offset steps up to 1.3 ROC steps each, so some frames pass and some are blocked by the + rate limits and the error band. Returns each frame's tx result. curvature_rate stays 0: its ROC + check can never fire (#22), so varying it would show nothing.""" + self.reinit() + self.safety.set_controls_allowed(True) + self.set_meas(0, 12.0) # band on around 0 + if reset_frame is not None: + self.assertTrue(self.tx(reset_frame)) + v_min, _ = self.speeds() + steps = (self.model.rate_delta(v_min), + angle_roc_delta(PATH_ANGLE_LOOKUP_X, PATH_ANGLE_LOOKUP_Y, PATH_ANGLE_TO_CAN, v_min), + angle_roc_delta(PATH_OFFSET_LOOKUP_X, PATH_OFFSET_LOOKUP_Y, PATH_OFFSET_TO_CAN, v_min)) + bounds = (150, 300, 80) # CAN units: past the band (101), inside the path_angle / path_offset ranges + rng = random.Random(9) + pos = [self.TINY_CURVATURE_CAN, 0, 0] + results = [] + for _ in range(FORMER_LATCH_WINDOW_FRAMES): + pos = [max(-b, min(b, p + round(rng.uniform(-1.3, 1.3) * s))) for p, s, b in zip(pos, steps, bounds, strict=True)] + pos[0] = pos[0] or self.TINY_CURVATURE_CAN # curvature 0 would skip the value checks + curvature, angle, offset = pos + results.append(self.tx(self.lat(True, offset / PATH_OFFSET_TO_CAN, angle / PATH_ANGLE_TO_CAN, curvature / CURVATURE_TO_CAN, 0))) + return results + + def test_reset_frame_leaves_no_state(self): + """After a reset frame, enabled (curvature mode's human-turn reset) or disabled (angle mode's + mode 0), every check gives the same result, frame for frame, as without it, across the whole + window the removed bypass latch used to cover (#9).""" + baseline = self._walk(None) + self.assertTrue(any(baseline)) # some frames pass + self.assertFalse(all(baseline)) # and some are blocked + for enabled in (True, False): + with self.subTest(enabled=enabled): + self.assertEqual(baseline, self._walk(self.lat(enabled, 0, 0, 0, 0))) + + def test_init_resets_bp_state(self): + """ford_bp_init (from ford_init) clears the BP path_angle / path_offset last values and the angle-mode flag, the + way set_safety_hooks clears upstream's lateral state: after a re-init the first frame is + checked against 0, not against the previous session. (The curvature_rate last value is reset + too, but its ROC can never fire, so that is not observable; see #22.)""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + for stale in ("path_angle", "path_offset", "angle_mode"): + with self.subTest(stale=stale): + self.set_meas(0, speed) + v_min, _ = self.speeds() + angle_delta = angle_roc_delta(PATH_ANGLE_LOOKUP_X, PATH_ANGLE_LOOKUP_Y, PATH_ANGLE_TO_CAN, v_min) + offset_delta = angle_roc_delta(PATH_OFFSET_LOOKUP_X, PATH_OFFSET_LOOKUP_Y, PATH_OFFSET_TO_CAN, v_min) + # leave the one piece of state 3 ROC steps away from 0 (or angle mode engaged, with a + # shadow value that fails the window), then re-init + for step in (1, 2, 3): + angle = step * angle_delta if stale == "path_angle" else 0 + offset = step * offset_delta if stale == "path_offset" else 0 + self.set_prev_curvature_can(self.TINY_CURVATURE_CAN) + self.assertTrue(self.tx(self.lat(True, offset / PATH_OFFSET_TO_CAN, angle / PATH_ANGLE_TO_CAN, + self.TINY_CURVATURE_CAN / CURVATURE_TO_CAN, 0))) + if stale == "angle_mode": + self.tx(self.lka_bp_status_msg(True, 0.02)) + self.reinit() + self.safety.set_controls_allowed(True) + self.set_meas(0.005 if stale == "angle_mode" else 0, speed) + self.set_prev_curvature_can(self.TINY_CURVATURE_CAN) + if stale == "angle_mode": + # the flag is cleared, so a curvature-0 frame is not checked against the shadow value + # (ford_bp_init leaves the raw shadow value alone; the flag gates its use) + self.assertTrue(self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0))) + else: + # one full ROC step below 0: within the ROC only if the last value is exactly 0 (from the + # stale value it is 4 steps, from 1 it is one unit too many) + angle = -angle_delta if stale == "path_angle" else 0 + offset = -offset_delta if stale == "path_offset" else 0 + self.assertTrue(self.tx(self.lat(True, offset / PATH_OFFSET_TO_CAN, angle / PATH_ANGLE_TO_CAN, + self.TINY_CURVATURE_CAN / CURVATURE_TO_CAN, 0))) + + # --- the other three signals --- + + # the three non-curvature probes carry 1 CAN unit of curvature (measured 0, last 1), which + # passes the curvature checks, so only the signal under test decides + TINY_CURVATURE_CAN = 1 + + def _angle_probe(self, angle_can: int) -> bool: + self.set_prev_curvature_can(self.TINY_CURVATURE_CAN) + return self.tx(self.lat(True, 0, angle_can / PATH_ANGLE_TO_CAN, self.TINY_CURVATURE_CAN / CURVATURE_TO_CAN, 0)) + + def test_path_angle_range_and_roc(self): + """Per-frame ROC from the {10,15,25} table (path_angle_cmd_checks) and the +-0.25 rad range in curvature + mode. A frame always becomes the new 'last' even when blocked, so each probe re-baselines.""" + for speed in (8.0, 12.0, 20.0, 25.0): + self.set_meas(0, speed) + v_min, _ = self.speeds() + delta = angle_roc_delta(PATH_ANGLE_LOOKUP_X, PATH_ANGLE_LOOKUP_Y, PATH_ANGLE_TO_CAN, v_min) + for base in (0, 200, -200): + for sign in (1, -1): + with self.subTest(speed=speed, base=base, sign=sign): + self._angle_probe(base) + self.assertTrue(self._angle_probe(base + sign * delta)) + self._angle_probe(base) + self.assertFalse(self._angle_probe(base + sign * (delta + 1))) + # range: step from just inside so the ROC allows it + if delta > 2: + for sign in (1, -1): + self._angle_probe(sign * (PATH_ANGLE_MAX_CAN - 1)) + self.assertTrue(self._angle_probe(sign * PATH_ANGLE_MAX_CAN)) + self.assertFalse(self._angle_probe(sign * (PATH_ANGLE_MAX_CAN + 1))) + + def test_path_angle_dbc_range_in_angle_mode(self): + """With angle mode engaged (Lane_Assist_Data1 bit) the range widens to the DBC's [-0.5, 0.5235].""" + self.set_meas(0, 8.0) # below the shadow-check speed gate so only the range matters + self.tx(self.lka_bp_status_msg(True, 0.0)) + v_min, _ = self.speeds() + delta = angle_roc_delta(PATH_ANGLE_LOOKUP_X, PATH_ANGLE_LOOKUP_Y, PATH_ANGLE_TO_CAN, v_min) + self.assertGreater(delta, 60) + for target, over in ((PATH_ANGLE_DBC_MAX_CAN, PATH_ANGLE_DBC_MAX_CAN + 1), (PATH_ANGLE_DBC_MIN_CAN, PATH_ANGLE_DBC_MIN_CAN - 1)): + # walk up in ROC-sized steps (the signal can pack these values) + pos = 0 + while abs(target - pos) > delta: + pos += delta if target > pos else -delta + self._angle_probe(pos) + self.assertTrue(self._angle_probe(target)) + self.assertFalse(self._angle_probe(over)) + self._angle_probe(pos) + self.tx(self.lka_bp_status_msg(False, 0.0)) + + def _offset_probe(self, offset_can: int) -> bool: + self.set_prev_curvature_can(self.TINY_CURVATURE_CAN) + return self.tx(self.lat(True, offset_can / PATH_OFFSET_TO_CAN, SMALL_ANGLE, self.TINY_CURVATURE_CAN / CURVATURE_TO_CAN, 0)) + + def test_path_offset_range_and_roc(self): + """Per-frame ROC from the {5,15,25} table (path_offset_cmd_checks) and the +-1.0 m range. + At 9 m/s the speed fudge's sign decides the limit (5 with v - 1, 4 with v + 1).""" + for speed in (3.0, 8.0, 9.0, 12.0, 20.0, 25.0): + self.set_meas(0, speed) + v_min, _ = self.speeds() + delta = angle_roc_delta(PATH_OFFSET_LOOKUP_X, PATH_OFFSET_LOOKUP_Y, PATH_OFFSET_TO_CAN, v_min) + for base in (0, 50, -50): + for sign in (1, -1): + with self.subTest(speed=speed, base=base, sign=sign): + self._offset_probe(base) + self.assertTrue(self._offset_probe(base + sign * delta)) + self._offset_probe(base) + self.assertFalse(self._offset_probe(base + sign * (delta + 1))) + for sign in (1, -1): + self._offset_probe(sign * (PATH_OFFSET_MAX_CAN - 1)) + self.assertTrue(self._offset_probe(sign * PATH_OFFSET_MAX_CAN)) + self.assertFalse(self._offset_probe(sign * (PATH_OFFSET_MAX_CAN + 1))) + + RATE_TO_CAN = CURVATURE_RATE_TO_CAN_CANFD + + def _rate_probe(self, rate_can: int) -> bool: + self.set_prev_curvature_can(self.TINY_CURVATURE_CAN) + return self.tx(self.lat(True, 0, SMALL_ANGLE, self.TINY_CURVATURE_CAN / CURVATURE_TO_CAN, rate_can / self.RATE_TO_CAN)) + + def test_curvature_rate_must_be_zero(self): + """Curvature rate must be 0, the rule stock Ford applies (#22): one wire unit either way is + blocked at any speed, steer enabled or disabled. The disabled frame carries only the rate, so + the rate check is the one deciding (the all-zero disabled frame passes).""" + for speed in (3.0, 12.0, 30.0): + self.set_meas(0, speed) + for rate_can in (1, -1, 1023, -1024): + with self.subTest(speed=speed, rate_can=rate_can): + self.assertTrue(self._rate_probe(0)) + self.assertFalse(self._rate_probe(rate_can)) + # disabled frames last: their path_angle of 0 would make the next enabled probe's path_angle step decide + for speed in (3.0, 12.0, 30.0): + self.set_meas(0, speed) + for rate_can in (1, -1, 1023, -1024): + with self.subTest(speed=speed, rate_can=rate_can, enabled=False): + self.assertTrue(self.tx(self.lat(False, 0, 0, 0, 0))) + self.assertFalse(self.tx(self.lat(False, 0, 0, 0, rate_can / self.RATE_TO_CAN))) + + def test_curvature_zero_frames_are_rate_limited(self): + """#24: angle mode sends curvature 0 on every frame. Those frames are held to the same message-rate + window as the rest (upstream's rolling 250 ms window, split in two halves): sent faster than 20 Hz, + every frame past the limit is blocked until the window rolls over. Path angle carries the command, + so these frames never arm the reset latch.""" + self.set_meas(0, 15.0) + max_rt_msgs = int(20 * common.RT_INTERVAL / 1e6 * 1.2 + 1) + half = common.RT_INTERVAL // 2 + + def send(): + return self.tx(self.lat(True, 0, SMALL_ANGLE, 0, 0, increment_timer=False)) + + self.safety.set_timer(0) + for i in range(max_rt_msgs * 2): + self.assertEqual(i <= max_rt_msgs, send(), i) + self.safety.set_timer(half) + self.assertFalse(send()) # the overflow moves into the previous half + self.safety.set_timer(half + common.RT_INTERVAL) + self.assertFalse(send()) + self.safety.set_timer(half + 2 * common.RT_INTERVAL) + for _ in range(max_rt_msgs): + self.assertTrue(send()) + +class TestFordBPLimitsCAN(TestFordBPLimitsCANFD): + """Same checks on the CAN (non-FD) LateralMotionControl message: no lateral-accel cap, and the + curvature-rate signal has 4x the wire resolution.""" + CANFD = False + RATE_TO_CAN = CURVATURE_RATE_TO_CAN_CAN + + def test_lateral_accel_cap(self): + """CAN: no cap. A curvature well above the CAN FD cap is accepted up to the 0.02 hard limit.""" + self.set_meas(MAX_CURVATURE_CAN / CURVATURE_TO_CAN, 25.0) + v_min, _ = self.speeds() + self.assertLess(self.model.accel_cap(v_min), MAX_CURVATURE_CAN) # the FD cap would bite here + self.assertTrue(self._probe(MAX_CURVATURE_CAN, MAX_CURVATURE_CAN)) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_matrix.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_matrix.py new file mode 100644 index 0000000000..fa7416e790 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_matrix.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +""" +BluePilot Ford lateral: upstream's whole Ford safety matrix with the BP_LATERAL bit set. + +Subclasses the three stock test classes so relay-malfunction, forwarding, button, longitudinal, +rx-hook and every other upstream test runs against the BluePilot tx hook. Only the tests whose +stock assertions the 4-signal lateral changes on purpose are replaced: + + * test_steer_allowed, test_curvature_rate_limits: assert upstream's single-signal limits; the + BluePilot equivalents are in test_ford_bluepilot_limits.py (every signal, exact boundaries). + * test_max_lateral_acceleration: BluePilot caps at ISO_LATERAL_ACCEL - g*roll (~2.41 m/s^2) + with a speed fudge, not upstream's ~3.6 m/s^2; CAN FD only. + * test_rt_limits runs unchanged: it sends curvature-0 frames, which still get the message-rate + check in ford_bp_curvature_cmd_checks (#24, #60). test_rt_limits_nonzero_curvature pins it for + curvature mode. +""" +import unittest + +from ....safety.tests import common +from ...core.values_ext import FordSafetyFlagsSP +from ....safety.tests import test_ford +from ....safety.tests.libsafety import libsafety_py +from .ford_bluepilot_common import ( + BPFordLateralModel, CURVATURE_TO_CAN, MAX_CURVATURE_ERROR_CAN, CURVATURE_SIGNAL_MAX_CAN, +) +from ...tests import host + +SKIP_REASON = "BluePilot 4-signal limits are asserted in test_ford_bluepilot_limits.py" + + +class BPLateralMatrixMixin: + """Set the bit before ford_init runs, clear it after, and swap the host's stock lateral tests + (skipped below, or replaced). + + Class names must start with 'TestFord': common.py's cross-mode TX sweep only skips + Ford-vs-Ford comparisons by that prefix.""" + + CANFD = True + PARAM_SP = int(FordSafetyFlagsSP.BP_LATERAL) + MAX_CURVATURE_ERROR_CAN_BP = MAX_CURVATURE_ERROR_CAN # FORD_LIMITS(_, 100); the pinion classes use MAX_CURVATURE_ERROR_CAN_PINION + + def setUp(self): + self.safety = libsafety_py.libsafety + host.set_safety_param(self.safety, self.PARAM_SP) + # StarPilot: the bits ride the Ford param. Only the upstream bases that define SAFETY_PARAM + # (sunnypilot, StarPilot) need this; IQ.Pilot's iqdbc base doesn't define it, and ford_param + # is a no-op for non-StarPilot hosts anyway. + if hasattr(self, "SAFETY_PARAM"): + self.SAFETY_PARAM = host.ford_param(self.SAFETY_PARAM, self.PARAM_SP) + super().setUp() # stock setUp: packer, set_safety_hooks, init_tests + self.model = BPFordLateralModel(self.CANFD, self.MAX_CURVATURE_ERROR_CAN_BP) + + def tearDown(self): + host.set_safety_param(self.safety, 0) + super().tearDown() # the host's own resets (IQ.Pilot's AOL state) + + @unittest.skip(SKIP_REASON) + def test_steer_allowed(self): + pass + + @unittest.skip(SKIP_REASON) + def test_curvature_rate_limits(self): + pass + + # IQ.Pilot's test_ford: its stock controller (CURVATURE_LIMITS.apply_limits) against the stock checks. + # With BP_LATERAL set the car controller sends the BluePilot commands instead, never these. + @unittest.skip(SKIP_REASON) + def test_controller_limits_pass_safety(self): + pass + + def test_max_lateral_acceleration(self): + """BluePilot's cap (CAN FD only) at ISO_LATERAL_ACCEL - g*roll over the fudged speed squared.""" + for speed in (12.0, 15.0, 20.0, 25.0, 30.0, 35.0): + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(0, speed) + v_min = float(self.safety.get_vehicle_speed_min()) + cap = self.model.cap(v_min) + for sign in (1, -1): + for offset in (-5, -1, 0, 1, 5): + curvature_can = sign * (cap + offset) + if abs(curvature_can) > CURVATURE_SIGNAL_MAX_CAN: + continue + self._reset_curvature_measurement(curvature_can / CURVATURE_TO_CAN, speed) + self._set_prev_desired_angle(curvature_can / CURVATURE_TO_CAN) + should_tx = abs(curvature_can) <= cap + with self.subTest(speed=speed, curvature_can=curvature_can): + self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, curvature_can / CURVATURE_TO_CAN, 0))) + + def test_rt_limits_nonzero_curvature(self): + """Upstream's rolling-window message-rate check, driven with a non-zero curvature (curvature mode).""" + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(0, 0) + curvature = 1 / CURVATURE_TO_CAN + max_rt_msgs = int(self.LATERAL_FREQUENCY * common.RT_INTERVAL / 1e6 * 1.2 + 1) + half = common.RT_INTERVAL // 2 + + def send(): + self._set_prev_desired_angle(curvature) + return self._tx(self._lat_ctl_msg(True, 0, 0, curvature, 0, increment_timer=False)) + + self.safety.set_timer(0) + for i in range(max_rt_msgs * 2): + self.assertEqual(i <= max_rt_msgs, send()) + self.safety.set_timer(half) + self.assertFalse(send()) + self.safety.set_timer(half + 2 * common.RT_INTERVAL // 5) + self.assertFalse(send()) + self.assertFalse(send()) + self.safety.set_timer(half + common.RT_INTERVAL) + self.assertFalse(send()) + self.safety.set_timer(half + 2 * common.RT_INTERVAL) + for _ in range(max_rt_msgs): + self.assertTrue(send()) + + +class TestFordBPCANFDStockSafety(BPLateralMatrixMixin, test_ford.TestFordCANFDStockSafety): + CANFD = True + + +class TestFordBPCANFDLongitudinalSafety(BPLateralMatrixMixin, test_ford.TestFordCANFDLongitudinalSafety): + CANFD = True + + +class TestFordBPLongitudinalSafety(BPLateralMatrixMixin, test_ford.TestFordLongitudinalSafety): + CANFD = False + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_pinion.py b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_pinion.py new file mode 100644 index 0000000000..6e999fd25c --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_pinion.py @@ -0,0 +1,262 @@ +#!/usr/bin/env python3 +""" +BluePilot Ford lateral: the pinion-sourced curvature measurement (STEER_ANGLE_CURVATURE). + +With bit 0 of the SP safety param set (and BP_LATERAL, which it requires), the panda measures +curvature from the PSCM steering pinion angle through the per-platform geometry table in +bluepilot_lateral/safety/ford.h instead of the RCM yaw rate, and widens the error band to 150 CAN units. +The geometry index rides bits 1-4. Ported from BluePilot's earlier vendored tests onto the +BP matrix classes; the geometry-table check reads the C table from source (no libsafety +getters), so no upstream harness file is touched. +""" +import unittest +from types import SimpleNamespace + +import numpy as np + +from ....car.ford.values import CAR +from ....car.vehicle_model import VehicleModel, calc_slip_factor +from ...core.values_ext import ( + FordSafetyFlagsSP, FORD_HOST_ONLY_PLATFORMS, FORD_NO_PINION_PLATFORMS, FORD_PINION_GEOMETRY_INDEX, FORD_PINION_GEOMETRY_SHIFT, + FORD_PINION_WHEELBASE, +) +from ...core.carcontroller_ext import pinion_vehicle_model, scale_tire_stiffness_per_mass +from ....car.ford.interface import CarInterface +from .ford_bluepilot_common import ( + CURVATURE_TO_CAN, CURVATURE_ERROR_MIN_SPEED, MAX_CURVATURE_ERROR_CAN_PINION, pinion_geometry_table, +) +from . import test_ford_bluepilot_matrix as matrix + + +def pinion_param_sp(geometry_index: int) -> int: + return int(FordSafetyFlagsSP.BP_LATERAL) | int(FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) | (geometry_index << FORD_PINION_GEOMETRY_SHIFT) + + +GEOMETRY = pinion_geometry_table() # the C table, so the platform mixins can never drift from it + + +class BPPinionMixin: + """Pinion-angle measurement source. The geometry comes from the C table row for GEOMETRY_INDEX; + TestFordBPPinionGeometryTable checks that row against CarSpecs.""" + + MAX_CURVATURE_ERROR_CAN_BP = MAX_CURVATURE_ERROR_CAN_PINION + MAX_CURVATURE_ERROR = MAX_CURVATURE_ERROR_CAN_PINION / CURVATURE_TO_CAN # the host's own error-band tests (IQ.Pilot's test_ford) + GEOMETRY_INDEX = 0 + PINION_SLIP_FACTOR, PINION_STEER_RATIO, PINION_WHEELBASE = GEOMETRY[0] + cnt_pinion = 0 + + def __init_subclass__(cls, **kwargs): + # a platform sets GEOMETRY_INDEX only; the geometry and the safety param follow from it + super().__init_subclass__(**kwargs) + cls.PINION_SLIP_FACTOR, cls.PINION_STEER_RATIO, cls.PINION_WHEELBASE = GEOMETRY[cls.GEOMETRY_INDEX] + cls.PARAM_SP = pinion_param_sp(cls.GEOMETRY_INDEX) + + def _curvature_factor(self, speed: float) -> float: + speed = max(speed, 0.1) + return 1. / (1. - (self.PINION_SLIP_FACTOR * (speed ** 2))) / self.PINION_WHEELBASE + + def _curvature_to_pinion_angle_deg(self, curvature: float, speed: float) -> float: + # inverse of ford_bp_rx_hook: curvature = angle_rad * curvature_factor(speed) / steer_ratio + return float(np.degrees(curvature * self.PINION_STEER_RATIO / self._curvature_factor(speed))) + + def _pinion_quant_tol(self, speed: float) -> int: + # 0.1 deg DBC quantization in curvature CAN units at this speed, +2 for float rounding + return int(np.radians(0.1) * self._curvature_factor(speed) / self.PINION_STEER_RATIO * CURVATURE_TO_CAN) + 2 + + def _pinion_msg(self, curvature: float, speed: float, quality_flag=True, bus=0): + values = {"StePinComp_An_Est": self._curvature_to_pinion_angle_deg(curvature, speed), + "StePinCompAnEst_D_Qf": 3 if quality_flag else 0, + "StePinAn_No_Cnt": self.cnt_pinion % 16} + self.__class__.cnt_pinion += 1 + return self.packer.make_can_msg_safety("SteeringPinion_Data", bus, values) + + def _reset_curvature_measurement(self, curvature, speed): + # 14 frames, not 6: after a counter discontinuity (rejected bad-QF frames still advance the + # python-side counter) the rx counter check drops frames until it re-syncs + for _ in range(14): + self._rx(self._speed_msg(speed)) + self._rx(self._speed_msg_2(speed)) # ford_bp_curvature_cmd_checks cross-checks the two speed sources + self._rx(self._pinion_msg(curvature, speed)) + + def test_rx_hook(self): + # checksum, counter and quality flag checks: the stock matrix plus the pinion message + for quality_flag in (True, False): + for msg_type in ("speed", "speed_2", "yaw", "pinion"): + self.safety.set_controls_allowed(True) + for _ in range(10): + if msg_type == "speed": + msg = self._speed_msg(0, quality_flag=quality_flag) + elif msg_type == "speed_2": + msg = self._speed_msg_2(0, quality_flag=quality_flag) + elif msg_type == "yaw": + msg = self._yaw_rate_msg(0, 0, quality_flag=quality_flag) + else: + msg = self._pinion_msg(0, 0, quality_flag=quality_flag) + self.assertEqual(quality_flag, self._rx(msg)) + self.assertEqual(quality_flag, self.safety.get_controls_allowed()) + + # corrupt the checksum byte; not checked on the 2nd speed source or the pinion message + # (unknown OEM checksum: integrity is counter + quality flag) + msg[0].data[3] = 0 + should_rx = msg_type in ("speed_2", "pinion") and quality_flag + self.assertEqual(should_rx, self._rx(msg)) + self.assertEqual(should_rx, self.safety.get_controls_allowed()) + + def test_angle_measurements(self): + """The rx hook converts pinion angle to curvature through the geometry row, within the + signal's 0.1 deg quantization.""" + for speed in np.arange(0.5, 40, 0.5): + for curvature in np.arange(0, 0.02 * 2, 2e-3): + self._rx(self._speed_msg(speed)) + for c in (curvature, -curvature, 0, 0, 0, 0): + self._rx(self._pinion_msg(c, speed)) + tol = self._pinion_quant_tol(speed) + self.assertAlmostEqual(self.safety.get_curvature_meas_min(), round(-curvature * CURVATURE_TO_CAN), delta=tol) + self.assertAlmostEqual(self.safety.get_curvature_meas_max(), round(curvature * CURVATURE_TO_CAN), delta=tol) + self._rx(self._pinion_msg(0, speed)) + self.assertAlmostEqual(self.safety.get_curvature_meas_min(), round(-curvature * CURVATURE_TO_CAN), delta=tol) + self.assertAlmostEqual(self.safety.get_curvature_meas_max(), 0, delta=tol) + self._rx(self._pinion_msg(0, speed)) + self.assertAlmostEqual(self.safety.get_curvature_meas_min(), 0, delta=tol) + self.assertAlmostEqual(self.safety.get_curvature_meas_max(), 0, delta=tol) + + def test_pinion_quality_flag_gates_measurement(self): + speed = CURVATURE_ERROR_MIN_SPEED + 5 + self._reset_curvature_measurement(0.005, speed) + before = self.safety.get_curvature_meas_max() + self.assertGreater(before, 0) + for _ in range(6): + self.assertFalse(self._rx(self._pinion_msg(0, speed, quality_flag=False))) + self.assertEqual(self.safety.get_curvature_meas_max(), before) + + def test_pinion_other_bus_ignored(self): + # only the main bus pinion frame feeds the measurement (its rx check entry is bus 0; ford_bp_rx_hook also matches the bus) + speed = CURVATURE_ERROR_MIN_SPEED + 5 + self._reset_curvature_measurement(0.005, speed) + before = self.safety.get_curvature_meas_max() + for _ in range(6): + self._rx(self._pinion_msg(-0.005, speed, bus=2)) + self.assertEqual(self.safety.get_curvature_meas_max(), before) + + def test_pinion_sign_convention(self): + """A command matching the measured sign passes the error band; a sign-inverted command (the + broken-yaw failure mode this source exists for) is blocked above the gate speed.""" + speed = CURVATURE_ERROR_MIN_SPEED + 5 + curvature = 0.005 # well above the 150-unit band + for sign in (1, -1): + with self.subTest(sign=sign): + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(sign * curvature, speed) + self._set_prev_desired_angle(sign * curvature) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, sign * curvature, 0))) + self._set_prev_desired_angle(-sign * curvature) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, -sign * curvature, 0))) + + def test_pinion_check_inert_below_gate_speed(self): + self.safety.set_controls_allowed(True) + speed = CURVATURE_ERROR_MIN_SPEED - 2 + self._reset_curvature_measurement(0.005, speed) + inverted = -0.005 + self._set_prev_desired_angle(inverted) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, inverted, 0))) + + +class FordExplorerPinionGeometry: # GEOMETRY_INDEX only; BPPinionMixin.__init_subclass__ fills in the rest + """FORD_EXPLORER_MK6, the on-road-validated primary platform.""" + GEOMETRY_INDEX = 5 + + +class FordBroncoSportPinionGeometry: # GEOMETRY_INDEX only; BPPinionMixin.__init_subclass__ fills in the rest + """FORD_BRONCO_SPORT_MK1, smallest wheelbase in the table.""" + GEOMETRY_INDEX = 1 + + +class FordF150PinionGeometry: # GEOMETRY_INDEX only; BPPinionMixin.__init_subclass__ fills in the rest + """FORD_F_150_MK14, largest wheelbase in the table.""" + GEOMETRY_INDEX = 8 + + +class TestFordBPPinionLongitudinalSafety(FordExplorerPinionGeometry, BPPinionMixin, matrix.TestFordBPLongitudinalSafety): + pass + + +class TestFordBPPinionCANFDStockSafety(FordExplorerPinionGeometry, BPPinionMixin, matrix.TestFordBPCANFDStockSafety): + pass + + +class TestFordBPPinionCANFDLongitudinalSafety(FordExplorerPinionGeometry, BPPinionMixin, matrix.TestFordBPCANFDLongitudinalSafety): + pass + + +class TestFordBPPinionBroncoSportSafety(FordBroncoSportPinionGeometry, BPPinionMixin, matrix.TestFordBPLongitudinalSafety): + pass + + +class TestFordBPPinionF150Safety(FordF150PinionGeometry, BPPinionMixin, matrix.TestFordBPCANFDLongitudinalSafety): + pass + + +class TestFordBPPinionGeometryTable(unittest.TestCase): + """The C geometry table must match CarSpecs + calc_slip_factor(VehicleModel(CP)) for every + platform in FORD_PINION_GEOMETRY_INDEX, so it cannot rot as platforms change. Where BluePilot + deliberately uses another wheelbase (FORD_PINION_WHEELBASE), the row uses that one, and so does + the Python pinion path.""" + + def _assert_row_matches_carspecs(self, car, idx): + specs = car.config.specs + wheelbase = FORD_PINION_WHEELBASE.get(car, specs.wheelbase) + center_to_front = wheelbase * specs.centerToFrontRatio + front, rear = scale_tire_stiffness_per_mass(wheelbase, center_to_front, specs.tireStiffnessFactor) + # both VehicleModel signatures, as pinion_vehicle_model builds it (slip factor doesn't read inertia) + CP = SimpleNamespace(wheelbase=wheelbase, steerRatio=specs.steerRatio, centerToFront=center_to_front, + tireStiffnessFrontPerMass=front, tireStiffnessRearPerMass=rear, rotationalInertiaPerMass=1.0, + mass=1.0, tireStiffnessFront=front, tireStiffnessRear=rear, rotationalInertia=1.0, steerRatioRear=0.) + slip_factor = calc_slip_factor(VehicleModel(CP)) + + slip, sr, wb = GEOMETRY[idx] + self.assertAlmostEqual(sr, specs.steerRatio, places=3, msg=str(car)) + self.assertAlmostEqual(wb, wheelbase, places=3, msg=str(car)) + self.assertAlmostEqual(slip, slip_factor, delta=abs(slip_factor) * 1e-4, msg=str(car)) + + def test_geometry_matches_carspecs(self): + rows = GEOMETRY + # the index rides bits 1-4 of current_safety_param_sp; growing past 15 would silently + # disable the firmware side while the control side still enables + self.assertLessEqual(len(rows) - 1, 15) + # every supported platform has a row unless it can't take the pinion source (FORD_NO_PINION_PLATFORMS), every + # Python index entry points at a C row. A row for a platform this host's CAR doesn't have + # (FORD_HOST_ONLY_PLATFORMS) has no CarSpecs here + self.assertEqual({n for n in FORD_PINION_GEOMETRY_INDEX if n in set(CAR)}, set(CAR) - FORD_NO_PINION_PLATFORMS, + "every Ford platform needs a geometry row") + self.assertTrue(set(FORD_PINION_GEOMETRY_INDEX) - set(CAR) <= FORD_HOST_ONLY_PLATFORMS) + self.assertTrue(set(FORD_PINION_GEOMETRY_INDEX.values()) <= set(rows) - {0}) + self.assertEqual(len(set(FORD_PINION_GEOMETRY_INDEX.values())), len(FORD_PINION_GEOMETRY_INDEX), "duplicate index") + + for name, idx in FORD_PINION_GEOMETRY_INDEX.items(): + if name in set(CAR): + self._assert_row_matches_carspecs(CAR(name), idx) + + def test_f150_uses_the_average_wheelbase(self): + """#21: F-150s with lane centering come in 3.68 and 3.99 m wheelbases and nothing tells them + apart, so pinion mode uses the average on both sides: the C row and the Python model.""" + self.assertEqual(FORD_PINION_WHEELBASE[CAR.FORD_F_150_MK14], 3.84) + self.assertAlmostEqual(GEOMETRY[FORD_PINION_GEOMETRY_INDEX[CAR.FORD_F_150_MK14]][2], 3.84, places=3) + CP = CarInterface.get_non_essential_params(CAR.FORD_F_150_MK14) + VM = pinion_vehicle_model(CP) + self.assertAlmostEqual(VM.l, 3.84, places=3) + self.assertAlmostEqual(VM.aF / VM.l, CP.centerToFront / CP.wheelbase, places=6) # the platform's weight split is kept + + def test_other_platforms_use_carspecs(self): + for car in FORD_PINION_GEOMETRY_INDEX: + if car in FORD_PINION_WHEELBASE or car not in set(CAR): + continue + CP = CarInterface.get_non_essential_params(car) + with self.subTest(car=car): + self.assertAlmostEqual(pinion_vehicle_model(CP).l, CP.wheelbase, places=6) + + def test_invalid_index_row_is_inert(self): + self.assertEqual(GEOMETRY[0], (0.0, 1.0, 1.0)) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/host.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/host.py new file mode 100644 index 0000000000..4c99db8af0 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/host.py @@ -0,0 +1,79 @@ +"""The host's own names, for the tests that touch them: what carries the per-frame inputs and settings, where +its safety bits ride, its adapter (hosts/), and its libsafety setters. Detected from the host's opendbc, so +the same tests run on every host (HOSTS.md, "Running the tests").""" +import importlib.util + +from ...car import structs + +if hasattr(structs, "CarControlSP"): # sunnypilot + from ..hosts.sunnypilot.lateral import BluePilotLateral, params_from_cc_sp as params_from_cc + from ..hosts.sunnypilot.safety_param import init_ford_safety_param_sp + + NAME = "sunnypilot" + CarControlX = structs.CarControlSP + SET_SAFETY_PARAM = "set_current_safety_param_sp" + SET_CONTROLS_ALLOWED_LAT = "set_controls_allowed_lateral" + + def _init(CP, params_dict): + CP_SP = structs.CarParamsSP() + init_ford_safety_param_sp(CP, CP_SP, params_dict) + return CP_SP.safetyParam, CP_SP.lateralInputsRequired +elif hasattr(structs, "IQCarControl"): # IQ.Pilot + from ..hosts.iqpilot.lateral import BluePilotLateral, params_from_cc_iq as params_from_cc + from ..hosts.iqpilot.safety_param import init_ford_safety_param_iq + + NAME = "iqpilot" + CarControlX = structs.IQCarControl + SET_SAFETY_PARAM = "set_current_safety_param_iq" + SET_CONTROLS_ALLOWED_LAT = "set_controls_allowed_lat" + + def _init(CP, params_dict): + CP_IQ = structs.IQCarParams() + init_ford_safety_param_iq(CP, CP_IQ, params_dict) + return CP_IQ.iqSafetyFlags, CP_IQ.lateralInputsRequired +elif importlib.util.find_spec(__package__.rsplit(".", 2)[0] + ".car.ford.aol"): # StarPilot: its Ford AOL + from ..hosts.starpilot.lateral import BluePilotLateral, FrameInputs as CarControlX, params_from_frame as params_from_cc + from ..hosts.starpilot.safety_param import (BP_SAFETY_PARAM_SHIFT, bp_safety_param, init_ford_safety_param_starpilot, + lateral_inputs_required as lateral_inputs_required_starpilot) + + NAME = "starpilot" + SET_SAFETY_PARAM = None # no second param: the bits ride the Ford param (ford_param) + SET_CONTROLS_ALLOWED_LAT = None # no lateral-only setter: AOL is off in these tests, so lateral follows controls_allowed + + def _init(CP, params_dict): + CP.safetyConfigs = [structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.ford)] + init_ford_safety_param_starpilot(CP, params_dict) + return bp_safety_param(CP), lateral_inputs_required_starpilot(CP) +else: + raise ImportError("unknown host: add it to tests/host.py and hosts/") + +__all__ = ["BluePilotLateral", "CarControlX", "params_from_cc"] + + +def startup_bits(CP, params_dict: dict[str, str]) -> int: + """What the host's car-init hook gives ford_bp_init (values_ext.ford_safety_param's bits).""" + return _init(CP, params_dict)[0] + + +def lateral_inputs_required(CP, params_dict: dict[str, str]) -> bool: + """Whether the host fills the per-frame inputs and settings for this car.""" + return _init(CP, params_dict)[1] + + +def set_safety_param(safety, bits: int) -> None: + """libsafety: the second safety param, the one ford_bp_init reads (current_safety_param_sp / _iq). A host + without one carries the bits in the Ford param instead (ford_param).""" + if SET_SAFETY_PARAM is not None: + getattr(safety, SET_SAFETY_PARAM)(bits) + + +def ford_param(base: int, bits: int) -> int: + """The Ford safety param set_safety_hooks takes for these startup bits: unchanged on a host with a second + param, the bits shifted in on StarPilot.""" + return base | (bits << BP_SAFETY_PARAM_SHIFT) if NAME == "starpilot" else base + + +def set_controls_allowed_lat(safety, allowed: bool) -> None: + """libsafety: the host's lateral-only engagement (sunnypilot MADS, IQ.Pilot AOL).""" + if SET_CONTROLS_ALLOWED_LAT is not None: + getattr(safety, SET_CONTROLS_ALLOWED_LAT)(allowed) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference.py deleted file mode 100644 index 1637576c87..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference.py +++ /dev/null @@ -1,101 +0,0 @@ -"""Hash-bound upstream angle execution and current-curvature controller oracle. - -Only imports are removed from the upstream angle/human module AST; every other -statement and function remains exact. Glue supplies current ABI types, source -fixed defaults and the source's no-op lazy-init/yaw-default adapter. No BluePilot -curvature strategy is loaded. Its diagnostic limiter cannot affect angle output. -""" -import ast -from collections import namedtuple -import hashlib -import json -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -from opendbc.car import structs, DT_CTRL -from opendbc.car.ford.values import CAR, CarControllerParams -from opendbc.car.ford.fordcan import CanBus -from opendbc.car.lateral import AngleSteeringLimits, apply_std_steer_angle_limits -from openpilot.selfdrive.modeld.constants import ModelConstants - -ROOT = Path(__file__).with_name("reference") -MANIFEST = json.loads((ROOT / "manifest.json").read_text()) - - -def source(name): - raw = (ROOT / (name + ".py.txt")).read_bytes() - if hashlib.sha256(raw).hexdigest() != MANIFEST[name]["sha256"]: - raise AssertionError("Reference source differs from its pinned manifest: " + name) - return ast.parse(raw.decode(), filename=MANIFEST[name]["origin"]) - - -def execute(tree, namespace, *, names=None): - body = [] - for node in tree.body: - if isinstance(node, (ast.Import, ast.ImportFrom)): - continue - if names is not None: - assigned = [target.id for target in node.targets if isinstance(target, ast.Name)] if isinstance(node, ast.Assign) else [] - if getattr(node, "name", None) not in names and not any(name in names for name in assigned): - continue - body.append(node) - exec(compile(ast.Module(body=body, type_ignores=[]), "", "exec"), namespace) - - -def angle(): - LateralResult = namedtuple("LateralResult", "apply_curvature curvature_rate path_offset path_angle ramp_type precision_type lateralUncertainty") - namespace = {"np": np, "clip": np.clip, "interp": np.interp, "DT_CTRL": DT_CTRL, - "CAR": CAR, "CarControllerParams": CarControllerParams, "ModelConstants": ModelConstants, - "LateralResult": LateralResult, "AngleSteeringLimits": AngleSteeringLimits, - "apply_std_steer_angle_limits": apply_std_steer_angle_limits} - execute(source("human"), namespace) - execute(source("values"), namespace, names={"_BP_ANGLE_RATE_UP", "_BP_ANGLE_RATE_DOWN", "BP_ANGLE_LIMITS"}) - execute(source("angle"), namespace) - return namespace["LateralAngleExt"] - - -def builders(): - namespace = {"CanBus": CanBus, "structs": structs} - execute(source("fordcan"), namespace, names={"calculate_lat_ctl2_checksum"}) - execute(source("builders"), namespace, names={"create_lka_msg", "create_lat_ctl_msg", "create_lat_ctl2_msg", "_BP_LKA_SHADOW_CURVATURE_SCALE"}) - return namespace - - -def curvature_controller(): - # Full exact pre-angle production controller, with unchanged current imports. - namespace = {"__name__": "ford_curvature_pre_angle_reference"} - exec(compile(source("starpilot_curvature_controller"), "", "exec"), namespace) - return namespace["CarController"] - - -class Angle: - def __init__(self, identity): - self.controller = angle()() - self.controller.CP = SimpleNamespace(carFingerprint=identity) - self.controller._ensure_lateral_curv_initialized = lambda _cp: None # Exact source guard is a no-op. - self.controller.get_current_curvature = lambda cs: -cs.out.yawRate / max(cs.out.vEgoRaw, .1) - self.controller.path_angle_last = 0. - self.controller.lp = None # Read but unused by the source angle strategy. - self.controller.lane_change_factor_bp = [0., 50.] - self.controller.lane_change_factor_low = 1. - self.controller.model = None - self.controller.update_angle_params(None) - - def step(self, inputs): - c = self.controller - c.sm = {"liveDelay": SimpleNamespace(lateralDelay=inputs.lateral_delay)} - c.model = SimpleNamespace(orientationRate=SimpleNamespace(z=list(inputs.yaw_predictions)), - meta=SimpleNamespace(laneChangeState=1 if inputs.lane_change else 0, - laneChangeDirection=inputs.lane_direction)) if inputs.yaw_predictions else None - cc = SimpleNamespace(latActive=inputs.active) - cs = SimpleNamespace(out=SimpleNamespace(vEgoRaw=inputs.speed, yawRate=-inputs.measured_curvature * inputs.speed, - steeringPressed=inputs.steering_pressed, steeringAngleDeg=inputs.steering_angle), - lat_ctl_lim_stat=inputs.limit_status) - return c.update_angle_strategy(cc, cs, SimpleNamespace(curvature=inputs.desired_curvature), c.CP) - - -def trim(): - namespace = {"np": np, "interp": np.interp} - execute(source("bpdev_trim"), namespace) - return namespace["LaneCenterTrim"] diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/builders.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/builders.py.txt deleted file mode 100644 index 296351f704..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/builders.py.txt +++ /dev/null @@ -1,377 +0,0 @@ -""" -BluePilot Ford CAN message builder extensions. - -Provides extended versions of the stock fordcan.py message builders with additional -parameters for BluePilot features: - - Dynamic ramp_type and precision_type for lateral control - - Split brake_actuate / precharge_actuate for smoother deceleration - - BlueCruise cluster HUD status, TJA warnings, and hands-free messaging - - Independent left/right lane line logic for LKAS UI - - ICBM button injection for intelligent cruise button management - -Each function mirrors the stock fordcan.py function it replaces, with an extended -parameter list. When stock carcontroller.py is refactored, it will import these -functions instead of the stock versions. -""" - -from opendbc.car import structs -from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum - -HUDControl = structs.CarControl.HUDControl - - -_BP_LKA_SHADOW_CURVATURE_SCALE = 1e-6 # 1/meter per raw unit, matches ford.h's decode - - -def create_lka_msg(packer, CAN: CanBus, lat_active: bool, hud_control, - angle_mode_engaged: bool = False, shadow_curvature: float = 0.0): - """ - Creates a CAN message for the Ford LKA Command. - - BluePilot extension: accepts lat_active and hud_control parameters for future - lane departure warning integration (unused so far). - - Also carries angle_mode_engaged + shadow_curvature packed into bits with no DBC signal mapped to - them -- confirmed unused (always 0, no cabana signal) on real F-150 dashcam routes. This message - is one openpilot itself originates every cycle, and ford_tx_hook already reads other fields - (LkaActvStats_D2_Req) directly out of these same bytes synchronously, in the same tx_hook call -- - no separate CAN ID, no RX round-trip needed (panda does not self-receive its own TX, confirmed - 2026-07-09 investigating a controlsMismatch caused by an earlier dedicated-message design). - - Byte layout (bits not covered by any Lane_Assist_Data1 DBC signal): - byte 4 bit 0: angle_mode_engaged - byte 4 bits 1-4: reserved (future bools) - byte 5-6: shadow_curvature (int16, scale 1e-6 1/m) - byte 7: reserved (future value) - Must match the decode in ford.h's FORD_Lane_Assist_Data1 tx_hook check exactly. - - Frequency is 33Hz. - """ - addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) - dat = bytearray(dat) - - shadow_curvature_raw = int(round(shadow_curvature / _BP_LKA_SHADOW_CURVATURE_SCALE)) - shadow_curvature_raw = max(-32768, min(32767, shadow_curvature_raw)) & 0xFFFF - - dat[4] |= 1 if angle_mode_engaged else 0 - dat[5] = (shadow_curvature_raw >> 8) & 0xFF - dat[6] = shadow_curvature_raw & 0xFF - - return addr, bytes(dat), bus - - -def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, ramp_type: int, precision_type: int, - path_offset: float, path_angle: float, curvature: float, curvature_rate: float): - """ - Creates a CAN message for the Ford TJA/LCA Command (non-CAN FD). - - BluePilot extension: dynamic ramp_type and precision_type parameters. - Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise). - - Ford lane centering uses a third-order polynomial to describe the road centerline: - c0 (path_offset): lateral offset between vehicle and centerline (positive is right) - c1 (path_angle): heading angle between vehicle and centerline (positive is right) - c2 (curvature): curvature of the centerline (positive is left) - c3 (curvature_rate): rate of change of curvature - - Frequency is 20Hz. - """ - values = { - "LatCtlRng_L_Max": 0, # Unknown [0|126] meter - "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, - # 3=InterventionRight, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": precision_type, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter - } - return packer.make_can_msg("LateralMotionControl", CAN.main, values) - - -def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int, - path_offset: float, path_angle: float, curvature: float, - curvature_rate: float, counter: int): - """ - Creates a CAN message for the Ford Lane Centering command (CAN FD). - - BluePilot extension: dynamic ramp_type and precision_type parameters. - Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise). - - This message replaces LateralMotionControl on CAN FD platforms and includes - counter and checksum fields. - - Frequency is 20Hz. - """ - values = { - "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, - # 3=SafeRampOut, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": ramp_type, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": precision_type, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians - "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter - "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 - "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] - "LatCtlPath_No_Cnt": counter, # [0|15] - "LatCtlPath_No_Cs": 0, # [0|255] - } - - # Calculate checksum (reuse stock function) - dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1] - values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) - - return packer.make_can_msg("LateralMotionControl2", CAN.main, values) - - -def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, accel_pred: float, - stopping: bool, brake_actuate: bool, precharge_actuate: bool, v_ego_kph: float): - """ - Creates a CAN message for the Ford ACC Command. - - BluePilot extension: split brake control into brake_actuate and precharge_actuate - (each with independent hysteresis thresholds) for smoother deceleration. Also - accepts accel_pred as a parameter instead of hardcoding -5.0. - - Precharge engages slightly before full brake for smoother initial decel feel. - Both use configurable hysteresis to avoid binary on/off feel. - - Frequency is 50Hz. - """ - values = { - "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 - "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes - "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 - "AccPrpl_A_Pred": accel_pred, # Predicted accel (from carcontroller, not hardcoded) - "AccResumEnbl_B_Rq": 1 if long_active else 0, - "AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h - "AccBrkPrchg_B_Rq": 1 if precharge_actuate else 0, # Pre-charge brake request (independent hysteresis) - "AccBrkDecel_B_Rq": 1 if brake_actuate else 0, # Deceleration request (independent hysteresis) - "AccStopStat_B_Rq": 1 if stopping else 0, - } - return packer.make_can_msg("ACCDATA", CAN.main, values) - - -def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, - standstill: bool, hud_control, stock_values: dict, send_hands_free_msg: bool, - send_ui: bool, send_bars: bool, tja_warn: int, tja_msg: int): - """ - Creates a CAN message for the Ford IPC adaptive cruise, FCW and TJA status. - - BluePilot extension: replaces stock show_distance_bars with explicit send_ui, - send_bars, and TJA parameters. Adds BlueCruise status 7 for hands-free cluster - UI. TJA warn/msg are set from DM state computation rather than stock passthrough. - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 5Hz. - """ - - # Tja_D_Stat: TJA status for cluster display - if enabled: - if hud_control.leftLaneDepart: - status = 3 # ActiveInterventionLeft - elif hud_control.rightLaneDepart: - status = 4 # ActiveInterventionRight - elif send_hands_free_msg: - status = 7 # BlueCruise UI in the cluster - else: - status = 2 # Active - elif main_on: - if hud_control.leftLaneDepart: - status = 5 # ActiveWarningLeft - elif hud_control.rightLaneDepart: - status = 6 # ActiveWarningRight - else: - status = 1 # Standby - elif standstill: - status = 0 # Off - else: - status = 1 # Standby - - values = {s: stock_values[s] for s in [ - "HaDsply_No_Cs", - "HaDsply_No_Cnt", - "AccStopStat_D_Dsply", # ACC stopped status message - "AccTrgDist2_D_Dsply", # ACC target distance - "AccStopRes_B_Dsply", - # TjaWarn_D_Rq and TjaMsgTxt_D_Dsply are set explicitly below, not passed through - "IaccLamp_D_Rq", # iACC status icon - "AccMsgTxt_D2_Rq", # ACC text - "FcwDeny_B_Dsply", # FCW disabled - "FcwMemStat_B_Actl", # FCW enabled setting - "AccTGap_B_Dsply", # ACC time gap display setting - "CadsAlignIncplt_B_Actl", - "AccFllwMde_B_Dsply", # ACC follow mode display setting - "CadsRadrBlck_B_Actl", - "CmbbPostEvnt_B_Dsply", # AEB event status - "AccStopMde_B_Dsply", # ACC stop mode display setting - "FcwMemSens_D_Actl", # FCW sensitivity setting - "FcwMsgTxt_D_Rq", # FCW text - "AccWarn_D_Dsply", # ACC warning - "FcwVisblWarn_B_Rq", # FCW visible alert - "FcwAudioWarn_B_Rq", # FCW audio alert - "AccTGap_D_Dsply", # ACC time gap - "AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting - "FdaMem_B_Stat", # FDA enabled setting - ]} - - values.update({ - "Tja_D_Stat": status, # TJA status - "TjaWarn_D_Rq": tja_warn, # TJA warning (from DM state, not stock passthrough) - "TjaMsgTxt_D_Dsply": tja_msg, # TJA text (from DM state, not stock passthrough) - }) - - if CP.openpilotLongitudinalControl: - values.update({ - "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text - "AccMsgTxt_D2_Rq": 0, # ACC text - "AccTGap_B_Dsply": 1 if send_bars else 0, # Show time gap control UI - "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator - "AccStopMde_B_Dsply": 1 if standstill else 0, - "AccWarn_D_Dsply": 0, # ACC warning - "AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap - }) - - # Forward FCW alert from IPMA - if fcw_alert: - values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert - values["FcwAudioWarn_B_Rq"] = 1 # FCW audio alert - - return packer.make_can_msg("ACCDATA_3", CAN.main, values) - - -def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, hands: int, - hud_control, stock_values: dict): - """ - Creates a CAN message for the Ford IPC IPMA/LKAS status. - - BluePilot extension: replaces stock steer_alert bool with hands int (0-3), - and uses independent left/right lane line status logic. - - hands values: - 0 = HandsOn - 1 = Level1 (warning without chime) - 2 = Level2 (warning with chime) - 3 = Suppressed - - LaActvStats_D_Dsply value table (left \\ right): - Right → | Intervene | Warning | Suppress | Available | None - Intervene | 24 | 19 | 14 | 9 | 4 - Warning | 23 | 18 | 13 | 8 | 3 - Suppress | 22 | 17 | 12 | 7 | 2 - Available | 21 | 16 | 11 | 6 | 1 - None | 20 | 15 | 10 | 5 | 0 - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 1Hz. - """ - - lines = 0 - - if hud_control is not None: - # Determine left lane status independently - if hud_control.leftLaneDepart: - left_status = 4 # Intervene (Yellow) - elif hud_control.leftLaneVisible: - left_status = 1 # Available - else: - left_status = 2 # Suppress - - # Determine right lane status independently - if hud_control.rightLaneDepart: - right_status = 20 # Intervene (Yellow) - elif hud_control.rightLaneVisible: - right_status = 5 # Available - else: - right_status = 10 # Suppress - - # Combine left and right lane status - lines = left_status + right_status - - values = {s: stock_values[s] for s in [ - "FeatConfigIpmaActl", - "FeatNoIpmaActl", - "PersIndexIpma_D_Actl", - "AhbcRampingV_D_Rq", # AHB ramping - "LaDenyStats_B_Dsply", # LKAS error - "CamraDefog_B_Req", # Windshield heater? - "CamraStats_D_Dsply", # Camera status - "DasAlrtLvl_D_Dsply", # DAS alert level - "DasStats_D_Dsply", # DAS status - "DasWarn_D_Dsply", # DAS warning - "AhbHiBeam_D_Rq", # AHB status - "Passthru_63", - "Passthru_48", - ]} - - values.update({ - "LaActvStats_D_Dsply": lines, # LKAS status (lane lines) [0|31] - "LaHandsOff_D_Dsply": hands, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed - }) - return packer.make_can_msg("IPMA_Data", CAN.main, values) - - -def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, - tja_toggle=False, icbm_button=None): - """ - Creates a CAN message for the Ford SCCM buttons/switches. - - BluePilot extension: adds icbm_button parameter for Intelligent Cruise Button - Management. When set, the specified CAN signal is set to 1 in the outgoing - message, enabling openpilot to emulate cruise button presses for speed adjustment. - - Args: - icbm_button: Optional string signal name (e.g., "CcAslButtnSetIncPress", - "CcAslButtnSetDecPress") for ICBM button injection. - - Frequency is 10Hz. - """ - values = {s: stock_values[s] for s in [ - "HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons - "TurnLghtSwtch_D_Stat", # SCCM Turn signal switch - "WiprFront_D_Stat", - "LghtAmb_D_Sns", - "AccButtnGapDecPress", - "AccButtnGapIncPress", - "AslButtnOnOffCnclPress", - "AslButtnOnOffPress", - "LaSwtchPos_D_Stat", - "CcAslButtnCnclResPress", - "CcAslButtnDeny_B_Actl", - "CcAslButtnIndxDecPress", - "CcAslButtnIndxIncPress", - "CcAslButtnOffCnclPress", - "CcAslButtnOnOffCncl", - "CcAslButtnOnPress", - "CcAslButtnResDecPress", - "CcAslButtnResIncPress", - "CcAslButtnSetDecPress", - "CcAslButtnSetIncPress", - "CcAslButtnSetPress", - "CcButtnOffPress", - "CcButtnOnOffCnclPress", - "CcButtnOnOffPress", - "CcButtnOnPress", - "HeadLghtHiFlash_D_Actl", - "HeadLghtHiOn_B_StatAhb", - "AhbStat_B_Dsply", - "AccButtnGapTogglePress", - "WiprFrontSwtch_D_Stat", - "HeadLghtHiCtrl_D_RqAhb", - ]} - - values.update({ - "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button - "CcAsllButtnResPress": 1 if resume else 0, # CC resume button - "TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button - }) - - # ICBM button support — set the specified button signal to 1 - if icbm_button is not None: - values[icbm_button] = 1 - - return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/fordcan.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/fordcan.py.txt deleted file mode 100644 index 2ac68c4427..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/fordcan.py.txt +++ /dev/null @@ -1,342 +0,0 @@ -from opendbc.car import CanBusBase, structs - -HUDControl = structs.CarControl.HUDControl - - -class CanBus(CanBusBase): - def __init__(self, CP=None, fingerprint=None) -> None: - super().__init__(CP, fingerprint) - - @property - def main(self) -> int: - return self.offset - - @property - def radar(self) -> int: - return self.offset + 1 - - @property - def camera(self) -> int: - return self.offset + 2 - - -def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int: - curvature = (dat[2] << 3) | ((dat[3]) >> 5) - curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5) - path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2) - path_offset = ((dat[4] & 0x3) << 8) | dat[5] - - checksum = mode + counter - for sig_val in (curvature, curvature_rate, path_angle, path_offset): - checksum += sig_val + (sig_val >> 8) - - return 0xFF - (checksum & 0xFF) - - -def create_lka_msg(packer, CAN: CanBus): - """ - Creates an empty CAN message for the Ford LKA Command. - - This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout. - - Frequency is 33Hz. - """ - - return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) - - -def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float, - curvature_rate: float): - """ - Creates a CAN message for the Ford TJA/LCA Command. - - This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway - driving. It is not subject to the PSCM lockout. - - Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined - by the following coefficients: - c0: lateral offset between the vehicle and the centerline (positive is right) - c1: heading angle between the vehicle and the centerline (positive is right) - c2: curvature of the centerline (positive is left) - c3: rate of change of curvature of the centerline - As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering - angle cannot be easily controlled. - - The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done - using tools such as Forscan. - - Frequency is 20Hz. - """ - - values = { - "LatCtlRng_L_Max": 0, # Unknown [0|126] meter - "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, - # 3=InterventionRight, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - # Makes no difference with curvature control - "LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - # The stock system always uses comfortable - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter - } - return packer.make_can_msg("LateralMotionControl", CAN.main, values) - - -def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float, - curvature_rate: float, counter: int): - """ - Create a CAN message for the new Ford Lane Centering command. - - This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has - additional signals for a counter and checksum. - - Frequency is 20Hz. - """ - - values = { - "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, - # 3=SafeRampOut, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians - "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter - "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 - "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] - "LatCtlPath_No_Cnt": counter, # [0|15] - "LatCtlPath_No_Cs": 0, # [0|255] - } - - # calculate checksum - dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1] - values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) - - return packer.make_can_msg("LateralMotionControl2", CAN.main, values) - - -def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float): - """ - Creates a CAN message for the Ford ACC Command. - - This command can be used to enable ACC, to set the ACC gas/brake/decel values - and to disable ACC. - - Frequency is 50Hz. - """ - values = { - "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 - "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes - "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 - # No observed acceleration seen from this signal alone. During stock system operation, it appears to - # be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative) - "AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2 - "AccResumEnbl_B_Rq": 1 if long_active else 0, - # No observed acceleration seen from this signal alone - "AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h - # TODO: we may be able to improve braking response by utilizing pre-charging better - # When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq - "AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes - "AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active - "AccStopStat_B_Rq": 1 if stopping else 0, - } - return packer.make_can_msg("ACCDATA", CAN.main, values) - - -def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool, - show_distance_bars: bool, hud_control, stock_values: dict): - """ - Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam - assist status. - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 5Hz. - """ - - # Tja_D_Stat - if enabled: - if hud_control.leftLaneDepart: - status = 3 # ActiveInterventionLeft - elif hud_control.rightLaneDepart: - status = 4 # ActiveInterventionRight - else: - status = 2 # Active - elif main_on: - if hud_control.leftLaneDepart: - status = 5 # ActiveWarningLeft - elif hud_control.rightLaneDepart: - status = 6 # ActiveWarningRight - else: - status = 1 # Standby - else: - status = 0 # Off - - values = {s: stock_values[s] for s in [ - "HaDsply_No_Cs", - "HaDsply_No_Cnt", - "AccStopStat_D_Dsply", # ACC stopped status message - "AccTrgDist2_D_Dsply", # ACC target distance - "AccStopRes_B_Dsply", - "TjaWarn_D_Rq", # TJA warning - "TjaMsgTxt_D_Dsply", # TJA text - "IaccLamp_D_Rq", # iACC status icon - "AccMsgTxt_D2_Rq", # ACC text - "FcwDeny_B_Dsply", # FCW disabled - "FcwMemStat_B_Actl", # FCW enabled setting - "AccTGap_B_Dsply", # ACC time gap display setting - "CadsAlignIncplt_B_Actl", - "AccFllwMde_B_Dsply", # ACC follow mode display setting - "CadsRadrBlck_B_Actl", - "CmbbPostEvnt_B_Dsply", # AEB event status - "AccStopMde_B_Dsply", # ACC stop mode display setting - "FcwMemSens_D_Actl", # FCW sensitivity setting - "FcwMsgTxt_D_Rq", # FCW text - "AccWarn_D_Dsply", # ACC warning - "FcwVisblWarn_B_Rq", # FCW visible alert - "FcwAudioWarn_B_Rq", # FCW audio alert - "AccTGap_D_Dsply", # ACC time gap - "AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting - "FdaMem_B_Stat", # FDA enabled setting - ]} - - values.update({ - "Tja_D_Stat": status, # TJA status - }) - - if CP.openpilotLongitudinalControl: - values.update({ - "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text - "AccMsgTxt_D2_Rq": 0, # ACC text - "AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI - "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator - "AccStopMde_B_Dsply": 1 if standstill else 0, - "AccWarn_D_Dsply": 0, # ACC warning - "AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap - }) - - # Forwards FCW alert from IPMA - if fcw_alert: - values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert - - return packer.make_can_msg("ACCDATA_3", CAN.main, values) - - -def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control, - stock_values: dict): - """ - Creates a CAN message for the Ford IPC IPMA/LKAS status. - - Show the LKAS status with the "driver assist" lines in the IPC. - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 1Hz. - """ - - # LaActvStats_D_Dsply - # R Intvn Warn Supprs Avail No - # L - # Intvn 24 19 14 9 4 - # Warn 23 18 13 8 3 - # Supprs 22 17 12 7 2 - # Avail 21 16 11 6 1 - # No 20 15 10 5 0 - # - # TODO: test suppress state - if enabled: - lines = 0 # NoLeft_NoRight - if hud_control.leftLaneDepart: - lines += 4 - elif hud_control.leftLaneVisible: - lines += 1 - if hud_control.rightLaneDepart: - lines += 20 - elif hud_control.rightLaneVisible: - lines += 5 - elif main_on: - lines = 0 - else: - if hud_control.leftLaneDepart: - lines = 3 # WarnLeft_NoRight - elif hud_control.rightLaneDepart: - lines = 15 # NoLeft_WarnRight - else: - lines = 30 # LA_Off - - hands_on_wheel_dsply = 1 if steer_alert else 0 - - values = {s: stock_values[s] for s in [ - "FeatConfigIpmaActl", - "FeatNoIpmaActl", - "PersIndexIpma_D_Actl", - "AhbcRampingV_D_Rq", # AHB ramping - "LaDenyStats_B_Dsply", # LKAS error - "CamraDefog_B_Req", # Windshield heater? - "CamraStats_D_Dsply", # Camera status - "DasAlrtLvl_D_Dsply", # DAS alert level - "DasStats_D_Dsply", # DAS status - "DasWarn_D_Dsply", # DAS warning - "AhbHiBeam_D_Rq", # AHB status - "Passthru_63", - "Passthru_48", - ]} - - values.update({ - "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] - "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed - }) - return packer.make_can_msg("IPMA_Data", CAN.main, values) - - -def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False): - """ - Creates a CAN message for the Ford SCCM buttons/switches. - - Includes cruise control buttons, turn lights and more. - - Frequency is 10Hz. - """ - - values = {s: stock_values[s] for s in [ - "HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons - "TurnLghtSwtch_D_Stat", # SCCM Turn signal switch - "WiprFront_D_Stat", - "LghtAmb_D_Sns", - "AccButtnGapDecPress", - "AccButtnGapIncPress", - "AslButtnOnOffCnclPress", - "AslButtnOnOffPress", - "LaSwtchPos_D_Stat", - "CcAslButtnCnclResPress", - "CcAslButtnDeny_B_Actl", - "CcAslButtnIndxDecPress", - "CcAslButtnIndxIncPress", - "CcAslButtnOffCnclPress", - "CcAslButtnOnOffCncl", - "CcAslButtnOnPress", - "CcAslButtnResDecPress", - "CcAslButtnResIncPress", - "CcAslButtnSetDecPress", - "CcAslButtnSetIncPress", - "CcAslButtnSetPress", - "CcButtnOffPress", - "CcButtnOnOffCnclPress", - "CcButtnOnOffPress", - "CcButtnOnPress", - "HeadLghtHiFlash_D_Actl", - "HeadLghtHiOn_B_StatAhb", - "AhbStat_B_Dsply", - "AccButtnGapTogglePress", - "WiprFrontSwtch_D_Stat", - "HeadLghtHiCtrl_D_RqAhb", - ]} - - values.update({ - "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button - "CcAsllButtnResPress": 1 if resume else 0, # CC resume button - "TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button - }) - return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/manifest.json b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/manifest.json deleted file mode 100644 index 9d43526b68..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/manifest.json +++ /dev/null @@ -1,43 +0,0 @@ -{ - "angle": { - "origin": "opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py", - "sha256": "18fa5290edade52137c7c6705a2eb2d42c94d39d1fa649eae06b7828c68207b3", - "revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a" - }, - "human": { - "origin": "opendbc_repo/opendbc/sunnypilot/car/ford/human_turn.py", - "sha256": "82735e348c5ce3e7b2c6a28ea3e298cca71ae8867433c270f7b54a9ee315ef37", - "revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a" - }, - "builders": { - "origin": "opendbc_repo/opendbc/sunnypilot/car/ford/fordcan_ext.py", - "sha256": "65ad2114db7210371fc5de06de962c0e29ce396a651c1dd2d58b07a80555a1bf", - "revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a" - }, - "fordcan": { - "origin": "opendbc_repo/opendbc/car/ford/fordcan.py", - "sha256": "0c35b8663afcaa3a58ab028a56f076c2c9f2f0296416c0abaf5905c674672956", - "revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a" - }, - "values": { - "origin": "opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py", - "sha256": "74802d82314eaa27eb610810d2815d3da77003c4a1c62e0437e57e785ba25f52", - "revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a" - }, - "starpilot_curvature_controller": { - "origin": "opendbc_repo/opendbc/car/ford/carcontroller.py", - "sha256": "9838f258b7a4eef48c8c1ed4fb40d180939b86e22ac9079a734d1eabf4c0060e", - "revision": "d2ae98eb3959c88011ed537e7cbc033d91fd3efa", - "intentional_changes": [ - { - "commit": "d60f2d7ada6cf6aa0f32e881aa916809686a65a6", - "contract": "Generic CAN-FD stock-idle and same-batch mode-before-bounded-command handoff. Retains the pre-angle curvature oracle and all full-byte comparisons." - } - ] - }, - "bpdev_trim": { - "origin": "opendbc_repo/opendbc/sunnypilot/car/ford/lane_center_trim.py", - "sha256": "1affc0abeaeeabaefca4741b0d75d8f249590c157340c5a9760a395d28f0a621", - "revision": "e22afa6be9b881fa784c92ebb316db47728a3d81" - } -} diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/starpilot_curvature_controller.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/starpilot_curvature_controller.py.txt deleted file mode 100644 index 307a324624..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/starpilot_curvature_controller.py.txt +++ /dev/null @@ -1,337 +0,0 @@ -import math -import numpy as np -from opendbc.can import CANPacker -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs -from opendbc.car.ford import fordcan -from opendbc.car.ford.manual_turn import ManualTurnLatch -from opendbc.car.ford.aol import qualified as qualified_aol -from opendbc.car.ford.stock_cruise import FordStockCruiseButton, qualified as stock_switch_qualified -from opendbc.car.ford.curvature_preview import blend_curvature -from opendbc.car.ford import mache_can -from opendbc.car.ford.extended_classic_can import create_extended_classic_lat_ctl_msg -from opendbc.car.ford.classic_lateral import create_controller as create_classic_controller, bounded_command as classic_bounded_command -from opendbc.car.ford.generic_canfd_can import create_extended_canfd_msg -from opendbc.car.ford.generic_canfd_lateral import ( - GenericCanfdLateralController, qualified as generic_canfd_qualified, bounded_command as generic_canfd_bounded_command, -) -from opendbc.car.ford.mache_lateral import MachELateralController, FordLateralResult, bounded_command, qualified as mache_qualified -from opendbc.car.ford.values import CarControllerParams, FordFlags, FordSafetyFlags, CAR -from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX - -LongCtrlState = structs.CarControl.Actuators.LongControlState -VisualAlert = structs.CarControl.HUDControl.VisualAlert - -def anti_overshoot(apply_curvature, apply_curvature_last, v_ego): - diff = 0.1 - tau = 5 # 5s smooths over the overshoot - dt = DT_CTRL * CarControllerParams.STEER_STEP - alpha = 1 - np.exp(-dt / tau) - - lataccel = apply_curvature * (v_ego ** 2) - last_lataccel = apply_curvature_last * (v_ego ** 2) - last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff) - last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel - - output_curvature = last_lataccel / (max(v_ego, 1) ** 2) - - return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature])) - - -def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float: - if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping): - return accel - creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.]) - creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.]) - accel -= creep_accel - return float(accel) - - -def apply_brake_rate_limit(accel: float, gas: float, accel_last: float, v_ego: float, car_fingerprint: str, - *, stopping: bool, standstill: bool, fcw: bool) -> tuple[float, float]: - mild_mach_e_braking = ( - car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and - -1.0 < gas < 0.0 and v_ego >= 5.0 and not (stopping or standstill or fcw) - ) - if mild_mach_e_braking: - accel = max(accel, min(accel_last, 0.0) - 2.0 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL) - gas = accel - else: - accel = max(accel, accel_last - 3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL) - return float(accel), float(gas) - - -class CarController(CarControllerBase): - def __init__(self, dbc_names, CP): - super().__init__(dbc_names, CP) - self.packer = CANPacker(dbc_names[Bus.pt]) - self.CAN = fordcan.CanBus(CP) - - self.manual_turn = ManualTurnLatch() if CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not CP.flags & FordFlags.LKA_STEERING else None - self.stock_cruise_button = FordStockCruiseButton() - self.manual_turn_inputs = None - self.mache_lateral = MachELateralController(CP) if mache_qualified(CP) else None - self.mache_extended_announced = False - self.generic_canfd = generic_canfd_qualified(CP) - self.classic_lateral = GenericCanfdLateralController(CP) if self.generic_canfd else create_classic_controller(CP) - self.classic_extended_announced = False - self.classic_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and - s.safetyParam & (FordSafetyFlags.CLASSIC_EXTENDED | FordSafetyFlags.GENERIC_CANFD_EXTENDED) for s in CP.safetyConfigs) - self.mache_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and - s.safetyParam & FordSafetyFlags.MACH_E_EXTENDED for s in CP.safetyConfigs) - self.apply_curvature_last = 0 - self.anti_overshoot_curvature_last = 0 - self.accel = 0.0 - self.gas = 0.0 - self.brake_request = False - self.main_on_last = False - self.lkas_enabled_last = False - self.steer_alert_last = False - self.lead_distance_bars_last = None - self.distance_bar_frame = 0 - self.apply_lka_angle_last = 0. - self.apply_lka_curvature_last = 0. - - def update(self, CC, CS, now_nanos): - can_sends = [] - if (self.mache_lateral is not None or self.classic_lateral is not None) and self.manual_turn_inputs is not None: - self.manual_turn_inputs.update() - self.manual_turn_inputs.apply_blend_settings(self) - - actuators = CC.actuators - hud_control = CC.hudControl - - main_on = CS.out.cruiseState.available - steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) - fcw_alert = hud_control.visualAlert == VisualAlert.fcw - - ### acc buttons ### - stock_cancel = False - stock_resume = False - if stock_switch_qualified(self.CP): - stock_cancel, stock_resume = self.stock_cruise_button.update( - bool(CS.buttons_stock_values["CcAslButtnCnclResPress"]), - CS.out.cruiseState.available, CS.out.cruiseState.enabled) - - if CC.cruiseControl.cancel: - can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True)) - can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True)) - elif (stock_cancel or stock_resume) and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: - can_sends.append(fordcan.create_button_msg( - self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume)) - can_sends.append(fordcan.create_button_msg( - self.packer, self.CAN.main, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume)) - elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: - can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True)) - can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True)) - # if stock lane centering isn't off, send a button press to toggle it off - # the stock system checks for steering pressed, and eventually disengages cruise control - elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0: - can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True)) - - ### lateral control ### - # send steer msg at 20Hz - if (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING: - # Preserve the stock LMC heartbeat without requesting lateral action. - can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, False, 0., 0., 0., 0., - stock_lmc=CS.lateral_motion_control)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.mache_lateral is not None: - permission = getattr(self.manual_turn_inputs, "assist_permission", None) - self.mache_lateral.set_assist_permission(*(permission() if permission is not None else ((), False))) - inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None - if inputs is not None: - self.mache_lateral.set_inputs(*inputs) - else: - # Withdraw stale preview while retaining current actuator/manual-turn control. - self.mache_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True) - previous = self.mache_lateral.curvature_last - previous_path_angle = self.mache_lateral.path_angle_last - demanded = self.mache_lateral.update(CC, CS, actuators) if self.mache_extended_announced else FordLateralResult() - lateral = bounded_command(self.mache_lateral, demanded, previous, CS.out.vEgoRaw, previous_path_angle) - self.mache_lateral_demand = demanded - self.apply_curvature_last = lateral.curvature - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(mache_can.create_lat_ctl2_msg( - self.packer, self.CAN, int(lateral.active), lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate, counter, -lateral.path_angle)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.classic_lateral is not None: - inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None - if inputs is not None: - self.classic_lateral.set_inputs(*inputs) - else: - self.classic_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True) - previous = self.classic_lateral.curvature_last - demanded = self.classic_lateral.update(CC, CS, actuators) if self.classic_extended_announced or self.generic_canfd else FordLateralResult() - bound = generic_canfd_bounded_command if self.generic_canfd else classic_bounded_command - lateral = bound(self.classic_lateral, demanded, previous, CS.out.vEgoRaw, - -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)) - self.classic_lateral_demand = demanded - self.apply_curvature_last = lateral.curvature - if self.generic_canfd: - if self.classic_extended_announced != bool(lateral.active): - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN) if lateral.active else - fordcan.create_lka_msg(self.packer, self.CAN)) - self.classic_extended_announced = bool(lateral.active) - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(create_extended_canfd_msg( - self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate, counter)) - else: - can_sends.append(create_extended_classic_lat_ctl_msg( - self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0: - lateral_active = CC.latActive and not self.mache_profile and not self.classic_profile - manual_turn = False - if self.manual_turn is not None and self.manual_turn_inputs is not None: - detection_enabled, lane_change, model_ready = self.manual_turn_inputs.update() - manual_turn = self.manual_turn.update(CC, CS, float(actuators.curvature), detection_enabled, lane_change, model_ready) - if manual_turn: - lateral_active = False - self.apply_curvature_last = 0. - # Bronco and some other cars consistently overshoot curv requests - # Apply some deadzone + smoothing convergence to avoid oscillations - if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): - self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw) - apply_curvature = self.anti_overshoot_curvature_last - else: - apply_curvature = actuators.curvature - - # apply rate limits, curvature error limit, and clip to signal range - current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - if self.manual_turn is not None and self.manual_turn_inputs is not None and lateral_active: - prediction = getattr(self.manual_turn_inputs, "preview_curvature", None) - preview = prediction(CS.out.vEgoRaw) if prediction is not None else None - if preview is not None: - apply_curvature = blend_curvature(apply_curvature, preview, current_curvature) - # No blending at low speed due to lack of torque wind-up and inaccurate current curvature - if CS.out.vEgoRaw > 9: - apply_curvature = float(np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, - current_curvature + CarControllerParams.CURVATURE_ERROR)) - apply_curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw, - 0., lateral_active, CarControllerParams.STEER_STEP) - self.apply_curvature_last = 0. if manual_turn else apply_curvature - - if self.CP.flags & FordFlags.CANFD: - # TODO: extended mode - # Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02 m^-1) - # can actuate the steering for a large portion of any lateral movements. However, in order to get further control on - # steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes. - # A detailed explanation on ford control can be found here: - # https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706 - mode = 1 if lateral_active else 0 - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter)) - else: - can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, lateral_active, 0., 0., -self.apply_curvature_last, 0.)) - - # send lka msg at 33Hz - if (self.frame % CarControllerParams.LKA_STEP) == 0 and not any( - address == 0x3CA for address, _, _ in can_sends): - if self.CP.flags & FordFlags.LKA_STEERING: - source_age_ns = now_nanos - CS.lkas_available_ts_nanos - lka_active = (CC.latActive and CS.lkas_available and 0 <= source_age_ns <= 100_000_000 and - (CS.out.cruiseState.enabled or qualified_aol(self.CP, marked_only=True)) and - not CS.out.steerFaultTemporary and - not CS.out.vehicleSensorsInvalid) - if lka_active: - desired_angle = float(np.clip(actuators.steeringAngleDeg - CS.out.steeringAngleDeg, -5.8, 5.8)) - self.apply_lka_angle_last = float(np.clip(desired_angle, self.apply_lka_angle_last - 1., - self.apply_lka_angle_last + 1.)) - desired_curvature = -actuators.curvature - current_curvature = CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - if CS.out.vEgoRaw > 9.: - desired_curvature = float(np.clip(desired_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, - current_curvature + CarControllerParams.CURVATURE_ERROR)) - self.apply_lka_curvature_last = CarControllerParams.LKA_CURVATURE_LIMITS.apply_limits( - desired_curvature, self.apply_lka_curvature_last, CS.out.vEgoRaw, 0., True, CarControllerParams.LKA_STEP) - else: - self.apply_lka_angle_last = 0. - self.apply_lka_curvature_last = 0. - self.apply_curvature_last = -self.apply_lka_curvature_last - direction = 2 if self.apply_lka_angle_last >= 0. else 4 - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, lka_active, self.apply_lka_angle_last, - direction, self.apply_lka_curvature_last, transit=True)) - elif self.mache_lateral is not None: - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN)) - self.mache_extended_announced = True - elif self.classic_lateral is not None: - if self.generic_canfd and not self.classic_extended_announced: - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) - else: - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN)) - self.classic_extended_announced = True - else: - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) - - ### longitudinal control ### - # send acc msg at 50Hz - if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: - accel = actuators.accel - gas = accel - stopping = actuators.longControlState == LongCtrlState.stopping - - if CC.longActive: - accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint, - standstill=CS.out.standstill, stopping=stopping) - - accel, gas = apply_brake_rate_limit( - accel, gas, self.accel, CS.out.vEgo, self.CP.carFingerprint, - stopping=stopping, standstill=CS.out.standstill, - fcw=fcw_alert or CS.out.stockFcw or CS.out.stockAeb, - ) - - accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) - gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) - - # Both gas and accel are in m/s^2, accel is used solely for braking - if not CC.longActive or gas < CarControllerParams.MIN_GAS: - gas = CarControllerParams.INACTIVE_GAS - - # PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits - accel_due_to_pitch = 0.0 - if len(CC.orientationNED) == 3: - accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY - - accel_pitch_compensated = accel + accel_due_to_pitch - if accel_pitch_compensated > 0.3 or not CC.longActive: - self.brake_request = False - elif accel_pitch_compensated < 0.0: - self.brake_request = True - - # TODO: look into using the actuators packet to send the desired speed - can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX)) - - self.accel = accel - self.gas = gas - - ### ui ### - send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) - # send lkas ui msg at 1Hz or if ui state changes - if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) - - # send acc ui msg at 5Hz or if ui state changes - if hud_control.leadDistanceBars != self.lead_distance_bars_last: - send_ui = True - self.distance_bar_frame = self.frame - - if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - show_distance_bars = self.frame - self.distance_bar_frame < 400 - can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, - fcw_alert, CS.out.cruiseState.standstill, show_distance_bars, - hud_control, CS.acc_tja_status_stock_values)) - - self.main_on_last = main_on - self.lkas_enabled_last = CC.latActive - self.steer_alert_last = steer_alert - self.lead_distance_bars_last = hud_control.leadDistanceBars - - new_actuators = actuators.as_builder() - new_actuators.curvature = self.apply_curvature_last - if self.CP.flags & FordFlags.LKA_STEERING: - new_actuators.steeringAngleDeg = CS.out.steeringAngleDeg + self.apply_lka_angle_last - new_actuators.accel = self.accel - new_actuators.gas = self.gas - - self.frame += 1 - return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/values.py.txt b/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/values.py.txt deleted file mode 100644 index a748a6f97c..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/reference/values.py.txt +++ /dev/null @@ -1,125 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" - -from collections import namedtuple - -from opendbc.car import structs -from opendbc.car.docs_definitions import CarParts, Device -from opendbc.car.ford.values import CAR -from opendbc.car.lateral import AngleSteeringLimits - -ButtonType = structs.CarState.ButtonEvent.Type -Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) - -# Ford cruise control buttons are in the Steering_Data_FD1 message (CAN ID 131) -# These signals are 1-bit flags: 1 = pressed, 0 = not pressed -# -# Note: Some buttons are combo buttons that emit multiple ButtonEvent types: -# - CcAslButtnSetIncPress emits both accelCruise (type 3) and setCruise (type 9) -# - CcAslButtnCnclResPress emits both cancel (type 5) and resumeCruise (type 10) -# -# There is also a separate CcAslButtnSetPress signal for the standalone "Set" button, -# but based on user mapping, setCruise is mapped to the combo button instead. -BUTTONS = [ - # Combo button: Set + Increase (emits accelCruise when enabled, setCruise when disabled) - Button(ButtonType.accelCruise, "Steering_Data_FD1", "CcAslButtnSetIncPress", [1]), - Button(ButtonType.setCruise, "Steering_Data_FD1", "CcAslButtnSetIncPress", [1]), - - # Combo button: Set + Decrease (emits decelCruise when enabled, setCruise when disabled) - Button(ButtonType.decelCruise, "Steering_Data_FD1", "CcAslButtnSetDecPress", [1]), - Button(ButtonType.setCruise, "Steering_Data_FD1", "CcAslButtnSetDecPress", [1]), - - # Combo button: Cancel/Resume (emits cancel when enabled, resumeCruise when disabled) - Button(ButtonType.cancel, "Steering_Data_FD1", "CcAslButtnCnclResPress", [1]), - Button(ButtonType.resumeCruise, "Steering_Data_FD1", "CcAslButtnCnclResPress", [1]), - - # Main cruise button (on/off toggle) - Button(ButtonType.mainCruise, "Steering_Data_FD1", "CcButtnOnOffPress", [1]), -] - - -class FordSafetyFlagsSP: - """Sunnypilot-level safety flags for Ford. - - Carried in CP_SP.safetyParam and delivered to the safety firmware as - current_safety_param_sp (the separate SP uint16, USB control 0xdf) -- NOT the main - safetyConfigs[].safetyParam. ford_init reads it with GET_FLAG(current_safety_param_sp, - ...), same pattern as Subaru STOP_AND_GO (subaru_common.h). Plain int constants, not - IntFlag: CP_SP.safetyParam must stay a plain int through capnp serialization in card. - """ - STEER_ANGLE_CURVATURE = 1 - - -# Geometry-table index for the steering-angle curvature measurement, packed into -# CP_SP.safetyParam bits 1-4 when STEER_ANGLE_CURVATURE is set. Must match the -# ford_pinion_geometry table in safety/modes/ford.h row for row (enforced by -# test_ford.py's geometry-consistency test against CarSpecs + calc_slip_factor). -# Index 0 is reserved as invalid: the firmware treats flag-set-but-no-index as feature -# off, so a half-configured param can never select the wrong geometry silently. -# FORD_EDGE_MK2 is deliberately absent: ALT_STEER_ANGLE platforms read a RELATIVE pinion -# angle (SteeringPinion_Data_Alt + learned offset) and lack the absolute measurement -# this feature needs -- the toggle no-ops there and yaw behavior is kept. -FORD_PINION_GEOMETRY_SHIFT = 1 -FORD_PINION_GEOMETRY_INDEX = { - CAR.FORD_BRONCO_SPORT_MK1: 1, - CAR.FORD_ESCAPE_MK4: 2, - CAR.FORD_ESCAPE_MK4_5: 3, - CAR.FORD_EXPEDITION_MK4: 4, - CAR.FORD_EXPLORER_MK6: 5, - CAR.FORD_FOCUS_MK4: 6, - CAR.FORD_F_150_LIGHTNING_MK1: 7, - CAR.FORD_F_150_MK14: 8, - CAR.FORD_MAVERICK_MK1: 9, - CAR.FORD_MONDEO_MK5: 10, - CAR.FORD_MUSTANG_MACH_E_MK1: 11, - CAR.FORD_RANGER_MK2: 12, -} - - -# BluePilot: Max curvature for steering command (m^-1), from DBC file limits -CURVATURE_MAX = 0.02 - -# BluePilot: Curvature rate limits — 3-point breakpoints for smoother lateral control. -# Upstream opendbc uses 2-point ([5, 25]) with more conservative values. -# These allow higher rates at low speed for responsiveness, lower rates at mid-speed -# for comfort, and very low rates at highway speed for stability. -# -# Control (Python) uses stricter windup than unwind so OP stays inside panda when apply_std -# picks the wrong table vs steer_angle_cmd_checks. Safety firmware uses looser symmetric ROCs -# (former “down” table for both up/down) — see ford.h FORD_LIMITS. -# Tests: test_ford.py ANGLE_RATE_* match ford.h, not the stricter BP_ANGLE_LIMITS up row. -_BP_ANGLE_RATE_UP = ([5, 16, 25], [0.0025, 0.0012, 0.00008]) -_BP_ANGLE_RATE_DOWN = ([5, 16, 25], [0.0025, 0.0014, 0.00018]) -BP_ANGLE_LIMITS = AngleSteeringLimits( - 0.02, # Max curvature for steering command, m^-1 - _BP_ANGLE_RATE_UP, - _BP_ANGLE_RATE_DOWN, -) - - -def apply_bp_device_mount(car_docs, CP): - """BluePilot: Select comma3 mount type per vehicle. - - Most Ford vehicles use the angled mount due to windshield angle. - Vehicles not in this list use the standard mount. - """ - from opendbc.car.ford.values import CAR, CarHarness, FordFlags - harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3 - if CP.carFingerprint in ( - CAR.FORD_BRONCO_SPORT_MK1, - CAR.FORD_MAVERICK_MK1, - CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, - CAR.FORD_ESCAPE_MK4_5, - CAR.FORD_MUSTANG_MACH_E_MK1, - CAR.FORD_RANGER_MK2, - CAR.FORD_EDGE_MK2, - ): - car_docs.car_parts = CarParts([Device.threex_angled_mount, harness]) - else: - car_docs.car_parts = CarParts([Device.threex, harness]) - diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py deleted file mode 100644 index b81870ff06..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py +++ /dev/null @@ -1,250 +0,0 @@ -"""Pure angle host-independent finite inputs and explicit override/blip reset.""" -import math -import unittest -from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs, Command -from opendbc.bluepilot_lateral.params import limits -from opendbc.bluepilot_lateral.tests import reference -from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, TRUCKS -from opendbc.bluepilot_lateral.can.ford import lateral, lka -from opendbc.can import CANPacker -from types import SimpleNamespace -from dataclasses import replace -from opendbc.bluepilot_lateral.core.trim import LaneCenterTrim, LaneGeometry, TrimConfig -from opendbc.bluepilot_lateral.hosts.starpilot import model_geometry -from openpilot.selfdrive.modeld.constants import ModelConstants - - -class TestAngleCore(unittest.TestCase): - def test_finite_permission_and_inactive_state_clear_pending_history(self): - for value in (math.nan, math.inf, -math.inf): - for field in ("speed", "desired_curvature", "measured_curvature", "steering_angle", "lateral_delay"): - core = AngleController() - inputs = Inputs(active=True, speed=15., measured_curvature=0., desired_curvature=.001) - self.assertEqual(core.step(replace(inputs, **{field: value})), Command()) - core = AngleController() - for _ in range(10): - self.assertTrue(core.step(Inputs(True, 15., 0., .001)).active) - self.assertEqual(core.step(Inputs(False, 15., 0., .001)), Command()) - self.assertEqual(core.path_last, 0.) - - def test_actual_equivalent_jerk_and_cap_both_signs_and_speeds(self): - for speed in (1., 9., 10., 15., 25., 40., 55.): - for direction in (-1, 1): - core = AngleController(low_gain=.95, high_gain=.95) - previous = 0. - for _ in range(60): - command = core.step(Inputs(True, speed, 0., direction * .002)) - if command.active: - equivalent = command.path_angle / speed / limits.GAIN_MIN - self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2)) - self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05) - previous = equivalent - else: - previous = 0. - - def test_human_turn_and_release_blips_are_neutral_then_bounded(self): - core = AngleController() - for _ in range(61): - result = core.step(Inputs(True, 15., 0., .001, steering_pressed=True, steering_angle=60.)) - self.assertFalse(result.active) - self.assertEqual(result.path_angle, 0.) - core = AngleController() - for _ in range(12): - core.step(Inputs(True, 15., 0., .001, steering_pressed=True)) - for _ in range(6): - self.assertEqual(core.step(Inputs(True, 15., 0., .001)), Command()) - resumed = core.step(Inputs(True, 15., 0., .001)) - self.assertTrue(resumed.active) - self.assertLess(abs(resumed.path_angle), .02) - - def test_retained_aol_keeps_active_after_release_and_tracking_error(self): - for stalled in (False, True): - core = AngleController(recovery_blips=False) - for tick in range(100): - inputs = Inputs(True, 15., 0., .006 if stalled else .001, - steering_pressed=not stalled and tick < 20) - result = core.step(inputs) - self.assertTrue(result.active, (stalled, tick)) - self.assertLessEqual(abs(result.path_angle / (15. * limits.GAIN_MIN)), limits.LATERAL_ACCEL / 15. ** 2) - self.assertEqual(core.blip_frames, 0) - - def test_retained_aol_preserves_human_turn_and_invalid_input_neutral(self): - core = AngleController(recovery_blips=False) - for _ in range(61): - result = core.step(Inputs(True, 15., 0., .001, steering_pressed=True, steering_angle=60.)) - self.assertEqual(result, Command()) - self.assertEqual(core.step(Inputs(True, 15., 0., math.nan)), Command()) - self.assertEqual(core.step(Inputs(False, 15., 0., .001)), Command()) - # Infeasible measured-curvature and actuator envelopes must still go neutral. - self.assertEqual(core.step(Inputs(True, 15., .1, .001)), Command()) - - def test_pinned_bp7_pure_angle_and_actual_control_bytes_with_documented_limits(self): - builders = reference.builders() - bus = SimpleNamespace(main=0) - differences = set() - packer = CANPacker("ford_lincoln_base_pt") - for identity in sorted(CLASSIC | CANFD): - gains = (.95, .95) if identity in TRUCKS else ((1., 1.05) if identity in CANFD else (1., 1.15)) - for speed in (5., 15., 25., 40.): - for direction in (-1, 1): - adapted, upstream = AngleController(low_gain=gains[0], high_gain=gains[1]), reference.Angle(identity) - for tick in range(60): - # Tiny tracking curves stay clear of all intentional envelope changes. - desired = direction * .00005 * (1. + .2 * math.sin(tick / 8.)) - inputs = Inputs(True, speed, desired, desired, - yaw_predictions=tuple(desired * speed for _ in ModelConstants.T_IDXS), - time_indices=tuple(ModelConstants.T_IDXS), lateral_delay=.15, - lane_change=tick >= 30, lane_direction=1 if direction < 0 else 2) - result = upstream.step(inputs) - command = adapted.step(inputs) - self.assertTrue(command.active) - self.assertAlmostEqual(command.path_angle, result.path_angle, places=12) - self.assertEqual(command.precision, result.precision_type) - fd = identity in CANFD - actual = lateral(packer, 0, command, canfd=fd, counter=tick % 16) - expected = (builders["create_lat_ctl2_msg"](packer, bus, 1, result.ramp_type, result.precision_type, - 0., -result.path_angle, 0., 0., tick % 16) if fd else - builders["create_lat_ctl_msg"](packer, bus, True, result.ramp_type, result.precision_type, - 0., -result.path_angle, 0., 0.)) - self.assertEqual(actual, expected) - old_shadow = builders["create_lka_msg"](packer, bus, True, SimpleNamespace(), True, upstream.controller.bp_kappa_cmd) - new_shadow = lka(packer, 0, command.shadow_curvature, tick % 8) - # bp7 packs an empty body as raw zeros; the strict native contract requires physical zeros. - self.assertEqual(old_shadow[1][:4], b"\x00\x00\x00\x00") - self.assertEqual(new_shadow[1][:4], b"\x00\x80\x08\x00") - differences.update(("physical_neutral_dbc_offsets", "versioned_side_channel")) - # Aggressive entry demonstrates the required stricter native-equivalent derivative/cap. - adapted, upstream = AngleController(low_gain=gains[0], high_gain=gains[1]), reference.Angle(identity) - previous = 0. - for _tick in range(8): - inputs = Inputs(True, speed, 0., direction * .02, - yaw_predictions=tuple(direction * .02 * speed for _ in ModelConstants.T_IDXS), - time_indices=tuple(ModelConstants.T_IDXS), lateral_delay=.15) - result, command = upstream.step(inputs), adapted.step(inputs) - if command.active: - equivalent = command.path_angle / max(speed, .1) / limits.GAIN_MIN - self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2)) - self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05) - previous = equivalent - if abs(command.path_angle - result.path_angle) > 1e-7: - differences.add("native_cap_jerk_deviation_packing") - else: - previous = 0. - self.assertEqual(differences, {"physical_neutral_dbc_offsets", "versioned_side_channel", "native_cap_jerk_deviation_packing"}) - - -class TestAngleTrim(unittest.TestCase): - @staticmethod - def geometry(direction=1., *, width=3.3, probability=.9): - xs = (0., 10., 40.) - return LaneGeometry(xs, (direction * .3,) * 3, xs, (-width / 2,) * 3, - xs, (width / 2,) * 3, (probability,) * 2, (.1,) * 2, True) - - @staticmethod - def model(g): - return SimpleNamespace(position=SimpleNamespace(x=g.position_x, y=g.position_y), - laneLines=[SimpleNamespace(x=(), y=()), SimpleNamespace(x=g.left_x, y=g.left_y), - SimpleNamespace(x=g.right_x, y=g.right_y)], - laneLineProbs=[0., *g.probabilities], laneLineStds=[0., *g.standard_deviations]) - - def test_disabled_trim_preserves_all_eleven_default_commands_and_wire_bytes(self): - packer = CANPacker("ford_lincoln_base_pt") - for identity in sorted(CLASSIC | CANFD): - low, high = (.95, .95) if identity in TRUCKS else ((1., 1.05) if identity in CANFD else (1., 1.15)) - for speed in (5., 15., 25., 40.): - for direction in (-1, 1): - baseline, supplied = AngleController(low_gain=low, high_gain=high), AngleController(low_gain=low, high_gain=high) - self.assertFalse(supplied.trim_config.enabled) - for tick in range(30): - i = Inputs(True, speed, 0., direction * .0002) - a, b = baseline.step(i), supplied.step(replace(i, geometry=self.geometry(direction))) - self.assertEqual(a, b) - self.assertEqual(lateral(packer, 0, a, canfd=identity in CANFD, counter=tick % 16), - lateral(packer, 0, b, canfd=identity in CANFD, counter=tick % 16)) - self.assertEqual(lka(packer, 0, a.shadow_curvature, tick % 8), lka(packer, 0, b.shadow_curvature, tick % 8)) - self.assertEqual(supplied.trim.correction, 0.) - - def test_fresh_geometry_matches_exact_currentdev_trim_and_model_fallback(self): - for speed in (5., 9., 12., 15., 25., 40.): - for direction in (-1, 1): - adapted, upstream = LaneCenterTrim(), reference.trim()() - config = TrimConfig(True, direction * .1, .25) - for tick in range(90): - g = self.geometry(direction, width=3.3 if tick < 30 or tick >= 60 else 1.5, - probability=.9 if tick < 60 else .3) - actual = adapted.update(.0001, speed, g, config, active=True, lane_change=False) - expected = upstream.update(.0001, self.model(g), speed, True, config.offset_m, config.gain, True, False) - self.assertAlmostEqual(actual, expected, places=14) - self.assertAlmostEqual(adapted.correction, upstream.correction, places=14) - projected = model_geometry(self.model(g), fresh=True) - self.assertEqual(projected, g) - self.assertIsNone(model_geometry(self.model(g), fresh=False)) - - def test_invalid_stale_driver_and_lane_change_suppress_trim_and_reset_history(self): - original = self.geometry() - config = TrimConfig(True, 0., .25) - for geometry in (replace(original, fresh=False), replace(original, position_x=(0., 0., 40.)), - replace(original, position_y=(0., math.nan, 0.)), replace(original, position_y=(0.,)), None): - trim = LaneCenterTrim() - self.assertNotEqual(trim.update(0., 15., original, config, active=True, lane_change=False), 0.) - self.assertEqual(trim.update(.001, 15., geometry, config, active=True, lane_change=False), .001) - self.assertEqual(trim.correction, 0.) - for changes in ({"active": False}, {"lane_change": True}, {"steering_pressed": True}): - trim = LaneCenterTrim() - trim.update(0., 15., original, config, active=True, lane_change=False) - flags = {"active": True, "lane_change": False, "steering_pressed": False} - flags.update(changes) - self.assertEqual(trim.update(.001, 15., original, config, **flags), .001) - self.assertEqual(trim.correction, 0.) - for invalid in (TrimConfig(True, math.nan, .25), TrimConfig(True, 0., math.inf), TrimConfig(True, .6, .25)): - self.assertEqual(LaneCenterTrim().update(.001, 15., original, invalid, active=True, lane_change=False), .001) - # Bad lane-only references fall back to the model+offset, not a stale lane correction. - bad = replace(original, probabilities=(math.nan, .9)) - without_lines = replace(original, left_x=(), right_x=()) - a, b = LaneCenterTrim(), LaneCenterTrim() - self.assertEqual(a.update(0., 15., bad, TrimConfig(True, .1, .25), active=True, lane_change=False), - b.update(0., 15., without_lines, TrimConfig(True, .1, .25), active=True, lane_change=False)) - core = AngleController(trim_config=config) - core.step(Inputs(True, 15., 0., 0., geometry=original)) - self.assertNotEqual(core.trim.correction, 0.) - self.assertEqual(core.step(Inputs(False, 15., 0., 0., geometry=original)), Command()) - self.assertEqual(core.trim.correction, 0.) - - def test_enabled_trim_remains_inside_final_curvature_jerk_and_shadow_bounds(self): - for speed in (9., 10., 15., 25., 40.): - for direction in (-1, 1): - core = AngleController(low_gain=.95, high_gain=.95, trim_config=TrimConfig(True, direction * .5, 1.)) - previous, previous_trim, previous_active = 0., 0., False - for _tick in range(100): - command = core.step(Inputs(True, speed, 0., direction * .02, geometry=self.geometry(-direction))) - if command.active and previous_active: - self.assertLessEqual(abs(core.trim.correction - previous_trim), .00015 + 1e-12) - previous_trim, previous_active = core.trim.correction, command.active - if command.active: - equivalent = command.path_angle / speed / limits.GAIN_MIN - self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2)) - self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05) - self.assertLessEqual(abs(command.shadow_curvature), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2)) - # Shadow comes from the final path; no independent trim/shadow command can bypass bounds. - ratio = command.path_angle / (speed * command.shadow_curvature) if command.shadow_curvature else 1. - self.assertTrue(limits.GAIN_MIN - 1e-12 <= ratio <= limits.GAIN_MAX + 1e-12) - previous = equivalent - else: - previous = 0. - self.assertEqual(core.trim.correction, 0.) - - -class TestAngleInputParity(unittest.TestCase): - def test_missing_model_fallback_matches_pinned_bp7(self): - for identity in sorted(CLASSIC | CANFD): - gains = (.95, .95) if identity in TRUCKS else ((1., 1.05) if identity in CANFD else (1., 1.15)) - for speed in (5., 8.): - for direction in (-1, 1): - adapted = AngleController(low_gain=gains[0], high_gain=gains[1]) - upstream = reference.Angle(identity) - for _ in range(20): - inputs = Inputs(True, speed, direction * .001, direction * .001) - command, expected = adapted.step(inputs), upstream.step(inputs) - self.assertTrue(command.active) - self.assertAlmostEqual(command.path_angle, expected.path_angle, places=12) - self.assertEqual(command.precision, expected.precision_type) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_native.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_native.py deleted file mode 100644 index 0e2cbfc4cf..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_native.py +++ /dev/null @@ -1,307 +0,0 @@ -"""Public packed angle/native boundaries; uses the runner-selected safety library.""" -from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs, Command -from opendbc.bluepilot_lateral.can.ford import lateral, lka, checksum4 -from opendbc.safety.tests import common -from opendbc.safety.tests.libsafety import libsafety_py -from opendbc.car.structs import CarParams -from opendbc.can import CANPacker - - -class TestFordAngleNative(common.SafetyTestBase): - DBC = "ford_lincoln_base_pt" - SAFETY_MODEL = CarParams.SafetyModel.ford - SAFETY_PARAM = 128 - - def setUp(self): - super().setUp() - self.debug = self.safety.set_safety_hooks(CarParams.SafetyModel.allOutput, 0) == 0 - self.safety.set_safety_hooks(self.SAFETY_MODEL, self.SAFETY_PARAM) - self.safety.init_tests() - self.now = 1_000_000 - self.lka_count = 0 - self.control_count = 0 - self.word = self.SAFETY_PARAM - - def packet(self, msg): - addr, dat, bus = msg - return libsafety_py.make_CANPacket(addr, bus, dat) - - def rx(self, name, values, *, bus=0): - self.assertTrue(self._rx(self.packer.make_can_msg_safety(name, bus, values))) - - def warm(self, speed=15.0, curvature=0.0): - self.safety.set_timer(self.now) - for _ in range(6): - self.rx("BrakeSysFeatures", {"Veh_V_ActlBrk": speed * 3.6, "VehVActlBrk_D_Qf": 3}) - self.rx("EngVehicleSpThrottle2", {"Veh_V_ActlEng": speed * 3.6, "VehVActlEng_D_Qf": 3}) - self.rx("Yaw_Data_FD1", {"VehYaw_W_Actl": curvature * speed, "VehYawWActl_D_Qf": 3}) - self.rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 0}) - self.rx("DesiredTorqBrk", {"VehStop_D_Stat": 0}) - self.rx("Steering_Data_FD1", {"CcAslButtnCnclResPress": 0}) - if not self.safety.get_controls_allowed(): - self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 0}) - self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 5}) - self.assertTrue(self.safety.get_controls_allowed()) - - def reset_word(self, word): - self.safety.set_alternative_experience(0) - self.safety.set_safety_hooks(self.SAFETY_MODEL, word) - self.word = word - self.lka_count = self.control_count = 0 - self.now += 1_000_000 - self.warm() - - def shadow(self, command): - msg = lka(self.packer, 0, command.shadow_curvature, self.lka_count) - self.lka_count = (self.lka_count + 1) % 8 - return msg - - def control(self, command): - msg = lateral(self.packer, 0, command, canfd=bool(self.word & 2), counter=self.control_count) - self.control_count = (self.control_count + 1) % 16 - return msg - - def send(self, msg, expected=True): - self.assertEqual(bool(self._tx(self.packet(msg))), expected) - - def tick(self, command): - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - self.send(self.control(command)) - - def test_real_packer_physical_neutral_shadow_and_corruptions(self): - packer = CANPacker(self.DBC) - for word in (128, 130): - for direction in (-1, 1): - for fault in ("angle", "curvature", "checksum", "version", "counter", "replay"): - with self.subTest(word=word, direction=direction, fault=fault): - self.reset_word(word) - command = Command(True, direction * 0.005, direction * 0.005 / 15.0) - # Real RX and PCM edges in warm() establish permission, with no allowance setter. - self.assertTrue(self.safety.get_controls_allowed()) - good = lka(packer, 0, command.shadow_curvature, 0) - addr, data, bus = good - self.assertEqual((addr, bus), (0x3CA, 0)) - self.assertEqual(data[:4], b"\x00\x80\x08\x00") - self.assertEqual(((data[2] & 15) << 8) | data[3], 2048) - self.assertEqual((data[1] << 4) | (data[2] >> 4), 2048) - self.assertEqual(data[7] & 15, 1) - self.assertEqual(data[7] >> 4, checksum4(data)) - self.assertEqual(int.from_bytes(data[5:7], "big", signed=True), round(-command.shadow_curvature * 1e6)) - self.send(good) - self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=0)) - self.now += 50_000 - self.warm() - next_good = lka(packer, 0, command.shadow_curvature, 1) - mutated = bytearray(next_good[1]) - if fault == "angle": - mutated[3] ^= 1 - elif fault == "curvature": - mutated[1] ^= 1 - elif fault == "checksum": - mutated[7] ^= 16 - elif fault == "version": - mutated[7] ^= 1 - elif fault == "counter": - mutated[4] ^= 4 - if fault in ("angle", "curvature", "counter"): - mutated[7] = 1 | (checksum4(mutated) << 4) - bad = good if fault == "replay" else (addr, bytes(mutated), bus) - self.send(bad, False) - self.assertTrue(self.safety.get_controls_allowed()) - self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=1), False) - # A new valid announcement restores the same bounded path without relaxing native checks. - self.now += 50_000 - self.warm() - self.send(next_good) - self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=1)) - - def test_actual_core_packer_boundary_all_namespaces(self): - words = [128, 129, 130] - if self.debug: - words.append(131) - for word in words: - for direction in (-1, 1): - self.reset_word(word) - core = AngleController(low_gain=0.95, high_gain=0.95) - active_seen = False - for _ in range(40): - command = core.step(Inputs(True, 15.0, 0.0, direction * 0.02)) - active_seen |= command.active - self.tick(command) - self.assertTrue(active_seen) - self.tick(Command()) - - def test_cold_and_stale_shadow_require_new_valid_announcement(self): - command = Command(True, 0.005, 0.005 / 15.0) - self.warm() - self.send(self.control(command), False) - self.send(self.shadow(command)) - self.send(self.control(command)) - self.now += 60_001 - self.warm() - self.send(self.control(command), False) - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - self.send(self.control(command)) - - def test_invalid_shadow_version_checksum_and_dbc_bits_withdraw(self): - command = Command(True, 0.005, 0.005 / 15.0) - for index, mask in ((7, 1), (7, 16), (0, 32), (4, 2), (4, 32)): - self.reset_word(128) - valid = self.shadow(command) - addr, data, bus = valid - mutated = bytearray(data) - mutated[index] ^= mask - if index != 7: - mutated[7] = 1 | (checksum4(mutated) << 4) - self.send((addr, bytes(mutated), bus), False) - self.send(self.control(command), False) - self.send(valid) - self.send(self.control(command)) - - def test_replay_wrong_bus_and_length_cannot_refresh_shadow(self): - command = Command(True, 0.005, 0.005 / 15.0) - self.warm() - msg = self.shadow(command) - self.send(msg) - self.send(msg, False) - self.send(self.control(command), False) - for bus, size in ((2, 8), (0, 7), (0, 16)): - addr, dat, _ = self.shadow(command) - self.send((addr, (dat + b"\x00" * 8)[:size], bus), False) - self.now += 30_000 - self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 0}) - self.send(self.shadow(Command())) - self.now += 30_000 - self.warm() - self.send(self.shadow(command)) - self.send(self.control(command)) - - def test_native_actual_path_cannot_be_laundered_by_shadow(self): - for command in (Command(True, 0.0005, 0.0), Command(True, -0.0005, 0.0), Command(True, 0.5, 0.0), Command(True, -0.01, 0.001), - Command(True, 0.04, 0.002), Command(True, 0.10, 0.005)): - self.reset_word(128) - self.send(self.shadow(command)) - self.send(self.control(command), False) - # A rejected command does not seed an excessive subsequent ROC. - self.reset_word(128) - large = Command(True, 0.20, 0.01) - self.send(self.shadow(large)) - self.send(self.control(large), False) - self.now += 50_000 - self.warm() - small = Command(True, 0.005, 0.005 / 15.0) - self.send(self.shadow(small)) - self.send(self.control(small)) - - def test_permission_and_rx_withdrawals(self): - command = Command(True, 0.005, 0.005 / 15.0) - for source in ("brake", "gas", "cruise", "speed_quality", "yaw_checksum", "stale"): - self.reset_word(128) - self.send(self.shadow(command)) - self.send(self.control(command)) - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - if source == "brake": - self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 2, "CcStat_D_Actl": 5}) - elif source == "gas": - self.rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 50}) - elif source == "cruise": - self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 3}) - elif source == "speed_quality": - self.assertFalse(self._rx(self.packer.make_can_msg_safety("BrakeSysFeatures", 0, {"VehVActlBrk_D_Qf": 0}))) - elif source == "yaw_checksum": - packet = self.packer.make_can_msg_safety("Yaw_Data_FD1", 0, {"VehYawWActl_D_Qf": 3}) - packet[0].data[4] ^= 1 - self.assertFalse(self._rx(packet)) - else: - self.now += 200_001 - self.safety.set_timer(self.now) - self.send(self.control(command), False) - self.send(self.control(Command())) - - def test_fd_bad_crc_counter_and_mode_neutral_recovery(self): - self.reset_word(130) - command = Command(True, 0.005, 0.005 / 15.0) - self.send(self.shadow(command)) - good = self.control(command) - addr, dat, bus = good - bad = bytearray(dat) - bad[1] ^= 1 - self.send((addr, bytes(bad), bus), False) - self.send(good, False) # bad CRC withdrew shadow - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - self.send(self.control(Command())) # valid mode0 resynchronizes skipped counters - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - self.send(self.control(command)) - self.now += 50_000 - self.warm() - self.send(self.shadow(command)) - self.control_count = (self.control_count + 1) % 16 - self.send(self.control(command), False) - - def test_adjacent_profile_version_and_release_long_denials(self): - command = Command(True, 0.005, 0.005 / 15.0) - for word in (0, 2, 4, 8, 10, 12, 18, 32, 66, 132, 134, 136, 160, 194, 256): - self.reset_word(word) - announcement = self.shadow(command) - if word & 128: - self.send(announcement, False) - else: - self._tx(self.packet(announcement)) # Existing neutral metadata is not angle authority. - self.send(self.control(command), False) - self.reset_word(131) - expected = bool(self.debug) - self.send(self.shadow(command), expected) - self.send(self.control(command), expected) - - def test_rejected_metadata_does_not_erase_last_accepted_path_and_neutral_resync(self): - self.reset_word(128) - # At25m/s ROC is tight enough that erasing the accepted path would reject a same-path retry. - core = AngleController(low_gain=.95, high_gain=.95) - for _ in range(30): - self.now += 50_000 - self.warm(speed=25., curvature=.001) - command = core.step(Inputs(True, 25., -.001, -.001)) - self.send(self.shadow(command)) - self.send(self.control(command)) - self.assertGreater(abs(command.path_angle), .01) - self.now += 50_000 - self.warm(speed=25., curvature=.001) - good = self.shadow(command) - addr, data, bus = good - bad = bytearray(data) - bad[7] ^= 16 - self.send((addr, bytes(bad), bus), False) - self.send(good) - self.send(self.control(command)) - self.now += 50_000 - self.warm(speed=25., curvature=.001) - self.send(self.control(Command())) - self.now += 50_000 - self.warm(speed=25., curvature=.001) - recovery = AngleController(low_gain=.95, high_gain=.95).step(Inputs(True, 25., -.001, -.001)) - self.send(self.shadow(recovery)) - self.send(self.control(recovery)) - - def test_rt_rate_denial_expires_and_neutral_can_recover(self): - self.reset_word(130) - for _ in range(8): - self.send(self.control(Command())) - self.send(self.control(Command()), False) - self.now += 250_000 - self.warm() - self.send(self.control(Command())) - self.now += 50_000 - self.warm() - command = Command(True, .005, .005 / 15.) - self.send(self.shadow(command)) - self.send(self.control(command)) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_transport.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_transport.py deleted file mode 100644 index 08527e592e..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_transport.py +++ /dev/null @@ -1,237 +0,0 @@ -"""Mach-E route 0000000d: refresh before publication; neutral restarts from zero.""" -import pytest - -from collections import defaultdict -from types import SimpleNamespace - -from opendbc.car import structs, Bus -from opendbc.car.ford.values import CAR -from opendbc.car.ford.carcontroller import CarController -from opendbc.bluepilot_lateral import BluePilotLateral -from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs, Command - - -class CoreHost: - def __init__(self): - self.controller = AngleController(low_gain=1., high_gain=1.05, recovery_blips=False) - self.steering_rejected = False - self.steering_pressed = False - self.steering_angle = 0. - - def update(self, cc, cs): - active = cc.latActive and not self.steering_rejected - self.steering_rejected = False - return self.controller.step(Inputs(active, 32.6, 0., .0012, steering_pressed=self.steering_pressed, steering_angle=self.steering_angle)) - - -def setup(canfd=True): - cp = structs.CarParams(brand='ford', carFingerprint=CAR.FORD_MUSTANG_MACH_E_MK1 if canfd else CAR.FORD_FOCUS_MK4, - flags=3 if canfd else 2, pcmCruise=True, alternativeExperience=32, - transmissionType=structs.CarParams.TransmissionType.automatic, - steerControlType=structs.CarParams.SteerControlType.angle, - openpilotLongitudinalControl=True) - cp.safetyConfigs = [structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.ford, safetyParam=147 if canfd else 161)] - owner = CarController({Bus.pt:'ford_lincoln_base_pt'}, cp) - owner.bp_lat = BluePilotLateral(cp, host=CoreHost()) - cc = structs.CarControl(latActive=True) - state = structs.CarState(vEgo=32.6, vEgoRaw=32.6, canValid=True, - gearShifter=structs.CarState.GearShifter.drive) - state.cruiseState.available = True - cs = SimpleNamespace(out=state.as_reader(), buttons_stock_values=defaultdict(int), - acc_tja_status_stock_values=defaultdict(int), lkas_status_stock_values=defaultdict(int)) - return owner, cc.as_reader(), cs - - -def control_active(message): - return ((message[1][0] >> 4) if message[0] == 0x3D6 else (message[1][4] >> 2)) & 7 - - -def shadow_echo(owner, messages, now): - returned = [(address, data, bus | 0x80) for address, data, bus in messages if address == 0x3CA] - owner.bp_lat.observe_can([(now, returned)], 0) - - -def prime(owner, cc, cs): - for now in range(1_000_000_000, 2_000_000_000, 10_000_000): - _, messages = owner.update(cc, cs, now) - shadow_echo(owner, messages, now + 1) - assert not owner.bp_lat.step_lka(owner.packer, owner.CAN, now + 1) - assert owner.bp_lat.pending.active - assert abs(owner.bp_lat.host.controller.path_last) > .015 - - -def test_due_shadow_precedes_control_after_recorded_72ms_gap(): - owner, cc, cs = setup() - prime(owner, cc, cs) - bp = owner.bp_lat - now = max(bp.shadow_sent_ns, bp.control_sent_ns) + 72_000_000 - _, messages = owner.update(cc, cs, now) - angle = [m for m in messages if m[0] in (0x3CA, 0x3D6)] - assert [m[0] for m in angle] == [0x3CA, 0x3D6] - assert control_active(angle[-1]) == 1 - assert bp.announced_ns == now - - -def test_unrefreshable_stale_lease_neutral_clears_pending_actuator_history(): - owner, cc, cs = setup() - prime(owner, cc, cs) - bp = owner.bp_lat - now = max(bp.shadow_sent_ns, bp.control_sent_ns) + 72_000_000 - # A newly consumed late shadow echo still enforces its pacing delay. - bp.shadow_echo_pending = True - _, messages = owner.update(cc, cs, now) - angle = [m for m in messages if m[0] in (0x3CA, 0x3D6)] - assert [m[0] for m in angle] == [0x3D6] - assert control_active(angle[0]) == 0 - assert bp.pending == Command() - assert bp.host.controller.path_last == 0. - # A subsequent announcement cannot revive the old nonzero actuator. - seen = [] - for t in range(now + 10_000_000, now + 300_000_000, 10_000_000): - _, messages = owner.update(cc, cs, t) - shadow_echo(owner, messages, t + 1) - seen.extend(m for m in messages if m[0] == 0x3D6 and control_active(m)) - assert seen - sig = owner.packer.dbc.name_to_msg['LateralMotionControl2'].sigs['LatCtlPath_An_Actl'] - first_path = sig.get_raw_value(seen[0][1]) * sig.factor + sig.offset - assert abs(first_path) <= .005 - - -@pytest.mark.parametrize('canfd, word', [(True, 147), (False, 161)]) -def test_aol_native_accepts_due_refresh_and_stale_restart_without_relaxation(canfd, word): - from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent - from opendbc.safety.tests.libsafety import libsafety_py - native = TestFordAolDriverIntent() - native.setUp() - native.reset(word) - native.arm() - native.pump(30, speed=32.6, mask=1) - owner, cc, cs = setup(canfd) - active = 0 - try: - for tick in range(200): - native.pump(1, speed=32.6, mask=1) - now = (1_000_000 + native.tick * 10_000) * 1000 - if tick in (110, 150): - # No host update for 70ms, but native receives fresh vehicle sources. - native.pump(7, speed=32.6, mask=1) - now = (1_000_000 + native.tick * 10_000) * 1000 - if tick == 150: - owner.bp_lat.shadow_echo_pending = True - _, messages = owner.update(cc, cs, now) - for address, data, bus in messages: - if address not in (0x3CA, 0x3D6, 0x3D3): - continue - assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (tick, hex(address), data.hex()) - if address == 0x3CA: - owner.bp_lat.observe_can([(now + 1, [(address, data, bus | 0x80)])], 0) - active += address in (0x3D6, 0x3D3) and bool(control_active((address, data, bus))) - assert active > 20 - finally: - native.tearDown() - - -def test_rejected_shadow_is_not_an_ack_and_withdraws_next_control(): - owner, cc, cs = setup() - prime(owner, cc, cs) - bp = owner.bp_lat - now = max(bp.shadow_sent_ns, bp.control_sent_ns) + 72_000_000 - payload = bp.step_lka(owner.packer, owner.CAN, now)[0][1] - bp.observe_can([(now, [(0x3CA, payload, 0xC0)])], 0) - assert bp.host.steering_rejected - assert not bp.shadow_echo_pending - assert bp.shadow_return_payload is None - _, messages = owner.update(cc, cs, now) - controls = [m for m in messages if m[0] == 0x3D6] - assert controls and control_active(controls[0]) == 0 - assert bp.host.controller.path_last == 0. - - -def test_unrelated_or_out_of_order_returns_do_not_refresh_shadow_time(): - owner, cc, cs = setup() - prime(owner, cc, cs) - bp = owner.bp_lat - sent = max(bp.shadow_sent_ns, bp.control_sent_ns) + 72_000_000 - payload = bp.step_lka(owner.packer, owner.CAN, sent)[0][1] - bp.shadow_returned_ns = sent + 10_000_000 - bp.observe_can([(sent, [(0x3CA, payload, 0x80)])], 0) - bp.observe_can([(sent + 20_000_000, [(0x3CA, bytes(8), 0x80)])], 0) - assert not bp.shadow_echo_pending - assert bp.announced_ns == sent - - -def test_permission_withdrawal_does_not_publish_active_even_with_due_shadow(): - owner, cc, cs = setup() - prime(owner, cc, cs) - now = max(owner.bp_lat.shadow_sent_ns, owner.bp_lat.control_sent_ns) + 72_000_000 - _, messages = owner.update(structs.CarControl().as_reader(), cs, now) - controls = [m for m in messages if m[0] == 0x3D6] - assert controls and control_active(controls[0]) == 0 - assert owner.bp_lat.pending == Command() - - -def test_stale_transport_reset_preserves_continuous_manual_turn_pause(): - owner, cc, cs = setup() - prime(owner, cc, cs) - host = owner.bp_lat.host - host.steering_pressed = True - host.steering_angle = 60. - for _ in range(70): - command = host.update(cc, cs) - assert not command.active - assert host.controller.human_timer >= 3. - now = max(owner.bp_lat.shadow_sent_ns, owner.bp_lat.control_sent_ns) + 72_000_000 - owner.bp_lat.shadow_echo_pending = True - _, messages = owner.update(cc, cs, now) - assert not any(control_active(m) for m in messages if m[0] == 0x3D6) - assert host.controller.human_timer >= 3. - assert host.controller.press_last and host.controller.preturned - for t in range(now + 10_000_000, now + 500_000_000, 10_000_000): - _, messages = owner.update(cc, cs, t) - assert not any(control_active(m) for m in messages if m[0] == 0x3D6) - - -def test_shadow_waits_for_return_before_another_request(): - owner, _, _ = setup() - bp = owner.bp_lat - bp.pending = Command(True, .02, .001) - sent = 1_000_000_000 - first = bp.step_lka(owner.packer, owner.CAN, sent)[0] - assert not bp.step_lka(owner.packer, owner.CAN, sent + 30_000_000) - assert not bp.step_lka(owner.packer, owner.CAN, sent + 60_000_000) - bp.observe_can([(sent + 60_000_001, [(first[0], first[1], 0x80)])], 0) - assert not bp.step_lka(owner.packer, owner.CAN, sent + 70_000_000) - assert not bp.step_lka(owner.packer, owner.CAN, sent + 90_000_000) - second = bp.step_lka(owner.packer, owner.CAN, sent + 100_000_000)[0] - assert ((second[1][4] >> 2) & 7) == 1 - - -def test_native_rejected_shadow_retries_the_unaccepted_counter(): - from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent - from opendbc.safety.tests.libsafety import libsafety_py - native = TestFordAolDriverIntent() - native.setUp() - try: - native.reset(147) - owner, _, _ = setup() - bp = owner.bp_lat - bp.pending = Command(True, .02, .001) - sent = 1_000_000_000 - first = bp.step_lka(owner.packer, owner.CAN, sent)[0] - native.safety.set_timer(1_000_000) - assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(first[0], first[2], first[1])) - bp.observe_can([(sent + 1, [(first[0], first[1], 0x80)])], 0) - bp.step_lka(owner.packer, owner.CAN, sent + 10_000_000) - second = bp.step_lka(owner.packer, owner.CAN, sent + 40_000_000)[0] - # Independent MCU time reproduces two queued packets arriving only 12 ms apart. - native.safety.set_timer(1_012_000) - assert not native.safety.safety_tx_hook(libsafety_py.make_CANPacket(second[0], second[2], second[1])) - bp.observe_can([(sent + 40_000_001, [(second[0], second[1], 0xC0)])], 0) - assert bp.host.steering_rejected - assert not bp.step_lka(owner.packer, owner.CAN, sent + 50_000_000) - retry = bp.step_lka(owner.packer, owner.CAN, sent + 80_000_000)[0] - assert ((retry[1][4] >> 2) & 7) == ((second[1][4] >> 2) & 7) - native.safety.set_timer(1_080_000) - assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(retry[0], retry[2], retry[1])) - finally: - native.tearDown() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_bluepilot_upstream_copies.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_bluepilot_upstream_copies.py new file mode 100644 index 0000000000..4975fe256e --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_bluepilot_upstream_copies.py @@ -0,0 +1,102 @@ +"""BluePilot carries near-copies of these upstream functions. An upstream change to one of them syncs +in without a conflict and never reaches the copy, so each upstream body is pinned here by a hash +that ignores whitespace, comments and docstrings. When upstream changes one, this fails on the sync +PR: the PR that resolves it ports what applies to the BluePilot copy and updates the hash (#47).""" +import ast +import hashlib +import io +import re +import tokenize +import unittest +from pathlib import Path + +from . import host + +# the host's opendbc package (opendbc, iqdbc): the one the library is mounted in +HOST = Path(__file__).resolve().parents[2] + +# per host (tests/host.py NAME; sunnypilot and StarPilot share the package name opendbc): (upstream file, +# function) -> hash of its normalized body; the copy is named in the comment +PINNED = { + "sunnypilot": { # sunnypilot/opendbc + ("safety/lateral.h", "steer_curvature_cmd_checks"): "9b13985a8d01e88b", # ford_bp_curvature_cmd_checks, bluepilot_lateral/safety/lateral.h + ("car/ford/fordcan.py", "create_lka_msg"): "c7e38e4f0215dcf4", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl_msg"): "f00a87f31389cb91", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl2_msg"): "1653ca5622eccc77", # bluepilot_lateral/can/fordcan_ext.py + }, + "iqpilot": { # IQ.Pilot: Ford on the native-curvature names; its packer leaves the LMC2 checksum to fordcan.py + ("safety/lateral.h", "steer_native_curvature_cmd_checks"): "8088e24c40855caa", # ford_bp_curvature_cmd_checks, bluepilot_lateral/safety/lateral.h + ("car/ford/fordcan.py", "create_lka_msg"): "c7e38e4f0215dcf4", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl_msg"): "f00a87f31389cb91", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl2_msg"): "4147e71aa7c6c51f", # bluepilot_lateral/can/fordcan_ext.py + }, + # StarPilot: the lateral checks gate on lateral_controls_allowed() (its AOL; BP_HOST_LAT_ALLOWED), and its + # Transit adds active-LKA arguments to create_lka_msg and a stock-LMC passthrough to create_lat_ctl_msg, neither + # used on the cars BluePilot lateral runs on (hosts/starpilot/safety_param.py) + "starpilot": { + ("safety/lateral.h", "steer_curvature_cmd_checks"): "bac2b4c0de5b2a97", # ford_bp_curvature_cmd_checks, bluepilot_lateral/safety/lateral.h + ("car/ford/fordcan.py", "create_lka_msg"): "a3876bd67e579195", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl_msg"): "59eec05e601ac97e", # bluepilot_lateral/can/fordcan_ext.py + ("car/ford/fordcan.py", "create_lat_ctl2_msg"): "1653ca5622eccc77", # bluepilot_lateral/can/fordcan_ext.py + }, +} + + +def c_function(src: str, name: str) -> str: + """The definition of a top-level C function (not a prototype or a call), comments and whitespace removed.""" + src = re.sub(r"/\*.*?\*/|//[^\n]*", "", src, flags=re.S) + m = re.search(rf"^\w[^\n;]*\b{name}\([^;{{]*\{{", src, re.M) # starts in column 0, ends in "{" + assert m, f"{name} not defined" + depth = 0 + for i in range(m.end() - 1, len(src)): + depth += {"{": 1, "}": -1}.get(src[i], 0) + if depth == 0: + return re.sub(r"\s+", "", src[m.start():i + 1]) + raise AssertionError(f"{name}: unbalanced braces") + + +def py_function(src: str, name: str) -> str: + """A top-level Python function as its token strings, without comments, layout or docstring.""" + func = next(n for n in ast.parse(src).body if isinstance(n, ast.FunctionDef | ast.AsyncFunctionDef) and n.name == name) + doc = func.body[0] + doc_line = doc.lineno if isinstance(doc, ast.Expr) and isinstance(doc.value, ast.Constant) and isinstance(doc.value.value, str) else None + lines = src.splitlines(keepends=True)[func.lineno - 1:func.end_lineno] + skip = {tokenize.COMMENT, tokenize.NL, tokenize.NEWLINE, tokenize.INDENT, tokenize.DEDENT, tokenize.ENDMARKER} + toks = [t for t in tokenize.generate_tokens(io.StringIO("".join(lines)).readline) + if t.type not in skip and not (t.type == tokenize.STRING and t.start[0] + func.lineno - 1 == doc_line)] + return " ".join(t.string for t in toks) + + +def body_hash(path: str, name: str) -> str: + src = (HOST / path).read_text() + body = c_function(src, name) if path.endswith(".h") else py_function(src, name) + return hashlib.sha256(body.encode()).hexdigest()[:16] + + +class TestBluePilotUpstreamCopies(unittest.TestCase): + def test_upstream_bodies_unchanged(self): + if host.NAME not in PINNED: + self.skipTest(f"no pins for host {host.NAME}: add its upstream hashes to PINNED") + for (path, name), pinned in PINNED[host.NAME].items(): + with self.subTest(function=name): + self.assertEqual(body_hash(path, name), pinned, + f"upstream {name} ({path}) changed: port what applies to the BluePilot copy, then update the hash") + + def test_normalization_ignores_layout_but_not_code(self): + c = "int f(int a) { return a; }\n" + self.assertEqual(c_function(c, "f"), c_function("int f(int a)\n{\n // note\n return a; /* x */\n}\n", "f")) + self.assertNotEqual(c_function(c, "f"), c_function("int f(int a) { return a + 1; }\n", "f")) + self.assertEqual(c_function("int f(int a);\n" + c, "f"), c_function(c, "f")) # prototype skipped + self.assertEqual(c_function("void g(void) {\n if (f(1)) {\n }\n}\n" + c, "f"), c_function(c, "f")) # call skipped + self.assertEqual(c_function("int f(int a) {\n // a } in a comment\n return a;\n}\n", "f"), c_function(c, "f")) + + py = 'def f(a):\n """doc"""\n return a\n' + self.assertEqual(py_function(py, "f"), py_function('def f(a):\n """other doc"""\n\n return a # note\n', "f")) + self.assertNotEqual(py_function(py, "f"), py_function("def f(a):\n return a + 1\n", "f")) + self.assertEqual(py_function(py + "\ndef g():\n pass\n", "f"), py_function(py, "f")) # stops at the next def + self.assertEqual(py_function('def f(a):\n """doc"""\n# column-0 comment\n return a\n', "f"), py_function(py, "f")) + self.assertNotEqual(py_function("def f(a): return a\n", "f"), py_function("def f(a): return a + 1\n", "f")) # one-liner + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_core_imports.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_core_imports.py new file mode 100644 index 0000000000..c84e409208 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_core_imports.py @@ -0,0 +1,59 @@ +import ast +import unittest +from pathlib import Path + +LIBRARY = Path(__file__).resolve().parents[1] +# the host-independent parts: only hosts/ (an adapter per host) may know a host's types +CORE_DIRS = ("core", "can", "params", "safety") +# The library reaches its host's opendbc package only through relative imports (HOSTS.md, "Mounting the +# library"), so it runs under whatever name the host gives that package (opendbc, iqdbc). An absolute +# import of any of these anywhere in the library ties it to one host. +HOST_PACKAGES = ("opendbc", "iqdbc", "openpilot", "iqpilot", "cereal") +# host trees the core must not use, resolved against the host package root +FORBIDDEN_HOST_TREES = ("sunnypilot", "lvbs") + + +def _package(path): + """The module's package, as parts below the host package root: ['bluepilot_lateral', 'core'].""" + return ["bluepilot_lateral", *path.relative_to(LIBRARY).parts[:-1]] + + +def _imports(path): + """Every module an import names, absolute, under for the host package root. Relative imports + are resolved; importing a name from a package also yields package.name (from .. import hosts).""" + pkg = _package(path) + tree = ast.parse(path.read_text()) + for node in ast.walk(tree): + if isinstance(node, ast.Import): + yield from (alias.name for alias in node.names) + elif isinstance(node, ast.ImportFrom): + if node.level: + base = pkg[:len(pkg) - (node.level - 1)] + module = ".".join(["", *base, *([node.module] if node.module else [])]) + else: + module = node.module or "" + yield module + yield from (f"{module}.{alias.name}" for alias in node.names) + + +class TestCoreImports(unittest.TestCase): + def test_core_imports_no_host(self): + files = [p for d in CORE_DIRS for p in sorted((LIBRARY / d).rglob("*.py"))] + self.assertTrue(files) + forbidden = (".bluepilot_lateral.hosts", *(f".{t}" for t in FORBIDDEN_HOST_TREES)) + for path in files: + rel = path.relative_to(LIBRARY) + for module in _imports(path): + self.assertFalse(module.startswith(forbidden), f"{rel} imports {module}") + + def test_no_absolute_host_imports(self): + files = [p for p in sorted(LIBRARY.rglob("*.py")) if ".git" not in p.parts] + self.assertTrue(files) + for path in files: + rel = path.relative_to(LIBRARY) + for module in _imports(path): + self.assertFalse(module.split(".")[0] in HOST_PACKAGES, f"{rel} imports {module}: use a relative import") + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_host_types.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_host_types.py new file mode 100644 index 0000000000..25856bc6f0 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_host_types.py @@ -0,0 +1,50 @@ +import ast +import dataclasses +import sys +import unittest +from pathlib import Path + +from . import host +from .. import host_types + +HOST_TYPES = Path(host_types.__file__) + +# LateralInputs as it was when it lived in the structs.py hook (#63 moved it, fields unchanged): the +# host fills these, so a rename or a new default changes what every host sends +LATERAL_INPUTS = { + "valid": False, + "pathYawRate": [], "pathX": [], "pathY": [], + "laneLineLeftX": [], "laneLineLeftY": [], "laneLineRightX": [], "laneLineRightY": [], + "laneLineProbs": [], "laneLineStds": [], + "laneChangeState": 0, "laneChangeDirection": 0, + "steerRatio": 0.0, "stiffnessFactor": 0.0, "roll": 0.0, "angleOffsetDeg": 0.0, "lateralDelay": 0.0, +} + + +class TestHostTypes(unittest.TestCase): + def test_lateral_inputs_fields_and_defaults(self): + li = host_types.LateralInputs() + self.assertEqual([f.name for f in dataclasses.fields(li)], list(LATERAL_INPUTS)) + for name, default in LATERAL_INPUTS.items(): + self.assertEqual(getattr(li, name), default, name) + self.assertIs(type(getattr(li, name)), type(default), name) + + def test_list_defaults_are_not_shared(self): + a, b = host_types.LateralInputs(), host_types.LateralInputs() + a.pathX.append(1.0) + self.assertEqual(b.pathX, []) + + def test_host_car_control_carries_the_library_type(self): + self.assertIsInstance(host.CarControlX().lateralInputs, host_types.LateralInputs) + + def test_imports_only_the_standard_library(self): + # structs.py imports this module, so anything beyond the standard library risks an import cycle + tree = ast.parse(HOST_TYPES.read_text()) + modules = {alias.name for node in ast.walk(tree) if isinstance(node, ast.Import) for alias in node.names} + modules |= {node.module or "" for node in ast.walk(tree) if isinstance(node, ast.ImportFrom)} + for module in modules: + self.assertTrue(module.split(".")[0] in sys.stdlib_module_names, module) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_assist_data1.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_assist_data1.py new file mode 100644 index 0000000000..390105b1fe --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_assist_data1.py @@ -0,0 +1,49 @@ +"""The Lane_Assist_Data1 side channel (can/LANE_ASSIST_DATA1.md) is written on both sides: Python packs +it (can/fordcan_ext.py), the panda reads it (safety/ford.h). This pins the two to each other and to +bits the DBC leaves free.""" +import re +import unittest +from pathlib import Path + +from ...can.dbc import DBC +from ..can import fordcan_ext + +DECLARATIONS = Path(__file__).resolve().parents[1] / "safety" / "ford_declarations.h" +CURVATURE_TO_CAN = 50000 # curvature CAN units per 1/m (FORD_LIMITS .curvature_to_can) + + +def _c_define(text, name): + m = re.search(rf"#define {name}\s+(0x[0-9A-Fa-f]+|[0-9.]+)[Uf]?\b", text) + assert m, name + return float(m.group(1)) if "." in m.group(1) else int(m.group(1), 0) + + +def _dbc_bits(sig): + """Bit indices (byte * 8 + bit) a big-endian DBC signal occupies.""" + assert not sig.is_little_endian, sig.name # Lane_Assist_Data1's signals are all big-endian + bits, b = set(), sig.msb + for _ in range(sig.size): + bits.add(b) + b = b + 15 if b % 8 == 0 else b - 1 + return bits + + +class TestLaneAssistData1(unittest.TestCase): + def test_python_and_c_agree(self): + c = DECLARATIONS.read_text() + self.assertEqual(_c_define(c, "FORD_BP_LKA_ANGLE_MODE_BYTE"), fordcan_ext.BP_LKA_ANGLE_MODE_BYTE) + self.assertEqual(_c_define(c, "FORD_BP_LKA_ANGLE_MODE_MASK"), fordcan_ext.BP_LKA_ANGLE_MODE_MASK) + self.assertEqual(_c_define(c, "FORD_BP_LKA_SHADOW_MSB_BYTE"), fordcan_ext.BP_LKA_SHADOW_MSB_BYTE) + self.assertEqual(_c_define(c, "FORD_BP_LKA_SHADOW_LSB_BYTE"), fordcan_ext.BP_LKA_SHADOW_LSB_BYTE) + # the panda turns the raw value (scale BP_LKA_SHADOW_CURVATURE_SCALE) into curvature CAN units + self.assertAlmostEqual(_c_define(c, "FORD_BP_LKA_SHADOW_RAW_TO_CAN"), fordcan_ext.BP_LKA_SHADOW_CURVATURE_SCALE * CURVATURE_TO_CAN) + + def test_side_channel_bits_are_free_in_the_dbc(self): + used = set().union(*(_dbc_bits(s) for s in DBC("ford_lincoln_base_pt").msgs[970].sigs.values())) + ours = {fordcan_ext.BP_LKA_ANGLE_MODE_BYTE * 8 + i for i in range(8) if fordcan_ext.BP_LKA_ANGLE_MODE_MASK >> i & 1} + ours |= {byte * 8 + i for byte in (fordcan_ext.BP_LKA_SHADOW_MSB_BYTE, fordcan_ext.BP_LKA_SHADOW_LSB_BYTE) for i in range(8)} + self.assertFalse(ours & used, sorted(ours & used)) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_center_trim.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_center_trim.py new file mode 100644 index 0000000000..673b35616d --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_center_trim.py @@ -0,0 +1,307 @@ +# Unit tests for LaneCenterTrim (angle-mode lane centering / advanced lane positioning). +# +# Isolated from CarController/LateralAngleExt -- exercises the class directly against a +# minimal fake modelV2-shaped object. + +import unittest + +import numpy as np + +from ..core.lane_center_trim import LaneCenterTrim, _CORRECTION_ROC_PER_TICK + + +class _XY: + def __init__(self, x, y): + self.x = x + self.y = y + + +class _Position: + def __init__(self, x, y): + self.x = x + self.y = y + + +class _Model: + def __init__(self, lane_lines, probs, stds, pos_x, pos_y): + self.laneLines = lane_lines + self.laneLineProbs = probs + self.laneLineStds = stds # add stds + self.position = _Position(pos_x, pos_y) + + +def _good_model(lane_center_y=0.0, model_y=0.0, width=3.7, probs=(0.9, 0.9, 0.9, 0.9), stds=(0.1, 0.1, 0.1, 0.1)): + """Confident lane lines (probability + typical width) centered at lane_center_y.""" + xs = list(range(0, 60, 2)) + half = width / 2.0 + lane_lines = [ + _XY(xs, [lane_center_y - half - 3.7] * len(xs)), # far left (unused) + _XY(xs, [lane_center_y - half] * len(xs)), # ego left + _XY(xs, [lane_center_y + half] * len(xs)), # ego right + _XY(xs, [lane_center_y + half + 3.7] * len(xs)), # far right (unused) + ] + pos_x = list(range(0, 60, 2)) + pos_y = [model_y] * len(pos_x) + return _Model(lane_lines, list(probs), list(stds), pos_x, pos_y) + + +def _no_lanelines_model(model_y=0.0): + """No lane lines detected at all (e.g. center-stripe-only road) -- zero probability on both + ego lines. laneLines arrays still present/well-formed (as modelV2 always publishes them), + just untrustworthy.""" + return _good_model(model_y=model_y, probs=(0.0, 0.0, 0.0, 0.0)) + + +def _one_sided_model(model_y=0.0): + """Only the right lane line is confidently detected (e.g. curb on the left, no line there) -- + the real-world case this blend exists for.""" + return _good_model(model_y=model_y, probs=(0.9, 0.05, 0.9, 0.9)) + + +class TestLaneCenterTrim(unittest.TestCase): + V_EGO = 15.0 # m/s -- at/above the 9-15 m/s ramp top, full speed authority + + def setUp(self): + self.trim = LaneCenterTrim() + + def _run(self, model, kappa_cmd=0.0, enabled=True, offset=0.0, gain=1.0, lat_active=True, + lane_change=False, v_ego=None, iterations=1): + result = kappa_cmd + for _ in range(iterations): + result = self.trim.update(kappa_cmd, model, v_ego if v_ego is not None else self.V_EGO, + enabled, offset, gain, lat_active, lane_change) + return result + + def test_disabled_no_correction(self): + model = _good_model() + self._run(model, enabled=False, offset=1.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + def test_inactive_no_correction(self): + model = _good_model() + self._run(model, lat_active=False, offset=1.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + def test_lane_change_no_correction(self): + model = _good_model() + self._run(model, lane_change=True, offset=1.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + def test_no_model_no_correction(self): + self._run(None, offset=1.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + def test_zero_offset_centered_no_correction(self): + # Model already tracks lane center exactly; no error to correct. + model = _good_model(lane_center_y=0.0, model_y=0.0) + self._run(model, offset=0.0, iterations=50) + self.assertAlmostEqual(self.trim.correction, 0.0, places=6) + + def test_offset_converges_to_clipped_ceiling(self): + # A large offset saturates the raw correction against its safety ceiling; full gain and full + # speed authority means the filtered correction converges to that ceiling. + model = _good_model() + self._run(model, offset=5.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=4) + + def test_gain_scales_correction_linearly(self): + model = _good_model() + self._run(model, offset=5.0, gain=0.25, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.001, places=4) + + def test_negative_offset_flips_sign(self): + model = _good_model() + self._run(model, offset=-5.0, gain=0.5, iterations=500) + self.assertAlmostEqual(self.trim.correction, -0.002, places=4) + + def test_speed_ramp_zero_below_9ms(self): + model = _good_model() + self._run(model, offset=5.0, gain=1.0, v_ego=5.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + def test_speed_ramp_partial_between_9_and_15ms(self): + model = _good_model() + self._run(model, offset=5.0, gain=0.5, v_ego=12.0, iterations=500) + # speed_factor at 12 m/s is 0.5 on the [9, 15] -> [0, 1] ramp + self.assertAlmostEqual(self.trim.correction, 0.001, places=4) + + def test_added_to_kappa_cmd(self): + model = _good_model() + result = self._run(model, kappa_cmd=0.01, offset=5.0, gain=1.0, iterations=500) + self.assertAlmostEqual(result, 0.01 + 0.004, places=4) + + def test_reset_zeroes_filter(self): + model = _good_model() + self._run(model, offset=5.0, gain=1.0, iterations=200) + self.assertNotEqual(self.trim.correction, 0.0) + self.trim.reset() + self.assertEqual(self.trim.correction, 0.0) + + def test_disable_mid_stream_resets(self): + model = _good_model() + self._run(model, offset=5.0, gain=1.0, iterations=200) + self.assertNotEqual(self.trim.correction, 0.0) + self._run(model, enabled=False, offset=5.0, gain=1.0, iterations=1) + self.assertEqual(self.trim.correction, 0.0) + + # --- Model-position fallback / blend (the point of this class) --- + + def test_no_lanelines_still_applies_offset(self): + # The scenario this exists for: center-stripe-only road, no lane line on the curbed side. + # Bad detection must NOT zero out the trim -- the offset bias should still apply, riding on + # the model's own path, exactly as if lane lines were centered and confident. + no_lines = _no_lanelines_model() + self._run(no_lines, offset=5.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=4) # same ceiling as good lanelines + + def test_one_sided_laneline_still_applies_offset(self): + # Only one ego line confidently detected -- min(probL, probR, width_tol) is dragged down by + # the missing side, same as fully-missing lines. + one_sided = _one_sided_model() + self._run(one_sided, offset=5.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=4) + + def test_no_lanelines_zero_offset_no_correction(self): + # No lanelines AND no user bias: target collapses to the model's own path, so there's truly + # nothing to correct (not even a spurious pull toward a phantom lane center). + no_lines = _no_lanelines_model() + self._run(no_lines, offset=0.0, gain=1.0, iterations=50) + self.assertAlmostEqual(self.trim.correction, 0.0, places=6) + + def test_confident_lanelines_pull_toward_laneline_center_even_with_zero_offset(self): + # Lane center is offset from the model's current path (model drifted left of true center); + # with full confidence and no user bias, the trim should still pull toward the laneline + # center -- this is the "full centering" behavior confidence unlocks. + model = _good_model(lane_center_y=2.0, model_y=0.0) # model 2m left of true lane center + self._run(model, offset=0.0, gain=1.0, iterations=500) + self.assertGreater(self.trim.correction, 0.0) # pulls right, toward center + + def test_low_confidence_ignores_laneline_center_offset(self): + # Same lane-position error as above, but lines are unreliable -- with zero user bias the + # trim must NOT invent a correction from a laneline center it shouldn't trust. + unreliable = _good_model(lane_center_y=2.0, model_y=0.0, probs=(0.1, 0.1, 0.1, 0.1)) + self._run(unreliable, offset=0.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.0, places=6) + + def test_implausible_width_falls_back_to_model_position(self): + # A width far outside anything the width-tolerance curve models still degrades confidence + # smoothly (not a crash / hard reject) and the offset-only fallback still applies. + weird_width = _good_model(width=15.0) + self._run(weird_width, offset=5.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=4) + + def test_no_model_position_hard_resets(self): + # Unlike laneline-only failures, a missing/invalid model.position leaves no baseline to + # offset from at all -- this is the one case that still hard-resets to zero. + broken = _Model(lane_lines=[_XY([], []), _XY([], []), _XY([], []), _XY([], [])], + probs=[0.9, 0.9, 0.9, 0.9],stds=[0.1, 0.1, 0.1, 0.1], pos_x=[], pos_y=[]) + self._run(broken, offset=5.0, gain=1.0, iterations=50) + self.assertEqual(self.trim.correction, 0.0) + + # --- Hardening: guards restored from the reference implementation --- + + def test_narrow_lane_not_centered(self): + # Two confident stripes 1.5 m apart (double-stripe repaint, gore-point paint) must NOT be + # treated as a lane to center in: the width-tolerance low edge zeroes the laneline scale, + # so with no user bias there is no correction at all. + narrow = _good_model(lane_center_y=1.0, model_y=0.0, width=1.5) + self._run(narrow, offset=0.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.0, places=6) + + def test_high_std_ignores_laneline_center(self): + # High positional uncertainty (rain/glare: the model is sure lines exist but not where) + # must drag centering authority to zero even when probabilities are high. + blurry = _good_model(lane_center_y=2.0, model_y=0.0, stds=(0.1, 0.6, 0.1, 0.1)) + self._run(blurry, offset=0.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.0, places=6) + + def test_nan_offset_is_inert(self): + # A corrupt float param must never reach the wire: NaN offset -> no correction, kappa_cmd + # passes through untouched. + result = self._run(_good_model(), kappa_cmd=0.01, offset=float("nan"), iterations=10) + self.assertEqual(self.trim.correction, 0.0) + self.assertEqual(result, 0.01) + + def test_nan_gain_is_inert(self): + result = self._run(_good_model(), kappa_cmd=0.01, offset=0.3, gain=float("nan"), iterations=10) + self.assertEqual(self.trim.correction, 0.0) + self.assertEqual(result, 0.01) + + def test_nan_laneline_probability_does_not_poison_correction(self): + # A single NaN laneline sample (e.g. a corrupt model frame) must not leak into min()/ + # np.interp() -- unlike the structural array checks above, NaN doesn't raise, so without an + # explicit finiteness guard it silently corrupts confidence (and therefore the correction) + # with a NaN that survives every later frame until a hard reset. Treat it the same as any + # other untrustworthy laneline sample: fall back to the model-position blend, not NaN. + nan_prob = _good_model(probs=(0.9, float("nan"), 0.9, 0.9)) + result = self._run(nan_prob, kappa_cmd=0.01, offset=0.3, iterations=10) + self.assertFalse(np.isnan(self.trim.correction)) + self.assertFalse(np.isnan(result)) + + def test_nan_laneline_std_does_not_poison_correction(self): + nan_std = _good_model(stds=(0.1, float("nan"), 0.1, 0.1)) + result = self._run(nan_std, kappa_cmd=0.01, offset=0.3, iterations=10) + self.assertFalse(np.isnan(self.trim.correction)) + self.assertFalse(np.isnan(result)) + + def test_nan_laneline_probability_recovers_without_reset(self): + # Converge under confident lanelines, take one NaN-probability frame, then confirm later + # good frames recover on their own -- the fallback for a bad sample should behave like any + # other low-confidence frame (self-healing), not latch like the pre-fix NaN corruption did + # (which required a hard reset: lane change / disengage / human-turn). + good = _good_model(lane_center_y=2.0, model_y=0.0) + self._run(good, offset=0.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=3) + + nan_prob = _good_model(lane_center_y=2.0, model_y=0.0, probs=(0.9, float("nan"), 0.9, 0.9)) + self._run(nan_prob, offset=0.0, gain=1.0, iterations=1) + self.assertFalse(np.isnan(self.trim.correction)) + + self._run(good, offset=0.0, gain=1.0, iterations=500) + self.assertFalse(np.isnan(self.trim.correction)) + self.assertAlmostEqual(self.trim.correction, 0.004, places=3) + + # --- Rate-of-change limit: smooths confidence transitions (merge lanes, lines dropping out) --- + + def test_correction_rate_limited_on_confidence_jump(self): + # Converge to a strong positive correction under confident, off-center lane lines. + centered_confident = _good_model(lane_center_y=2.0, model_y=0.0) + self._run(centered_confident, offset=0.0, gain=1.0, iterations=500) + before = self.trim.correction + self.assertAlmostEqual(before, 0.004, places=3) + + # Lane lines vanish entirely in the very next frame (e.g. crossing into a too-wide merge + # lane) while a negative user offset now drives the fallback target the opposite way -- a + # large, instantaneous target swing (+0.004 -> -0.004 worth of target). + no_lines = _no_lanelines_model(model_y=0.0) + after = self.trim.update(0.0, no_lines, self.V_EGO, True, -5.0, 1.0, True, False) + + # The correction must not have snapped toward the new target -- bounded to roughly one + # tick's worth of rate-of-change, regardless of how far the target actually moved. + self.assertLessEqual(abs(after - before), _CORRECTION_ROC_PER_TICK + 1e-9) + + def test_correction_eventually_converges_despite_rate_limit(self): + # The rate limit paces the transition but must not prevent it from completing. + centered_confident = _good_model(lane_center_y=2.0, model_y=0.0) + self._run(centered_confident, offset=0.0, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, 0.004, places=3) + + no_lines = _no_lanelines_model(model_y=0.0) + self._run(no_lines, offset=-5.0, gain=1.0, iterations=1000) + self.assertAlmostEqual(self.trim.correction, -0.004, places=3) + + def test_fallback_bias_is_position_independent_by_design(self): + # Pins the fallback semantics deliberately: with no lanelines, error == offset regardless of + # where the model path is (target moves with the baseline), i.e. the bias is a constant push, + # not a position controller. If this ever changes, it should change on purpose. + a = _no_lanelines_model(model_y=0.0) + b = _no_lanelines_model(model_y=-3.0) + self._run(a, offset=0.3, gain=1.0, iterations=500) + correction_a = self.trim.correction + self.trim.reset() + self._run(b, offset=0.3, gain=1.0, iterations=500) + self.assertAlmostEqual(self.trim.correction, correction_a, places=9) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lat_ctl2_checksum.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lat_ctl2_checksum.py new file mode 100644 index 0000000000..f9cf905841 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lat_ctl2_checksum.py @@ -0,0 +1,48 @@ +"""LateralMotionControl2's checksum (LatCtlPath_No_Cs) is computed in can/fordcan_ext.py, since a host's +packer may not fill Ford checksums. This checks it against the fields' raw values as the DBC lays them +out, and that the frame carries it.""" +import unittest + +from ...can.dbc import DBC +from ...can.packer import CANPacker +from ...car.ford.fordcan import CanBus +from ..can import fordcan_ext + +DBC_NAME = "ford_lincoln_base_pt" +CHECKSUM_FIELDS = ("LatCtl_D2_Rq", "LatCtlPath_No_Cnt", "LatCtlCurv_No_Actl", "LatCtlCrv_NoRate2_Actl", + "LatCtlPath_An_Actl", "LatCtlPathOffst_L_Actl") + + +def _raw(sig, dat): + """A big-endian DBC signal's raw value.""" + assert not sig.is_little_endian, sig.name + value, b = 0, sig.msb + for _ in range(sig.size): + value = (value << 1) | ((dat[b // 8] >> (b % 8)) & 1) + b = b + 15 if b % 8 == 0 else b - 1 + return value + + +class TestLatCtl2Checksum(unittest.TestCase): + def test_checksum_matches_the_dbc_fields(self): + msg = next(m for m in DBC(DBC_NAME).msgs.values() if m.name == "LateralMotionControl2") + packer = CANPacker(DBC_NAME) + CAN = CanBus(fingerprint={}) + cases = [ + (0, 0, 0., 0., 0., 0., 0), + (2, 3, 0.4, 0.02, 0.0123, 0.0005, 7), + (2, 1, -1.2, -0.3, -0.0199, -0.001, 15), + (1, 2, 5.11, 0.5235, 0.02094, 0.001023, 9), + ] + for mode, ramp, offset, angle, curvature, rate, counter in cases: + with self.subTest(mode=mode, curvature=curvature, counter=counter): + _, dat, _ = fordcan_ext.create_lat_ctl2_msg(packer, CAN, mode, ramp, 1, offset, angle, curvature, rate, counter) + checksum = 0 + for name in CHECKSUM_FIELDS: + value = _raw(msg.sigs[name], dat) + checksum += value + (value >> 8) + self.assertEqual(_raw(msg.sigs["LatCtlPath_No_Cs"], dat), ~checksum & 0xFF) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_inputs.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_inputs.py new file mode 100644 index 0000000000..e3971fc07d --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_inputs.py @@ -0,0 +1,121 @@ +import unittest + +from . import host +from ..core.carcontroller_ext import InputTracker +from ..core.values_ext import MODEL_T_IDXS + + +def _cc(valid=True): + cc = host.CarControlX() + li = cc.lateralInputs + li.valid = valid + li.pathYawRate = [0.01 * i for i in range(33)] + li.pathX = [float(i) for i in range(33)] + li.pathY = [0.1 * i for i in range(33)] + li.laneLineLeftX = li.laneLineRightX = [float(i) for i in range(33)] + li.laneLineLeftY = [-1.8] * 33 + li.laneLineRightY = [1.8] * 33 + li.laneLineProbs = [0.1, 0.9, 0.8, 0.1] + li.laneLineStds = [1.0, 0.1, 0.1, 1.0] + li.laneChangeState = 2 + li.laneChangeDirection = 1 + li.steerRatio = 15.5 + li.stiffnessFactor = 0.9 + li.roll = 0.02 + li.angleOffsetDeg = 0.3 + li.lateralDelay = 0.12 + return cc + + +class _FakeInputs: + """Bare object carrying only the attributes InputTracker.update touches.""" + def __init__(self): + self.model = None + self.lp = None + self.lateral_delay = 0.0 + self.VM = self + self.updated = None + + def update_params(self, x, sr): + self.updated = (x, sr) + + +class TestInputTracker(unittest.TestCase): + def test_model_view_has_modelv2_shape(self): + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + m = ext.model + self.assertEqual(len(m.orientationRate.z), 33) + self.assertEqual(m.position.y[10], 1.0) + self.assertEqual(m.laneLines[1].y[0], -1.8) + self.assertEqual(m.laneLines[2].y[0], 1.8) + self.assertEqual(m.laneLineProbs[2], 0.8) + self.assertEqual(m.laneLineStds[1], 0.1) + self.assertEqual(m.meta.laneChangeState, 2) + self.assertEqual(m.meta.laneChangeDirection, 1) + + def test_vehicle_params_and_delay(self): + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + self.assertEqual(ext.lp.angleOffsetDeg, 0.3) + self.assertEqual(ext.lp.roll, 0.02) + self.assertEqual(ext.lateral_delay, 0.12) + self.assertEqual(ext.updated, (0.9, 15.5)) + + def test_invalid_inputs_leave_previous_values(self): + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + InputTracker.update(ext, _cc(valid=False).lateralInputs) + self.assertEqual(ext.lateral_delay, 0.12) + self.assertIsNotNone(ext.model) + + def test_default_cc_is_not_valid(self): + self.assertFalse(host.CarControlX().lateralInputs.valid) + + def test_t_idxs_matches_modeld(self): + # openpilot ModelConstants: index_function(i, max_val=10.0) = 10 * (i/32)^2, 33 points + self.assertEqual(len(MODEL_T_IDXS), 33) + self.assertEqual(MODEL_T_IDXS[0], 0.0) + self.assertAlmostEqual(MODEL_T_IDXS[16], 2.5) + self.assertEqual(MODEL_T_IDXS[32], 10.0) + + def test_short_arrays_are_ignored_even_if_valid(self): + # fuzzed / partial host CarControl: valid=True but arrays not the modelV2 shape -> frame ignored + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + bad = _cc() + bad.lateralInputs.laneLineLeftY = [] + InputTracker.update(ext, bad.lateralInputs) + self.assertEqual(ext.model.laneLines[1].y[0], -1.8) # previous inputs kept + empty = _cc() + empty.lateralInputs.pathYawRate = [0.0] * 10 + InputTracker.update(_FakeInputs(), empty.lateralInputs) # must not raise + fresh = _FakeInputs() + InputTracker.update(fresh, empty.lateralInputs) + self.assertIsNone(fresh.model) + + def test_non_finite_inputs_are_ignored(self): + # a NaN or inf anywhere in the frame -> frame ignored, previous inputs kept + for field, index in (("laneLineProbs", 1), ("laneLineStds", 2), ("pathYawRate", 5), ("pathY", 0), ("laneLineRightY", 32)): + for bad_value in (float("nan"), float("inf")): + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + model, lp = ext.model, ext.lp + bad = _cc() + values = list(getattr(bad.lateralInputs, field)) + values[index] = bad_value + setattr(bad.lateralInputs, field, values) + InputTracker.update(ext, bad.lateralInputs) + self.assertIs(ext.model, model, field) + self.assertIs(ext.lp, lp, field) + for field in ("steerRatio", "lateralDelay"): + ext = _FakeInputs() + InputTracker.update(ext, _cc().lateralInputs) + bad = _cc() + setattr(bad.lateralInputs, field, float("nan")) + InputTracker.update(ext, bad.lateralInputs) + self.assertEqual(ext.lp.steerRatio, 15.5, field) + self.assertEqual(ext.lateral_delay, 0.12, field) + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_status.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_status.py new file mode 100644 index 0000000000..03933a2e4f --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_status.py @@ -0,0 +1,89 @@ +import unittest +from dataclasses import fields +from types import SimpleNamespace + +from ...can import CANPacker +from ...car.ford import fordcan +from ...car.ford.interface import CarInterface +from ...car.ford.values import CAR +from ..core.carcontroller_ext import BluePilotLateral +from ..core.lateral_curv_ext import LateralResult, PrimaryLateralControl +from ..core.values_ext import FordSafetyFlagsSP +from ..host_types import LateralStatus + +# the names BP7's controllerStateBP publisher reads off its car controller (bp_card_publisher.py): +# a rename or a dropped value breaks BP7's UI +STATUS_NAMES = ["lateralUncertainty", "angleRateLimited", "curvatureRateLimited", "curvatureDeviationLimited", + "humanTurnLateralPaused", "stallBlipActive", "disable_BP_lat_UI", "primary_lateral_control"] + + +def _bp(): + CP = CarInterface.get_non_essential_params(CAR.FORD_F_150_MK14) + return BluePilotLateral(CP, FordSafetyFlagsSP.BP_LATERAL, CANPacker("ford_lincoln_base_pt"), fordcan.CanBus(CP)) + + +def _result(uncertainty): + return LateralResult(apply_curvature=0.0, curvature_rate=0.0, path_offset=0.0, path_angle=0.0, ramp_type=0, + precision_type=1, lateralUncertainty=uncertainty) + + +class TestLateralStatus(unittest.TestCase): + def setUp(self): + self.bp = _bp() + self.bp.angle.bp_angle_rate_limited = True + self.bp.angle.bp_curvature_rate_limited = True + self.bp.angle.angle_human_turn_active = True + self.bp.angle.angle_stall_blip_active = True + self.bp.shared.bp_curvature_deviation_limited = True + self.bp.disable_BP_lat_UI = True + + def test_names(self): + self.assertEqual([f.name for f in fields(LateralStatus)], STATUS_NAMES) + + def test_angle_values_read_false_outside_angle_mode(self): + # as BP7 sets them: only angle mode runs the path_angle clip, the rate sim, the human-turn pause and the blip + self.bp.curv.primary_lateral_control = PrimaryLateralControl.curvature + s = self.bp.status() + self.assertEqual((s.angleRateLimited, s.curvatureRateLimited, s.humanTurnLateralPaused, s.stallBlipActive), + (False, False, False, False)) + self.assertTrue(s.curvatureDeviationLimited) # either mode's strategy sets it + self.assertTrue(s.disable_BP_lat_UI) + self.assertEqual(s.primary_lateral_control, 0) + + def test_angle_mode_values(self): + self.bp.curv.primary_lateral_control = PrimaryLateralControl.angle + s = self.bp.status() + self.assertEqual((s.angleRateLimited, s.curvatureRateLimited, s.humanTurnLateralPaused, s.stallBlipActive), + (True, True, True, True)) + self.assertEqual(s.primary_lateral_control, 1) + self.assertIs(type(s.primary_lateral_control), int) + + def test_each_value_from_its_own_source(self): + # one source at a time, so two swapped sources can't pass + sources = {"angleRateLimited": (self.bp.angle, "bp_angle_rate_limited"), + "curvatureRateLimited": (self.bp.angle, "bp_curvature_rate_limited"), + "humanTurnLateralPaused": (self.bp.angle, "angle_human_turn_active"), + "stallBlipActive": (self.bp.angle, "angle_stall_blip_active"), + "curvatureDeviationLimited": (self.bp.shared, "bp_curvature_deviation_limited"), + "disable_BP_lat_UI": (self.bp, "disable_BP_lat_UI")} + self.bp.curv.primary_lateral_control = PrimaryLateralControl.angle + for field, (obj, attr) in sources.items(): + for o, a in sources.values(): + setattr(o, a, False) + setattr(obj, attr, True) + s = self.bp.status() + self.assertEqual([f for f in sources if getattr(s, f)], [field], field) + + def test_uncertainty_from_whichever_strategy_ran(self): + self.bp.angle.angle_human_turn_active = self.bp.angle.angle_stall_blip_active = False + self.bp.angle.update_angle_strategy = lambda *a: _result(0.25) + self.bp.curv.update = lambda *a: _result(0.5) + cc = SimpleNamespace(latActive=True) + for mode, expected in ((PrimaryLateralControl.angle, 0.25), (PrimaryLateralControl.curvature, 0.5)): + self.bp.curv.primary_lateral_control = mode + self.bp.step_lateral(cc, None, None, 0.0) + self.assertEqual(self.bp.status().lateralUncertainty, expected, mode) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_param_store.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_param_store.py new file mode 100644 index 0000000000..73b61f1253 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_param_store.py @@ -0,0 +1,74 @@ +import unittest + +from ..params.param_store import ParamStore +from . import host +from ..core.lateral_curv_ext import LateralCurvExt, PrimaryLateralControl +from ..params.manifest import value +from ..core.values_ext import BP_LATERAL_PARAMS + + +def _cc(**kv): + cc = host.CarControlX() + for k, v in kv.items(): + p = host.CarControlX.Param() + p.key, p.value = k, v + cc.params.append(p) + return cc + + +class TestParamStore(unittest.TestCase): + def test_host_adapter_reads_cc_params(self): + ps = host.params_from_cc(_cc(disable_BP_lat_UI=b"1", LC_PID_gain_UI_curv=b"3.5")) + self.assertEqual((ps.get("disable_BP_lat_UI"), ps.get("LC_PID_gain_UI_curv"), ps.get("custom_profile_curv")), ("1", "3.5", None)) + + def test_reads_bool_int_float_like_params(self): + ps = ParamStore(dict(disable_BP_lat_UI=b"1", custom_profile_curv=b"2", LC_PID_gain_UI_curv=b"3.5").items()) + self.assertTrue(value(ps, "disable_BP_lat_UI")) + self.assertEqual(value(ps, "custom_profile_curv"), 2) + self.assertEqual(value(ps, "LC_PID_gain_UI_curv"), 3.5) + + def test_unset_keys_behave_like_params(self): + ps = ParamStore(dict().items()) + self.assertIsNone(ps.get("FordLowSpeedFactor_ang")) + # the manifest default applies to any key the fork did not publish, bools included + self.assertTrue(value(ps, "enable_human_turn_detection_curv")) + self.assertEqual(value(ps, "LC_PID_gain_UI_curv"), 1.0) + + def test_empty_params_give_registry_defaults(self): + """#6: with nothing published, the lateral runs the defaults the settings UI shows + (params_keys.h in the fork): human-turn detection on, low-curvature PID gain 1.0.""" + ext = type("Ext", (), {})() + LateralCurvExt.update_lateral_params(ext, ParamStore(dict().items())) + self.assertTrue(ext.enable_human_turn_detection_curv) + self.assertEqual(ext.LC_PID_gain_UI_curv, 1.0) + # a published value still wins + LateralCurvExt.update_lateral_params(ext, ParamStore(dict(enable_human_turn_detection_curv=b"0").items())) + self.assertFalse(ext.enable_human_turn_detection_curv) + + def test_lateral_mode_outside_the_enum_falls_back_to_curvature(self): + """#66: a mode other than 0 or 1 is curvature mode, the default, instead of raising every frame. + Text that isn't a number already was; 0 and 1 are unchanged.""" + ext = type("Ext", (), {})() + for published, expected in ((b"0", PrimaryLateralControl.curvature), (b"1", PrimaryLateralControl.angle), + (b"2", PrimaryLateralControl.curvature), (b"-1", PrimaryLateralControl.curvature), + (b"angle", PrimaryLateralControl.curvature)): + LateralCurvExt.update_lateral_params(ext, ParamStore(dict(FordPrefLateralControl=published).items())) + self.assertIs(ext.primary_lateral_control, expected, published) + + def test_bool_wire_format(self): + # the fork publishes bools as "1"/"0" + ps = ParamStore(dict(enable_lane_positioning_curv=b"1", enable_human_turn_detection_curv=b"0").items()) + self.assertIs(value(ps, "enable_lane_positioning_curv"), True) + self.assertIs(value(ps, "enable_human_turn_detection_curv"), False) + + def test_angle_side_values(self): + ps = ParamStore(dict(FordLowSpeedFactor_ang=b"1.2").items()) + self.assertEqual(value(ps, "FordLowSpeedFactor_ang"), 1.2) + + def test_key_list_is_consistent(self): + # the count is pinned against the manifest in test_settings_manifest.py + self.assertEqual(len(BP_LATERAL_PARAMS), len(set(BP_LATERAL_PARAMS))) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_platform_tables.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_platform_tables.py new file mode 100644 index 0000000000..20902d45e6 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_platform_tables.py @@ -0,0 +1,41 @@ +import unittest + +from ...car.ford.values import CAR +from ..core.values_ext import ( + ANGLE_GAIN_CANFD_BOF_CARS, ANGLE_GAIN_CANFD_SUV_CARS, FORD_HOST_ONLY_PLATFORMS, FORD_NO_PINION_PLATFORMS, + FORD_PINION_GEOMETRY_INDEX, FORD_PINION_WHEELBASE, +) + +TABLES = { + "FORD_PINION_GEOMETRY_INDEX": FORD_PINION_GEOMETRY_INDEX, + "FORD_PINION_WHEELBASE": FORD_PINION_WHEELBASE, + "FORD_NO_PINION_PLATFORMS": FORD_NO_PINION_PLATFORMS, + "ANGLE_GAIN_CANFD_BOF_CARS": ANGLE_GAIN_CANFD_BOF_CARS, + "ANGLE_GAIN_CANFD_SUV_CARS": ANGLE_GAIN_CANFD_SUV_CARS, +} + + +class TestPlatformTables(unittest.TestCase): + def test_names_are_known_platforms(self): + """Every per-platform row is keyed by a plain platform name, so a host without that platform + imports fine, and a name this host's CAR doesn't have must be a declared host-only one: a typo + would otherwise silently drop the platform's row. (A host that has a host-only platform, e.g. + BP7 with Mondeo, sees it in CAR, so this holds there too.)""" + here = {str(c) for c in CAR} + for table, names in TABLES.items(): + for name in names: + with self.subTest(table=table, name=name): + self.assertIs(type(name), str) + self.assertTrue(name in here or name in FORD_HOST_ONLY_PLATFORMS, name) + + def test_gain_groups_do_not_overlap(self): + """update_angle_params checks the body-on-frame group first, so a platform in both would + silently get those gains.""" + self.assertFalse(ANGLE_GAIN_CANFD_BOF_CARS & ANGLE_GAIN_CANFD_SUV_CARS) + + def test_no_pinion_platforms_have_no_row(self): + self.assertFalse(FORD_NO_PINION_PLATFORMS & set(FORD_PINION_GEOMETRY_INDEX)) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_safety_param_bits.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_safety_param_bits.py new file mode 100644 index 0000000000..317401dbea --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_safety_param_bits.py @@ -0,0 +1,86 @@ +import re +import unittest +from pathlib import Path + +from ...car import structs +from . import host +from ..core.values_ext import FordSafetyFlagsSP, FORD_PINION_GEOMETRY_INDEX, FORD_PINION_GEOMETRY_SHIFT +from ...car.ford.values import CAR + +SAFETY = Path(__file__).resolve().parents[1] / "safety" + + +def _c_const(text, name): + m = re.search(rf"{name}\s*=?\s*(\d+)U?\s*;?", text) + return int(m.group(1)) + + +class TestSafetyParamBits(unittest.TestCase): + """The SP safety-param bit layout is written on both sides (Python here, C in ford_bp_init). + Pin them to each other and to non-overlap.""" + + def test_python_and_c_agree(self): + ford_h = (SAFETY / "ford.h").read_text() + decl_h = (SAFETY / "ford_declarations.h").read_text() + self.assertEqual(_c_const(ford_h, "FORD_PARAM_SP_BP_LATERAL"), FordSafetyFlagsSP.BP_LATERAL) + self.assertEqual(_c_const(ford_h, "FORD_PARAM_SP_STEER_ANGLE_CURVATURE"), FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) + self.assertGreaterEqual(_c_const(decl_h, "#define FORD_PINION_GEOMETRY_COUNT"), max(FORD_PINION_GEOMETRY_INDEX.values())) + + def test_geometry_index_never_touches_the_lateral_bit(self): + # ford_bp_init masks the index with 0xF after a 1-bit shift: bits 1-4 + for index in FORD_PINION_GEOMETRY_INDEX.values(): + self.assertLessEqual(index, 0xF) + self.assertEqual((index << FORD_PINION_GEOMETRY_SHIFT) & FordSafetyFlagsSP.BP_LATERAL, 0) + self.assertEqual((index << FORD_PINION_GEOMETRY_SHIFT) & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE, 0) + self.assertEqual(FordSafetyFlagsSP.BP_LATERAL, 0x20) + + +class TestInitFordSafetyParamSp(unittest.TestCase): + """The startup bits the host adapter's car-init hook gives ford_bp_init.""" + + def _param_sp(self, params_dict, brand="ford", fingerprint=CAR.FORD_F_150_MK14): + CP = structs.CarParams() + CP.brand, CP.carFingerprint = brand, fingerprint + return host.startup_bits(CP, params_dict) + + def test_bp_lateral_on_unless_disabled(self): + self.assertEqual(self._param_sp({}), FordSafetyFlagsSP.BP_LATERAL) + for off in (True, 1, "1", "true", " True "): + self.assertEqual(self._param_sp({"disable_BP_lat_UI": off}), 0) + for on in (False, 0, None, "0", "false", ""): + self.assertEqual(self._param_sp({"disable_BP_lat_UI": on}), FordSafetyFlagsSP.BP_LATERAL) + + def test_pinion_sets_flag_and_geometry_index(self): + self.assertEqual(self._param_sp({"FordPrefSteerAngleCurvature": "1"}), + FordSafetyFlagsSP.BP_LATERAL | FordSafetyFlagsSP.STEER_ANGLE_CURVATURE | + (FORD_PINION_GEOMETRY_INDEX[CAR.FORD_F_150_MK14] << FORD_PINION_GEOMETRY_SHIFT)) + + def test_pinion_needs_bp_lateral_and_a_geometry_row(self): + self.assertEqual(self._param_sp({"FordPrefSteerAngleCurvature": "1", "disable_BP_lat_UI": "1"}), 0) + # the Edge MK2 (#30) has no geometry row and isn't a platform in this baseline + self.assertEqual(self._param_sp({"FordPrefSteerAngleCurvature": "1"}, fingerprint="FORD_EDGE_MK2"), + FordSafetyFlagsSP.BP_LATERAL) + + def test_other_brands_untouched(self): + self.assertEqual(self._param_sp({"FordPrefSteerAngleCurvature": "1"}, brand="toyota"), 0) + + +class TestLateralInputsRequired(unittest.TestCase): + """The host adapter tells the host to fill the per-frame lateralInputs and + settings for every Ford, whether BluePilot lateral is on or off, and for no other brand.""" + + def _required(self, params_dict, brand="ford"): + CP = structs.CarParams() + CP.brand, CP.carFingerprint = brand, CAR.FORD_F_150_MK14 + return host.lateral_inputs_required(CP, params_dict) + + def test_every_ford(self): + self.assertTrue(self._required({})) + self.assertTrue(self._required({"disable_BP_lat_UI": "1"})) + + def test_other_brands(self): + self.assertFalse(self._required({}, brand="toyota")) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_settings_manifest.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_settings_manifest.py new file mode 100644 index 0000000000..f94cc0a356 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_settings_manifest.py @@ -0,0 +1,100 @@ +import unittest + +from ..params import manifest +from ..params.manifest import SETTINGS, PER_FRAME_KEYS, startup_value, value +from ..params.param_store import ParamStore +from ..core.values_ext import BP_LATERAL_PARAMS + + +def _store(**kv): + return ParamStore(kv.items()) + + +PARAMS_KEYS_H = [ + '{"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}},', + '{"enable_human_turn_detection_curv", {PERSISTENT | BACKUP, BOOL, "1"}},', + '{"enable_lane_positioning_curv", {PERSISTENT | BACKUP, BOOL, "0"}},', + '{"enable_lane_full_mode_curv", {PERSISTENT | BACKUP, BOOL, "0"}},', + '{"enable_lane_positioning_ang", {PERSISTENT | BACKUP, BOOL, "0"}},', + '{"FordPrefLateralControl", {PERSISTENT | BACKUP, INT, "0"}},', + '{"custom_profile_curv", {PERSISTENT | BACKUP, INT, "0"}},', + '{"lane_change_factor_high_curv", {PERSISTENT | BACKUP, FLOAT, "0.85"}},', + '{"pc_blend_ratio_high_C_UI_curv", {PERSISTENT | BACKUP, FLOAT, "0.4"}},', + '{"pc_blend_ratio_low_C_UI_curv", {PERSISTENT | BACKUP, FLOAT, "0.4"}},', + '{"custom_path_offset_curv", {PERSISTENT | BACKUP, FLOAT, "0.0"}},', + '{"LC_PID_gain_UI_curv", {PERSISTENT | BACKUP, FLOAT, "1.0"}},', + '{"FordLowSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},', + '{"FordHighSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},', + '{"FordHighSpeedDampening_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},', + '{"lane_change_factor_high_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},', + '{"custom_path_offset_ang", {PERSISTENT | BACKUP, FLOAT, "0.0"}},', + '{"lane_centering_strength_ang", {PERSISTENT | BACKUP, FLOAT, "0.25"}},', + '{"FordPrefSteerAngleCurvature", {PERSISTENT | BACKUP, BOOL, "0"}},', +] + + +class TestSettingsManifest(unittest.TestCase): + def test_entries_are_consistent(self): + keys = [s.key for s in SETTINGS] + self.assertEqual(len(keys), len(set(keys))) + for s in SETTINGS: + with self.subTest(key=s.key): + self.assertTrue(s.type in (bool, int, float)) + self.assertIs(type(s.default), s.type) + self.assertTrue(s.read in (manifest.STARTUP, manifest.PER_FRAME, manifest.BOTH)) + self.assertTrue(s.label and s.description) + if s.range is not None: + self.assertLessEqual(s.range[0], s.default) + self.assertLessEqual(s.default, s.range[1]) + self.assertTrue(not s.clamp or s.range is not None) + + def test_per_frame_keys_are_what_hosts_publish(self): + # hosts (bp-sunnypilot's controlsd) publish BP_LATERAL_PARAMS in CarControlSP.params + self.assertEqual(BP_LATERAL_PARAMS, PER_FRAME_KEYS) + self.assertEqual((len(SETTINGS), len(PER_FRAME_KEYS)), (19, 18)) + + def test_one_read_rule(self): + """A published, parseable value is used; missing, empty or unparsable gives the default.""" + self.assertEqual(value(_store(lane_change_factor_high_curv=b"0.7"), "lane_change_factor_high_curv"), 0.7) + self.assertEqual(value(_store(), "lane_change_factor_high_curv"), 0.85) + self.assertEqual(value(_store(lane_change_factor_high_curv=b""), "lane_change_factor_high_curv"), 0.85) + self.assertEqual(value(_store(lane_change_factor_high_curv=b"x"), "lane_change_factor_high_curv"), 0.85) + self.assertEqual(value(_store(custom_profile_curv=b"1"), "custom_profile_curv"), 1) + self.assertIs(value(_store(enable_human_turn_detection_curv=b"0"), "enable_human_turn_detection_curv"), False) + self.assertIs(value(_store(), "enable_human_turn_detection_curv"), True) + self.assertIs(value(_store(enable_human_turn_detection_curv=b""), "enable_human_turn_detection_curv"), True) + + def test_only_the_angle_factors_are_clamped(self): + self.assertEqual(value(_store(FordLowSpeedFactor_ang=b"9"), "FordLowSpeedFactor_ang"), 1.5) + self.assertEqual(value(_store(lane_centering_strength_ang=b"-1"), "lane_centering_strength_ang"), 0.0) + self.assertEqual(value(_store(lane_change_factor_high_curv=b"9"), "lane_change_factor_high_curv"), 9.0) + + def test_startup_values(self): + # get_car's params dict holds whatever Params returned; a restored backup can hold text + for on in (True, 1, "1", "true", " True "): + self.assertIs(startup_value({"disable_BP_lat_UI": on}, "disable_BP_lat_UI"), True, on) + for off in (False, 0, None, "0", "false", "", "junk"): + self.assertIs(startup_value({"disable_BP_lat_UI": off}, "disable_BP_lat_UI"), False, off) + self.assertIs(startup_value({}, "FordPrefSteerAngleCurvature"), False) + + def test_params_keys_lines(self): + """Pins every key, type and default: these are bp-sunnypilot's params_keys.h lines (#6), so a typo + in a default can't change what the car runs unnoticed.""" + self.assertEqual(manifest.params_keys_lines(), PARAMS_KEYS_H) + + def test_clamped_ranges(self): + """The bounds the library clamps to (the angle-mode factors, as BluePilot's own fork does).""" + self.assertEqual({s.key: s.range for s in SETTINGS if s.clamp}, { + "FordLowSpeedFactor_ang": (0.5, 1.5), "FordHighSpeedFactor_ang": (0.5, 1.5), "FordHighSpeedDampening_ang": (0.25, 1.25), + "lane_change_factor_high_ang": (0.85, 1.5), "custom_path_offset_ang": (-0.5, 0.5), "lane_centering_strength_ang": (0.0, 1.0), + }) + + def test_read_kind_is_enforced(self): + with self.assertRaises(AssertionError): + value(_store(), "FordPrefSteerAngleCurvature") # startup only + with self.assertRaises(AssertionError): + startup_value({}, "FordLowSpeedFactor_ang") # per frame only + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tests/test_vendor_tools.py b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_vendor_tools.py new file mode 100644 index 0000000000..6f2847c364 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tests/test_vendor_tools.py @@ -0,0 +1,93 @@ +"""tools/vendor.sh and tools/check_vendored.sh (#24), on a scratch library repo: a fresh copy is the tagged commit +without .github/ plus VERSION, and the check passes on it and fails on an edited, added or removed file.""" +import shutil +import subprocess +import tempfile +import unittest +from pathlib import Path + +TOOLS = Path(__file__).resolve().parents[1] / "tools" + + +def git(repo, *args): + return subprocess.run(["git", "-C", str(repo), "-c", "user.name=t", "-c", "user.email=t@t", *args], + check=True, capture_output=True, text=True).stdout.strip() + + +@unittest.skipUnless(shutil.which("git") and shutil.which("bash"), "needs git and bash") +class TestVendorTools(unittest.TestCase): + def setUp(self): + self.tmp = Path(tempfile.mkdtemp()) + self.lib = self.tmp / "lib" + (self.lib / "tools").mkdir(parents=True) + for name in ("vendor.sh", "check_vendored.sh"): + shutil.copy(TOOLS / name, self.lib / "tools" / name) + (self.lib / "core").mkdir() + (self.lib / "core" / "a.py").write_text("A = 1\n") + (self.lib / ".github").mkdir() + (self.lib / ".github" / "ci.yml").write_text("ci\n") + (self.lib / ".gitattributes").write_text(".github export-ignore\n") + git(self.lib, "init", "-q") + git(self.lib, "add", "-A") + git(self.lib, "commit", "-q", "-m", "lib") + git(self.lib, "tag", "v9.9.9") + self.commit = git(self.lib, "rev-parse", "HEAD") + self.copy = self.tmp / "host" / "pkg" / "bluepilot_lateral" + + def tearDown(self): + shutil.rmtree(self.tmp) + + def run_tool(self, *args): + return subprocess.run(["bash", *args], capture_output=True, text=True) + + def vendor(self, ref="v9.9.9"): + r = self.run_tool(str(self.lib / "tools" / "vendor.sh"), ref, str(self.copy)) + self.assertEqual(r.returncode, 0, r.stderr) + + def check(self): + return self.run_tool(str(self.lib / "tools" / "check_vendored.sh"), str(self.copy)) + + def test_copy_is_the_tag_with_its_version(self): + self.vendor() + self.assertEqual((self.copy / "VERSION").read_text(), f"v9.9.9 {self.commit}\n") + self.assertEqual((self.copy / "core" / "a.py").read_text(), "A = 1\n") + self.assertFalse((self.copy / ".github").exists()) + r = self.check() + self.assertEqual(r.returncode, 0, r.stderr) + + def test_untagged_ref(self): + (self.lib / "core" / "b.py").write_text("B = 2\n") + git(self.lib, "add", "-A") + git(self.lib, "commit", "-q", "-m", "after the tag") + self.vendor("HEAD") + self.assertTrue((self.copy / "VERSION").read_text().startswith("untagged ")) + + def test_vendor_replaces_the_folder(self): + self.copy.mkdir(parents=True) + (self.copy / "stale.py").write_text("old\n") + self.vendor() + self.assertFalse((self.copy / "stale.py").exists()) + + def test_caches_are_ignored(self): + self.vendor() + (self.copy / "core" / "__pycache__").mkdir() + (self.copy / "core" / "__pycache__" / "a.cpython-312.pyc").write_bytes(b"\0") + self.assertEqual(self.check().returncode, 0) + + def test_drift_fails(self): + for change in ("edit", "add", "remove"): + with self.subTest(change=change): + self.vendor() + if change == "edit": + (self.copy / "core" / "a.py").write_text("A = 2\n") + elif change == "add": + (self.copy / "core" / "extra.py").write_text("X = 0\n") + else: + (self.copy / "core" / "a.py").unlink() + r = self.check() + self.assertEqual(r.returncode, 1, r.stdout) + self.assertRegex(r.stderr, "differs") + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tools/check_vendored.sh b/opendbc_repo/opendbc/bluepilot_lateral/tools/check_vendored.sh new file mode 100755 index 0000000000..fb3934c726 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tools/check_vendored.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +# BluePilot: does a host's copy of the library (tools/vendor.sh) still match the release in its VERSION file (#24)? +# Lists every added, removed or changed file and fails if there is one. Ignores VERSION and Python's caches. +# usage, from a clone of this repo that has the commit: tools/check_vendored.sh +set -euo pipefail +if [ $# -ne 1 ]; then + echo "usage: $0 " >&2 + exit 2 +fi +LIB="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +read -r TAG COMMIT < "$1/VERSION" +if ! git -C "$LIB" cat-file -e "$COMMIT^{commit}" 2>/dev/null; then + echo "$COMMIT ($TAG) isn't in $LIB: fetch it first" >&2 + exit 2 +fi +REF="$(mktemp -d)" +trap 'rm -rf "$REF"' EXIT +git -C "$LIB" archive "$COMMIT" | tar -x -C "$REF" + +if diff -rq -x VERSION -x __pycache__ -x '*.pyc' "$REF" "$1"; then + echo "$1 matches library $TAG ($COMMIT)" +else + echo "$1 differs from library $TAG ($COMMIT): vendor a release instead of editing the copy" >&2 + exit 1 +fi diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tools/make_hook_patch.sh b/opendbc_repo/opendbc/bluepilot_lateral/tools/make_hook_patch.sh new file mode 100755 index 0000000000..ac0e05a12b --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tools/make_hook_patch.sh @@ -0,0 +1,24 @@ +#!/usr/bin/env bash +# BluePilot: print the hook patch, everything a sunnypilot/opendbc checkout needs besides the library +# itself (HOSTS.md, #63): every file outside opendbc/bluepilot_lateral/ that differs from master, minus +# ford-op/opendbc's own .github. Fails unless the changed files are exactly the hook list's non-.github +# entries; the rest are new files (the safety test loader). +# usage, in a ford-op/opendbc checkout: opendbc/bluepilot_lateral/tools/make_hook_patch.sh [base, default master] > hooks.patch +set -euo pipefail +cd "$(git rev-parse --show-toplevel)" +BASE=${1:-master} +SPEC=(. ':!opendbc/bluepilot_lateral' ':!.github') + +changed=$(git diff --name-only --diff-filter=M "$BASE"...HEAD -- "${SPEC[@]}" | sort) +listed=$(python3 -c 'import sys; sys.path.insert(0, ".github"); from bluepilot_hooks import listed_hooks; print("\n".join(sorted(f for f in listed_hooks(open(".github/bluepilot-hooks.txt").read()) if not f.startswith(".github/"))))') +if [ "$changed" != "$listed" ]; then + echo "make_hook_patch: the files that differ from $BASE don't match .github/bluepilot-hooks.txt (< listed, > changed):" >&2 + diff <(echo "$listed") <(echo "$changed") >&2 || true + exit 1 +fi +other=$(git diff --name-only --diff-filter=DRCTUX "$BASE"...HEAD -- "${SPEC[@]}") +if [ -n "$other" ]; then + echo "make_hook_patch: only edits and new files can go in the patch; also changed: $other" >&2 + exit 1 +fi +git diff --binary "$BASE"...HEAD -- "${SPEC[@]}" diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tools/test_on_host.sh b/opendbc_repo/opendbc/bluepilot_lateral/tools/test_on_host.sh new file mode 100755 index 0000000000..e62950efb8 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tools/test_on_host.sh @@ -0,0 +1,35 @@ +#!/usr/bin/env bash +# BluePilot: run the library's tests on a sunnypilot/opendbc checkout (HOSTS.md, #63), in a temporary +# clone of it with this repo's library copied in and the hook patch applied. The host checkout itself +# isn't touched. Uses this repo's Python environment; the working copy goes first on PYTHONPATH. +# usage, in a ford-op/opendbc checkout: opendbc/bluepilot_lateral/tools/test_on_host.sh [base] +set -euo pipefail +if [ $# -lt 1 ]; then + echo "usage: $0 [base, default master]" >&2 + exit 2 +fi +HOST=$(cd "$1" && pwd) +BASE=${2:-master} +TOOLS=$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd) +LIB=$(cd "$TOOLS/.." && pwd) +REPO=$(cd "$LIB/../.." && pwd) +PY=${PYTHON:-$REPO/.venv/bin/python} +WORK=$(mktemp -d) +trap 'rm -rf "$WORK"' EXIT + +git clone --quiet --shared "$HOST" "$WORK/host" +git -C "$WORK/host" checkout --quiet "$(git -C "$HOST" rev-parse HEAD)" +# the library as committed, like the patch (a dirty working tree would test a mix that exists nowhere); +# archived from inside it, so a plain folder, a submodule and a subtree all work +mkdir -p "$WORK/host/opendbc/bluepilot_lateral" +git -C "$LIB" archive HEAD | tar -x -C "$WORK/host/opendbc/bluepilot_lateral" +(cd "$REPO" && "$TOOLS/make_hook_patch.sh" "$BASE") > "$WORK/hooks.patch" +git -C "$WORK/host" apply "$WORK/hooks.patch" +echo "host $(git -C "$HOST" rev-parse --short HEAD) + library + hook patch ($(grep -c '^diff --git' "$WORK/hooks.patch") files)" + +cd "$WORK/host" +export PYTHONPATH="$WORK/host" +echo "== library tests" +"$PY" -m unittest discover -s opendbc/bluepilot_lateral/tests -t . +echo "== safety tests, as upstream's test.sh finds them (the BluePilot ones through the loader)" +(cd opendbc/safety/tests && "$PY" -m unittest discover -s .) diff --git a/opendbc_repo/opendbc/bluepilot_lateral/tools/vendor.sh b/opendbc_repo/opendbc/bluepilot_lateral/tools/vendor.sh new file mode 100755 index 0000000000..8dc8ca7362 --- /dev/null +++ b/opendbc_repo/opendbc/bluepilot_lateral/tools/vendor.sh @@ -0,0 +1,21 @@ +#!/usr/bin/env bash +# BluePilot: copy a release of this library into a host's folder, for a host that takes no submodules (#24). +# The folder becomes exactly the library at (files the library doesn't have are removed), minus what +# .gitattributes marks export-ignore (.github/), plus VERSION: " " ("untagged" when isn't one). +# tools/check_vendored.sh tells whether the folder still matches. Never edit inside the folder: change the +# library, release it, vendor the new tag. +# usage, from a clone of this repo: tools/vendor.sh +set -euo pipefail +if [ $# -ne 2 ]; then + echo "usage: $0 " >&2 + exit 2 +fi +LIB="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +COMMIT="$(git -C "$LIB" rev-parse --verify "$1^{commit}")" +TAG="$(git -C "$LIB" describe --tags --exact-match "$COMMIT" 2>/dev/null || echo untagged)" + +rm -rf "$2" +mkdir -p "$2" +git -C "$LIB" archive "$COMMIT" | tar -x -C "$2" +echo "$TAG $COMMIT" > "$2/VERSION" +echo "$2: library $TAG ($COMMIT)" diff --git a/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE b/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE deleted file mode 100644 index 7dafa9443b..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE +++ /dev/null @@ -1,7 +0,0 @@ -Copyright (c) 2018, Comma.ai, Inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE.md b/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE.md deleted file mode 100644 index 650b6f55b3..0000000000 --- a/opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE.md +++ /dev/null @@ -1,21 +0,0 @@ -# Custom MIT License - -Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions: - -1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s). - -2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment: - "This software is licensed under a custom license requiring permission for use." - -3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment: - "This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use." - -4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - -Contact sunnypilot Support for permission requests. - ---- - -Haibin Wen, SUNNYPILOT LLC diff --git a/opendbc_repo/opendbc/car/ford/aol.py b/opendbc_repo/opendbc/car/ford/aol.py index dd894938e6..f92d3f5221 100644 --- a/opendbc_repo/opendbc/car/ford/aol.py +++ b/opendbc_repo/opendbc/car/ford/aol.py @@ -1,20 +1,18 @@ from opendbc.car import structs -from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags +from opendbc.car.ford.values import CAR, FORD_CANFD_CARS, FORD_CLASSIC_CARS, FordFlags, FordSafetyFlags AOL_EXPERIENCE = 32 -CLASSIC = frozenset((CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_FOCUS_MK4, - CAR.FORD_MAVERICK_MK1, CAR.FORD_EXPLORER_MK6)) -GENERIC_CANFD = frozenset((CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2)) MANUAL = int(FordSafetyFlags.MANUAL) -BASE_WORDS = frozenset((8, 9, 10, 11, 12, 13, 18, 19, 32, 33, 66, 67)) -ANGLE = 128 -ANGLE_BASE_WORDS = frozenset((18, 19, 32, 33, 66, 67)) -AOL_WORDS = (BASE_WORDS | frozenset(word | ANGLE for word in ANGLE_BASE_WORDS)) -AOL_WORDS |= frozenset(word | MANUAL for word in AOL_WORDS) -from opendbc.car.ford.pinion import ENABLE, SHIFT, PROFILES, base_word, configured -AOL_WORDS |= frozenset(word | ENABLE | index << SHIFT for index, words in PROFILES.items() - for word in tuple(AOL_WORDS) if (word & ~MANUAL) in words) +# bits 9-14 carry the BluePilot lateral library's startup bits (safety/modes/ford_param.h); bit 14 is its +# BluePilot lateral bit, set when the panda runs the library's steering checks +BP_PARAM_MASK = 0x7E00 +BP_LATERAL = 0x4000 +BASE_WORDS = frozenset((0, 1, 2, 3, 8, 9, 10, 11, 12, 13)) +AOL_WORDS = BASE_WORDS | frozenset(word | MANUAL for word in BASE_WORDS) + + +def profile_word(word): + return int(word) & ~BP_PARAM_MASK def aol_safety_config(cp): @@ -35,23 +33,9 @@ def base_safety_param(cp): manual = cp.transmissionType == structs.CarParams.TransmissionType.manual if cp.transmissionType not in (structs.CarParams.TransmissionType.automatic, structs.CarParams.TransmissionType.manual): return -1 - word = int(owner.safetyParam) - normalized = base_word(word) - if normalized != word and not configured(cp): - return -1 - word = normalized + word = profile_word(owner.safetyParam) if (word & MANUAL and not manual) or (manual and cp.alternativeExperience == AOL_EXPERIENCE and not word & MANUAL): return -1 - if word & ANGLE: - if cp.alternativeExperience == 0: - from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified - if not angle_qualified(cp): - return -1 - base = 18 if cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 else (66 if cp.carFingerprint in GENERIC_CANFD else 32) - return base | int(cp.openpilotLongitudinalControl) - if word not in AOL_WORDS: - return -1 - word &= ~ANGLE return word & ~MANUAL @@ -61,12 +45,10 @@ def qualified(cp, *, marked_only=False): cp.alternativeExperience not in ((32,) if marked_only else (0, 32))): return False flags = int(cp.flags) & ~int(FordFlags.HAS_BSM) - if cp.carFingerprint in CLASSIC and flags == 0: - base = 32 - elif cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and flags == int(FordFlags.CANFD): - base = 18 - elif cp.carFingerprint in GENERIC_CANFD and flags == int(FordFlags.CANFD): - base = 66 + if cp.carFingerprint in FORD_CLASSIC_CARS and flags == 0: + base = 0 + elif cp.carFingerprint in FORD_CANFD_CARS and flags == int(FordFlags.CANFD): + base = 2 elif cp.carFingerprint == CAR.FORD_EDGE_MK2 and flags == int(FordFlags.NEW_PORT | FordFlags.ALT_STEER_ANGLE): base = 8 elif cp.carFingerprint == CAR.FORD_MONDEO_MK5 and flags == int(FordFlags.NEW_PORT | FordFlags.CANFD): diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 8b631fc277..e7792e0676 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -1,24 +1,13 @@ import math import numpy as np from opendbc.can import CANPacker -from opendbc.bluepilot_lateral import BluePilotLateral -from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import BluePilotLateral, FrameInputs # BluePilot from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs from opendbc.car.ford import fordcan -from opendbc.car.ford.manual_turn import ManualTurnLatch -from opendbc.car.ford.aol import qualified as qualified_aol, base_safety_param +from opendbc.car.ford.aol import qualified as qualified_aol from opendbc.car.ford.stock_cruise import FordStockCruiseButton, qualified as stock_switch_qualified -from opendbc.car.ford.curvature_preview import blend_curvature -from opendbc.car.ford import mache_can -from opendbc.car.ford.extended_classic_can import create_extended_classic_lat_ctl_msg -from opendbc.car.ford.classic_lateral import create_controller as create_classic_controller, bounded_command as classic_bounded_command -from opendbc.car.ford.generic_canfd_can import create_extended_canfd_msg -from opendbc.car.ford.generic_canfd_lateral import ( - GenericCanfdLateralController, qualified as generic_canfd_qualified, bounded_command as generic_canfd_bounded_command, -) -from opendbc.car.ford.mache_lateral import MachELateralController, FordLateralResult, bounded_command, path_speed_window_supported, qualified as mache_qualified -from opendbc.car.ford.new_port_curvature import create_controller as create_new_port_curvature, bounded_command as new_port_bounded_command -from opendbc.car.ford.values import CarControllerParams, FordFlags, FordSafetyFlags, CAR +from opendbc.car.ford.aol import profile_word +from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -69,23 +58,13 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) - self.bp_lat = BluePilotLateral(CP) if angle_qualified(CP) else None - self.manual_turn = ManualTurnLatch() if CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not CP.flags & FordFlags.LKA_STEERING else None + self.bp = BluePilotLateral(CP, self.packer, self.CAN) # BluePilot + # StarPilot's openpilot side attaches the per-frame BluePilot inputs and settings (controller_extensions.py) + self.bluepilot_inputs = None self.stock_cruise_button = FordStockCruiseButton() - self.manual_turn_inputs = None - self.mache_lateral = MachELateralController(CP) if mache_qualified(CP) else None - self.mache_extended_announced = False - self.mache_ui_owned = False + self.lat_ui_owned = False self.hands_free_cluster_input = None self.cluster_extended_announced = False - self.generic_canfd = generic_canfd_qualified(CP) - self.classic_lateral = GenericCanfdLateralController(CP) if self.generic_canfd else create_classic_controller(CP) - self.new_port_curvature = create_new_port_curvature(CP) - self.classic_extended_announced = False - self.classic_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and - s.safetyParam & (FordSafetyFlags.CLASSIC_EXTENDED | FordSafetyFlags.GENERIC_CANFD_EXTENDED) for s in CP.safetyConfigs) - self.mache_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and - s.safetyParam & FordSafetyFlags.MACH_E_EXTENDED for s in CP.safetyConfigs) self.apply_curvature_last = 0 self.anti_overshoot_curvature_last = 0 self.accel = 0.0 @@ -94,7 +73,6 @@ class CarController(CarControllerBase): self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False - self.lkas_ui_sent_ns = self.acc_ui_sent_ns = 0 self.lead_distance_bars_last = None self.distance_bar_frame = 0 self.apply_lka_angle_last = 0. @@ -102,13 +80,12 @@ class CarController(CarControllerBase): def update(self, CC, CS, now_nanos): can_sends = [] + frame_inputs = self.bluepilot_inputs.update() if self.bluepilot_inputs is not None else FrameInputs() + bp_active = self.bp.update(frame_inputs, self.frame) # BluePilot if self.hands_free_cluster_input is not None: self.hands_free_cluster_input.update() if not CC.latActive: - self.mache_ui_owned = False - if (self.mache_lateral is not None or self.classic_lateral is not None or self.new_port_curvature is not None) and self.manual_turn_inputs is not None: - self.manual_turn_inputs.update() - self.manual_turn_inputs.apply_blend_settings(self) + self.lat_ui_owned = False actuators = CC.actuators hud_control = CC.hudControl @@ -144,92 +121,15 @@ class CarController(CarControllerBase): ### lateral control ### # send steer msg at 20Hz - if self.bp_lat is not None: - # Publish a due shadow before its dependent actuator, not after a stale-lease withdrawal. - can_sends += self.bp_lat.step_lka(self.packer, self.CAN, now_nanos) - lateral_sends = self.bp_lat.step_lateral(CC, CS, self.packer, self.CAN, self.frame, now_nanos) - can_sends += lateral_sends - self.apply_curvature_last = self.bp_lat.apply_curvature_last - self.mache_ui_owned |= bool(lateral_sends and self.bp_lat.announced.active and self.bp_lat.pending.active) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING: + if (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING: # Preserve the stock LMC heartbeat without requesting lateral action. can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, False, 0., 0., 0., 0., stock_lmc=CS.lateral_motion_control)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.mache_lateral is not None: - permission = getattr(self.manual_turn_inputs, "assist_permission", None) - self.mache_lateral.set_assist_permission(*(permission() if permission is not None else ((), False))) - inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None - if inputs is not None: - self.mache_lateral.set_inputs(*inputs) - else: - # Withdraw stale preview while retaining current actuator/manual-turn control. - self.mache_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True) - previous = self.mache_lateral.curvature_last - previous_path_angle = self.mache_lateral.path_angle_last - demanded = self.mache_lateral.update(CC, CS, actuators) if self.mache_extended_announced else FordLateralResult() - lateral = bounded_command(self.mache_lateral, demanded, previous, CS.out.vEgoRaw, previous_path_angle, - getattr(CS, "bluepilot_speed_window", None) if path_speed_window_supported(self.CP) else None) - self.mache_lateral_demand = demanded - self.mache_ui_owned |= bool(lateral.active) - self.apply_curvature_last = lateral.curvature - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(mache_can.create_lat_ctl2_msg( - self.packer, self.CAN, int(lateral.active), lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate, counter, -lateral.path_angle)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.classic_lateral is not None: - inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None - if inputs is not None: - self.classic_lateral.set_inputs(*inputs) - else: - self.classic_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True) - previous = self.classic_lateral.curvature_last - demanded = self.classic_lateral.update(CC, CS, actuators) if self.classic_extended_announced or self.generic_canfd else FordLateralResult() - bound = generic_canfd_bounded_command if self.generic_canfd else classic_bounded_command - lateral = bound(self.classic_lateral, demanded, previous, CS.out.vEgoRaw, - self.classic_lateral._current_curvature(CS)) - self.classic_lateral_demand = demanded - self.apply_curvature_last = lateral.curvature - if self.generic_canfd: - # Stay in the stock interface while inactive. Announce a mode change in - # the same batch, before the corresponding bounded steering command. - if self.classic_extended_announced != bool(lateral.active): - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN) if lateral.active else - fordcan.create_lka_msg(self.packer, self.CAN)) - self.classic_extended_announced = bool(lateral.active) - self.mache_ui_owned |= bool(lateral.active) - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(create_extended_canfd_msg( - self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate, counter)) - else: - can_sends.append(create_extended_classic_lat_ctl_msg( - self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type, - -lateral.curvature, -lateral.curvature_rate)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.new_port_curvature is not None: - inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None - self.new_port_curvature.set_inputs(*(inputs if inputs is not None else - (None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True))) - previous = self.new_port_curvature.curvature_last - demanded = self.new_port_curvature.update(CC, CS, actuators) - lateral = new_port_bounded_command(self.new_port_curvature, demanded, previous, CS.out.vEgoRaw, - self.new_port_curvature._current_curvature(CS)) - self.new_port_curvature_demand = demanded - self.apply_curvature_last = lateral.curvature - if self.CP.flags & FordFlags.CANFD: - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, int(lateral.active), 0., 0., - -lateral.curvature, 0., counter)) - else: - can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, lateral.active, 0., 0., -lateral.curvature, 0.)) + elif bp_active and (self.frame % CarControllerParams.STEER_STEP) == 0: # BluePilot + lat_sends, self.apply_curvature_last = self.bp.step_lateral(CC, CS, actuators, self.apply_curvature_last) + can_sends.extend(lat_sends) + self.lat_ui_owned |= CC.latActive elif (self.frame % CarControllerParams.STEER_STEP) == 0: - lateral_active = CC.latActive and not self.mache_profile and not self.classic_profile - manual_turn = False - if self.manual_turn is not None and self.manual_turn_inputs is not None: - detection_enabled, lane_change, model_ready = self.manual_turn_inputs.update() - manual_turn = self.manual_turn.update(CC, CS, float(actuators.curvature), detection_enabled, lane_change, model_ready) - if manual_turn: - lateral_active = False - self.apply_curvature_last = 0. # Bronco and some other cars consistently overshoot curv requests # Apply some deadzone + smoothing convergence to avoid oscillations if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): @@ -240,18 +140,14 @@ class CarController(CarControllerBase): # apply rate limits, curvature error limit, and clip to signal range current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - if self.manual_turn is not None and self.manual_turn_inputs is not None and lateral_active: - prediction = getattr(self.manual_turn_inputs, "preview_curvature", None) - preview = prediction(CS.out.vEgoRaw) if prediction is not None else None - if preview is not None: - apply_curvature = blend_curvature(apply_curvature, preview, current_curvature) # No blending at low speed due to lack of torque wind-up and inaccurate current curvature if CS.out.vEgoRaw > 9: apply_curvature = float(np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR)) apply_curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw, - 0., lateral_active, CarControllerParams.STEER_STEP) - self.apply_curvature_last = 0. if manual_turn else apply_curvature + 0., CC.latActive, CarControllerParams.STEER_STEP) + self.apply_curvature_last = apply_curvature + self.lat_ui_owned |= CC.latActive if self.CP.flags & FordFlags.CANFD: # TODO: extended mode @@ -260,18 +156,15 @@ class CarController(CarControllerBase): # steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes. # A detailed explanation on ford control can be found here: # https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706 - mode = 1 if lateral_active else 0 + mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter)) else: - can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, lateral_active, 0., 0., -self.apply_curvature_last, 0.)) + can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.)) # send lka msg at 33Hz - if (self.bp_lat is not None or (self.frame % CarControllerParams.LKA_STEP) == 0) and not any( - address == 0x3CA for address, _, _ in can_sends): - if self.bp_lat is not None: - can_sends += self.bp_lat.step_lka(self.packer, self.CAN, now_nanos) - elif self.CP.flags & FordFlags.LKA_STEERING: + if (self.frame % CarControllerParams.LKA_STEP) == 0: + if self.CP.flags & FordFlags.LKA_STEERING: source_age_ns = now_nanos - CS.lkas_available_ts_nanos lka_active = (CC.latActive and CS.lkas_available and 0 <= source_age_ns <= 100_000_000 and (CS.out.cruiseState.enabled or qualified_aol(self.CP, marked_only=True)) and @@ -295,15 +188,8 @@ class CarController(CarControllerBase): direction = 2 if self.apply_lka_angle_last >= 0. else 4 can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, lka_active, self.apply_lka_angle_last, direction, self.apply_lka_curvature_last, transit=True)) - elif self.mache_lateral is not None: - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN)) - self.mache_extended_announced = True - elif self.classic_lateral is not None: - if self.generic_canfd and not self.classic_extended_announced: - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) - else: - can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN)) - self.classic_extended_announced = True + elif bp_active: # BluePilot + can_sends.append(self.bp.step_lka(CC, hud_control)) else: can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) @@ -355,8 +241,10 @@ class CarController(CarControllerBase): ### ui ### # Acquire the display after the first active replacement, then retain it while # AOL stays active, including a manual-turn steering handoff (as on Dom). - aol_owned_ui = qualified_aol(self.CP, marked_only=True) and base_safety_param(self.CP) in (18, 19, 66, 67) - ui_enabled = CC.latActive and (not aol_owned_ui or self.mache_ui_owned) + aol_owned_ui = qualified_aol(self.CP, marked_only=True) and any( + s.safetyModel == structs.CarParams.SafetyModel.ford and (profile_word(s.safetyParam) & ~(1 | 256)) == 2 + for s in self.CP.safetyConfigs) + ui_enabled = CC.latActive and (not aol_owned_ui or self.lat_ui_owned) neutral_ui = (aol_owned_ui and self.CP.openpilotLongitudinalControl and not CC.latActive and not steer_alert and CS.out.canValid and not CS.out.canTimeout and not CS.out.vehicleSensorsInvalid and not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent and not CS.out.accFaulted and @@ -367,10 +255,7 @@ class CarController(CarControllerBase): if not aol_owned_ui or ui_enabled or neutral_ui: send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != ui_enabled) or (self.steer_alert_last != steer_alert) # send lkas ui msg at 1Hz or if ui state changes - lkas_due = (now_nanos - self.lkas_ui_sent_ns >= 1_000_000_000 if neutral_ui else - self.frame % CarControllerParams.LKAS_UI_STEP == 0) - if lkas_due or send_ui: - self.lkas_ui_sent_ns = now_nanos + if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, ui_enabled, steer_alert, hud_control, CS.lkas_status_stock_values)) # send acc ui msg at 5Hz or if ui state changes @@ -378,10 +263,8 @@ class CarController(CarControllerBase): send_ui = True self.distance_bar_frame = self.frame - acc_due = (now_nanos - self.acc_ui_sent_ns >= 100_000_000 if neutral_ui else - self.frame % CarControllerParams.ACC_UI_STEP == 0) - if acc_due or send_ui: - self.acc_ui_sent_ns = now_nanos + acc_ui_step = CarControllerParams.ACC_UI_STEP // 2 if neutral_ui else CarControllerParams.ACC_UI_STEP + if (self.frame % acc_ui_step) == 0 or send_ui: show_distance_bars = self.frame - self.distance_bar_frame < 400 hands_free_cluster = bool(self.cluster_extended_announced and self.hands_free_cluster_input is not None and self.hands_free_cluster_input.enabled) diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index dcd59184f9..b5a0d8ded6 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -6,7 +6,6 @@ from opendbc.can.dbc import DBC as CANDBC from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.ford.fordcan import CanBus -from opendbc.car.ford.mache_lateral import path_speed_window_supported from opendbc.car.ford.stock_cruise import FordStockCruiseButton from opendbc.car.ford.values import CAR, DBC, CarControllerParams, FordFlags from opendbc.car.interfaces import CarStateBase @@ -41,9 +40,6 @@ class CarState(CarStateBase): self.lkas_available = False self.lkas_available_ts_nanos = 0 self.lateral_motion_control = None - self.bluepilot_speed_samples = [0] * 6 if (CP.safetyConfigs and CP.safetyConfigs[-1].safetyParam & 128 or - path_speed_window_supported(CP)) else None - self.bluepilot_speed_window = None def get_car_gps(self): return self.car_gps_tracker.get() @@ -67,14 +63,6 @@ class CarState(CarStateBase): # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS - if self.bluepilot_speed_samples is not None: - # Match Panda's six raw ABS speed samples, including its millimetre/second rounding. - for value in cp.vl_all["BrakeSysFeatures"]["Veh_V_ActlBrk"]: - self.bluepilot_speed_samples.append(math.floor(value * CV.KPH_TO_MS * 1000 + .5)) - self.bluepilot_speed_samples = self.bluepilot_speed_samples[-6:] - samples = self.bluepilot_speed_samples - step = max(abs(b - a) for a, b in zip(samples, samples[1:], strict=False)) / 1000 - self.bluepilot_speed_window = (min(samples) / 1000, max(samples) / 1000, step) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 diff --git a/opendbc_repo/opendbc/car/ford/classic_lateral.py b/opendbc_repo/opendbc/car/ford/classic_lateral.py deleted file mode 100644 index 053813ee68..0000000000 --- a/opendbc_repo/opendbc/car/ford/classic_lateral.py +++ /dev/null @@ -1,51 +0,0 @@ -"""Shared source-derived classic Ford family; model transport stays outside opendbc. - -The reused extended strategy derives from BluePilot Ford work by Alan Polk and -contributors. Its original attribution and licenses remain in lateral_strategy.py, -CREDITS.md and THIRD_PARTY_NOTICES.md; this module only selects admitted callers. -""" - -from opendbc.car import structs -from opendbc.car.ford.aol import base_safety_param -from opendbc.car.ford.explorer_lateral import ExplorerLateralController, bounded_command as bounded_command, qualified as explorer_qualified -from opendbc.car.ford.lateral_strategy import FordLateralController -from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags - -GENERIC_CLASSIC_CARS = frozenset({CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, - CAR.FORD_FOCUS_MK4, CAR.FORD_MAVERICK_MK1}) -CLASSIC_EXTENDED_CARS = GENERIC_CLASSIC_CARS | {CAR.FORD_EXPLORER_MK6} - - -def qualified(cp): - if cp.carFingerprint == CAR.FORD_EXPLORER_MK6: - return explorer_qualified(cp) - if (cp.brand != "ford" or cp.carFingerprint not in GENERIC_CLASSIC_CARS or - cp.flags & ~int(FordFlags.HAS_BSM) or cp.passive or cp.dashcamOnly or cp.notCar or - cp.alternativeExperience not in (0, 32) or len(cp.safetyConfigs) not in (1, 2)): - return False - if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - cp.safetyConfigs[0].safetyParam != 0): - return False - safety = cp.safetyConfigs[-1] - expected = int(FordSafetyFlags.CLASSIC_EXTENDED | (FordSafetyFlags.LONG_CONTROL if cp.openpilotLongitudinalControl else 0)) - return safety.safetyModel == structs.CarParams.SafetyModel.ford and base_safety_param(cp) == expected - - -class GenericClassicLateralController(FordLateralController): - """Original generic liveDelay preview; no Explorer fixed-delay or Mach-E latch.""" - - def __init__(self, cp): - if not qualified(cp) or cp.carFingerprint not in GENERIC_CLASSIC_CARS: - raise ValueError("Generic classic controller requires the exact admitted family") - super().__init__(cp) - self.manual_turn_detected = False - - def _manual_turn(self, CC, CS, desired, driver_assisting): - self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting) - return self.manual_turn_detected - - -def create_controller(cp): - if not qualified(cp): - return None - return ExplorerLateralController(cp) if cp.carFingerprint == CAR.FORD_EXPLORER_MK6 else GenericClassicLateralController(cp) diff --git a/opendbc_repo/opendbc/car/ford/curvature_preview.py b/opendbc_repo/opendbc/car/ford/curvature_preview.py deleted file mode 100644 index 6b63937d31..0000000000 --- a/opendbc_repo/opendbc/car/ford/curvature_preview.py +++ /dev/null @@ -1,14 +0,0 @@ -"""Mach-E default model preview, before the upstream actuator limits.""" - -import math - - -def blend_curvature(desired: float, predicted: float, current: float) -> float: - if not all(math.isfinite(value) for value in (desired, predicted, current)): - return desired - blend = 0.4 - if desired * predicted <= 0.0: - blend = 0.0 - elif current * predicted > 0.0 and abs(current) > abs(desired) and abs(predicted) > abs(desired): - blend *= abs(desired) / abs(predicted) - return predicted * blend + desired * (1.0 - blend) diff --git a/opendbc_repo/opendbc/car/ford/explorer_lateral.py b/opendbc_repo/opendbc/car/ford/explorer_lateral.py deleted file mode 100644 index ae88e9650b..0000000000 --- a/opendbc_repo/opendbc/car/ford/explorer_lateral.py +++ /dev/null @@ -1,91 +0,0 @@ -"""Explorer/Aviator source-derived demand and exact native profile contract. - -The inherited strategy is adapted from BluePilot Ford work. Attribution and -license notices are preserved in lateral_strategy.py, CREDITS.md and -THIRD_PARTY_NOTICES.md. -""" - -from dataclasses import replace -from typing import Protocol - -import numpy as np - -from opendbc.car import structs -from opendbc.car.ford.aol import base_safety_param -from opendbc.car.ford.lateral_strategy import FordLateralController, FordLateralResult, FORD_CURVATURE_LIMITS, STEER_DT -from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags, FordSafetyFlags - - -class ExplorerLateralController(FordLateralController): - """Exact classic Explorer family, including the Aviator identity alias.""" - - def __init__(self, CP): - if CP.carFingerprint != CAR.FORD_EXPLORER_MK6 or CP.flags & ~int(FordFlags.HAS_BSM): - raise ValueError("Explorer demand strategy requires the exact classic family") - super().__init__(CP) - self.manual_turn_detected = False - - def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool: - self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting) - return self.manual_turn_detected - - def _curvature_lookahead(self) -> float: - # Original reached family override precedes liveDelay inspection. - return 0.20 - - -def qualified(CP) -> bool: - if (CP.brand != "ford" or CP.carFingerprint != CAR.FORD_EXPLORER_MK6 or - CP.flags & ~int(FordFlags.HAS_BSM) or CP.passive or CP.dashcamOnly or CP.notCar or - CP.alternativeExperience not in (0, 32) or len(CP.safetyConfigs) not in (1, 2)): - return False - if len(CP.safetyConfigs) == 2 and (CP.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - CP.safetyConfigs[0].safetyParam != 0): - return False - safety = CP.safetyConfigs[-1] - expected = int(FordSafetyFlags.EXPLORER_EXTENDED | (FordSafetyFlags.LONG_CONTROL if CP.openpilotLongitudinalControl else 0)) - return safety.safetyModel == structs.CarParams.SafetyModel.ford and base_safety_param(CP) == expected - - -class CurvatureOwner(Protocol): - manual_turn_detected: bool - curvature_last: float - path_angle_last: float - - -def bounded_command(owner: CurvatureOwner, demanded: FordLateralResult, - previous: float, speed: float, measured: float, *, absolute_cap: float | None = None) -> FordLateralResult: - """Keep classic demand inside the modern common ISO/jerk envelope.""" - limits = CarControllerParams.CURVATURE_LIMITS - cap = min(limits.CURVATURE_MAX, limits.MAX_LATERAL_ACCEL / max(speed, 1.0) ** 2) - if absolute_cap is not None: - cap = min(cap, absolute_cap) - modern_delta = limits.MAX_LATERAL_JERK / max(speed, 1.0) ** 2 * STEER_DT - source_up = FORD_CURVATURE_LIMITS.ANGLE_RATE_LIMIT_UP - source_down = FORD_CURVATURE_LIMITS.ANGLE_RATE_LIMIT_DOWN - rise = float(np.interp(speed, source_up[0], source_up[1])) - fall = float(np.interp(speed, source_down[0], source_down[1])) - # Same sign plus increasing magnitude uses rise; unwind/opposite uses fall. - source_low = previous - (fall if previous > 0.0 else rise) - source_high = previous + (fall if previous < 0.0 else rise) - low = max(-cap, previous - modern_delta, source_low) - high = min(cap, previous + modern_delta, source_high) - # Match modern recovery toward the actual measured-curvature error window. - # Outside the window, require directional progress without jumping into it. - if speed > 10.0: - error_low = measured - CarControllerParams.CURVATURE_ERROR - error_high = measured + CarControllerParams.CURVATURE_ERROR - if previous < error_low: - low = max(low, min(previous + modern_delta, error_low, cap)) - elif previous > error_high: - high = min(high, max(previous - modern_delta, error_high, -cap)) - else: - low = max(low, error_low) - high = min(high, error_high) - if not demanded.active or owner.manual_turn_detected or low > high: - command = FordLateralResult() - else: - command = replace(demanded, curvature=float(np.clip(demanded.curvature, low, high)), path_angle=0.0) - owner.curvature_last = command.curvature - owner.path_angle_last = 0.0 - return command diff --git a/opendbc_repo/opendbc/car/ford/extended_classic_can.py b/opendbc_repo/opendbc/car/ford/extended_classic_can.py deleted file mode 100644 index f7cffbcb87..0000000000 --- a/opendbc_repo/opendbc/car/ford/extended_classic_can.py +++ /dev/null @@ -1,25 +0,0 @@ -"""Source-derived classic Ford extended lateral constructor. - -Adapted from BluePilot fordcan_ext.py; see CREDITS.md and -THIRD_PARTY_NOTICES.md. This codec does not grant native authority. -""" - -from opendbc.car.ford.fordcan import CanBus - - -def create_extended_classic_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precision_type: int, - curvature: float, curvature_rate: float): - # 13-bit DBC endpoint: positive .001024 would wrap to negative .001024. - curvature_rate = max(-0.001024, min(curvature_rate, 0.00102375)) - values = { - "LatCtlRng_L_Max": 0, - "HandsOffCnfm_B_Rq": 0, - "LatCtl_D_Rq": 1 if active else 0, - "LatCtlRampType_D_Rq": ramp_type, - "LatCtlPrecision_D_Rq": precision_type, - "LatCtlPathOffst_L_Actl": 0.0, - "LatCtlPath_An_Actl": 0.0, - "LatCtlCurv_NoRate_Actl": curvature_rate, - "LatCtlCurv_No_Actl": curvature, - } - return packer.make_can_msg("LateralMotionControl", CAN.main, values) diff --git a/opendbc_repo/opendbc/car/ford/generic_canfd_can.py b/opendbc_repo/opendbc/car/ford/generic_canfd_can.py deleted file mode 100644 index aee70d36ce..0000000000 --- a/opendbc_repo/opendbc/car/ford/generic_canfd_can.py +++ /dev/null @@ -1,9 +0,0 @@ -"""Generic extended CAN FD wire endpoint, using the attributed shared codec.""" -from opendbc.car.ford.mache_can import create_lat_ctl2_msg - - -def create_extended_canfd_msg(packer, CAN, active, ramp, precision, curvature, rate, counter): - # Generic original lower rate negates to +.001024, outside the 11-bit field. - # Clamp the physical wire endpoint before packing instead of wrapping sign. - rate = max(-.001024, min(.001023, rate)) - return create_lat_ctl2_msg(packer, CAN, int(active), ramp, precision, curvature, rate, counter, 0.) diff --git a/opendbc_repo/opendbc/car/ford/generic_canfd_lateral.py b/opendbc_repo/opendbc/car/ford/generic_canfd_lateral.py deleted file mode 100644 index 204982e0d0..0000000000 --- a/opendbc_repo/opendbc/car/ford/generic_canfd_lateral.py +++ /dev/null @@ -1,46 +0,0 @@ -"""Exact generic CAN FD caller; no Mach-E error, path assist, or latch. - -The reused BluePilot-derived strategy retains its original attribution in -lateral_strategy.py, CREDITS.md and THIRD_PARTY_NOTICES.md. -""" -from opendbc.car import structs -from opendbc.car.ford.aol import base_safety_param -from opendbc.car.ford.explorer_lateral import bounded_command as shared_bound -from opendbc.car.ford.lateral_strategy import FordLateralController -from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags - -GENERIC_CANFD_CARS = frozenset({CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, - CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2}) -SOURCE_ACCEL_CAP = 3.0 - 9.81 * .06 - - -def qualified(cp): - if (cp.brand != "ford" or cp.carFingerprint not in GENERIC_CANFD_CARS or - not cp.flags & FordFlags.CANFD or cp.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM) or - cp.passive or cp.dashcamOnly or cp.notCar or cp.alternativeExperience not in (0, 32) or - len(cp.safetyConfigs) not in (1, 2)): - return False - if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - cp.safetyConfigs[0].safetyParam != 0): - return False - s = cp.safetyConfigs[-1] - expected = int(FordSafetyFlags.GENERIC_CANFD_EXTENDED | FordSafetyFlags.CANFD | - (FordSafetyFlags.LONG_CONTROL if cp.openpilotLongitudinalControl else 0)) - return s.safetyModel == structs.CarParams.SafetyModel.ford and base_safety_param(cp) == expected - - -class GenericCanfdLateralController(FordLateralController): - def __init__(self, cp): - if not qualified(cp): - raise ValueError("Generic CAN FD controller requires its exact source-owned profile") - super().__init__(cp) - self.manual_turn_detected = False - - def _manual_turn(self, CC, CS, desired, driver_assisting): - self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting) - return self.manual_turn_detected - - -def bounded_command(owner, demanded, previous, speed, measured): - return shared_bound(owner, demanded, previous, speed, measured, - absolute_cap=SOURCE_ACCEL_CAP / max(speed, 1.) ** 2) diff --git a/opendbc_repo/opendbc/car/ford/interface.py b/opendbc_repo/opendbc/car/ford/interface.py index fb1827ab72..8256af5bdb 100644 --- a/opendbc_repo/opendbc/car/ford/interface.py +++ b/opendbc_repo/opendbc/car/ford/interface.py @@ -4,11 +4,9 @@ from opendbc.car.carlog import carlog from opendbc.car.common.conversions import Conversions as CV from opendbc.car.ford.carcontroller import CarController from opendbc.car.ford.carstate import CarState -from opendbc.car.ford.classic_lateral import CLASSIC_EXTENDED_CARS -from opendbc.car.ford.generic_canfd_lateral import GENERIC_CANFD_CARS from opendbc.car.ford.fordcan import CanBus from opendbc.car.ford.radar_interface import RadarInterface -from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags, CAR +from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags, CAR, FORD_CANFD_CARS, FORD_CLASSIC_CARS from opendbc.car.interfaces import CarInterfaceBase TransmissionType = structs.CarParams.TransmissionType @@ -21,11 +19,6 @@ class CarInterface(CarInterfaceBase): DRIVABLE_GEARS = (structs.CarState.GearShifter.low, structs.CarState.GearShifter.manumatic) - def update(self, can_packets): - if self.CC is not None and self.CC.bp_lat is not None: - self.CC.bp_lat.observe_can(can_packets, self.CC.CAN.main) - return super().update(can_packets) - @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): # PCM doesn't allow acceleration near cruise_speed, @@ -59,18 +52,9 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = cfgs new_port = bool(ret.flags & FordFlags.NEW_PORT) - if candidate in GENERIC_CANFD_CARS: + if candidate in FORD_CANFD_CARS | FORD_CLASSIC_CARS: + # Explicit stock/OP-long selection on these families ret.alphaLongitudinalAvailable = True - ret.steerActuatorDelay = 0.22 - longitudinal_requested = bool(alpha_long) - elif candidate == CAR.FORD_MUSTANG_MACH_E_MK1: - ret.alphaLongitudinalAvailable = True - longitudinal_requested = bool(alpha_long) - elif candidate in CLASSIC_EXTENDED_CARS: - # Original shared classic ownership is explicit stock/OP-long selection. - # Actuator delay is live; modern LongControl already supplies the original P=0. - ret.alphaLongitudinalAvailable = True - ret.steerActuatorDelay = 0.22 longitudinal_requested = bool(alpha_long) else: ret.alphaLongitudinalAvailable = True if new_port else ret.radarUnavailable @@ -126,28 +110,4 @@ class CarInterface(CarInterfaceBase): ret.autoResumeSng = ret.minEnableSpeed == -1. ret.centerToFront = ret.wheelbase * 0.44 - classic_extended = ( - candidate in CLASSIC_EXTENDED_CARS and not (ret.flags & ~int(FordFlags.HAS_BSM)) - and not ret.dashcamOnly and ret.alternativeExperience == 0 - and ret.safetyConfigs[-1].safetyParam in (0, 1) - ) - if classic_extended: - ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CLASSIC_EXTENDED.value - - # The source-derived Mach-E owner has a separate native namespace. SecOC, - # unrelated static flags and alternative experiences retain no extension. - mach_e_extended = ( - candidate == CAR.FORD_MUSTANG_MACH_E_MK1 - and bool(ret.flags & FordFlags.CANFD) - and not (ret.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM)) - and not ret.dashcamOnly and ret.alternativeExperience == 0 - and ret.safetyConfigs[-1].safetyParam in (2, 3) - ) - if mach_e_extended: - ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.MACH_E_EXTENDED.value - if (candidate in GENERIC_CANFD_CARS and ret.flags & FordFlags.CANFD and - not ret.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM) and - not ret.dashcamOnly and not ret.passive and not ret.notCar and ret.alternativeExperience == 0): - ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.GENERIC_CANFD_EXTENDED.value - return ret diff --git a/opendbc_repo/opendbc/car/ford/lateral_strategy.py b/opendbc_repo/opendbc/car/ford/lateral_strategy.py deleted file mode 100644 index 336018eb87..0000000000 --- a/opendbc_repo/opendbc/car/ford/lateral_strategy.py +++ /dev/null @@ -1,758 +0,0 @@ -"""Ford lateral-control extensions. - -The extended curvature strategy, manual-turn detector, and related safety protocol are substantially -adapted from BluePilot's Ford work, principally by Alan Polk and additional contributors. The audited -bp-7.0 reference is e1d051d7ba270261b4455068bd68f1a58db15a4a; source attribution is retained in -CREDITS.md and THIRD_PARTY_NOTICES.md. StarPilot reorganized that work for its own architecture and has since -changed its tuning and lookahead behavior. - -See CREDITS.md for feature-level authorship and upstream commits, and THIRD_PARTY_NOTICES.md for the -published upstream license notices. Upstream contributors do not maintain this adaptation. -""" - -from collections import deque -from dataclasses import dataclass - -import numpy as np - -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL, structs -from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags, FordSafetyFlags -from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, apply_std_steer_angle_limits - - -# These rate-limit values descend from BluePilot's ``values_ext.py``, which carries the Haibin Wen -# and sunnypilot contributors copyright notice reproduced in THIRD_PARTY_NOTICES.md. -FORD_CURVATURE_LIMITS = AngleSteeringLimits( - 0.02, - ([5, 16, 25], [0.0025, 0.0012, 0.00008]), - ([5, 16, 25], [0.0025, 0.0014, 0.00018]), -) - -MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06 -STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL -CURVATURE_LOOKAHEAD_MIN = 0.20 -CURVATURE_LOOKAHEAD_MAX = 0.40 -MACH_E_HIGH_SPEED_LOOKAHEAD_EXTRA = 0.40 -MACH_E_HIGH_SPEED_LOOKAHEAD_START_SPEED = 16.0 -MACH_E_HIGH_SPEED_LOOKAHEAD_FULL_SPEED = 23.0 -MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.80 -MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA = 1.60 -MACH_E_LOW_SPEED_TURN_IN_START_SPEED = 2.0 -MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED = 3.0 -MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED = 12.0 -MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 15.0 -MACH_E_TURN_IN_MIN_CURVATURE = 0.002 -MACH_E_TURN_IN_FULL_CURVATURE = 0.008 -MACH_E_TURN_IN_LAG_CURVATURE = 0.006 -MACH_E_TURN_IN_PREVIEW_HOLD_SECONDS = 0.10 -MACH_E_TURN_IN_PREVIEW_HOLD_MIN_LAG = 0.0005 -MACH_E_TURN_IN_RATE_MIN_SPEED = 3.0 -MACH_E_TURN_IN_RATE_MIN_DEFICIT = 0.002 -MACH_E_UNWIND_LOOKAHEAD_EXTRA = 0.80 -MACH_E_UNWIND_FULL_LAG_CURVATURE = 0.0005 -MACH_E_UNWIND_PREVIEW_LAG_CURVATURE = 0.002 -MACH_E_UNWIND_PREVIEW_HOLD_SECONDS = 0.10 -MACH_E_UNWIND_PREVIEW_RISE_TOLERANCE = 0.0002 -MACH_E_UNWIND_RELEASE_GAIN = 0.40 -MACH_E_DIRECTION_CHANGE_MIN_SPEED = 9.0 -MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED = 10.0 -MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED = 12.0 -MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED = 15.0 -MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA = 2.40 -MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE = 0.0005 -MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE = 0.002 -MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE = 0.0008 -MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0015 -MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE = -0.001 -MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE = 0.0008 -MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE = 0.002 -MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE = 0.004 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED = 1.8 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED = 2.0 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED = 2.8 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED = 3.5 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE = 0.0004 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE = 0.0006 -MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE = 0.0015 -MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED = 1.5 -MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED = 1.8 -MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED = 3.0 -MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED = 4.0 -MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE = 0.0002 -MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE = 0.0005 -MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE = 0.008 -MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE = 0.012 -MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL = 1.8 -MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL = 2.2 -MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE = -0.0005 -MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0008 -MACH_E_CURVATURE_ERROR_MAX = 0.006 -MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT = 0.002 -MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT = 0.004 -MACH_E_PATH_ANGLE_MAX = 0.16 -MACH_E_PATH_ANGLE_STEP = 0.055 -MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0 -MACH_E_PATH_ANGLE_MAX_SPEED = 8.8 -MACH_E_PATH_ANGLE_TRACKING_FACTOR = 0.75 -MACH_E_PATH_ANGLE_DRIVER_COOLDOWN = 0.75 -MACH_E_DRIVER_ASSIST_MIN_SPEED = 2.0 -MACH_E_DRIVER_ASSIST_MAX_SPEED = 15.0 -MACH_E_DRIVER_ASSIST_MAX_TORQUE = 3.5 -FORD_CURVATURE_LOOKAHEAD = { - CAR.FORD_EXPLORER_MK6: 0.20, -} -FORD_CONSERVATIVE_PREVIEW_CARS = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, -}) -FORD_SHARP_DIRECTION_CHANGE_CARS = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, -}) -FORD_MANUAL_TURN_LATCH_CARS = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, -}) -MANUAL_TURN_ENTRY_ANGLE_DEG = 12.0 -MANUAL_TURN_RELEASE_ANGLE_DEG = 12.0 -MANUAL_TURN_RECOVERY_SECONDS = 0.25 - - -@dataclass(frozen=True) -class FordLateralResult: - curvature: float = 0.0 - curvature_rate: float = 0.0 - path_angle: float = 0.0 - ramp_type: int = 0 - precision_type: int = 1 - active: bool = False - - -# Adapted from BluePilot HumanTurnDetector (Alan Polk, 97867c1eb57b7472f6fc3de62f0fef576e5a5497). -class HumanTurnDetector: - ANGLE_DEG = 45.0 - HOLD_SECONDS = 1.5 - PRETURNED_HOLD_SECONDS = 3.0 - - def __init__(self): - self.timer = 0.0 - self.active = False - self._pressed_last = False - self._press_started_preturned = False - - def update(self, enabled: bool, steering_pressed: bool, steering_angle_deg: float) -> bool: - if steering_pressed and not self._pressed_last: - self._press_started_preturned = abs(steering_angle_deg) > self.ANGLE_DEG - self._pressed_last = steering_pressed - - if enabled and steering_pressed and abs(steering_angle_deg) > self.ANGLE_DEG: - self.timer += STEER_DT - else: - self.timer = 0.0 - - hold_time = self.PRETURNED_HOLD_SECONDS if self._press_started_preturned else self.HOLD_SECONDS - self.active = self.timer + 1e-9 >= hold_time - return self.active - - def reset(self): - self.timer = 0.0 - self.active = False - self._pressed_last = False - self._press_started_preturned = False - - -class FordLateralController: - - def __init__(self, CP): - self.CP = CP - from opendbc.car.ford.pinion import Measurement - self.measurement = Measurement(CP) - self.model = None - self.time_indices = () - self.lookahead = CURVATURE_LOOKAHEAD_MIN - - self.human_turn_enabled = True - self.curvature_blend_low = 0.4 - self.curvature_blend_high = 0.4 - self.curvature_lane_change_factor = 0.85 - - self.human_turn = HumanTurnDetector() - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT))) - self.curvature_last = 0.0 - self.path_angle_last = 0.0 - self.path_angle_driver_cooldown = 0.0 - self.desired_curvature_last = 0.0 - self.turn_in_preview_hold_timer = 0.0 - self.unwind_preview_hold_timer = 0.0 - self.assist_engagement_retained = False - - def set_assist_permission(self, panda_states, healthy: bool): - ford_pandas = [p for p in panda_states if p.safetyModel == structs.CarParams.SafetyModel.ford] if healthy else [] - assist_flags = FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_EXTENDED - self.assist_engagement_retained = bool(ford_pandas) and all( - p.controlsAllowed and p.safetyParam & assist_flags == assist_flags and - not p.safetyParam & FordSafetyFlags.LKA_STEERING for p in ford_pandas - ) - - def _path_angle_assist_permitted(self, CC, CS) -> bool: - if not CC.enabled or CS.out.brakePressed: - return False - return not CS.out.gasPressed or (CS.out.cruiseState.enabled and self.assist_engagement_retained) - - def set_blend_settings(self, low: float, high: float, lane_change: float): - self.curvature_blend_low = float(np.clip(low if np.isfinite(low) else 0.4, 0.0, 1.0)) - self.curvature_blend_high = float(np.clip(high if np.isfinite(high) else 0.4, 0.0, 1.0)) - self.curvature_lane_change_factor = float(np.clip(lane_change if np.isfinite(lane_change) else 0.85, 0.5, 1.25)) - - def set_inputs(self, model, time_indices, lookahead: float, human_turn_enabled: bool): - self.model = model - self.time_indices = time_indices - self.lookahead = float(np.clip(lookahead, CURVATURE_LOOKAHEAD_MIN, CURVATURE_LOOKAHEAD_MAX)) - self.human_turn_enabled = human_turn_enabled - - def _predicted_curvature(self, v_ego: float, lookup_time: float) -> float: - if self.model is None or len(self.model.orientationRate.z) < 17: - return 0.0 - curvatures = np.asarray(self.model.orientationRate.z) / max(v_ego, 0.01) - return float(np.interp(lookup_time, self.time_indices, curvatures)) - - def _curvature_lookahead(self) -> float: - return self.lookahead - - def _lane_change(self) -> tuple[bool, int]: - if self.model is None: - return False, 0 - state = int(getattr(self.model.meta.laneChangeState, "raw", self.model.meta.laneChangeState)) - direction = int(getattr(self.model.meta.laneChangeDirection, "raw", self.model.meta.laneChangeDirection)) - return state in (1, 2, 3), direction - - def _high_speed_lookahead_extra(self, v_ego: float, steering_pressed: bool, lane_change: bool) -> float: - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - steering_pressed or lane_change): - return 0.0 - return MACH_E_HIGH_SPEED_LOOKAHEAD_EXTRA * float(np.interp( - v_ego, [MACH_E_HIGH_SPEED_LOOKAHEAD_START_SPEED, MACH_E_HIGH_SPEED_LOOKAHEAD_FULL_SPEED], [0.0, 1.0])) - - def _current_curvature(self, CS) -> float: - return self.measurement.curvature(CS.out) - - def _driver_assisting_curve(self, CS, desired: float) -> bool: - v_ego = float(CS.out.vEgoRaw) - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - not CS.out.steeringPressed or not MACH_E_DRIVER_ASSIST_MIN_SPEED <= v_ego < MACH_E_DRIVER_ASSIST_MAX_SPEED or - self._lane_change()[0] or abs(desired) < MACH_E_TURN_IN_MIN_CURVATURE or - desired * CS.out.steeringTorque >= 0.0 or abs(CS.out.steeringTorque) > MACH_E_DRIVER_ASSIST_MAX_TORQUE): - return False - current = self._current_curvature(CS) - preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) - return bool(desired * current > 0.0 and desired * preview > 0.0 and - abs(preview) >= MACH_E_TURN_IN_FULL_CURVATURE and - np.sign(desired) * (current - desired) <= CarControllerParams.CURVATURE_ERROR) - - def _curvature_error_limit(self, requested: float, desired: float, current: float, v_ego: float, - steering_pressed: bool, lane_change: bool, predicted: float = 0.0) -> float: - base = CarControllerParams.CURVATURE_ERROR - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - steering_pressed or lane_change): - return base - speed_weight = float(np.interp(v_ego, [8.0, 9.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0])) - deficit_weight = 0.0 - planned_curve = abs(desired) >= 0.003 or (abs(requested) >= 0.003 and requested * predicted > 0.0) - if requested * desired > 0.0 and planned_curve: - deficit = np.sign(desired) * (requested - current) - deficit_weight = float(np.interp( - deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0])) - if current * desired < 0.0: - predicted = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) - reversal_weight = 0.0 - if current * predicted < 0.0 and current * (requested - current) < 0.0: - reversal_weight = float(np.interp( - abs(predicted), - [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], - [0.0, 1.0], - )) - unwind_weight = 0.0 - if (self.model is not None and len(self.model.orientationRate.z) >= 17 and - requested * current >= 0.0 and desired * current >= 0.0 and abs(desired) < abs(current)): - unwind_deficit = np.sign(current) * (current - requested) - preview_deficit = np.sign(current) * (current - predicted) - unwind_weight = float(np.interp( - min(unwind_deficit, preview_deficit), - [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], - [0.0, 1.0], - )) - return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight, unwind_weight) - - def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float, - steering_pressed: bool, lane_change: bool) -> float: - if steering_pressed: - self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN - else: - self.path_angle_driver_cooldown = max(0.0, self.path_angle_driver_cooldown - STEER_DT) - target = 0.0 - if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and - not steering_pressed and self.path_angle_driver_cooldown == 0.0 and not lane_change and - 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and - requested * desired > 0.0 and requested * applied > 0.0 and - abs(requested) > 0.0198 and abs(desired) > MACH_E_TURN_IN_FULL_CURVATURE and abs(applied) >= 0.0195 and - np.sign(desired) * (desired - current) > 0.002): - max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2 - residual = max(0.0, min(max(abs(requested), abs(desired)), max_curvature) - abs(applied)) - tracking_deficit = max(0.0, np.sign(applied) * (applied - current)) - acceleration_headroom = max(0.0, (max_curvature - abs(applied)) * v_ego) - speed_weight = float(np.interp( - v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0])) - target = float(np.sign(applied) * min( - (residual + MACH_E_PATH_ANGLE_TRACKING_FACTOR * tracking_deficit) * v_ego, - acceleration_headroom, MACH_E_PATH_ANGLE_MAX) * speed_weight) - if target == 0.0 or target * self.path_angle_last < 0.0: - self.path_angle_last = 0.0 - else: - self.path_angle_last = float(np.clip( - target, self.path_angle_last - MACH_E_PATH_ANGLE_STEP, self.path_angle_last + MACH_E_PATH_ANGLE_STEP)) - return self.path_angle_last - - def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, current: float = 0.0, - allow_opposite_preview: bool = False) -> tuple[float, int]: - blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high])) - if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS: - if desired * predicted <= 0.0 and not allow_opposite_preview: - blend = 0.0 - elif current * predicted > 0.0 and abs(current) > abs(desired) and abs(predicted) > abs(desired): - blend *= abs(desired) / abs(predicted) - requested = predicted * blend + desired * (1.0 - blend) - lane_change, direction = self._lane_change() - precision = 1 - if lane_change: - factor = float(np.interp(v_ego, [4.4, 40.23], [0.95, self.curvature_lane_change_factor])) - if (direction == 1 and requested < 0.0) or (direction == 2 and requested > 0.0): - requested *= factor - precision = 0 - return requested, precision - - def _unwind_preview(self, desired: float, predicted: float, current: float, v_ego: float, - allow_plateau: bool = False) -> float: - if self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1: - return predicted - if desired * current <= 0.0 or desired * predicted <= 0.0 or desired * self.desired_curvature_last <= 0.0: - return predicted - if abs(desired) >= abs(self.desired_curvature_last) and not allow_plateau: - return predicted - preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_UNWIND_LOOKAHEAD_EXTRA) - if desired * preview <= 0.0 or abs(preview) >= min(abs(desired), abs(predicted), abs(current)): - return predicted - speed_weight = float(np.interp(v_ego, [5.0, 7.0, 12.0, 15.0], [0.0, 1.0, 1.0, 0.0])) - curvature_weight = float(np.interp(abs(desired), [0.002, 0.008], [0.0, 1.0])) - lag_weight = float(np.interp(abs(current) - abs(desired), [0.0, MACH_E_UNWIND_FULL_LAG_CURVATURE], [0.0, 1.0])) - preview_lag_weight = float(np.interp( - abs(current) - abs(preview), [0.0, MACH_E_UNWIND_PREVIEW_LAG_CURVATURE], [0.0, 1.0])) - lag_weight = max(lag_weight, preview_lag_weight) - return predicted + speed_weight * curvature_weight * lag_weight * (preview - predicted) - - def _turn_in_preview_plateau(self, desired: float, current: float, v_ego: float, - steering_pressed: bool, lane_change: bool) -> float: - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - not MACH_E_LOW_SPEED_TURN_IN_START_SPEED <= v_ego < MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED or - steering_pressed or lane_change or desired * self.desired_curvature_last < 0.0 or - abs(desired) < abs(self.desired_curvature_last)): - self.turn_in_preview_hold_timer = 0.0 - return 0.0 - if abs(desired) > abs(self.desired_curvature_last): - self.turn_in_preview_hold_timer = MACH_E_TURN_IN_PREVIEW_HOLD_SECONDS - return 0.0 - self.turn_in_preview_hold_timer = max(0.0, self.turn_in_preview_hold_timer - STEER_DT) - if (self.turn_in_preview_hold_timer <= 1e-9 or - np.sign(desired) * (desired - current) <= MACH_E_TURN_IN_PREVIEW_HOLD_MIN_LAG): - return 0.0 - return float(np.interp(v_ego, [2.0, 3.0, 14.0, 15.0], [0.0, 1.0, 1.0, 0.0])) - - def _unwind_preview_plateau(self, desired: float, v_ego: float, steering_pressed: bool, lane_change: bool) -> bool: - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - not 5.0 < v_ego < 15.0 or steering_pressed or lane_change or - desired * self.desired_curvature_last <= 0.0 or - abs(desired) - abs(self.desired_curvature_last) > MACH_E_UNWIND_PREVIEW_RISE_TOLERANCE): - self.unwind_preview_hold_timer = 0.0 - return False - if abs(desired) < abs(self.desired_curvature_last): - self.unwind_preview_hold_timer = MACH_E_UNWIND_PREVIEW_HOLD_SECONDS - return False - self.unwind_preview_hold_timer = max(0.0, self.unwind_preview_hold_timer - STEER_DT) - return self.unwind_preview_hold_timer > 1e-9 - - def _unwind_request(self, desired: float, predicted: float, command_predicted: float, requested: float) -> float: - delta = command_predicted - predicted - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - not np.isfinite([desired, predicted, command_predicted, requested]).all() or - desired * command_predicted <= 0.0 or desired * delta >= 0.0): - return requested - release_weight = float(np.interp(abs(delta), [0.002, 0.006], [0.0, 1.0])) - blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high])) - preview_authority = float(np.clip(blend / 0.4, 0.0, 1.0)) - return float(np.clip(requested + MACH_E_UNWIND_RELEASE_GAIN * preview_authority * release_weight * delta, - min(desired, command_predicted), max(desired, command_predicted))) - - def _reversal_followthrough_preview(self, desired: float, predicted: float, current: float, v_ego: float, - steering_pressed: bool, lane_change: bool) -> float: - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - steering_pressed or lane_change or desired * current >= 0.0): - return predicted - speed_weight = float(np.interp(v_ego, [8.0, 9.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0])) - if speed_weight == 0.0: - return predicted - preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) - if not np.isfinite(preview) or desired * preview <= 0.0 or np.sign(desired) * (preview - predicted) <= 0.0: - return predicted - preview_weight = float(np.interp( - abs(preview), [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], - [0.0, 1.0])) - lag_weight = float(np.interp( - abs(current), [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE], - [0.0, 1.0])) - return predicted + speed_weight * preview_weight * lag_weight * (preview - predicted) - - def _turn_in_preview_weight(self, desired: float, preview: float, current: float, - allow_plateau: float = 0.0) -> float: - if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS: - return 0.0 - if desired * preview <= 0.0 or desired * self.desired_curvature_last < 0.0: - return 0.0 - if (abs(desired) < abs(self.desired_curvature_last) or - (abs(desired) == abs(self.desired_curvature_last) and not allow_plateau)): - return 0.0 - - target = max(abs(desired), abs(preview)) - curvature_weight = float(np.interp( - target, - [MACH_E_TURN_IN_MIN_CURVATURE, MACH_E_TURN_IN_FULL_CURVATURE], - [0.0, 1.0], - )) - direction = float(np.sign(desired)) - lag_weight = float(np.clip( - (target - direction * current) / MACH_E_TURN_IN_LAG_CURVATURE, - 0.0, 1.0, - )) - plateau_weight = allow_plateau if abs(desired) == abs(self.desired_curvature_last) else 1.0 - return curvature_weight * lag_weight * plateau_weight - - def _turn_in_curvature_rate(self, desired: float, current: float, curvature_rate: float, - v_ego: float, lookahead: float, steering_pressed: bool, lane_change: bool) -> float: - if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or - not MACH_E_TURN_IN_RATE_MIN_SPEED <= v_ego < MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED or - steering_pressed or lane_change or desired * self.desired_curvature_last <= 0.0 or - abs(desired) < abs(self.desired_curvature_last) or desired * curvature_rate >= 0.0 or - np.sign(desired) * (desired - current) <= MACH_E_TURN_IN_RATE_MIN_DEFICIT): - return curvature_rate - preview = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) - if (desired * preview > 0.0 and abs(preview) >= abs(desired) and - np.sign(desired) * (preview - current) > MACH_E_TURN_IN_RATE_MIN_DEFICIT): - return 0.0 - return curvature_rate - - @staticmethod - def _turn_in_lookahead_extra(v_ego: float) -> float: - return float(np.interp( - v_ego, - [MACH_E_LOW_SPEED_TURN_IN_START_SPEED, MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED, - MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED, MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED], - [MACH_E_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA, - MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_TURN_IN_LOOKAHEAD_EXTRA], - )) - - @staticmethod - def _direction_change_lookahead_extra(v_ego: float) -> float: - return float(np.interp( - v_ego, - [MACH_E_DIRECTION_CHANGE_MIN_SPEED, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED], - [MACH_E_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA, MACH_E_TURN_IN_LOOKAHEAD_EXTRA], - )) - - def _direction_change_preview_weight(self, desired: float, preview: float, current: float, - allow_rising_desired: bool = False, early_handoff_weight: float = 0.0, - sharp_handoff: bool = False) -> float: - if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS: - return 0.0 - if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0 or desired * current <= 0.0: - return 0.0 - early_handoff_weight = float(np.clip(early_handoff_weight, 0.0, 1.0)) - lag = abs(current) - abs(desired) - if sharp_handoff: - lag_min = MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE - lag_full = MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE - else: - lag_min = float(np.interp( - early_handoff_weight, [0.0, 1.0], - [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE], - )) - lag_full = float(np.interp( - early_handoff_weight, [0.0, 1.0], - [MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE], - )) - desired_rising = abs(desired) >= abs(self.desired_curvature_last) - rising_handoff = (allow_rising_desired and abs(desired) > abs(self.desired_curvature_last) and - abs(desired) <= MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE) - early_rising_handoff = early_handoff_weight > 0.0 and lag > lag_min - if desired_rising and not rising_handoff and not early_rising_handoff: - return 0.0 - - preview_weight = float(np.interp( - abs(preview), - [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], - [0.0, 1.0], - )) - if sharp_handoff: - preview_weight *= float(np.interp( - abs(preview), - [MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], - [0.0, 1.0], - )) - lag_weight = float(np.interp( - lag, - [lag_min, lag_full], - [0.0, 1.0], - )) - return preview_weight * lag_weight - - @staticmethod - def _low_speed_direction_change_weight(v_ego: float, desired: float) -> float: - speed_weight = float(np.interp( - v_ego, - [MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED, MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED, MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED], - [0.0, 1.0, 1.0, 0.0], - )) - curvature_weight = float(np.interp( - abs(desired), - [MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE, MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE], - [0.0, 1.0], - )) - return speed_weight * curvature_weight - - @staticmethod - def _sharp_direction_change_weight(v_ego: float, a_ego: float, desired: float, preview: float) -> float: - speed_weight = float(np.interp( - v_ego, - [MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED, MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED, - MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED, MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED], - [0.0, 1.0, 1.0, 0.0], - )) - curvature_weight = float(np.interp( - abs(desired), - [MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE], - [0.0, 1.0], - )) - preview_weight = float(np.interp( - abs(preview), - [MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], - [0.0, 1.0], - )) - acceleration_weight = float(np.interp( - a_ego, - [MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL, MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL], - [0.0, 1.0], - )) - return speed_weight * curvature_weight * preview_weight * acceleration_weight - - def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool: - if not CC.latActive: - self.human_turn.reset() - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - detected = self.human_turn.update( - self.human_turn_enabled and not driver_assisting, CS.out.steeringPressed, CS.out.steeringAngleDeg) - if self.CP.carFingerprint not in FORD_MANUAL_TURN_LATCH_CARS: - return detected - - if not self.human_turn_enabled: - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - - blinker_direction = float(CS.out.rightBlinker) - float(CS.out.leftBlinker) - driver_turning_with_signal = ( - CS.out.steeringPressed and not driver_assisting and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and - blinker_direction != 0.0 and not self._lane_change()[0] and - CS.out.steeringTorque * blinker_direction < 0.0 - ) - if self.model is not None and (detected or driver_turning_with_signal): - self.manual_turn_latched = True - if blinker_direction != 0.0: - self.manual_turn_direction = blinker_direction - elif self.manual_turn_direction == 0.0: - self.manual_turn_direction = -float(np.sign(CS.out.steeringAngleDeg)) - - if not self.manual_turn_latched: - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - - following_next_curve = ( - driver_assisting and not CS.out.leftBlinker and not CS.out.rightBlinker and - CS.out.vEgoRaw >= MACH_E_DIRECTION_CHANGE_MIN_SPEED and self.manual_turn_direction * desired < 0.0 - ) - if not following_next_curve and (CS.out.steeringPressed or blinker_direction != 0.0 or - abs(CS.out.steeringAngleDeg) > MANUAL_TURN_RELEASE_ANGLE_DEG): - self.manual_turn_recovery_timer = 0.0 - else: - self.manual_turn_recovery_timer += STEER_DT - if self.manual_turn_recovery_timer + 1e-9 >= MANUAL_TURN_RECOVERY_SECONDS: - current = self._current_curvature(CS) - if (self.manual_turn_direction * desired > 0.0 and - self.manual_turn_direction * (desired - current) > CarControllerParams.CURVATURE_ERROR): - self.manual_turn_recovery_timer = MANUAL_TURN_RECOVERY_SECONDS - else: - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - - return self.manual_turn_latched - - def update(self, CC, CS, actuators) -> FordLateralResult: - current = self._current_curvature(CS) - if not CC.latActive: - self.human_turn.reset() - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - self.curvature_samples.clear() - self.curvature_last = 0.0 - self.path_angle_last = 0.0 - self.path_angle_driver_cooldown = 0.0 - self.desired_curvature_last = 0.0 - self.turn_in_preview_hold_timer = 0.0 - self.unwind_preview_hold_timer = 0.0 - return FordLateralResult() - - desired = float(actuators.curvature) - driver_assisting = self._driver_assisting_curve(CS, desired) - driver_override = bool(CS.out.steeringPressed) and not driver_assisting - manual_turn = self._manual_turn(CC, CS, desired, driver_assisting) - if manual_turn or CS.out.vEgoRaw < 0.1: - if CS.out.steeringPressed: - self.path_angle_driver_cooldown = MACH_E_PATH_ANGLE_DRIVER_COOLDOWN - self.curvature_samples.clear() - self.curvature_last = 0.0 - self.path_angle_last = 0.0 - self.desired_curvature_last = 0.0 - self.turn_in_preview_hold_timer = 0.0 - self.unwind_preview_hold_timer = 0.0 - return FordLateralResult(active=not ( - manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS)) - - v_ego = float(CS.out.vEgoRaw) - lookahead = self._curvature_lookahead() - lookahead += self._high_speed_lookahead_extra(v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) - predicted = self._predicted_curvature(v_ego, lookahead) - allow_opposite_preview = False - if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS: - turn_in_plateau = self._turn_in_preview_plateau( - desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) - turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) - direction_change_predicted = turn_in_predicted - direction_change_weight = 0.0 - sharp_direction_change_weight = 0.0 - direction_change_speed_weight = float(v_ego > MACH_E_DIRECTION_CHANGE_MIN_SPEED) - low_speed_direction_change = direction_change_speed_weight == 0.0 - if direction_change_speed_weight == 0.0: - direction_change_speed_weight = self._low_speed_direction_change_weight(v_ego, desired) - if self.CP.carFingerprint in FORD_SHARP_DIRECTION_CHANGE_CARS: - sharp_direction_change_weight = self._sharp_direction_change_weight( - v_ego, float(CS.out.aEgo), desired, turn_in_predicted) - direction_change_speed_weight = max(direction_change_speed_weight, sharp_direction_change_weight) - if direction_change_speed_weight > 0.0 and not CS.out.steeringPressed and not self._lane_change()[0]: - direction_change_lookahead_extra = self._direction_change_lookahead_extra(v_ego) - early_handoff_weight = float(np.interp( - direction_change_lookahead_extra, - [MACH_E_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA], - [0.0, 1.0], - )) - early_handoff_weight *= float(np.interp( - abs(desired), - [MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE], - [0.0, 1.0], - )) - if direction_change_lookahead_extra > MACH_E_TURN_IN_LOOKAHEAD_EXTRA: - direction_change_predicted = self._predicted_curvature(v_ego, lookahead + direction_change_lookahead_extra) - direction_change_weight = self._direction_change_preview_weight( - desired, direction_change_predicted, current, allow_rising_desired=low_speed_direction_change, - early_handoff_weight=early_handoff_weight, sharp_handoff=sharp_direction_change_weight > 0.0) - direction_change_weight *= direction_change_speed_weight - if direction_change_weight > 0.0: - predicted = float(np.interp(direction_change_weight, [0.0, 1.0], [predicted, direction_change_predicted])) - allow_opposite_preview = True - else: - turn_in_lookahead_extra = self._turn_in_lookahead_extra(v_ego) - if (turn_in_lookahead_extra > MACH_E_TURN_IN_LOOKAHEAD_EXTRA and - desired * self.desired_curvature_last >= 0.0 and - (abs(desired) > abs(self.desired_curvature_last) or turn_in_plateau)): - low_speed_turn_in_predicted = self._predicted_curvature(v_ego, lookahead + turn_in_lookahead_extra) - if (desired * low_speed_turn_in_predicted > 0.0 and - abs(low_speed_turn_in_predicted) > abs(turn_in_predicted)): - turn_in_predicted = low_speed_turn_in_predicted - turn_in_weight = self._turn_in_preview_weight(desired, turn_in_predicted, current, turn_in_plateau) - if turn_in_weight > 0.0: - turn_in_target = float(np.copysign(max(abs(desired), abs(turn_in_predicted)), desired)) - predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_target])) - unwind_plateau = self._unwind_preview_plateau( - desired, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) - command_predicted = predicted - canfd_mach_e = self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD - if (not allow_opposite_preview or canfd_mach_e) and not CS.out.steeringPressed and not self._lane_change()[0]: - command_predicted = self._unwind_preview(desired, predicted, current, v_ego, unwind_plateau) - unwind_selected = command_predicted != predicted - command_predicted = self._reversal_followthrough_preview( - desired, command_predicted, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0]) - requested, precision = self._blend_and_scale(desired, command_predicted, v_ego, current, allow_opposite_preview) - if unwind_selected: - requested = self._unwind_request(desired, predicted, command_predicted, requested) - - if v_ego > 9.0: - error_limit = self._curvature_error_limit( - requested, desired, current, v_ego, driver_override, self._lane_change()[0], command_predicted) - requested = float(np.clip(requested, current - error_limit, current + error_limit)) - applied = float(apply_std_steer_angle_limits( - requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS)) - if self.CP.flags & FordFlags.CANFD: - max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2 - applied = float(np.clip(applied, -max_curvature, max_curvature)) - path_angle = self._path_angle_assist( - requested, desired, applied, current, v_ego, driver_override, self._lane_change()[0]) - if path_angle != 0.0 and not self._path_angle_assist_permitted(CC, CS): - self.path_angle_last = 0.0 - path_angle = 0.0 - - self.curvature_samples.append(predicted) - curvature_rate = 0.0 - if len(self.curvature_samples) > 1: - sample_time = (len(self.curvature_samples) - 1) * STEER_DT - curvature_rate = (self.curvature_samples[-1] - self.curvature_samples[0]) / max(sample_time * v_ego, 0.01) - curvature_rate *= float(np.interp(abs(predicted), [0.0, 0.008, 0.01], [0.0, 0.0, 1.0])) - curvature_rate *= float(np.interp(v_ego, [0.0, 14.5, 15.5], [1.0, 1.0, 0.0])) - if self._lane_change()[0]: - curvature_rate = 0.0 - - if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and - unwind_selected and command_predicted * curvature_rate > 0.0): - curvature_rate = 0.0 - - curvature_rate = self._turn_in_curvature_rate( - desired, current, curvature_rate, v_ego, lookahead, bool(CS.out.steeringPressed), self._lane_change()[0]) - self.desired_curvature_last = desired - - self.curvature_last = float(np.clip(applied, -0.02, 0.02)) - min_curvature_rate = -0.001024 - if self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD: - min_curvature_rate = -0.001023 - curvature_rate = float(np.clip(curvature_rate, min_curvature_rate, 0.001023)) - return FordLateralResult( - curvature=self.curvature_last, - curvature_rate=curvature_rate, - path_angle=path_angle, - ramp_type=2, - precision_type=precision, - active=True, - ) diff --git a/opendbc_repo/opendbc/car/ford/mache_can.py b/opendbc_repo/opendbc/car/ford/mache_can.py deleted file mode 100644 index be451d801c..0000000000 --- a/opendbc_repo/opendbc/car/ford/mache_can.py +++ /dev/null @@ -1,50 +0,0 @@ -"""Ford extended-lateral CAN constructors. - -Adapted from BluePilot's ``fordcan_ext.py`` and extended-lateral protocol at bp-7.0 commit -e1d051d7ba270261b4455068bd68f1a58db15a4a, including panda integration developed principally by -Alan Polk. See CREDITS.md and THIRD_PARTY_NOTICES.md for detailed provenance and terms. -""" - -from opendbc.car.ford.fordcan import CanBus - - -def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int: - curvature = (dat[2] << 3) | ((dat[3]) >> 5) - curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5) - path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2) - path_offset = ((dat[4] & 0x3) << 8) | dat[5] - - checksum = mode + counter - for sig_val in (curvature, curvature_rate, path_angle, path_offset): - checksum += sig_val + (sig_val >> 8) - - return 0xFF - (checksum & 0xFF) - - - -def create_lka_msg(packer, CAN: CanBus): - addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) - dat = bytearray(dat) - dat[4] |= 0x2 - return addr, bytes(dat), bus - - - - -def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int, - curvature: float, curvature_rate: float, counter: int, path_angle: float = 0.0): - values = { - "LatCtl_D2_Rq": mode, - "LatCtlRampType_D_Rq": ramp_type, - "LatCtlPrecision_D_Rq": precision_type, - "LatCtlPathOffst_L_Actl": 0.0, - "LatCtlPath_An_Actl": path_angle, - "LatCtlCurv_No_Actl": curvature, - "LatCtlCrv_NoRate2_Actl": curvature_rate, - "HandsOffCnfm_B_Rq": 0, - "LatCtlPath_No_Cnt": counter, - "LatCtlPath_No_Cs": 0, - } - dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1] - values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) - return packer.make_can_msg("LateralMotionControl2", CAN.main, values) diff --git a/opendbc_repo/opendbc/car/ford/mache_lateral.py b/opendbc_repo/opendbc/car/ford/mache_lateral.py deleted file mode 100644 index 7fff75cac6..0000000000 --- a/opendbc_repo/opendbc/car/ford/mache_lateral.py +++ /dev/null @@ -1,155 +0,0 @@ -"""Ford lateral-control extensions. - -The extended curvature strategy, manual-turn detector, and related safety protocol are substantially -adapted from BluePilot's Ford work, principally by Alan Polk and additional contributors. The audited -bp-7.0 reference is e1d051d7ba270261b4455068bd68f1a58db15a4a; source attribution is retained in -CREDITS.md and THIRD_PARTY_NOTICES.md. StarPilot reorganized that work for its own architecture and has since -changed its tuning and lookahead behavior. - -See CREDITS.md for feature-level authorship and upstream commits, and THIRD_PARTY_NOTICES.md for the -published upstream license notices. Upstream contributors do not maintain this adaptation. - -Mach-E admission and modern command envelope; pure strategy is shared separately. -""" - -from dataclasses import replace - -import numpy as np - -from opendbc.car import structs -from opendbc.car.ford.aol import base_safety_param -from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags -from opendbc.car.lateral import MAX_LATERAL_JERK -from opendbc.car.ford.lateral_strategy import ( - FordLateralController, - FORD_CURVATURE_LIMITS as FORD_CURVATURE_LIMITS, - MAX_LATERAL_ACCEL as MAX_LATERAL_ACCEL, - STEER_DT as STEER_DT, - CURVATURE_LOOKAHEAD_MIN as CURVATURE_LOOKAHEAD_MIN, - CURVATURE_LOOKAHEAD_MAX as CURVATURE_LOOKAHEAD_MAX, - MACH_E_TURN_IN_LOOKAHEAD_EXTRA as MACH_E_TURN_IN_LOOKAHEAD_EXTRA, - MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA as MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA, - MACH_E_LOW_SPEED_TURN_IN_START_SPEED as MACH_E_LOW_SPEED_TURN_IN_START_SPEED, - MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED as MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED, - MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED as MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED, - MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED as MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED, - MACH_E_TURN_IN_MIN_CURVATURE as MACH_E_TURN_IN_MIN_CURVATURE, - MACH_E_TURN_IN_FULL_CURVATURE as MACH_E_TURN_IN_FULL_CURVATURE, - MACH_E_TURN_IN_LAG_CURVATURE as MACH_E_TURN_IN_LAG_CURVATURE, - MACH_E_UNWIND_LOOKAHEAD_EXTRA as MACH_E_UNWIND_LOOKAHEAD_EXTRA, - MACH_E_UNWIND_FULL_LAG_CURVATURE as MACH_E_UNWIND_FULL_LAG_CURVATURE, - MACH_E_UNWIND_PREVIEW_LAG_CURVATURE as MACH_E_UNWIND_PREVIEW_LAG_CURVATURE, - MACH_E_DIRECTION_CHANGE_MIN_SPEED as MACH_E_DIRECTION_CHANGE_MIN_SPEED, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED as MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED as MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED as MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED, - MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA as MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA, - MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE as MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, - MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE as MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE, - MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE as MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, - MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE as MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, - MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE as MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE, - MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE as MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE, - MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE as MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE, - MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE as MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED as MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED as MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED as MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED as MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE as MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE as MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE, - MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE as MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED as MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED, - MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED as MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED, - MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED as MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED, - MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED as MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED, - MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL as MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL, - MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL as MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL, - MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE, - MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE as MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE, - MACH_E_CURVATURE_ERROR_MAX as MACH_E_CURVATURE_ERROR_MAX, - MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT as MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, - MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT as MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT, - MACH_E_PATH_ANGLE_MAX as MACH_E_PATH_ANGLE_MAX, - MACH_E_PATH_ANGLE_STEP as MACH_E_PATH_ANGLE_STEP, - MACH_E_PATH_ANGLE_FADE_START_SPEED as MACH_E_PATH_ANGLE_FADE_START_SPEED, - MACH_E_PATH_ANGLE_MAX_SPEED as MACH_E_PATH_ANGLE_MAX_SPEED, - MACH_E_PATH_ANGLE_TRACKING_FACTOR as MACH_E_PATH_ANGLE_TRACKING_FACTOR, - MACH_E_PATH_ANGLE_DRIVER_COOLDOWN as MACH_E_PATH_ANGLE_DRIVER_COOLDOWN, - MACH_E_DRIVER_ASSIST_MIN_SPEED as MACH_E_DRIVER_ASSIST_MIN_SPEED, - MACH_E_DRIVER_ASSIST_MAX_SPEED as MACH_E_DRIVER_ASSIST_MAX_SPEED, - MACH_E_DRIVER_ASSIST_MAX_TORQUE as MACH_E_DRIVER_ASSIST_MAX_TORQUE, - FORD_CURVATURE_LOOKAHEAD as FORD_CURVATURE_LOOKAHEAD, - FORD_CONSERVATIVE_PREVIEW_CARS as FORD_CONSERVATIVE_PREVIEW_CARS, - FORD_SHARP_DIRECTION_CHANGE_CARS as FORD_SHARP_DIRECTION_CHANGE_CARS, - FORD_MANUAL_TURN_LATCH_CARS as FORD_MANUAL_TURN_LATCH_CARS, - MANUAL_TURN_ENTRY_ANGLE_DEG as MANUAL_TURN_ENTRY_ANGLE_DEG, - MANUAL_TURN_RELEASE_ANGLE_DEG as MANUAL_TURN_RELEASE_ANGLE_DEG, - MANUAL_TURN_RECOVERY_SECONDS as MANUAL_TURN_RECOVERY_SECONDS, - FordLateralResult as FordLateralResult, - HumanTurnDetector as HumanTurnDetector, -) - - -class MachELateralController(FordLateralController): - """Compatibility owner for the exact admitted Mach-E profile.""" - - -def qualified(CP) -> bool: - if (CP.brand != "ford" or CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or - not CP.flags & FordFlags.CANFD or CP.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM) or - CP.passive or CP.dashcamOnly or CP.notCar or CP.alternativeExperience not in (0, 32) or - len(CP.safetyConfigs) not in (1, 2)): - return False - if len(CP.safetyConfigs) == 2 and (CP.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - CP.safetyConfigs[0].safetyParam != 0): - return False - safety = CP.safetyConfigs[-1] - return (safety.safetyModel == structs.CarParams.SafetyModel.ford and - base_safety_param(CP) == int(FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_EXTENDED | - (FordSafetyFlags.LONG_CONTROL if CP.openpilotLongitudinalControl else 0))) - - -def path_speed_window_supported(CP) -> bool: - return (qualified(CP) and CP.alternativeExperience == 32 and - len(CP.safetyConfigs) == 1 and CP.safetyConfigs[0].safetyParam == - int(FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_EXTENDED | FordSafetyFlags.LONG_CONTROL)) - - -def bounded_command(owner: MachELateralController, demanded: FordLateralResult, previous: float, speed: float, - previous_path_angle: float = 0.0, speed_window=None) -> FordLateralResult: - """Intersect original demand with modern transient limits and old absolute cap. - - A discontinuous speed change may leave no legal active interval. Withdraw the - request then, rather than clipping outside the jerk interval. Histories always - follow emitted fields; the original demand remains observable separately. - """ - cap = min(0.02, MAX_LATERAL_ACCEL / max(speed, 1.0) ** 2) - jerk_delta = MAX_LATERAL_JERK / max(speed, 1.0) ** 2 * STEER_DT - low, high = max(-cap, previous - jerk_delta), min(cap, previous + jerk_delta) - if not demanded.active or low > high: - command = FordLateralResult() - else: - curvature = float(np.clip(demanded.curvature, low, high)) - path_angle = 0.0 - # Native path assistance checks the maximum of six raw ABS speed samples. - # Do not emit an assist that only the newest (lower) speed sample permits. - path_speed = max(speed, speed_window[1]) if speed_window is not None else speed - if (3.0 <= speed and path_speed < MACH_E_PATH_ANGLE_MAX_SPEED and abs(curvature) >= 0.0195 and - curvature * demanded.path_angle > 0.0): - headroom = max(0.0, (MAX_LATERAL_ACCEL / path_speed ** 2 - abs(curvature)) * path_speed) - path_cap = min(MACH_E_PATH_ANGLE_MAX, headroom) - path_low = max(-path_cap, previous_path_angle - MACH_E_PATH_ANGLE_STEP) - path_high = min(path_cap, previous_path_angle + MACH_E_PATH_ANGLE_STEP) - if path_low <= path_high: - path_angle = float(np.clip(demanded.path_angle, path_low, path_high)) - if path_angle * curvature <= 0.0: - path_angle = 0.0 - command = replace(demanded, curvature=curvature, path_angle=path_angle) - owner.curvature_last = command.curvature - owner.path_angle_last = command.path_angle - return command diff --git a/opendbc_repo/opendbc/car/ford/manual_turn.py b/opendbc_repo/opendbc/car/ford/manual_turn.py deleted file mode 100644 index 3380c79212..0000000000 --- a/opendbc_repo/opendbc/car/ford/manual_turn.py +++ /dev/null @@ -1,101 +0,0 @@ -import numpy as np - -from opendbc.car import DT_CTRL -from opendbc.car.ford.values import CarControllerParams - -STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL -MANUAL_TURN_ENTRY_ANGLE_DEG = 12.0 -MANUAL_TURN_RELEASE_ANGLE_DEG = 12.0 -MANUAL_TURN_RECOVERY_SECONDS = 0.25 - - -class HumanTurnDetector: - ANGLE_DEG = 45.0 - HOLD_SECONDS = 1.5 - PRETURNED_HOLD_SECONDS = 3.0 - - def __init__(self): - self.timer = 0.0 - self.active = False - self._pressed_last = False - self._press_started_preturned = False - - def update(self, enabled: bool, steering_pressed: bool, steering_angle_deg: float) -> bool: - if steering_pressed and not self._pressed_last: - self._press_started_preturned = abs(steering_angle_deg) > self.ANGLE_DEG - self._pressed_last = steering_pressed - - if enabled and steering_pressed and abs(steering_angle_deg) > self.ANGLE_DEG: - self.timer += STEER_DT - else: - self.timer = 0.0 - - hold_time = self.PRETURNED_HOLD_SECONDS if self._press_started_preturned else self.HOLD_SECONDS - self.active = self.timer + 1e-9 >= hold_time - return self.active - - def reset(self): - self.timer = 0.0 - self.active = False - self._pressed_last = False - self._press_started_preturned = False - - -class ManualTurnLatch: - def __init__(self): - self.human_turn = HumanTurnDetector() - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - self.human_turn_enabled = True - - def update(self, CC, CS, desired: float, enabled: bool, lane_change: bool, entry_allowed: bool = True) -> bool: - if not CC.latActive: - self.human_turn.reset() - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - self.human_turn_enabled = enabled - detected = self.human_turn.update( - self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg) - if not self.human_turn_enabled: - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - - blinker_direction = float(CS.out.rightBlinker) - float(CS.out.leftBlinker) - driver_turning_with_signal = ( - CS.out.steeringPressed and abs(CS.out.steeringAngleDeg) >= MANUAL_TURN_ENTRY_ANGLE_DEG and - blinker_direction != 0.0 and not lane_change and - CS.out.steeringTorque * blinker_direction < 0.0 - ) - if entry_allowed and (detected or driver_turning_with_signal): - self.manual_turn_latched = True - if blinker_direction != 0.0: - self.manual_turn_direction = blinker_direction - elif self.manual_turn_direction == 0.0: - self.manual_turn_direction = -float(np.sign(CS.out.steeringAngleDeg)) - - if not self.manual_turn_latched: - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - return False - - if (CS.out.steeringPressed or blinker_direction != 0.0 or - abs(CS.out.steeringAngleDeg) > MANUAL_TURN_RELEASE_ANGLE_DEG): - self.manual_turn_recovery_timer = 0.0 - else: - self.manual_turn_recovery_timer += STEER_DT - if self.manual_turn_recovery_timer + 1e-9 >= MANUAL_TURN_RECOVERY_SECONDS: - current = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - if (self.manual_turn_direction * desired > 0.0 and - self.manual_turn_direction * (desired - current) > CarControllerParams.CURVATURE_ERROR): - self.manual_turn_recovery_timer = MANUAL_TURN_RECOVERY_SECONDS - else: - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 - self.manual_turn_direction = 0.0 - - return self.manual_turn_latched diff --git a/opendbc_repo/opendbc/car/ford/new_port_curvature.py b/opendbc_repo/opendbc/car/ford/new_port_curvature.py deleted file mode 100644 index 725438db7c..0000000000 --- a/opendbc_repo/opendbc/car/ford/new_port_curvature.py +++ /dev/null @@ -1,70 +0,0 @@ -"""Reuse the current Ford curvature strategy for exact Edge/Mondeo callers. - -Only scalar curvature crosses the existing native boundary; no protocol extension. -The reused strategy retains its required provenance and notices in lateral_strategy.py. -""" -from dataclasses import replace -import math - -from opendbc.car import structs -from opendbc.car.ford.aol import base_safety_param -from opendbc.car.ford.explorer_lateral import bounded_command as shared_bound -from opendbc.car.ford.lateral_strategy import FordLateralController, FordLateralResult -from opendbc.car.ford.values import CAR, FordFlags - -PROFILES = { - CAR.FORD_EDGE_MK2: (int(FordFlags.NEW_PORT | FordFlags.ALT_STEER_ANGLE), 8), - CAR.FORD_MONDEO_MK5: (int(FordFlags.NEW_PORT | FordFlags.CANFD), 10), -} - - -def qualified(cp): - profile = PROFILES.get(cp.carFingerprint) - if (profile is None or cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar or - cp.alternativeExperience not in (0, 32) or not cp.pcmCruise or - cp.steerControlType != structs.CarParams.SteerControlType.angle or - len(cp.safetyConfigs) not in (1, 2)): - return False - flags, word = profile - if (int(cp.flags) & ~int(FordFlags.HAS_BSM)) != flags: - return False - if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or - cp.safetyConfigs[0].safetyParam != 0): - return False - safety = cp.safetyConfigs[-1] - return safety.safetyModel == structs.CarParams.SafetyModel.ford and base_safety_param(cp) == word + int(cp.openpilotLongitudinalControl) - - -class NewPortCurvatureController(FordLateralController): - def __init__(self, cp): - if not qualified(cp): - raise ValueError("Edge/Mondeo curvature requires its exact current native profile") - super().__init__(cp) - self.manual_turn_detected = False - - def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool: - self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting) - return self.manual_turn_detected - - def update(self, CC, CS, actuators): - out = CS.out - if (not CC.latActive or not out.canValid or out.canTimeout or out.steerFaultTemporary or - out.steerFaultPermanent or out.vehicleSensorsInvalid or - not all(math.isfinite(value) for value in (out.vEgoRaw, out.yawRate, out.steeringAngleDeg, actuators.curvature))): - self.human_turn.reset() - self.curvature_samples.clear() - self.curvature_last = self.path_angle_last = 0.0 - self.manual_turn_detected = False - return FordLateralResult() - return super().update(CC, CS, actuators) - - -def create_controller(cp): - return NewPortCurvatureController(cp) if qualified(cp) else None - - -def bounded_command(owner, demanded, previous, speed, measured): - cap = (3.0 - 9.81 * .06) / max(speed, 1.) ** 2 if owner.CP.flags & FordFlags.CANFD else None - command = shared_bound(owner, demanded, previous, speed, measured, absolute_cap=cap) - # These profiles keep their current curvature-only native/codec contract. - return replace(command, curvature_rate=0.0, path_angle=0.0, ramp_type=0, precision_type=1) diff --git a/opendbc_repo/opendbc/car/ford/pinion.py b/opendbc_repo/opendbc/car/ford/pinion.py deleted file mode 100644 index f6cac968ed..0000000000 --- a/opendbc_repo/opendbc/car/ford/pinion.py +++ /dev/null @@ -1,84 +0,0 @@ -"""Init-only pinion measurement selection adapted from BluePilot PR #145. - -See CREDITS.md and THIRD_PARTY_NOTICES.md for upstream attribution and license. -The geometry is carried in the existing exact Panda safety word, not live Params. -""" -from opendbc.car.ford.values import CAR - -KEY = "FordPrefSteerAngleCurvature" -ENABLE = 0x200 -SHIFT = 10 -MASK = 0x3E00 -GEOMETRY = { - CAR.FORD_BRONCO_SPORT_MK1: 1, CAR.FORD_ESCAPE_MK4: 2, CAR.FORD_ESCAPE_MK4_5: 3, - CAR.FORD_EXPEDITION_MK4: 4, CAR.FORD_EXPLORER_MK6: 5, CAR.FORD_FOCUS_MK4: 6, - CAR.FORD_F_150_LIGHTNING_MK1: 7, CAR.FORD_F_150_MK14: 8, CAR.FORD_MAVERICK_MK1: 9, - CAR.FORD_MONDEO_MK5: 10, CAR.FORD_MUSTANG_MACH_E_MK1: 11, CAR.FORD_RANGER_MK2: 12, -} -CLASSIC = frozenset((32, 33, 128, 129, 160, 161)) -GENERIC = frozenset((66, 67, 130, 131, 194, 195)) -MACH_E = frozenset((18, 19, 130, 131, 146, 147)) -PROFILES = {i: CLASSIC if i in (1, 2, 5, 6, 9) else MACH_E if i == 11 else frozenset((10, 11)) if i == 10 else GENERIC - for i in range(1, 13)} - - -def decode(word): - """Return (original profile, geometry), or None for malformed metadata.""" - if not word & MASK: - return word, 0 - index = (word >> SHIFT) & 15 - base = word & ~MASK - if not word & ENABLE or index not in PROFILES or (base & ~256) not in PROFILES[index]: - return None - return base, index - - -def base_word(word): - result = decode(int(word)) - return result[0] if result is not None else -1 - - -def configured(cp): - if cp.brand != "ford" or not cp.safetyConfigs: - return False - result = decode(int(cp.safetyConfigs[-1].safetyParam)) - return bool(result is not None and result[1] and result[1] == GEOMETRY.get(cp.carFingerprint)) - - -def select(cp, enabled): - if not enabled or cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar: - return False - from opendbc.car.ford.aol import qualified - if not qualified(cp): - return False - index = GEOMETRY.get(cp.carFingerprint) - if index is None: - return False - cp.safetyConfigs[-1].safetyParam |= ENABLE | index << SHIFT - return True - - -class Measurement: - """Same init-time family as native; live model correction never changes the source.""" - def __init__(self, cp): - self.enabled = configured(cp) - self.offset = self.roll = 0.0 - self.model = None - if self.enabled: - from opendbc.car.vehicle_model import VehicleModel - self.model = VehicleModel(cp) - - def update_parameters(self, parameters): - import math - if self.model is None or parameters is None: - return - values = (parameters.angleOffsetDeg, parameters.roll, parameters.stiffnessFactor, parameters.steerRatio) - if parameters.valid and all(math.isfinite(v) for v in values): - self.offset, self.roll = values[:2] - self.model.update_params(max(values[2], 0.1), max(values[3], 0.1)) - - def curvature(self, state): - if not self.enabled: - return -state.yawRate / max(state.vEgoRaw, 0.1) - import math - return -self.model.calc_curvature(math.radians(state.steeringAngleDeg - self.offset), state.vEgoRaw, self.roll) diff --git a/opendbc_repo/opendbc/car/ford/radar_interface.py b/opendbc_repo/opendbc/car/ford/radar_interface.py index c87ea86d0c..c5da2ef7e0 100644 --- a/opendbc_repo/opendbc/car/ford/radar_interface.py +++ b/opendbc_repo/opendbc/car/ford/radar_interface.py @@ -7,9 +7,7 @@ from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.ford.fordcan import CanBus -from opendbc.car.ford.values import DBC, RADAR -from opendbc.car.ford.generic_canfd_lateral import qualified as generic_canfd_qualified -from opendbc.car.ford.mache_lateral import qualified as mache_qualified +from opendbc.car.ford.values import DBC, FORD_CANFD_CARS, FordFlags, RADAR from opendbc.car.interfaces import RadarInterfaceBase DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540)) @@ -102,7 +100,8 @@ class RadarInterface(RadarInterfaceBase): self.points: list[list[float]] = [] self.clusters: list[Cluster] = [] - self.generic_canfd_lead = generic_canfd_qualified(CP) or mache_qualified(CP) + self.generic_canfd_lead = (CP.carFingerprint in FORD_CANFD_CARS and bool(CP.flags & FordFlags.CANFD) and + not CP.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM)) self.v_rel_history = deque(maxlen=20) self.updated_messages = set() diff --git a/opendbc_repo/opendbc/car/ford/stock_cruise.py b/opendbc_repo/opendbc/car/ford/stock_cruise.py index a04ae321ff..fc831efc20 100644 --- a/opendbc_repo/opendbc/car/ford/stock_cruise.py +++ b/opendbc_repo/opendbc/car/ford/stock_cruise.py @@ -3,8 +3,6 @@ from opendbc.car import structs from opendbc.car.ford.aol import base_safety_param from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags -from opendbc.car.ford.classic_lateral import CLASSIC_EXTENDED_CARS -from opendbc.car.ford.generic_canfd_lateral import qualified as generic_canfd_qualified class FordStockCruiseButton: @@ -28,9 +26,6 @@ class FordStockCruiseButton: def qualified(CP) -> bool: - from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified - if angle_qualified(CP): - return not CP.openpilotLongitudinalControl """Existing stock profiles with the paired physical-driver button contract.""" if (CP.brand != "ford" or CP.carFingerprint not in tuple(CAR) or CP.openpilotLongitudinalControl or CP.alternativeExperience not in (0, 32) or CP.passive or CP.dashcamOnly or CP.notCar or len(CP.safetyConfigs) not in (1, 2) or @@ -40,17 +35,9 @@ def qualified(CP) -> bool: CP.safetyConfigs[0].safetyParam != 0): return False safety = CP.safetyConfigs[-1] - if safety.safetyModel != structs.CarParams.SafetyModel.ford or base_safety_param(CP) not in (2, 8, 10, 12, 18, 32, 66): + if safety.safetyModel != structs.CarParams.SafetyModel.ford or base_safety_param(CP) not in (0, 2, 8, 10, 12): return False - if safety.safetyModel == structs.CarParams.SafetyModel.ford and base_safety_param(CP) == 32: - return CP.carFingerprint in CLASSIC_EXTENDED_CARS and not CP.flags & ~int(FordFlags.HAS_BSM) - if base_safety_param(CP) == 66: - return generic_canfd_qualified(CP) expected = (int(FordSafetyFlags.CANFD) if CP.flags & FordFlags.CANFD else 0) expected |= int(FordSafetyFlags.NEW_PORT) if CP.flags & FordFlags.NEW_PORT else 0 expected |= int(FordSafetyFlags.LKA_STEERING) if CP.flags & FordFlags.LKA_STEERING else 0 - if base_safety_param(CP) == 18: - if CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or expected != int(FordSafetyFlags.CANFD): - return False - expected |= int(FordSafetyFlags.MACH_E_EXTENDED) return base_safety_param(CP) == expected diff --git a/opendbc_repo/opendbc/car/ford/tests/test_aol_startup.py b/opendbc_repo/opendbc/car/ford/tests/test_aol_startup.py index 2911dc1e28..bf3d3094f2 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_aol_startup.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_aol_startup.py @@ -9,11 +9,11 @@ from opendbc.car.ford.values import CAR PROFILES = ( - (CAR.FORD_BRONCO_SPORT_MK1, 32), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_FOCUS_MK4, 32), - (CAR.FORD_MAVERICK_MK1, 32), (CAR.FORD_EXPLORER_MK6, 32), - (CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4_5, 66), - (CAR.FORD_EXPEDITION_MK4, 66), (CAR.FORD_F_150_MK14, 66), - (CAR.FORD_F_150_LIGHTNING_MK1, 66), (CAR.FORD_RANGER_MK2, 66), + (CAR.FORD_BRONCO_SPORT_MK1, 0), (CAR.FORD_ESCAPE_MK4, 0), (CAR.FORD_FOCUS_MK4, 0), + (CAR.FORD_MAVERICK_MK1, 0), (CAR.FORD_EXPLORER_MK6, 0), + (CAR.FORD_MUSTANG_MACH_E_MK1, 2), (CAR.FORD_ESCAPE_MK4_5, 2), + (CAR.FORD_EXPEDITION_MK4, 2), (CAR.FORD_F_150_MK14, 2), + (CAR.FORD_F_150_LIGHTNING_MK1, 2), (CAR.FORD_RANGER_MK2, 2), (CAR.FORD_EDGE_MK2, 8), (CAR.FORD_MONDEO_MK5, 10), (CAR.FORD_TRANSIT_MK5, 12), ) @@ -62,7 +62,6 @@ def run_startup(identity, base_word, alpha, release, topology, monkeypatch): from openpilot.selfdrive.car.card import Car from openpilot.selfdrive.controls.controlsd import Controls from openpilot.starpilot.aol.vehicle import policy_for - from openpilot.starpilot.controller_extensions import ManualTurnInputs from openpilot.starpilot.lateral.tests.test_lane_runtime import feed monkeypatch.setenv("SIMULATION", "1") @@ -111,12 +110,6 @@ def run_startup(identity, base_word, alpha, release, topology, monkeypatch): assert policy.normal_runtime_supported == admitted assert ci.CP is card.CP assert ci.CC is not None - if base_word in (18, 32, 66): - owner = ci.CC.mache_lateral if base_word == 18 else ci.CC.classic_lateral - assert owner is not None - assert owner.CP is card.CP - assert isinstance(ci.CC.manual_turn_inputs, ManualTurnInputs) - assert not ci.CC.manual_turn_inputs.enabled if admitted: assert card.aol_card_intent is not None healthy = structs.CarState(canValid=True, canTimeout=False, gearShifter="drive", buttonEvents=[]) @@ -178,8 +171,6 @@ def run_startup(identity, base_word, alpha, release, topology, monkeypatch): assert active_command.enabled assert active_command.latActive == (acknowledged and not stale) assert not active_command.longActive - if base_word in (18, 32, 66): - assert not owner.human_turn_enabled finally: del controls, card, ci, subscriber gc.collect() diff --git a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py deleted file mode 100644 index a039847775..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py +++ /dev/null @@ -1,81 +0,0 @@ -from pathlib import Path -from types import SimpleNamespace - -import pytest - -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.lateral_strategy import FordLateralController -from opendbc.car.ford.values import CAR - - -@pytest.mark.parametrize("values,expected", ((None, (0.4, 0.4, 0.85)), - ((0.25, 0.65, 1.1), (0.25, 0.65, 1.1)), - ((-1.0, 2.0, 4.0), (0.0, 1.0, 1.25)), - ((2.0, -1.0, -4.0), (1.0, 0.0, 0.5)))) -def test_typed_saved_blends_reach_owner_without_model(values, expected, monkeypatch): - from openpilot.common.params import Params - from openpilot.common.prefix import OpenpilotPrefix - from openpilot.starpilot import controller_extensions - - model = SimpleNamespace(meta=SimpleNamespace(laneChangeState=0)) - sm = SimpleNamespace(update=lambda timeout: None, alive={"modelV2": False}, valid={"modelV2": False}) - - class Transport: - alive = sm.alive - valid = sm.valid - - def update(self, timeout): - return None - - def __getitem__(self, key): - return model - monkeypatch.setattr(controller_extensions.messaging, "SubMaster", lambda names: Transport()) - with OpenpilotPrefix(): - saved = Params() - keys = ("FordCurvatureBlendLow", "FordCurvatureBlendHigh", "FordCurvatureLaneChangeFactor") - if values is not None: - for key, value in zip(keys, values, strict=True): - saved.put(key, value, block=True) - inputs = controller_extensions.ManualTurnInputs(saved) - owner = FordLateralController(CarInterface.get_non_essential_params(CAR.FORD_RANGER_MK2)) - calls = [] - setter = owner.set_blend_settings - - def counted_setter(*values): - calls.append(values) - setter(*values) - monkeypatch.setattr(owner, "set_blend_settings", counted_setter) - controller = SimpleNamespace(classic_lateral=owner) - inputs.apply_blend_settings(controller) - assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == expected - inputs.update() - inputs.apply_blend_settings(controller) - assert inputs.blend_settings == expected - assert len(calls) == 1 - for key, raw in zip(keys, (b"nan", b"not-a-float", b"inf"), strict=True): - Path(saved.get_param_path(key)).write_bytes(raw) - for _ in range(99): - inputs.update() - assert inputs.blend_settings == expected - inputs.update() - inputs.apply_blend_settings(controller) - assert inputs.blend_settings == (0.4, 0.4, 0.85) - assert len(calls) == (1 if expected == (0.4, 0.4, 0.85) else 2) - assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.4, 0.4, 0.85) - - -@pytest.mark.parametrize("desired,predicted,direction", ((-0.0005, -0.002, 1), (0.0005, 0.002, 2), - (-0.001, -0.003, 2), (0.001, 0.003, 1))) -def test_saved_blends_preserve_source_units_and_lane_direction(desired, predicted, direction): - owner = FordLateralController(CarInterface.get_non_essential_params(CAR.FORD_RANGER_MK2)) - owner.set_blend_settings(0.25, 0.65, 1.1) - owner.model = SimpleNamespace(meta=SimpleNamespace(laneChangeState=2, laneChangeDirection=direction)) - blend = 0.25 + min(abs(desired) / 0.001, 1.0) * 0.4 - expected = predicted * blend + desired * (1.0 - blend) - precision = 1 - if (direction == 1 and expected < 0.0) or (direction == 2 and expected > 0.0): - expected *= 0.95 + (20.0 - 4.4) / (40.23 - 4.4) * 0.15 - precision = 0 - result, actual_precision = owner._blend_and_scale(desired, predicted, v_ego=20.0, current=0.0) - assert result == pytest.approx(expected) - assert actual_precision == precision diff --git a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py deleted file mode 100644 index db671c7139..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py +++ /dev/null @@ -1,91 +0,0 @@ -import gc -import time - -import pytest - -from opendbc.car import structs -from opendbc.car.ford.generic_canfd_lateral import GENERIC_CANFD_CARS -from opendbc.car.ford.tests.test_three_ports import params - - -@pytest.mark.parametrize("identity", sorted(GENERIC_CANFD_CARS)) -def test_generic_actual_card_publication_reaches_blend_settings(identity, monkeypatch): - alpha = release = disable = False - from opendbc.car import car_helpers, gen_empty_fingerprint - from opendbc.car.ford.generic_canfd_lateral import qualified as generic_qualified - from openpilot.selfdrive.car import card as card_module - from openpilot.cereal import messaging - from openpilot.common.params import Params - from openpilot.common.prefix import OpenpilotPrefix - from openpilot.selfdrive.car.card import Car - from openpilot.selfdrive.controls.controlsd import Controls - from openpilot.starpilot.controller_extensions import ManualTurnInputs - from openpilot.starpilot.lateral.tests.test_lane_runtime import feed - - monkeypatch.setenv('SIMULATION', '1') - monkeypatch.setenv('REPLAY', '1') - with OpenpilotPrefix(): - saved = Params() - saved.put_bool('OpenpilotEnabledToggle', True, block=True) - saved.put_bool('AlwaysOnLateral', False, block=True) - saved.put_bool('AlphaLongitudinalEnabled', alpha, block=True) - saved.put_bool('IsReleaseBranch', release, block=True) - saved.put_bool('DisableOpenpilotLongitudinal', disable, block=True) - saved.put_bool('FordHumanTurnDetection', False, block=True) - for key, value in (("FordCurvatureBlendLow", 0.25), ("FordCurvatureBlendHigh", 0.65), ("FordCurvatureLaneChangeFactor", 1.1)): - saved.put(key, value, block=True) - cp = params(identity, alpha, release) - cp.carVin = '0' * 17 - cp.carFw = [] - expected = cp.to_dict() - fingerprint = gen_empty_fingerprint() - fingerprint[0][0x5A] = 8 - fingerprint[2][0x3D6] = 8 - fingerprint[2][0x186] = 8 - observed = (identity, fingerprint, cp.carVin, [], cp.fingerprintSource, True) - subscriber = messaging.sub_sock('carParams', timeout=100, conflate=True) - ci = card = controls = None - try: - with monkeypatch.context() as discovery: - discovery.setattr(car_helpers, 'fingerprint', lambda *args: observed) - discovery.setattr(card_module, 'can_comm_callbacks', lambda *args: (lambda *args: [], lambda frames: None)) - initial_can = messaging.new_message('can', 1) - discovery.setattr(card_module.messaging, 'recv_one_retry', lambda socket: initial_can) - card = Car() - ci = card.CI - assert card.CP.to_dict() == expected - assert generic_qualified(card.CP) - owner = ci.CC.classic_lateral - assert owner is not None - assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.25, 0.65, 1.1) - assert isinstance(ci.CC.manual_turn_inputs, ManualTurnInputs) - assert not ci.CC.manual_turn_inputs.enabled - assert not ci.CC.mache_extended_announced and not ci.CC.classic_extended_announced - with structs.CarParams.from_bytes(saved.get('CarParams')) as published: - assert published.to_dict() == expected - event = None - for _ in range(10): - card.car_params_published = False - card.state_publish(ci.update([]), None) - event = messaging.recv_one(subscriber) - if event is not None: - break - time.sleep(.01) - assert event is not None and event.valid and event.carParams.to_dict() == expected - controls = Controls() - assert controls.CP.to_dict() == expected - feed(controls, 1_000_000_000, 0, active=False, enabled=False, can_valid=False, can_timeout=True) - command, lateral_log = controls.state_control() - assert not command.enabled and not command.latActive and not command.longActive - controls.publish(command, lateral_log) - _, packets = ci.apply(command.as_reader(), 1_000_000_000) - assert packets - assert not ci.CC.classic_extended_announced - heartbeat = [data for address, data, _ in packets if address == 0x3CA] - assert heartbeat and all(not (data[4] & 2) for data in heartbeat) - assert all(((data[0] >> 4) & 7) == 0 for address, data, _ in packets if address == 0x3D6) - assert not owner.human_turn_enabled - assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.25, 0.65, 1.1) - finally: - del controls, card, ci, subscriber - gc.collect() diff --git a/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py b/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py new file mode 100644 index 0000000000..fc24044021 --- /dev/null +++ b/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py @@ -0,0 +1,141 @@ +"""BluePilot lateral end to end, without a device: StarPilot's car-init hook sets the startup bits, the real Ford +CarController runs with BluePilot inputs, and every steering and LKA frame it sends goes through libsafety configured +with the same Ford param.""" +import math + +import pytest + +from opendbc.can import CANPacker +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.ford.carcontroller import CarController +from opendbc.car.ford.carstate import CarState +from opendbc.car.ford.interface import CarInterface +from opendbc.car.ford.values import CAR, DBC, FordFlags +from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model +from opendbc.bluepilot_lateral.core.values_ext import BP_LATERAL_PARAMS, FordSafetyFlagsSP +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import FrameInputs +from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import bp_safety_param, init_ford_safety_param_starpilot +from opendbc.bluepilot_lateral.params.manifest import value as manifest_value +from opendbc.bluepilot_lateral.params.param_store import ParamStore +from opendbc.safety.tests.libsafety import libsafety_py + +SPEED = 20.0 # m/s: above the 10 m/s gate of the angle-mode deviation check +T_IDXS = [10.0 * (i / 32) ** 2 for i in range(33)] +INACTIVE_PATH_ANGLE = 1000 +STARTUP = { + "off": {"disable_BP_lat_UI": "1"}, + "on": {"disable_BP_lat_UI": "0"}, + "on+pinion": {"disable_BP_lat_UI": "0", "FordPrefSteerAngleCurvature": "1"}, + "pinion-only": {"disable_BP_lat_UI": "1", "FordPrefSteerAngleCurvature": "1"}, +} + + +class StraightRoadInputs: + """A road of constant curvature, refreshed every frame like StarPilot's BluePilotInputs.""" + def __init__(self, settings): + self.settings = settings + self.curvature = 0.0 + + def update(self): + frame = FrameInputs(params=[FrameInputs.Param(k, v) for k, v in self.settings.items()]) + li = frame.lateralInputs + li.valid = True + li.pathYawRate = [self.curvature * SPEED] * 33 + li.pathX = [SPEED * t for t in T_IDXS] + li.pathY = [0.5 * self.curvature * (SPEED * t) ** 2 for t in T_IDXS] + li.laneLineLeftX = li.laneLineRightX = list(li.pathX) + li.laneLineLeftY = [y - 1.8 for y in li.pathY] + li.laneLineRightY = [y + 1.8 for y in li.pathY] + li.laneLineProbs = [0.1, 0.9, 0.9, 0.1] + li.laneLineStds = [1.0, 0.1, 0.1, 1.0] + li.steerRatio, li.stiffnessFactor, li.lateralDelay = 15.0, 1.0, 0.15 + return frame + + +def settings(startup, angle): + defaults = {k: manifest_value(ParamStore([]), k) for k in BP_LATERAL_PARAMS} + encoded = {k: (b"1" if v else b"0") if isinstance(v, bool) else str(v).encode() for k, v in defaults.items()} + encoded["disable_BP_lat_UI"] = startup["disable_BP_lat_UI"].encode() + encoded["FordPrefLateralControl"] = b"1" if angle else b"0" + return encoded + + +def path_angle(address, data): + return ((data[3] << 3) | (data[4] >> 5)) if address == 0x3D3 else (((data[3] & 0x1F) << 6) | (data[4] >> 2)) + + +@pytest.mark.parametrize("car", (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14)) +@pytest.mark.parametrize("startup", STARTUP) +@pytest.mark.parametrize("angle", (False, True), ids=("curvature", "angle")) +def test_every_steering_frame_passes_panda_safety(car, startup, angle): + fp = gen_empty_fingerprint() + fp[0][0x5A] = 8 + fp[2].update({0x3D6: 8, 0x186: 8}) + cp = CarInterface.get_params(car, fp, [], False, False, False) + init_ford_safety_param_starpilot(cp, STARTUP[startup]) + bits = bp_safety_param(cp) + lateral = startup in ("on", "on+pinion") + assert bool(bits & FordSafetyFlagsSP.BP_LATERAL) == lateral + assert bool(bits & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) == (startup == "on+pinion") + + controller = CarController(DBC[cp.carFingerprint], cp) + inputs = StraightRoadInputs(settings(STARTUP[startup], angle)) + controller.bluepilot_inputs = inputs + state = CarState(cp) + state.update(state.get_can_parsers(cp)) + vm = pinion_vehicle_model(cp) + + safety, packer = libsafety_py.libsafety, CANPacker("ford_lincoln_base_pt") + safety.init_tests() + safety.set_alternative_experience(0) + assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0 + safety.set_controls_allowed(True) + + def rx(name, values): + address, data, bus = packer.make_can_msg(name, 0, values) + assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)), name + + sent, blocked, path_angles = 0, [], set() + timer = 1_000_000 + try: + for frame in range(200): + curvature = 0.0015 * math.sin(frame / 60) + inputs.curvature = curvature + timer += 10_000 + safety.set_timer(timer) + # the car tracks the command: yaw (or the pinion angle) measures the commanded curvature + angle_deg = math.degrees(vm.get_steer_from_curvature(-curvature, SPEED, 0.0)) + if frame % 2 == 0: + rx("BrakeSysFeatures", {"Veh_V_ActlBrk": SPEED * 3.6, "VehVActlBrk_D_Qf": 3, "VehVActlBrk_No_Cnt": (frame // 2) % 16}) + rx("EngVehicleSpThrottle2", {"Veh_V_ActlEng": SPEED * 3.6, "VehVActlEng_D_Qf": 3}) + rx("DesiredTorqBrk", {"VehStop_D_Stat": 0}) + rx("Yaw_Data_FD1", {"VehYaw_W_Actl": -curvature * SPEED, "VehYawWActl_D_Qf": 3, "VehRollYaw_No_Cnt": frame % 256}) + rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 0}) + if frame % 10 == 0: + rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 5}) + rx("Steering_Data_FD1", {}) + if bits & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE: + rx("SteeringPinion_Data", {"StePinComp_An_Est": angle_deg, "StePinCompAnEst_D_Qf": 3, "StePinAn_No_Cnt": frame % 16}) + + cs = structs.CarState(vEgo=SPEED, vEgoRaw=SPEED, yawRate=-curvature * SPEED, steeringAngleDeg=angle_deg, + canValid=True, gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = cs.cruiseState.enabled = True + state.out = cs.as_reader() + cc = structs.CarControl(enabled=True, latActive=True) + cc.actuators.curvature = curvature + _, sends = controller.update(cc.as_reader(), state, timer * 1000) + for address, data, bus in sends: + if address in (0x3CA, 0x3D3, 0x3D6): + sent += 1 + if address != 0x3CA: + path_angles.add(path_angle(address, data)) + if not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)): + blocked.append((frame, hex(address), data.hex())) + finally: + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + + assert sent > 100 and not blocked, blocked[:5] + assert controller.bp.disable_BP_lat_UI == (not lateral) + # angle mode steers through the path angle; the stock path and curvature mode at this curvature don't + assert (path_angles != {INACTIVE_PATH_ANGLE}) == (lateral and angle), sorted(path_angles)[:5] + assert bool(cp.flags & FordFlags.CANFD) == (car == CAR.FORD_F_150_MK14) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_classic_generic.py b/opendbc_repo/opendbc/car/ford/tests/test_classic_generic.py deleted file mode 100644 index e5db248e3a..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_classic_generic.py +++ /dev/null @@ -1,166 +0,0 @@ -"""One classic contract, distinct preview and equipment identities.""" -from types import SimpleNamespace - -import pytest - -from opendbc.car import gen_empty_fingerprint -from opendbc.car.ford.classic_lateral import GENERIC_CLASSIC_CARS, create_controller, qualified -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.values import CAR, FordFlags - - -@pytest.mark.parametrize("car", sorted(GENERIC_CLASSIC_CARS)) -@pytest.mark.parametrize("alpha,release", ((False, False), (True, False), (False, True), (True, True))) -def test_actual_classic_identity_and_explicit_long_owner(car, alpha, release): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(car, fp, [], alpha, release, False) - assert cp.alphaLongitudinalAvailable - assert cp.openpilotLongitudinalControl == alpha - assert cp.safetyConfigs[-1].safetyParam == (33 if alpha else 32) - assert cp.steerActuatorDelay == pytest.approx(.22) - assert qualified(cp) - from openpilot.selfdrive.controls.lib.longcontrol import LongControl - control = LongControl(cp) - assert control.pid.k_p == 0. - assert control.pid.k_i == .5 - - -@pytest.mark.parametrize("car", sorted(GENERIC_CLASSIC_CARS)) -@pytest.mark.parametrize("delay,expected", ((.1, .2), (.3, .3), (.8, .4))) -def test_generic_model_preview_remains_live_delay(car, delay, expected): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(car, fp, [], False, False, False) - owner = create_controller(cp) - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[i * .01 for i in range(33)])) - owner.set_inputs(model, tuple(i * .1 for i in range(33)), delay, True) - assert owner._curvature_lookahead() == expected - assert owner._predicted_curvature(10., expected) == pytest.approx(expected * .01) - - -def test_explorer_fixed_preview_is_not_generalized(): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], False, False, False) - owner = create_controller(cp) - owner.set_inputs(None, (), .4, True) - assert owner._curvature_lookahead() == .2 - cp.flags = int(cp.flags) | int(FordFlags.CANFD) - assert not qualified(cp) - assert create_controller(cp) is None - - -@pytest.mark.parametrize("car", sorted(GENERIC_CLASSIC_CARS)) -@pytest.mark.parametrize("automatic,bsm", ((False, False), (True, False), (False, True), (True, True))) -def test_equipment_does_not_change_shared_profile(car, automatic, bsm): - from opendbc.car import structs - fp = gen_empty_fingerprint() - if automatic: - fp[0][0x5A] = 8 - if bsm: - fp[0][0x3A6] = fp[0][0x3A7] = 8 - cp = CarInterface.get_params(car, fp, [], False, False, False) - assert bool(cp.flags & FordFlags.HAS_BSM) == bsm - assert cp.transmissionType == (structs.CarParams.TransmissionType.automatic if automatic else structs.CarParams.TransmissionType.manual) - assert cp.minEnableSpeed == pytest.approx(-1. if automatic else 20. * .44704) - assert cp.safetyConfigs[-1].safetyParam == 32 - assert qualified(cp) - - -@pytest.mark.parametrize("car", sorted(GENERIC_CLASSIC_CARS)) -def test_namespace_and_topology_fail_closed(car): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(car, fp, [], False, False, False) - for word in (0, 1, 2, 18, 34, 35, 65535): - cp.safetyConfigs[-1].safetyParam = word - assert not qualified(cp) - cp.safetyConfigs[-1].safetyParam = 32 - cp.alternativeExperience = 32 - assert qualified(cp) - cp.safetyConfigs = [cp.safetyConfigs[0], cp.safetyConfigs[0]] - assert not qualified(cp) - cp.safetyConfigs = [cp.safetyConfigs[0]] - for experience in (1, 64, 96): - cp.alternativeExperience = experience - assert not qualified(cp) - cp.alternativeExperience = 0 - for flag in (FordFlags.CANFD, FordFlags.NEW_PORT, FordFlags.LKA_STEERING, FordFlags.ALT_STEER_ANGLE): - cp.flags = int(flag) - assert not qualified(cp) - cp.flags = 0 - cp.passive = True - assert create_controller(cp) is None - - -@pytest.mark.parametrize("car", sorted(GENERIC_CLASSIC_CARS)) -@pytest.mark.parametrize("valid", (False, True)) -def test_actual_eps_capability_gate(car, valid): - from opendbc.car import structs - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fw = structs.CarParams.CarFw.new_message() - fw.ecu = structs.CarParams.Ecu.eps - fw.request = [b"\x22\xde\x01"] - payload = bytearray(24) - payload[7] = payload[8] = 255 if valid else 0 - fw.fwVersion = bytes(payload) - cp = CarInterface.get_params(car, fp, [fw.as_reader()], False, False, False) - assert cp.dashcamOnly == (not valid) - assert qualified(cp) == valid - - -def test_actual_explorer_factory_provider_controller_keeps_fixed_preview(): - from unittest.mock import patch - from opendbc.car import structs - from opendbc.car.ford.explorer_lateral import ExplorerLateralController - from openpilot.cereal import messaging - from openpilot.common.params import Params - from openpilot.common.prefix import OpenpilotPrefix - from openpilot.selfdrive.modeld.constants import ModelConstants - from openpilot.starpilot.controller_extensions import configure_controller - with OpenpilotPrefix(): - params = Params() - params.put_bool("FordHumanTurnDetection", True, block=True) - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], False, False, False) - ci = CarInterface(cp) - configure_controller(ci, params) - assert isinstance(ci.CC.classic_lateral, ExplorerLateralController) - import time - now = time.monotonic_ns() - ci.update([(now - 10_000_000, [])]) - assert not ci.CS.out.canValid # This is dispatch/transport, not healthy raw-CAN qualification. - from opendbc.can import CANPacker - from opendbc.car import Bus - from opendbc.car.ford.values import DBC - packer = CANPacker(DBC[cp.carFingerprint][Bus.pt]) - speed_frame = packer.make_can_msg("BrakeSysFeatures", 0, {"Veh_V_ActlBrk":36., "VehVActlBrk_D_Qf":3, "VehVActlBrk_No_Cnt":1}) - out = ci.update([(now, [speed_frame])]) - assert out.vEgoRaw == pytest.approx(10., abs=.03) - assert not out.canValid # The deliberately partial raw fixture makes no complete health claim. - model = messaging.new_message("modelV2") - model.valid = True - model.logMonoTime = now - model.modelV2.orientationRate.z = [float(t) * .1 for t in ModelConstants.T_IDXS] - delay = messaging.new_message("lateralDelay") - delay.valid = True - delay.logMonoTime = now - delay.lateralDelay.lateralDelay = .4 - owner = ci.CC.manual_turn_inputs - try: - with patch("openpilot.starpilot.controller_extensions.time.monotonic_ns", return_value=now), \ - patch.object(owner.sm, "update", side_effect=lambda timeout=0: owner.sm.update_msgs(now / 1e9, [model.as_reader(), delay.as_reader()])): - # An inactive actual controller tick must still transport inputs into the selected owner. - ci.apply(structs.CarControl().as_reader(), now) - assert ci.CC.classic_lateral._curvature_lookahead() == .2 - assert ci.CC.classic_lateral._predicted_curvature(10., .2) == pytest.approx(.002) - assert owner.lateral_snapshot(10.)[2] == pytest.approx(.4) - finally: - owner.sm.sock.clear() - owner.sm.poller = None - owner.sm = None - owner.params = None - params = None diff --git a/opendbc_repo/opendbc/car/ford/tests/test_curvature_preview.py b/opendbc_repo/opendbc/car/ford/tests/test_curvature_preview.py deleted file mode 100644 index 2c72081d94..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_curvature_preview.py +++ /dev/null @@ -1,69 +0,0 @@ -import unittest -from types import SimpleNamespace - -from opendbc.can import CANParser -from opendbc.car import Bus, structs -from opendbc.car.ford.curvature_preview import blend_curvature -from opendbc.car.ford.carcontroller import CarController -from opendbc.car.ford.carstate import CarState -from opendbc.car.ford.tests.test_three_ports import params -from opendbc.car.ford.values import CAR, DBC - - -class TestCurvaturePreview(unittest.TestCase): - def test_original_default_signed_preview_and_conservative_overshoot(self): - for sign in (-1, 1): - self.assertAlmostEqual(blend_curvature(sign * .001, sign * .002, 0.), sign * .0014) - self.assertAlmostEqual(blend_curvature(sign * .001, sign * .002, sign * .003), sign * .0012) - self.assertEqual(blend_curvature(sign * .001, -sign * .002, 0.), sign * .001) - self.assertEqual(blend_curvature(0., sign * .002, 0.), 0.) - for bad in (float('nan'), float('inf'), -float('inf')): - self.assertEqual(blend_curvature(.001, bad, 0.), .001) - - def test_actual_controller_blends_before_unchanged_envelope_and_yields(self): - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - cp.safetyConfigs[0].safetyParam = 3 if cp.openpilotLongitudinalControl else 2 - controller = CarController(DBC[cp.carFingerprint], cp) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - out = structs.CarState() - out.vEgoRaw = 20. - state.out = out.as_reader() - command = structs.CarControl() - command.latActive = True - command.actuators.curvature = .001 - preview = SimpleNamespace(update=lambda: (True, False, True), preview_curvature=lambda speed: .002) - controller.manual_turn_inputs = preview - controller.apply_curvature_last = .0014 - from opendbc.car.ford.values import CarControllerParams - expected = CarControllerParams.CURVATURE_LIMITS.apply_limits(.0014, .0014, 20., 0., True, CarControllerParams.STEER_STEP) - actual, frames = controller.update(command.as_reader(), state, 0) - self.assertAlmostEqual(actual.curvature, expected) - parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('LateralMotionControl2', 20)], controller.CAN.main) - parser.update([(50_000_000, frames)]) - self.assertAlmostEqual(parser.vl['LateralMotionControl2']['LatCtlCurv_No_Actl'], -.0014, places=5) - self.assertEqual(parser.vl['LateralMotionControl2']['LatCtl_D2_Rq'], 1) - controller.frame = 5 - out.steeringPressed = True - out.steeringAngleDeg = -12. - out.steeringTorque = -1. - out.rightBlinker = True - state.out = out.as_reader() - actual, _ = controller.update(command.as_reader(), state, 50_000_000) - self.assertEqual(actual.curvature, 0.) - - def test_neighbor_controller_does_not_query_preview(self): - cp = params(CAR.FORD_F_150_MK14) - controller = CarController(DBC[cp.carFingerprint], cp) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - state.out = structs.CarState(vEgoRaw=20.).as_reader() - command = structs.CarControl(latActive=True) - command.actuators.curvature = .001 - - def unexpected(speed): - raise AssertionError('neighbor queried Mach-E preview') - controller.manual_turn_inputs = SimpleNamespace(update=lambda: None, apply_blend_settings=lambda owner: None, - lateral_snapshot=lambda speed: None, enabled=True, - preview_curvature=unexpected) - controller.update(command.as_reader(), state, 0) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_explorer_lateral.py b/opendbc_repo/opendbc/car/ford/tests/test_explorer_lateral.py deleted file mode 100644 index a71cf28a24..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_explorer_lateral.py +++ /dev/null @@ -1,185 +0,0 @@ -"""Explorer fixed preview must not broaden the admitted Mach-E strategy.""" -from types import SimpleNamespace -from opendbc.car.ford.interface import CarInterface - -import pytest - -from opendbc.car.ford.explorer_lateral import ExplorerLateralController -from opendbc.car.ford.mache_lateral import MachELateralController -from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags - -def controller_params(car, flags): - cp = CarInterface.get_non_essential_params(car) - return SimpleNamespace(brand=cp.brand, safetyConfigs=cp.safetyConfigs, carFingerprint=car, flags=flags) - - - -def model(): - return SimpleNamespace(orientationRate=SimpleNamespace(z=[i * 0.01 for i in range(33)])) - - -@pytest.mark.parametrize("delay", (0.20, 0.30, 0.40)) -def test_explorer_fixed_lookahead_selects_original_model_sample(delay): - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - owner.set_inputs(model(), tuple(i * 0.1 for i in range(33)), delay, True) - assert owner._curvature_lookahead() == 0.20 - assert owner._predicted_curvature(10.0, owner._curvature_lookahead()) == pytest.approx(0.002) - - -def test_mach_e_compatibility_keeps_current_provider_lookahead(): - owner = MachELateralController(controller_params(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD)) - owner.set_inputs(model(), tuple(i * 0.1 for i in range(33)), 0.40, True) - assert owner._curvature_lookahead() == 0.40 - assert owner._predicted_curvature(10.0, owner._curvature_lookahead()) == pytest.approx(0.004) - - -def test_explorer_strategy_rejects_unrelated_topology(): - for car, flags in ((CAR.FORD_EXPLORER_MK6, FordFlags.CANFD), - (CAR.FORD_EDGE_MK2, FordFlags.ALT_STEER_ANGLE | FordFlags.NEW_PORT)): - with pytest.raises(ValueError): - ExplorerLateralController(controller_params(car, flags)) - - -@pytest.mark.parametrize("alpha,release", ((False, False), (True, False), (False, True), (True, True))) -def test_actual_interface_restores_explicit_longitudinal_ownership(alpha, release): - from opendbc.car import gen_empty_fingerprint - from opendbc.car.ford.interface import CarInterface - from opendbc.car.ford.explorer_lateral import qualified - from opendbc.car.ford.stock_cruise import qualified as stock_switch_qualified - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], alpha, release, False) - assert cp.alphaLongitudinalAvailable - assert cp.openpilotLongitudinalControl == alpha - assert cp.safetyConfigs[-1].safetyParam == (33 if alpha else 32) - assert cp.steerActuatorDelay == pytest.approx(.22) - from openpilot.selfdrive.controls.lib.longcontrol import LongControl - control = LongControl(cp) - assert control.pid.k_p == 0. - assert control.pid.k_i == .5 - assert list(cp.longitudinalTuning.kiV) == [.5] - assert qualified(cp) - assert stock_switch_qualified(cp) == (not alpha) - - -def test_classic_modern_envelope_withdraws_infeasible_speed_transition(): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - result = bounded_command(owner, FordLateralResult(curvature=.02, active=True), .02, 20., .02) - assert not result.active - assert result.curvature == owner.curvature_last == 0. - assert result.path_angle == owner.path_angle_last == 0. - - -def test_manual_turn_withdraws_without_mach_e_latch_or_gain_compensation(): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - owner.manual_turn_detected = True - result = bounded_command(owner, FordLateralResult(active=True), .002, 10., .002) - assert not result.active - assert result.curvature == 0. - assert not owner.manual_turn_latched - - -def test_speed_change_cap_cannot_escape_original_rate_interval(): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - # At26m/s modernjerk admits~.000265, originalnative lookup only.000180. - # Modern bank-tolerant cap~.005309 requires.000211 fromprevious.005520. - result = bounded_command(owner, FordLateralResult(curvature=.00552, active=True), .00552, 26., .00552) - assert not result.active - assert result.curvature == owner.curvature_last == 0. - - -def test_actual_long_control_uses_original_zero_p_half_i_and_physical_units(): - from opendbc.car import gen_empty_fingerprint - from opendbc.car.ford.interface import CarInterface - from openpilot.selfdrive.controls.lib.longcontrol import LongControl - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], True, False, False) - owner = LongControl(cp) - assert owner.extension is None - assert owner.pid.k_p == 0. - assert owner.pid.k_i == .5 - limits = CarInterface.get_pid_accel_limits(cp, 10., 20.) - assert limits == pytest.approx((-3.5, 2.)) - assert owner.pid.update(0., speed=10., feedforward=.2) == pytest.approx(.2) - - -def test_typed_offset_eps_and_namespace_guards(): - from opendbc.car import gen_empty_fingerprint, structs - from opendbc.car.ford.interface import CarInterface - from opendbc.car.ford.explorer_lateral import qualified - fp = gen_empty_fingerprint() - fp[4][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], False, False, False) - assert len(cp.safetyConfigs) == 2 - assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput - assert cp.safetyConfigs[-1].safetyParam == 32 - assert qualified(cp) - cp.alternativeExperience = 1 - assert not qualified(cp) - cp.alternativeExperience = 0 - cp.safetyConfigs[0].safetyParam = 1 - assert not qualified(cp) - invalid = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, request=[b"\x22\xde\x01"], fwVersion=b"bad") - bad = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [invalid], False, False, False) - assert bad.dashcamOnly - assert bad.safetyConfigs[-1].safetyParam == 0 - assert not qualified(bad) - - -@pytest.mark.parametrize("sign", (-1., 1.)) -def test_final_intersection_preserves_source_rise_and_unwind(sign): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - rise = bounded_command(owner, FordLateralResult(curvature=sign*.0015, active=True), sign*.001, 26., sign*.001) - assert rise.curvature == pytest.approx(sign*.00108) - fall = bounded_command(owner, FordLateralResult(curvature=0., active=True), sign*.001, 26., sign*.001) - assert fall.curvature == pytest.approx(sign*.00082) - - -@pytest.mark.parametrize("rate,expected", ((.001024, .00102375), (-.001024, -.001024), - (.00102375, .00102375), (-.00102425, -.001024))) -def test_classic_rate_endpoint_has_correct_decoded_sign(rate, expected): - from opendbc.can import CANPacker, CANParser - from opendbc.car import Bus - from opendbc.car.ford.values import DBC - from opendbc.car.ford.extended_classic_can import create_extended_classic_lat_ctl_msg - dbc = DBC[CAR.FORD_EXPLORER_MK6][Bus.pt] - frame = create_extended_classic_lat_ctl_msg(CANPacker(dbc), SimpleNamespace(main=0), True, 2, 1, 0., rate) - parser = CANParser(dbc, [("LateralMotionControl", 20)], 0) - parser.update([(50_000_000, [frame])]) - actual = parser.vl["LateralMotionControl"]["LatCtlCurv_NoRate_Actl"] - assert actual == pytest.approx(expected, abs=1e-10) - assert actual * rate > 0. - - -@pytest.mark.parametrize("sign", (-1., 1.)) -def test_measured_recovery_with_empty_source_interval_withdraws(sign): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - command = bounded_command(owner, FordLateralResult(curvature=sign*.008, active=True), 0., 35., sign*.008) - assert not command.active and command.curvature == 0. - assert owner.curvature_last == owner.path_angle_last == 0. - - -@pytest.mark.parametrize("sign", (-1., 1.)) -def test_measured_window_requires_recovery_and_bounds_unwind(sign): - from opendbc.car.ford.explorer_lateral import bounded_command - from opendbc.car.ford.lateral_strategy import FordLateralResult - owner = ExplorerLateralController(controller_params(CAR.FORD_EXPLORER_MK6, 0)) - # Recovery outside the error window may proceed when source fall permits it. - recovery = bounded_command(owner, FordLateralResult(curvature=sign*.001, active=True), sign*.003, 35., 0.) - delta = CarControllerParams.CURVATURE_LIMITS.MAX_LATERAL_JERK / 35.**2 * .05 - assert recovery.active - assert recovery.curvature == pytest.approx(sign*(.003-delta)) - # Once inside, even an unwind demand cannot leave the measured error window. - unwind = bounded_command(owner, FordLateralResult(curvature=0., active=True), sign*.00195, 20., sign*.00395) - assert unwind.active and unwind.curvature == pytest.approx(sign*.00195) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_generic_canfd.py b/opendbc_repo/opendbc/car/ford/tests/test_generic_canfd.py deleted file mode 100644 index 8b77995aff..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_generic_canfd.py +++ /dev/null @@ -1,219 +0,0 @@ -"""Exact source-owned CAN FD identity/release and physical wire contracts.""" -from types import SimpleNamespace -import pytest -from opendbc.car import gen_empty_fingerprint -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.generic_canfd_lateral import GENERIC_CANFD_CARS, qualified, bounded_command, SOURCE_ACCEL_CAP -from opendbc.car.ford.lateral_strategy import FordLateralResult -from opendbc.car.ford.values import CAR, FordFlags - - -@pytest.mark.parametrize("car", sorted(GENERIC_CANFD_CARS)) -@pytest.mark.parametrize("alpha,release", ((False,False),(True,False),(False,True),(True,True))) -def test_actual_profile_and_release_long_support(car, alpha, release): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fp[2].update({0x3D6:8,0x186:8}) - cp = CarInterface.get_params(car, fp, [], alpha, release, False) - assert cp.alphaLongitudinalAvailable - assert cp.openpilotLongitudinalControl == alpha - assert cp.safetyConfigs[-1].safetyParam == (67 if alpha else 66) - assert cp.steerActuatorDelay == pytest.approx(.22) - assert qualified(cp) - - -def test_unknown_identity_flags_and_secoctopology_denied(): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fp[2].update({0x3D6:8,0x186:8}) - cp = CarInterface.get_params(CAR.FORD_RANGER_MK2, fp, [], False, False, False) - for flag in (FordFlags.NEW_PORT, FordFlags.LKA_STEERING, FordFlags.ALT_STEER_ANGLE): - bad = cp.as_reader().as_builder() - bad.flags = int(bad.flags) | int(flag) - assert not qualified(bad) - bad = cp.as_reader().as_builder() - bad.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - assert not qualified(bad) - fp[2][0x3D6] = 16 - bad = CarInterface.get_params(CAR.FORD_RANGER_MK2, fp, [], False, False, False) - assert bad.dashcamOnly and not qualified(bad) - - -def test_old_canfd_cap_intersection_withdraws_without_history_leak(): - owner = SimpleNamespace(manual_turn_detected=False, curvature_last=.006, path_angle_last=0.) - command = bounded_command(owner, FordLateralResult(active=True,curvature=.006), .006, 25., 0.) - assert not command.active and owner.curvature_last == 0. and owner.path_angle_last == 0. - owner = SimpleNamespace(manual_turn_detected=False, curvature_last=0., path_angle_last=0.) - command = bounded_command(owner, FordLateralResult(active=True,curvature=.001), 0., 5., 0.) - assert command.active and abs(command.curvature) <= SOURCE_ACCEL_CAP / 25. - - -def test_f150_source_metadata_reaches_dynamic_consumers(): - from opendbc.car import STD_CARGO_KG - from opendbc.car.interfaces import scale_rot_inertia, scale_tire_stiffness - from opendbc.car.vehicle_model import VehicleModel - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_F_150_MK14, fp, [], False, False, False) - assert cp.mass == pytest.approx(3334 + STD_CARGO_KG) - assert cp.wheelbase == pytest.approx(3.99) - assert cp.rotationalInertia == pytest.approx(scale_rot_inertia(cp.mass, cp.wheelbase)) - front,rear = scale_tire_stiffness(cp.mass, cp.wheelbase, cp.centerToFront, cp.tireStiffnessFactor) - assert cp.tireStiffnessFront == pytest.approx(front) - assert cp.tireStiffnessRear == pytest.approx(rear) - vm = VehicleModel(cp) - assert vm.m == cp.mass and vm.l == cp.wheelbase - - -@pytest.mark.parametrize("delay,expected", ((.1,.2),(.3,.3),(.8,.4))) -def test_live_preview_is_not_mache_or_explorer_override(delay, expected): - from opendbc.car.ford.generic_canfd_lateral import GenericCanfdLateralController - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_RANGER_MK2, fp, [], False, False, False) - owner = GenericCanfdLateralController(cp) - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[i*.01 for i in range(33)])) - owner.set_inputs(model, tuple(i*.1 for i in range(33)), delay, True) - assert owner._curvature_lookahead() == expected - assert owner._predicted_curvature(10.,expected) == pytest.approx(expected*.01) - - -def test_wire_rate_endpoint_does_not_wrap_sign(): - from opendbc.can import CANPacker - from opendbc.car import Bus - from opendbc.car.ford.values import DBC - from opendbc.car.ford.fordcan import CanBus - from opendbc.car.ford.generic_canfd_can import create_extended_canfd_msg - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_RANGER_MK2, fp, [], False, False, False) - packer = CANPacker(DBC[cp.carFingerprint][Bus.pt]) - for rate,raw in ((-.001024,0),(.001023,2047),(.001024,2047)): - address,data,bus = create_extended_canfd_msg(packer,CanBus(cp),True,2,1,0.,rate,3) - assert address == 0x3D6 and bus == 0 - assert ((data[6] << 3) | (data[7] >> 5)) == raw - # Independent field-based checksum includes actual packed endpoint. - mode = (data[0] >> 4) & 7 - counter = (data[7] >> 1) & 15 - fields = ((data[2] << 3) | (data[3] >> 5), raw, - ((data[3] & 31) << 6) | (data[4] >> 2), ((data[4] & 3) << 8) | data[5]) - assert data[1] == 255-((mode+counter+sum(v+(v >> 8) for v in fields)) & 255) - - -@pytest.mark.parametrize("car", sorted(GENERIC_CANFD_CARS) + [CAR.FORD_MUSTANG_MACH_E_MK1]) -def test_actual_camera_lead_parser_and_source_window(car): - from opendbc.can import CANPacker - from opendbc.car import Bus - from opendbc.car.ford.values import DBC, RADAR - from opendbc.car.ford.radar_interface import RadarInterface - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(car, fp, [], False, False, False) - assert not cp.radarUnavailable and DBC[car][Bus.radar] == RADAR.STEER_ASSIST_DATA - radar = RadarInterface(cp) - assert radar.rcp is not None and radar.rcp.bus == 2 - packer = CANPacker(RADAR.STEER_ASSIST_DATA) - tick = 0 - - def feed(distance=40., velocity=-1., confidence=3): - nonlocal tick - tick += 1 - msg = packer.make_can_msg("Steer_Assist_Data",2,{ - "CmbbObjConfdnc_D_Stat":confidence,"CmbbObjDistLong_L_Actl":distance, - "CmbbObjRelLong_V_Actl":velocity,"CmbbObjDistLat_L_Actl":1.2}) - return radar.update([(1_000_000_000+tick*50_000_000,[msg])]) - - for _ in range(30): - result = feed() - assert radar.rcp.can_valid and len(result.points) == 1 - point = result.points[0] - assert point.dRel == pytest.approx(40.) and point.yRel == pytest.approx(1.2) - assert point.vRel == pytest.approx(-1.) - # Exercise the real radard point consumer and typed radarState lead carrier. - from openpilot.selfdrive.controls.radard import Track, KalmanParams, get_lead, RADAR_TO_CAMERA - from openpilot.cereal import messaging - model = messaging.new_message('modelV2') - model.modelV2.init('leadsV3', 2) - vision = model.modelV2.leadsV3[0] - vision.x = [point.dRel + RADAR_TO_CAMERA] - vision.y = [-point.yRel] - vision.v = [10. + point.vRel] - vision.xStd = [1.] - vision.yStd = [1.] - vision.vStd = [1.] - consumer = Track(point.trackId, 10.+point.vRel, KalmanParams(.05)) - consumer.update(point.dRel, point.yRel, point.vRel, 10.+point.vRel) - lead = get_lead(10., True, {point.trackId:consumer}, vision.as_reader(), 10., .9) - state = messaging.new_message('radarState') - state.radarState.leadOne = lead - decoded = state.as_reader().radarState.leadOne - assert decoded.present and decoded.radar and decoded.radarTrackId == point.trackId - assert decoded.dRel == pytest.approx(point.dRel) and decoded.yRel == pytest.approx(point.yRel) - assert decoded.vRel == pytest.approx(point.vRel) - track = point.trackId - expected_window = [] - previous = 40. - for i in range(1,24): - distance = 40.-i*.1 - result = feed(distance,0.) - expected_window.append(distance-previous) - expected_window = expected_window[-20:] - assert result.points[0].vRel == pytest.approx(sum(expected_window),abs=1e-5) - assert result.points[0].trackId == track - previous = distance - result = feed(30.,-4.) - assert result.points[0].trackId != track and not radar.v_rel_history - assert not feed(confidence=0).points and not radar.v_rel_history - assert feed(20.,-2.).points[0].vRel == pytest.approx(-2.) - assert not feed(distance=0).points - from opendbc.can.parser import CAN_INVALID_CNT - for tick in range(CAN_INVALID_CNT): - radar.rcp.update([(10_000_000_000 + tick * 50_000_000, [])]) - radar._update_steer_assist() - assert not radar.rcp.can_valid and not radar.pts and not radar.v_rel_history - - -def test_camera_mapping_is_explicit_for_mache(): - from opendbc.car import Bus - from opendbc.car.ford.values import DBC - assert DBC[CAR.FORD_MUSTANG_MACH_E_MK1][Bus.radar] == "ford_lincoln_base_pt" - - -def test_f150_original_wheelbase_changes_real_plant_and_turn_gate(): - from opendbc.car import STD_CARGO_KG - from opendbc.car.vehicle_model import VehicleModel - from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - cp = CarInterface.get_params(CAR.FORD_F_150_MK14, fp, [], False, False, False) - assert cp.mass == pytest.approx(3334 + STD_CARGO_KG) - assert cp.wheelbase == pytest.approx(3.99) - vm = VehicleModel(cp) - assert vm.calc_curvature(.1, 0., 0.) == pytest.approx(.1 / (17. * 3.99)) - assert vm.get_steer_from_curvature(.001, 0., 0.) == pytest.approx(.001 * 17. * 3.99) - # Compare the actual reached turn gate to the prior upstream wheelbase, - # not a speculative trim or a tuned controller gain. Only this consumer's - # wheelbase is changed; no alternate source/plant is imported. - prior = cp.as_reader().as_builder() - prior.wheelbase = 3.69 - restored = get_cruise_accel(False, 35., 25., 0., 12.4, cp, 1., 0., True) - upstream = get_cruise_accel(False, 35., 25., 0., 12.4, prior, 1., 0., True) - assert upstream == pytest.approx(0.) - assert 0. < restored <= .8 - - -@pytest.mark.parametrize("alpha,release", ((False, False), (True, False), (False, True), (True, True))) -@pytest.mark.parametrize("bsm", (False, True)) -def test_mache_camera_source_preserves_exact_factory_words(alpha, release, bsm): - from opendbc.car.ford.radar_interface import RadarInterface - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fp[2].update({0x3D6: 8, 0x186: 8}) - if bsm: - fp[0].update({0x3A6: 8, 0x3A7: 8}) - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fp, [], alpha, release, False) - assert cp.safetyConfigs[-1].safetyParam == (19 if alpha else 18) - assert cp.openpilotLongitudinalControl == alpha - assert not cp.radarUnavailable - radar = RadarInterface(cp) - assert radar.rcp.bus == 2 and radar.generic_canfd_lead diff --git a/opendbc_repo/opendbc/car/ford/tests/test_mach_e_profile.py b/opendbc_repo/opendbc/car/ford/tests/test_mach_e_profile.py deleted file mode 100644 index 6f16abfa0c..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_mach_e_profile.py +++ /dev/null @@ -1,52 +0,0 @@ -import unittest - -from opendbc.car import gen_empty_fingerprint, structs -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.values import CAR, FordSafetyFlags - - -def fingerprint(offset=0, secoc=False): - fp = gen_empty_fingerprint() - fp[offset][0x5A] = 8 - fp[offset + 2][0x3D6] = 16 if secoc else 8 - fp[offset + 2][0x186] = 16 if secoc else 8 - return fp - - -class TestMachEProfile(unittest.TestCase): - def test_actual_params_stock_debug_long_and_release_long(self): - for alpha, release, word in ((False, False, 18), (True, False, 19), (False, True, 18), (True, True, 19)): - with self.subTest(alpha=alpha, release=release): - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint(), [], alpha, release, False) - self.assertEqual(cp.safetyConfigs[-1].safetyParam, word) - self.assertEqual(cp.openpilotLongitudinalControl, word == 19) - self.assertEqual(cp.alternativeExperience, 0) - if release: - self.assertTrue(cp.alphaLongitudinalAvailable) - - def test_secoc_unrelated_flags_and_ae_do_not_select_extension(self): - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint(secoc=True), [], False, False, False) - self.assertTrue(cp.dashcamOnly) - self.assertFalse(cp.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_EXTENDED) - for added_flag, ae in ((4, 0), (8, 0), (16, 0), (0, 32)): - with self.subTest(added_flag=added_flag, ae=ae): - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint(), [], False, False, False) - cp.flags |= added_flag - cp.alternativeExperience = ae - cp = CarInterface._get_params(cp, CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint(), [], False, False, False) - self.assertFalse(cp.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_EXTENDED) - - def test_sibling_canfd_uses_separate_generic_profile(self): - for candidate in (CAR.FORD_F_150_MK14, CAR.FORD_ESCAPE_MK4_5, CAR.FORD_F_150_LIGHTNING_MK1): - for alpha, word in ((False, 66), (True, 67)): - with self.subTest(candidate=candidate, alpha=alpha): - cp = CarInterface.get_params(candidate, fingerprint(), [], alpha, False, False) - self.assertEqual(cp.safetyConfigs[-1].safetyParam, word) - self.assertFalse(cp.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_EXTENDED) - - def test_bus_offset_keeps_no_output_first(self): - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint(offset=4), [], False, False, False) - self.assertEqual(len(cp.safetyConfigs), 2) - self.assertEqual(cp.safetyConfigs[0].safetyModel, structs.CarParams.SafetyModel.noOutput) - self.assertEqual(cp.safetyConfigs[0].safetyParam, 0) - self.assertEqual(cp.safetyConfigs[1].safetyParam, 18) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py b/opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py deleted file mode 100644 index 776aa67d37..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py +++ /dev/null @@ -1,184 +0,0 @@ -"""Recorded Mach-E turn inputs retain Dom display state across steering handoff.""" -from pathlib import Path -from types import SimpleNamespace - -import pytest - -from opendbc.can import CANParser -from opendbc.car import gen_empty_fingerprint, structs -from opendbc.car.ford.carcontroller import CarController -from opendbc.car.ford.carstate import CarState -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.values import CAR, DBC -from opendbc.safety.tests.libsafety import libsafety_py -from opendbc.safety.tests import test_ford_aol -from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.starpilot.car.ford.cluster_preferences import FordClusterPreference, KEY - - -class RecordedPreview: - def __init__(self): - self.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - - def update(self): - pass - - def apply_blend_settings(self, controller): - lateral = controller.mache_lateral or controller.classic_lateral - lateral.set_blend_settings(.4, .4, .85) - - def lateral_snapshot(self, speed): - return self.model, ModelConstants.T_IDXS, .2, True - - def assist_permission(self): - return (), False - - -def run_turn(*, hands_free, angle, torque, desired, speed, yaw, identity=CAR.FORD_MUSTANG_MACH_E_MK1): - native = test_ford_aol.TestFordAolDriverIntent() - native.setUp() - native.reset(18 if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else 66) - native.arm() - with OpenpilotPrefix(): - saved = Params() - Path(saved.get_param_path(KEY)).write_bytes(b"1" if hands_free else b"0") - fingerprint = gen_empty_fingerprint() - fingerprint[0].update({0x5A: 8, 0x3A6: 8, 0x3A7: 8}) - fingerprint[2].update({0x3D6: 8, 0x186: 8}) - cp = CarInterface.get_params(identity, fingerprint, [], False, False, False) - cp.alternativeExperience = 32 - assert cp.safetyConfigs[-1].safetyParam == (18 if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else 66) - assert cp.flags == 3 - controller = CarController(DBC[cp.carFingerprint], cp) - controller.hands_free_cluster_input = FordClusterPreference(saved) - controller.manual_turn_inputs = RecordedPreview() - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - cs = structs.CarState(canValid=True, vEgo=speed, vEgoRaw=speed, yawRate=yaw, - gearShifter=structs.CarState.GearShifter.drive) - cs.cruiseState.available = True - command = structs.CarControl(latActive=False) - command.actuators.curvature = desired - state.acc_tja_status_stock_values['FcwVisblWarn_B_Rq'] = 1 - state.acc_tja_status_stock_values['FcwAudioWarn_B_Rq'] = 1 - parser = CANParser('ford_lincoln_base_pt', [('ACCDATA_3', 5)], 0) - steering = [] - displays = [] - - def step(): - native.pump(1, mask=int(command.latActive), speed=speed, curvature=yaw / speed) - state.out = cs.as_reader() - _, frames = controller.update(command.as_reader(), state, (1_000_000 + native.tick * 10_000) * 1000) - if identity != CAR.FORD_MUSTANG_MACH_E_MK1: - lateral_frames = [(index, data) for index, (address, data, _) in enumerate(frames) if address == 0x3D6] - announcements = [(index, data) for index, (address, data, _) in enumerate(frames) if address == 0x3CA] - assert len(announcements) <= 1 - for index, data in lateral_frames: - active = ((data[0] >> 4) & 7) != 0 - previous_active = bool(steering and ((steering[-1][0] >> 4) & 7)) - if active != previous_active: - assert announcements and announcements[0][0] < index - assert bool(announcements[0][1][4] & 2) == active - if not command.latActive: - assert all(not (data[4] & 2) for _, data in announcements) - for address, data, bus in frames: - assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) - if address == 0x3D6: - steering.append(bytes(data)) - if address == 0x18A: - parser.update([(native.tick * 10_000_000, [(address, data, bus)])]) - displays.append(dict(parser.vl['ACCDATA_3'])) - return frames - - # Startup remains stock until an active replacement is actually accepted. - for _ in range(25): - assert not {address for address, _, _ in step()} & {0x18A, 0x3D8} - assert native.safety.safety_fwd_hook(2, 0x18A) == 0 - native.arm() - command.latActive = True - for _ in range(60): - step() - assert displays and displays[-1]['Tja_D_Stat'] == (7 if hands_free else 2) - assert native.safety.aol_get_permission_mask() == 1 - assert native.safety.safety_fwd_hook(2, 0x18A) == -1 - - # Both original onset tuples have driver input, a right signal and >12deg. - cs.steeringPressed = True - cs.steeringAngleDeg = angle - cs.steeringTorque = torque - cs.rightBlinker = True - if identity != CAR.FORD_MUSTANG_MACH_E_MK1: - # Generic curvature retains its existing 45deg/3s preturned detector. - for _ in range(310): - step() - count = len(displays) - for _ in range(40): - frames = step() - if any(address == 0x3D6 for address, _, _ in frames): - assert ((steering[-1][0] >> 4) & 7) == 0 - assert native.safety.safety_fwd_hook(2, 0x3D6) == 0 - assert native.safety.safety_fwd_hook(2, 0x18A) == -1 - assert native.safety.safety_fwd_hook(2, 0x3D8) == -1 - assert native.safety.aol_get_permission_mask() == 1 - assert len(displays) > count - for display in displays[count:]: - assert display['Tja_D_Stat'] == (7 if hands_free else 2) - assert display['FcwVisblWarn_B_Rq'] == 1 - assert display['FcwAudioWarn_B_Rq'] == 1 - - cs.steeringPressed = False - cs.steeringAngleDeg = 0. - cs.steeringTorque = 0. - cs.rightBlinker = False - for _ in range(50): - step() - assert ((steering[-1][0] >> 4) & 7) == 1 - assert displays[-1]['Tja_D_Stat'] == (7 if hands_free else 2) - command.latActive = False - for _ in range(25): - assert not {address for address, _, _ in step()} & {0x18A, 0x3D8} - assert native.safety.safety_fwd_hook(2, 0x18A) == 0 - assert native.safety.safety_fwd_hook(2, 0x3D8) == 0 - assert native.safety.aol_get_permission_mask() == 0 - return steering - - -@pytest.mark.parametrize('angle,torque,desired,speed,yaw', [ - (-12.2, -1.6875, .000041086183, 2.55, -.006), - (-38.7, -2., -.000324735534, 2.563888788, -.033), -]) -def test_recorded_turn_keeps_dom_display_and_hands_free_does_not_change_steering(angle, torque, desired, speed, yaw): - hands_free = run_turn(hands_free=True, angle=angle, torque=torque, desired=desired, speed=speed, yaw=yaw) - plain = run_turn(hands_free=False, angle=angle, torque=torque, desired=desired, speed=speed, yaw=yaw) - assert plain == hands_free - - -@pytest.mark.parametrize('saved', [False, True]) -@pytest.mark.parametrize('flag', [ParamKeyFlag.CLEAR_ON_MANAGER_START, ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION, - ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION, ParamKeyFlag.CLEAR_ON_IGNITION_ON]) -def test_cluster_preference_survives_native_cleanup(tmp_path, saved, flag): - params = Params(str(tmp_path)) - key = 'FordHandsFreeCluster' - assert params.check_key(key) and params.get_type(key) == ParamKeyType.BOOL - params.put(key, saved, block=True) - sentinel = Path(params.get_param_path('FordUnregisteredSentinel')) - sentinel.write_bytes(b'1') - params.clear_all(flag) - assert params.get(key) == saved and not sentinel.exists() - reopened = Params(str(tmp_path)) - assert reopened.get(key) == saved - assert Path(reopened.get_param_path(key)).read_bytes() == (b'1' if saved else b'0') - assert FordClusterPreference(reopened).enabled == saved - - -@pytest.mark.parametrize('identity', [CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2]) -def test_generic_curvature_native_display_handoff_retains_hands_free_during_manual_turn(identity): - # Nonzero demand exercises the bounded first-active command, not just UI. - inputs = dict(identity=identity, angle=-55., torque=-2., desired=.000041086183, speed=2.55, yaw=-.006) - hands_free = run_turn(hands_free=True, **inputs) - plain = run_turn(hands_free=False, **inputs) - assert plain == hands_free diff --git a/opendbc_repo/opendbc/car/ford/tests/test_mache_lateral.py b/opendbc_repo/opendbc/car/ford/tests/test_mache_lateral.py deleted file mode 100644 index b2506851ea..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_mache_lateral.py +++ /dev/null @@ -1,893 +0,0 @@ -"""Original-derived Mach-E demand and driver handoff regressions.""" -from types import SimpleNamespace -from opendbc.car.ford.interface import CarInterface -import pytest -from opendbc.car import Bus, structs -from opendbc.car.lateral import MAX_LATERAL_JERK -from opendbc.can import CANParser -from opendbc.car.ford.values import CAR, DBC, FordFlags, FordSafetyFlags -from opendbc.car.ford.mache_lateral import MachELateralController, FordLateralResult, bounded_command, STEER_DT, MAX_LATERAL_ACCEL, qualified -from opendbc.car.ford import mache_can as fordcan -from opendbc.car.ford.tests.test_three_ports import params -from opendbc.car.ford.carcontroller import CarController -from opendbc.car.ford.carstate import CarState - -def controller_params(car, flags): - cp = CarInterface.get_non_essential_params(car) - return SimpleNamespace(brand=cp.brand, safetyConfigs=cp.safetyConfigs, carFingerprint=car, flags=flags) - - - -@pytest.fixture -def controller(): - owner = MachELateralController(controller_params(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD)) - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.]*33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - owner.set_inputs(model, tuple(i*.1 for i in range(33)), .2, True) - return owner - - -def car_state(speed=15.0, accel=0.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, - steering_torque=0.0, left_blinker=False, right_blinker=False, gas_pressed=False, brake_pressed=False, - cruise_enabled=False): - return SimpleNamespace(out=SimpleNamespace(vEgoRaw=speed, aEgo=accel, yawRate=-curvature*speed, - steeringPressed=steering_pressed, steeringAngleDeg=steering_angle, steeringTorque=steering_torque, - leftBlinker=left_blinker, rightBlinker=right_blinker, gasPressed=gas_pressed, brakePressed=brake_pressed, - cruiseState=SimpleNamespace(enabled=cruise_enabled))) - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("speed,weight", ((4.0, 0.0), (5.0, 0.0), (6.0, 0.5), (7.0, 1.0), - (12.0, 1.0), (13.5, 0.5), (15.0, 0.0), (20.0, 0.0))) -def test_mach_e_unwind_preview_speed_and_direction(controller, monkeypatch, sign, speed, weight): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.desired_curvature_last = sign * 0.011 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.004) - result = controller._unwind_preview(sign * 0.010, sign * 0.009, sign * 0.011, speed) - assert result == pytest.approx(sign * (0.009 - 0.005 * weight)) - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_turn_in_preview_holds_one_repeated_sample(controller, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = sign * 0.003 - assert not controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False) - controller.desired_curvature_last = sign * 0.004 - assert controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False) - assert controller._turn_in_preview_weight(sign * 0.004, sign * 0.03, sign * 0.001, True) == 1.0 - assert not controller._turn_in_preview_plateau(sign * 0.004, sign * 0.001, 6.0, False, False) - assert controller._turn_in_preview_weight(sign * 0.004, sign * 0.03, sign * 0.001) == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("current", (0.0036, 0.004, 0.005)) -def test_mach_e_turn_in_plateau_requires_tracking_lag(controller, sign, current): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = sign * 0.004 - controller.turn_in_preview_hold_timer = 0.1 - assert not controller._turn_in_preview_plateau(sign * 0.004, sign * current, 6.0, False, False) - - -@pytest.mark.parametrize("desired", (0.003, -0.004, 0.0)) -def test_mach_e_turn_in_plateau_resets_on_unwind_and_reversal(controller, desired): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = 0.004 - controller.turn_in_preview_hold_timer = 0.1 - assert not controller._turn_in_preview_plateau(desired, 0.001, 6.0, False, False) - assert controller.turn_in_preview_hold_timer == 0.0 - - -@pytest.mark.parametrize("fingerprint,flags,speed,driver,lane_change", ( - (CAR.FORD_EDGE_MK2, FordFlags.CANFD, 6.0, False, False), - (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, 6.0, False, False), - (CAR.FORD_F_150_MK14, FordFlags.CANFD, 6.0, False, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, 0, 6.0, False, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 1.99, False, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 15.0, False, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 6.0, True, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, 6.0, False, True), -)) -def test_turn_in_plateau_preserves_other_fords_and_handoffs(controller, fingerprint, flags, speed, driver, lane_change): - controller.CP.carFingerprint = fingerprint - controller.CP.flags = flags - controller.desired_curvature_last = 0.004 - controller.turn_in_preview_hold_timer = 0.1 - assert not controller._turn_in_preview_plateau(0.004, 0.001, speed, driver, lane_change) - assert controller.turn_in_preview_hold_timer == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_repeated_turn_in_sample_preserves_extended_preview(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.set_inputs(controller.model, controller.time_indices, .4, True) - controller.desired_curvature_last = sign * 0.003 - controller.curvature_last = sign * 0.012 - monkeypatch.setattr(controller, "_predicted_curvature", - lambda _v, t: sign * (0.002 if t < 1.0 else 0.01 if t < 1.5 else 0.03)) - state = car_state(speed=6.0, curvature=sign * 0.001) - cc = SimpleNamespace(latActive=True, enabled=False) - actuators = SimpleNamespace(curvature=sign * 0.004) - first = controller.update(cc, state, actuators) - repeated = controller.update(cc, state, actuators) - assert sign * repeated.curvature >= sign * first.curvature - assert sign * repeated.curvature == pytest.approx(0.0144) - expired = controller.update(cc, state, actuators) - assert sign * expired.curvature < sign * repeated.curvature - assert repeated.path_angle == 0.0 - controller.update(SimpleNamespace(latActive=False), state, actuators) - assert controller.turn_in_preview_hold_timer == 0.0 - controller.update(cc, state, actuators) - assert controller.turn_in_preview_hold_timer > 0.0 - controller.update(cc, car_state(speed=0.0), actuators) - assert controller.turn_in_preview_hold_timer == 0.0 - - -@pytest.mark.parametrize("speed,weight", ((1.99, 0.0), (2.0, 0.0), (2.5, 0.5), (3.0, 1.0), - (14.0, 1.0), (14.5, 0.5), (15.0, 0.0), (16.0, 0.0))) -def test_mach_e_turn_in_plateau_speed_boundaries_are_continuous(controller, speed, weight): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = 0.004 - controller.turn_in_preview_hold_timer = 0.1 - actual_weight = controller._turn_in_preview_plateau(0.004, 0.001, speed, False, False) - assert actual_weight == pytest.approx(weight) - assert controller._turn_in_preview_weight(0.004, 0.03, 0.001, actual_weight) == pytest.approx(weight) - - -@pytest.mark.parametrize("speed,desired,requested,current,driver,lane_change,expected", ( - (12.0, 0.012, 0.012, 0.004, False, False, 0.006), - (12.0, -0.012, -0.012, 0.004, False, False, 0.006), - (12.0, 0.012, 0.012, 0.010, False, False, 0.002), - (12.0, 0.012, 0.012, 0.014, False, False, 0.002), - (9.5, 0.012, 0.012, 0.004, False, False, 0.006), - (15.0, 0.012, 0.012, 0.004, False, False, 0.004), - (16.0, 0.012, 0.012, 0.004, False, False, 0.002), - (12.0, 0.012, 0.012, 0.004, True, False, 0.002), - (12.0, 0.012, 0.012, 0.004, False, True, 0.002), - (12.0, 0.012, -0.004, 0.004, False, False, 0.002), -)) -def test_mach_e_understeer_error_scope(controller, speed, desired, requested, current, - driver, lane_change, expected): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - assert controller._curvature_error_limit( - requested, desired, current, speed, driver, lane_change) == pytest.approx(expected) - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (12.0, 0.006), - (14.0, 0.006), (15.0, 0.004), (16.0, 0.002))) -def test_mach_e_unwind_error_tracks_opening_path_before_direction_changes(controller, sign, speed, expected): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) - assert controller._curvature_error_limit( - sign * 0.001, sign * 0.002, sign * 0.005, speed, False, False, sign * 0.001) == pytest.approx(expected) - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("requested,preview,expected", ((0.003, 0.001, 0.002), (0.002, 0.001, 0.004), - (0.001, 0.001, 0.006), (0.001, 0.004, 0.002), - (0.001, 0.003, 0.002), (0.001, 0.002, 0.004), - (0.001, -0.0002, 0.006), (0.001, 0.0, 0.006), - (0.0, 0.0, 0.006))) -def test_mach_e_unwind_error_requires_measured_lag_and_opening_preview(controller, sign, requested, preview, expected): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) - assert controller._curvature_error_limit( - sign * requested, sign * 0.002, sign * 0.005, 12.0, False, False, sign * preview) == pytest.approx(expected) - - -@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", ( - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True), - (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False), - (CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False), - (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False), - (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False), -)) -def test_unwind_error_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change): - controller.CP.carFingerprint = fingerprint - controller.CP.flags = flags - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) - assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, driver, lane_change, 0.001) == 0.002 - - -@pytest.mark.parametrize("model", (None, SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 16)))) -def test_mach_e_unwind_error_requires_model_preview(controller, model): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = model - assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, False, False, 0.001) == 0.002 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_curve_exit_releases_without_waiting_for_left_right_reversal(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - controller.curvature_last = sign * 0.004 - controller.desired_curvature_last = sign * 0.003 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.001) - commands = [] - for _ in range(4): - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.005), - SimpleNamespace(curvature=sign * 0.002)) - assert result.active - assert result.path_angle == 0.0 - commands.append(sign * result.curvature) - assert commands[0] == pytest.approx(0.004 - 0.0018) - assert commands[-1] == pytest.approx(0.0016) - assert commands[-1] < 0.005 - 0.002 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_reversal_does_not_reapply_old_direction_at_desired_zero_crossing(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.set_inputs(controller.model, controller.time_indices, 0.4, True) - controller.desired_curvature_last = sign * 0.00155 - controller.curvature_last = -sign * 0.00065 - monkeypatch.setattr(controller, "_predicted_curvature", - lambda _v, t: sign * 0.0007 if t < 1.0 else -sign * 0.005) - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.00462), - SimpleNamespace(curvature=-sign * 0.00034)) - assert result.active - # Reversal followthrough selects -sign*.005; the existing .006 error - # envelope around sign*.00462 bounds the opposite demand to -sign*.00138. - assert result.curvature == pytest.approx(-sign * 0.00138) - assert result.path_angle == 0.0 - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_path_angle_assist_starts_at_saturation_and_releases_after_driver(controller, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - request = (sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.007, 7.0) - assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055) - assert controller._path_angle_assist(*request, True, False) == 0.0 - for _ in range(round(0.75 / STEER_DT) - 1): - assert controller._path_angle_assist(*request, False, False) == 0.0 - assert controller._path_angle_assist(*request, False, False) == pytest.approx(sign * 0.055) - assert controller._path_angle_assist( - sign * 0.0205, sign * 0.019, sign * 0.02, sign * 0.021, 7.0, False, False) == 0.0 - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_driver_assistance_handoff_and_takeover(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.curvature_last = sign * 0.020 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030) - CC = SimpleNamespace(latActive=True, enabled=True) - actuators = SimpleNamespace(curvature=sign * 0.022) - helping = car_state(speed=7.0, curvature=sign * 0.008, steering_pressed=True, - steering_angle=-sign * 50.0, steering_torque=-sign * 2.0, - left_blinker=sign < 0, right_blinker=sign > 0) - for _ in range(round(3.5 / STEER_DT)): - result = controller.update(CC, helping, actuators) - assert result.active - assert result.curvature == pytest.approx(sign * 0.020) - assert sign * result.path_angle > 0.0 - assert not controller.manual_turn_latched - - helping.out.steeringPressed = False - helping.out.steeringTorque = 0.0 - result = controller.update(CC, helping, actuators) - assert result.active - assert sign * result.path_angle > 0.0 - assert controller.path_angle_driver_cooldown == 0.0 - - helping.out.steeringPressed = True - helping.out.steeringTorque = sign * 2.0 - result = controller.update(CC, helping, actuators) - assert result.path_angle == 0.0 - assert controller.path_angle_driver_cooldown > 0.0 - - helping.out.steeringTorque = -sign * 3.6 - result = controller.update(CC, helping, actuators) - assert not result.active - assert result.curvature == result.path_angle == 0.0 - assert controller.manual_turn_latched - helping.out.steeringTorque = -sign * 2.0 - assert not controller.update(CC, helping, actuators).active - - controller.update(SimpleNamespace(latActive=False), helping, actuators) - helping.out.steeringTorque = -sign * 2.0 - helping.out.yawRate = -sign * 0.025 * helping.out.vEgoRaw - result = controller.update(CC, helping, actuators) - assert not result.active - assert result.curvature == result.path_angle == 0.0 - assert controller.manual_turn_latched - - controller.update(SimpleNamespace(latActive=False), helping, actuators) - assert not controller.manual_turn_latched - assert controller.path_angle_driver_cooldown == 0.0 - -@pytest.mark.parametrize("speed,expected", ( - (1.0, 0.80), - (2.0, 0.80), - (2.5, 1.20), - (3.0, 1.60), - (8.0, 1.60), - (9.0, 1.60), - (10.5, 1.60), - (11.0, 1.60), - (12.0, 1.60), - (13.0, 4.0 / 3.0), - (14.0, 16.0 / 15.0), - (15.0, 0.80), - (16.0, 0.80), -)) -def test_mach_e_turn_in_lookahead_extra_fades_by_speed(controller, speed, expected): - assert controller._turn_in_lookahead_extra(speed) == pytest.approx(expected) - - -def test_exact_profile_rejects_collision_and_siblings(): - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - for word in (18, 19): - cp.openpilotLongitudinalControl = word == 19 - cp.safetyConfigs[0].safetyParam = word - assert qualified(cp) - for word in (2, 3, 10, 11, 22, 26, 34): - cp.safetyConfigs[0].safetyParam = word - assert not qualified(cp) - cp.safetyConfigs[0].safetyParam = 19 - cp.passive = True - assert not qualified(cp) - neighbor = params(CAR.FORD_F_150_MK14) - neighbor.safetyConfigs[0].safetyParam = 18 - assert not qualified(neighbor) - - -def test_actual_controller_announces_before_full_physical_frame_and_resets_off(): - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - cp.openpilotLongitudinalControl = False - cp.safetyConfigs[0].safetyParam = 18 - cc = CarController(DBC[cp.carFingerprint], cp) - cs = CarState(cp) - cs.update(cs.get_can_parsers(cp)) - cs.out = structs.CarState(vEgoRaw=7.).as_reader() - command = structs.CarControl(latActive=True) - command.actuators.curvature = .008 - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[.07]*33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - times = tuple(i*.1 for i in range(33)) - cc.manual_turn_inputs = SimpleNamespace(update=lambda: None, apply_blend_settings=lambda owner: None, enabled=True, - lateral_snapshot=lambda speed: (model, times, .2, True)) - _, frames = cc.update(command.as_reader(), cs, 0) - first = next(data for addr, data, _ in frames if addr == 0x3d6) - assert (first[0] >> 4) & 7 == 0 - announcement = next(data for addr, data, _ in frames if addr == 0x3ca) - assert announcement[4] & 2 - cc.frame = 5 - actual, frames = cc.update(command.as_reader(), cs, 50_000_000) - frame = next(data for addr, data, _ in frames if addr == 0x3d6) - assert (frame[0] >> 4) & 7 == 1 - assert frame[1] == fordcan.calculate_lat_ctl2_checksum(1, 1, frame) - parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('LateralMotionControl2', 20)], cc.CAN.main) - parser.update([(50_000_000, frames)]) - assert parser.vl['LateralMotionControl2']['LatCtlCurv_No_Actl'] == pytest.approx(-actual.curvature, abs=1.01e-5) - command.latActive = False - cc.frame = 10 - actual, frames = cc.update(command.as_reader(), cs, 100_000_000) - assert actual.curvature == 0 - assert cc.mache_lateral.path_angle_last == 0 - assert cc.mache_lateral.desired_curvature_last == 0 - - -def test_modern_transient_bound_updates_emitted_history_without_hidden_windup(): - owner = MachELateralController(controller_params(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD)) - raw = FordLateralResult(curvature=.006, path_angle=.16, active=True) - command = bounded_command(owner, raw, .001, 15.) - assert command.curvature < raw.curvature - assert abs(command.curvature-.001) <= MAX_LATERAL_JERK / 15.**2 * STEER_DT + 1e-12 - assert command.path_angle == 0. - assert owner.curvature_last == command.curvature - assert owner.path_angle_last == command.path_angle - assert raw.curvature == .006 and raw.path_angle == .16 - slow = bounded_command(owner, FordLateralResult(curvature=.02, active=True), .02, 15.) - assert not slow.active and slow.curvature == slow.path_angle == slow.curvature_rate == 0. - assert owner.curvature_last == 0. - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_speed_changes_intersect_bounds_or_withdraw_and_preserve_path_headroom(sign): - owner = MachELateralController(controller_params(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD)) - previous, path = sign * .0198, sign * .055 - for speed in (5., 5.1, 5.2, 5.3, 8., 15., 5.): - demand = FordLateralResult(curvature=sign*.02, path_angle=sign*.16, active=True) - emitted = bounded_command(owner, demand, previous, speed, path) - if emitted.active: - assert abs(emitted.curvature-previous) <= MAX_LATERAL_JERK / speed**2 * STEER_DT + 1e-12 - assert abs(emitted.curvature) <= MAX_LATERAL_ACCEL / speed**2 + 1e-12 - if emitted.path_angle: - assert abs(emitted.path_angle-path) <= .055+1e-12 - assert (abs(emitted.curvature)+abs(emitted.path_angle)/speed)*speed**2 <= MAX_LATERAL_ACCEL+1e-12 - else: - assert emitted.curvature == emitted.path_angle == emitted.curvature_rate == 0. - previous, path = emitted.curvature, emitted.path_angle - - -def test_final_typed_cp_binds_real_provider_and_freshness(): - import tempfile - from unittest.mock import patch - import openpilot.cereal.messaging as messaging - from openpilot.common.params import Params - from openpilot.common.prefix import OpenpilotPrefix - from openpilot.starpilot.controller_extensions import configure_controller, ManualTurnInputs - from openpilot.selfdrive.modeld.constants import ModelConstants - from opendbc.car.ford.interface import CarInterface - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - assert cp.safetyConfigs[0].safetyParam == (19 if cp.openpilotLongitudinalControl else 18) - ci = CarInterface(cp) - # Native qualification may run with an unwritable home. Keep the real - # messaging prefix and its filesystem lifecycle inside this fixture. - with tempfile.TemporaryDirectory() as directory, \ - patch("openpilot.common.prefix.Paths.comma_home", return_value=directory + "/comma"), \ - patch("openpilot.common.prefix.Paths.log_root", return_value=directory + "/logs"), OpenpilotPrefix(): - preferences = Params(directory) - preferences.put_bool('FordHumanTurnDetection', True, block=True) - configure_controller(ci, preferences) - provider = ci.CC.manual_turn_inputs - assert isinstance(provider, ManualTurnInputs) - try: - event = messaging.new_message('modelV2') - event.valid = True - event.logMonoTime = 1_000_000_000 - event.modelV2.orientationRate.z = [.07] * len(ModelConstants.T_IDXS) - event.modelV2.meta.laneChangeState = 0 - event.modelV2.meta.laneChangeDirection = 0 - provider.sm.update_msgs(1., [event.as_reader()]) - with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=1_000_000_000): - sample = provider.lateral_snapshot(7.) - assert sample is not None - assert sample[0].orientationRate.z[0] == pytest.approx(.07) - with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=1_200_000_000): - assert provider.lateral_snapshot(7.) is None - finally: - ci.CC.manual_turn_inputs = None - del provider - - -def test_real_offset_cp_keeps_owner_and_invalid_extended_cp_never_falls_back_active(): - from opendbc.car import gen_empty_fingerprint - from opendbc.car.ford.interface import CarInterface - fingerprint = gen_empty_fingerprint() - fingerprint[4][0x5a] = 8 - fingerprint[6][0x3d6] = 8 - fingerprint[6][0x186] = 8 - cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], False, False, False) - assert len(cp.safetyConfigs) == 2 - assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput - assert cp.safetyConfigs[-1].safetyParam == 18 - assert qualified(cp) - controller = CarController(DBC[cp.carFingerprint], cp) - assert controller.mache_lateral is not None and controller.CAN.main == 4 - cp.passive = True - controller = CarController(DBC[cp.carFingerprint], cp) - assert controller.mache_lateral is None - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - state.out = structs.CarState(vEgoRaw=10.).as_reader() - command = structs.CarControl(latActive=True) - command.actuators.curvature = .008 - actual, frames = controller.update(command.as_reader(), state, 0) - frame = next(data for addr, data, _ in frames if addr == 0x3d6) - assert frame[0] >> 4 & 7 == 0 - assert actual.curvature == 0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_real_controller_gas_permission_loss_withdraws_path_only(sign): - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - owner = CarController(DBC[cp.carFingerprint], cp) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - control = structs.CarControl(latActive=True, enabled=True) - control.actuators.curvature = sign * .03 - states, healthy = [SimpleNamespace(safetyModel=structs.CarParams.SafetyModel.ford, controlsAllowed=True, - safetyParam=FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_EXTENDED)], True - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[sign * .28] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - owner.manual_turn_inputs = SimpleNamespace(update=lambda: None, apply_blend_settings=lambda _: None, enabled=True, - lateral_snapshot=lambda _: (model, tuple(i*.1 for i in range(33)), .2, True), - assist_permission=lambda: (states, healthy)) - parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('LateralMotionControl2', 20)], owner.CAN.main) - out = structs.CarState(vEgoRaw=7., yawRate=-sign * .049) - out.cruiseState.enabled = True - state.out = out.as_reader() - # First production frame announces the extension; subsequent commands run the actual demand and envelope. - owner.update(control.as_reader(), state, 0) - assert owner.mache_lateral is not None - owner.mache_lateral.curvature_last = sign * .02 - for tick, (gas, allowed, valid, enabled, brake, cruise) in enumerate(( - (False, True, True, True, False, True), (True, True, True, True, False, True), - (False, True, True, True, False, True), (True, False, True, True, False, True), - (True, True, True, True, False, True), (True, True, False, True, False, True), - (True, True, True, True, False, False), - (False, True, True, False, False, True), (False, True, True, True, True, True), - ), 1): - out.gasPressed, out.brakePressed = gas, brake - out.cruiseState.enabled = cruise - state.out = out.as_reader() - control.enabled = enabled - states[0].controlsAllowed, healthy = allowed, valid - owner.frame = tick * 5 - _, frames = owner.update(control.as_reader(), state, tick * 50_000_000) - parser.update([(tick * 50_000_000, frames)]) - fields = parser.vl['LateralMotionControl2'] - permitted = enabled and not brake and (not gas or (cruise and allowed and valid)) - assert fields['LatCtl_D2_Rq'] == 1 - assert fields['LatCtlCurv_No_Actl'] == pytest.approx(-sign * .02, abs=1.01e-5) - assert (fields['LatCtlPath_An_Actl'] != 0.) == permitted - assert (owner.mache_lateral.path_angle_last != 0.) == permitted - owner.manual_turn_inputs = None - out.gasPressed, out.brakePressed, control.enabled = True, False, True - state.out = out.as_reader() - owner.frame = 50 - owner.update(control.as_reader(), state, 500_000_000) - assert not owner.mache_lateral.assist_engagement_retained - assert owner.mache_lateral.path_angle_last == 0. - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_real_controller_recovers_opposite_curve_after_continuous_agreement(sign): - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - owner = CarController(DBC[cp.carFingerprint], cp) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - control = structs.CarControl(latActive=True, enabled=True) - control.actuators.curvature = sign * .006 - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - owner.manual_turn_inputs = SimpleNamespace(update=lambda: None, apply_blend_settings=lambda _: None, enabled=True, - lateral_snapshot=lambda _: (model, tuple(i*.1 for i in range(33)), .2, True)) - out = structs.CarState(vEgoRaw=9.5, yawRate=-sign * .038, steeringPressed=True, - steeringAngleDeg=-sign * 30., steeringTorque=-sign * 2., - leftBlinker=sign < 0, rightBlinker=sign > 0) - state.out = out.as_reader() - owner.update(control.as_reader(), state, 0) - owner.frame = 5 - owner.update(control.as_reader(), state, 50_000_000) - assert owner.mache_lateral.manual_turn_latched - out.yawRate, out.steeringAngleDeg, out.steeringTorque = sign * .038, sign * 20., sign * 2. - out.leftBlinker = out.rightBlinker = False - state.out = out.as_reader() - control.actuators.curvature = -sign * .006 - model.orientationRate.z = [-sign * .095] * 33 - # An opposing driver torque after four agreeing frames must restart the full dwell. - for tick, (torque, recovered) in enumerate([(2., False)] * 4 + [(-2., False)] + [(2., False)] * 4 + [(2., True)], 1): - out.steeringTorque = sign * torque - state.out = out.as_reader() - owner.frame = (tick + 1) * 5 - actual, frames = owner.update(control.as_reader(), state, (tick + 1) * 50_000_000) - active = next(data for address, data, _ in frames if address == 0x3d6)[0] >> 4 & 7 - assert bool(active) == recovered - assert owner.mache_lateral.path_angle_last == 0. - if recovered: - assert -sign * actual.curvature > 0. - assert not owner.mache_lateral.manual_turn_latched - - -@pytest.mark.parametrize("speed,expected", ((0.0, 0.0), (15.0, 0.0), (16.0, 0.0), (19.5, 0.2), - (23.0, 0.4), (30.0, 0.4), (40.0, 0.4))) -def test_mach_e_high_speed_preview_ramps_continuously(controller, speed, expected): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - assert controller._high_speed_lookahead_extra(speed, False, False) == pytest.approx(expected) - - -@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", ( - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True), - (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False), - (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False), - (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False), - (CAR.FORD_EDGE_MK2, 0, False, False), -)) -def test_high_speed_preview_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change): - controller.CP.carFingerprint = fingerprint - controller.CP.flags = flags - assert controller._high_speed_lookahead_extra(30.0, driver, lane_change) == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("speed", (16.0, 19.5, 23.0, 30.0)) -def test_mach_e_high_speed_preview_preserves_constant_curve_authority(controller, monkeypatch, sign, speed): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - curvature = sign * 0.001 - controller.curvature_last = curvature - controller.desired_curvature_last = curvature - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: curvature) - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed, curvature=curvature), - SimpleNamespace(curvature=curvature)) - assert result.curvature == pytest.approx(curvature) - assert result.curvature_rate == 0.0 - assert result.path_angle == 0.0 - - -@pytest.mark.parametrize("driver,lane_change", ((False, False), (True, False), (False, True))) -def test_mach_e_high_speed_preview_update_uses_bounded_horizon(controller, monkeypatch, driver, lane_change): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.set_inputs(controller.model, controller.time_indices, 0.38, True) - monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0)) - lookaheads = [] - monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: lookaheads.append(t) or 0.0) - controller.update(SimpleNamespace(latActive=True), car_state(speed=30.0, steering_pressed=driver), - SimpleNamespace(curvature=0.0001)) - assert lookaheads[0] == pytest.approx(0.38 if driver or lane_change else 0.78) - assert controller._curvature_lookahead() == pytest.approx(0.38) - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("conflicting_rate", (False, True)) -@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", ( - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True), - (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False), - (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False), - (CAR.FORD_EDGE_MK2, 0, False, False), -)) -def test_mach_e_unwind_rate_does_not_fight_selected_release(controller, monkeypatch, sign, conflicting_rate, - fingerprint, flags, driver, lane_change): - controller.CP.carFingerprint = fingerprint - controller.CP.flags = flags - controller.desired_curvature_last = sign * 0.013 - controller.curvature_last = sign * 0.012 - predicted = sign * 0.011 - rate = sign * 0.0002 * (1 if conflicting_rate else -1) - controller.curvature_samples.append(predicted - rate * STEER_DT * 8.0) - monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: predicted if t < 0.5 else sign * 0.006) - monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0)) - result = controller.update(SimpleNamespace(latActive=True), - car_state(speed=8.0, curvature=sign * 0.012, steering_pressed=driver), - SimpleNamespace(curvature=sign * 0.012)) - suppress = fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and flags & FordFlags.CANFD and not driver and not lane_change - assert result.curvature_rate == pytest.approx(0.0 if lane_change or suppress and conflicting_rate else rate) - assert result.path_angle == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("last,desired,current,preview,rate,suppress", ( - (0.011, 0.012, 0.004, 0.014, -0.001023, True), - (0.012, 0.012, 0.004, 0.014, -0.001023, True), - (0.011, 0.012, -0.004, 0.014, -0.001023, True), - (0.013, 0.012, 0.004, 0.014, -0.001023, False), - (-0.011, 0.012, 0.004, 0.014, -0.001023, False), - (0.011, 0.012, 0.011, 0.014, -0.001023, False), - (0.011, 0.012, 0.004, 0.005, -0.001023, False), - (0.011, 0.012, 0.004, 0.010, -0.001023, False), - (0.011, 0.012, 0.004, -0.014, -0.001023, False), - (0.011, 0.012, 0.004, 0.000, -0.001023, False), - (0.011, 0.012, 0.004, 0.014, 0.001023, False), - (0.011, 0.012, 0.004, 0.014, 0.000000, False), -)) -def test_mach_e_turn_in_rate_preserves_entry_and_opening_phases(controller, monkeypatch, sign, - last, desired, current, preview, rate, suppress): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = sign * last - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * preview) - result = controller._turn_in_curvature_rate(sign * desired, sign * current, sign * rate, 8.0, 0.4, False, False) - assert result == pytest.approx(0.0 if suppress else sign * rate) - - -@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", ( - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False), - (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True), - (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False), - (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False), - (CAR.FORD_EDGE_MK2, 0, False, False), -)) -@pytest.mark.parametrize("sign", (-1, 1)) -def test_turn_in_rate_preserves_driver_lane_change_and_other_fords(controller, monkeypatch, sign, - fingerprint, flags, driver, lane_change): - controller.CP.carFingerprint = fingerprint - controller.CP.flags = flags - controller.desired_curvature_last = sign * 0.011 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.014) - rate = -sign * 0.001023 - assert controller._turn_in_curvature_rate(sign * 0.012, sign * 0.004, rate, 8.0, 0.4, driver, lane_change) == rate - - -@pytest.mark.parametrize("speed,suppress", ((0.3, False), (2.9, False), (3.0, True), (14.9, True), (15.0, False), (25.0, False))) -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_turn_in_rate_does_not_use_near_zero_speed_yaw_or_change_highway_behavior(controller, monkeypatch, - sign, speed, suppress): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = sign * 0.011 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.014) - rate = -sign * 0.001023 - result = controller._turn_in_curvature_rate(sign * 0.012, sign * 0.004, rate, speed, 0.4, False, False) - assert result == pytest.approx(0.0 if suppress else rate) - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_turn_in_rate_update_uses_previous_desired_and_preserves_unwind(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.desired_curvature_last = sign * 0.011 - controller.curvature_last = sign * 0.012 - monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.014) - rate = -sign * 0.0002 - for desired in (0.012, 0.012, 0.011): - controller.curvature_samples.clear() - controller.curvature_samples.append(sign * 0.014 - rate * STEER_DT * 8.0) - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=8.0, curvature=sign * 0.004), - SimpleNamespace(curvature=sign * desired)) - assert result.curvature_rate == pytest.approx(rate if desired == 0.011 else 0.0) - assert controller.desired_curvature_last == sign * desired - assert result.active and result.path_angle == 0.0 - - -def test_mach_e_mild_braking_actual_wire_and_driver_override(): - from opendbc.car.ford.interface import CarInterface - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1, alpha=True) - assert list(cp.longitudinalTuning.kiV) == pytest.approx([0.3]) - assert cp.longitudinalActuatorDelay == pytest.approx(0.15) - assert CarInterface.get_pid_accel_limits(cp, 11., 20.)[0] == -3.5 - cc = CarController(DBC[cp.carFingerprint], cp) - cs = CarState(cp) - cs.update(cs.get_can_parsers(cp)) - cs.out = structs.CarState(vEgo=11., vEgoRaw=11.).as_reader() - command = structs.CarControl(enabled=True) - command.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid - parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('ACCDATA', 50)], 0) - for frame, active, request, expected in ((2, True, -.5, -.04), (4, False, 0., 0.), - (8, True, -.5, -.04), (14, True, -3.5, -.11)): - cc.frame = frame - command.longActive = active - command.actuators.accel = request - actual, frames = cc.update(command.as_reader(), cs, frame * 10_000_000) - parser.update([(frame * 10_000_000, [msg for msg in frames if msg[0] == 0x186])]) - values = parser.vl['ACCDATA'] - assert actual.accel == pytest.approx(expected) - assert values['AccBrkTot_A_Rq'] == pytest.approx(expected, abs=.01) - assert values['AccPrpl_A_Rq'] == pytest.approx(expected if active and request > -1. else -5., abs=.01) - assert values['Cmbb_B_Enbl'] == int(active) - assert values['AccResumEnbl_B_Rq'] == int(active) - - -# Dom Green Garden / Sea Turtles / Waffles actual demand update regressions. -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_unwind_plateau_sequence_and_reset(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: sign * (0.009 if t < 0.5 else 0.004)) - controller.desired_curvature_last = sign * 0.012 - controller.curvature_last = sign * 0.009 - state = car_state(speed=10.0, curvature=sign * 0.009) - CC = SimpleNamespace(latActive=True) - commands = [controller.update(CC, state, SimpleNamespace(curvature=sign * x)).curvature for x in (0.011, 0.011)] - assert commands[1] == pytest.approx(commands[0]) - for _ in range(6): - result = controller.update(CC, state, SimpleNamespace(curvature=sign * 0.011)) - assert sign * result.curvature > sign * commands[0] - assert controller.unwind_preview_hold_timer == 0.0 - controller.unwind_preview_hold_timer = 0.10 - state.out.steeringPressed = True - controller.update(CC, state, SimpleNamespace(curvature=sign * 0.011)) - assert controller.unwind_preview_hold_timer == 0.0 - controller.unwind_preview_hold_timer = 0.10 - assert not controller.update(SimpleNamespace(latActive=False), state, SimpleNamespace(curvature=0.0)).active - assert controller.unwind_preview_hold_timer == 0.0 - state.out.steeringPressed = False - controller.update(CC, state, SimpleNamespace(curvature=sign * 0.011)) - assert controller.unwind_preview_hold_timer == 0.0 - - -@pytest.mark.parametrize("rate_sign", (-1, 1)) -def test_mach_e_unwind_plateau_rate_consistency(controller, monkeypatch, rate_sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - controller.desired_curvature_last = 0.010 - controller.curvature_last = 0.010 - controller.unwind_preview_hold_timer = 0.10 - rate = rate_sign * 0.0001 - controller.curvature_samples.append(0.010 - rate * STEER_DT * 10.0) - monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: 0.010 if t < 0.5 else 0.004) - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=10.0, curvature=0.011), - SimpleNamespace(curvature=0.010)) - assert result.curvature_rate == pytest.approx(0.0 if rate_sign > 0 else rate) - - -@pytest.mark.parametrize("manual_turn", (False, True)) -def test_mach_e_unwind_plateau_resets_when_stopped_or_manual_turn_latched(controller, manual_turn): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.unwind_preview_hold_timer = 0.10 - controller.manual_turn_latched = manual_turn - state = car_state(speed=10.0 if manual_turn else 0.0, steering_angle=20.0 if manual_turn else 0.0) - controller.update(SimpleNamespace(latActive=True), state, SimpleNamespace(curvature=0.010)) - assert controller.unwind_preview_hold_timer == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_unwind_release_update_yields_and_reverses(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: sign * (0.014 if t < 0.5 else 0.004)) - controller.desired_curvature_last = sign * 0.016 - controller.curvature_last = sign * 0.012 - CC = SimpleNamespace(latActive=True) - state = car_state(speed=10.0, curvature=sign * 0.010) - result = controller.update(CC, state, SimpleNamespace(curvature=sign * 0.012)) - assert result.curvature == pytest.approx(sign * 0.010) - assert result.path_angle == 0.0 - state.out.steeringPressed = True - controller.update(CC, state, SimpleNamespace(curvature=sign * 0.012)) - assert controller.unwind_preview_hold_timer == 0.0 - state.out.steeringPressed = False - state.out.yawRate = -sign * 0.004 * state.out.vEgoRaw - for _ in range(20): - result = controller.update(CC, state, SimpleNamespace(curvature=-sign * 0.008)) - assert sign * result.curvature < 0.0 - assert controller.unwind_preview_hold_timer == 0.0 - assert not controller.update(SimpleNamespace(latActive=False), state, SimpleNamespace(curvature=0.0)).active - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_mach_e_reversal_zero_crossing_after_preview_sequence(controller, monkeypatch, sign): - controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 - controller.CP.flags = FordFlags.CANFD - controller.lookahead = 0.4 - controller.desired_curvature_last = sign * 0.00155 - controller.curvature_last = -sign * 0.00065 - monkeypatch.setattr(controller, "_predicted_curvature", - lambda _v, t: sign * 0.0007 if t < 1.0 else -sign * 0.005) - result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.00462), - SimpleNamespace(curvature=-sign * 0.00034)) - assert result.active - assert result.curvature == pytest.approx(-sign * 0.00138) - assert result.path_angle == 0.0 - - -@pytest.mark.parametrize("sign", (-1, 1)) -def test_dom_unwind_release_actual_caller_keeps_modern_envelope_and_wire(sign): - import struct - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - cc = CarController(DBC[cp.carFingerprint], cp) - cs = CarState(cp) - cs.update(cs.get_can_parsers(cp)) - cs.out = structs.CarState(vEgoRaw=10., yawRate=-sign * .10).as_reader() - control = structs.CarControl(latActive=True, enabled=True) - control.actuators.curvature = sign * .012 - model = SimpleNamespace(orientationRate=SimpleNamespace(z=[sign * (.14 if i < 5 else .04) for i in range(33)]), - meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) - cc.manual_turn_inputs = SimpleNamespace(update=lambda: None, apply_blend_settings=lambda _: None, enabled=True, - lateral_snapshot=lambda _: (model, tuple(i*.1 for i in range(33)), .2, True)) - cc.update(control.as_reader(), cs, 0) - cc.mache_lateral.desired_curvature_last = sign * .016 - cc.mache_lateral.curvature_last = sign * .012 - cc.frame = 5 - actual, frames = cc.update(control.as_reader(), cs, 50_000_000) - assert cc.mache_lateral_demand.curvature == pytest.approx(sign * .010) - assert abs(actual.curvature - sign * .012) <= MAX_LATERAL_JERK / 100. * STEER_DT + 1e-12 - assert abs(actual.curvature) <= min(.02, MAX_LATERAL_ACCEL / 100.) - assert cc.mache_lateral.curvature_last == cc.apply_curvature_last - # Actuators.curvature is Float32; internal emitted histories are doubles. - assert actual.curvature == struct.unpack("f", struct.pack("f", cc.apply_curvature_last))[0] - parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('LateralMotionControl2', 20)], cc.CAN.main) - parser.update([(50_000_000, frames)]) - fields = parser.vl['LateralMotionControl2'] - assert fields['LatCtl_D2_Rq'] == 1 - assert fields['LatCtlCurv_No_Actl'] == pytest.approx(-actual.curvature, abs=1.01e-5) - assert fields['LatCtlPath_An_Actl'] == 0. - control.latActive = False - cc.frame = 10 - actual, _ = cc.update(control.as_reader(), cs, 100_000_000) - assert actual.curvature == 0. - assert cc.mache_lateral.unwind_preview_hold_timer == 0. diff --git a/opendbc_repo/opendbc/car/ford/tests/test_mache_speed_window.py b/opendbc_repo/opendbc/car/ford/tests/test_mache_speed_window.py deleted file mode 100644 index 074b5392eb..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_mache_speed_window.py +++ /dev/null @@ -1,140 +0,0 @@ -import pytest -from opendbc.car.ford import mache_lateral as original -from opendbc.car import Bus -from opendbc.can import CANPacker - -candidate = original - - -def cp(): - from opendbc.car import gen_empty_fingerprint - from opendbc.car.ford.interface import CarInterface - from opendbc.car.ford.values import CAR - - fingerprint = gen_empty_fingerprint() - fingerprint[0] = {0x5A: 8, 0x3A6: 8, 0x3A7: 8} - result = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], True, False, False) - result.alternativeExperience = 32 - assert result.safetyConfigs[-1].safetyParam == 19 and result.flags == 3 - return result - - -@pytest.mark.parametrize("sign", [-1, 1]) -def test_recorded_native_window_prevents_first_rejection_and_cascade(sign): - from opendbc.safety.tests.test_ford_mach_e_extended import TestFordMachEExtended - from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent - - f = TestFordMachEExtended() - f.SAFETY_PARAM = 19 - f.setUp() - a = TestFordAolDriverIntent() - a.setUp() - - def setup(): - a.reset(19) - a.arm() - a.pump(12, mask=3, speed=8.805556, curvature=sign * 0.02) - f._rx(f._pcm_status_msg(True)) - a.safety.aol_set_host_request(3) - assert a.safety.aol_get_permission_mask() == 3 - assert f.announce() - a.pump(2, mask=3, main=4, speed=8.775, curvature=sign * 0.02) - f._set_prev_desired_angle(sign * 0.02) - - demand = original.FordLateralResult(active=True, curvature=sign * 0.02, path_angle=sign * 0.0035, curvature_rate=-0.001023) - setup() - old = original.bounded_command(candidate.MachELateralController(cp()), demand, sign * 0.02, 8.775) - assert not f._tx(f.command(curvature=old.curvature, angle=old.path_angle, rate=old.curvature_rate, increment_timer=False)) - assert not f._tx(f.command(curvature=sign * 0.02, angle=0, rate=0, increment_timer=False)) - setup() - new = candidate.bounded_command(candidate.MachELateralController(cp()), demand, sign * 0.02, 8.775, speed_window=(8.775, 8.806, 0.031)) - assert new.curvature == old.curvature and new.path_angle == 0 - assert f._tx(f.command(curvature=new.curvature, angle=new.path_angle, rate=new.curvature_rate, increment_timer=False)) - assert f._tx(f.command(curvature=sign * 0.02, angle=0, rate=0, increment_timer=False)) - a.tearDown() - - -@pytest.mark.parametrize("speed", [0, 2.99, 3, 5, 8.7, 8.8, 12, 30]) -@pytest.mark.parametrize("active", [False, True]) -def test_zero_span_window_preserves_default_command(speed, active): - d = original.FordLateralResult(active=active, curvature=0.02, path_angle=0.1) - assert candidate.bounded_command(candidate.MachELateralController(cp()), d, 0.019, speed) == candidate.bounded_command( - candidate.MachELateralController(cp()), d, 0.019, speed, speed_window=(speed, speed, 0.0) - ) - - -@pytest.mark.parametrize("param,ae,supported", [(19, 32, True), (18, 32, False), (19, 0, False), (147, 32, False), (67, 32, False)]) -def test_exact_profile(param, ae, supported): - c = cp() - c.safetyConfigs[0].safetyParam = param - c.alternativeExperience = ae - assert candidate.path_speed_window_supported(c) == supported - - -def test_actual_carstate_tracks_six_raw_samples(): - from opendbc.car.ford.carstate import CarState - - c = cp() - cs = CarState(c) - parsers = cs.get_can_parsers(c) - p = CANPacker("ford_lincoln_base_pt") - values = [8.7, 8.805556, 8.79, 8.78, 8.775, 8.77, 8.76] - for i, v in enumerate(values): - parsers[Bus.pt].update( - [[(i + 1) * 20_000_000, [p.make_can_msg("BrakeSysFeatures", 0, {"Veh_V_ActlBrk": v * 3.6, "VehVActlBrk_D_Qf": 3, "VehVActlBrk_No_Cnt": i + 1})]]] - ) - cs.update(parsers) - assert cs.bluepilot_speed_window is not None - assert cs.bluepilot_speed_samples is not None - assert cs.bluepilot_speed_window[1] >= 8.8 - assert len(cs.bluepilot_speed_samples) == 6 - - for i in range(7, 13): - parsers[Bus.pt].update( - [[(i + 1) * 20_000_000, [p.make_can_msg("BrakeSysFeatures", 0, {"Veh_V_ActlBrk": 8.7 * 3.6, "VehVActlBrk_D_Qf": 3, "VehVActlBrk_No_Cnt": i + 1})]]] - ) - cs.update(parsers) - assert cs.bluepilot_speed_window is not None - assert cs.bluepilot_speed_window[1] < 8.8 - - -@pytest.mark.parametrize("high", [8.79, 8.8, 8.81, 9.0]) -def test_rising_window_previous_nonzero_path(high): - demand = original.FordLateralResult(active=True, curvature=0.02, path_angle=0.05) - out = candidate.bounded_command(candidate.MachELateralController(cp()), demand, 0.02, 8.775, 0.05, speed_window=(8.7, high, 0.1)) - if high >= 8.8: - assert out.path_angle == 0 - else: - assert 0 <= out.path_angle <= 0.05 - assert out.curvature == 0.02 - - -def test_controller_uses_native_speed_window(): - from opendbc.can import CANParser - from opendbc.car import structs - from opendbc.car.ford.carcontroller import CarController - from opendbc.car.ford.carstate import CarState - from opendbc.car.ford.values import DBC - from openpilot.common.prefix import OpenpilotPrefix - - with OpenpilotPrefix(): - c = cp() - controller = CarController(DBC[c.carFingerprint], c) - controller.manual_turn_inputs = None - controller.mache_extended_announced = True - assert controller.mache_lateral is not None - controller.mache_lateral.curvature_last = 0.02 - controller.mache_lateral.update = lambda *_: candidate.FordLateralResult(active=True, curvature=0.02, path_angle=0.0035) - state = CarState(c) - state.update(state.get_can_parsers(c)) - state.bluepilot_speed_window = (8.775, 8.806, 0.031) - cs = structs.CarState(canValid=True, vEgo=8.775, vEgoRaw=8.775, gearShifter=structs.CarState.GearShifter.drive) - cs.cruiseState.available = True - state.out = cs.as_reader() - command = structs.CarControl(latActive=True, longActive=True) - _, frames = controller.update(command.as_reader(), state, 1_000_000_000) - parser = CANParser("ford_lincoln_base_pt", [("LateralMotionControl2", 50)], 0) - parser.update([[1_000_000_000, frames]]) - assert any(address == 0x3D6 for address, _, _ in frames) - assert abs(parser.vl["LateralMotionControl2"]["LatCtlPath_An_Actl"]) < 0.00001 - assert abs(abs(parser.vl["LateralMotionControl2"]["LatCtlCurv_No_Actl"]) - 0.02) < 0.00001 diff --git a/opendbc_repo/opendbc/car/ford/tests/test_manual_turn.py b/opendbc_repo/opendbc/car/ford/tests/test_manual_turn.py deleted file mode 100644 index 5ddc95aaf6..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_manual_turn.py +++ /dev/null @@ -1,144 +0,0 @@ -import unittest -from types import SimpleNamespace - -from opendbc.can import CANParser -from opendbc.car import Bus, structs -from opendbc.car.ford import fordcan -from opendbc.car.ford.carcontroller import CarController -from opendbc.car.ford.carstate import CarState -from opendbc.car.ford.manual_turn import HumanTurnDetector, ManualTurnLatch -from opendbc.car.ford.tests.test_three_ports import params as _params -from opendbc.car.ford.values import CAR, DBC - - -def turn_state(direction=1): - return SimpleNamespace(out=SimpleNamespace(steeringPressed=True, steeringAngleDeg=-direction * 12., - steeringTorque=-direction, rightBlinker=direction == 1, - leftBlinker=direction == -1, yawRate=0., vEgoRaw=20.)) - - -def params(candidate): - cp = _params(candidate) - if candidate == CAR.FORD_MUSTANG_MACH_E_MK1: - cp.safetyConfigs[0].safetyParam = 3 if cp.openpilotLongitudinalControl else 2 - return cp - - -class TestManualTurn(unittest.TestCase): - def test_detector_preturned_and_new_turn_hold(self): - for preturned, frames in ((True, 60), (False, 30)): - detector = HumanTurnDetector() - if not preturned: - self.assertFalse(detector.update(True, True, 0.)) - for _ in range(frames - 1): - self.assertFalse(detector.update(True, True, 46.)) - self.assertTrue(detector.update(True, True, 46.)) - self.assertFalse(detector.update(True, False, 46.)) - self.assertFalse(detector.update(False, True, 46.)) - - def test_signed_directional_recovery_and_resets(self): - command = SimpleNamespace(latActive=True) - for direction in (-1, 1): - for desired, current, release in ((.006, .003, False), (.005, .003, True), (0., .003, True), (-.006, .003, True)): - with self.subTest(direction=direction, desired=desired): - latch = ManualTurnLatch() - state = turn_state(direction) - self.assertTrue(latch.update(command, state, direction * desired, True, False)) - state.out.steeringPressed = False - state.out.steeringAngleDeg = 0. - state.out.rightBlinker = state.out.leftBlinker = False - state.out.yawRate = -direction * current * 20. - for _ in range(4): - self.assertTrue(latch.update(command, state, direction * desired, True, False)) - self.assertEqual(latch.update(command, state, direction * desired, True, False), not release) - self.assertFalse(latch.update(command, state, direction * desired, False, False)) - self.assertEqual(latch.manual_turn_direction, 0.) - state = turn_state(direction) - self.assertFalse(latch.update(command, state, .006, True, True)) - self.assertTrue(latch.update(command, state, .006, True, False)) - command.latActive = False - self.assertFalse(latch.update(command, state, .006, True, False)) - command.latActive = True - - def test_no_signal_direction_and_recovery_interruptions(self): - latch = ManualTurnLatch() - command = SimpleNamespace(latActive=True) - state = turn_state(-1) - state.out.leftBlinker = False - state.out.steeringAngleDeg = 46. - for _ in range(60): - latched = latch.update(command, state, -.006, True, False) - self.assertTrue(latched) - self.assertEqual(latch.manual_turn_direction, -1.) - for attribute, value in (('steeringPressed', True), ('rightBlinker', True), ('steeringAngleDeg', 13.)): - state.out.steeringPressed = False - state.out.rightBlinker = False - state.out.steeringAngleDeg = 0. - latch.update(command, state, -.006, True, False) - setattr(state.out, attribute, value) - self.assertTrue(latch.update(command, state, -.006, True, False)) - self.assertEqual(latch.manual_turn_recovery_timer, 0.) - - def test_missing_model_blocks_entry_but_preserves_recovery(self): - command = SimpleNamespace(latActive=True) - state = turn_state() - latch = ManualTurnLatch() - self.assertFalse(latch.update(command, state, .006, True, False, False)) - self.assertTrue(latch.update(command, state, .006, True, False, True)) - state.out.steeringPressed = False - state.out.rightBlinker = False - state.out.steeringAngleDeg = 0. - for _ in range(4): - self.assertTrue(latch.update(command, state, 0., True, False, False)) - self.assertFalse(latch.update(command, state, 0., True, False, False)) - - def test_controller_real_can_yield_counter_and_recovery(self): - candidate = CAR.FORD_MUSTANG_MACH_E_MK1 - cp = params(candidate) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - out = structs.CarState() - out.vEgoRaw = 20. - out.steeringPressed = True - out.steeringAngleDeg = -12. - out.steeringTorque = -1. - out.rightBlinker = True - state.out = out.as_reader() - command = structs.CarControl() - command.latActive = True - command.actuators.curvature = .006 - controller = CarController(DBC[candidate], cp) - controller.manual_turn_inputs = SimpleNamespace(update=lambda: (True, False, True)) - parser = CANParser(DBC[candidate][Bus.pt], [('LateralMotionControl2', 20)], controller.CAN.main) - for index in range(10): - controller.frame = index * 5 - if index == 1: - out.steeringPressed = False - out.steeringAngleDeg = 0. - out.rightBlinker = False - if index == 7: - command.actuators.curvature = -.001 - state.out = out.as_reader() - _, messages = controller.update(command.as_reader(), state, (index + 1) * 50_000_000) - wire = next(message for message in messages if message[0] == 0x3D6) - expected = fordcan.create_lat_ctl2_msg(controller.packer, controller.CAN, int(index >= 7), 0., 0., - -controller.apply_curvature_last, 0., index) - self.assertEqual(wire, expected) - parser.update([((index + 1) * 50_000_000, messages)]) - values = parser.vl['LateralMotionControl2'] - self.assertEqual(values['LatCtlPath_No_Cnt'], index) - self.assertEqual(values['LatCtl_D2_Rq'], int(index >= 7)) - if index < 7: - self.assertAlmostEqual(values['LatCtlCurv_No_Actl'], 0., places=5) - self.assertEqual(controller.apply_curvature_last, 0.) - self.assertTrue(parser.can_valid) - - def test_other_platform_and_lka_do_not_create_inputs(self): - for candidate in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_TRANSIT_MK5): - cp = params(candidate) - controller = CarController(DBC[candidate], cp) - self.assertIsNone(controller.manual_turn) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - state.out = structs.CarState().as_reader() - controller.update(structs.CarControl().as_reader(), state, 0) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_new_port_curvature.py b/opendbc_repo/opendbc/car/ford/tests/test_new_port_curvature.py deleted file mode 100644 index e8ad2d290a..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_new_port_curvature.py +++ /dev/null @@ -1,207 +0,0 @@ -"""Current-strategy Edge/Mondeo wiring and current native scalar-curvature proof. - -Source-only authored cases; runtime must bind actual DEBUG/RELEASE libraries. -""" -import os -from pathlib import Path -import unittest -from unittest.mock import patch - -from openpilot.cereal import messaging -from openpilot.common.params import Params -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.controls.controlsd import Controls -from openpilot.starpilot.tests import test_vehicle_preferences as startup -from openpilot.starpilot.lateral.tests import test_lane_runtime as lane -from opendbc.car import gen_empty_fingerprint, structs -from opendbc.car.ford.new_port_curvature import qualified, create_controller -from opendbc.car.ford.tests.test_three_ports import params as factory -from opendbc.car.ford.values import CAR, FordFlags -from opendbc.safety.tests import test_ford_aol as physical -from opendbc.safety.tests.libsafety import libsafety_py - -PROFILES = (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5) - - -class TestNewPortCurvature(unittest.TestCase): - def test_exact_existing_factory_profiles_and_no_namespace_or_topology_inference(self): - for identity in PROFILES: - for alpha, release in ((False, False), (True, False), (False, True), (True, True)): - cp = factory(identity, alpha, release) - expected_long = bool(alpha) - self.assertTrue(cp.pcmCruise) - self.assertEqual(cp.openpilotLongitudinalControl, expected_long) - self.assertEqual(cp.safetyConfigs[-1].safetyParam, (8 if identity == CAR.FORD_EDGE_MK2 else 10) + int(expected_long)) - self.assertTrue(qualified(cp)) - self.assertIsNotNone(create_controller(cp)) - original = cp.to_bytes() - cp.clear_write_flag() - create_controller(cp) - self.assertEqual(cp.to_bytes(), original) - cp.clear_write_flag() - for field, value in (("passive", True), ("dashcamOnly", True), ("notCar", True), ("alternativeExperience", 64), - ("pcmCruise", False), ("openpilotLongitudinalControl", not expected_long)): - with structs.CarParams.from_bytes(original) as prior: - changed = prior.as_builder() - setattr(changed, field, value) - self.assertFalse(qualified(changed)) - for word in (0, 2, 12, 18, 32, 66, 128, 130, 65535): - with structs.CarParams.from_bytes(original) as prior: - changed = prior.as_builder() - changed.safetyConfigs[-1].safetyParam = word - self.assertIsNone(create_controller(changed)) - cp.alternativeExperience = 32 - self.assertTrue(qualified(cp)) - cp.safetyConfigs = [cp.safetyConfigs[-1], cp.safetyConfigs[-1]] - self.assertFalse(qualified(cp)) - self.assertIsNone(create_controller(factory(CAR.FORD_TRANSIT_MK5))) - self.assertIsNone(create_controller(factory(CAR.FORD_BRONCO_SPORT_MK1))) - - def test_saved_card_controls_actual_parser_provider_sender_and_native_both_signs(self): - safety = libsafety_py.libsafety - for identity in PROFILES: - for direction in (-1, 1): - with self.subTest(identity=identity, direction=direction), OpenpilotPrefix(), \ - patch.dict(os.environ, {"REPLAY": "1", "SIMULATION": "1"}), \ - patch("openpilot.selfdrive.controls.controlsd.messaging.PubMaster"): - saved = Params() - helper = startup.TestVehicleStartupPreferences() - helper.setUp() - helper.params = saved - native = physical.TestFordAolDriverIntent() - try: - for key, value in (("OpenpilotEnabledToggle", True), ("SafeMode", False), ("AlwaysOnLateral", False), - ("AlphaLongitudinalEnabled", False), ("IsReleaseBranch", False)): - saved.put_bool(key, value, block=True) - observed = gen_empty_fingerprint() - observed[0][0x5A] = 8 - observed[2].update({0x3D6: 8, 0x186: 8}) - host, constructed, cp = helper.start(identity, key="FordHumanTurnDetection", observed=observed, - capture=lambda ci: (ci.CP.safetyConfigs[-1].safetyParam, ci.CC.new_port_curvature is not None)) - self.assertEqual(constructed, [(8 if identity == CAR.FORD_EDGE_MK2 else 10, True)]) - self.assertEqual(host.CI.CP.to_dict(), cp.to_dict()) - self.assertTrue(qualified(cp)) - owner = host.CI.CC.new_port_curvature - inputs = host.CI.CC.manual_turn_inputs - self.assertIsNotNone(inputs) - inputs.sm = messaging.SubMaster(["modelV2", "lateralDelay"]) - host.sm = messaging.SubMaster(["carControl"]) - controls = Controls() - native.setUp() - native.word, native.tick = cp.safetyConfigs[-1].safetyParam, 0 - safety.set_alternative_experience(0) - self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, native.word), 0) - safety.init_tests() - received, sent, active_curvatures = [], [], [] - - def rx(name, values, *, native=native, received=received): - self.assertLessEqual(set(values), set(native.packer.dbc.name_to_msg[name].sigs)) - frame = native.packer.make_can_msg(name, 0, values) - addr, data, bus = frame - self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data))) - received.append(frame) - - host.publish_sendcan = lambda frames, valid=True, sent=sent: sent.extend(frames) if valid else self.fail("Invalid Card trace") - host.ci_initialized = True - host.CI.update([]) - saw_human_zero = saw_fault_zero = saw_stale_model = saw_rate_neutral = False - for tick in range(600): - received.clear() - with patch.object(native, "rx", rx): - native.pump(1, main=0 if tick < 30 else 4, speed=15.) - now = (1_000_000 + native.tick * 10_000) * 1000 - pressed = 100 <= tick < 430 - fault = 460 <= tick < 480 - extra: list[tuple[str, int, dict[str, float]]] = [("Cluster_Info1_FD1", 0, {"AccEnbl_B_RqDrv": 1}), - ("BodyInfo_3_FD1", 0, {}), ("RCMStatusMessage2_FD1", 0, {"FirstRowBuckleDriver": 1}), - ("INSTRUMENT_PANEL", 0, {}), ("IPMA_Data", 2, {}), ("ACCDATA", 2, {}), - ("ACCDATA_2", 2, {}), ("ACCDATA_3", 2, {}), - ("EPAS_INFO", 0, {"SteeringColumnTorque": 2 if pressed else 0, "EPAS_Failure": 2 if fault else 0})] - if cp.flags & FordFlags.ALT_STEER_ANGLE: - extra += [("ParkAid_Data", 0, {"ExtSteeringAngleReq2": 60 if pressed else 0, "EPASExtAngleStatReq": 0, "ApaSys_D_Stat": 0}), - ("SteeringPinion_Data_Alt", 0, {"StePinRelInit_An_Sns": 60 if pressed else 0})] - else: - extra += [("SteeringPinion_Data", 0, {"StePinComp_An_Est": 60 if pressed else 0, "StePinCompAnEst_D_Qf": 3})] - host_sources = [native.packer.make_can_msg(name, bus, values) for name, bus, values in extra] - for addr, data, bus in host_sources: - self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data))) - received.extend(host_sources) - state = host.CI.update([(now, list(received))]) - if tick < 40: - continue - self.assertTrue(state.canValid) - self.assertFalse(state.canTimeout) - self.assertTrue(safety.get_controls_allowed()) - with patch("openpilot.starpilot.controller_extensions.time.monotonic_ns", return_value=now), \ - patch("openpilot.selfdrive.car.card.time.monotonic", return_value=now / 1e9), \ - patch.object(inputs.sm, "update", return_value=None): - feed_events = [] - with patch.object(controls.sm, "update_msgs", side_effect=lambda _, events, collector=feed_events: collector.extend(events)): - lane.feed(controls, now, tick, speed=15.) - car_state = messaging.new_message("carState", valid=True, logMonoTime=now) - car_state.carState = state - - preview = messaging.new_message("modelV2", valid=True, logMonoTime=now) - preview.modelV2 = lane.model() - preview.modelV2.action.desiredCurvature = direction * .0002 - predicted = .012 + (tick - 430) * .0002 if 430 <= tick < 460 else .0012 - preview.modelV2.orientationRate.z = [direction * predicted * 15.] * 33 - delay = messaging.new_message("lateralDelay", valid=True, logMonoTime=now) - delay.lateralDelay.lateralDelay = .38 - replacements = {"carState": car_state.as_reader(), "modelV2": preview.as_reader(), "lateralDelay": delay.as_reader()} - controls.sm.update_msgs(now / 1e9, [event for event in feed_events - if event.which() not in replacements] + list(replacements.values())) - if tick < 490: - inputs.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader()]) - command, controller_log = controls.state_control() - self.assertTrue(command.enabled) - control_events = [] - - def capture_control(service, event, collector=control_events): - if service == "carControl": - collector.append(messaging.log_from_bytes(event.to_bytes())) - with patch.object(controls.pm, "send", side_effect=capture_control): - controls.publish(command, controller_log) - self.assertEqual(len(control_events), 1) - self.assertEqual(control_events[0].carControl.to_dict(), command.to_dict()) - self.assertEqual(control_events[0].valid, state.canValid) - host.sm.update_msgs(now / 1e9, control_events) - command = host.sm["carControl"] - host.can_log_mono_time = now - sent.clear() - host.controls_update(state, command) - if tick > 510: - self.assertIsNone(owner.model) - saw_stale_model = True - for addr, data, bus in sent: - if addr not in (0x3CA, 0x3D3, 0x3D6): - continue - self.assertTrue(safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, data)), (identity, tick, hex(addr))) - if addr == 0x3CA: - self.assertEqual(data[4] & 3, 0) # Existing neutral body, no extension announcement. - elif addr in (0x3D3, 0x3D6): - fd = addr == 0x3D6 - mode = (data[0] >> 4) & 7 if fd else (data[4] >> 2) & 7 - curvature = ((data[2] << 3) | (data[3] >> 5)) - 1000 if fd else ((data[0] << 3) | (data[1] >> 5)) - 1000 - rate = (data[6] << 3) | (data[7] >> 5) if fd else ((data[1] & 31) << 8) | data[2] - self.assertEqual(rate, 1024 if fd else 4096) - if 430 <= tick < 460 and host.CI.CC.new_port_curvature_demand.curvature_rate != 0: - saw_rate_neutral = True - if 400 <= tick < 425: - self.assertEqual((mode, curvature), (0, 0)) - saw_human_zero = True - if fault: - self.assertEqual((mode, curvature), (0, 0)) - saw_fault_zero = True - if 70 <= tick < 100 and mode and curvature: - active_curvatures.append(curvature) - self.assertAlmostEqual(owner._curvature_lookahead(), .38, places=6) - self.assertGreater(direction * host.CI.CC.new_port_curvature_demand.curvature, .0002) - self.assertTrue(active_curvatures) - self.assertTrue(all(value * direction < 0 for value in active_curvatures)) - self.assertTrue(saw_human_zero and saw_fault_zero and saw_stale_model and saw_rate_neutral) - self.assertEqual(Path(saved.get_param_path("FordHumanTurnDetection")).read_bytes(), b"1") - finally: - helper.doCleanups() - safety.set_alternative_experience(0) - safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_pinion_measurement.py b/opendbc_repo/opendbc/car/ford/tests/test_pinion_measurement.py deleted file mode 100644 index 43ed71e242..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_pinion_measurement.py +++ /dev/null @@ -1,177 +0,0 @@ -"""Closed, init-only measurement selection; disabled behavior stays unchanged.""" -from types import SimpleNamespace - -import pytest -from opendbc.car import gen_empty_fingerprint, structs -from opendbc.car.ford.interface import CarInterface -from opendbc.car.ford.pinion import GEOMETRY, ENABLE, SHIFT, MASK, decode, select, configured, Measurement -from opendbc.car.ford.aol import qualified -from opendbc.car.vehicle_model import VehicleModel, calc_slip_factor -from opendbc.can.packer import CANPacker -from opendbc.safety.tests.libsafety import libsafety_py - - -def params(car, long=False): - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fp[2].update({0x3D6: 8, 0x186: 8}) - return CarInterface.get_params(car, fp, [], long, False, False) - - -def test_closed_decoder_matches_native_exhaustively(): - n = libsafety_py.libsafety - for word in range(65536): - decoded = decode(word) - assert n.get_ford_pinion_base(word) == (decoded[0] if decoded is not None else 65535) - - -@pytest.mark.parametrize('car', list(GEOMETRY)) -@pytest.mark.parametrize('long', [False, True]) -def test_default_off_and_exact_geometry_selection(car, long): - cp = params(car, long) - before = cp.to_dict() - assert not select(cp, False) - assert cp.to_dict() == before - assert select(cp, True) - assert configured(cp) and qualified(cp) - assert decode(cp.safetyConfigs[-1].safetyParam)[1] == GEOMETRY[car] - # Cross-family/identity corruption is not accepted by the host. - cp.safetyConfigs[-1].safetyParam = (cp.safetyConfigs[-1].safetyParam & ~MASK) | ENABLE | ((GEOMETRY[car] % 12 + 1) << SHIFT) - assert not configured(cp) and not qualified(cp) - - -@pytest.mark.parametrize('car', list(GEOMETRY)) -def test_geometry_matches_actual_vehicle_model_and_native_sign(car): - cp = params(car) - index = GEOMETRY[car] - n = libsafety_py.libsafety - vm = VehicleModel(cp) - assert n.get_ford_pinion_slip(index) == pytest.approx(calc_slip_factor(vm), abs=2e-10) - assert n.get_ford_pinion_ratio(index) == pytest.approx(cp.steerRatio) - assert n.get_ford_pinion_wheelbase(index) == pytest.approx(cp.wheelbase) - assert select(cp, True) - host = Measurement(cp) - packer = CANPacker('ford_lincoln_base_pt') - for speed in (0., 5., 15., 30.): - for angle in (-100., -10., 0., 10., 100.): - n.set_alternative_experience(0) - n.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam) - assert n.get_ford_pinion_enabled() - for counter in range(6): - a, d, b = packer.make_can_msg('BrakeSysFeatures', 0, {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3}) - assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)) - a, d, b = packer.make_can_msg('SteeringPinion_Data', 0, {'StePinComp_An_Est': angle, 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': counter}) - assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)) - state = SimpleNamespace(vEgoRaw=max(speed, .1), yawRate=99., steeringAngleDeg=angle) - expected = -host.curvature(state) * 50000 - assert n.get_curvature_meas_min() == pytest.approx(expected, abs=1.) - assert n.get_curvature_meas_max() == pytest.approx(expected, abs=1.) - # A valid but contradictory yaw packet never changes selected pinion measurement. - a, d, b = packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': 1., 'VehYawWActl_D_Qf': 3}) - assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)) - assert n.get_curvature_meas_min() == pytest.approx(expected, abs=1.) - - -def test_default_measurement_is_yaw_even_with_conflicting_pinion(): - cp = params(next(iter(GEOMETRY))) - m = Measurement(cp) - state = SimpleNamespace(vEgoRaw=15., yawRate=.3, steeringAngleDeg=100.) - assert m.curvature(state) == -.3 / 15. - assert not m.enabled - - -@pytest.mark.parametrize('index,base', [(1, 32), (3, 66), (10, 10), (11, 18)]) -@pytest.mark.parametrize('long', [False, True]) -@pytest.mark.parametrize('manual', [False, True]) -def test_pinion_inventory_and_invalid_quality_withdraw_authority(index, base, long, manual): - from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent - n = TestFordAolDriverIntent() - n.setUp() - base |= int(long) | (256 if manual else 0) - n.reset(base) - count = n.safety.get_ford_pinion_rx_count() - n.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, base | ENABLE | index << SHIFT) - n.safety.set_aol_test_heartbeat(True) - assert n.safety.get_ford_pinion_rx_count() == count + 1 - original_rx = n.rx - - def rx(name, values): - if name == 'Yaw_Data_FD1': - original_rx('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': n.tick % 16}) - original_rx(name, values) - n.rx = rx - try: - n.arm() - assert n.safety.aol_get_permission_mask() == 1 - a, d, b = n.packer.make_can_msg('SteeringPinion_Data', 0, - {'StePinComp_An_Est': 100., 'StePinCompAnEst_D_Qf': 0, 'StePinAn_No_Cnt': (n.tick + 1) % 16}) - assert not n.safety.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)) - assert n.safety.aol_get_permission_mask() == 0 - assert n.safety.get_curvature_meas_max() == 0 - finally: - n.tearDown() - - -@pytest.mark.parametrize('failure', ['missing', 'wrong_bus', 'short', 'counter']) -def test_pinion_integrity_is_required_not_substituted_by_healthy_yaw(failure): - from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent - n = TestFordAolDriverIntent() - n.setUp() - n.reset(66) - n.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, 66 | ENABLE | 7 << SHIFT) - n.safety.set_aol_test_heartbeat(True) - original_rx = n.rx - inject = True - - def rx(name, values): - if name == 'Yaw_Data_FD1': - if inject: - original_rx('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': n.tick % 16}) - elif failure != 'missing': - a, d, b = n.packer.make_can_msg('SteeringPinion_Data', 0, - {'StePinComp_An_Est': 100., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': 2}) - n.safety.safety_rx_hook(libsafety_py.make_CANPacket(a, 1 if failure == 'wrong_bus' else b, d[:7] if failure == 'short' else d)) - original_rx(name, values) - n.rx = rx - try: - n.arm() - inject = False - n.pump(120, mask=1) - assert n.safety.aol_get_permission_mask() == 0 - finally: - n.tearDown() - - -@pytest.mark.parametrize('car', [c for c in GEOMETRY if 'MONDEO' not in c]) -@pytest.mark.parametrize('long', [False, True]) -def test_angle_selection_preserves_pinion_and_aol_exact_profile(car, long): - from opendbc.bluepilot_lateral.hosts.starpilot import select as angle_select, qualified as angle_qualified - from opendbc.car.ford.aol import base_safety_param, AOL_WORDS - cp = params(car, long) - assert select(cp, True) - assert angle_select(cp, 1) - assert configured(cp) and angle_qualified(cp) - cp.safetyConfigs[-1].safetyParam |= base_safety_param(cp) - cp.alternativeExperience = 32 - assert cp.safetyConfigs[-1].safetyParam in AOL_WORDS - assert configured(cp) and angle_qualified(cp) and qualified(cp, marked_only=True) - - -@pytest.mark.parametrize('word', [32 | ENABLE, 32 | (1 << SHIFT), 32 | ENABLE | (13 << SHIFT), - 66 | ENABLE | (1 << SHIFT), 32 | ENABLE | (1 << SHIFT) | 0x4000, - 32 | ENABLE | (1 << SHIFT) | 0x8000]) -def test_invalid_pinion_native_profile_denies_all_commands(word): - n = libsafety_py.libsafety - n.set_alternative_experience(32) - n.set_safety_hooks(structs.CarParams.SafetyModel.ford, word) - n.set_controls_allowed(True) - assert not n.get_ford_pinion_enabled() - for address in (0x3D3, 0x3D6, 0x3CA, 0x186, 0x18A, 0x3D8): - assert not n.safety_tx_hook(libsafety_py.make_CANPacket(address, 0, bytes(8))) - - -def test_pinion_rx_capacity_guard_withdraws_authority_without_overwrite(): - n = libsafety_py.libsafety - n.set_alternative_experience(32) - n.set_safety_hooks(structs.CarParams.SafetyModel.ford, 66 | ENABLE | (7 << SHIFT)) - assert n.test_ford_pinion_rx_overflow() diff --git a/opendbc_repo/opendbc/car/ford/tests/test_production_extensions.py b/opendbc_repo/opendbc/car/ford/tests/test_production_extensions.py deleted file mode 100644 index b1fcfeaf0b..0000000000 --- a/opendbc_repo/opendbc/car/ford/tests/test_production_extensions.py +++ /dev/null @@ -1,121 +0,0 @@ -import gc -import time - -import pytest - -from opendbc.car import structs -from opendbc.car.ford.classic_lateral import CLASSIC_EXTENDED_CARS -from opendbc.car.ford.tests.test_three_ports import params -from opendbc.car.ford.values import CAR, CarControllerParams - - -@pytest.mark.parametrize('identity', sorted(CLASSIC_EXTENDED_CARS | {CAR.FORD_MUSTANG_MACH_E_MK1})) -@pytest.mark.parametrize('disable', (False, True)) -@pytest.mark.parametrize('alpha,release', ((False, False), (True, False), (False, True), (True, True))) -def test_actual_card_publication_reaches_ford_extensions(identity, alpha, release, disable, monkeypatch): - from opendbc.car import car_helpers, gen_empty_fingerprint - from opendbc.car.ford.classic_lateral import qualified as classic_qualified - from opendbc.car.ford.mache_lateral import qualified as mache_qualified - from openpilot.selfdrive.car import card as card_module - from openpilot.cereal import messaging - from openpilot.common.params import Params - from openpilot.common.prefix import OpenpilotPrefix - from openpilot.selfdrive.car.card import Car - from openpilot.selfdrive.controls.controlsd import Controls - from openpilot.starpilot.controller_extensions import ManualTurnInputs - from openpilot.starpilot.lateral.tests.test_lane_runtime import feed - - monkeypatch.setenv('SIMULATION', '1') - monkeypatch.setenv('REPLAY', '1') - monkeypatch.setenv('AOL_REPLAY_RUNTIME', '0') - with OpenpilotPrefix(): - saved = Params() - saved.put_bool('OpenpilotEnabledToggle', True, block=True) - saved.put_bool('AlwaysOnLateral', False, block=True) - saved.put_bool('AlphaLongitudinalEnabled', alpha, block=True) - saved.put_bool('IsReleaseBranch', release, block=True) - saved.put_bool('DisableOpenpilotLongitudinal', disable, block=True) - saved.put_bool('FordHumanTurnDetection', False, block=True) - cp = params(identity, alpha, release) - cp.carVin = '0' * 17 - cp.carFw = [] - expected = cp.to_dict() - fingerprint = gen_empty_fingerprint() - fingerprint[0][0x5A] = 8 - fingerprint[2][0x3D6] = 8 - fingerprint[2][0x186] = 8 - observed = (identity, fingerprint, cp.carVin, [], cp.fingerprintSource, True) - subscriber = messaging.sub_sock('carParams', timeout=100, conflate=True) - ci = card = controls = None - try: - with monkeypatch.context() as discovery: - discovery.setattr(car_helpers, 'fingerprint', lambda *args: observed) - discovery.setattr(card_module, 'can_comm_callbacks', lambda *args: (lambda *args: [], lambda frames: None)) - initial_can = messaging.new_message('can', 1) - discovery.setattr(card_module.messaging, 'recv_one_retry', lambda socket: initial_can) - card = Car() - ci = card.CI - assert card.CP.to_dict() == expected - assert mache_qualified(card.CP) if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else classic_qualified(card.CP) - owner = ci.CC.mache_lateral if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else ci.CC.classic_lateral - assert owner is not None - assert isinstance(ci.CC.manual_turn_inputs, ManualTurnInputs) - assert not ci.CC.manual_turn_inputs.enabled - assert not ci.CC.mache_extended_announced and not ci.CC.classic_extended_announced - with structs.CarParams.from_bytes(saved.get('CarParams')) as published: - assert published.to_dict() == expected - event = None - for _ in range(10): - card.car_params_published = False - card.state_publish(ci.update([]), None) - event = messaging.recv_one(subscriber) - if event is not None: - break - time.sleep(.01) - assert event is not None and event.valid and event.carParams.to_dict() == expected - controls = Controls() - assert controls.CP.to_dict() == expected - assert not controls.aol_replay and not controls.ordinary_axis_ack_required - feed(controls, 1_000_000_000, 0, active=False, enabled=False, can_valid=False, can_timeout=True) - command, lateral_log = controls.state_control() - assert not command.enabled and not command.latActive and not command.longActive - controls.publish(command, lateral_log) - _, packets = ci.apply(command.as_reader(), 1_000_000_000) - assert packets - assert ci.CC.mache_extended_announced if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else ci.CC.classic_extended_announced - assert not owner.human_turn_enabled - feed(controls, 1_010_000_000, 1, active=True, enabled=True, can_valid=True, can_timeout=False) - command, _ = controls.state_control() - assert command.enabled and command.latActive - if identity == CAR.FORD_BRONCO_SPORT_MK1 and not release and not disable: - from opendbc.safety.tests.test_ford_explorer_extended import TestFordExplorerExtended - from opendbc.safety.tests.libsafety import libsafety_py - - native = TestFordExplorerExtended() - native.setUp() - try: - native.select(card.CP.safetyConfigs[-1].safetyParam, card.CP.alternativeExperience) - for address, data, bus in packets: - if address == 0x3CA: - assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) - native._reset_curvature_measurement(0., 20.) - assert native._rx(native._pcm_status_msg(True)) - assert native.safety.get_controls_allowed() - ci.CS.out = controls.sm['carState'] - active_packets = 0 - for frame in range(CarControllerParams.STEER_STEP): - _, outgoing = ci.apply(command.as_reader(), 1_020_000_000 + frame * 10_000_000) - for address, data, bus in outgoing: - if address == 0x3D3: - active_packets += 1 - packet = libsafety_py.make_CANPacket(address, bus, data) - assert native.safety.safety_tx_hook(packet) - assert native._rx(native._pcm_status_msg(False)) - assert not native.safety.get_controls_allowed() - assert not native.safety.safety_tx_hook(packet) - assert active_packets > 0 - finally: - native.tearDown() - finally: - del controls, card, ci, subscriber - gc.collect() diff --git a/opendbc_repo/opendbc/car/ford/tests/test_stock_cruise.py b/opendbc_repo/opendbc/car/ford/tests/test_stock_cruise.py index 5ece387295..45b70886b2 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_stock_cruise.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_stock_cruise.py @@ -91,13 +91,13 @@ def test_reached_controller_preserves_precedence_cadence_and_stock_bits(): def test_unknown_ae_longitudinal_and_unpaired_profiles_deny_translation(): - # Actual factory results, not fabricated safety words. Classic stock0 is unreached. + # Actual factory results, not fabricated safety words. fingerprint = gen_empty_fingerprint() fingerprint[0][0x5a] = 8 fingerprint[2][0x3d6] = fingerprint[2][0x186] = 8 - matrix = ((CAR.FORD_F_150_MK14, 66), (CAR.FORD_EDGE_MK2, 8), + matrix = ((CAR.FORD_F_150_MK14, 2), (CAR.FORD_EDGE_MK2, 8), (CAR.FORD_MONDEO_MK5, 10), (CAR.FORD_TRANSIT_MK5, 12), - (CAR.FORD_MUSTANG_MACH_E_MK1, 18)) + (CAR.FORD_MUSTANG_MACH_E_MK1, 2), (CAR.FORD_BRONCO_SPORT_MK1, 0)) for vehicle, word in matrix: actual = CarInterface.get_params(vehicle, fingerprint, [], False, False, False) assert actual.safetyConfigs[-1].safetyParam == word, vehicle @@ -119,7 +119,7 @@ def test_unknown_ae_longitudinal_and_unpaired_profiles_deny_translation(): cp.openpilotLongitudinalControl = True assert not qualified(cp) cp.openpilotLongitudinalControl = False - cp.safetyConfigs[-1].safetyParam = 19 + cp.safetyConfigs[-1].safetyParam = 3 assert not qualified(cp) for word in (0, 4, 34): cp.safetyConfigs[-1].safetyParam = word diff --git a/opendbc_repo/opendbc/car/ford/values.py b/opendbc_repo/opendbc/car/ford/values.py index 60d1f0b447..9f1d629531 100644 --- a/opendbc_repo/opendbc/car/ford/values.py +++ b/opendbc_repo/opendbc/car/ford/values.py @@ -45,12 +45,7 @@ class FordSafetyFlags(IntFlag): CANFD = 2 LKA_STEERING = 4 NEW_PORT = 8 - MACH_E_EXTENDED = 16 - CLASSIC_EXTENDED = 32 - EXPLORER_EXTENDED = CLASSIC_EXTENDED # Existing public profile spelling. MANUAL = 256 - BLUEPILOT_ANGLE = 128 - GENERIC_CANFD_EXTENDED = 64 class FordFlags(IntFlag): @@ -354,3 +349,10 @@ FW_QUERY_CONFIG = FwQueryConfig( ) DBC = CAR.create_dbc_map() + +# StarPilot's Ford families: openpilot longitudinal is an explicit choice on these, and AOL, the stock +# cruise switch and the cluster/radar handling key off the family rather than a safety-param profile +FORD_CLASSIC_CARS = frozenset({CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_FOCUS_MK4, + CAR.FORD_MAVERICK_MK1, CAR.FORD_EXPLORER_MK6}) +FORD_CANFD_CARS = frozenset({CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, CAR.FORD_F_150_MK14, + CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2, CAR.FORD_MUSTANG_MACH_E_MK1}) diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index f3db6bdb41..a415289742 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -1,14 +1,8 @@ #pragma once #include "opendbc/safety/declarations.h" +#include "opendbc/safety/modes/ford_param.h" #include "opendbc/safety/modes/ford_aol.h" -#include "opendbc/safety/modes/ford_pinion_measurement.h" -#include "opendbc/bluepilot_lateral/safety/ford_bp.h" - -// StarPilot's extended Ford curvature enforcement below is substantially adapted from -// BluePilot bp-7.0 panda work, principally Alan Polk's 8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d -// See /CREDITS.md and /THIRD_PARTY_NOTICES.md. This comment does not attribute the surrounding -// upstream openpilot code. // Safety-relevant CAN messages for Ford vehicles. @@ -17,8 +11,8 @@ #define FORD_DesiredTorqBrk 0x213U // RX from ABS, for standstill state #define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed #define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed -#define FORD_SteeringPinion_Data 0x7EU #define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate +#define FORD_SteeringPinion_Data 0x7EU // RX from PSCM, optional angle_meas source (BluePilot STEER_ANGLE_CURVATURE) #define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons #define FORD_ACCDATA 0x186U // TX by OP, ACC controls #define FORD_ACCDATA_3 0x18AU // TX by OP, ACC/TJA user interface @@ -43,6 +37,7 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) { cnt = msg->data[5]; } if (msg->addr == FORD_SteeringPinion_Data) { + // Signal: StePinAn_No_Cnt (47|4@0+) cnt = (msg->data[5] >> 4) & 0xFU; } return cnt; @@ -92,7 +87,7 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf } if (msg->addr == FORD_SteeringPinion_Data) { - valid = ((msg->data[5] >> 2) & 0x3U) == 0x3U; + valid = ((msg->data[5] >> 2) & 0x3U) == 0x3U; // StePinCompAnEst_D_Qf (3=OK) } return valid; } @@ -113,17 +108,11 @@ static const CurvatureSteeringLimits FORD_STEERING_LIMITS = { .max_steer_power = 0, // disabled, Ford has no steed power signal }; -static CurvatureSteeringLimits ford_measurement_limits(CurvatureSteeringLimits limits) { - const int max_curvature_error = (ford_pinion_enabled && (limits.max_curvature_error < 150)) ? 150 : limits.max_curvature_error; - return (CurvatureSteeringLimits){ - .max_curvature = limits.max_curvature, - .curvature_to_can = limits.curvature_to_can, - .frequency = limits.frequency, - .max_curvature_error = max_curvature_error, - .curvature_error_min_speed = limits.curvature_error_min_speed, - .max_steer_power = limits.max_steer_power, - }; -} +// BluePilot: the Ford 4-signal lateral checks, pinion angle_meas source and angle-mode state, from the +// bluepilot_lateral library. Lateral follows StarPilot's lateral permission, which includes AOL. +#define BP_HOST_LAT_ALLOWED() lateral_controls_allowed() +// cppcheck-suppress misra-c2012-20.1; needs the Ford constants defined above +#include "opendbc/bluepilot_lateral/safety/ford.h" // Transit 0x3CA has 0.000005 rad/m per raw step and runs at 33 Hz. Keep its // command history separate from the inactive 0x3D3 heartbeat at 20 Hz. @@ -136,18 +125,10 @@ static const CurvatureSteeringLimits FORD_LKA_STEERING_LIMITS = { .max_steer_power = 0, }; +// The physical cancel/resume switch passes through on stock-ACC profiles +static bool ford_stock_switch = false; static bool ford_cancel_resume_button = false; -static bool ford_generic_canfd_extended = false; -static bool ford_generic_canfd_announced = false; - -static bool ford_explorer_extended = false; -static bool ford_explorer_announced = false; - -static bool ford_mach_e_extended = false; -static bool ford_mach_e_announced = false; -static int ford_mach_e_path_angle_last = 0; - static bool ford_lka_steering = false; static bool ford_lka_available = false; static uint32_t ford_lka_last_us = 0U; @@ -198,7 +179,7 @@ static void ford_rx_hook(const CANPacket_t *msg) { } // Update vehicle yaw rate - if (msg_matches(msg, FORD_Yaw_Data_FD1, FORD_MAIN_BUS) && !ford_pinion_enabled) { + if (msg_matches(msg, FORD_Yaw_Data_FD1, FORD_MAIN_BUS) && !ford_bp_pinion_curvature) { // BluePilot: pinion source replaces yaw // FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received // Signal: VehYaw_W_Actl // TODO: we should use the speed which results in the closest angle measurement to the desired angle @@ -213,14 +194,6 @@ static void ford_rx_hook(const CANPacket_t *msg) { } } - if (ford_pinion_enabled && msg_matches(msg, FORD_SteeringPinion_Data, FORD_MAIN_BUS)) { - const int raw = ((msg->data[2] & 0x7FU) << 8) | msg->data[3]; - const float radians = (((float)raw * 0.1F) - 1600.0F) * 0.017453292519943295F; - const float speed = SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1F); - const float measured = radians * get_curvature_factor(speed, *ford_pinion_params) / ford_pinion_params->steer_ratio; - update_sample(&curvature_state.meas, ROUND(measured * FORD_STEERING_LIMITS.curvature_to_can)); - } - // Update gas pedal if (msg_matches(msg, FORD_EngVehicleSpThrottle, FORD_MAIN_BUS)) { // Pedal position: (0.1 * val) in percent @@ -247,7 +220,7 @@ static void ford_rx_hook(const CANPacket_t *msg) { // Physical combined cancel/resume switch, not the outgoing resume signal. ford_cancel_resume_button = (msg->data[2] & 0x20U) != 0U; } - ford_bp_rx(msg); + ford_bp_rx_hook(msg); // BluePilot: pinion angle_meas source } static bool ford_tx_hook(const CANPacket_t *msg) { @@ -319,10 +292,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) { } // Safety check for Lane_Assist_Data1 action - if ((msg->addr == FORD_Lane_Assist_Data1) && !ford_bp_enabled) { - if (ford_mach_e_extended || ford_explorer_extended) { - tx &= (msg->data[4] & 0x1U) == 0U; - } + if (msg->addr == FORD_Lane_Assist_Data1) { // Do not allow steering using Lane_Assist_Data1 (Lane-Departure Aid). // This message must be sent for Lane Centering to work, and can include // values such as the steering angle or lane curvature for debugging, @@ -368,22 +338,11 @@ static bool ford_tx_hook(const CANPacket_t *msg) { } else { // Other Ford profiles retain their neutral status handling. } - } - - if (ford_mach_e_extended && tx && (msg->addr == FORD_Lane_Assist_Data1)) { - ford_mach_e_announced = (msg->data[4] & 0x2U) != 0U; - } - - if (ford_generic_canfd_extended && tx && (msg->addr == FORD_Lane_Assist_Data1)) { - ford_generic_canfd_announced = (msg->data[4] & 0x2U) != 0U; - } - - if (ford_explorer_extended && tx && (msg->addr == FORD_Lane_Assist_Data1)) { - ford_explorer_announced = (msg->data[4] & 0x2U) != 0U; + ford_bp_lka_tx(msg); // BluePilot: angle-mode state from the frame's unused bits } // Safety check for LateralMotionControl action - if ((msg->addr == FORD_LateralMotionControl) && !ford_bp_enabled) { + if (msg->addr == FORD_LateralMotionControl) { // Signal: LatCtl_D_Rq bool steer_control_enabled = ((msg->data[4] >> 2) & 0x7U) != 0U; unsigned int raw_curvature = (msg->data[0] << 3) | (msg->data[1] >> 5); @@ -391,39 +350,27 @@ static bool ford_tx_hook(const CANPacket_t *msg) { unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5); unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[6] >> 6); - // Explorer restores only the classic source-derived curvature-rate field. - // Every command still intersects the modern common curvature envelope. - bool violation = (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || - (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); - const unsigned int curvature_bits = raw_curvature; - const int desired_curvature = (int)curvature_bits - 1000; - if (ford_explorer_extended) { - static const struct lookup_t source_rate = {{5.0F, 16.0F, 25.0F}, {0.0025F, 0.0014F, 0.00018F}}; - if (steer_control_enabled && lateral_controls_allowed()) { - const float source_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0F; - const float source_delta_float = (safety_interpolate(source_rate, source_speed) * 50000.0F) + 1.0F; - const int source_delta = (int)source_delta_float; - violation |= SAFETY_ABS(desired_curvature - curvature_state.desired_last) > source_delta; - } - violation |= steer_control_enabled && !ford_explorer_announced; - violation |= !steer_control_enabled && ((desired_curvature != 0) || - (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE)); + bool violation = false; + if (ford_bp_lateral) { // BluePilot: 4-signal lateral + violation = ford_bp_lmc_tx(false, raw_curvature, raw_curvature_rate, raw_path_offset, raw_path_angle, steer_control_enabled); } else { - violation |= raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE; + // These signals are not yet tested with the current safety limits + violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + + // Check angle error and steer_control_enabled + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature + violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS); } - violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS)); + // The Transit steers through Lane_Assist_Data1; its LateralMotionControl is an inactive heartbeat violation |= ford_lka_steering && steer_control_enabled; if (violation) { tx = false; - if (ford_explorer_extended) { - curvature_state.desired_last = 0; - } } } // Safety check for LateralMotionControl2 action - if ((msg->addr == FORD_LateralMotionControl2) && !ford_bp_enabled) { + if (msg->addr == FORD_LateralMotionControl2) { // Signal: LatCtl_D2_Rq bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U; unsigned int raw_curvature = (msg->data[2] << 3) | (msg->data[3] >> 5); @@ -431,86 +378,23 @@ static bool ford_tx_hook(const CANPacket_t *msg) { unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2); unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[5]; - const unsigned int curvature_bits = raw_curvature; - const int desired_curvature = (int)curvature_bits - (int)FORD_INACTIVE_CURVATURE; - const int desired_path_angle = (int)raw_path_angle - (int)FORD_INACTIVE_PATH_ANGLE; - bool violation = raw_path_offset != FORD_INACTIVE_PATH_OFFSET; - if (ford_mach_e_extended) { - // Exact Mach-E extension retains the source envelope and the modern common - // ISO/RT checks. This is an intersection, not generic Ford limit widening. - static const CurvatureSteeringLimits FORD_MACH_E_STEERING_LIMITS = { - .max_curvature = 1000, - .curvature_to_can = 50000, - .frequency = 20, - .max_curvature_error = 300, - .curvature_error_min_speed = 10.0, - .max_steer_power = 0, - }; - const struct lookup_t source_rate = {{5.0F, 16.0F, 25.0F}, {0.0025F, 0.0014F, 0.00018F}}; - const float speed = vehicle_speed.max / VEHICLE_SPEED_FACTOR; - const float speed_min = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0F); - const int source_delta = (safety_interpolate(source_rate, speed_min - 1.0F) * 50000.0F) + 1.0F; - const float source_max_float = ((3.0F - (9.81F * 0.06F)) / (speed_min * speed_min) * 50000.0F) + 1.0F; - const int source_max = (int)source_max_float; - if (!ford_mach_e_announced) { - violation |= steer_control_enabled || (desired_path_angle != 0) || (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE); - } - if (steer_control_enabled) { - violation |= SAFETY_ABS(desired_curvature - curvature_state.desired_last) > source_delta; - violation |= SAFETY_ABS(desired_curvature) > source_max; - } else { - violation |= (desired_curvature != 0) || (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE); - } - if (desired_path_angle != 0) { - const float curvature = SAFETY_ABS(desired_curvature) / 50000.0F; - const float path_angle = SAFETY_ABS(desired_path_angle) / 2000.0F; - const float combined_accel = (curvature + (path_angle / SAFETY_MAX(speed, 1.0F))) * speed * speed; - violation |= !steer_control_enabled || !controls_allowed; - violation |= (speed < 3.0F) || (speed >= 8.8F); - violation |= (SAFETY_ABS(desired_curvature) < 975) || (SAFETY_ABS(desired_path_angle) > 320); - violation |= ((desired_curvature * desired_path_angle) <= 0) || (combined_accel > 2.5F); - violation |= SAFETY_ABS(desired_path_angle - ford_mach_e_path_angle_last) > 110; - } - violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_MACH_E_STEERING_LIMITS)); - } else if (ford_generic_canfd_extended) { - // One generic CAN FD owner; Mach-E's wider error/path assist stays separate. - static const struct lookup_t source_rate = {{5.0F, 16.0F, 25.0F}, {0.0025F, 0.0014F, 0.00018F}}; - const float source_speed = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0F); - const float source_delta_float = (safety_interpolate(source_rate, source_speed - 1.0F) * 50000.0F) + 1.0F; - const int source_delta = (int)source_delta_float; - const float source_max_float = ((3.0F - (9.81F * 0.06F)) / (source_speed * source_speed) * 50000.0F) + 1.0F; - const int source_max = (int)source_max_float; - violation |= raw_path_angle != FORD_INACTIVE_PATH_ANGLE; - violation |= steer_control_enabled && !ford_generic_canfd_announced; - if (steer_control_enabled) { - violation |= SAFETY_ABS(desired_curvature - curvature_state.desired_last) > source_delta; - violation |= SAFETY_ABS(desired_curvature) > source_max; - } else { - violation |= (desired_curvature != 0) || (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE); - } - violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS)); + bool violation = false; + if (ford_bp_lateral) { // BluePilot: 4-signal lateral + violation = ford_bp_lmc_tx(true, raw_curvature, raw_curvature_rate, raw_path_offset, raw_path_angle, steer_control_enabled); } else { - violation |= (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE); - violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS)); + // These signals are not yet tested with the current safety limits + violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET); + + // Check angle error and steer_control_enabled + int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.curvature_to_can to get real curvature + violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS); } - tx &= !violation; - if (ford_generic_canfd_extended && !tx) { - curvature_state.desired_last = 0; - } - if (ford_mach_e_extended && !tx) { - // External path/announcement checks must not retain a rejected command. - curvature_state.desired_last = 0; - ford_mach_e_path_angle_last = 0; - } - if (ford_mach_e_extended && tx) { - ford_mach_e_path_angle_last = desired_path_angle; + + if (violation) { + tx = false; } } - if (ford_bp_enabled) { - if (msg->addr == FORD_Lane_Assist_Data1) { tx &= ford_bp_lka(msg); } - if ((msg->addr == FORD_LateralMotionControl) || (msg->addr == FORD_LateralMotionControl2)) { tx &= ford_bp_lateral(msg); } - } ford_aol_tx(msg, tx); return tx; } @@ -533,15 +417,16 @@ static void ford_aol_bind_tx(safety_config *ret, CanMsg workspace[7]) { } } - -static void ford_append_pinion_rx(safety_config *config, RxCheck workspace[11]) { +// BluePilot: the selected RX checks plus the pinion message, enforced only while the pinion angle_meas source +// is on (ford_bp_init), so a pinion hiccup can't disable controls for users who never consume the message. +static void ford_bp_append_pinion_rx(safety_config *config, RxCheck workspace[11]) { if (config->rx_checks_len <= 10) { const int count = config->rx_checks_len; for (int i = 0; i < count; i++) { workspace[i] = config->rx_checks[i]; } - workspace[count] = (RxCheck){.msg = {{FORD_SteeringPinion_Data, 0, 8, 100U, - .max_counter = 15U, .ignore_checksum = true}, {0}, {0}}}; + // StePinAn_No_Cs matches no Ford checksum pattern on real frames: integrity via counter (0-15) + quality flag + 100 Hz + workspace[count] = (RxCheck){.msg = {{FORD_SteeringPinion_Data, 0, 8, 100U, .max_counter = 15U, .ignore_checksum = true}, {0}, {0}}}; config->rx_checks = workspace; config->rx_checks_len = count + 1; } else { @@ -553,15 +438,7 @@ static void ford_append_pinion_rx(safety_config *config, RxCheck workspace[11]) static safety_config ford_init(uint16_t configured_param) { static RxCheck ford_rx_workspace[11]; - const bool pinion_valid = ford_pinion_word_valid(configured_param); - const uint16_t profile_param = ford_pinion_base_word(configured_param); - const uint16_t geometry = (configured_param >> 10U) & 15U; - ford_pinion_enabled = pinion_valid && ((configured_param & FORD_PINION_ENABLE) != 0U); - ford_pinion_params = ford_pinion_geometry_at(ford_pinion_enabled ? geometry : 0U); - const uint16_t bp_param = profile_param & ~256U; - const bool joint_angle = ((unsigned int)alternative_experience == 32U) && - ((bp_param == 146U) || (bp_param == 147U) || (bp_param == 160U) || (bp_param == 161U) || (bp_param == 194U) || (bp_param == 195U)); - const uint16_t param = joint_angle ? (bp_param & ~128U) : bp_param; + const uint16_t param = ford_profile_word(configured_param) & ~256U; const bool manual = (configured_param & 256U) != 0U; // Manual variants are only defined for the exact registered Ford profiles. const bool manual_valid = !manual || ford_aol_param_valid(configured_param); @@ -621,30 +498,9 @@ static safety_config ford_init(uint16_t configured_param) { const uint16_t FORD_PARAM_CANFD = 2; const uint16_t FORD_PARAM_LKA_STEERING = 4; const uint16_t FORD_PARAM_NEW_PORT = 8; - const uint16_t FORD_PARAM_MACH_E_EXTENDED = 16U; - const uint16_t FORD_PARAM_EXPLORER_EXTENDED = 32U; - const uint16_t FORD_PARAM_GENERIC_CANFD_EXTENDED = 64U; - const bool generic_canfd_namespace = GET_FLAG(param, FORD_PARAM_GENERIC_CANFD_EXTENDED); - ford_generic_canfd_extended = (param == 66U) && (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))); - ford_generic_canfd_extended |= (param == 67U) && (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))); - ford_generic_canfd_announced = false; - const bool explorer_namespace = GET_FLAG(param, FORD_PARAM_EXPLORER_EXTENDED); - ford_explorer_extended = ((param == 32U) || (param == 33U)) && - (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))); - ford_explorer_announced = false; - const bool mach_e_namespace = GET_FLAG(param, FORD_PARAM_MACH_E_EXTENDED); - ford_mach_e_extended = (param == 18U) && (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))); - ford_mach_e_extended |= (param == 19U) && (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))); - const bool bp_config_valid = ford_bp_configure(bp_param); - ford_stock_switch = ((((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))) && - ((param == 2U) || (param == 8U) || (param == 10U) || (param == 12U) || - ((param == 18U) && ford_mach_e_extended) || - ((param == 32U) && ford_explorer_extended) || - ((param == 66U) && ford_generic_canfd_extended))) || - (ford_bp_enabled && ((param & 1U) == 0U)); + ford_stock_switch = (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))) && + ((param == 0U) || (param == 2U) || (param == 8U) || (param == 10U) || (param == 12U)); ford_cancel_resume_button = false; - ford_mach_e_path_angle_last = 0; - ford_mach_e_announced = false; const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD); ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING); const bool ford_new_port = GET_FLAG(param, FORD_PARAM_NEW_PORT); @@ -662,7 +518,7 @@ static safety_config ford_init(uint16_t configured_param) { safety_config ret; if (ford_canfd) { ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS); - bool long_profile = (param == 11U) || (param == 19U) || (param == 67U); + bool long_profile = (param == 3U) || (param == 11U); #ifdef ALLOW_DEBUG long_profile |= GET_FLAG(param, 1U); #endif @@ -670,8 +526,9 @@ static safety_config ford_init(uint16_t configured_param) { ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS); } } else { - ret = ford_new_port ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS) : - BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS); + // Classic stock ACC (0) and the new ports send no ACCDATA unless openpilot long is set + ret = (ford_new_port || (param == 0U)) ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS) : + BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS); if (ford_lka_steering) { SET_RX_CHECKS(ford_lka_rx_checks, ret); } @@ -679,10 +536,6 @@ static safety_config ford_init(uint16_t configured_param) { SET_TX_MSGS(FORD_LONG_TX_MSGS, ret); } } - if (ford_explorer_extended && (param == 32U)) { - SET_TX_MSGS(FORD_STOCK_TX_MSGS, ret); - } - if (ford_bp_enabled && (param == 128U)) { SET_TX_MSGS(FORD_STOCK_TX_MSGS, ret); } if (ford_stock_switch) { if (ford_lka_steering) { SET_RX_CHECKS(ford_stock_lka_rx_checks, ret); @@ -690,9 +543,7 @@ static safety_config ford_init(uint16_t configured_param) { SET_RX_CHECKS(ford_stock_rx_checks, ret); } } - if (!manual_valid || !bp_config_valid || (mach_e_namespace && !ford_mach_e_extended) || - (explorer_namespace && !ford_explorer_extended) || - (generic_canfd_namespace && !ford_generic_canfd_extended)) { + if (!manual_valid) { ret.tx_msgs = NULL; ret.tx_msgs_len = 0; } @@ -736,7 +587,7 @@ static safety_config ford_init(uint16_t configured_param) { } else if (base == 10U) { selected_rx = ford_aol_mondeo_rx; selected_len = 10; - } else if ((base == 12U) || (base == 18U) || (base == 66U)) { + } else if ((base == 12U) || (base == 2U)) { selected_rx = ford_aol_transit_rx; selected_len = 10; } else { @@ -762,14 +613,11 @@ static safety_config ford_init(uint16_t configured_param) { _Static_assert(sizeof(FORD_CANFD_LONG_TX_MSGS) <= sizeof(ford_aol_tx_msgs), "Ford CAN FD long TX capacity"); ford_aol_bind_tx(&ret, ford_aol_tx_msgs); } - if (ford_pinion_enabled) { - // Append after profile-specific RX selection so AOL retains every existing check. - ford_append_pinion_rx(&ret, ford_rx_workspace); - } - if (!pinion_valid) { - ret.tx_msgs = NULL; - ret.tx_msgs_len = 0; - ford_aol_reset(); + // BluePilot: the library's startup bits ride bits 9-14 (ford_param.h). The Transit steers through + // Lane_Assist_Data1 alone and never takes them. + const uint16_t bp_param = ford_lka_steering ? 0U : ((configured_param & FORD_BP_PARAM_MASK) >> FORD_BP_PARAM_SHIFT); + if (ford_bp_init(bp_param)) { + ford_bp_append_pinion_rx(&ret, ford_rx_workspace); } return ret; } diff --git a/opendbc_repo/opendbc/safety/modes/ford_aol.h b/opendbc_repo/opendbc/safety/modes/ford_aol.h index aafbb7fa28..5c0d75b5d3 100644 --- a/opendbc_repo/opendbc/safety/modes/ford_aol.h +++ b/opendbc_repo/opendbc/safety/modes/ford_aol.h @@ -1,5 +1,5 @@ #pragma once -#include "opendbc/safety/modes/ford_pinion.h" +#include "opendbc/safety/modes/ford_param.h" static bool ford_aol_manual = false; static bool ford_aol_enabled = false; @@ -16,9 +16,10 @@ static bool ford_aol_session = false; static bool ford_aol_request_seen = false; static uint16_t ford_aol_param = 0U; +// CAN FD (not the Mondeo's new port) keeps the cluster display while AOL owns steering static bool ford_aol_retained_display_profile(void) { const uint16_t base = ford_aol_param & 0xFFFEU; - return (base == 18U) || (base == 66U); + return base == 2U; } static bool ford_aol_drive = false; static uint8_t ford_aol_eps = 0U; @@ -29,6 +30,19 @@ static bool ford_aol_lka_ready = false; static uint8_t ford_aol_owned = 0U; static uint32_t ford_aol_accepted_ts = 0U; +// The host sends the steering actuator every frame, active or not. While it is sending, the +// camera's copy stays off the bus, so the PSCM hears one sender, as it would under static +// blocking. A rejected host frame is a missed frame, as without AOL: letting the camera's +// inactive frame through in its place, mid-steer, makes the PSCM ramp out and fault. Only a +// host that has stopped sending gives the actuator back to the camera. +static bool ford_aol_host_actuator_seen = false; +static uint32_t ford_aol_host_actuator_ts = 0U; + +static bool ford_aol_host_actuator_current(void) { + return ford_aol_host_actuator_seen && + (safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_host_actuator_ts) <= AOL_HOST_REQUEST_TIMEOUT_US); +} + // An accepted inactive actuator plus a truthful OFF/standby display can own // cluster continuity without owning steering. Keep the existing command lease. #define FORD_AOL_NEUTRAL_STEERING_SEEN 0x80U @@ -108,6 +122,8 @@ static void ford_aol_reset(void) { ford_aol_eps = 0U; ford_aol_lka_ready = false; ford_aol_accepted_ts = 0U; + ford_aol_host_actuator_seen = false; + ford_aol_host_actuator_ts = 0U; } // Fresh physical MAIN/LKAS or ordinary cruise activation begins a new session. @@ -173,7 +189,7 @@ static uint8_t ford_aol_permission_mask(void) { ford_aol_clear(); } const uint16_t base = ford_aol_param & 0xFFFEU; - const bool needs_lka = (base == 10U) || (base == 12U) || (base == 18U) || (base == 66U); + const bool needs_lka = (base == 2U) || (base == 10U) || (base == 12U); const uint8_t requested = ford_aol_request_mask(); if (((requested & 1U) != 0U) && (controls_allowed || ford_aol_token) && ford_aol_main_available && ford_aol_drive && (ford_aol_eps == 0U) && (!needs_lka || ford_aol_lka_ready)) { @@ -184,7 +200,7 @@ static uint8_t ford_aol_permission_mask(void) { !brake_pressed && !gas_pressed && !gas_pressed_prev) { result |= 2U; } - if ((base == 18U) && ((result & 1U) == 0U)) { + if ((base == 2U) && ((result & 1U) == 0U)) { ford_aol_owned &= FORD_AOL_STEERING_OWNED; } return result; @@ -271,12 +287,9 @@ static void ford_aol_full_engagement(bool cruise_rising) { static bool ford_aol_param_valid(uint16_t param) { bool result = false; - switch (ford_pinion_base_word(param) & ~256U) { - case 8U: case 9U: case 10U: case 12U: case 13U: case 18U: case 32U: case 33U: case 66U: - result = true; - break; - case 146U: case 147U: case 160U: case 161U: case 194U: case 195U: - case 11U: case 19U: case 67U: + switch (ford_profile_word(param) & ~256U) { + case 0U: case 1U: case 2U: case 3U: + case 8U: case 9U: case 10U: case 11U: case 12U: case 13U: result = true; break; default: @@ -289,7 +302,7 @@ static void ford_aol_configure(uint16_t param) { ford_aol_reset(); ford_aol_enabled = ford_aol_param_valid(param) && ((unsigned int)alternative_experience == 32U); ford_aol_manual = (param & 256U) != 0U; - ford_aol_param = ford_pinion_base_word(param) & ~(256U | 128U); + ford_aol_param = ford_profile_word(param) & ~256U; if (ford_aol_enabled) { static const AolSafetyPolicy policy = { .reset = ford_aol_reset, @@ -304,7 +317,7 @@ static void ford_aol_configure(uint16_t param) { static unsigned int ford_aol_replacement(void) { const uint16_t base = ford_aol_param & 0xFFFEU; - return (base == 12U) ? 0x3CAU : (((base == 10U) || (base == 18U) || (base == 66U)) ? 0x3D6U : 0x3D3U); + return (base == 12U) ? 0x3CAU : (((base == 2U) || (base == 10U)) ? 0x3D6U : 0x3D3U); } static void ford_aol_tx(const CANPacket_t *msg, bool accepted) { @@ -326,6 +339,8 @@ static void ford_aol_tx(const CANPacket_t *msg, bool accepted) { const bool display_owned = steering_owned || (accepted && retained_display && ((permission & 1U) != 0U) && !relay_malfunction); ford_aol_owned = (steering_owned ? FORD_AOL_STEERING_OWNED : 0U) | (display_owned ? FORD_AOL_DISPLAY_OWNED : 0U); ford_aol_accepted_ts = microsecond_timer_get(); + ford_aol_host_actuator_seen = true; + ford_aol_host_actuator_ts = ford_aol_accepted_ts; } if ((msg->bus == 0U) && ((msg->addr == 0x18AU) || (msg->addr == 0x3D8U))) { if (accepted && ford_aol_neutral_steering_current() && ford_aol_neutral_packet(msg)) { @@ -347,6 +362,9 @@ static bool ford_aol_fwd(int bus, int addr) { blocked = (((ford_aol_owned & FORD_AOL_STEERING_OWNED) != 0U) && (addr == (int)ford_aol_replacement())) || (((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) && display_permitted && ((addr == 0x3D8) || (addr == 0x18A))); } + if (ford_aol_enabled && (bus == 2) && (addr == (int)ford_aol_replacement()) && ford_aol_host_actuator_current()) { + blocked = true; + } if (ford_aol_neutral_current() && (bus == 2)) { blocked |= ((addr == 0x18A) && ((ford_aol_neutral_display_mask & 1U) != 0U)) || ((addr == 0x3D8) && ((ford_aol_neutral_display_mask & 2U) != 0U)); diff --git a/opendbc_repo/opendbc/safety/modes/ford_param.h b/opendbc_repo/opendbc/safety/modes/ford_param.h new file mode 100644 index 0000000000..b82e02f290 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/ford_param.h @@ -0,0 +1,9 @@ +#pragma once +// The Ford safety param: StarPilot's Ford profile in bits 0-8 (long, CAN FD, LKA steering, new port, and 256 +// for AOL's manual transmission), the BluePilot lateral library's startup bits in 9-14. +#define FORD_BP_PARAM_SHIFT 9U +#define FORD_BP_PARAM_MASK 0x7E00U + +static inline uint16_t ford_profile_word(uint16_t word) { + return word & (uint16_t)~FORD_BP_PARAM_MASK; +} diff --git a/opendbc_repo/opendbc/safety/modes/ford_pinion.h b/opendbc_repo/opendbc/safety/modes/ford_pinion.h deleted file mode 100644 index d3f6f868d5..0000000000 --- a/opendbc_repo/opendbc/safety/modes/ford_pinion.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once -// Pinion measurement selection adapted from BluePilot PR #145 (fe5b0195). -// See CREDITS.md and THIRD_PARTY_NOTICES.md. No live mode switching. -#define FORD_PINION_ENABLE 0x200U -#define FORD_PINION_MASK 0x3E00U - -static bool ford_pinion_word_valid(uint16_t word) { - bool valid = true; - if ((word & FORD_PINION_MASK) != 0U) { - const uint16_t index = (word >> 10U) & 15U; - const uint16_t base = word & ~(FORD_PINION_MASK | 256U); - valid = (word & FORD_PINION_ENABLE) != 0U; - if ((index == 1U) || (index == 2U) || (index == 5U) || (index == 6U) || (index == 9U)) { - valid &= (base == 32U) || (base == 33U) || (base == 128U) || (base == 129U) || (base == 160U) || (base == 161U); - } else if ((index == 3U) || (index == 4U) || (index == 7U) || (index == 8U) || (index == 12U)) { - valid &= (base == 66U) || (base == 67U) || (base == 130U) || (base == 131U) || (base == 194U) || (base == 195U); - } else if (index == 11U) { - valid &= (base == 18U) || (base == 19U) || (base == 130U) || (base == 131U) || (base == 146U) || (base == 147U); - } else if (index == 10U) { - valid &= (base == 10U) || (base == 11U); - } else { - valid = false; - } - } - return valid; -} - -static uint16_t ford_pinion_base_word(uint16_t word) { - return ford_pinion_word_valid(word) ? (word & ~FORD_PINION_MASK) : 0xFFFFU; -} - diff --git a/opendbc_repo/opendbc/safety/modes/ford_pinion_measurement.h b/opendbc_repo/opendbc/safety/modes/ford_pinion_measurement.h deleted file mode 100644 index 53ad721d84..0000000000 --- a/opendbc_repo/opendbc/safety/modes/ford_pinion_measurement.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once -#define FORD_PINION_GEOMETRY_COUNT 12U -static const AngleSteeringParams *ford_pinion_geometry_at(uint16_t index) { - static const AngleSteeringParams ford_pinion_geometry[FORD_PINION_GEOMETRY_COUNT + 1U] = { - {.slip_factor = 0.0f, .steer_ratio = 1.0f, .wheelbase = 1.0f}, // 0: invalid - {.slip_factor = -0.00062819555f, .steer_ratio = 17.7f, .wheelbase = 2.670f}, // 1: FORD_BRONCO_SPORT_MK1 - {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 2: FORD_ESCAPE_MK4 - {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 3: FORD_ESCAPE_MK4_5 - {.slip_factor = -0.00045454798f, .steer_ratio = 17.0f, .wheelbase = 3.690f}, // 4: FORD_EXPEDITION_MK4 - {.slip_factor = -0.00055447339f, .steer_ratio = 16.8f, .wheelbase = 3.025f}, // 5: FORD_EXPLORER_MK6 - {.slip_factor = -0.00062121569f, .steer_ratio = 15.0f, .wheelbase = 2.700f}, // 6: FORD_FOCUS_MK4 - {.slip_factor = -0.00045331952f, .steer_ratio = 16.9f, .wheelbase = 3.700f}, // 7: FORD_F_150_LIGHTNING_MK1 - {.slip_factor = -0.00042037149f, .steer_ratio = 17.0f, .wheelbase = 3.990f}, // 8: FORD_F_150_MK14 - {.slip_factor = -0.00054528036f, .steer_ratio = 17.0f, .wheelbase = 3.076f}, // 9: FORD_MAVERICK_MK1 - {.slip_factor = -0.00058852001f, .steer_ratio = 14.8f, .wheelbase = 2.850f}, // 10: FORD_MONDEO_MK5 - {.slip_factor = -0.00056209187f, .steer_ratio = 17.0f, .wheelbase = 2.984f}, // 11: FORD_MUSTANG_MACH_E_MK1 - {.slip_factor = -0.00051293030f, .steer_ratio = 17.0f, .wheelbase = 3.270f}, // 12: FORD_RANGER_MK2 - }; - return &ford_pinion_geometry[index]; -} - -static bool ford_pinion_enabled = false; -static const AngleSteeringParams *ford_pinion_params = NULL; diff --git a/opendbc_repo/opendbc/safety/tests/ford_release_contract.py b/opendbc_repo/opendbc/safety/tests/ford_release_contract.py index de269a24d7..773699cbee 100644 --- a/opendbc_repo/opendbc/safety/tests/ford_release_contract.py +++ b/opendbc_repo/opendbc/safety/tests/ford_release_contract.py @@ -8,11 +8,12 @@ from opendbc.safety.tests.libsafety import libsafety_py libsafety_py.ffi.cdef('extern safety_config current_safety_config;') STOCK_CANFD_TX = frozenset(((0x83, 0, 8), (0x83, 2, 8), (0x18A, 0, 8), (0x3CA, 0, 8), (0x3D8, 0, 8), (0x3D6, 0, 8))) -RELEASE_LEDGER = {3: (6, STOCK_CANFD_TX)} +# the retired Mach-E (19) and generic CAN FD (67) extended long words: stock CAN FD TX, no AOL or LONG +RELEASE_LEDGER = {19: (6, STOCK_CANFD_TX), 67: (6, STOCK_CANFD_TX)} def assert_release_denial(native, word): - """Prove absent AOL/LONG, preserving the existing stock fallback of legacy parameter3. + """Prove absent AOL/LONG, preserving the existing stock fallback of a retired parameter. A physical PCM edge can still set the ordinary boolean even with an empty TX table. No assertion equates that boolean or hook return code with authority. diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index 2dc1752265..3e0044d9e8 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c @@ -466,20 +466,17 @@ bool safety_test_hyundai_gv60_catalog_rejection(bool alternate) { return rejected; } -// Pinion protocol introspection for exact host/native geometry and namespace tests. -uint16_t get_ford_pinion_base(uint16_t word) { return ford_pinion_base_word(word); } -bool get_ford_pinion_enabled(void) { return ford_pinion_enabled; } -float get_ford_pinion_slip(int index) { return ford_pinion_geometry_at((uint16_t)index)->slip_factor; } -float get_ford_pinion_ratio(int index) { return ford_pinion_geometry_at((uint16_t)index)->steer_ratio; } -float get_ford_pinion_wheelbase(int index) { return ford_pinion_geometry_at((uint16_t)index)->wheelbase; } -int get_ford_pinion_rx_count(void) { return current_safety_config.rx_checks_len; } +// BluePilot lateral hooks (modes/ford.h): the startup state ford_init gave the library, and the pinion RX +// append's guard. A selected RX list with no room left denies TX instead of dropping a check. +bool get_ford_bp_lateral(void) { return ford_bp_lateral; } +bool get_ford_bp_pinion_curvature(void) { return ford_bp_pinion_curvature; } +int get_ford_rx_checks_len(void) { return current_safety_config.rx_checks_len; } -bool test_ford_pinion_rx_overflow(void) { +bool test_ford_bp_pinion_rx_overflow(void) { RxCheck workspace[11] = {0}; safety_config config = current_safety_config; config.rx_checks = workspace; config.rx_checks_len = 11; - ford_append_pinion_rx(&config, workspace); - return (config.tx_msgs == NULL) && (config.tx_msgs_len == 0) && - (config.rx_checks_len == 11) && !ford_aol_enabled; + ford_bp_append_pinion_rx(&config, workspace); + return (config.tx_msgs == NULL) && (config.tx_msgs_len == 0) && (config.rx_checks_len == 11) && !ford_aol_enabled; } diff --git a/opendbc_repo/opendbc/safety/tests/mutation.py b/opendbc_repo/opendbc/safety/tests/mutation.py index baa2ae124c..2a77d4e7b4 100755 --- a/opendbc_repo/opendbc/safety/tests/mutation.py +++ b/opendbc_repo/opendbc/safety/tests/mutation.py @@ -529,8 +529,7 @@ def print_live_status(text, *, final=False): def _discover_test_catalog(): loader = unittest.TestLoader() catalog = {} - test_files = [*sorted(SAFETY_TESTS_DIR.glob("test_*.py")), - ROOT / "opendbc" / "bluepilot_lateral" / "tests" / "test_angle_native.py"] + test_files = [*sorted(SAFETY_TESTS_DIR.glob("test_*.py")), *sorted((BLUEPILOT_SAFETY_DIR / "tests").glob("test_*.py"))] for test_file in test_files: module_name = ".".join(test_file.relative_to(ROOT).with_suffix("").parts) suite = loader.loadTestsFromName(module_name) diff --git a/opendbc_repo/opendbc/safety/tests/test.sh b/opendbc_repo/opendbc/safety/tests/test.sh index 0ae505cfb8..904586247e 100755 --- a/opendbc_repo/opendbc/safety/tests/test.sh +++ b/opendbc_repo/opendbc/safety/tests/test.sh @@ -9,12 +9,16 @@ source ../../../setup.sh # TODO: get UBSan and coverage without running the tests twice. # Run UBSan separately so its checks do not count as coverage branches. SAFETY_COVERAGE=0 python -m unittest discover -s . +SAFETY_COVERAGE=0 python -m unittest discover -s ../../bluepilot_lateral/safety/tests -t ../../.. # reset coverage data, including metadata from previous builds rm -f ./libsafety/*.gcda ./libsafety/*.gcno # run safety tests and generate coverage data SAFETY_COVERAGE=1 python -m unittest discover -s . +# the BluePilot lateral library's safety tests, by directory: a loader here would put its classes in the +# cross-mode TX sweep, which reads every Test* class in this directory +SAFETY_COVERAGE=1 python -m unittest discover -s ../../bluepilot_lateral/safety/tests -t ../../.. # NOTE: we accept that these tools will have slight differences, # and in return, we get to use the stock toolchain instead of @@ -33,6 +37,13 @@ if [ "$1" == "--report" ]; then fi # test coverage +# the library's headers in a run of their own: its safety/ford.h shares modes/ford.h's file name, and one run +# credits one file's lines to the other +BP_GCOV="gcovr -r $DIR/../../.. --gcov-executable \"$GCOV_EXEC\" --fail-under-line=100 -f $DIR/../../bluepilot_lateral/safety/" +if ! BP_GCOV_OUTPUT="$(eval $BP_GCOV)"; then + echo -e "FAILED:\n$BP_GCOV_OUTPUT" + exit 1 +fi GCOV="gcovr -r $DIR/../ --gcov-executable \"$GCOV_EXEC\" -d --fail-under-line=100 -e ^libsafety" if ! GCOV_OUTPUT="$(eval $GCOV)"; then echo -e "FAILED:\n$GCOV_OUTPUT" diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index 70897ba3f6..62684f0635 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -493,11 +493,24 @@ class TestFordLongitudinalSafetyBase(TestFordSafetyBase): self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake, brake_actuation))) -class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): - # Make sure we enforce long safety even without long flag for CAN +class TestFordStockSafety(TestFordSafetyBase): + # StarPilot: classic stock ACC sends no ACCDATA SAFETY_PARAM = 0 STEER_MESSAGE = MSG_LateralMotionControl + TX_MSGS = [ + [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], + [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], + ] + RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data)} + + FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} + + +class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): + SAFETY_PARAM = FordSafetyFlags.LONG_CONTROL + STEER_MESSAGE = MSG_LateralMotionControl + TX_MSGS = [ [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA, 0], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0], diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_alpha_aol.py b/opendbc_repo/opendbc/safety/tests/test_ford_alpha_aol.py index 7f4e2b0ae6..1e57db4c84 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_alpha_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_alpha_aol.py @@ -7,8 +7,8 @@ from opendbc.car.ford.fordcan import CanBus, create_acc_msg from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests.test_ford_full_engagement import ford_native as ford_native, select -LONG_WORDS = (9, 11, 13, 19, 33, 67) -STOCK_WORDS = (8, 10, 12, 18, 32, 66) +LONG_WORDS = (1, 3, 9, 11, 13) +STOCK_WORDS = (0, 2, 8, 10, 12) def tx(native, *, active): @@ -53,7 +53,7 @@ def test_stock_long_off_never_gains_a_long_axis_from_any_request(ford_native, wo @pytest.mark.parametrize('fault', ('source', 'heartbeat', 'request-expired', 'eps-permanent')) def test_native_long_recovery_requires_new_activation_after_real_revocation(ford_native, fault): n = ford_native - select(n, 33) + select(n, 1) n.pump(40, main=3) n.pump(10, main=4, mask=3) assert n.safety.aol_get_permission_mask() == 3 diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_aol.py b/opendbc_repo/opendbc/safety/tests/test_ford_aol.py index 5243553eb9..7e746db552 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_aol.py @@ -16,19 +16,8 @@ class TestFordAolAdmission(unittest.TestCase): self.safety.set_alternative_experience(0) self.safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) - def test_angle_profiles_reject_malformed_lateral_frames_without_healthy_sources(self): - for word in (128, 129, 130, 131): - with self.subTest(word=word): - self.safety.init_tests() - self.safety.set_alternative_experience(0) - self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, word), 0) - self.safety.set_controls_allowed(False) - for address in (0x3CA, 0x3D3 if word & 2 else 0x3D6): - self.assertFalse(self.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, 0, bytes(8)))) - self.assertFalse(self.safety.get_controls_allowed()) - def test_no_frames_no_lateral_or_longitudinal_authority(self): - for word in (8, 9, 10, 11, 12, 13, 18, 19, 32, 33, 66, 67): + for word in (0, 1, 2, 3, 8, 9, 10, 11, 12, 13): for ae in (0, 32, 33): with self.subTest(word=word, ae=ae): self.safety.init_tests() @@ -42,7 +31,7 @@ class TestFordAolAdmission(unittest.TestCase): class TestFordAolDriverIntent(unittest.TestCase): - BASE_PROFILES = (8, 9, 10, 11, 12, 13, 18, 19, 32, 33, 66, 67) + BASE_PROFILES = (0, 1, 2, 3, 8, 9, 10, 11, 12, 13) PROFILES = BASE_PROFILES + tuple(word | 256 for word in BASE_PROFILES) def setUp(self): @@ -68,7 +57,7 @@ class TestFordAolDriverIntent(unittest.TestCase): self.assertTrue(self.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))) def pump(self, count, *, main=3, tja=False, gear=3, eps=0, mask=0, heartbeat=True, speed=10., curvature=0.): - base = self.word & ~(257 | 128) + base = self.word & ~257 gear_name, gear_signal = ('TransGearData', 'GearLvrPos_D_Actl') if base == 8 else ( ('Gear_Shift_by_Wire_FD1', 'TrnRng_D_RqGsm') if base == 10 else ('PowertrainData_10', 'TrnRng_D_Rq')) if self.word & 256: @@ -92,7 +81,7 @@ class TestFordAolDriverIntent(unittest.TestCase): for name, hz, fields in values: if self.tick * hz // 100 != (self.tick - 1) * hz // 100: self.rx(name, fields) - if base in (10, 12, 18, 66) and self.tick * 30 // 100 != (self.tick - 1) * 30 // 100: + if base in (2, 10, 12) and self.tick * 30 // 100 != (self.tick - 1) * 30 // 100: self.rx('Lane_Assist_Data3_FD1', {'LatCtlSte_D_Stat': 1, 'LaActAvail_D_Actl': 3, 'LaActDeny_B_Actl': 0}) self.safety.aol_set_host_request(mask) self.safety.aol_get_permission_mask() @@ -146,14 +135,13 @@ class TestFordAolDriverIntent(unittest.TestCase): packer = CANPacker("ford_lincoln_base_pt") self.safety.init_tests() self.safety.set_alternative_experience(32) - profiles = ((32, "PowertrainData_10", "TrnRng_D_Rq", False), + profiles = ((0, "PowertrainData_10", "TrnRng_D_Rq", False), (8, "TransGearData", "GearLvrPos_D_Actl", False), (10, "Gear_Shift_by_Wire_FD1", "TrnRng_D_RqGsm", True), (12, "PowertrainData_10", "TrnRng_D_Rq", True), - (18, "PowertrainData_10", "TrnRng_D_Rq", True), - (66, "PowertrainData_10", "TrnRng_D_Rq", True), + (2, "PowertrainData_10", "TrnRng_D_Rq", True), (8, "TransGearData", "GearLvrPos_D_Actl", False), - (32, "PowertrainData_10", "TrnRng_D_Rq", False)) + (0, "PowertrainData_10", "TrnRng_D_Rq", False)) for epoch, (word, gear_name, gear_signal, needs_lka) in enumerate(profiles): with self.subTest(word=word, epoch=epoch): self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, word), 0) @@ -193,12 +181,12 @@ class TestFordAolDriverIntent(unittest.TestCase): def test_main_cancel_states_and_physical_cancel_buttons_withdraw(self): for state in (1, 2): - self.reset(32) + self.reset(0) self.arm() self.pump(10, main=state, mask=1) self.assertEqual(self.safety.aol_get_permission_mask(), 0) for bit in (8, 24): - self.reset(32) + self.reset(0) self.arm() data = (1 << bit).to_bytes(8, 'little') self.assertTrue(self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x83, 0, data))) @@ -209,18 +197,13 @@ class TestFordAolDriverIntent(unittest.TestCase): def test_accepted_lateral_owns_only_replacement_until_lease_or_rejection(self): from opendbc.car.ford.fordcan import CanBus, create_lat_ctl_msg - from opendbc.car.ford.mache_can import create_lka_msg from opendbc.car import gen_empty_fingerprint - self.reset(32) + self.reset(0) self.arm() bus = CanBus(fingerprint=gen_empty_fingerprint()) frame = create_lat_ctl_msg(self.packer, bus, True, 0., 0., 0., 0.) command = libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]) self.assertEqual(self.safety.safety_fwd_hook(2, 0x3D3), 0) - self.assertFalse(self.safety.safety_tx_hook(command)) - announcement = create_lka_msg(self.packer, bus) - self.assertTrue(self.safety.safety_tx_hook(libsafety_py.make_CANPacket(announcement[0], announcement[2], announcement[1]))) - self.assertEqual(self.safety.safety_fwd_hook(2, 0x3D3), 0) self.assertTrue(self.safety.safety_tx_hook(command)) for address in (0x3D3, 0x3D8, 0x18A): self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) @@ -230,10 +213,74 @@ class TestFordAolDriverIntent(unittest.TestCase): for address in (0x3D3, 0x3D8, 0x18A): self.assertEqual(self.safety.safety_fwd_hook(2, address), 0) + def host_actuator(self, *, valid=True): + """This profile's steering actuator, inactive, as the host sends it every frame. Invalid adds a path offset, + or on the Transit requests steering without lateral permission.""" + from opendbc.car.ford.fordcan import CanBus, create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg + from opendbc.car import gen_empty_fingerprint + bus = CanBus(fingerprint=gen_empty_fingerprint()) + base = self.word & ~257 + offset = 0. if valid else 1. + if base == 12: + frame = create_lka_msg(self.packer, bus, transit=True) if valid else \ + create_lka_msg(self.packer, bus, True, 1., 2, .0002, transit=True) + elif base in (2, 10): + frame = create_lat_ctl2_msg(self.packer, bus, 0, offset, 0., 0., 0., 0) + else: + frame = create_lat_ctl_msg(self.packer, bus, False, offset, 0., 0., 0.) + return frame[0], libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]) + + def test_one_actuator_sender_while_the_host_sends(self): + # The camera's actuator is forwarded under AOL so the stock system keeps its message when the host + # isn't sending; while the host sends, two senders confuse the PSCM, and a camera frame standing in + # for a rejected host frame mid-steer makes it ramp out and fault + for word in self.PROFILES: + with self.subTest(word=word): + self.reset(word) + self.pump(10) + address, inactive = self.host_actuator() + self.assertEqual(self.safety.safety_fwd_hook(2, address), 0) + for _ in range(20): + self.pump(5) + self.assertTrue(self.safety.safety_tx_hook(inactive)) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + self.assertEqual(self.safety.safety_fwd_hook(0, address), 2) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x321), 0) + # replacing the camera's frame grants nothing + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.assertFalse(self.safety.get_controls_allowed()) + self.pump(30) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + self.pump(1) + self.assertEqual(self.safety.safety_fwd_hook(2, address), 0) + self.assertTrue(self.safety.safety_tx_hook(inactive)) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + # a rejected host frame is a missed frame, not the camera's turn + _, rejected = self.host_actuator(valid=False) + for _ in range(10): + self.pump(5) + self.assertFalse(self.safety.safety_tx_hook(rejected)) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + self.pump(31) + self.assertEqual(self.safety.safety_fwd_hook(2, address), 0) + self.assertFalse(self.safety.safety_tx_hook(rejected)) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + self.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, word) + self.assertEqual(self.safety.safety_fwd_hook(2, address), 0) + + def test_without_aol_the_actuator_stays_statically_blocked(self): + for word in self.BASE_PROFILES: + with self.subTest(word=word): + self.reset(word) + address, _ = self.host_actuator() + self.safety.set_alternative_experience(0) + self.assertEqual(self.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, word), 0) + self.assertEqual(self.safety.safety_fwd_hook(2, address), -1) + def test_native_bounded_catalog_copy_denies_invalid_and_restores_registered_table(self): for length, null_catalog in ((0, False), (1, False), (7, False), (8, False), (1, True)): with self.subTest(length=length, null_catalog=null_catalog): - self.reset(32) + self.reset(0) self.assertTrue(self.bounded_catalog_contract(length, null_catalog)) self.assertFalse(self.safety.get_controls_allowed()) self.assertEqual(self.safety.aol_get_permission_mask(), 0) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_aol.py b/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_aol.py deleted file mode 100644 index fb7c46844c..0000000000 --- a/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_aol.py +++ /dev/null @@ -1,44 +0,0 @@ -"""Combined angle authority retains the existing physical AOL lifecycle.""" -import unittest -from opendbc.safety.tests import test_ford_aol as existing -from opendbc.bluepilot_lateral.core.angle import Command -from opendbc.bluepilot_lateral.can.ford import lateral, lka -from opendbc.safety.tests.libsafety import libsafety_py - -class TestFordBluePilotAol(unittest.TestCase): - setUp = existing.TestFordAolDriverIntent.setUp - tearDown = existing.TestFordAolDriverIntent.tearDown - reset = existing.TestFordAolDriverIntent.reset - rx = existing.TestFordAolDriverIntent.rx - pump = existing.TestFordAolDriverIntent.pump - arm = existing.TestFordAolDriverIntent.arm - rearm = existing.TestFordAolDriverIntent.rearm - test_permanent_eps_then_clean_in_same_batch_revokes = existing.TestFordAolDriverIntent.test_permanent_eps_then_clean_in_same_batch_revokes - test_reverse_temporary_eps_retain_but_heartbeat_loss_rearms = ( - existing.TestFordAolDriverIntent.test_reverse_temporary_eps_retain_but_heartbeat_loss_rearms) - BASE_PROFILES = (146,147,160,161,194,195) - PROFILES = BASE_PROFILES + tuple(word|256 for word in BASE_PROFILES) - - def test_nonzero_angle_after_brake_requires_fresh_shadow_and_native_lat(self): - for word in self.PROFILES: - self.reset(word) - self.arm() - assert not self.safety.get_controls_allowed() - command = Command(True,0.005,0.005/10.) - def send(frame): - addr,data,bus=frame - return bool(self.safety.safety_tx_hook(libsafety_py.make_CANPacket(addr,bus,data))) - assert send(lka(self.packer,0,command.shadow_curvature,0)) - assert send(lateral(self.packer,0,command,canfd=bool(word&2),counter=0)) - self.rx('EngBrakeData',{'BpedDrvAppl_D_Actl':2,'CcStat_D_Actl':3}) - self.pump(5,mask=1) - assert self.safety.aol_get_permission_mask()==1 and not self.safety.get_controls_allowed() - assert send(lka(self.packer,0,command.shadow_curvature,1)) - assert send(lateral(self.packer,0,command,canfd=bool(word&2),counter=1)) - self.pump(10,mask=1) - assert self.safety.aol_get_permission_mask()==1 and not self.safety.get_controls_allowed() - assert send(lka(self.packer,0,command.shadow_curvature,2)) - assert send(lateral(self.packer,0,command,canfd=bool(word&2),counter=2)) - self.safety.aol_set_host_request(0) - self.pump(5,mask=0) - assert not send(lateral(self.packer,0,command,canfd=bool(word&2),counter=2)) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_hooks.py b/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_hooks.py new file mode 100644 index 0000000000..4ea2b96f26 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_hooks.py @@ -0,0 +1,77 @@ +"""StarPilot's hooks for the BluePilot lateral library in modes/ford.h: its startup bits ride bits 9-14 of the +Ford param, and StarPilot's own profiles keep their RX/TX contracts around them.""" +from pathlib import Path +import unittest + +from opendbc.car.structs import CarParams +from opendbc.car.ford.aol import BP_LATERAL, BP_PARAM_MASK +from opendbc.bluepilot_lateral.core.values_ext import FORD_PINION_GEOMETRY_INDEX, FORD_PINION_GEOMETRY_SHIFT, FordSafetyFlagsSP +from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import BP_SAFETY_PARAM_MASK, BP_SAFETY_PARAM_SHIFT +from opendbc.safety.tests.libsafety import libsafety_py + +LATERAL = int(FordSafetyFlagsSP.BP_LATERAL) +PINION = LATERAL | int(FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) | (FORD_PINION_GEOMETRY_INDEX["FORD_BRONCO_SPORT_MK1"] << FORD_PINION_GEOMETRY_SHIFT) + + +def word(base, bits=0): + return base | (bits << BP_SAFETY_PARAM_SHIFT) + + +class TestFordBluePilotHooks(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + + def tearDown(self): + self.safety.set_alternative_experience(0) + self.safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0) + + def init(self, param, ae=0): + self.safety.init_tests() + self.safety.set_alternative_experience(ae) + self.assertEqual(self.safety.set_safety_hooks(CarParams.SafetyModel.ford, param), 0) + + def test_starpilot_layout_matches_the_library(self): + self.assertEqual(BP_LATERAL, LATERAL << BP_SAFETY_PARAM_SHIFT) + self.assertEqual(BP_PARAM_MASK, BP_SAFETY_PARAM_MASK << BP_SAFETY_PARAM_SHIFT) + header = (Path(__file__).parents[1] / "modes" / "ford_param.h").read_text() + self.assertIn(f"#define FORD_BP_PARAM_SHIFT {BP_SAFETY_PARAM_SHIFT}U", header) + self.assertIn(f"#define FORD_BP_PARAM_MASK 0x{BP_PARAM_MASK:04X}U", header) + + def test_bits_reach_the_library(self): + for base in (0, 1, 2, 3, 8, 9, 10, 11): + with self.subTest(base=base): + self.init(word(base, LATERAL)) + self.assertTrue(self.safety.get_ford_bp_lateral()) + self.init(word(base)) + self.assertFalse(self.safety.get_ford_bp_lateral()) + + def test_transit_never_takes_the_bits(self): + for base in (12, 13): + with self.subTest(base=base): + self.init(word(base, PINION)) + self.assertFalse(self.safety.get_ford_bp_lateral()) + self.assertFalse(self.safety.get_ford_bp_pinion_curvature()) + + def test_pinion_rx_check_joins_the_selected_list(self): + for ae in (0, 32): # the stock list, and AOL's own + with self.subTest(ae=ae): + self.init(word(0, LATERAL), ae) + count = self.safety.get_ford_rx_checks_len() + self.init(word(0, PINION), ae) + self.assertTrue(self.safety.get_ford_bp_pinion_curvature()) + self.assertEqual(self.safety.get_ford_rx_checks_len(), count + 1) + + def test_full_rx_list_denies_tx_instead_of_dropping_a_check(self): + self.init(word(0, PINION), 32) + self.assertTrue(self.safety.test_ford_bp_pinion_rx_overflow()) + + def test_manual_bit_outside_the_registered_profiles_sends_nothing(self): + neutral_lka = libsafety_py.make_CANPacket(0x3CA, 0, bytes(8)) + self.init(word(256, LATERAL)) + self.assertTrue(self.safety.safety_tx_hook(neutral_lka)) + self.init(word(4 | 256, LATERAL)) # LKA steering without the new port: not a registered profile + self.assertFalse(self.safety.safety_tx_hook(neutral_lka)) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py b/opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py similarity index 90% rename from opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py rename to opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py index 4d1826a764..e047836c87 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py @@ -1,14 +1,14 @@ -"""Display continuity never grants steering or survives loss of native authority.""" +"""CAN FD display continuity never grants steering or survives loss of native authority.""" import pytest from opendbc.car import gen_empty_fingerprint -from opendbc.car.ford import mache_can, fordcan +from opendbc.car.ford import fordcan from opendbc.car.ford.fordcan import CanBus from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests import test_ford_aol -@pytest.fixture(params=(18, 19, 274, 275, 66, 67, 322, 323)) +@pytest.fixture(params=(2, 3, 258, 259)) def owner(request): native = test_ford_aol.TestFordAolDriverIntent() native.setUp() @@ -18,12 +18,13 @@ def owner(request): native.safety.set_alternative_experience(32) native.safety.set_aol_test_heartbeat(True) native.arm() - return native + yield native + native.tearDown() # AE 32 must not reach the next test's profile def command(owner, *, active, curvature=0., offset_byte=False): - frame = mache_can.create_lat_ctl2_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint()), - int(active), 0, 1, curvature, 0., 0) + frame = fordcan.create_lat_ctl2_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint()), + int(active), 0., 0., curvature, 0., 0) data = bytearray(frame[1]) if offset_byte: data[5] ^= 1 @@ -31,7 +32,7 @@ def command(owner, *, active, curvature=0., offset_byte=False): def announce(owner): - frame = mache_can.create_lka_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint())) + frame = fordcan.create_lka_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint())) assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])) @@ -69,7 +70,7 @@ def test_display_survives_fresh_inactive_packets_without_steering_ownership(owne for _ in range(30): owner.pump(5, mask=1) assert owner.safety.safety_tx_hook(command(owner, active=False)) - assert owner.safety.safety_fwd_hook(2, 0x3D6) == 0 + assert owner.safety.safety_fwd_hook(2, 0x3D6) == -1 # one 0x3D6 sender: the accepted inactive host frame for address in (0x18A, 0x3D8): assert owner.safety.safety_fwd_hook(2, address) == -1 assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, 0, bytes(8))) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_configuration_parity.py b/opendbc_repo/opendbc/safety/tests/test_ford_configuration_parity.py index 4738499bf6..ea7e296222 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_configuration_parity.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_configuration_parity.py @@ -11,7 +11,7 @@ from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests.test_ford_full_engagement import ford_native as ford_native, select from opendbc.safety.tests.test_ford_alpha_aol import tx -WORDS = (8, 9, 10, 11, 12, 13, 18, 19, 32, 33, 66, 67) +WORDS = (0, 1, 2, 3, 8, 9, 10, 11, 12, 13) @pytest.mark.parametrize('base', WORDS) @@ -133,7 +133,8 @@ def ford_release_native(tmp_path, record_property): native.tearDown() -def test_release_keeps_unregistered_legacy_canfd_long_parameter_denied(ford_release_native): +@pytest.mark.parametrize('word', (19, 67)) +def test_release_keeps_retired_extended_canfd_long_words_denied(ford_release_native, word): from opendbc.safety.tests.ford_release_contract import assert_release_denial - select(ford_release_native, 3) - assert_release_denial(ford_release_native, 3) + select(ford_release_native, word) + assert_release_denial(ford_release_native, word) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_explorer_extended.py b/opendbc_repo/opendbc/safety/tests/test_ford_explorer_extended.py deleted file mode 100644 index e3f2235d29..0000000000 --- a/opendbc_repo/opendbc/safety/tests/test_ford_explorer_extended.py +++ /dev/null @@ -1,102 +0,0 @@ -"""Exact Explorer classic extension intersects original and modern limits.""" -from opendbc.car.structs import CarParams -from opendbc.safety.tests.libsafety import libsafety_py -import unittest -from opendbc.safety.tests import test_ford as ford -from opendbc.safety.tests import test_ford_stock_switch as stock - - -class TestFordExplorerExtended(unittest.TestCase): - - def setUp(self): - libsafety_py.libsafety.set_alternative_experience(0) - self.fixture = ford.TestFordLongitudinalSafety() - self.fixture.SAFETY_PARAM = 32 - self.fixture.setUp() - - def tearDown(self): - self.safety.set_alternative_experience(0) - self.safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0) - - def __getattr__(self, name): - fixture = self.__dict__.get('fixture') - if fixture is None: - raise AttributeError(name) - return getattr(fixture, name) - - def select(self, word=32, ae=0): - self.safety.set_alternative_experience(ae) - self.safety.set_safety_hooks(CarParams.SafetyModel.ford, word) - self.safety.set_timer(0) - - def announce(self, active=True, forbidden=False): - msg = self._lkas_command_msg(0) - msg[0].data[4] = (2 if active else 0) | int(forbidden) - return self._tx(msg) - - def prepare(self, speed=5.0): - self._reset_curvature_measurement(0.0, speed) - self._set_prev_desired_angle(0.0) - self.safety.set_controls_allowed(True) - - def test_namespace_and_classic_long_both_variants(self): - for word in (32, 33): - self.select(word) - self.assertTrue(self.announce()) - self.prepare() - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0.0002))) - self.assertEqual(word == 33, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL, False))) - for word in (32, 33): - self.select(word, 32) - self.assertTrue(self.announce()) - self.assertTrue(self._tx(self._lat_ctl_msg(False, 0, 0, 0, 0))) - for word, ae in ((34, 0), (36, 0), (40, 0), (48, 0), (96, 0), (32, 1), (33, 1)): - self.select(word, ae) - self.assertFalse(self.announce()) - self.assertFalse(self._tx(self._lat_ctl_msg(False, 0, 0, 0, 0))) - - def test_accepted_announcement_and_inactive_fields(self): - self.prepare() - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0.0002))) - self.assertFalse(self.announce(forbidden=True)) - self.prepare() - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0.0002))) - self.assertTrue(self.announce()) - for rate in (-0.001024, 0.00102375): - self.prepare() - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, 0, rate))) - for offset, angle, curvature, rate in ((0.01, 0, 0, 0), (0, 0.01, 0, 0), (0, 0, 0, 0.0002), (0, 0, 0.001, 0)): - self.prepare() - self.assertFalse(self._tx(self._lat_ctl_msg(False, offset, angle, curvature, rate))) - self.assertTrue(self._tx(self._lat_ctl_msg(False, 0, 0, 0, 0))) - self.assertTrue(self.announce(active=False)) - self.prepare() - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, 0, 0))) - - def test_modern_jerk_and_original_lookup_intersect(self): - self.assertTrue(self.announce()) - for speed, accepted_can, rejected_can in ((15.0, 40, 70), (26.0, 8, 12)): - self.prepare(speed) - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, accepted_can / self.DEG_TO_CAN, 0))) - self.prepare(speed) - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, rejected_can / self.DEG_TO_CAN, 0))) - self.assertEqual(self.safety.get_desired_curvature_last(), 0) - # A follow-up from zero must pass without resetting previous desire. - # Retaining the rejected positive command would make this reversal fail. - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, -accepted_can / self.DEG_TO_CAN, 0))) - - def cruise(self, state): - return stock.TestFordStockSwitch.cruise(self, state) - - def physical_switch(self, pressed, bus=0): - return stock.TestFordStockSwitch.physical_switch(self, pressed, bus) - - def test_stock_resume_does_not_grant_steering(self): - for word in (32, 33): - self.select(word) - stock.TestFordStockSwitch.healthy(self, word) - self.assertFalse(self.safety.get_controls_allowed()) - self.assertEqual(word == 32, self._tx(self._acc_button_msg(ford.Buttons.RESUME, 0))) - self.assertFalse(self.safety.get_controls_allowed()) - self.assertTrue(self._rx(self.physical_switch(False))) - self.assertFalse(self._tx(self._acc_button_msg(ford.Buttons.RESUME, 0))) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_full_engagement.py b/opendbc_repo/opendbc/safety/tests/test_ford_full_engagement.py index 28ce7e3179..f68813a9f3 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_full_engagement.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_full_engagement.py @@ -5,7 +5,7 @@ import pytest from opendbc.car import structs from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent as _FordNative -WORDS = (8, 9, 10, 11, 12, 13, 18, 19, 32, 33, 66, 67) +WORDS = (0, 1, 2, 3, 8, 9, 10, 11, 12, 13) @pytest.fixture @@ -94,7 +94,7 @@ def test_full_engagement_never_bypasses_native_qualification(ford_native, word, def test_full_engagement_token_dies_on_source_health_heartbeat_or_permanent_eps(ford_native): n = ford_native for fault in ('health', 'heartbeat', 'eps'): - select(n, 32) + select(n, 0) n.pump(40, main=3) n.pump(10, main=4, mask=1) n.rx('EngBrakeData', {'BpedDrvAppl_D_Actl': 2, 'CcStat_D_Actl': 3}) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_generic_canfd_extended.py b/opendbc_repo/opendbc/safety/tests/test_ford_generic_canfd_extended.py deleted file mode 100644 index 43e136aaf3..0000000000 --- a/opendbc_repo/opendbc/safety/tests/test_ford_generic_canfd_extended.py +++ /dev/null @@ -1,89 +0,0 @@ -"""Generic CAN FD extension; helpers composed without inheriting other suites.""" -import unittest -from opendbc.car import structs -from opendbc.safety.tests import test_ford as ford -from opendbc.safety.tests.libsafety import libsafety_py - - -class TestFordGenericCanfdExtended(unittest.TestCase): - def setUp(self): - self.safety = libsafety_py.libsafety - self.safety.set_alternative_experience(0) - self.debug = self.safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) == 0 - self.fixture = ford.TestFordCANFDLongitudinalSafety() - self.fixture.SAFETY_PARAM = 66 - self.fixture.setUp() - - def __getattr__(self, name): - fixture = self.__dict__.get("fixture") - if fixture is not None: - return getattr(fixture, name) - raise AttributeError(name) - - def tearDown(self): - self.safety.set_alternative_experience(0) - self.safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) - - def select(self, word, ae=0): - self.safety.set_alternative_experience(ae) - self.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, word) - self.safety.init_tests() - self.safety.set_timer(0) - - def announce(self): - msg = self._lkas_command_msg(0) - msg[0].data[4] = 2 - return self._tx(msg) - - def prepare(self, speed=5.): - # Unit-limit setup only; actual caller packet proof uses physical RX instead. - self._reset_curvature_measurement(0., speed) - self._set_prev_desired_angle(0.) - self.safety.set_controls_allowed(True) - - def test_exact_namespace_and_release_long_support(self): - for word in (66,67): - self.select(word) - admitted = True - self.assertEqual(admitted, self.announce()) - self.prepare() - self.assertEqual(admitted, self._tx(self._lat_ctl_msg(True,0,0,0,.0002))) - self.assertEqual(word == 67, - self._tx(self._acc_command_msg(self.INACTIVE_GAS,self.INACTIVE_ACCEL,False))) - for word, ae in ((64,0),(65,0),(68,0),(70,0),(74,0),(82,0),(98,0),(194,0),(66,33),(67,33)): - self.select(word,ae) - self.assertFalse(self.announce()) - self.assertFalse(self._tx(self._lat_ctl_msg(False,0,0,0,0))) - for word in (66, 67): - self.select(word, 32) - self.assertTrue(self.announce()) - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, .0002, 0))) - - def test_announcement_path_and_inactive_contract(self): - self.prepare() - self.assertFalse(self._tx(self._lat_ctl_msg(True,0,0,0,.0002))) - self.assertTrue(self.announce()) - for rate in (-.001024,.001023): - self.prepare() - self.assertTrue(self._tx(self._lat_ctl_msg(True,0,0,0,rate))) - for offset,angle,curvature,rate in ((.01,0,0,0),(0,.01,0,0),(0,0,.001,0),(0,0,0,.0002)): - self.prepare() - self.assertTrue(self._tx(self._lat_ctl_msg(True,0,0,.0002,0))) - self.assertEqual(10,self.safety.get_desired_curvature_last()) - self.assertFalse(self._tx(self._lat_ctl_msg(False,offset,angle,curvature,rate))) - self.assertEqual(0,self.safety.get_desired_curvature_last()) - self.assertTrue(self._tx(self._lat_ctl_msg(False,0,0,0,0))) - - def test_modern_envelope_and_rejected_history(self): - self.assertTrue(self.announce()) - for speed,accept,reject in ((15.,40,70),(26.,8,12)): - self.prepare(speed) - self.assertTrue(self._tx(self._lat_ctl_msg(True,0,0,accept/self.DEG_TO_CAN,0))) - self.assertEqual(accept,self.safety.get_desired_curvature_last()) - # Keep the accepted previous command for the extension-only path rejection. - self.assertFalse(self._tx(self._lat_ctl_msg(True,0,.01,accept/self.DEG_TO_CAN,0))) - self.assertEqual(0,self.safety.get_desired_curvature_last()) - self.prepare(speed) - self.assertFalse(self._tx(self._lat_ctl_msg(True,0,0,reject/self.DEG_TO_CAN,0))) - self.assertEqual(0,self.safety.get_desired_curvature_last()) - self.assertTrue(self._tx(self._lat_ctl_msg(True,0,0,-accept/self.DEG_TO_CAN,0))) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py b/opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py deleted file mode 100644 index e64326d922..0000000000 --- a/opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py +++ /dev/null @@ -1,179 +0,0 @@ -"""Exact Mach-E extension intersects current Ford safety; variant-select methods.""" -import unittest - -from opendbc.safety.tests import test_ford as ford -from opendbc.car.structs import CarParams -from opendbc.safety.tests.libsafety import libsafety_py -from opendbc.safety.tests import test_ford_aol - - -class TestFordMachEExtended(unittest.TestCase): - SAFETY_PARAM = 18 - REQUIRES_DEBUG = None - MAX_CURVATURE_ERROR = 0.006 - - def setUp(self): - safety = libsafety_py.libsafety - debug = safety.set_safety_hooks(CarParams.SafetyModel.allOutput, 0) == 0 - safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0) - safety.set_alternative_experience(0) - if self.REQUIRES_DEBUG is not None and debug != self.REQUIRES_DEBUG: - self.skipTest("This case requires " + ("DEBUG" if self.REQUIRES_DEBUG else "RELEASE") + " safety") - self._message_fixture = ford.TestFordCANFDStockSafety() - self._message_fixture.SAFETY_PARAM = self.SAFETY_PARAM - self._message_fixture.setUp() - - def __getattr__(self, name): - fixture = self.__dict__.get("_message_fixture") - if fixture is None: - raise AttributeError(name) - return getattr(fixture, name) - - def announce(self, active=True, forbidden=False): - msg = self._lkas_command_msg(0) - msg[0].data[4] = (2 if active else 0) | int(forbidden) - return self._tx(msg) - - def prepare(self, speed=5.0, curvature=0.0195): - self._reset_curvature_measurement(curvature, speed) - self._set_prev_desired_angle(curvature) - self.safety.set_controls_allowed(True) - - def command(self, enabled=True, curvature=0.0195, rate=0.0, angle=0.02, offset=0.0, *, increment_timer=True): - return self._lat_ctl_msg(enabled, offset, angle, curvature, rate, increment_timer=increment_timer) - - def test_accepted_announcement_required_rejected_frame_cannot_grant(self): - self.prepare() - self.assertFalse(self._tx(self.command())) - self.assertFalse(self.announce(forbidden=True)) - self.prepare() - self.assertFalse(self._tx(self.command())) - self.assertTrue(self.announce()) - self.prepare() - self.assertTrue(self._tx(self.command())) - self.assertTrue(self.announce(active=False)) - self.prepare() - self.assertFalse(self._tx(self.command())) - - def test_original_path_recipe_and_inactive_fields(self): - self.assertTrue(self.announce()) - self.prepare() - self.assertTrue(self._tx(self.command())) - for kwargs in ({'angle': -0.02}, {'angle': 0.161}, {'angle': 0.08}, {'curvature': 0.0194}, {'offset': 0.01}): - with self.subTest(kwargs=kwargs): - self.prepare() - self.assertFalse(self._tx(self.command(**kwargs))) - self.prepare() - self.assertTrue(self._tx(self.command(enabled=False, curvature=0.0, angle=0.0))) - self.prepare() - self.assertFalse(self._tx(self.command(enabled=False, rate=0.000001, curvature=0.0, angle=0.0))) - - def test_modern_jerk_rejects_original_rate_counterexample(self): - self.assertTrue(self.announce()) - self.prepare(speed=15.0, curvature=0.0) - # Original15m/s lookup permits75CANunits; modernISOjerk permits46. - self.assertFalse(self._tx(self.command(curvature=0.0015, angle=0.0))) - self.prepare(speed=15.0, curvature=0.0) - self.assertTrue(self._tx(self.command(curvature=0.0009, angle=0.0))) - - def test_speed_combined_accel_and_authority_denials(self): - self.assertTrue(self.announce()) - for speed in (2.9, 8.8): - self.prepare(speed=speed) - self.assertFalse(self._tx(self.command())) - self.prepare() - self.assertTrue(self._tx(self.command(angle=0.05))) - self.prepare() - self.assertTrue(self._tx(self.command(angle=0.1))) - self.prepare(speed=8.7) - # 0.0195 * 8.7**2 + 0.12 * 8.7 > 2.5; path delta is only40CAN. - self.assertFalse(self._tx(self.command(angle=0.12))) - self.prepare() - self.safety.set_controls_allowed(False) - self.assertFalse(self._tx(self.command())) - - def test_rejected_path_does_not_commit_curvature_history(self): - self.assertTrue(self.announce()) - self.prepare() - self.assertFalse(self._tx(self.command(curvature=0.0175))) - self._reset_curvature_measurement(0.0, 5.0) - self.safety.set_controls_allowed(True) - # Reset history permits125CAN from zero; a leaked875CAN rejects this step. - self.assertTrue(self._tx(self.command(curvature=0.0025, angle=0.0))) - - def test_namespace_unknown_words_and_siblings_cannot_acquire(self): - for word, ae in ((2, 0), (3, 0), (10, 0), (11, 0), (22, 0), (26, 0), (50, 0), (18, 32)): - with self.subTest(word=word, ae=ae): - self.safety.set_alternative_experience(ae) - self.safety.set_safety_hooks(CarParams.SafetyModel.ford, word) - self.announce() - self.prepare() - self.assertFalse(self._tx(self.command())) - - def test_typed_aol_retains_curvature_without_path_angle(self): - for sign in (-1, 1): - with self.subTest(sign=sign): - self.setUp() - intent = test_ford_aol.TestFordAolDriverIntent() - intent.setUp() - intent.reset(self.SAFETY_PARAM) - intent.arm() - intent.pump(12, mask=1, speed=7.5, curvature=sign * .02) - self.assertTrue(self.safety.safety_config_valid()) - self.assertEqual(self.safety.aol_get_permission_mask(), 1) - self.assertFalse(self.safety.get_controls_allowed()) - self.assertTrue(self.announce()) - self._set_prev_desired_angle(sign * .02) - self.assertFalse(self._tx(self.command(curvature=sign * .02, angle=sign * .055, increment_timer=False))) - self._set_prev_desired_angle(sign * .02) - self.assertTrue(self._tx(self.command(curvature=sign * .02, angle=0., increment_timer=False))) - - def test_accelerator_assist_requires_retained_full_engagement(self): - for sign in (-1, 1): - with self.subTest(sign=sign): - self.setUp() - intent = test_ford_aol.TestFordAolDriverIntent() - intent.setUp() - intent.reset(self.SAFETY_PARAM) - intent.arm() - intent.pump(12, mask=1, speed=7.5, curvature=sign * .02) - self.assertTrue(self.safety.safety_config_valid()) - self.assertEqual(self.safety.aol_get_permission_mask(), 1) - self._set_prev_desired_angle(sign * .02) - self._rx(self._pcm_status_msg(True)) - self.assertTrue(self.announce()) - for gas in (0., 25., 0.): - self._rx(self._user_gas_msg(gas)) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertTrue(self._tx(self.command(curvature=sign * .02, angle=sign * .055, increment_timer=False))) - - # Cruise returning to available revokes full engagement, while typed AOL keeps the lateral axis. - intent.rx('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 3}) - self.assertFalse(self.safety.get_controls_allowed()) - self.assertEqual(self.safety.aol_get_permission_mask(), 1) - self.assertFalse(self._tx(self.command(curvature=sign * .02, angle=sign * .055, increment_timer=False))) - self._set_prev_desired_angle(sign * .02) - self.assertTrue(self._tx(self.command(curvature=sign * .02, angle=0., increment_timer=False))) - - self._rx(self._pcm_status_msg(True)) - self._rx(self._user_brake_msg(True)) - self.assertFalse(self.safety.get_controls_allowed()) - self.assertEqual(self.safety.aol_get_permission_mask(), 1) - self.assertFalse(self._tx(self.command(curvature=sign * .02, angle=sign * .055, increment_timer=False))) - self._set_prev_desired_angle(sign * .02) - self.assertTrue(self._tx(self.command(curvature=sign * .02, angle=0., increment_timer=False))) - - -class TestFordMachEDebugLongExtended(TestFordMachEExtended): - SAFETY_PARAM = 19 - REQUIRES_DEBUG = True - - -class TestFordMachELongReleaseExtended(TestFordMachEExtended): - SAFETY_PARAM = 19 - REQUIRES_DEBUG = False - - -# Compatibility for existing CI selector names: this runs the promoted profile's -# complete positive RELEASE contract, including the unchanged actuation bounds. -TestFordMachELongReleaseDenied = TestFordMachELongReleaseExtended diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_neutral_display.py b/opendbc_repo/opendbc/safety/tests/test_ford_neutral_display.py index ee7994695b..a69c862d9c 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_neutral_display.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_neutral_display.py @@ -2,13 +2,13 @@ import pytest from opendbc.car import gen_empty_fingerprint -from opendbc.car.ford import mache_can +from opendbc.car.ford import fordcan from opendbc.car.ford.fordcan import CanBus from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests import test_ford_aol -@pytest.fixture(params=(19, 67, 275, 323)) +@pytest.fixture(params=(3, 259)) def owner(request): native = test_ford_aol.TestFordAolDriverIntent() native.setUp() @@ -17,14 +17,15 @@ def owner(request): native.safety.set_alternative_experience(32) native.pump(40, main=0, mask=0, heartbeat=False) bus = CanBus(fingerprint=gen_empty_fingerprint()) - address, data, src = mache_can.create_lka_msg(native.packer, bus) + address, data, src = fordcan.create_lka_msg(native.packer, bus) assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) - return native + yield native + native.tearDown() # AE 32 must not reach the next test's profile def inactive(owner): bus = CanBus(fingerprint=gen_empty_fingerprint()) - address, data, src = mache_can.create_lat_ctl2_msg(owner.packer, bus, 0, 0, 1, 0., 0., 0) + address, data, src = fordcan.create_lat_ctl2_msg(owner.packer, bus, 0, 0., 0., 0., 0., 0) assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) @@ -38,7 +39,7 @@ def acquire(owner): assert ui(owner, 0x3D8) assert owner.safety.safety_fwd_hook(2, 0x18A) == -1 assert owner.safety.safety_fwd_hook(2, 0x3D8) == -1 - assert owner.safety.safety_fwd_hook(2, 0x3D6) == 0 + assert owner.safety.safety_fwd_hook(2, 0x3D6) == -1 # the accepted inactive actuator replaces the camera's copy assert owner.safety.aol_get_permission_mask() == 0 assert not owner.safety.get_controls_allowed() @@ -67,11 +68,11 @@ def test_main_off_continuity_has_no_steering_or_longitudinal_authority(owner): assert ui(owner, 0x18A) assert owner.safety.safety_fwd_hook(2, 0x18A) == -1 assert owner.safety.safety_fwd_hook(2, 0x3D8) == -1 - assert owner.safety.safety_fwd_hook(2, 0x3D6) == 0 + assert owner.safety.safety_fwd_hook(2, 0x3D6) == -1 assert owner.safety.aol_get_permission_mask() == 0 assert not owner.safety.get_controls_allowed() bus = CanBus(fingerprint=gen_empty_fingerprint()) - address, data, src = mache_can.create_lat_ctl2_msg(owner.packer, bus, 1, 0, 1, 0., 0., 1) + address, data, src = fordcan.create_lat_ctl2_msg(owner.packer, bus, 1, 0., 0., 0., 0., 1) assert not owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) assert owner.safety.safety_fwd_hook(2, 0x18A) == 0 @@ -139,7 +140,7 @@ def test_neutral_continuity_across_real_microsecond_timer_wrap(owner): owner.safety.set_alternative_experience(32) owner.pump(40, main=0, mask=0, heartbeat=False) bus = CanBus(fingerprint=gen_empty_fingerprint()) - address, data, src = mache_can.create_lka_msg(owner.packer, bus) + address, data, src = fordcan.create_lka_msg(owner.packer, bus) assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) acquire(owner) for tick in range(40): @@ -155,9 +156,9 @@ def test_neutral_continuity_across_real_microsecond_timer_wrap(owner): def active_then_off(owner, full=False): owner.arm() bus = CanBus(fingerprint=gen_empty_fingerprint()) - address, data, src = mache_can.create_lka_msg(owner.packer, bus) + address, data, src = fordcan.create_lka_msg(owner.packer, bus) assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) - address, data, src = mache_can.create_lat_ctl2_msg(owner.packer, bus, 1, 0, 1, 0., 0., 0) + address, data, src = fordcan.create_lat_ctl2_msg(owner.packer, bus, 1, 0., 0., 0., 0., 0) assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, src, data)) assert owner.safety.aol_get_permission_mask() == 1 if full: @@ -177,7 +178,7 @@ def test_off_display_precedes_next_inactive_actuator_without_stock_warning_gap(o assert owner.safety.safety_fwd_hook(2, 0x3D8) == 0 assert ui(owner, 0x3D8) assert owner.safety.safety_fwd_hook(2, 0x3D8) == -1 - assert owner.safety.safety_fwd_hook(2, 0x3D6) == 0 + assert owner.safety.safety_fwd_hook(2, 0x3D6) == -1 # the host is still sending the actuator assert owner.safety.aol_get_permission_mask() == 0 owner.pump(4, main=3, mask=0, heartbeat=False) inactive(owner) diff --git a/opendbc_repo/opendbc/safety/tests/test_ford_stock_switch.py b/opendbc_repo/opendbc/safety/tests/test_ford_stock_switch.py index 326bf33a7e..d80241181b 100644 --- a/opendbc_repo/opendbc/safety/tests/test_ford_stock_switch.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford_stock_switch.py @@ -17,6 +17,9 @@ class TestFordStockSwitch(unittest.TestCase): self._message_fixture.SAFETY_PARAM = self.SAFETY_PARAM self._message_fixture.setUp() + def tearDown(self): + libsafety_py.libsafety.set_alternative_experience(0) + def __getattr__(self, name): fixture = self.__dict__.get("_message_fixture") if fixture is None: @@ -46,7 +49,7 @@ class TestFordStockSwitch(unittest.TestCase): self.assertTrue(self._rx(self.physical_switch(button))) def test_physical_available_inactive_resume_does_not_enable_controls(self): - for word in (2, 8, 10, 12, 18): + for word in (0, 2, 8, 10, 12): with self.subTest(word=word): self.select(word) self.healthy(word) @@ -83,7 +86,7 @@ class TestFordStockSwitch(unittest.TestCase): self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, 0))) def test_long_unknown_ae_and_unreached_words_cannot_use_exception(self): - for word, ae in ((0, 0), (4, 0), (1, 0), (3, 0), (9, 0), (11, 0), (13, 0), (19, 0), (34, 0), (2, 32)): + for word, ae in ((4, 0), (1, 0), (3, 0), (9, 0), (11, 0), (13, 0), (34, 0), (4, 32)): with self.subTest(word=word, ae=ae): self.select(word, ae) self.healthy(word) diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index c0009f53e1..c5b1fdc575 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -43,13 +43,26 @@ inline static std::unordered_map keys = { {"HondaBoschARadar", {PERSISTENT, BOOL, "1"}}, {"HondaGasFactorParams", {PERSISTENT, FLOAT, "1.0"}}, {"HondaWindFactorParams", {PERSISTENT, FLOAT, "1.0"}}, - {"FordPrefSteerAngleCurvature", {PERSISTENT, BOOL, "0"}}, - {"FordLateralMode", {PERSISTENT, INT, "0"}}, {"FordHandsFreeCluster", {PERSISTENT, BOOL, "0"}}, - {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1"}}, - {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4"}}, - {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4"}}, - {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85"}}, + {"disable_BP_lat_UI", {PERSISTENT, BOOL, "1"}}, + {"enable_human_turn_detection_curv", {PERSISTENT, BOOL, "1"}}, + {"enable_lane_positioning_curv", {PERSISTENT, BOOL, "0"}}, + {"enable_lane_full_mode_curv", {PERSISTENT, BOOL, "0"}}, + {"enable_lane_positioning_ang", {PERSISTENT, BOOL, "0"}}, + {"FordPrefLateralControl", {PERSISTENT, INT, "0"}}, + {"custom_profile_curv", {PERSISTENT, INT, "0"}}, + {"lane_change_factor_high_curv", {PERSISTENT, FLOAT, "0.85"}}, + {"pc_blend_ratio_high_C_UI_curv", {PERSISTENT, FLOAT, "0.4"}}, + {"pc_blend_ratio_low_C_UI_curv", {PERSISTENT, FLOAT, "0.4"}}, + {"custom_path_offset_curv", {PERSISTENT, FLOAT, "0.0"}}, + {"LC_PID_gain_UI_curv", {PERSISTENT, FLOAT, "1.0"}}, + {"FordLowSpeedFactor_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"FordHighSpeedFactor_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"FordHighSpeedDampening_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"lane_change_factor_high_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"custom_path_offset_ang", {PERSISTENT, FLOAT, "0.0"}}, + {"lane_centering_strength_ang", {PERSISTENT, FLOAT, "0.25"}}, + {"FordPrefSteerAngleCurvature", {PERSISTENT, BOOL, "0"}}, {"OnroadCustomizations", {PERSISTENT | DONT_LOG, JSON}}, {"StarPilotFavoriteSlots", {PERSISTENT, JSON, R"([{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""}])"}}, {"StarPilotControllerBindings", {PERSISTENT | DONT_LOG, JSON}}, diff --git a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc index 5be4e071de..db7decd639 100644 --- a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc +++ b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc @@ -249,22 +249,12 @@ int main() { assert(aol_capable(preap, 39U, VEHICLE_REGISTRY) == preap_expected); assert(!aol_capable(preap, 35U, VEHICLE_REGISTRY)); } - constexpr uint16_t ford_words[] = {8U, 9U, 10U, 11U, 12U, 13U, 18U, 19U, 32U, 33U, 66U, 67U, 146U, 147U, 160U, 161U, 194U, 195U}; + constexpr uint16_t ford_words[] = {0U, 1U, 2U, 3U, 8U, 9U, 10U, 11U, 12U, 13U}; for (uint32_t word = 0U; word <= 65535U; ++word) { bool expected = false; - for (const auto exact : ford_words) expected |= (word == exact) || (word == (exact | 256U)); - // Exact opt-in geometry families, independently enumerated from the native decoder. - for (uint16_t geometry = 1U; geometry <= 12U; geometry++) { - const bool classic = (geometry == 1U) || (geometry == 2U) || (geometry == 5U) || (geometry == 6U) || (geometry == 9U); - const uint16_t curvature = classic ? 32U : ((geometry == 11U) ? 18U : ((geometry == 10U) ? 10U : 66U)); - for (uint16_t longitudinal = 0U; longitudinal <= 1U; longitudinal++) { - for (uint16_t angle = 0U; angle <= 1U; angle++) { - if ((geometry == 10U) && angle) continue; - const uint16_t exact = 512U | (geometry << 10U) | curvature | longitudinal | (angle ? 128U : 0U); - expected |= (word == exact) || (word == (exact | 256U)); - } - } - } + // Bits 9-14 carry the BluePilot lateral library's startup bits, whatever they hold. + const uint32_t profile = word & ~0x7E00U; + for (const auto exact : ford_words) expected |= (profile == exact) || (profile == (exact | 256U)); auto ford = status(); ford.safety_mode = 6U; ford.safety_param = static_cast(word); diff --git a/openpilot/selfdrive/pandad/tests/test_panda_owner.cc b/openpilot/selfdrive/pandad/tests/test_panda_owner.cc index 8248bd03f2..961155987e 100644 --- a/openpilot/selfdrive/pandad/tests/test_panda_owner.cc +++ b/openpilot/selfdrive/pandad/tests/test_panda_owner.cc @@ -145,7 +145,7 @@ void test_actual_config_slots_and_owner_handle(cereal::CarParams::SafetyModel mo void test_registered_owners() { test_actual_config_slots_and_owner_handle(cereal::CarParams::SafetyModel::HYUNDAI_CANFD, 0x809); - for (uint16_t word : {8U, 9U, 10U, 11U, 12U, 13U, 18U, 19U, 32U, 33U, 66U, 67U}) { + for (uint16_t word : {0U, 1U, 2U, 3U, 8U, 9U, 10U, 11U, 12U, 13U}) { test_actual_config_slots_and_owner_handle(cereal::CarParams::SafetyModel::FORD, word); test_actual_config_slots_and_owner_handle(cereal::CarParams::SafetyModel::FORD, word | 256U); } diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index e7ebb38691..a6ad9b073e 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -817,11 +817,11 @@ class SelfdriveD: return CS def ford_source_deadline_supported(self): - from opendbc.car.ford.aol import qualified, base_safety_param + from opendbc.car.ford.aol import profile_word, qualified from opendbc.car.ford.values import CAR return (qualified(self.CP, marked_only=True) and self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and - len(self.CP.safetyConfigs) == 1 and base_safety_param(self.CP) == 19) + len(self.CP.safetyConfigs) == 1 and profile_word(self.CP.safetyConfigs[0].safetyParam) == 3) def source_deadline_deferral_supported(self): from openpilot.starpilot.car.hyundai.aol import source_deadline_supported @@ -1785,13 +1785,14 @@ class SelfdriveD: """ from openpilot.starpilot.aol.transport_pause import independent_lateral_health from opendbc.car.ford.values import CAR as FORD_CAR + from opendbc.car.ford.aol import profile_word from openpilot.starpilot.car.ford.aol import native_accepts_cp as ford_native_accepts_cp # Established ordinary Mach-E cruise does not require an independent AOL latch. # Its native LONG pedal withdrawal may precede Card; the original combined # publication, live LAT grant and every health/deadline check below still apply. - ordinary_ford = bool(self.CP.carFingerprint == FORD_CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.safetyConfigs and - ford_native_accepts_cp(self.CP, int(car.CarParams.SafetyModel.ford), - self.CP.safetyConfigs[-1].safetyParam)) + ford_word = self.CP.safetyConfigs[-1].safetyParam + ordinary_ford = bool(self.CP.carFingerprint == FORD_CAR.FORD_MUSTANG_MACH_E_MK1 and profile_word(ford_word) == 3 and + ford_native_accepts_cp(self.CP, int(car.CarParams.SafetyModel.ford), ford_word)) proof = getattr(self, 'aol_published_input_proof', None) prior = getattr(self, 'aol_published_input_native', None) anchor = getattr(self, 'aol_ordinary_source_anchor', None) @@ -1839,9 +1840,8 @@ class SelfdriveD: # Ioniq6 also checks the physical brake before its separately published host sample. # Retain only its already granted LAT; all proof, health and LONG withdrawal gates remain. # The exact vehicle policies and all proof/health checks above remain required. - from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long return bool(ordinary_ford or gm_native_accepts_cp(self.CP, native.safetyModel, native.safetyParam) or - early_native_long_withdrawal_supported(self.CP) or ford_early_long(self.CP) or + early_native_long_withdrawal_supported(self.CP) or toyota_aol_qualified(self.CP, marked_only=True) or not native.pandaInventory[owners[0]].controlsAllowed and not self.sm['pandaStates'][owners[0]].controlsAllowed) diff --git a/openpilot/starpilot/aol/tests/test_ford_ordinary_long_withdrawal.py b/openpilot/starpilot/aol/tests/test_ford_ordinary_long_withdrawal.py index 7473e9de8b..865dbef72c 100644 --- a/openpilot/starpilot/aol/tests/test_ford_ordinary_long_withdrawal.py +++ b/openpilot/starpilot/aol/tests/test_ford_ordinary_long_withdrawal.py @@ -8,8 +8,8 @@ from opendbc.car.ford.interface import CarInterface from opendbc.car.ford.values import CAR -@pytest.fixture(params=[(False, False), (True, False), (False, True), (True, True)]) -def caller(request): +@pytest.fixture +def caller(): case = fixture.IoniqAvailabilityTests(methodName='runTest') with ExitStack() as stack: stack.callback(case.doCleanups) @@ -19,11 +19,6 @@ def caller(request): cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], True, False, False) cp.alternativeExperience = 32 assert cp.safetyConfigs[-1].safetyParam == 19 and cp.flags == 3 - angle, pinion = request.param - cp.safetyConfigs[0].safetyParam |= 128 if angle else 0 - if pinion: - from opendbc.car.ford.pinion import select - assert select(cp, True) sd, controls, step = case._authority_callers(True, cp=cp, genuine_events=True) cs = fixture.car_state() cs.cruiseState.enabled = True diff --git a/openpilot/starpilot/aol/tests/test_ford_source_deadline.py b/openpilot/starpilot/aol/tests/test_ford_source_deadline.py index 5a8042dfbe..a4f52baf07 100644 --- a/openpilot/starpilot/aol/tests/test_ford_source_deadline.py +++ b/openpilot/starpilot/aol/tests/test_ford_source_deadline.py @@ -15,21 +15,17 @@ SOURCE=10_020_000_000 PUBLISHED_OFFSET=5_920_000 SOURCE_AGE=33_452_000 -def profile(angle=False, pinion=False): +def profile(): fingerprint = gen_empty_fingerprint() fingerprint[0].update({0x5A: 8, 0x3A6: 8, 0x3A7: 8}) fingerprint[2].update({0x3D6: 8, 0x186: 8}) cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], True, False, False) cp.alternativeExperience = 32 assert cp.flags == 3 and cp.safetyConfigs[0].safetyParam == 19 - cp.safetyConfigs[0].safetyParam |= 128 if angle else 0 - if pinion: - from opendbc.car.ford.pinion import select - assert select(cp, True) return cp -def callers(case, angle=False, pinion=False): - sd, controls, drive = case._authority_callers(False, cp=profile(angle, pinion), genuine_events=True) +def callers(case): + sd, controls, drive = case._authority_callers(False, cp=profile(), genuine_events=True) def stationary(tick, **kwargs): state = kwargs.get('observed_state') if state is None: @@ -41,15 +37,14 @@ def callers(case, angle=False, pinion=False): return sd, controls, stationary @pytest.mark.parametrize('pedal', ['brake', 'gas']) -@pytest.mark.parametrize('angle,pinion', [(False, False), (True, False), (False, True), (True, True)]) @pytest.mark.parametrize('socket_timeout', [False, True]) @pytest.mark.parametrize('published_offset,source_age', [(PUBLISHED_OFFSET, SOURCE_AGE)]) -def test_ford_held_brake_keeps_only_the_recorded_unexpired_proof(socket_timeout, published_offset, source_age, angle, pinion, pedal): +def test_ford_held_brake_keeps_only_the_recorded_unexpired_proof(socket_timeout, published_offset, source_age, pedal): case = fixture.IoniqAvailabilityTests(methodName='runTest') with ExitStack() as cleanup: cleanup.callback(case.doCleanups) cleanup.enter_context(OpenpilotPrefix()) - sd, controls, drive = callers(case, angle, pinion) + sd, controls, drive = callers(case) for tick in (0, 1): stamp = 10_000_000_000 + tick * 10_000_000 drive(tick, brake=pedal == 'brake', gas=pedal == 'gas', long_allowed=False, **settings('lateral_only', stamp, tick + 1)) diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 91b1e101b2..2141923c26 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -304,9 +304,8 @@ class TransportPauseFeedback: from opendbc.car import structs from opendbc.car.ford.values import CAR from opendbc.car.ford.aol import qualified as ford_qualified - from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported - first_long = early_native_long_withdrawal_supported(cp) or (ford_qualified(cp, marked_only=True) and - cp.openpilotLongitudinalControl and cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1) + first_long = (ford_qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and + cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1) pending_ack = str(axis.mode) == 'off' and not axis.lateralActive if (not first_long or not latched or self.failed or self.paused or self.pending_event or self.retention_started_ns or self.source_retention_started_ns or @@ -343,7 +342,6 @@ class TransportPauseFeedback: from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified from opendbc.car import structs ev9 = cp.brand == 'hyundai' and ev9_qualified(cp, marked_only=True) - from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long from opendbc.car.ford.aol import qualified as ford_qualified from opendbc.car.ford.values import CAR # Before 10Hz LONG echo: physical pedal/cancel retains only real independent LAT intent. @@ -356,7 +354,7 @@ class TransportPauseFeedback: # Fresh LAT grant; all health/fault checks required. ford_mache_long = bool(ford_qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1) - early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9 or ford_early_long(cp) or ford_withdrawal or ford_mache_long + early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9 or ford_withdrawal or ford_mache_long from openpilot.starpilot.car.gm.aol import f1_conventional_lateral_owner, gm_physical_pedal_withdrawal gm_pedal = gm_physical_pedal_withdrawal(cp, cs) and not f1_conventional_lateral_owner(cp) event_ns = int(sm.logMonoTime['onroadEvents']) diff --git a/openpilot/starpilot/car/ford/aol.py b/openpilot/starpilot/car/ford/aol.py index 34165220d9..2bb754fff2 100644 --- a/openpilot/starpilot/car/ford/aol.py +++ b/openpilot/starpilot/car/ford/aol.py @@ -1,6 +1,6 @@ from dataclasses import replace from opendbc.car import structs -from opendbc.car.ford.aol import qualified, AOL_WORDS, MANUAL, temporary_restriction, aol_safety_config, base_safety_param, ANGLE +from opendbc.car.ford.aol import qualified, AOL_WORDS, MANUAL, temporary_restriction, aol_safety_config, profile_word from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE from openpilot.starpilot.aol.policy import AolVehiclePolicy @@ -10,26 +10,18 @@ def policy_for(cp): return AolVehiclePolicy(intent_supported=True, settings_supported=True, runtime_supported=True, normal_runtime_supported=True, ordinary_axis_ack_required=qualified(cp, marked_only=True), explicit_latch=True, alternative_experience_addition=32, - safety_param_addition=(MANUAL if cp.transmissionType == structs.CarParams.TransmissionType.manual else 0) | - (base_safety_param(cp) if aol_safety_config(cp).safetyParam & ANGLE else 0)) + safety_param_addition=MANUAL if cp.transmissionType == structs.CarParams.TransmissionType.manual else 0) return AolVehiclePolicy() def native_profile_supported(model, param): - return model == int(structs.CarParams.SafetyModel.ford) and param in AOL_WORDS + return model == int(structs.CarParams.SafetyModel.ford) and profile_word(param) in AOL_WORDS def native_accepts_cp(cp, model, param): return qualified(cp, marked_only=True) and native_profile_supported(model, param) and param == aol_safety_config(cp).safetyParam -def early_native_long_withdrawal_supported(cp): - # Panda can observe a pedal before the parsed host sample. The existing - # independent-LAT proof remains mandatory; this never authorizes LONG. - return bool(qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and - aol_safety_config(cp).safetyParam & ANGLE) - - def native_latch_rejected(cp, native): return qualified(cp, marked_only=True) and native.requestedLateral and not native.lateralAllowed diff --git a/openpilot/starpilot/car/ford/aol_policy.h b/openpilot/starpilot/car/ford/aol_policy.h index 5ac9947947..1928f88328 100644 --- a/openpilot/starpilot/car/ford/aol_policy.h +++ b/openpilot/starpilot/car/ford/aol_policy.h @@ -1,14 +1,13 @@ #pragma once #include "openpilot/selfdrive/pandad/aol_protocol.h" -#include "opendbc/safety/modes/ford_pinion.h" +#include "opendbc/safety/modes/ford_param.h" static inline bool ford_aol_param(uint16_t param) { bool result = false; - switch (ford_pinion_base_word(param) & ~256U) { + switch (ford_profile_word(param) & ~256U) { + case 0U: case 1U: case 2U: case 3U: case 8U: case 9U: case 10U: case 11U: case 12U: case 13U: - case 18U: case 19U: case 32U: case 33U: case 66U: case 67U: - case 146U: case 147U: case 160U: case 161U: case 194U: case 195U: result = true; break; default: diff --git a/openpilot/starpilot/car/ford/bluepilot.py b/openpilot/starpilot/car/ford/bluepilot.py new file mode 100644 index 0000000000..eac8d82c0e --- /dev/null +++ b/openpilot/starpilot/car/ford/bluepilot.py @@ -0,0 +1,84 @@ +"""BluePilot lateral: StarPilot's startup settings and the per-frame inputs its Ford CarController reads +(opendbc bluepilot_lateral/hosts/starpilot).""" +import openpilot.cereal.messaging as messaging +from openpilot.common.swaglog import cloudlog +from openpilot.starpilot.saved_source import read_saved + +from opendbc.bluepilot_lateral.core.values_ext import BP_LATERAL_PARAMS +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import FrameInputs + +STARTUP_KEYS = ("disable_BP_lat_UI", "FordPrefSteerAngleCurvature") +SETTINGS_REFRESH_FRAMES = 300 # 3 s of card's 100 Hz loop + + +def startup_params(params, *, enabled: bool) -> dict[str, str]: + """The startup settings init_ford_safety_param_starpilot reads. BluePilot lateral stays off without the openpilot + toggle, in Safe Mode, or when a setting can't be read; an unset key gets its params_keys.h default.""" + safe, safe_readable = read_saved(params, "SafeMode", 8) + if not enabled or not safe_readable or safe not in (None, b"0"): + return {"disable_BP_lat_UI": "1"} + settings = {} + for key in STARTUP_KEYS: + raw, readable = read_saved(params, key, 8) + if not readable: + return {"disable_BP_lat_UI": "1"} + settings[key] = raw.decode() if raw is not None else ("1" if params.get_default_value(key) else "0") + return settings + + +def encode(value) -> bytes: + return (b"1" if value else b"0") if isinstance(value, bool) else str(value).encode() + + +class BluePilotInputs: + """Card-owned: this frame's model and vehicle-parameter inputs, and the per-frame settings (BP_LATERAL_PARAMS).""" + + def __init__(self, params): + self.params = params + self.sm = messaging.SubMaster(["modelV2", "vehicleParameters", "lateralDelay"]) + self.frames = 0 + self.settings = [] + self.unknown_keys = set() + + def read_settings(self): + entries = [] + for key in BP_LATERAL_PARAMS: + try: + value = self.params.get(key, return_default=True) + except Exception: + if key not in self.unknown_keys: + cloudlog.warning(f"BluePilot lateral param {key} not registered in params_keys.h; skipping") + self.unknown_keys.add(key) + continue + if value is not None: + entries.append(FrameInputs.Param(key, encode(value))) + return entries + + def update(self) -> FrameInputs: + self.sm.update(0) + if self.frames % SETTINGS_REFRESH_FRAMES == 0: + self.settings = self.read_settings() + self.frames += 1 + + frame = FrameInputs(params=self.settings) + li, md, vp = frame.lateralInputs, self.sm["modelV2"], self.sm["vehicleParameters"] + li.valid = bool(self.sm.seen["modelV2"] and self.sm.seen["vehicleParameters"] and + len(md.laneLines) >= 3 and len(md.laneLineProbs) >= 3 and len(md.laneLineStds) >= 3) + if li.valid: + li.pathYawRate = list(md.orientationRate.z) + li.pathX = list(md.position.x) + li.pathY = list(md.position.y) + li.laneLineLeftX = list(md.laneLines[1].x) + li.laneLineLeftY = list(md.laneLines[1].y) + li.laneLineRightX = list(md.laneLines[2].x) + li.laneLineRightY = list(md.laneLines[2].y) + li.laneLineProbs = list(md.laneLineProbs) + li.laneLineStds = list(md.laneLineStds) + li.laneChangeState = int(md.meta.laneChangeState.raw) + li.laneChangeDirection = int(md.meta.laneChangeDirection.raw) + li.steerRatio = vp.steerRatio + li.stiffnessFactor = vp.stiffnessFactor + li.roll = vp.roll + li.angleOffsetDeg = vp.angleOffsetDeg + li.lateralDelay = self.sm["lateralDelay"].lateralDelay + return frame diff --git a/openpilot/starpilot/car/ford/cluster_preferences.py b/openpilot/starpilot/car/ford/cluster_preferences.py index 042e66e165..d59163f54e 100644 --- a/openpilot/starpilot/car/ford/cluster_preferences.py +++ b/openpilot/starpilot/car/ford/cluster_preferences.py @@ -6,14 +6,10 @@ CHOICES = {0: "Off", 1: "On"} def qualified(cp): - """Dom's CAN-FD/non-LKA scope, constrained to existing admitted owners.""" - from opendbc.car.ford.values import FordFlags - from opendbc.car.ford.mache_lateral import qualified as mache - from opendbc.car.ford.generic_canfd_lateral import qualified as generic - from opendbc.car.ford.new_port_curvature import qualified as new_port - from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle + """Dom's CAN-FD/non-LKA scope: the CAN FD Fords and the Mondeo.""" + from opendbc.car.ford.values import CAR, FORD_CANFD_CARS, FordFlags return bool(cp.brand == "ford" and cp.flags & FordFlags.CANFD and not cp.flags & FordFlags.LKA_STEERING and - (mache(cp) or generic(cp) or new_port(cp) or angle(cp))) + not cp.dashcamOnly and not cp.passive and cp.carFingerprint in FORD_CANFD_CARS | {CAR.FORD_MONDEO_MK5}) def read_choice(params): diff --git a/openpilot/starpilot/car/ford/lateral_preferences.py b/openpilot/starpilot/car/ford/lateral_preferences.py deleted file mode 100644 index 32e115f9c6..0000000000 --- a/openpilot/starpilot/car/ford/lateral_preferences.py +++ /dev/null @@ -1,19 +0,0 @@ -"""Saved Ford controller selection; no onroad CP mutation or saved-byte repair.""" -from openpilot.starpilot.saved_source import read_saved - -KEY = "FordLateralMode" -CHOICES = {0: "Current curvature", 1: "BluePilot angle"} - - -def read_choice(params): - raw, readable = read_saved(params, KEY, 8) - valid = readable and raw in (None, b"0", b"1") - return (1 if valid and raw == b"1" else 0), raw, readable, valid - - -def selected(params, *, enabled): - value, _, readable, valid = read_choice(params) - safe, safe_ok = read_saved(params, "SafeMode", 8) - aol, aol_ok = read_saved(params, "AlwaysOnLateral", 8) - return int(enabled and readable and valid and value == 1 and safe_ok and safe in (None, b"0") and - aol_ok and aol in (None, b"0", b"1")) diff --git a/openpilot/starpilot/car/ford/longitudinal_preferences.py b/openpilot/starpilot/car/ford/longitudinal_preferences.py index 77708ea814..d73c8f4b72 100644 --- a/openpilot/starpilot/car/ford/longitudinal_preferences.py +++ b/openpilot/starpilot/car/ford/longitudinal_preferences.py @@ -7,9 +7,6 @@ def prepare_release_long(cp, params, *, is_release, enabled): if not is_release or not enabled or cp.brand != 'ford': return cp try: - owner = aol_safety_config(cp) - if owner is not None and owner.safetyParam & 128 and read_saved(params, 'AlwaysOnLateral', 8) != (b'1', True): - return cp if (cp.alternativeExperience != 0 or not qualified(cp) or not cp.alphaLongitudinalAvailable or cp.openpilotLongitudinalControl or read_saved(params, 'AlphaLongitudinalEnabled', 8) != (b'1', True) or read_saved(params, 'SafeMode', 8) not in ((None, True), (b'0', True))): diff --git a/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py b/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py index d7d138d555..fd0ae60acd 100644 --- a/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py +++ b/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py @@ -414,7 +414,7 @@ def test_recorded_ford_stopped_geometry_does_not_release_native_stop(request): endpoint = TestFordAolDriverIntent(methodName='runTest') endpoint.setUp() request.addfinalizer(endpoint.tearDown) - endpoint.reset(19) + endpoint.reset(3) endpoint.pump(40, main=0) endpoint.pump(10, main=3) endpoint.pump(10, main=5, mask=3) diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index 7ddd5d0699..f6091cf257 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -1,149 +1,10 @@ -import math import time -import numpy as np - import openpilot.cereal.messaging as messaging -from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.swaglog import cloudlog from openpilot.starpilot.controllers.toyota_cruise import ToyotaCruisePreference, capability as toyota_cruise_capability -from opendbc.car.ford.values import CAR, FordFlags -from opendbc.car.ford.pinion import configured as pinion_configured - - -class ManualTurnInputs: - def __init__(self, params, *, track_assist_permission: bool = False, pinion: bool = False): - self.params = params - self.pinion = pinion - self.vehicle_parameters = None - self.track_assist_permission = track_assist_permission - services = ["modelV2", "lateralDelay"] + (["pandaStates", "aolSafetyWire"] if track_assist_permission else []) - self.sm = messaging.SubMaster(services + (["vehicleParameters"] if pinion else [])) - self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True)) - self.frames = 0 - self.blend_settings = self._read_blend_settings() - self._applied_blends = {} - - def _read_blend_settings(self): - settings = [] - for key, default, low, high in (("FordCurvatureBlendLow", 0.4, 0.0, 1.0), - ("FordCurvatureBlendHigh", 0.4, 0.0, 1.0), - ("FordCurvatureLaneChangeFactor", 0.85, 0.5, 1.25)): - try: - value = float(self.params.get(key, return_default=True)) - if not math.isfinite(value): - value = default - except (TypeError, ValueError, OverflowError, RuntimeError): - value = default - settings.append(float(np.clip(value, low, high))) - return tuple(settings) - - def apply_blend_settings(self, controller): - for name in ("mache_lateral", "classic_lateral", "new_port_curvature"): - owner = getattr(controller, name, None) - if owner is not None and self.pinion: - owner.measurement.update_parameters(self.vehicle_parameters) - if owner is not None and self._applied_blends.get(owner) != self.blend_settings: - owner.set_blend_settings(*self.blend_settings) - self._applied_blends[owner] = self.blend_settings - - def update(self): - self.sm.update(0) - if self.pinion and self.sm.valid["vehicleParameters"] and self.sm.alive["vehicleParameters"]: - self.vehicle_parameters = self.sm["vehicleParameters"] - if self.frames % 100 == 0: - self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True)) - self.blend_settings = self._read_blend_settings() - self.frames += 1 - state = self.sm["modelV2"].meta.laneChangeState - model_ready = self.sm.alive["modelV2"] and self.sm.valid["modelV2"] - return self.enabled, int(getattr(state, "raw", state)) in (1, 2, 3), model_ready - - - def preview_curvature(self, speed: float) -> float | None: - try: - now = time.monotonic_ns() - stamp = self.sm.logMonoTime["modelV2"] - if (not self.sm.alive["modelV2"] or not self.sm.valid["modelV2"] or - not 0 < stamp <= now <= stamp + 100_000_000 or not math.isfinite(speed) or speed < 0.1): - return None - yaw = tuple(self.sm["modelV2"].orientationRate.z) - if len(yaw) != len(ModelConstants.T_IDXS) or not all(math.isfinite(value) for value in yaw): - return None - delay = 0.2 - delay_stamp = self.sm.logMonoTime["lateralDelay"] - if (self.sm.alive["lateralDelay"] and self.sm.valid["lateralDelay"] and - 0 < delay_stamp <= now <= delay_stamp + 1_000_000_000): - measured_delay = float(self.sm["lateralDelay"].lateralDelay) - if math.isfinite(measured_delay): - delay = float(np.clip(measured_delay, 0.2, 0.4)) - return float(np.interp(delay, ModelConstants.T_IDXS, yaw)) / speed - except (AttributeError, KeyError, TypeError, ValueError, OverflowError): - return None - - def lateral_snapshot(self, speed: float, *, angle: bool = False): - if self.preview_curvature(speed) is None: - return None - delay = 0.15 if angle else 0.2 - now = time.monotonic_ns() - stamp = self.sm.logMonoTime["lateralDelay"] - if (self.sm.alive["lateralDelay"] and self.sm.valid["lateralDelay"] and - 0 < stamp <= now <= stamp + 1_000_000_000): - value = float(self.sm["lateralDelay"].lateralDelay) - if math.isfinite(value): - delay = float(np.clip(value, 0.1, 0.15)) if angle else float(np.clip(value, 0.2, 0.4)) - return self.sm["modelV2"], ModelConstants.T_IDXS, delay, self.enabled - - def native_permission(self, cp, *, max_age_ns): - """Exact current public Panda acknowledgment for the optional angle owner.""" - if not self.track_assist_permission: - return False - try: - # Drain happens in update(); capture one SM snapshot before sampling clocks. - pandas = self.sm["pandaStates"] - stamp = int(self.sm.logMonoTime["pandaStates"]) - receipt = int(self.sm.recv_time["pandaStates"] * 1e9) - now, boot = time.monotonic_ns(), time.clock_gettime_ns(time.CLOCK_BOOTTIME) - if (not self.sm.seen["pandaStates"] or not self.sm.all_checks(["pandaStates"]) or - not 0 < stamp <= boot <= stamp + max_age_ns or - not 0 < receipt <= now <= receipt + max_age_ns): - return False - if len(pandas) != len(cp.safetyConfigs): - return False - aol = cp.brand == "ford" and cp.alternativeExperience == 32 - if aol: - from openpilot.starpilot.aol.runtime import current_native - native = current_native(self.sm, cp, now_ns=boot) - if (native is None or not native.pandaInventory or not native.requestedLateral or not native.lateralAllowed or - native.sourcePandaStatesMonoTime != stamp or boot - native.observedMonoTime > max_age_ns): - return False - for i, (panda, config) in enumerate(zip(pandas, cp.safetyConfigs, strict=True)): - if (panda.safetyModel != config.safetyModel or panda.safetyParam != config.safetyParam or - panda.alternativeExperience != cp.alternativeExperience or panda.safetyRxChecksInvalid or - panda.heartbeatLost or len(panda.faults) or str(panda.faultStatus) != "none"): - return False - if (not aol and bool(panda.controlsAllowed) != (i == len(pandas) - 1) or - aol and i != len(pandas) - 1 and panda.controlsAllowed): - return False - return True - except (AttributeError, KeyError, TypeError, ValueError, OverflowError): - return False - - def native_tx_blocked(self): - """Read only after native_permission qualified this same Panda snapshot.""" - try: - value = self.sm["pandaStates"][-1].safetyTxBlocked - return int(value) if 0 <= value <= 0xFFFFFFFF else None - except (AttributeError, IndexError, KeyError, TypeError, ValueError, OverflowError): - return None - - def assist_permission(self): - if not self.track_assist_permission: - return (), False - return self.sm["pandaStates"], self.sm.all_checks(["pandaStates"]) - class ResumePlanInputs: """Card-owned fresh plan evidence, independent of Controls' saved preference snapshot.""" @@ -166,6 +27,10 @@ def configure_controller(CI, params): from openpilot.starpilot.car.ford.cluster_preferences import FordClusterPreference, qualified as cluster_qualified if controller is not None and cluster_qualified(cp): controller.hands_free_cluster_input = FordClusterPreference(params) + from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import lateral_inputs_required + if controller is not None and lateral_inputs_required(cp) and hasattr(controller, "bluepilot_inputs"): + from openpilot.starpilot.car.ford.bluepilot import BluePilotInputs + controller.bluepilot_inputs = BluePilotInputs(params) if controller is not None and cp.brand == 'gm': from opendbc.car.gm.values import (is_bolt_euv_longitudinal, is_volt_longitudinal, camera_acc_pedal_profile, volt_cc_pedal_profile) @@ -254,27 +119,6 @@ def configure_controller(CI, params): cloudlog.exception('Optional Volt resume input transport unavailable') if controller is not None and toyota_cruise_capability(cp) is not None: controller.reverse_cruise_input = ToyotaCruisePreference(cp, params) - if (controller is not None and cp.brand == "ford" and cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and - not cp.flags & FordFlags.LKA_STEERING and not cp.passive and not cp.dashcamOnly and not cp.notCar and - getattr(controller, "manual_turn", None) is not None): - controller.manual_turn_inputs = ManualTurnInputs(params, track_assist_permission=bool(cp.flags & FordFlags.CANFD), pinion=pinion_configured(cp)) - - from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified - if controller is not None and angle_qualified(cp) and getattr(controller, "bp_lat", None) is not None: - controller.bp_lat.host.inputs = ManualTurnInputs(params, track_assist_permission=True, pinion=pinion_configured(cp)) - - from opendbc.car.ford.classic_lateral import qualified as classic_qualified - from opendbc.car.ford.generic_canfd_lateral import qualified as generic_canfd_qualified - if controller is not None and (classic_qualified(cp) or generic_canfd_qualified(cp)) and getattr(controller, "classic_lateral", None) is not None: - controller.manual_turn_inputs = ManualTurnInputs(params, pinion=pinion_configured(cp)) - - from opendbc.car.ford.new_port_curvature import qualified as new_port_curvature_qualified - if controller is not None and new_port_curvature_qualified(cp) and getattr(controller, "new_port_curvature", None) is not None: - controller.manual_turn_inputs = ManualTurnInputs(params, pinion=pinion_configured(cp)) - - if controller is not None and cp.brand == "ford" and getattr(controller, "manual_turn_inputs", None) is not None: - controller.manual_turn_inputs.apply_blend_settings(controller) - if controller is not None and cp.brand == "honda" and getattr(controller, "nidec_interceptor", None) is not None: from openpilot.starpilot.car.honda.longitudinal import BoschLearningParams controller.interceptor_learning_params = BoschLearningParams(params, controller.nidec_interceptor) diff --git a/openpilot/starpilot/flm/runtime_adapter_registry.py b/openpilot/starpilot/flm/runtime_adapter_registry.py index 9f8e9abbbf..a8a7571ab0 100644 --- a/openpilot/starpilot/flm/runtime_adapter_registry.py +++ b/openpilot/starpilot/flm/runtime_adapter_registry.py @@ -197,6 +197,9 @@ def ford_public_identity_key(cp, pandas): ) +FORD_PANDA_AGE_NS = 300_000_000 # a pandaStates sample older than this is not current + + def current_ford_authority(sm, cp, *, now_ns): """Optional parent20Hz mirror of the existing successful Ford verifier.""" key = (now_ns, id(cp)) @@ -205,8 +208,6 @@ def current_ford_authority(sm, cp, *, now_ns): return cached[1:] allowed, identity = False, '' try: - from opendbc.bluepilot_lateral.params import limits as bounds - if not ford_downstream(cp): raise ValueError('Ford downstream role') pandas = sm['pandaStates'] @@ -216,8 +217,8 @@ def current_ford_authority(sm, cp, *, now_ns): allowed = bool( sm.seen['pandaStates'] and sm.all_checks(['pandaStates']) - and 0 < stamp <= boot <= stamp + bounds.PANDA_AGE_NS - and 0 < receipt <= now_ns <= receipt + bounds.PANDA_AGE_NS + and 0 < stamp <= boot <= stamp + FORD_PANDA_AGE_NS + and 0 < receipt <= now_ns <= receipt + FORD_PANDA_AGE_NS and len(pandas) == len(cp.safetyConfigs) ) for index, (panda, config) in enumerate(zip(pandas, cp.safetyConfigs, strict=True)): @@ -434,11 +435,12 @@ def selected_identity(descriptor, source_epoch, baseline=""): def ford_downstream(cp): - # The existing final admitted CP safety word selects the actual library; + # The BluePilot lateral library owns Ford steering when its bit is in the final admitted CP safety word; # no settings read or generic angle-output inference establishes this role. - from opendbc.bluepilot_lateral.hosts.starpilot import qualified + from opendbc.car.ford.aol import BP_LATERAL, aol_safety_config - return bool(str(cp.carFingerprint) in interfaces and qualified(cp)) + config = aol_safety_config(cp) if cp.brand == 'ford' else None + return bool(str(cp.carFingerprint) in interfaces and config is not None and config.safetyParam & BP_LATERAL) def adapter_for_ford(cp, context): diff --git a/openpilot/starpilot/flm/tests/test_native_health.py b/openpilot/starpilot/flm/tests/test_native_health.py index 770c409351..1c3ad584e8 100644 --- a/openpilot/starpilot/flm/tests/test_native_health.py +++ b/openpilot/starpilot/flm/tests/test_native_health.py @@ -124,9 +124,9 @@ def ford_state(cp, observed, *, heartbeat=False): def ford_setup(): - from opendbc.bluepilot_lateral.hosts.starpilot import select + from opendbc.car.ford.aol import BP_LATERAL cp = interfaces[FORD.FORD_ESCAPE_MK4].get_non_essential_params(FORD.FORD_ESCAPE_MK4) - assert select(cp, 1) + cp.safetyConfigs[-1].safetyParam |= BP_LATERAL now = time.monotonic_ns() sm = ford_state(cp, now-1_000_000) allowed, identity = current_ford_authority(sm, cp, now_ns=now) diff --git a/openpilot/starpilot/galaxy/setting_descriptions.py b/openpilot/starpilot/galaxy/setting_descriptions.py index 50d78bc087..0995805744 100644 --- a/openpilot/starpilot/galaxy/setting_descriptions.py +++ b/openpilot/starpilot/galaxy/setting_descriptions.py @@ -11,7 +11,6 @@ DESCRIPTIONS = { 'ReduceMotion': 'Reduces interface animations for a steadier display.', 'IsMetric': 'On displays speed and distance in kilometers; Off uses miles. Saved speed-limit offsets keep their physical values.', 'IsRhdDetected': 'Sets the initial steering-wheel side used by driver monitoring. Applies next drive; automatic side detection continues.', - 'FordPrefSteerAngleCurvature': 'Uses steering pinion angle instead of yaw rate to estimate Ford steering curvature. Applies at the next vehicle startup.', 'lane_change:turn_signal_guidance': 'Uses your turn signal to help the model choose a turning path at intersections.', 'AlphaLongitudinalEnabled': ('Requests StarPilot speed control instead of factory ACC. May disable Automatic Emergency Braking. ' + 'Applies after the next startup; exact vehicle takeover checks remain required.'), @@ -84,7 +83,6 @@ DESCRIPTIONS = { 'GMLongitudinalTune': 'Selects the supported vehicle speed-control response tune.', 'TruckTuning': 'Softens small acceleration requests, reduces grade correction and smooths light braking on supported GM trucks.', 'EVTuning': 'Selects EV acceleration curves for inherited acceleration presets.', - 'FordLateralMode': 'Chooses the native Ford steering controller or the supported BluePilot angle controller.', 'TeslaAOLScreenTap': 'Lets a three-finger tap on a supported Tesla screen toggle Always On Lateral.', 'TeslaAOLDisengageOnBrake': 'Disarms screen-tap Always On Lateral when you press the brake.', 'RemoteStartBootsComma': 'Wakes comma during GM remote start.', @@ -139,6 +137,30 @@ DESCRIPTIONS = { 'conditional:mode': 'Chooses Chill, Experimental or automatic switching based on the configured driving conditions.', 'conditional:reset': 'Restores the default conditional driving-mode settings.', 'conditional:manual_reset': 'Returns both remembered manual driving-mode choices to Automatic.', + 'disable_BP_lat_UI': ('Steers with the BluePilot lateral controller instead of the stock Ford curvature path: curvature or' + + ' angle as the primary control, with lane positioning and its own tuning. Off by default.'), + 'FordPrefLateralControl': ('Chooses what BluePilot steers with. Curvature follows the planned curvature like the stock path;' + + ' Angle uses the path angle as the main actuator.'), + 'FordPrefSteerAngleCurvature': ('Measures curvature from the steering pinion angle instead of the yaw-rate sensor, in control and' + + ' Panda safety. For Fords with a faulty yaw sensor.'), + 'enable_human_turn_detection_curv': 'Eases off steering while you turn the wheel through a turn yourself.', + 'lane_change_factor_high_curv': 'Scales steering during a lane change. Values under 1.0 soften it.', + 'enable_lane_positioning_curv': 'Nudges the car toward the lane center, with an optional in-lane offset.', + 'custom_path_offset_curv': "Biases the car's position inside the lane, in meters.", + 'enable_lane_full_mode_curv': ("Centers between the lane lines when both are confidently detected, instead of following the model's" + + ' path.'), + 'LC_PID_gain_UI_curv': 'Sets how strongly lane positioning pulls toward its target.', + 'custom_profile_curv': 'Uses the two predicted-curvature blend settings below instead of the built-in profile.', + 'pc_blend_ratio_low_C_UI_curv': 'Mixes predicted curvature into gentle curves: 0 follows the plan, 1 follows the prediction.', + 'pc_blend_ratio_high_C_UI_curv': 'Mixes predicted curvature into sharp curves: 0 follows the plan, 1 follows the prediction.', + 'FordLowSpeedFactor_ang': 'Scales the low-speed steering response in angle mode. 1.0 is the default feel.', + 'FordHighSpeedFactor_ang': 'Scales the high-speed steering response in angle mode. 1.0 is the default feel.', + 'FordHighSpeedDampening_ang': ('Tunes the straight-road response at highway speed in angle mode: lower it if the car oversteers,' + + ' raise it if it understeers.'), + 'lane_change_factor_high_ang': 'Scales steering during a lane change in angle mode. Values over 1.0 strengthen it.', + 'enable_lane_positioning_ang': 'Nudges the car toward the lane-line center in angle mode, with an optional in-lane offset.', + 'custom_path_offset_ang': "Biases the car's position inside the lane in angle mode, in meters.", + 'lane_centering_strength_ang': "Sets how strongly lane positioning pulls against the model's own path in angle mode.", } # These are initialization explanations, not a second defaults table. @@ -155,13 +177,14 @@ INITIALIZATION = { 'VoltOnePedalMode': ('First enable requires the next vehicle startup to configure the braking safety ' + 'permission. An already admitted mode can be disabled live.'), 'VoltSNG': 'Applies when the vehicle controller next starts: its fresh resume-plan input is created for the selected stop-and-go mode.', - 'FordLateralMode': 'Requires the next vehicle startup to select the steering controller, admitted CAN mode and Panda safety word.', 'TeslaAOLScreenTap': 'Requires the next vehicle startup to configure the screen-button CAN source and matching safety permission.', 'TeslaAOLDisengageOnBrake': 'Requires the next vehicle startup to configure the screen-button brake/disarm safety permission.', 'RemoteStartBootsComma': 'Requires a device restart to load the matching Panda ignition firmware.', 'HKGRemoteStartBootsComma': 'Requires a device restart to load the matching Panda ignition firmware.', 'IgnoreIgnitionLine': 'Requires a device restart to load the matching Panda ignition firmware.', 'TeslaWakeOnCAN': 'Requires a device restart to load the matching Panda ignition firmware.', + 'disable_BP_lat_UI': 'Requires the next vehicle startup to configure Panda safety for the BluePilot steering checks.', + 'FordPrefSteerAngleCurvature': 'Requires the next vehicle startup to configure the pinion measurement in Panda safety.', } CONDITIONS = { diff --git a/openpilot/starpilot/galaxy/tests/description-key-inventory.json b/openpilot/starpilot/galaxy/tests/description-key-inventory.json index a5155e83b0..57e8fce346 100644 --- a/openpilot/starpilot/galaxy/tests/description-key-inventory.json +++ b/openpilot/starpilot/galaxy/tests/description-key-inventory.json @@ -29,7 +29,10 @@ "ForceAutoTuneOff", "ForceStopDistanceOffset", "ForceStops", - "FordLateralMode", + "FordHighSpeedDampening_ang", + "FordHighSpeedFactor_ang", + "FordLowSpeedFactor_ang", + "FordPrefLateralControl", "FordPrefSteerAngleCurvature", "GMAutoHold", "GMLongitudinalTune", @@ -41,6 +44,7 @@ "IsMetric", "IsRhdDetected", "KeepScreenOnGpuLoading", + "LC_PID_gain_UI_curv", "LaneCenterOffset", "LaneCentering", "LaneCenteringE2EAuthority", @@ -159,8 +163,17 @@ "conditional:mode", "conditional:reset", "curve_reset", + "custom_path_offset_ang", + "custom_path_offset_curv", + "custom_profile_curv", + "disable_BP_lat_UI", "display:auto:ScreenBrightness", "display:auto:ScreenBrightnessOnroad", + "enable_human_turn_detection_curv", + "enable_lane_full_mode_curv", + "enable_lane_positioning_ang", + "enable_lane_positioning_curv", + "lane_centering_strength_ang", "lane_change:auto", "lane_change:close_gap", "lane_change:close_gap_seconds", @@ -172,6 +185,8 @@ "lane_change:speed", "lane_change:turn_signal_guidance", "lane_change:width", + "lane_change_factor_high_ang", + "lane_change_factor_high_curv", "long_repair:AggressiveFollow", "long_repair:AggressiveFollowHigh", "long_repair:AggressiveJerkAcceleration", @@ -197,6 +212,8 @@ "long_reset:aggressive", "long_reset:relaxed", "long_reset:standard", + "pc_blend_ratio_high_C_UI_curv", + "pc_blend_ratio_low_C_UI_curv", "pip:format:1344x760", "pip:format:1928x1208", "pip:mask:editor", diff --git a/openpilot/starpilot/galaxy/tests/test_ford_angle_settings.py b/openpilot/starpilot/galaxy/tests/test_ford_angle_settings.py deleted file mode 100644 index 3cb243fc4d..0000000000 --- a/openpilot/starpilot/galaxy/tests/test_ford_angle_settings.py +++ /dev/null @@ -1,118 +0,0 @@ -"""One typed Ford choice through the real Galaxy shared settings owner.""" -from pathlib import Path -import tempfile -import unittest - -from openpilot.common.params import Params -from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsChanged, SettingsGateway -from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner -from opendbc.car.ford.tests.test_three_ports import params as ford_params -from opendbc.car.ford.values import CAR -from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, select - - -class FordContext: - def __init__(self, owner): - self.owner = owner - - def sample(self) -> AuthorityContext: - return AuthorityContext(self.owner.parked, self.owner.cp, self.owner.cp.as_reader().as_builder().to_bytes()) - - -class TestFordAngleSettings(unittest.TestCase): - def setUp(self): - temporary = tempfile.TemporaryDirectory() - self.addCleanup(temporary.cleanup) - self.params = Params(temporary.name) - self.params.put_bool("SafeMode", False, block=True) - self.params.put_bool("AlwaysOnLateral", False, block=True) - self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1) - self.parked = True - self.context = FordContext(self) - self.gateway = SettingsGateway(self.params, self.context) - self.addCleanup(self.gateway.close) - - def page(self): - return self.gateway.page("torque", "ford-session", b"ford-generation") - - def row(self, page): - rows = [(i, r) for i, r in enumerate(page["rows"]) if r["label"] == "Ford steering controller"] - self.assertEqual(len(rows), 1) - return rows[0] - - def choose(self, label): - page = self.page() - index, row = self.row(page) - self.assertTrue(row["available"]) - intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value=label) - return self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation") - - def test_actual_final_profiles_one_dropdown_and_typed_explicit_persistence(self): - for identity in sorted(CLASSIC | CANFD): - for already_selected in (False, True): - self.cp = ford_params(identity) - if already_selected: - self.assertTrue(select(self.cp, 1)) - page = self.page() - _, row = self.row(page) - self.assertEqual(row["choices"], ["Current curvature", "BluePilot angle"]) - self.choose("BluePilot angle") - self.assertEqual(self.params.get("FordLateralMode"), 1) - self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"1") - self.choose("Current curvature") - self.assertEqual(self.params.get("FordLateralMode"), 0) - owner = FeatureSettingsOwner(self.params, lambda _: True, vehicle_fingerprint=lambda: self.cp.carFingerprint, - vehicle_params=lambda: self.cp) - rows = owner.snapshot("torque", parked=True, system_long=False, lateral_context=True, metric=False).rows - self.assertEqual(sum(r.key == "FordLateralMode" for r in rows), 1) - - def test_excluded_or_modified_final_capability_has_no_angle_row(self): - for identity in (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5, CAR.FORD_TRANSIT_MK5): - self.cp = ford_params(identity) - self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"])) - for field, value in (("alternativeExperience", 16), ("passive", True), ("dashcamOnly", True), ("notCar", True)): - self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1) - setattr(self.cp, field, value) - self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"])) - self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1) - self.cp.safetyConfigs[-1].safetyParam = 0 - self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"])) - - def test_stale_expected_dependency_vehicle_and_parked_changes_deny_commit(self): - for change in ("saved", "safe", "aol", "vehicle", "parked"): - self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1) - self.parked = True - self.params.put("FordLateralMode", 0, block=True) - self.params.put_bool("SafeMode", False, block=True) - self.params.put_bool("AlwaysOnLateral", False, block=True) - page = self.page() - index, _ = self.row(page) - intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value="BluePilot angle") - if change == "saved": - Path(self.params.get_param_path("FordLateralMode")).write_bytes(b"invalid") - elif change in ("safe", "aol"): - self.params.put_bool("SafeMode" if change == "safe" else "AlwaysOnLateral", True, block=True) - elif change == "vehicle": - self.cp.safetyConfigs[-1].safetyParam = 128 - else: - self.parked = False - if change == "vehicle": - with self.assertRaises(SettingsChanged): - self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation") - else: - self.assertFalse(self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation")) - self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"invalid" if change == "saved" else b"0") - with self.assertRaises(SettingsChanged): - self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation") - - def test_invalid_saved_choice_can_only_repair_to_current_curvature(self): - path = Path(self.params.get_param_path("FordLateralMode")) - path.write_bytes(b"2") - page = self.page() - index, row = self.row(page) - self.assertEqual(row["value"], "Invalid saved choice") - with self.assertRaises(SettingsChanged): - self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value="BluePilot angle") - intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", reset_default=True) - self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation") - self.assertEqual(path.read_bytes(), b"0") diff --git a/openpilot/starpilot/galaxy/tests/test_ford_pinion_settings.py b/openpilot/starpilot/galaxy/tests/test_ford_pinion_settings.py deleted file mode 100644 index e7ef851d4e..0000000000 --- a/openpilot/starpilot/galaxy/tests/test_ford_pinion_settings.py +++ /dev/null @@ -1,63 +0,0 @@ -"""Actual typed Params, Galaxy parked write, and startup configuration for pinion.""" -from pathlib import Path -import tempfile - -import pytest -from opendbc.car import gen_empty_fingerprint -from opendbc.car.ford.pinion import KEY, GEOMETRY, configured -from opendbc.car.ford.values import CAR -from opendbc.car.ford.tests.test_three_ports import params as ford_params -from openpilot.common.params import Params -from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsGateway - - -def test_real_galaxy_typed_parked_selection_and_stale_dependency(): - with tempfile.TemporaryDirectory() as directory: - params = Params(directory) - assert params.get_default_value(KEY) is False - params.put_bool('SafeMode', False, block=True) - params.put_bool('AlwaysOnLateral', False, block=True) - cp = ford_params(CAR.FORD_EXPLORER_MK6) - parked = True - class TestContext: - def sample(self) -> AuthorityContext: - return AuthorityContext(parked, cp, cp.as_reader().as_builder().to_bytes()) - - gateway = SettingsGateway(params, TestContext()) - try: - page = gateway.page('torque', 'pinion-test', b'generation') - index, row = next((i, row) for i, row in enumerate(page['rows']) if row['label'] == 'Use Pinion Yaw Sensor') - assert row['choices'] == ['Off', 'On'] and row['available'] - intent = gateway.preview(page['view'], index, 0, 'pinion-test', b'generation', value='On') - assert gateway.confirm(intent['intent'], 'pinion-test', b'generation') - assert params.get(KEY) is True - assert Path(params.get_param_path(KEY)).read_bytes() == b'1' - assert not configured(cp) # Saved preference cannot mutate the current driving configuration. - parked = False - page = gateway.page('torque', 'pinion-test', b'generation') - row = next(row for row in page['rows'] if row['label'] == 'Use Pinion Yaw Sensor') - assert not row['available'] - finally: - gateway.close() - - -@pytest.mark.parametrize('identity', list(GEOMETRY)) -def test_actual_factory_applies_before_controller_and_published_cp(identity): - from openpilot.starpilot.tests.test_vehicle_preferences import TestVehicleStartupPreferences - helper = TestVehicleStartupPreferences() - helper.setUp() - try: - helper.raw('SafeMode', b'0') - helper.raw('AlwaysOnLateral', b'0') - helper.params.put_bool('AlphaLongitudinalEnabled', False, block=True) - helper.params.put_bool('IsReleaseBranch', False, block=True) - fp = gen_empty_fingerprint() - fp[0][0x5A] = 8 - fp[2].update({0x3D6: 8, 0x186: 8}) - host, constructed, published = helper.start(identity, key=KEY, requested=True, observed=fp, - capture=lambda ci: configured(ci.CP)) - assert constructed == [True] - assert configured(published) - assert host.CI.CP.to_dict() == published.to_dict() - finally: - helper.doCleanups() diff --git a/openpilot/starpilot/galaxy/tests/test_ford_selector_visibility.py b/openpilot/starpilot/galaxy/tests/test_ford_selector_visibility.py deleted file mode 100644 index 9d16ea104c..0000000000 --- a/openpilot/starpilot/galaxy/tests/test_ford_selector_visibility.py +++ /dev/null @@ -1,74 +0,0 @@ -import tempfile -import unittest -from pathlib import Path -from types import SimpleNamespace - -from opendbc.car import structs -from opendbc.car.ford.values import CAR, FordFlags -from openpilot.starpilot.ui.ford_lateral_feature import FordLateralFeature - - -class TestFordSelectorVisibility(unittest.TestCase): - def setUp(self): - directory = tempfile.TemporaryDirectory() - self.addCleanup(directory.cleanup) - self.root = Path(directory.name) - self.cp = structs.CarParams(brand='ford', carFingerprint=CAR.FORD_MUSTANG_MACH_E_MK1, - flags=int(FordFlags.CANFD), alternativeExperience=32, pcmCruise=True, - steerControlType='angle', transmissionType='automatic', - safetyConfigs=[{'safetyModel': 'ford', 'safetyParam': 18}]) - self.params = SimpleNamespace(get_param_path=lambda key: str(self.root / key)) - self.owner = SimpleNamespace(params=self.params, vehicle_params=lambda: self.cp, - authority=lambda _: True, vehicle_fingerprint=lambda: self.cp.carFingerprint) - self.feature = FordLateralFeature(self.owner) - self.save('AlwaysOnLateral', b'1') - self.save('SafeMode', b'0') - - def save(self, key, value): - (self.root / key).write_bytes(value) - - def test_aol_marked_mach_e_visible_without_mutating_configuration(self): - for alpha in (False, True): - self.cp.openpilotLongitudinalControl = alpha - self.cp.safetyConfigs[0].safetyParam = 18 + int(alpha) - before = self.cp.to_dict() - rows = self.feature.rows(True) - self.assertEqual(len(rows), 1) - self.assertEqual(rows[0].choices, ('Current curvature', 'BluePilot angle')) - self.assertTrue(rows[0].available) - self.assertIn('works with Always On Lateral', rows[0].reason) - self.assertEqual(self.cp.to_dict(), before) - - def test_saved_aol_off_allows_selection_without_waiting_for_cp_restart(self): - self.save('AlwaysOnLateral', b'0') - self.assertTrue(self.feature.rows(True)[0].available) - self.assertEqual(self.cp.alternativeExperience, 32) - self.assertEqual(self.cp.safetyConfigs[0].safetyParam, 18) - - def test_onroad_and_safe_mode_still_deny_selection(self): - self.save('AlwaysOnLateral', b'0') - self.assertFalse(self.feature.rows(False)[0].available) - self.save('SafeMode', b'1') - self.assertFalse(self.feature.rows(True)[0].available) - - def test_reader_uses_the_same_display_only_capability(self): - self.cp = self.cp.as_reader() - self.assertEqual(len(self.feature.rows(True)), 1) - self.assertEqual(self.cp.alternativeExperience, 32) - - def test_invalid_native_owner_remains_hidden(self): - for word in (0, 2, 130, 65535): - self.cp.safetyConfigs[0].safetyParam = word - self.assertEqual(self.feature.rows(True), ()) - - def test_unrelated_experience_and_brand_remain_hidden(self): - self.cp.alternativeExperience = 16 - self.assertEqual(self.feature.rows(True), ()) - self.cp.alternativeExperience = 32 - self.cp.brand = 'toyota' - self.assertEqual(self.feature.rows(True), ()) - - def test_existing_unmarked_mach_e_stays_available(self): - self.cp.alternativeExperience = 0 - self.save('AlwaysOnLateral', b'0') - self.assertTrue(self.feature.rows(True)[0].available) diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index b37e006ee2..35de00aede 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -50,19 +50,19 @@ from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety FORD_FACTORIES = ( - (CAR.FORD_BRONCO_SPORT_MK1, 32), + (CAR.FORD_BRONCO_SPORT_MK1, 0), (CAR.FORD_EDGE_MK2, 8), - (CAR.FORD_ESCAPE_MK4, 32), - (CAR.FORD_ESCAPE_MK4_5, 66), - (CAR.FORD_EXPLORER_MK6, 32), - (CAR.FORD_EXPEDITION_MK4, 66), - (CAR.FORD_F_150_MK14, 66), - (CAR.FORD_F_150_LIGHTNING_MK1, 66), - (CAR.FORD_FOCUS_MK4, 32), + (CAR.FORD_ESCAPE_MK4, 0), + (CAR.FORD_ESCAPE_MK4_5, 2), + (CAR.FORD_EXPLORER_MK6, 0), + (CAR.FORD_EXPEDITION_MK4, 2), + (CAR.FORD_F_150_MK14, 2), + (CAR.FORD_F_150_LIGHTNING_MK1, 2), + (CAR.FORD_FOCUS_MK4, 0), (CAR.FORD_MONDEO_MK5, 10), - (CAR.FORD_MAVERICK_MK1, 32), - (CAR.FORD_MUSTANG_MACH_E_MK1, 18), - (CAR.FORD_RANGER_MK2, 66), + (CAR.FORD_MAVERICK_MK1, 0), + (CAR.FORD_MUSTANG_MACH_E_MK1, 2), + (CAR.FORD_RANGER_MK2, 2), (CAR.FORD_TRANSIT_MK5, 12), ) @@ -89,14 +89,11 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property): ('SafeMode', False), ('AlphaLongitudinalEnabled', alpha), ('IsReleaseBranch', release), - ('FordHumanTurnDetection', False), - ('FordPrefSteerAngleCurvature', getattr(monkeypatch, 'ford_pinion', False)), ): saved.put_bool(key, value, block=True) from pathlib import Path - for key, value in (('FordLateralMode', b'1' if getattr(monkeypatch, 'ford_angle', False) else b'0'), - ('PauseAOLOnBrake', b'0.0'), ('CancelButtonControl', b'0'), ('MainCruiseButtonControl', b'0')): + for key, value in (('PauseAOLOnBrake', b'0.0'), ('CancelButtonControl', b'0'), ('MainCruiseButtonControl', b'0')): Path(saved.get_param_path(key)).write_bytes(value) Path(saved.get_param_path("FordHandsFreeCluster")).write_bytes(b"1" if hands_free else b"0") fp = gen_empty_fingerprint() @@ -121,10 +118,7 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property): assert str(host.CP.carFingerprint) == str(name) expected_long = bool(alpha) assert host.CP.openpilotLongitudinalControl == expected_long - expected_word = base | int(expected_long) | (256 if manual else 0) | (128 if getattr(monkeypatch, 'ford_angle', False) else 0) - if getattr(monkeypatch, 'ford_pinion', False): - from opendbc.car.ford.pinion import ENABLE, SHIFT, GEOMETRY - expected_word |= ENABLE | GEOMETRY[name] << SHIFT + expected_word = base | int(expected_long) | (256 if manual else 0) assert host.CP.safetyConfigs[-1].safetyParam == expected_word record_property('ford_factory_identity', str(name)) record_property('ford_saved_alpha', alpha) @@ -132,16 +126,7 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property): record_property('ford_longitudinal', expected_long) record_property('ford_safety_word', int(host.CP.safetyConfigs[-1].safetyParam)) native.reset(host.CP.safetyConfigs[-1].safetyParam) - from opendbc.car.ford.pinion import base_word - native.word = base_word(native.word) & ~(1 | 128) - if getattr(monkeypatch, 'ford_pinion', False): - pump = native.pump - def pinion_pump(count, **kwargs): - for _ in range(count): - pump(1, **kwargs) - native.rx('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': native.tick % 16}) - monkeypatch.setattr(native, 'pump', pinion_pump) - + native.word &= ~(1 | 128) # Real empty update registers lazily accessed parser messages before RX. # Card.controls_update owns CI initialization; no flag is assigned here. host.CI.update([]) @@ -274,7 +259,6 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag local.setattr(sd.sm, 'update_msgs', location_update) - controller_returns = [] delayed_pedal_frames = [] delayed_pedals_seen = set() route45_old = {} @@ -294,8 +278,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag request = 0 if prior_axis is None else (int(prior_axis.aolAxisState.desiredLateral) | (int(prior_axis.aolAxisState.desiredLongitudinal) << 1)) arm_heartbeat = retained_arm(published.get('aolIntentWire'), prior_axis, now[0]) heartbeat = bool(request or prior_state is not None and prior_state.selfdriveState.enabled or arm_heartbeat) - frames = delayed_pedal_frames[:] + controller_returns[:] - controller_returns.clear() + frames = delayed_pedal_frames[:] delayed_pedal_frames.clear() def receive(name, values): @@ -493,17 +476,6 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag host.can_log_mono_time = source_ns published.pop('sendcan', None) joined.last_lateral_packets = [] - if getattr(monkeypatch, 'ford_angle', False): - owner = host.CI.CC.bp_lat.host.inputs - # Real controller input reader consumes the same original fresh native - # receipt, Panda inventory and synthetic model boundary as Controls. - messages = [wire_message, panda_message] - for service in ('modelV2', 'lateralDelay'): - message = messaging.new_message(service, valid=True, logMonoTime=source_ns) - setattr(message, service, controls.sm[service]) - messages.append(message.as_reader()) - owner.sm.update_msgs(now[0] / 1e9, messages) - local.setattr(owner.sm, 'update', lambda *args: None) host.controls_update(state, host.sm['carControl']) if state.canValid: accepted_ui = [] @@ -513,13 +485,6 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag from opendbc.safety.tests.libsafety import libsafety_py accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(frame.address, frame.src - joined.offset, frame.dat)) - if getattr(monkeypatch, 'ford_angle', False) and frame.address in (0x3CA, 0x3D3, 0x3D6): - controller_returns.append((frame.address, bytes(frame.dat), (frame.src - joined.offset) | (0x80 if accepted else 0xC0))) - if (getattr(monkeypatch, 'ford_pinion', False) and (brake or cancel) and command.latActive and - frame.address in (0x3D3, 0x3D6)): - mode = ((frame.dat[4] >> 2) & 7) if frame.address == 0x3D3 else ((frame.dat[0] >> 4) & 7) - assert accepted and mode == 1, 'AOL flag stayed on while the steering packet withdrew' - joined.pedal_cancel_active_packets = getattr(joined, 'pedal_cancel_active_packets', 0) + 1 # A real native fault can precede the next 10Hz host receipt. Active # requests in that gap must be rejected, never treated as accepted. fault_before_receipt = (getattr(monkeypatch, 'ford_native_10hz', False) and @@ -562,7 +527,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag else: assert data[6] >> 3 in (0, 30) and ((data[6] >> 1) & 3) == 0 last_neutral_ui[address] = now[0] - if getattr(monkeypatch, 'ford_angle', False) or scenario in ( + if scenario in ( 'display', 'route24', 'route24-mixed-eps', 'route2a', 'route2a-main', 'route2a-main-fault'): ui = accepted_ui if state.canValid else [] owns = all(safety.safety_fwd_hook(2, frame.address) == -1 for frame in ui) @@ -889,12 +854,6 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first brake erased lateral intent' assert not command.longActive or joined.last_permission & 2 assert host.aol_card_intent.allowed_latch and command.latActive and not command.longActive - if getattr(monkeypatch, 'ford_angle', False): - for _ in range(20): - state, axis, sd_state, command = step() - assert not state.brakePressed and command.latActive and host.aol_card_intent.allowed_latch and not command.longActive - if getattr(monkeypatch, 'ford_nonzero', False): - assert nonzero_after_brake > 0, 'No native-accepted nonzero angle after releasing brake' for _ in range(30): state, axis, sd_state, command = step(reverse=True) assert host.aol_card_intent.allowed_latch and not command.latActive and not command.longActive, transition_trace[-40:] diff --git a/openpilot/starpilot/tests/test_controller_extensions.py b/openpilot/starpilot/tests/test_controller_extensions.py deleted file mode 100644 index bf4f507919..0000000000 --- a/openpilot/starpilot/tests/test_controller_extensions.py +++ /dev/null @@ -1,180 +0,0 @@ -import tempfile -import unittest -from types import SimpleNamespace -from unittest.mock import patch - -from openpilot.starpilot.controller_extensions import ManualTurnInputs, configure_controller -from openpilot.starpilot.tests import test_vehicle_preferences as preferences_tests -from opendbc.car.ford.values import CAR - - -class TestControllerExtensions(unittest.TestCase): - def test_real_saved_preference_default_and_disabled(self): - from openpilot.common.params import Params - with tempfile.TemporaryDirectory() as directory: - params = Params(directory) - self.assertTrue(params.get('FordHumanTurnDetection', return_default=True)) - params.put_bool('FordHumanTurnDetection', False, block=True) - self.assertFalse(params.get('FordHumanTurnDetection', return_default=True)) - - def test_actual_model_owner_and_cached_preference(self): - import openpilot.cereal.messaging as messaging - from openpilot.common.params import Params - message = messaging.new_message('modelV2') - message.modelV2.meta.laneChangeState = 'laneChangeStarting' - sm = SimpleNamespace(alive={'modelV2': True}, valid={'modelV2': True}, update=lambda timeout: None) - - class ModelReader: - alive = sm.alive - valid = sm.valid - - def update(self, timeout): - sm.update(timeout) - - def __getitem__(self, key): - return message.modelV2 - with patch.object(messaging, 'SubMaster', return_value=ModelReader()), patch.object(Params, 'get', return_value=True) as read: - inputs = ManualTurnInputs(Params()) - self.assertEqual(inputs.update(), (True, True, True)) - for _ in range(99): - inputs.update() - self.assertEqual(read.call_count, 8) - inputs.update() - self.assertEqual(read.call_count, 12) - sm.alive['modelV2'] = False - self.assertEqual(inputs.update(), (True, True, False)) - - def test_model_preview_freshness_delay_and_invalid_output(self): - import openpilot.cereal.messaging as messaging - from openpilot.selfdrive.modeld.constants import ModelConstants - now = 10_000_000_000 - model = messaging.new_message('modelV2') - model.modelV2.orientationRate.z = [20. * (value / 100.) for value in ModelConstants.T_IDXS] - delay = messaging.new_message('lateralDelay') - delay.lateralDelay.lateralDelay = .3 - class Reader: - alive = {'modelV2': True, 'lateralDelay': True} - valid = dict(alive) - logMonoTime = {'modelV2': now, 'lateralDelay': now} - def __getitem__(self, key): - return model.modelV2 if key == 'modelV2' else delay.lateralDelay - inputs = ManualTurnInputs.__new__(ManualTurnInputs) - vars(inputs)['sm'] = Reader() - with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now): - curvature = inputs.preview_curvature(20.) - assert curvature is not None - self.assertAlmostEqual(curvature, .003) - inputs.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001 - curvature = inputs.preview_curvature(20.) - assert curvature is not None - self.assertAlmostEqual(curvature, .002) - for stamp in (0, now + 1, now - 100_000_001): - inputs.sm.logMonoTime['modelV2'] = stamp - self.assertIsNone(inputs.preview_curvature(20.)) - inputs.sm.logMonoTime['modelV2'] = now - self.assertIsNone(inputs.preview_curvature(0.)) - model.modelV2.orientationRate.z = [float('nan')] * 33 - self.assertIsNone(inputs.preview_curvature(20.)) - - def test_assist_permission_tracks_panda_health_without_affecting_preview(self): - import openpilot.cereal.messaging as messaging - from openpilot.common.params import Params - - class Reader(dict): - def __init__(self): - super().__init__(pandaStates=[SimpleNamespace(controlsAllowed=True)]) - self.alive = self.valid = self.freq_ok = True - - def all_checks(self, services): - self.asserted_services = services - return self.alive and self.valid and self.freq_ok - - reader = Reader() - with patch.object(messaging, 'SubMaster', return_value=reader) as factory, patch.object(Params, 'get', return_value=True): - inputs = ManualTurnInputs(Params()) - self.assertNotIn('pandaStates', factory.call_args.args[0]) - self.assertEqual(inputs.assist_permission(), ((), False)) - inputs = ManualTurnInputs(Params(), track_assist_permission=True) - self.assertIn('pandaStates', factory.call_args.args[0]) - self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], True)) - for health in ('alive', 'valid', 'freq_ok'): - setattr(reader, health, False) - self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], False)) - setattr(reader, health, True) - self.assertEqual(reader.asserted_services, ['pandaStates']) - - def test_real_card_finalizes_then_binds_only_admitted_controller(self): - harness = preferences_tests.TestVehicleStartupPreferences(methodName='test_only_exact_saved_opt_in_is_loaded') - harness.setUp() - self.addCleanup(harness.doCleanups) - with patch('openpilot.starpilot.controller_extensions.messaging.SubMaster', return_value=SimpleNamespace()): - host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1) - self.assertFalse(published.passive) - self.assertIsInstance(host.CI.CC.manual_turn_inputs, ManualTurnInputs) - self.assertIs(host.CI.CC.manual_turn_inputs.params, harness.params) - self.assertTrue(host.CI.CC.manual_turn_inputs.track_assist_permission) - host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1, enabled=False) - self.assertTrue(published.passive) - self.assertIsNone(host.CI.CC.manual_turn_inputs) - - def test_standalone_controller_has_no_host_owner(self): - from opendbc.car.ford.tests.test_three_ports import params - from opendbc.car.ford.carcontroller import CarController - from opendbc.car.ford.values import DBC - from opendbc.car.ford.carstate import CarState - from opendbc.car import structs - cp = params(CAR.FORD_MUSTANG_MACH_E_MK1) - controller = CarController(DBC[cp.carFingerprint], cp) - state = CarState(cp) - state.update(state.get_can_parsers(cp)) - state.out = structs.CarState().as_reader() - controller.update(structs.CarControl().as_reader(), state, 0) - self.assertIsNone(controller.manual_turn_inputs) - cp.passive = True - configure_controller(SimpleNamespace(CP=cp, CC=controller), SimpleNamespace()) - self.assertIsNone(controller.manual_turn_inputs) - - -class TestAngleSnapshotParity(unittest.TestCase): - def test_angle_delay_reaches_pinned_bp7_without_changing_curvature_snapshot(self): - import openpilot.cereal.messaging as messaging - from openpilot.selfdrive.modeld.constants import ModelConstants - from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs - from opendbc.bluepilot_lateral.tests import reference - from dataclasses import replace - - now = 10_000_000_000 - model = messaging.new_message('modelV2') - model.modelV2.orientationRate.z = [5. * (.001 + .004 * value) for value in ModelConstants.T_IDXS] - delay = messaging.new_message('lateralDelay') - class Reader: - alive = {'modelV2': True, 'lateralDelay': True} - valid = dict(alive) - logMonoTime = {'modelV2': now, 'lateralDelay': now} - def __getitem__(self, key): - return model.modelV2 if key == 'modelV2' else delay.lateralDelay - provider = ManualTurnInputs.__new__(ManualTurnInputs) - vars(provider)['sm'] = Reader() - provider.enabled = True - with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now): - for value in (.05, .10, .125, .15, .30): - delay.lateralDelay.lateralDelay = value - snapshot = provider.lateral_snapshot(5., angle=True) - self.assertIsNotNone(snapshot) - self.assertAlmostEqual(snapshot[2], min(.15, max(.10, value))) - self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], min(.4, max(.2, value))) - adapted, upstream = AngleController(low_gain=1., high_gain=1.05), reference.Angle(CAR.FORD_MUSTANG_MACH_E_MK1) - for _ in range(20): - common = Inputs(active=True, speed=5., measured_curvature=.001, desired_curvature=.001, - yaw_predictions=tuple(snapshot[0].orientationRate.z), time_indices=tuple(snapshot[1]), lateral_delay=snapshot[2]) - command = adapted.step(common) - expected = upstream.step(replace(common, lateral_delay=delay.lateralDelay.lateralDelay)) - self.assertTrue(command.active) - self.assertAlmostEqual(command.path_angle, expected.path_angle, places=12) - provider.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001 - self.assertAlmostEqual(provider.lateral_snapshot(5., angle=True)[2], .15) - self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], .2) - for stamp in (0, now + 1, now - 100_000_001): - provider.sm.logMonoTime['modelV2'] = stamp - self.assertIsNone(provider.lateral_snapshot(5., angle=True)) - self.assertIsNone(provider.lateral_snapshot(5.)) diff --git a/openpilot/starpilot/tests/test_ford_angle.py b/openpilot/starpilot/tests/test_ford_angle.py deleted file mode 100644 index deace723f5..0000000000 --- a/openpilot/starpilot/tests/test_ford_angle.py +++ /dev/null @@ -1,543 +0,0 @@ -"""Actual saved owner/factory/Card/Controls/parser/sender/native qualification. - -These tests must run with the runner-selected DEBUG and RELEASE safety library. -No authority, measurement, desired-history or angle-ready test setters are used. -""" -import os -import ctypes -import hashlib -import shutil -import tempfile -from pathlib import Path -import unittest -from unittest.mock import patch -from contextlib import ExitStack -from types import SimpleNamespace - -from openpilot.cereal import messaging -from openpilot.common.params import Params, ParamKeyType -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.controls.controlsd import Controls -from openpilot.starpilot.tests import test_vehicle_preferences as startup -from openpilot.starpilot.lateral.tests import test_lane_runtime as lane -from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences -from opendbc.car import gen_empty_fingerprint, structs -from opendbc.car.ford.values import CAR, FordFlags, DBC -from opendbc.bluepilot_lateral.tests import reference -from openpilot.starpilot.controller_extensions import configure_controller -from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, qualified, select -from opendbc.safety.tests import test_ford_aol as physical -from opendbc.safety.tests.libsafety import libsafety_py - - -class TestFordAngleStartup(unittest.TestCase): - def setUp(self): - self.helper = startup.TestVehicleStartupPreferences() - self.helper.setUp() - self.addCleanup(self.helper.doCleanups) - self.params = self.helper.params - self.helper.raw("SafeMode", b"0") - self.helper.raw("AlwaysOnLateral", b"0") - self.params.put_bool("AlphaLongitudinalEnabled", False, block=True) - self.params.put_bool("IsReleaseBranch", False, block=True) - - def start(self, identity, requested=True, *, auxiliary=False, **kwargs): - observed = gen_empty_fingerprint() - offset = 4 if auxiliary else 0 - observed[offset][0x5A] = 8 - observed[offset + 2].update({0x3D6: 8, 0x186: 8}) - return self.helper.start(identity, key="FordLateralMode", requested=requested, observed=observed, - capture=lambda ci: (int(ci.CP.safetyConfigs[-1].safetyParam), ci.CC.bp_lat is not None), **kwargs) - - def test_actual_factory_card_saved_choice_before_controller_all_eleven(self): - for identity in sorted(CLASSIC | CANFD): - for requested in (False, True): - with self.subTest(identity=identity, selected=requested): - host, constructed, published = self.start(identity, requested) - baseline = 18 if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else 66 if identity in CANFD else 32 - word = 130 if identity in CANFD else 128 - self.assertEqual(constructed, [(word if requested else baseline, requested)]) - self.assertEqual(published.safetyConfigs[-1].safetyParam, constructed[0][0]) - self.assertEqual(qualified(published), requested) - self.assertEqual(host.CI.CP.to_dict(), published.to_dict()) - if requested: - self.assertIsNotNone(host.CI.CC.bp_lat.host.inputs) - self.assertAlmostEqual(published.steerActuatorDelay, .22, places=6) - self.assertEqual(self.params.get("FordLateralMode"), int(requested)) - self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"1" if requested else b"0") - - def test_absent_corrupt_dependencies_and_typed_value_fail_closed(self): - self.assertEqual(self.params.get_type("FordLateralMode"), ParamKeyType.INT) - self.assertEqual(self.params.get_default_value("FordLateralMode"), 0) - for raw in (None, b"0", b"", b"1\n", b"true", b"2", b"1" * 20, b"1"): - self.helper.raw("FordLateralMode", raw) - selected = VehicleStartupPreferences.read(self.params, enabled=True) - self.assertEqual(selected.ford_lateral_mode, int(raw == b"1")) - self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes() if raw is not None else None, raw) - from opendbc.car.ford.tests.test_three_ports import params - for raw in (1, b"1", "1", True, 1.0, None, "01"): - cp = params(CAR.FORD_BRONCO_SPORT_MK1) - self.assertEqual(select(cp, raw), type(raw) is int or type(raw) in (bytes, str) and raw in (b"1", "1")) - for key in ("SafeMode", "AlwaysOnLateral"): - for raw in (b"1", b"", b"false", b"0\n"): - self.helper.raw(key, raw) - self.assertEqual(VehicleStartupPreferences.read(self.params, enabled=True).ford_lateral_mode, - int(key == "AlwaysOnLateral" and raw == b"1")) - self.helper.raw(key, b"0") - self.assertEqual(VehicleStartupPreferences.read(self.params, enabled=False).ford_lateral_mode, 0) - - def test_saved_selection_excluded_profiles_and_final_longitudinal_owner(self): - for identity in (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5, CAR.FORD_TRANSIT_MK5): - host, constructed, published = self.start(identity) - self.assertFalse(qualified(published)) - self.assertFalse(constructed[0][1]) - self.assertIsNone(host.CI.CC.bp_lat) - self.assertEqual(self.params.get("FordLateralMode"), 1) - for identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MUSTANG_MACH_E_MK1): - for release in (False, True): - self.params.put_bool("AlphaLongitudinalEnabled", True, block=True) - self.params.put_bool("IsReleaseBranch", release, block=True) - host, constructed, published = self.start(identity) - word = (130 if identity in CANFD else 128) + int(published.openpilotLongitudinalControl) - self.assertEqual(constructed, [(word, True)]) - self.assertEqual(published.safetyConfigs[-1].safetyParam, word) - self.assertEqual(published.openpilotLongitudinalControl, not release) - self.assertTrue(published.pcmCruise) - self.assertEqual(published.alternativeExperience, 0) - self.assertTrue(qualified(published)) - self.assertIsNotNone(host.CI.CC.bp_lat) - self.assertEqual(self.params.get("FordLateralMode"), 1) - - def test_actual_factory_alpha_long_angle_admission_and_exact_config_denials(self): - from opendbc.car.ford.interface import CarInterface - for identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MUSTANG_MACH_E_MK1): - for alpha in (False, True): - for release in (False, True): - with self.subTest(identity=identity, alpha=alpha, release=release): - observed = gen_empty_fingerprint() - observed[0][0x5A] = 8 - observed[2].update({0x3D6: 8, 0x186: 8}) - cp = CarInterface.get_params(identity, observed, [], alpha, release, False) - long = alpha - self.assertEqual(cp.openpilotLongitudinalControl, long) - self.assertTrue(cp.pcmCruise) - self.assertEqual(cp.alternativeExperience, 0) - self.assertEqual(cp.flags, int(FordFlags.CANFD) if identity in CANFD else 0) - self.assertEqual(cp.safetyConfigs[-1].safetyParam, (18 if identity in CANFD else 32) + int(long)) - self.assertTrue(select(cp, 1)) - self.assertEqual(cp.safetyConfigs[-1].safetyParam, (130 if identity in CANFD else 128) + int(long)) - self.assertTrue(qualified(cp)) - for field, value in (("pcmCruise", False), ("alternativeExperience", 32), ("passive", True), - ("dashcamOnly", True), ("notCar", True), ("brand", "hyundai"), - ("flags", 128), ("steerControlType", structs.CarParams.SteerControlType.torque)): - invalid = cp.as_reader().as_builder() - setattr(invalid, field, value) - self.assertFalse(qualified(invalid), field) - for field, value in (("safetyModel", structs.CarParams.SafetyModel.noOutput), - ("safetyParam", cp.safetyConfigs[-1].safetyParam ^ 1)): - invalid = cp.as_reader().as_builder() - setattr(invalid.safetyConfigs[-1], field, value) - self.assertFalse(qualified(invalid), field) - - - -class TestFordAngleJoined(unittest.TestCase): - def test_saved_card_controls_parser_and_actual_native_trace_both_signs(self): - safety = libsafety_py.libsafety - native_debug = safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) == 0 - safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) - sources = ("BrakeSysFeatures", "EngVehicleSpThrottle2", "Yaw_Data_FD1", "EngBrakeData", - "EngVehicleSpThrottle", "DesiredTorqBrk", "Steering_Data_FD1") - faults = [("crc", source) for source in (sources[0], sources[2])] + \ - [("quality", source) for source in sources[:3]] + \ - [("counter", source) for source in (sources[0], sources[2])] + \ - [("freshness", source) for source in sources] + \ - [(name, None) for name in ("gas", "brake", "cruise", "relay", "buttons", "panda_stale", - "panda_model", "panda_param", "panda_ae", "panda_topology", - "panda_heartbeat", "panda_fault_status", "panda_controls")] - for identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_EXPLORER_MK6, - CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_F_150_MK14): - scenarios = [("angle", None), ("default0", None)] - if identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MUSTANG_MACH_E_MK1): - scenarios += [("aux", None)] + faults + [("alpha", None), ("alpha_cruise", None), ("alpha_buttons", None)] - for scenario, source in scenarios: - alpha_requested = scenario.startswith("alpha") - kind = "angle" if scenario == "alpha" else scenario.removeprefix("alpha_") - for direction in (-1, 1): - with self.subTest(identity=identity, direction=direction, kind=scenario, source=source), OpenpilotPrefix(), \ - patch.dict(os.environ, {"REPLAY": "1", "SIMULATION": "1"}), \ - patch("openpilot.selfdrive.controls.controlsd.messaging.PubMaster"): - params = Params() - helper = TestFordAngleStartup() - helper.setUp() - auxiliary = None - auxiliary_directory = None - try: - helper.params = helper.helper.params = params - for key, value in (("SafeMode", False), ("AlwaysOnLateral", False), ("AlphaLongitudinalEnabled", alpha_requested), - ("IsReleaseBranch", alpha_requested and not native_debug), ("OpenpilotEnabledToggle", True)): - params.put_bool(key, value, block=True) - selected = kind != "default0" - host, _, cp = helper.start(identity, selected, auxiliary=kind == "aux") - offset = 4 if kind == "aux" else 0 - self.assertEqual(qualified(cp), selected) - alpha_active = alpha_requested and native_debug - self.assertEqual(cp.openpilotLongitudinalControl, alpha_active) - self.assertTrue(cp.pcmCruise) - if alpha_requested: - self.assertEqual(cp.safetyConfigs[-1].safetyParam, (130 if identity in CANFD else 128) + int(alpha_active)) - self.assertTrue(params.get_bool("AlphaLongitudinalEnabled")) - controls = Controls() - native = physical.TestFordAolDriverIntent() - native.setUp() - native.word = 66 if cp.flags & FordFlags.CANFD else 32 - native.tick = 0 - self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam), 0) - safety.init_tests() - safety.set_alternative_experience(0) - auxiliary = None - auxiliary_handle = None - if kind == "aux": - self.assertEqual(len(cp.safetyConfigs), 2) - self.assertEqual(cp.safetyConfigs[0].safetyModel, structs.CarParams.SafetyModel.noOutput) - self.assertEqual(cp.safetyConfigs[0].safetyParam, 0) - self.assertEqual(host.CI.CC.CAN.main, 4) - self.assertEqual(host.CI.CC.CAN.camera, 6) - # Copy the actual library selected by the public test runner, - # so the noOutput Panda has independent native static storage. - main_file = Path(libsafety_py.libpath) - auxiliary_directory = tempfile.TemporaryDirectory() - aux_file = Path(auxiliary_directory.name) / "libsafety-auxiliary.so" - shutil.copyfile(main_file, aux_file) - self.assertNotEqual(main_file.resolve(), aux_file.resolve()) - self.assertEqual(hashlib.sha256(main_file.read_bytes()).digest(), hashlib.sha256(aux_file.read_bytes()).digest()) - auxiliary = libsafety_py.ffi.dlopen(str(aux_file)) - auxiliary_handle = ctypes.CDLL(str(aux_file)) - self.assertEqual(auxiliary.set_safety_hooks(int(cp.safetyConfigs[0].safetyModel.raw), cp.safetyConfigs[0].safetyParam), 0) - auxiliary.init_tests() - auxiliary.set_alternative_experience(0) - self.assertFalse(auxiliary.get_controls_allowed()) - self.assertEqual(safety.get_current_safety_param(), cp.safetyConfigs[-1].safetyParam) - baseline = None - if not selected: - baseline = reference.curvature_controller()(DBC[identity], cp) - configure_controller(SimpleNamespace(CP=cp, CC=baseline), params) - providers = [] - received, sent, validity = [], [], [] - controller_returns = [] - observed_allowed = False - observed_relay = False - observed_health = False - path_seen, resumed, withdraw_seen = [], False, False - paired_cancel, paired_resume = False, False - alpha_button_cancelled = alpha_button_acc_off = False - alpha_cancel_event_seen = alpha_cancel_requested = alpha_physical_rearmed = False - tx_rejected, invalid_rx = 0, 0 - counter_rx: list[tuple[int, int, bool]] = [] - long_positive = long_recovered = long_withdrawn = False - - def bind(host, selected, baseline, providers, sent, validity): - providers.clear() - providers.append(host.CI.CC.bp_lat.host.inputs if selected else host.CI.CC.manual_turn_inputs) - if selected and host.CI.CC.manual_turn_inputs is not None: - providers.append(host.CI.CC.manual_turn_inputs) - if baseline is not None: - providers.append(baseline.manual_turn_inputs) - for owner in providers: - self.assertIsNotNone(owner) - services = ["modelV2", "lateralDelay"] + (["pandaStates"] if owner.track_assist_permission else []) - owner.sm = messaging.SubMaster(services) - host.sm = messaging.SubMaster(["carControl"]) - host.publish_sendcan = lambda frames, valid=True: (sent.extend(frames), validity.append(valid)) - host.ci_initialized = True - host.CI.update([]) - - bind(host, selected, baseline, providers, sent, validity) - for tick in range(340): - failing = 120 <= tick < 180 - recovery = 180 <= tick < 220 - received.clear() - received.extend(controller_returns) - controller_returns.clear() - - def rx(name, values, *, failing=failing, kind=kind, source=source, tick=tick, - received=received, offset=offset, native=native, counter_rx=counter_rx): - nonlocal invalid_rx - if failing and kind == "freshness" and name == source: - return - values = dict(values) - if failing and name == source: - if kind == "quality": - signal = {sources[0]: "VehVActlBrk_D_Qf", sources[1]: "VehVActlEng_D_Qf", sources[2]: "VehYawWActl_D_Qf"}[name] - values[signal] = 0 - elif kind == "counter": - values["VehVActlBrk_No_Cnt" if name == sources[0] else "VehRollYaw_No_Cnt"] = 0 - if kind == "counter" and name == source and tick >= 180: - # The recovered sender continues from its last transmitted fault counter, zero. - brake_counter = name == sources[0] - signal = "VehVActlBrk_No_Cnt" if brake_counter else "VehRollYaw_No_Cnt" - values[signal] = ((native.tick - 180) // (2 if brake_counter else 1)) % (16 if brake_counter else 256) - if failing and kind == "gas" and name == "EngVehicleSpThrottle": - values["ApedPos_Pc_ActlArb"] = 50 - if failing and kind == "brake" and name == "EngBrakeData": - values["BpedDrvAppl_D_Actl"] = 2 - if kind == "buttons" and name == "Steering_Data_FD1": - values["CcAslButtnCnclResPress"] = int(119 <= tick < 140 or 159 <= tick < 180) - frame = native.packer.make_can_msg(name, 0, values) - if failing and kind == "crc" and name == source: - addr, data, bus = frame - data = bytearray(data) - data[3 if name == sources[0] else 4] ^= 1 - frame = (addr, bytes(data), bus) - accepted = safety.safety_rx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])) - if kind == "counter" and name == source and tick >= 120: - counter = (frame[1][2] >> 2) & 0xF if name == sources[0] else frame[1][5] - counter_rx.append((tick, counter, bool(accepted))) - if not accepted: - invalid_rx += 1 - if not (failing and kind in ("crc", "quality", "counter") and name == source): - self.assertTrue(accepted, (kind, source, tick, name)) - received.append((frame[0], frame[1], frame[2] + offset)) - # Same bytes/order: Panda-local0 maps to its actual host bus4 in the auxiliary factory topology. - - main = 0 if tick < 30 or recovery and kind not in ("angle", "default0", "aux", "buttons", "panda_stale") else 4 - if failing and kind == "cruise": - main = 3 - if kind == "buttons": - main = 3 if 129 <= tick < 169 else main - if kind == "relay" and tick == 180: - # A relay fault is latched. Only a new actual factory/native session can recover. - host, _, replacement = helper.start(identity) - self.assertEqual(replacement.to_dict(), cp.to_dict()) - self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam), 0) - safety.init_tests() - safety.set_alternative_experience(0) - bind(host, selected, baseline, providers, sent, validity) - with patch.object(native, "rx", rx): - native.pump(1, main=main, speed=15.) - now = (1_000_000 + native.tick * 10_000) * 1000 - if kind == "relay" and tick == 120: - frame = native.packer.make_can_msg("LateralMotionControl2" if cp.flags & FordFlags.CANFD else "LateralMotionControl", 0, {}) - safety.safety_rx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1])) - received.append(frame) - self.assertTrue(safety.get_relay_malfunction()) - safety.safety_tick() - extra: tuple[tuple[str, int, dict[str, float]], ...] = (("SteeringPinion_Data", 0, {"StePinCompAnEst_D_Qf": 3, "StePinComp_An_Est": 0.0}), - ("Cluster_Info1_FD1", 0, {"AccEnbl_B_RqDrv": 1}), - ("BodyInfo_3_FD1", 0, {}), ("RCMStatusMessage2_FD1", 0, {"FirstRowBuckleDriver": 1}), - ("INSTRUMENT_PANEL", 0, {}), ("IPMA_Data", 2, {}), - ("ACCDATA", 2, {}), ("ACCDATA_2", 2, {}), ("ACCDATA_3", 2, {})) - received.extend(native.packer.make_can_msg(name, bus + offset, values) for name, bus, values in extra) - state = host.CI.update([(now, list(received))]) - self.assertEqual(state.steeringAngleDeg, 0.0) - if alpha_active and kind == "buttons": - # This component feed consumes the actual parsed Cancel event; the full SD loop is qualified separately. - if any(event.type == structs.CarState.ButtonEvent.Type.cancel and event.pressed for event in state.buttonEvents): - alpha_button_cancelled = alpha_cancel_event_seen = True - if alpha_button_cancelled and not state.cruiseState.enabled: - alpha_button_acc_off = True - elif alpha_button_acc_off and state.cruiseState.enabled: - alpha_button_cancelled = alpha_button_acc_off = False - alpha_physical_rearmed = True - if tick < 40: - continue - if kind in ("angle", "default0", "aux"): - self.assertTrue(state.canValid and not state.canTimeout) - self.assertTrue(safety.get_controls_allowed()) - with ExitStack() as stack: - stack.enter_context(patch("openpilot.starpilot.controller_extensions.time.monotonic_ns", return_value=now)) - stack.enter_context(patch("openpilot.starpilot.controller_extensions.time.clock_gettime_ns", return_value=now)) - stack.enter_context(patch("openpilot.selfdrive.car.card.time.monotonic", return_value=now / 1e9)) - for owner in providers: - stack.enter_context(patch.object(owner.sm, "update", return_value=None)) - events = [] - if tick % 10 == 0 and not (failing and kind == "panda_stale"): - pandas = messaging.new_message("pandaStates", len(cp.safetyConfigs) + int(failing and kind == "panda_topology"), - valid=True, logMonoTime=now) - for i, _config in enumerate(cp.safetyConfigs): - p = pandas.pandaStates[i] - owner = safety if i == len(cp.safetyConfigs) - 1 else auxiliary - self.assertIsNotNone(owner) - p.safetyModel, p.safetyParam = owner.get_current_safety_mode(), owner.get_current_safety_param() - p.alternativeExperience = cp.alternativeExperience - p.controlsAllowed = owner.get_controls_allowed() - if owner is safety: - p.safetyRxChecksInvalid = not safety.safety_config_valid() - else: - assert auxiliary_handle is not None - p.safetyRxChecksInvalid = ctypes.c_bool.in_dll(auxiliary_handle, "safety_rx_checks_invalid").value - p.faultStatus = "faultPerm" if owner.get_relay_malfunction() else "none" - p.faults = ["relayMalfunction"] if owner.get_relay_malfunction() else [] - if failing: - p = pandas.pandaStates[len(cp.safetyConfigs) - 1] - if kind == "panda_model": - p.safetyModel = structs.CarParams.SafetyModel.noOutput - elif kind == "panda_param": - p.safetyParam = 130 if p.safetyParam == 128 else 128 - elif kind == "panda_ae": - p.alternativeExperience = 32 - elif kind == "panda_heartbeat": - p.heartbeatLost = True - elif kind == "panda_fault_status": - p.faultStatus = "faultTemp" - elif kind == "panda_controls": - p.controlsAllowed = False - observed_allowed = bool(pandas.pandaStates[len(cp.safetyConfigs) - 1].controlsAllowed) - observed_relay = safety.get_relay_malfunction() - observed_health = safety.safety_config_valid() - events.append(pandas.as_reader()) - # feed already publishes these services. Replace its synthetic - # state/model/delay before one update, preserving one sample per tick. - feed_events = [] - component_enabled = observed_allowed and not observed_relay and not alpha_button_cancelled - with patch.object(controls.sm, "update_msgs", side_effect=lambda _time, events, collector=feed_events: collector.extend(events)): - lane.feed(controls, now, tick, speed=15., active=component_enabled, enabled=component_enabled) - car_state = messaging.new_message("carState", valid=True, logMonoTime=now) - car_state.carState = state - preview = messaging.new_message("modelV2", valid=True, logMonoTime=now) - preview.modelV2 = lane.model() - preview.modelV2.action.desiredCurvature = direction * .0008 - preview.modelV2.orientationRate.z = [direction * .0008 * 15.] * 33 - delay = messaging.new_message("lateralDelay", valid=True, logMonoTime=now) - delay.lateralDelay.lateralDelay = .1 - controls.sm.update_msgs(now / 1e9, [ - *[event for event in feed_events if event.which() not in ("carState", "modelV2", "lateralDelay")], - car_state.as_reader(), preview.as_reader(), delay.as_reader()]) - for owner in providers: - owner.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader(), *events] if owner.track_assist_permission else - [preview.as_reader(), delay.as_reader()]) - command, controller_log = controls.state_control() - if kind in ("angle", "default0", "aux"): - self.assertTrue(command.enabled and command.latActive) - control_events = [] - def capture_control(service, event, collector=control_events): - if service == "carControl": - collector.append(messaging.log_from_bytes(event.to_bytes())) - with patch.object(controls.pm, "send", side_effect=capture_control): - controls.publish(command, controller_log) - self.assertEqual(len(control_events), 1) - self.assertEqual(control_events[0].carControl.to_dict(), command.to_dict()) - self.assertEqual(control_events[0].valid, state.canValid) - host.sm.update_msgs(now / 1e9, control_events) - command = host.sm["carControl"] - if alpha_active and kind == "buttons": - alpha_cancel_requested |= bool(command.cruiseControl.cancel) - host.can_log_mono_time = now - sent.clear() - validity.clear() - host.controls_update(state, command) - if baseline is not None: - _, expected = baseline.update(command, host.CI.CS, now) - self.assertEqual(sent, expected, (identity, direction, tick, "default0 current bytes")) - button_frames = [(bus, data) for addr, data, bus in sent if addr == 0x83] - if kind == "buttons" and button_frames: - self.assertEqual([bus for bus, _ in button_frames], [host.CI.CC.CAN.camera, host.CI.CC.CAN.main]) - paired_cancel |= any(data[1] & 1 for _, data in button_frames) - paired_resume |= any(data[3] & 2 for _, data in button_frames) - for addr, data, bus in sent: - if addr == 0x186: - self.assertTrue(alpha_active) - self.assertEqual(bus, offset) - accepted_long = bool(safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus - offset, data))) - gas = ((data[6] & 3) << 8) | data[7] - accel = ((data[0] & 31) << 8) | data[1] - gas_pred = ((data[2] & 3) << 8) | data[3] - if accepted_long and gas > 500: - self.assertTrue(command.longActive and safety.get_controls_allowed() and - safety.safety_config_valid() and not safety.get_relay_malfunction()) - self.assertGreater(command.actuators.accel, 0) - self.assertGreater(accel * .0039 - 20., 0) - self.assertLessEqual(gas, 700) - self.assertTrue(4231 <= accel <= 5641) - self.assertEqual(gas_pred, 0) - self.assertAlmostEqual(gas * .01 - 5., host.CI.CC.gas, delta=.01) - self.assertAlmostEqual(accel * .0039 - 20., host.CI.CC.accel, delta=.0039) - long_positive = True - long_recovered |= tick >= 240 - if 140 <= tick < (160 if kind == "buttons" else 180) and kind in ("cruise", "buttons"): - self.assertFalse(accepted_long and gas > 0) - long_withdrawn = True - if not accepted_long: - self.assertTrue(120 <= tick < 230 and - (not safety.get_controls_allowed() or not safety.safety_config_valid() or - safety.get_relay_malfunction())) - continue - if addr not in (0x83, 0x3CA, 0x3D3, 0x3D6): - continue - packet = libsafety_py.make_CANPacket(addr, bus - offset, data) - if auxiliary is not None: - # The other actual configured Panda has no steering/longitudinal TX authority. - self.assertFalse(auxiliary.safety_tx_hook(packet)) - self.assertFalse(auxiliary.get_controls_allowed()) - accepted = bool(safety.safety_tx_hook(packet)) - if addr in (0x3CA, 0x3D3, 0x3D6): - controller_returns.append((addr, bytes(data), bus | (0x80 if accepted else 0xC0))) - if addr == 0x83: - # Native retains its physical-context rules; a held redundant cancel after ACC-off is denied. - expected = not safety.get_relay_malfunction() and not (data[1] & 1 and not safety.get_cruise_engaged_prev()) - self.assertEqual(accepted, expected) - continue - fd = addr == 0x3D6 - mode = (data[0] >> 4) & 7 if fd else ((data[4] >> 2) & 7 if addr == 0x3D3 else data[0] >> 5) - if selected and addr in (0x3D3, 0x3D6): - bad_ack = failing and kind.startswith("panda_") and kind != "panda_stale" - if not observed_allowed or observed_relay or not observed_health or bad_ack or kind == "panda_stale" and 151 <= tick < 180: - self.assertEqual(mode, 0) - withdraw_seen |= kind not in ("angle", "default0", "aux") - if mode: - path = (((data[3] & 31) << 6) | (data[4] >> 2)) - 1000 if fd else ((data[3] << 3) | (data[4] >> 5)) - 1000 - if path: - path_seen.append(path) - resumed |= tick >= 240 - if not accepted: - tx_rejected += 1 - # During actual faults native blocks immediately; the10Hz public acknowledgment can lag <=one period. - self.assertNotIn(kind, ("angle", "default0", "aux", "panda_stale", "buttons")) - self.assertTrue(120 <= tick < 230 and (not safety.get_controls_allowed() or not safety.safety_config_valid() or - safety.get_relay_malfunction())) - elif selected and addr in (0x3D3, 0x3D6) and mode: - self.assertTrue(safety.get_controls_allowed() and safety.safety_config_valid() and not safety.get_relay_malfunction()) - if tick >= 260: - self.assertTrue(safety.get_controls_allowed() and safety.safety_config_valid() and not safety.get_relay_malfunction()) - if alpha_active: - self.assertTrue(long_positive and long_recovered, "Actual Controls/sender/native Alpha Long never accepted positive acceleration/recovery") - if kind in ("cruise", "buttons"): - self.assertTrue(long_withdrawn, "Physical ACC withdrawal did not block native active longitudinal output") - if selected: - self.assertTrue(path_seen, "Production sender never produced an active signed angle") - self.assertTrue(all(value * direction < 0 for value in path_seen), - (identity, scenario, source, direction, min(path_seen), max(path_seen), tuple(path_seen[:8]))) - self.assertTrue(resumed, "Actual factory/physical cruise rearm did not resume accepted angle") - if kind not in ("angle", "default0", "aux"): - self.assertTrue(withdraw_seen, "Fault did not withdraw production angle") - if kind == "counter": - cadence = 2 if source == sources[0] else 1 - modulus = 16 if source == sources[0] else 256 - fault_ticks = range(120 + cadence - 1, 180, cadence) - recovery_ticks = range(180 + cadence - 1, 340, cadence) - # Five consecutive wrong counters reject RX; every recovered sequential sample is accepted. - self.assertEqual(counter_rx, - [(tick, 0, i < 4) for i, tick in enumerate(fault_ticks)] + - [(tick, (i + 1) % modulus, True) for i, tick in enumerate(recovery_ticks)], - (identity, direction, source)) - if kind in ("crc", "quality", "counter"): - self.assertGreater(invalid_rx, 0) - if kind == "buttons": - button_context = (identity, scenario, direction, cp.safetyConfigs[-1].safetyParam, - alpha_cancel_event_seen, alpha_cancel_requested, alpha_physical_rearmed, - paired_cancel, paired_resume) - if alpha_active: - self.assertTrue(alpha_cancel_event_seen and alpha_cancel_requested and alpha_physical_rearmed and paired_cancel, button_context) - self.assertFalse(paired_resume, button_context) - else: - self.assertTrue(paired_cancel and paired_resume, button_context) - self.assertEqual(Path(params.get_param_path("FordLateralMode")).read_bytes(), b"1" if selected else b"0") - finally: - helper.doCleanups() - safety.set_alternative_experience(0) - safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) - if auxiliary is not None: - auxiliary.set_alternative_experience(0) - auxiliary.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) - if auxiliary_directory is not None: - auxiliary_directory.cleanup() diff --git a/openpilot/starpilot/tests/test_ford_aol_intent.py b/openpilot/starpilot/tests/test_ford_aol_intent.py index 22316232f4..5e08dc3f18 100644 --- a/openpilot/starpilot/tests/test_ford_aol_intent.py +++ b/openpilot/starpilot/tests/test_ford_aol_intent.py @@ -92,15 +92,15 @@ class TestFordAolIntent(unittest.TestCase): flags=int(FordFlags.CANFD), pcmCruise=True, openpilotLongitudinalControl=True, alternativeExperience=32, steerControlType=structs.CarParams.SteerControlType.angle, transmissionType=structs.CarParams.TransmissionType.automatic, - safetyConfigs=[structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.ford, safetyParam=19)]) + safetyConfigs=[structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.ford, safetyParam=3)]) state = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive, buttonEvents=[structs.CarState.ButtonEvent(type=structs.CarState.ButtonEvent.Type.cancel, pressed=True)]) state.cruiseState.available = True settings = AolSettings(True, 0., AOL_TOGGLE, 0, (0, 0, 0), (0, 0, 0)) intent = IntentState('card', 1, now, now, now + 200_000_000, True, False, False, True, True) previous = AxisDecision(True, True, True, True, True) - slot = PandaSlot(0, 'panda', int(structs.CarParams.SafetyModel.ford), 19, 32, True, False) - native = SafetyState(1, True, now, now + 200_000_000, slot.safetyModel, 19, True, False, + slot = PandaSlot(0, 'panda', int(structs.CarParams.SafetyModel.ford), 3, 32, True, False) + native = SafetyState(1, True, now, now + 200_000_000, slot.safetyModel, 3, True, False, True, True, 'panda', 'axis', now, (slot,)) def allowed(**changes): args = {'settings': settings, 'previous': previous, 'source_ns': now, 'now_ns': now, 'axis_session_id': 'axis'} diff --git a/openpilot/starpilot/tests/test_ford_bluepilot_aol.py b/openpilot/starpilot/tests/test_ford_bluepilot_aol.py deleted file mode 100644 index 3d94a9935e..0000000000 --- a/openpilot/starpilot/tests/test_ford_bluepilot_aol.py +++ /dev/null @@ -1,450 +0,0 @@ -"""Real Ford factory/Card/SelfdriveD/Controls/native combined-owner lifecycle.""" -import pytest -from opendbc.car.ford.values import CAR -from openpilot.starpilot.tests.ford_lifecycle_helpers import ford_joined as ford_joined, run_actual_producer - -@pytest.fixture(autouse=True) -def combined_angle(monkeypatch, topology): - monkeypatch.ford_angle = True - monkeypatch.ford_topology = topology - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1,18), (CAR.FORD_ESCAPE_MK4,32), (CAR.FORD_F_150_MK14,66)]) -@pytest.mark.parametrize('alpha', [False,True]) -@pytest.mark.parametrize('hands_free', [False,True]) -@pytest.mark.parametrize('topology', ['automatic','manual-multi']) -def test_angle_aol_brake_reverse_fault_and_hands_free(ford_joined,monkeypatch): - assert ford_joined.host.CI.CC.bp_lat is not None - run_actual_producer(ford_joined,monkeypatch,scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1,18)]) -@pytest.mark.parametrize('alpha', [False,True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_mache_nonzero_angle_after_brake_with_hands_free(ford_joined,monkeypatch): - monkeypatch.ford_nonzero = True - run_actual_producer(ford_joined,monkeypatch,scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_tight_turn_speed_window_stays_inside_native_caps(ford_joined, monkeypatch): - # Route19's first blocked frames exceed Panda's cap by using the latest - # speed instead of its six-sample minimum. Exercise real parser/native windows. - monkeypatch.ford_nonzero = True - monkeypatch.ford_speed_window = True - monkeypatch.ford_test_curvature = .03 - run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_real_rejected_packet_resynchronizes_without_permission_bypass(ford_joined, monkeypatch): - from types import SimpleNamespace - from opendbc.car import structs - from opendbc.safety.tests.libsafety import libsafety_py - - joined = ford_joined - native, safety = joined.native, joined.safety - native.arm() - adapter = joined.host.CI.CC.bp_lat - inputs = adapter.host.inputs - blocked = 0 - monkeypatch.setattr(inputs, 'update', lambda: None) - monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed, *, angle=False: None) - # Isolate the adapter recovery while preserving actual physical native authority. - # The full caller matrix above retains real wire/inventory/freshness qualification. - monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1)) - CC = structs.CarControl(enabled=True, latActive=True) - CC.actuators.curvature = .0001 - state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive) - state.cruiseState.available = True - CS = SimpleNamespace(out=state, lat_ctl_lim_stat=0) - rejected = recovered = False - neutral_after_rejection = False - for frame in range(180): - native.pump(1, mask=1, speed=15.) - now = (1_000_000 + native.tick * 10_000) * 1000 - from openpilot.cereal import messaging - panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now) - panda.pandaStates[0].safetyTxBlocked = blocked - inputs.sm.update_msgs(now / 1e9, [panda.as_reader()]) - packets = [] - if frame % 5 == 0: - packets += adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, frame, now) - if frame % 3 == 0: - packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now) - for address, original, bus in packets: - data = bytearray(original) - active = address == 0x3D6 and (data[0] >> 4) & 7 == 1 - if active and frame >= 80 and not rejected: - data[1] ^= 1 # One actual bad checksum; native must reject it. - assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) - blocked += 1 - # Feed the real rejected CAN return, not just the aggregate counter. - adapter.observe_can([(now, [(address, bytes(data), bus | 0xC0)])], bus) - rejected = True - continue - accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) - assert accepted, (frame, hex(address), data.hex()) - if address == 0x3CA: - adapter.observe_can([(now + 1, [(address, bytes(data), bus | 0x80)])], bus) - if rejected and address == 0x3D6: - neutral_after_rejection |= not active - recovered |= active and neutral_after_rejection - assert rejected and neutral_after_rejection and recovered - # Host recovery never replaces native authorization. - safety.aol_set_host_request(0) - neutral_seen = False - for frame in range(180, 195): - native.pump(1, mask=0, speed=15.) - now = (1_000_000 + native.tick * 10_000) * 1000 - packets = adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, frame, now) - packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now) - for address, data, bus in packets: - if address == 0x3D6: - assert (data[0] >> 4) & 7 == 0 - neutral_seen = True - assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) - if address == 0x3CA: - adapter.observe_can([(now + 1, [(address, bytes(data), bus | 0x80)])], bus) - assert neutral_seen - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_native_first_pedal_preserves_only_granted_lateral(ford_joined, monkeypatch): - monkeypatch.ford_nonzero = True - monkeypatch.ford_native_first_pedal = True - run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_early_pedal_admission_requires_exact_marked_angle_long_profile(ford_joined): - from opendbc.car import structs - from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported - cp = ford_joined.host.CP - assert early_native_long_withdrawal_supported(cp) - for field, value in (('alternativeExperience', 0), ('openpilotLongitudinalControl', False), - ('passive', True), ('dashcamOnly', True), ('brand', 'hyundai')): - changed = structs.CarParams(**cp.to_dict()) - setattr(changed, field, value) - assert not early_native_long_withdrawal_supported(changed) - for mask in (128, 1, 16): - changed = structs.CarParams(**cp.to_dict()) - changed.safetyConfigs[-1].safetyParam ^= mask - assert not early_native_long_withdrawal_supported(changed) - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_temporary_eps_recovery_with_gas_preserves_only_lateral_intent(ford_joined, monkeypatch): - # Route3b: cruise is off, gas remains pressed through a 150 ms EPS pulse. - # The actual native hook must still reject active output during that pulse. - monkeypatch.ford_nonzero = True - monkeypatch.ford_eps_gas = True - run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [False]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -@pytest.mark.parametrize('delivery', ['paired', 'axis-first', 'event-first', 'never-paired']) -def test_route45_stock_angle_eps_gas_release_join(ford_joined, monkeypatch, delivery): - monkeypatch.ford_nonzero = True - monkeypatch.ford_route45_delivery = delivery - run_actual_producer(ford_joined, monkeypatch, scenario='route45') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [False]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -@pytest.mark.parametrize('refusal', ['permanent-eps', 'main-off', 'cancel']) -def test_route45_eps_join_keeps_physical_refusals(ford_joined, monkeypatch, refusal): - monkeypatch.ford_nonzero = True - monkeypatch.ford_route45_delivery = 'axis-first' - monkeypatch.ford_route45_refusal = refusal - run_actual_producer(ford_joined, monkeypatch, scenario='route45') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_route22_gas_held_first_engagement_keeps_aol_through_braking(ford_joined, monkeypatch): - from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback - from opendbc.car import structs - - class Changed: - def __init__(self, source, **values): - self.source = source - self.__dict__.update(values) - - def __getattr__(self, name): - return getattr(self.source, name) - - checked = [] - original = TransportPauseFeedback._first_ford_gas_lateral_grant - - def check_first_grant(feedback, sm, cp, cs, axis, now_ns, *, latched, producer): - result = original(feedback, sm, cp, cs, axis, now_ns, latched=latched, producer=producer) - if result is not None and not checked: - def refuses(*, state=cs, proof=axis, armed=latched, owner=producer, params=cp): - assert original(feedback, sm, params, state, proof, now_ns, latched=armed, producer=owner) is None - for field, value in [('gasPressed', False), ('brakePressed', True), ('regenBraking', True), - ('canValid', False), ('canTimeout', True), ('steerFaultTemporary', True), - ('steerFaultPermanent', True), ('accFaulted', True), ('doorOpen', True), - ('seatbeltUnlatched', True), ('vehicleSensorsInvalid', True), - ('steeringDisengage', True), ('gearShifter', structs.CarState.GearShifter.reverse)]: - refuses(state=Changed(cs, **{field: value})) - for field, value in [('intentProducerSessionId', 'other'), ('intentIntegrityEpoch', axis.intentIntegrityEpoch + 1), - ('sessionId', 'other'), ('nativeAcknowledged', False), ('lateralActive', False), - ('desiredLateral', False), ('longitudinalActive', True), ('desiredLongitudinal', False), - ('faultReason', 'controlsMismatch'), ('mode', 'off'), - ('sourceCarStateMonoTime', now_ns - 30_000_001), ('validUntilMonoTime', now_ns - 1)]: - refuses(proof=Changed(axis, **{field: value})) - refuses(armed=False) - refuses(owner='other') - refuses(params=Changed(cp, alternativeExperience=0)) - for field, value in [('failed', True), ('paused', True), ('pending_event', True), - ('healthy_inventory', ('other', ())), ('lateral_baseline_ns', now_ns)]: - with monkeypatch.context() as local: - local.setattr(feedback, field, value) - refuses() - checked.append(True) - return result - - monkeypatch.setattr(TransportPauseFeedback, '_first_ford_gas_lateral_grant', check_first_grant) - monkeypatch.ford_nonzero = True - run_actual_producer(ford_joined, monkeypatch, scenario='route22') - assert checked - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -@pytest.mark.parametrize('fault', ['rx-quality', 'source-loss']) -def test_route22_native_fault_still_withdraws_aol(ford_joined, monkeypatch, fault): - monkeypatch.ford_nonzero = True - monkeypatch.ford_route22_final_fault = fault - run_actual_producer(ford_joined, monkeypatch, scenario='route22') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_F_150_LIGHTNING_MK1, 66), (CAR.FORD_ESCAPE_MK4_5, 66), - (CAR.FORD_EXPEDITION_MK4, 66), (CAR.FORD_F_150_MK14, 66), (CAR.FORD_RANGER_MK2, 66)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_lightning_cluster_startup_and_aol_recovery(ford_joined, monkeypatch): - monkeypatch.ford_nonzero = True - run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_F_150_LIGHTNING_MK1, 66)]) -@pytest.mark.parametrize('alpha', [False]) -@pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('topology', ['automatic']) -@pytest.mark.parametrize('armed', [False, True]) -def test_lightning_unarmed_startup_does_not_show_armed_eps_warning(ford_joined, monkeypatch, armed): - monkeypatch.ford_startup_armed = armed - run_actual_producer(ford_joined, monkeypatch, scenario='startup-warning') - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -@pytest.mark.parametrize('can_clock_offset_ns', [0, 9_000_000_000]) -def test_angle_transport_batching_keeps_native_spacing(ford_joined, monkeypatch, can_clock_offset_ns): - from opendbc.car import structs - from opendbc.safety.tests.libsafety import libsafety_py - from openpilot.cereal import messaging - - joined = ford_joined - native, safety, ci = joined.native, joined.safety, joined.host.CI - native.arm() - adapter = ci.CC.bp_lat - inputs = adapter.host.inputs - monkeypatch.setattr(inputs, 'update', lambda: None) - monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed, *, angle=False: None) - monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1)) - control = structs.CarControl(enabled=True, latActive=True) - control.actuators.curvature = .0001 - state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive) - state.cruiseState.available = True - queue, echoes, rejected = [], [], [] - active_count = 0 - control_address = 0x3D6 if adapter.canfd else 0x3D3 - last_delivery = 0 - for frame in range(300): - now_us = 1_000_000 + (native.tick + 1) * 10_000 - while queue and queue[0][0] <= now_us: - delivered, address, data, bus = queue.pop(0) - safety.set_timer(delivered) - accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)) - if not accepted: - rejected.append((frame, hex(address), data.hex())) - elif address == control_address: - active_count += int(((data[0] >> 4) & 7) == 1) if adapter.canfd else int(bool(data[4] & 4)) - echoes.append((delivered * 1000 + can_clock_offset_ns, [(address, data, bus | (0x80 if accepted else 0xC0))])) - native.pump(1, mask=1, speed=15.) - # Exercise the actual interface-to-controller feedback path, including Panda - # bus offsets. Only echoes produced by real native TX acceptance count. - ci.update(echoes) - echoes.clear() - ci.CS.out = state - now_ns = now_us * 1000 - panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now_ns) - panda.pandaStates[0].safetyTxBlocked = len(rejected) - inputs.sm.update_msgs(now_ns / 1e9, [panda.as_reader()]) - _, packets = ci.CC.update(control.as_reader(), ci.CS, now_ns) - for address, data, bus in packets: - if address not in (control_address, 0x3CA): - continue - # A delayed FIFO batch followed by an immediate one compresses a nominal - # 50/30ms send cadence below Panda's unchanged 40/20ms minima. - delivered = max(last_delivery, now_us + (25_000 if frame % 10 == 5 else 0)) - last_delivery = delivered - queue.append((delivered, address, data, bus)) - assert not rejected, rejected[:8] - assert active_count > 25 - safety.aol_set_host_request(0) - native.pump(10, mask=0, speed=15.) - ci.CS.out = state - now_ns = (1_000_000 + native.tick * 10_000) * 1000 - packets = adapter.step_lateral(control, ci.CS, native.packer, ci.CC.CAN, 310, now_ns) - assert packets and safety.aol_get_permission_mask() == 0 - for address, data, bus in packets: - assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)) - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_F_150_MK14, 66)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_stock_forwarding_returns_cannot_starve_angle_reengagement(ford_joined, monkeypatch): - from opendbc.car import structs - from opendbc.car.ford import fordcan - from opendbc.safety.tests.libsafety import libsafety_py - from openpilot.cereal import messaging - - joined = ford_joined - native, safety, ci = joined.native, joined.safety, joined.host.CI - native.arm() - adapter = ci.CC.bp_lat - inputs = adapter.host.inputs - monkeypatch.setattr(inputs, 'update', lambda: None) - monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed, *, angle=False: None) - monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1)) - command = structs.CarControl(enabled=True, latActive=True) - command.actuators.curvature = .0001 - state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive) - state.cruiseState.available = True - echoes = [] - active = [0, 0] - forwarded = 0 - neutral_seen = False - # Genuine camera frames are forwarded only while the real native hook permits - # it. Those forwarded TX returns share the src|0x80 namespace with host TX. - for frame in range(300): - native.pump(1, mask=1, speed=15.) - now_ns = (1_000_000 + native.tick * 10_000) * 1000 - if neutral_seen and frame % 5 == 1: - stock = fordcan.create_lat_ctl2_msg(native.packer, ci.CC.CAN, 0, 0., 0., 0., 0., (frame // 5) % 16) - if safety.safety_fwd_hook(2, stock[0]) == 0: - echoes.append((now_ns, [(stock[0], stock[1], ci.CC.CAN.main | 0x80)])) - forwarded += 1 - ci.update(echoes) - echoes.clear() - ci.CS.out = state - # Preserve native permission while exercising one normal host neutral pause. - # Continued forwarding must not prevent a later real active command. - command.latActive = not 80 <= frame < 130 - panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now_ns) - panda.pandaStates[0].safetyTxBlocked = 0 - inputs.sm.update_msgs(now_ns / 1e9, [panda.as_reader()]) - _, packets = ci.CC.update(command.as_reader(), ci.CS, now_ns) - for address, data, bus in packets: - assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)) - echoes.append((now_ns + 1, [(address, data, bus | 0x80)])) - if address == 0x3D6: - is_active = ((data[0] >> 4) & 7) == 1 - if frame < 80: - active[0] += int(is_active) - elif frame < 130: - neutral_seen |= not is_active - else: - active[1] += int(is_active) - assert forwarded > 2 - assert neutral_seen - assert active[0] > 3 and active[1] > 10, active - assert safety.aol_get_permission_mask() == 1 - - -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_unrelated_native_rejection_does_not_reset_granted_angle(ford_joined, monkeypatch): - from types import SimpleNamespace - from opendbc.car import structs - from opendbc.car.ford import fordcan - from opendbc.safety.tests.libsafety import libsafety_py - from openpilot.cereal import messaging - - joined = ford_joined - native, safety = joined.native, joined.safety - native.arm() - adapter = joined.host.CI.CC.bp_lat - inputs = adapter.host.inputs - monkeypatch.setattr(inputs, 'update', lambda: None) - monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed, *, angle=False: None) - monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1)) - control = structs.CarControl(enabled=True, latActive=True) - control.actuators.curvature = .0001 - state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive) - state.cruiseState.available = True - source = SimpleNamespace(out=state, lat_ctl_lim_stat=0) - blocked = active_after = 0 - rejected = False - for frame in range(180): - native.pump(1, mask=1, speed=15.) - now = (1_000_000 + native.tick * 10_000) * 1000 - if frame == 80: - # LONG is independently denied. Its real rejection must not erase LAT. - message = fordcan.create_acc_msg(native.packer, joined.host.CI.CC.CAN, True, 1., 1., False, False, v_ego_kph=50.) - address, data, bus = message - assert not safety.get_controls_allowed() - assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)) - assert safety.aol_get_permission_mask() == 1 - adapter.observe_can([(now, [(address, data, bus | 0xC0)])], bus) - blocked += 1 - rejected = True - panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now) - panda.pandaStates[0].safetyTxBlocked = blocked - inputs.sm.update_msgs(now / 1e9, [panda.as_reader()]) - packets = adapter.step_lateral(control, source, native.packer, joined.host.CI.CC.CAN, frame, now) - packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now) - for address, data, bus in packets: - assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)), (frame, hex(address), data.hex()) - if address == 0x3CA: - adapter.observe_can([(now + 1, [(address, data, bus | 0x80)])], joined.host.CI.CC.CAN.main) - if address in (0x3D3, 0x3D6) and frame >= 80: - active = ((data[4] >> 2) & 7) if address == 0x3D3 else ((data[0] >> 4) & 7) - assert active == 1, 'Unrelated LONG rejection reset independently granted steering' - active_after += 1 - assert rejected and active_after > 10 diff --git a/openpilot/starpilot/tests/test_ford_ci_registration.py b/openpilot/starpilot/tests/test_ford_ci_registration.py index 2cf9aa7084..3a176d8bcf 100644 --- a/openpilot/starpilot/tests/test_ford_ci_registration.py +++ b/openpilot/starpilot/tests/test_ford_ci_registration.py @@ -4,8 +4,7 @@ from tools.ci import run_vehicle_tests as runner FAMILY_TARGETS = ( - 'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py', - 'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py', + 'opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py', 'openpilot/starpilot/tests/test_ford_configuration_parity.py', 'openpilot/starpilot/tests/test_ford_configuration_admission.py', 'openpilot/starpilot/tests/test_ford_release_long_preferences.py', @@ -23,12 +22,12 @@ def test_both_actual_ci_selectors_collect_all_qualified_ford_configuration_contr for path in FAMILY_TARGETS: assert path in selected assert (runner.ROOT / path).is_file() + # RELEASE CAN FD Alpha: the stock CAN FD long word runs its complete positive contract on the release library release = runner.targets_for('safety-release') - prefix = 'opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py::' - assert prefix + 'TestFordMachELongReleaseExtended' in release - assert prefix + 'TestFordMachELongReleaseDenied' not in release + prefix = 'opendbc_repo/opendbc/safety/tests/test_ford.py::' + assert prefix + 'TestFordCANFDLongitudinalSafety' in release source = ast.parse((runner.ROOT / prefix.split('::', 1)[0]).read_text()) - assert any(isinstance(node, ast.ClassDef) and node.name == 'TestFordMachELongReleaseExtended' for node in source.body) + assert any(isinstance(node, ast.ClassDef) and node.name == 'TestFordCANFDLongitudinalSafety' for node in source.body) def test_actual_host_collector_discovers_release_ui_guards(tmp_path): diff --git a/openpilot/starpilot/tests/test_ford_configuration_admission.py b/openpilot/starpilot/tests/test_ford_configuration_admission.py index 6cb2072d0c..875fdeae38 100644 --- a/openpilot/starpilot/tests/test_ford_configuration_admission.py +++ b/openpilot/starpilot/tests/test_ford_configuration_admission.py @@ -25,7 +25,11 @@ def test_exact_dual_owner_and_manual_identity(identity, base, alpha, manual): word = base | int(alpha) | (256 if manual else 0) assert cp.safetyConfigs[-1].safetyParam == word assert qualified(cp, marked_only=True) and native_matches_cp(cp, 6, word) - for field, value in [('safetyParam', word ^ 256), ('safetyParam', word | 512), ('safetyModel', structs.CarParams.SafetyModel.noOutput)]: + # bits 9-14 are the BluePilot lateral library's; bit 15 belongs to no one + bluepilot = cp.as_reader().as_builder() + bluepilot.safetyConfigs[-1].safetyParam = word | 0x4000 + assert qualified(bluepilot, marked_only=True) and native_matches_cp(bluepilot, 6, word | 0x4000) + for field, value in [('safetyParam', word ^ 256), ('safetyParam', word | 0x8000), ('safetyModel', structs.CarParams.SafetyModel.noOutput)]: bad = cp.as_reader().as_builder() setattr(bad.safetyConfigs[-1], field, value) assert not qualified(bad) and not native_matches_cp(bad, 6, word) @@ -53,13 +57,13 @@ def test_marked_manual_policy_survives_cards_alternative_experience_reset(): fp = gen_empty_fingerprint() cp = CarInterface.get_params(FORD_FACTORIES[0][0], fp, [], False, False, False) initial = policy_for(cp) - assert cp.safetyConfigs[-1].safetyParam == 32 and initial.safety_param_addition == 256 + assert cp.safetyConfigs[-1].safetyParam == 0 and initial.safety_param_addition == 256 cp.safetyConfigs[-1].safetyParam |= initial.safety_param_addition cp.alternativeExperience |= initial.alternative_experience_addition - assert native_matches_cp(cp, 6, 288) + assert native_matches_cp(cp, 6, 256) cp.alternativeExperience = 0 # Car resets this before reapplying saved settings. recovered = policy_for(cp) assert recovered.intent_supported and recovered.safety_param_addition == 256 cp.safetyConfigs[-1].safetyParam |= recovered.safety_param_addition cp.alternativeExperience |= recovered.alternative_experience_addition - assert native_matches_cp(cp, 6, 288) + assert native_matches_cp(cp, 6, 256) diff --git a/openpilot/starpilot/tests/test_ford_configuration_parity.py b/openpilot/starpilot/tests/test_ford_configuration_parity.py index 193b7fc21e..76007f5a20 100644 --- a/openpilot/starpilot/tests/test_ford_configuration_parity.py +++ b/openpilot/starpilot/tests/test_ford_configuration_parity.py @@ -9,17 +9,12 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, for @pytest.fixture(autouse=True) def configuration_topology(monkeypatch, topology, request): monkeypatch.ford_topology = topology - monkeypatch.ford_pinion = request.node.callspec.params.get('pinion', False) if request.node.name.startswith(('test_brake_retains_lateral_with_native_first_feedback', 'test_cancel_retains_lateral_with_native_first_feedback', 'test_gas_first_engagement_native_cadence', 'test_first_main_gas_native_cadence', 'test_native_rejected_ui_cannot_replace_stock_cluster')): monkeypatch.ford_native_10hz = True - monkeypatch.ford_angle = request.node.callspec.params['angle'] - if request.node.name.startswith('test_recorded_radar_creep_enters_native_stopping'): - monkeypatch.ford_angle = request.node.callspec.params['angle'] if request.node.name.startswith('test_route49_first_main_engagement'): monkeypatch.ford_first_main = True - monkeypatch.ford_angle = request.node.callspec.params['angle'] @pytest.mark.parametrize('identity', FORD_FACTORIES) @@ -34,26 +29,23 @@ def test_actual_configuration_lifecycle(ford_joined, monkeypatch): @pytest.mark.parametrize('alpha', [False]) @pytest.mark.parametrize('hands_free', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -@pytest.mark.parametrize('angle', [False, True]) -def test_route49_first_main_engagement_arms_before_cruise_cancel(ford_joined, monkeypatch, angle): +def test_route49_first_main_engagement_arms_before_cruise_cancel(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='route49') @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_brake_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle): +def test_brake_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='brake-native-cadence') @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [False, True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle): +def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence') @@ -61,8 +53,7 @@ def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypa @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -@pytest.mark.parametrize('angle', [False, True]) -def test_recorded_radar_creep_enters_native_stopping(ford_joined, angle): +def test_recorded_radar_creep_enters_native_stopping(ford_joined): import json import gzip from pathlib import Path @@ -124,18 +115,16 @@ def test_recorded_radar_creep_enters_native_stopping(ford_joined, angle): @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_gas_first_engagement_native_cadence(ford_joined, monkeypatch, angle): +def test_gas_first_engagement_native_cadence(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='route22') @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_first_main_gas_native_cadence(ford_joined, monkeypatch, angle): +def test_first_main_gas_native_cadence(ford_joined, monkeypatch): monkeypatch.ford_main_off_warmup = True run_actual_producer(ford_joined, monkeypatch, scenario='route22') @@ -143,9 +132,8 @@ def test_first_main_gas_native_cadence(ford_joined, monkeypatch, angle): @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_cancel_retains_lateral_with_native_first_feedback_while_gas_held(ford_joined, monkeypatch, angle, topology): +def test_cancel_retains_lateral_with_native_first_feedback_while_gas_held(ford_joined, monkeypatch, topology): monkeypatch.ford_native_10hz = True monkeypatch.ford_cancel_gas = True run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence') @@ -180,43 +168,10 @@ def test_neutral_display_waits_for_same_batch_inactive_steering(ford_joined, mon @pytest.mark.parametrize('hands_free', [False]) @pytest.mark.parametrize('topology', ['automatic']) @pytest.mark.parametrize('fault', ['heartbeat', 'source-loss']) -@pytest.mark.parametrize('angle', [True]) -def test_native_rejected_ui_cannot_replace_stock_cluster(ford_joined, monkeypatch, fault, angle): +def test_native_rejected_ui_cannot_replace_stock_cluster(ford_joined, monkeypatch, fault): monkeypatch.ford_native_10hz = True monkeypatch.ford_route22_final_fault = fault run_actual_producer(ford_joined, monkeypatch, scenario='route22') assert ford_joined.rejected_ui_packets assert all(address in (0x18A, 0x3D8) for _, address in ford_joined.rejected_ui_packets) - -@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('angle', [True]) -@pytest.mark.parametrize('pinion', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_brake_retains_lateral_with_native_first_feedback_pinion_route1f(ford_joined, monkeypatch, angle, pinion): - run_actual_producer(ford_joined, monkeypatch, scenario='brake-native-cadence') - assert ford_joined.pedal_cancel_active_packets > 0 - - -@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('angle', [True]) -@pytest.mark.parametrize('pinion', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_cancel_retains_lateral_with_native_first_feedback_pinion_route1f(ford_joined, monkeypatch, angle, pinion): - run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence') - assert ford_joined.pedal_cancel_active_packets > 0 - - -@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) -@pytest.mark.parametrize('alpha', [True]) -@pytest.mark.parametrize('hands_free', [True]) -@pytest.mark.parametrize('angle', [True]) -@pytest.mark.parametrize('pinion', [True]) -@pytest.mark.parametrize('topology', ['automatic']) -def test_gas_first_engagement_native_cadence_pinion_route1f(ford_joined, monkeypatch, angle, pinion): - run_actual_producer(ford_joined, monkeypatch, scenario='route22') - assert ford_joined.pedal_cancel_active_packets > 0 diff --git a/openpilot/starpilot/tests/test_ford_manual_ordinary.py b/openpilot/starpilot/tests/test_ford_manual_ordinary.py deleted file mode 100644 index b518159631..0000000000 --- a/openpilot/starpilot/tests/test_ford_manual_ordinary.py +++ /dev/null @@ -1,46 +0,0 @@ -"""Manual AOL marking cannot replace existing ordinary/angle configuration words.""" -from pathlib import Path -from unittest.mock import patch -import pytest -from opendbc.car import car_helpers, gen_empty_fingerprint, structs -from opendbc.car.ford.values import CAR -from openpilot.cereal import messaging -from openpilot.common.params import Params -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.car import card as card_module - - -@pytest.mark.parametrize('identity,fd', [(CAR.FORD_ESCAPE_MK4, False), (CAR.FORD_MUSTANG_MACH_E_MK1, True)]) -@pytest.mark.parametrize('alpha', [False, True]) -@pytest.mark.parametrize('release', [False, True]) -@pytest.mark.parametrize('offset', [0, 4]) -def test_actual_manual_angle_startup_preserves_original_unmarked_owner(identity, fd, alpha, release, offset, monkeypatch): - monkeypatch.setenv('SIMULATION', '1') - monkeypatch.setenv('REPLAY', '1') - monkeypatch.setenv('AOL_REPLAY_RUNTIME', '0') - fp = gen_empty_fingerprint() - fp[offset + 2].update({0x3D6: 8, 0x186: 8}) - observed = (identity, fp, '0' * 17, [], structs.CarParams.FingerprintSource.can, True) - with OpenpilotPrefix(): - saved = Params() - for key, value in [('OpenpilotEnabledToggle', True), ('AlwaysOnLateral', False), ('SafeMode', False), - ('AlphaLongitudinalEnabled', alpha), ('IsReleaseBranch', release), ('FordHumanTurnDetection', False)]: - saved.put_bool(key, value, block=True) - Path(saved.get_param_path('FordLateralMode')).write_bytes(b'1') - with patch.object(car_helpers, 'fingerprint', return_value=observed), \ - patch.object(card_module, 'can_comm_callbacks', return_value=(lambda *args: [], lambda frames: None)), \ - patch.object(card_module.messaging, 'recv_one_retry', return_value=messaging.new_message('can', 1)): - host = card_module.Car() - try: - assert host.CP.transmissionType == structs.CarParams.TransmissionType.manual - assert host.CP.alternativeExperience == 0 and host.aol_card_intent is None - assert host.CP.minEnableSpeed == pytest.approx(20 * .44704) - assert host.CP.safetyConfigs[-1].safetyParam == 128 + (2 if fd else 0) + int(alpha and not release) - assert host.CP.openpilotLongitudinalControl == bool(alpha and not release) - assert host.CI.CC.bp_lat is not None - if offset: - assert host.CP.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput - assert host.CP.safetyConfigs[0].safetyParam == 0 - assert saved.get_bool('AlphaLongitudinalEnabled') == alpha - finally: - host.vehicle_startup.close() diff --git a/openpilot/starpilot/tests/test_ford_neutral_display.py b/openpilot/starpilot/tests/test_ford_neutral_display.py index 8db67a233c..516eb676ec 100644 --- a/openpilot/starpilot/tests/test_ford_neutral_display.py +++ b/openpilot/starpilot/tests/test_ford_neutral_display.py @@ -4,15 +4,13 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, for @pytest.fixture(autouse=True) def configuration(monkeypatch, request): - monkeypatch.ford_angle = request.node.callspec.params['angle'] monkeypatch.ford_topology = request.node.callspec.params['topology'] @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_main_off_neutral_cluster_and_actual_native_reengagement(ford_joined, monkeypatch, angle, topology): +def test_main_off_neutral_cluster_and_actual_native_reengagement(ford_joined, monkeypatch, topology): monkeypatch.ford_native_10hz = True run_actual_producer(ford_joined, monkeypatch, scenario='neutral-main-cadence') diff --git a/openpilot/starpilot/tests/test_ford_recorded_departure.py b/openpilot/starpilot/tests/test_ford_recorded_departure.py index fb724414ff..5d62dc3a60 100644 --- a/openpilot/starpilot/tests/test_ford_recorded_departure.py +++ b/openpilot/starpilot/tests/test_ford_recorded_departure.py @@ -20,7 +20,6 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, for @pytest.fixture(autouse=True) def topology_settings(monkeypatch, request): monkeypatch.ford_topology = request.node.callspec.params.get('topology', 'automatic') - monkeypatch.ford_angle = request.node.callspec.params.get('angle', False) @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @@ -106,9 +105,8 @@ def _motion_payloads(payloads, sample): @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_recorded_departure_with_real_cem_stop_hold(ford_joined, monkeypatch, angle, topology): +def test_recorded_departure_with_real_cem_stop_hold(ford_joined, monkeypatch, topology): # Stop acquisition and clocks use the existing caller fixture. Departure # geometry, filtered speed and radar values are recorded; MCU timing is synthetic. samples = [sample for sample in _samples() if 334.29 <= sample['routeSeconds'] <= 334.95] diff --git a/openpilot/starpilot/tests/test_ford_recorded_lifecycle.py b/openpilot/starpilot/tests/test_ford_recorded_lifecycle.py index 010f3f5b83..6ce6e1019a 100644 --- a/openpilot/starpilot/tests/test_ford_recorded_lifecycle.py +++ b/openpilot/starpilot/tests/test_ford_recorded_lifecycle.py @@ -9,7 +9,7 @@ from opendbc.car.ford.values import CAR from openpilot.starpilot.tests.ford_lifecycle_helpers import ford_joined as ford_joined, run_actual_producer -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 2)]) @pytest.mark.parametrize('alpha', [False]) @pytest.mark.parametrize('hands_free', [False, True]) @pytest.mark.parametrize('overlap', [False, True], ids=['isolated-eps', 'braking-during-eps']) @@ -17,21 +17,21 @@ def test_route24_accelerator_at_engagement_full_brake_stock_off_fault_reverse_an run_actual_producer(ford_joined, monkeypatch, scenario='route24-mixed-eps' if overlap else 'route24') -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 2)]) @pytest.mark.parametrize('alpha', [False]) @pytest.mark.parametrize('hands_free', [False, True]) def test_route2a_new_engagement_after_previous_native_session_retains_aol_on_stock_off(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='route2a') -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 2)]) @pytest.mark.parametrize('alpha', [False]) @pytest.mark.parametrize('hands_free', [False, True]) def test_ford_main_only_rearm_after_previous_session_uses_current_feedback(ford_joined, monkeypatch): run_actual_producer(ford_joined, monkeypatch, scenario='route2a-main') -@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 2)]) @pytest.mark.parametrize('alpha', [False]) @pytest.mark.parametrize('hands_free', [False, True]) def test_fault_at_main_edge_cannot_replay_after_native_health_recovers(ford_joined, monkeypatch): diff --git a/openpilot/starpilot/tests/test_ford_release_long_preferences.py b/openpilot/starpilot/tests/test_ford_release_long_preferences.py index 2891bf62cf..27030658ad 100644 --- a/openpilot/starpilot/tests/test_ford_release_long_preferences.py +++ b/openpilot/starpilot/tests/test_ford_release_long_preferences.py @@ -47,7 +47,7 @@ def test_release_opt_in_off_invalid_safe_mode_debug_and_disabled_stay_unchanged( assert prepare_release_long(cp, saved(tmp_path, alpha, safe), is_release=release, enabled=enabled) is cp -@pytest.mark.parametrize('restriction', ['secoc', 'extra-panda', 'angle', 'foreign-brand']) +@pytest.mark.parametrize('restriction', ['secoc', 'extra-panda', 'foreign-brand']) def test_release_never_promotes_unowned_configuration(restriction, tmp_path): fp = gen_empty_fingerprint() fp[0][0x5A] = 8 @@ -55,8 +55,6 @@ def test_release_never_promotes_unowned_configuration(restriction, tmp_path): cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fp, [], False, True, False) if restriction == 'extra-panda': cp.safetyConfigs = [cp.safetyConfigs[0], cp.safetyConfigs[0]] - elif restriction == 'angle': - cp.safetyConfigs[-1].safetyParam = 130 elif restriction == 'foreign-brand': cp.brand = 'hyundai' assert prepare_release_long(cp, saved(tmp_path), is_release=True, enabled=True) is cp diff --git a/openpilot/starpilot/tests/test_ford_standstill_brake.py b/openpilot/starpilot/tests/test_ford_standstill_brake.py index 6bb7f1f9cf..36d9132a04 100644 --- a/openpilot/starpilot/tests/test_ford_standstill_brake.py +++ b/openpilot/starpilot/tests/test_ford_standstill_brake.py @@ -4,16 +4,14 @@ from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, for @pytest.fixture(autouse=True) def configuration(monkeypatch, request): - monkeypatch.ford_angle = request.node.callspec.params['angle'] monkeypatch.ford_topology = request.node.callspec.params['topology'] @pytest.mark.parametrize('identity', [FORD_FACTORIES[11]]) @pytest.mark.parametrize('alpha', [True]) @pytest.mark.parametrize('hands_free', [False, True]) -@pytest.mark.parametrize('angle', [False, True]) @pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) -def test_native_first_brake_while_already_stopped(ford_joined, monkeypatch, angle, topology): +def test_native_first_brake_while_already_stopped(ford_joined, monkeypatch, topology): monkeypatch.ford_standstill_brake = True monkeypatch.ford_native_first_pedal = True monkeypatch.ford_native_10hz = True diff --git a/openpilot/starpilot/tests/test_vehicle_preferences_api.py b/openpilot/starpilot/tests/test_vehicle_preferences_api.py index a8852b4e0c..c3846db01e 100644 --- a/openpilot/starpilot/tests/test_vehicle_preferences_api.py +++ b/openpilot/starpilot/tests/test_vehicle_preferences_api.py @@ -4,7 +4,7 @@ from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences def test_original_positional_preferences_do_not_enable_door_features(): - values = (True, False, True, False, True, False, True, False, True, False, True, False, True, 1, 1) + values = (True, False, True, False, True, False, True, False, True, False, True, False, True, 1) preferences = VehicleStartupPreferences(*values) names = ( 'toyota_auto_hold', @@ -21,7 +21,6 @@ def test_original_positional_preferences_do_not_enable_door_features(): 'gm_camera_pedal', 'toyota_filter', 'gm_longitudinal_tune', - 'ford_lateral_mode', ) assert tuple(getattr(preferences, name) for name in names) == values assert not preferences.toyota_lock_doors and not preferences.toyota_unlock_doors diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index db65f2c35d..d2325a8605 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/openpilot/starpilot/ui/feature_settings_owner.py @@ -53,8 +53,8 @@ from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.ui.wheel_feature import WheelFeature, PREFIX as WHEEL_PREFIX from openpilot.starpilot.longitudinal.output_max import KEY as OUTPUT_MAX_KEY from openpilot.starpilot.ui.tesla_screen_feature import TeslaScreenFeature, KEYS as TESLA_SCREEN_KEYS -from openpilot.starpilot.ui.ford_lateral_feature import FordLateralFeature, FordClusterFeature -from openpilot.starpilot.ui.ford_pinion_feature import FordPinionFeature +from openpilot.starpilot.ui.ford_bluepilot_feature import FordBluePilotFeature, KEYS as FORD_BLUEPILOT_KEYS +from openpilot.starpilot.ui.ford_cluster_feature import FordClusterFeature from openpilot.starpilot.ui.preap_radar_feature import PreAPRadarFeature, KEYS as PREAP_RADAR_KEYS, capability as preap_radar_capability from openpilot.starpilot.ui.gm_tune_feature import GmTuneFeature, GmTruckFeature, GmEvPresetFeature from openpilot.starpilot.car.gm.tune_preferences import KEY as GM_TUNE_KEY @@ -1024,8 +1024,7 @@ class FeatureSettingsOwner: vehicle_fingerprint=self.vehicle_fingerprint(), dependencies=learning.sources)) elif page == FeaturePage.TORQUE: title = "Steering and Torque" - rows.extend(FordLateralFeature(self).rows(parked)) - rows.extend(FordPinionFeature(self).rows(parked)) + rows.extend(FordBluePilotFeature(self).rows(parked)) rows.extend(FordClusterFeature(self).rows(parked)) controller_row = self.controller.row() if controller_row is not None: @@ -1360,10 +1359,8 @@ class FeatureSettingsOwner: return self._apply_lateral_pause(request) if key == "FordHandsFreeCluster": return FordClusterFeature(self).apply(request) - if key == "FordPrefSteerAngleCurvature": - return FordPinionFeature(self).apply(request) - if key == "FordLateralMode": - return FordLateralFeature(self).apply(request) + if key in FORD_BLUEPILOT_KEYS: + return FordBluePilotFeature(self).apply(request) if key == "TurnAssist": return self._apply_turn_assist(request) if key == 'LateralControllerSelection': diff --git a/openpilot/starpilot/ui/ford_bluepilot_feature.py b/openpilot/starpilot/ui/ford_bluepilot_feature.py new file mode 100644 index 0000000000..33cd5e828e --- /dev/null +++ b/openpilot/starpilot/ui/ford_bluepilot_feature.py @@ -0,0 +1,205 @@ +"""BluePilot lateral settings for Fords, shared by Galaxy and the native feature presentation. + +Ranges and the controller's defaults come from the library's manifest, so a setting can't drift from what the +controller reads. BluePilot Lateral, Primary Control and Use Pinion Yaw Sensor change while parked; the first and +last apply at the next vehicle startup. The tuning rows change any time: the library reads them every frame.""" +import math +from dataclasses import dataclass + +from opendbc.bluepilot_lateral.core.values_ext import FORD_PINION_GEOMETRY_INDEX +from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import lateral_inputs_required +from opendbc.bluepilot_lateral.params.manifest import SETTINGS +from openpilot.starpilot.saved_source import read_saved +from openpilot.starpilot.saved_document import commit_exact +from openpilot.starpilot.ui.feature_settings_state import FeatureRow + +MANIFEST = {setting.key: setting for setting in SETTINGS} +ENABLE = "disable_BP_lat_UI" # stored inverted: "0" is on +MODE = "FordPrefLateralControl" +PINION = "FordPrefSteerAngleCurvature" +SWITCH = ("Off", "On") +MODES = ("Curvature", "Angle") +CURVATURE, ANGLE = 0, 1 + + +@dataclass(frozen=True) +class Tuning: + key: str + label: str + mode: int # shown in this primary control mode only + needs: tuple[str, ...] = () # switches that must be on for the row to change anything + step: float = 0.05 + + +TUNING = ( + Tuning("enable_human_turn_detection_curv", "Human Turn Detection", CURVATURE), + Tuning("lane_change_factor_high_curv", "Lane Change Factor", CURVATURE), + Tuning("enable_lane_positioning_curv", "Lane Positioning", CURVATURE), + Tuning("custom_path_offset_curv", "In-Lane Offset", CURVATURE, ("enable_lane_positioning_curv",)), + Tuning("enable_lane_full_mode_curv", "Lanefull Mode", CURVATURE, ("enable_lane_positioning_curv",)), + Tuning("LC_PID_gain_UI_curv", "Centering Gain", CURVATURE, ("enable_lane_positioning_curv",), step=0.5), + Tuning("custom_profile_curv", "Custom Tuning Profile", CURVATURE), + Tuning("pc_blend_ratio_low_C_UI_curv", "Predicted Blend, Low Curvature", CURVATURE, ("custom_profile_curv",)), + Tuning("pc_blend_ratio_high_C_UI_curv", "Predicted Blend, High Curvature", CURVATURE, ("custom_profile_curv",)), + Tuning("FordLowSpeedFactor_ang", "Low Speed Factor", ANGLE, step=0.01), + Tuning("FordHighSpeedFactor_ang", "High Speed Factor", ANGLE, step=0.01), + Tuning("FordHighSpeedDampening_ang", "High Speed Straight Factor", ANGLE, step=0.01), + Tuning("lane_change_factor_high_ang", "Lane Change Factor", ANGLE), + Tuning("enable_lane_positioning_ang", "Lane Positioning", ANGLE), + Tuning("custom_path_offset_ang", "In-Lane Offset", ANGLE, ("enable_lane_positioning_ang",), step=0.01), + Tuning("lane_centering_strength_ang", "Lane Centering Strength", ANGLE, ("enable_lane_positioning_ang",)), +) +TUNING_BY_KEY = {row.key: row for row in TUNING} +KEYS = frozenset((ENABLE, MODE, PINION, *TUNING_BY_KEY)) + + +def is_switch(key): + return MANIFEST[key].type is bool or key == "custom_profile_curv" + + +class FordBluePilotFeature: + def __init__(self, owner): + self.owner = owner + + def capability(self): + cp = self.owner.vehicle_params() + try: + if cp is None or not lateral_inputs_required(cp) or cp.passive or cp.dashcamOnly or cp.notCar: + return None + return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl), + tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs)) + except (AttributeError, TypeError, ValueError, OverflowError): + return None + + def read(self, key): + """(number or None if invalid, saved raw bytes, readable). An unset key reads its params_keys.h default.""" + raw, readable = read_saved(self.owner.params, key, 16) + if not readable: + return None, raw, False + try: + if raw is None: + number = self.default(key) + else: + number = float(raw.decode()) + except (TypeError, ValueError, UnicodeDecodeError): + return None, raw, True + low, high = MANIFEST[key].range or (0.0, 1.0) + valid = math.isfinite(number) and low <= number <= high and (not is_switch(key) and key != MODE or number in (0.0, 1.0)) + return (number if valid else None), raw, True + + def on(self): + number, _, _ = self.read(ENABLE) + return number == 0.0 + + def mode(self): + number, _, _ = self.read(MODE) + return ANGLE if number == 1.0 else CURVATURE + + def dependencies(self, key): + safe, safe_readable = read_saved(self.owner.params, "SafeMode", 8) + if not safe_readable: + return None + sources = [("SafeMode", safe)] + if key != ENABLE: + sources.append((ENABLE, read_saved(self.owner.params, ENABLE, 16)[0])) + if key in TUNING_BY_KEY: + sources.append((MODE, read_saved(self.owner.params, MODE, 16)[0])) + sources += [(need, read_saved(self.owner.params, need, 16)[0]) for need in TUNING_BY_KEY[key].needs] + return tuple(sources) + + def row(self, key, parked): + capability = self.capability() + if capability is None or key not in KEYS: + return None + if key == PINION and capability[0] not in FORD_PINION_GEOMETRY_INDEX: + return None + number, raw, readable = self.read(key) + dependencies = self.dependencies(key) + safe_off = dependencies is not None and dependencies[0][1] in (None, b"0") + parked_ok = parked and self.owner.authority("parked_preferences") + default = self.default(key) + + if key == ENABLE: + value = SWITCH[1] if number == 0.0 else SWITCH[0] + return FeatureRow(key, "BluePilot Lateral", value if number is not None else "Invalid saved value", raw, + choices=SWITCH, available=parked_ok and readable and safe_off, capability=capability, + dependencies=dependencies or (), default_value=SWITCH[0] if default != 0.0 else SWITCH[1], + repair_value="" if number is not None else SWITCH[0], + reason="" if safe_off else "Safe Mode must be off") + gate = "" if self.on() else "Turn on BluePilot Lateral first" + if key in (MODE, PINION): + labels = MODES if key == MODE else SWITCH + return FeatureRow(key, "Primary Control" if key == MODE else "Use Pinion Yaw Sensor", + labels[int(number)] if number is not None else "Invalid saved value", raw, choices=labels, + available=parked_ok and readable and safe_off and not gate, capability=capability, + dependencies=dependencies or (), default_value=labels[int(default)], + repair_value="" if number is not None else labels[int(default)], + reason=gate or ("" if parked else "Change while parked")) + + tuning = TUNING_BY_KEY[key] + if tuning.mode != self.mode(): + return None + needs = next((TUNING_BY_KEY[need].label for need in tuning.needs if self.read(need)[0] != 1.0), None) + reason = gate or (f"Turn on {needs} first" if needs else "") + # an invalid saved value can still be repaired to the default + available = self.owner.authority("preferences") and readable and not reason + if is_switch(key): + shown = SWITCH[int(default)] + return FeatureRow(key, tuning.label, SWITCH[int(number)] if number is not None else "Invalid saved value", raw, + choices=SWITCH, available=available, capability=capability, dependencies=dependencies or (), + default_value=shown, repair_value="" if number is not None else shown, reason=reason) + low, high = MANIFEST[key].range + shown = str(round(default, 4)) + return FeatureRow(key, tuning.label, str(round(number, 4)) if number is not None else "Invalid saved value", raw, + step=tuning.step if number is not None else 0.0, minimum=low, maximum=high, available=available, + capability=capability, dependencies=dependencies or (), default_value=shown, + repair_value="" if number is not None else shown, reason=reason) + + def default(self, key): + """The params_keys.h default as a number (StarPilot's own: BluePilot lateral is off by default).""" + default = self.owner.params.get_default_value(key) + return float(int(default)) if isinstance(default, bool) else float(default) + + def rows(self, parked): + if self.capability() is None: + return () + rows = (self.row(ENABLE, parked), self.row(MODE, parked), self.row(PINION, parked), + *(self.row(tuning.key, parked) for tuning in TUNING)) + return tuple(row for row in rows if row is not None) + + def encode(self, key, value): + """The bytes to save for a requested display value, or None if it isn't one of the row's values.""" + if key == ENABLE: + return {SWITCH[1]: b"0", SWITCH[0]: b"1"}.get(value) + if key == MODE: + return {MODES[0]: b"0", MODES[1]: b"1"}.get(value) + if is_switch(key): + return {SWITCH[0]: b"0", SWITCH[1]: b"1"}.get(value) + try: + number = float(value) + except ValueError: + return None + low, high = MANIFEST[key].range + step = TUNING_BY_KEY[key].step + if not math.isfinite(number) or not low <= number <= high or abs((number - low) / step - round((number - low) / step)) > 1e-6: + return None + return str(round(number, 4)).encode() + + def apply(self, request): + key = request.key + if key not in KEYS or request.related_source is not None or request.display_unit or request.direction: + return False + raw = self.encode(key, request.value) + if raw is None: + return False + + def authorized(): + row = self.row(key, parked=True) + return (row is not None and row.available and bool(request.vehicle_fingerprint) and + self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and + row.capability == request.capability and row.dependencies == request.dependencies and + row.source == request.expected) + + result = commit_exact(self.owner.params, key=key, max_bytes=16, raw=raw, expected=request.expected, + authorized=authorized, temp_prefix=".ford-bluepilot-") + return result.committed and result.verified diff --git a/openpilot/starpilot/ui/ford_cluster_feature.py b/openpilot/starpilot/ui/ford_cluster_feature.py new file mode 100644 index 0000000000..f10a4d2cd6 --- /dev/null +++ b/openpilot/starpilot/ui/ford_cluster_feature.py @@ -0,0 +1,48 @@ +"""Ford hands-free cluster display preference shared by Galaxy and the native feature presentation.""" +from openpilot.starpilot.saved_document import commit_exact +from openpilot.starpilot.ui.feature_settings_state import FeatureRow + + +class FordClusterFeature: + """Shared native/Galaxy saved display preference for Ford CAN-FD profiles.""" + def __init__(self, owner): + self.owner = owner + + def capability(self): + from openpilot.starpilot.car.ford.cluster_preferences import qualified + cp = self.owner.vehicle_params() + try: + if cp is None or not qualified(cp): + return None + return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl), + tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs)) + except (AttributeError, TypeError, ValueError, OverflowError): + return None + + def rows(self, parked): + from openpilot.starpilot.car.ford.cluster_preferences import KEY, CHOICES, read_choice + capability = self.capability() + if capability is None: + return () + value, raw, readable, valid = read_choice(self.owner.params) + return (FeatureRow(KEY, "Ford hands-free cluster display", CHOICES[value] if valid else "Invalid saved choice", raw, + choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and readable, + capability=capability, repair_value="" if valid else CHOICES[0], default_value=CHOICES[0], + reason="Changes the active cluster icon only; driver attention requirements remain unchanged."),) + + def apply(self, request): + from openpilot.starpilot.car.ford.cluster_preferences import KEY, CHOICES, read_choice + value = next((code for code, label in CHOICES.items() if label == request.value), None) + if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction): + return False + + def authorized(): + capability = self.capability() + _, raw, readable, valid = read_choice(self.owner.params) + return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and + self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and + capability == request.capability and readable and raw == request.expected and (valid or value == 0)) + + result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected, + authorized=authorized, temp_prefix=".ford-cluster-") + return result.committed and result.verified diff --git a/openpilot/starpilot/ui/ford_lateral_feature.py b/openpilot/starpilot/ui/ford_lateral_feature.py deleted file mode 100644 index 7c749c47ec..0000000000 --- a/openpilot/starpilot/ui/ford_lateral_feature.py +++ /dev/null @@ -1,115 +0,0 @@ -"""One saved Ford enum shared by Galaxy and the native feature presentation.""" -from opendbc.bluepilot_lateral.hosts.starpilot import supported -from opendbc.car import structs -from opendbc.car.ford.aol import qualified as aol_qualified -from openpilot.starpilot.car.ford.lateral_preferences import KEY, CHOICES, read_choice -from openpilot.starpilot.saved_source import read_saved -from openpilot.starpilot.saved_document import commit_exact -from openpilot.starpilot.ui.feature_settings_state import FeatureRow - - -class FordLateralFeature: - def __init__(self, owner): - self.owner = owner - - def capability(self): - cp = self.owner.vehicle_params() - try: - if cp is None: - return None - if not supported(cp): - if not aol_qualified(cp, marked_only=True): - return None - next_drive = structs.CarParams(**cp.to_dict()) - next_drive.alternativeExperience = 0 - if not supported(next_drive): - return None - return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl), - bool(cp.pcmCruise), str(cp.steerControlType), bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar), - tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs)) - except (AttributeError, TypeError, ValueError, OverflowError): - return None - - def dependencies(self): - values = [] - for key in ("SafeMode", "AlwaysOnLateral"): - raw, readable = read_saved(self.owner.params, key, 8) - if not readable or raw not in ((None, b"0") if key == "SafeMode" else (None, b"0", b"1")): - return None - values.append((key, raw)) - return tuple(values) - - def rows(self, parked): - capability = self.capability() - if capability is None: - return () - value, raw, readable, valid = read_choice(self.owner.params) - dependencies = self.dependencies() - return (FeatureRow(KEY, "Ford steering controller", CHOICES[value] if valid else "Invalid saved choice", raw, - choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and - readable and dependencies is not None, capability=capability, - dependencies=dependencies or (), repair_value="" if valid else CHOICES[0], default_value=CHOICES[0], - reason="Applies next drive. BluePilot angle works with Always On Lateral. Safe Mode must be off."),) - - def apply(self, request): - value = next((code for code, label in CHOICES.items() if label == request.value), None) - if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction): - return False - - def authorized(): - capability = self.capability() - _, raw, readable, valid = read_choice(self.owner.params) - dependencies = self.dependencies() - return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and - self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and - capability == request.capability and dependencies is not None and dependencies == request.dependencies and - readable and raw == request.expected and (valid or value == 0)) - - result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected, - authorized=authorized, temp_prefix=".ford-lateral-") - return result.committed and result.verified - - -class FordClusterFeature: - """Shared native/Galaxy saved display preference for Ford CAN-FD profiles.""" - def __init__(self, owner): - self.owner = owner - - def capability(self): - from openpilot.starpilot.car.ford.cluster_preferences import qualified - cp = self.owner.vehicle_params() - try: - if cp is None or not qualified(cp): - return None - return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl), - tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs)) - except (AttributeError, TypeError, ValueError, OverflowError): - return None - - def rows(self, parked): - from openpilot.starpilot.car.ford.cluster_preferences import KEY, CHOICES, read_choice - capability = self.capability() - if capability is None: - return () - value, raw, readable, valid = read_choice(self.owner.params) - return (FeatureRow(KEY, "Ford hands-free cluster display", CHOICES[value] if valid else "Invalid saved choice", raw, - choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and readable, - capability=capability, repair_value="" if valid else CHOICES[0], default_value=CHOICES[0], - reason="Changes the active cluster icon only; driver attention requirements remain unchanged."),) - - def apply(self, request): - from openpilot.starpilot.car.ford.cluster_preferences import KEY, CHOICES, read_choice - value = next((code for code, label in CHOICES.items() if label == request.value), None) - if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction): - return False - - def authorized(): - capability = self.capability() - _, raw, readable, valid = read_choice(self.owner.params) - return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and - self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and - capability == request.capability and readable and raw == request.expected and (valid or value == 0)) - - result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected, - authorized=authorized, temp_prefix=".ford-cluster-") - return result.committed and result.verified diff --git a/openpilot/starpilot/ui/ford_pinion_feature.py b/openpilot/starpilot/ui/ford_pinion_feature.py deleted file mode 100644 index 6c8419c431..0000000000 --- a/openpilot/starpilot/ui/ford_pinion_feature.py +++ /dev/null @@ -1,74 +0,0 @@ -"""Parked, init-only pinion measurement preference shared by native UI and Galaxy.""" -from opendbc.car.ford.aol import qualified as aol_qualified -from opendbc.car.ford.pinion import KEY -from openpilot.starpilot.saved_source import read_saved -from openpilot.starpilot.saved_document import commit_exact -from openpilot.starpilot.ui.feature_settings_state import FeatureRow - -CHOICES = {0: "Off", 1: "On"} - - -def read_choice(params): - raw, readable = read_saved(params, KEY, 8) - valid = readable and raw in (None, b"0", b"1") - return int(valid and raw == b"1"), raw, readable, valid - - -class FordPinionFeature: - def __init__(self, owner): - self.owner = owner - - def capability(self): - cp = self.owner.vehicle_params() - try: - if cp is None: - return None - from opendbc.car.ford.pinion import GEOMETRY - if cp.carFingerprint not in GEOMETRY or not aol_qualified(cp): - return None - return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl), - bool(cp.pcmCruise), str(cp.steerControlType), bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar), - tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs)) - except (AttributeError, TypeError, ValueError, OverflowError): - return None - - def dependencies(self): - values = [] - for key in ("SafeMode", "AlwaysOnLateral"): - raw, readable = read_saved(self.owner.params, key, 8) - if not readable or raw not in ((None, b"0") if key == "SafeMode" else (None, b"0", b"1")): - return None - values.append((key, raw)) - return tuple(values) - - def rows(self, parked): - capability = self.capability() - if capability is None: - return () - value, raw, readable, valid = read_choice(self.owner.params) - dependencies = self.dependencies() - return (FeatureRow(KEY, "Use Pinion Yaw Sensor", CHOICES[value] if valid else "Invalid saved choice", raw, - choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and - readable and dependencies is not None, capability=capability, - dependencies=dependencies or (), repair_value="" if valid else CHOICES[0], default_value=CHOICES[0], - reason="Off by default. Uses steering-pinion curvature instead of RCM yaw in control and Panda safety. " - + "Applies next vehicle startup; Safe Mode must be off."),) - - def apply(self, request): - value = next((code for code, label in CHOICES.items() if label == request.value), None) - if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction): - return False - - def authorized(): - capability = self.capability() - _, raw, readable, valid = read_choice(self.owner.params) - dependencies = self.dependencies() - return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and - self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and - capability == request.capability and dependencies is not None and dependencies == request.dependencies and - readable and raw == request.expected and (valid or value == 0)) - - result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected, - authorized=authorized, temp_prefix=".ford-pinion-") - return result.committed and result.verified - diff --git a/openpilot/starpilot/ui/tests/test_ford_bluepilot_feature.py b/openpilot/starpilot/ui/tests/test_ford_bluepilot_feature.py new file mode 100644 index 0000000000..16f491517d --- /dev/null +++ b/openpilot/starpilot/ui/tests/test_ford_bluepilot_feature.py @@ -0,0 +1,142 @@ +"""BluePilot lateral settings: which rows a Ford offers and when, and what each change saves.""" +from pathlib import Path + +import pytest +from opendbc.car.car_helpers import interfaces +from opendbc.car.ford.values import CAR +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.common.params import Params +from openpilot.starpilot.ui.feature_settings_state import FeatureSettingsRequest +from openpilot.starpilot.ui.ford_bluepilot_feature import ENABLE, MODE, PINION, FordBluePilotFeature + + +class Owner: + def __init__(self, params, cp, parked): + self.params, self.cp, self.parked = params, cp, parked + + def vehicle_params(self): + return self.cp + + def vehicle_fingerprint(self): + return str(self.cp.carFingerprint) + + def authority(self, group): + return group == "preferences" or (group == "parked_preferences" and self.parked) + + +def owner(tmp_path, car=CAR.FORD_F_150_MK14, *, parked=True): + return Owner(Params(str(tmp_path)), interfaces[car].get_non_essential_params(car), parked) + + +def rows(o): + return {row.key: row for row in FordBluePilotFeature(o).rows(o.parked)} + + +def save(o, key, raw): + Path(o.params.get_param_path(key)).write_bytes(raw) + + +def saved(o, key): + path = Path(o.params.get_param_path(key)) + return path.read_bytes() if path.exists() else None + + +def change(o, key, value, row=None): + row = row or rows(o)[key] + request = FeatureSettingsRequest(key, row.source, value, vehicle_fingerprint=o.vehicle_fingerprint(), + capability=row.capability, dependencies=row.dependencies) + return FordBluePilotFeature(o).apply(request) + + +def test_off_by_default_with_only_the_switch_editable(tmp_path): + o = owner(tmp_path) + shown = rows(o) + assert shown[ENABLE].value == "Off" and shown[ENABLE].available + assert not shown[MODE].available and not shown[PINION].available + assert shown["LC_PID_gain_UI_curv"].reason == "Turn on BluePilot Lateral first" + assert not any(key.endswith("_ang") for key in shown) + + +def test_turning_on_saves_the_inverse_and_unlocks_the_tuning(tmp_path): + o = owner(tmp_path) + assert change(o, ENABLE, "On") + assert saved(o, ENABLE) == b"0" + shown = rows(o) + assert shown[MODE].available and shown[PINION].available + assert shown["enable_lane_positioning_curv"].available + assert not shown["custom_path_offset_curv"].available + assert shown["custom_path_offset_curv"].reason == "Turn on Lane Positioning first" + assert change(o, "enable_lane_positioning_curv", "On") + assert rows(o)["custom_path_offset_curv"].available + + +def test_angle_mode_shows_its_own_rows(tmp_path): + o = owner(tmp_path) + save(o, ENABLE, b"0") + assert change(o, MODE, "Angle") + assert saved(o, MODE) == b"1" + shown = rows(o) + assert {key for key in shown if key.endswith("_ang")} == { + "FordLowSpeedFactor_ang", "FordHighSpeedFactor_ang", "FordHighSpeedDampening_ang", "lane_change_factor_high_ang", + "enable_lane_positioning_ang", "custom_path_offset_ang", "lane_centering_strength_ang"} + assert not any(key.endswith("_curv") for key in shown) + + +def test_startup_settings_change_only_while_parked_and_tuning_any_time(tmp_path): + o = owner(tmp_path, parked=False) + save(o, ENABLE, b"0") + shown = rows(o) + assert not shown[ENABLE].available and not shown[MODE].available and not shown[PINION].available + assert shown["lane_change_factor_high_curv"].available + assert not change(o, ENABLE, "Off") + assert change(o, "lane_change_factor_high_curv", "0.9") + assert saved(o, "lane_change_factor_high_curv") == b"0.9" + + +def test_numbers_stay_on_the_rows_steps_and_range(tmp_path): + o = owner(tmp_path) + save(o, ENABLE, b"0") + save(o, "enable_lane_positioning_curv", b"1") + row = rows(o)["LC_PID_gain_UI_curv"] + assert (row.minimum, row.maximum, row.step, row.value) == (0.0, 50.0, 0.5, "1.0") + assert not change(o, "LC_PID_gain_UI_curv", "1.3") + assert not change(o, "LC_PID_gain_UI_curv", "50.5") + assert change(o, "LC_PID_gain_UI_curv", "1.5") + assert saved(o, "LC_PID_gain_UI_curv") == b"1.5" + + +@pytest.mark.parametrize("car,pinion", ((CAR.FORD_F_150_MK14, True), (CAR.FORD_MONDEO_MK5, True), (CAR.FORD_EDGE_MK2, False))) +def test_pinion_only_on_platforms_with_a_geometry_row(tmp_path, car, pinion): + assert (PINION in rows(owner(tmp_path, car))) == pinion + + +@pytest.mark.parametrize("car", (CAR.FORD_TRANSIT_MK5, TOYOTA.TOYOTA_RAV4_TSS2_2023)) +def test_no_rows_without_bluepilot_lateral(tmp_path, car): + assert rows(owner(tmp_path, car)) == {} + + +def test_safe_mode_keeps_bluepilot_lateral_off(tmp_path): + o = owner(tmp_path) + save(o, "SafeMode", b"1") + assert not rows(o)[ENABLE].available + assert rows(o)[ENABLE].reason == "Safe Mode must be off" + assert not change(o, ENABLE, "On") + + +def test_a_request_from_before_its_dependency_changed_is_refused(tmp_path): + o = owner(tmp_path) + save(o, ENABLE, b"0") + stale = rows(o)["lane_change_factor_high_curv"] + save(o, ENABLE, b"1") + assert not change(o, "lane_change_factor_high_curv", "0.9", row=stale) + assert saved(o, "lane_change_factor_high_curv") is None + + +def test_an_invalid_saved_value_repairs_to_the_default(tmp_path): + o = owner(tmp_path) + save(o, ENABLE, b"0") + save(o, "lane_change_factor_high_curv", b"7") + row = rows(o)["lane_change_factor_high_curv"] + assert row.value == "Invalid saved value" and row.available and row.repair_value == "0.85" + assert change(o, "lane_change_factor_high_curv", row.repair_value) + assert saved(o, "lane_change_factor_high_curv") == b"0.85" diff --git a/openpilot/starpilot/vehicle_preferences.py b/openpilot/starpilot/vehicle_preferences.py index 332747aa04..d09f01245b 100644 --- a/openpilot/starpilot/vehicle_preferences.py +++ b/openpilot/starpilot/vehicle_preferences.py @@ -42,8 +42,7 @@ class VehicleStartupPreferences: gm_camera_pedal: bool = True toyota_filter: bool = True gm_longitudinal_tune: int = 0 - ford_lateral_mode: int = 0 - ford_pinion: bool = field(default=False, kw_only=True) + ford_bluepilot: tuple[tuple[str, str], ...] = field(default=(("disable_BP_lat_UI", "1"),), kw_only=True) toyota_lock_doors: bool = False toyota_unlock_doors: bool = False toyota_radar_mode: int = 0 @@ -52,12 +51,12 @@ class VehicleStartupPreferences: @classmethod def read(cls, params, *, enabled: bool): - from openpilot.starpilot.car.ford.lateral_preferences import selected as ford_mode from openpilot.starpilot.car.toyota.radar_preferences import selected as toyota_radar_mode from openpilot.starpilot.car.gm.tune_preferences import selected_tune from openpilot.starpilot.car.tesla.preap_preferences import stock_configuration from openpilot.starpilot.car.tesla.radar_settings import read_radar_settings from openpilot.starpilot.car.tesla.pedal_preferences import read_pedal_settings + from openpilot.starpilot.car.ford.bluepilot import startup_params as ford_bluepilot smoothing = read_snapshot(params, enabled=enabled) radar = read_radar_settings(params) pedal = read_pedal_settings(params) @@ -114,9 +113,7 @@ class VehicleStartupPreferences: subaru_sng=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SubaruSNG", 8) == (b"1", True)), subaru_software_cruise=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SubaruRedneckCruise", 8) == (b"1", True)), toyota_radar_mode=toyota_radar_mode(params, enabled=enabled), - ford_lateral_mode=ford_mode(params, enabled=enabled), - ford_pinion=bool(enabled and read_saved(params, "SafeMode", 8) in ((None, True), (b"0", True)) and - read_saved(params, "FordPrefSteerAngleCurvature", 8) == (b"1", True)), tesla_screen=screen, + ford_bluepilot=tuple(ford_bluepilot(params, enabled=enabled).items()), tesla_screen=screen, tesla_screen_brake=screen_brake, toyota_auto_hold=toyota, toyota_sng=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SNGHack", 8) == (b"1", True)), jeep_brake_hold=jeep_hold, @@ -225,10 +222,8 @@ class VehicleStartupPreferences: apply_toyota_auto_hold(cp, self.toyota_auto_hold) from opendbc.car.toyota.ars510_preferences import apply_radar_mode apply_radar_mode(cp, self.toyota_radar_mode) - from opendbc.bluepilot_lateral.hosts.starpilot import select as select_ford_lateral - select_ford_lateral(cp, self.ford_lateral_mode) - from opendbc.car.ford.pinion import select as select_pinion - select_pinion(cp, self.ford_pinion) + from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot + init_ford_safety_param_starpilot(cp, dict(self.ford_bluepilot)) return cp def finalize(self, cp) -> None: diff --git a/tools/ci/custom_pytest_workloads.json b/tools/ci/custom_pytest_workloads.json index d73d887134..77e990b2a4 100644 --- a/tools/ci/custom_pytest_workloads.json +++ b/tools/ci/custom_pytest_workloads.json @@ -2012,12 +2012,6 @@ "cases": 64, "seconds": 15.568039 }, - "openpilot/starpilot/tests/test_ford_bluepilot_aol.py": { - "basis": "Exact 65660a4 hosted main40623875380: all 149 nodes passed, 264.354436s setup/call/teardown; 25% estimate margin, retaining any higher same-source prior estimate. Scheduling only; no test deadline change.", - "blob": "0c39bf229eb908d3ba63abd9f57e3c0e68a48c0b", - "cases": 149, - "seconds": 330.443045 - }, "openpilot/starpilot/tests/test_ford_ci_registration.py": { "basis": "Separate complete AOL reference from same6eb source; not custom wall-time measurement", "blob": "8065ac29f007fc2d14ce32fe81e8e5ff456ddd6a", diff --git a/tools/ci/lifecycle_workloads.json b/tools/ci/lifecycle_workloads.json index 6798b69a14..92e51826f1 100644 --- a/tools/ci/lifecycle_workloads.json +++ b/tools/ci/lifecycle_workloads.json @@ -481,16 +481,6 @@ "seconds": 0.05543316599994341, "source_sha256": "65ad26851e9e115e8b3c9d6a57ccb72295619fa16d22a2e32261bae6e434ea77" }, - "opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py": { - "cases": 2, - "seconds": 0.17583017300006532, - "source_sha256": "dbd0af4c30c2c6ab163ddf5f31af75670d31efee1586a21a62f193a1f25043a8" - }, - "opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py": { - "cases": 78, - "seconds": 0.16058086799961302, - "source_sha256": "f7ea0ed06811453431e3b3aa8eb11cb7138293ca89c87deac09b95f2e6ad3803" - }, "openpilot/starpilot/aol/tests/test_gv70_two_panda_long.py": { "cases": 22, "seconds": 0.013164143999915723, @@ -778,34 +768,6 @@ "shard": 8, "results_sha256": "a14538044b22704179bab89d0f9ded3a4f90480c14d4ea9c76047f933d2e48c5" } - }, - "openpilot/starpilot/tests/test_ford_bluepilot_aol.py": { - "cases": 133, - "seconds": 274.055069753, - "source_sha256": "ef4d8ff607cedc41c2cf88375a707c0eecaaab623d1a6ba8c8bb3aeb13391e68", - "reference": { - "basis": "Maximum of complete same-source whole-module hosted measurements; phase time only, no grouping or deadline changes.", - "source_blob": "ce0ee89600b5b608eb2c905a43c0c8ec8db5cc0b", - "measurements": [ - { - "revision": "12dab8c31dd362a6198b9f2bb55facdf7dd34a9f", - "run_id": 40449589613, - "artifact_id": 12710448232, - "shard": 19, - "suite": "custom-pytest", - "seconds": 274.055069753 - }, - { - "revision": "5f84b6c763b9aa185ed4e169a63b058a4cbce3d9", - "run_id": 40477003765, - "artifact_id": 12722625581, - "shard": 8, - "suite": "aol-lifecycle", - "seconds": 266.7861787670016, - "results_sha256": "ab0b19b17563bd052360b59cac490415407f5c0196fa3762219a960e3a426cc1" - } - ] - } } }, "reference": { diff --git a/tools/ci/run_safety_qualification.py b/tools/ci/run_safety_qualification.py index 1e114e0621..cb53bfda3b 100644 --- a/tools/ci/run_safety_qualification.py +++ b/tools/ci/run_safety_qualification.py @@ -463,7 +463,7 @@ class Gate: from opendbc.safety.tests.libsafety import libsafety_py libsafety_py.load(sys.argv[1]) suite = unittest.TestLoader().discover('.', pattern='test_*.py') -suite.addTests(unittest.TestLoader().loadTestsFromName('opendbc.bluepilot_lateral.tests.test_angle_native')) +suite.addTests(unittest.TestLoader().discover('../../bluepilot_lateral/safety/tests', pattern='test_*.py', top_level_dir='../../..')) """ + UNITTEST_ACCOUNTING code, text = self.run("unittest", [self.python, "-c", suite_script, str(native)], cwd=SAFETY_TESTS) summary = parse_unittest_summary(text) diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index e6c76005e7..9b47ad0151 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -25,9 +25,6 @@ from tools.ci import lifecycle_shards, vehicle_pytest # The emitted selected_test_ids record exact parametrized configurations. AOL_LIFECYCLE_TARGETS = ( "opendbc_repo/opendbc/car/hyundai/tests/test_torque_ev_deferred_takeover.py", - 'openpilot/starpilot/tests/test_ford_bluepilot_aol.py', - 'opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py', - 'opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_aol.py', 'openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py', 'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock.py', 'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock_callers.py', @@ -59,8 +56,7 @@ AOL_LIFECYCLE_TARGETS = ( 'openpilot/starpilot/aol/tests/test_hkg_native_admission.py', 'openpilot/starpilot/aol/tests/test_native_observation_admission.py', 'openpilot/starpilot/aol/tests/test_forte_physical_arm_join.py', - 'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py', - 'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py', + 'opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py', 'openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py', 'openpilot/starpilot/aol/tests/test_gv70_two_panda_long.py', 'openpilot/starpilot/longitudinal/tests/test_gv70_stopping_parity.py', @@ -229,10 +225,6 @@ DEBUG_SAFETY_CLASS_TARGETS = { "opendbc_repo/opendbc/safety/tests/test_hyundai_ev9_long.py": ( "opendbc_repo/opendbc/safety/tests/test_hyundai_ev9_long.py::TestEv9LongDebugContract", ), - "opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py": ( - "opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py::TestFordMachEExtended", - "opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py::TestFordMachEDebugLongExtended", - ), "opendbc_repo/opendbc/safety/tests/test_hyundai_main_rearm.py": ( "opendbc_repo/opendbc/safety/tests/test_hyundai_main_rearm.py::TestHkgMainDebug", ), @@ -244,9 +236,6 @@ DEBUG_SAFETY_CLASS_TARGETS = { RELEASE_TARGETS = ( - 'openpilot/starpilot/tests/test_ford_bluepilot_aol.py', - 'opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py', - 'opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_aol.py', 'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock.py', 'openpilot/starpilot/car/toyota/tests/test_prius_retrofit_stock_callers.py', 'opendbc_repo/opendbc/car/toyota/tests/test_matrix_filter.py', @@ -271,8 +260,7 @@ RELEASE_TARGETS = ( 'opendbc_repo/opendbc/safety/tests/test_toyota_radar_lta_pedal.py', 'opendbc_repo/opendbc/safety/tests/test_toyota_prius_retrofit_advert.py', 'opendbc_repo/opendbc/car/hyundai/tests/test_long_cp_copy.py', - 'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py', - 'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py', + 'opendbc_repo/opendbc/safety/tests/test_ford_canfd_display.py', "opendbc_repo/opendbc/safety/tests/test_toyota_prius_advert.py", 'opendbc_repo/opendbc/safety/tests/test_hyundai_gv60_long.py::TestGV60LongReleaseDenied', 'openpilot/starpilot/tests/test_ford_configuration_parity.py', @@ -394,15 +382,9 @@ RELEASE_TARGETS = ( "opendbc_repo/opendbc/car/ford/tests/test_aol_startup.py", "openpilot/starpilot/tests/test_ford_aol_intent.py", "opendbc_repo/opendbc/safety/tests/test_ford_aol.py", - "opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py::TestFordMachEExtended", - "opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py::TestFordMachELongReleaseExtended", + "opendbc_repo/opendbc/safety/tests/test_ford.py::TestFordCANFDLongitudinalSafety", "opendbc_repo/opendbc/car/ford/tests/test_added_firmware_identity.py", "opendbc_repo/opendbc/car/ford/tests/test_required_source_health.py", - "opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py", - "opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py", - "opendbc_repo/opendbc/car/ford/tests/test_production_extensions.py", - "opendbc_repo/opendbc/car/ford/tests/test_generic_canfd.py", - "opendbc_repo/opendbc/safety/tests/test_ford_generic_canfd_extended.py", "opendbc_repo/opendbc/car/hyundai/tests/test_blended_stock_aol.py", "openpilot/starpilot/aol/tests/test_nexo_stock_aol.py", "opendbc_repo/opendbc/safety/tests/test_nexo_stock_aol.py", diff --git a/tools/ci/safety_pytest_coverage.py b/tools/ci/safety_pytest_coverage.py index 72521d0e8f..60cf6914d7 100644 --- a/tools/ci/safety_pytest_coverage.py +++ b/tools/ci/safety_pytest_coverage.py @@ -31,8 +31,7 @@ def targets_for_coverage(root=ROOT): selected.add(f'{relative}::{node.name}') # These existing end-to-end callers bind the supplied library. Other vehicle suites # deliberately compile distinct DEBUG/RELEASE artifacts and retain their own gates. - for relative in ('opendbc_repo/opendbc/car/ford/tests/test_pinion_measurement.py', - 'openpilot/starpilot/car/nissan/tests/test_aol.py', + for relative in ('openpilot/starpilot/car/nissan/tests/test_aol.py', 'openpilot/starpilot/car/rivian/tests/test_aol_native.py'): if (root / relative).is_file(): selected.add(relative) diff --git a/tools/ci/tests/test_safety_qualification.py b/tools/ci/tests/test_safety_qualification.py index b0e3ad30a1..0ee89d9ccf 100644 --- a/tools/ci/tests/test_safety_qualification.py +++ b/tools/ci/tests/test_safety_qualification.py @@ -95,8 +95,7 @@ class TestSafetyQualificationRunner(TestCase): def test_coverage_catalog_includes_native_pytest_and_external_callers(self): from tools.ci import safety_pytest_coverage as coverage selected = coverage.targets_for_coverage(qualification.ROOT) - for name in ('opendbc_repo/opendbc/car/ford/tests/test_pinion_measurement.py', - 'opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py::test_actual_registered_priming_rate_absolute_bus_and_counter_bounds', + for name in ('opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py::test_actual_registered_priming_rate_absolute_bus_and_counter_bounds', 'openpilot/starpilot/car/nissan/tests/test_aol.py', 'openpilot/starpilot/car/rivian/tests/test_aol_native.py'): self.assertIn(name, selected) @@ -220,7 +219,7 @@ class TestSafetyQualificationRunner(TestCase): self.assertEqual(len(libraries.builds[0]['source_sha256']), 64) libraries.load(variant) module.load.assert_called_with(variant.resolve()) - params = root / 'opendbc_repo/opendbc/bluepilot_lateral/params/limits.h' + params = root / 'opendbc_repo/opendbc/bluepilot_lateral/safety/ford.h' params.parent.mkdir(parents=True) params.write_text('changed native include') with self.assertRaisesRegex(RuntimeError, 'sources changed before loading'): diff --git a/tools/ci/tests/test_vehicle_ci.py b/tools/ci/tests/test_vehicle_ci.py index d809ebdf22..3ef2be5e90 100644 --- a/tools/ci/tests/test_vehicle_ci.py +++ b/tools/ci/tests/test_vehicle_ci.py @@ -292,7 +292,6 @@ class TestVehicleCollection(unittest.TestCase): self.assertIn(target, debug) self.assertIn(target, release) self.assertIn("opendbc_repo/opendbc/safety/tests/test_hyundai_ev9_long.py", mixed) - self.assertIn("opendbc_repo/opendbc/safety/tests/test_ford_mach_e_extended.py", mixed) def test_interfaces_do_not_mislabel_historical_route_test_base(self): root = Path(__file__).resolve().parents[3]