mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-12 23:43:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

BluePilot lateral library (stamped copy)

Browse Source 
opendbc_repo/opendbc/bluepilot_lateral is BluePilotDev/bluepilot-lateral as released, made by the
library's tools/vendor.sh and checked by its tools/check_vendored.sh (VERSION names the release and
commit). Nothing inside the folder is edited here: changes go to the library, get released, and are
vendored again.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
alan-polk
2026-10-29 16:47:03 -05:00
parent 8da0c9d916
commit c5ce014573
70 changed files with 6348 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/bluepilot_lateral/.gitattributes
+1
View File
@@ -0,0 +1 @@
.github export-ignore
opendbc_repo/opendbc/bluepilot_lateral/.gitignore
+1
View File
@@ -0,0 +1 @@
__pycache__/
opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md
+53
View File
@@ -0,0 +1,53 @@
# Credits
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).
## Contributors
- **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.
## 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.
opendbc_repo/opendbc/bluepilot_lateral/FORKS.md
+77
View File
@@ -0,0 +1,77 @@
# 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 `<pkg>/bluepilot_lateral/` in your opendbc (`<pkg>` 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: <fork>": 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/<fork>/`, plus host bindings if its safety names differ from upstream's;
- **its hooks**, `hosts/<fork>/hooks.patch`, until the fork carries them;
- **`hosts/<fork>/host.sh`**: `tree <dir>` builds the fork's opendbc at a pinned commit with the hooks and the library, placed by `vendor.sh`; `test <dir>` runs the library tests, the fork's safety suite with the BluePilot safety tests, and coverage; `make <dir>` writes `hooks.patch` back after a hook is edited in `<dir>/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 <tag> <folder>`** makes `<folder>` exactly the library at `<tag>`:
- 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 `<folder>/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 <tag> <fork checkout>/<path to its opendbc package>/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 <folder>`** reads `<folder>/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 <fork checkout>/<path to its opendbc package>/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.
## 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.
opendbc_repo/opendbc/bluepilot_lateral/HOSTS.md
+129
View File
@@ -0,0 +1,129 @@
# Hosting the BluePilot lateral
A host is an opendbc fork with this library at `<pkg>/bluepilot_lateral/` and the hook patch applied, plus the openpilot on top of it that fills the library's inputs each frame. `<pkg>` 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 `<pkg>/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/<file>.h` and the host's declarations as `../../safety/declarations.h`, relative to the including file. The host includes the library's safety as `"<pkg>/bluepilot_lateral/safety/ford.h"`.
### Without submodules: a stamped copy
From a clone of this repo, `tools/vendor.sh <release tag> <host>/<pkg>/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 <folder>` 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 <pkg>.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:
- `<pkg>.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: `<pkg>.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 <pkg>/bluepilot_lateral/tests -t .`, from the directory that holds `<pkg>`.
- Safety tests: the host's `<pkg>/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 <checkout>`, 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 <dir>` 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 <dir>` runs the library tests, IQPilot's safety suite with the BluePilot ones (minus `known_failures.txt`, IQPilot's own) and coverage; `host.sh make <dir>` writes the hook patch back after editing the hooks in `<dir>/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.
opendbc_repo/opendbc/bluepilot_lateral/LICENSE
+9
View File
@@ -0,0 +1,9 @@
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:
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.
opendbc_repo/opendbc/bluepilot_lateral/README.md
+14
View File
@@ -0,0 +1,14 @@
# BluePilot lateral
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.
**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.
**Status:** `v0.2.0`. It runs on bp-sunnypilot, a sunnypilot fork, through ford-op/opendbc (sunnypilot's opendbc plus the hooks).
- [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).
History before 2026-09-15 lives in [BluePilotDev/bluepilot](https://github.com/BluePilotDev/bluepilot).
opendbc_repo/opendbc/bluepilot_lateral/VERSION
+1
View File
@@ -0,0 +1 @@
untagged 3e63502e1b79c29e246582f3402ae4909c8c3aed
opendbc_repo/opendbc/bluepilot_lateral/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/can/LANE_ASSIST_DATA1.md
+60
View File
@@ -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.
opendbc_repo/opendbc/bluepilot_lateral/can/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/can/fordcan_ext.py
+128
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/core/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/core/carcontroller_ext.py
+234
View File
@@ -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
)
opendbc_repo/opendbc/bluepilot_lateral/core/human_turn.py
+71
View File
@@ -0,0 +1,71 @@
"""
BluePilot: shared manual-steering-override ("human turn") detection for Ford lateral control.
Both Ford lateral strategies hand control back to the driver on a sustained manual turn, and the
detection is identical, so it lives here as one class instead of being copied into each mode. What
each mode DOES with the signal differs: curvature-primary (``lateral_curv_ext``) zeroes its command
(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 ...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
HUMAN_TURN_HOLD_S = 1.5
# When the wheel was ALREADY past HUMAN_TURN_ANGLE_DEG at first contact -- lateral control had it
# turned mid-curve -- the angle condition is pre-satisfied, so a brief corrective nudge would latch
# after only HUMAN_TURN_HOLD_S of light contact and kill steering mid-curve. Require a longer hold
# there before reading it as an intentional takeover. Route 000000bd (2026-07-14) showed the
# discriminator holds on-road: all 7 deliberate turns began with the wheel below the threshold
# (driver wound it up through 45 deg); only mid-maneuver grabs/nudges began beyond it.
HUMAN_TURN_HOLD_PRETURNED_S = 3.0
_STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL # 20 Hz lateral tick
class HumanTurnDetector:
"""Latches ``active`` once the driver holds real steering pressure AND ``|wheel angle|`` >
``HUMAN_TURN_ANGLE_DEG`` continuously for ``HUMAN_TURN_HOLD_S`` — long enough to tell an
intentional turn from a brief nudge. ``just_released`` pulses True on the first frame after the
override clears, so a mode can re-seed its command as it re-engages.
Call ``update`` once per lateral tick while control is active. Modes that want the timer to zero
on disengage call ``reset`` on their inactive path; modes that want it to persist simply stop
calling ``update`` (the timer holds its value).
"""
def __init__(self):
self.hold_timer_s = 0.0
self.active = False
self._active_last = False
self._pressed_last = False
self._press_started_preturned = False
def update(self, enabled: bool, steering_pressed: bool, steering_angle_deg: float) -> bool:
self._active_last = self.active
# Was the wheel already past the angle threshold when this press began? If so the driver is
# touching a wheel that lateral control turned (mid-curve nudge), not driving a turn -- hold
# the longer HUMAN_TURN_HOLD_PRETURNED_S before latching.
if steering_pressed and not self._pressed_last:
self._press_started_preturned = abs(steering_angle_deg) > HUMAN_TURN_ANGLE_DEG
self._pressed_last = steering_pressed
if not enabled:
self.hold_timer_s = 0.0
elif steering_pressed and abs(steering_angle_deg) > HUMAN_TURN_ANGLE_DEG:
self.hold_timer_s += _STEER_DT
else:
self.hold_timer_s = 0.0
hold_req = HUMAN_TURN_HOLD_PRETURNED_S if self._press_started_preturned else HUMAN_TURN_HOLD_S
self.active = self.hold_timer_s >= hold_req
return self.active
@property
def just_released(self) -> bool:
return self._active_last and not self.active
def reset(self) -> None:
self.hold_timer_s = 0.0
self.active = False
self._active_last = False
self._pressed_last = False
self._press_started_preturned = False
opendbc_repo/opendbc/bluepilot_lateral/core/lane_center_trim.py
+210
View File
@@ -0,0 +1,210 @@
"""
BluePilot: angle-mode "advanced lane positioning" -- a curvature-domain trim toward true
lane-line center (plus an optional user left/right bias), for use with path_angle-primary
control (``lateral_angle_ext.py``).
Under curvature-primary control, lane positioning rode on a second DBC signal (path_angle as
a heading trim, c3) layered on top of curvature (c1, the primary). In angle mode, path_angle
IS the primary signal -- there is no separate trim slot on the wire. An earlier attempt ported
the old PID controller as an additive delta on the *final* path_angle value and it never tracked
lane center correctly: path_angle there 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 bypasses every limiter angle mode already applies to
``kappa_cmd`` (deviation clip, PSCM-saturation clamp, DBC clip, soft ROC).
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. 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.
**Model-position fallback (blend, not hard gate)**: mirrors ``lateral_curv_ext.py``'s
``path_offset`` blend of ``path_offset_position`` (the model's own predicted path -- a "no
correction" baseline) vs. ``path_offset_lanelines`` (the geometric lane center), weighted by a
laneline confidence score. The *target* we correct toward is that same blend here, so:
- Good lane lines (confidence -> 1): target -> lane-line center + offset (full centering + bias).
- No/poor lane lines (confidence -> 0): target -> the model's own current path + offset, i.e. the
correction reduces to *just* the user's left/right bias riding on top of whatever the model
already wants to do -- it never goes to zero just because lane lines dropped out. This is the
case that matters on center-stripe-only roads with a curbed edge: there's no reliable lane line
to center between, but the user's bias should still nudge the car off the curb.
Confidence uses the exact formula and breakpoints ``lateral_curv_ext.py`` already uses for
``laneline_confidence`` / ``min_laneline_confidence_bp`` (smooth width-tolerance + laneline probs,
not a hard cutoff) -- reusing an already-tuned, production formula instead of inventing a second one.
"""
import numpy as np
from numpy import interp
# Laneline confidence blend -- ported verbatim from lateral_curv_ext.py's path_offset blend
# (laneline_width_tolerance / min_laneline_confidence_bp) so both control schemes agree on what
# "good lane lines" means.
_WIDTH_TOLERANCE_BP = (2.4, 2.8, 3.75, 4.25) # m - the floor is added from StarPilot
_WIDTH_TOLERANCE_V = (0.0, 0.81, 0.81, 0.59) # Adds StarPilot's low edge
_STD_TOLERANCE_BP = (0.3, 0.5) #0.3 is from StarPilot to define bad references
_STD_TOLERANCE_V = (0.81, 0.0) #0.81 is a known good value and allows it to fade to zero as std increases
_CONFIDENCE_BP = (0.6, 0.8)
_CONFIDENCE_V = (0.0, 1.0)
# Speed ramp: reuses curvature-mode's already-tuned centering-authority envelope
# (LC_PID_speed_bp/v in lateral_curv_ext.py) instead of inventing a new one.
_SPEED_RAMP_BP = (0.0, 9.0, 15.0) # m/s
_SPEED_RAMP_V = (0.0, 0.0, 1.0)
# Lookahead distance for the position-error sample, clipped to a sane range (m).
_LOOKAHEAD_MIN_M = 8.0
_LOOKAHEAD_MAX_M = 35.0
# Hard safety ceiling on the raw correction magnitude (1/m) -- fixed, not a "feel" knob, same
# treatment as _PSCM_SAT_UNWIND_RATE / _soft_roc in lateral_angle_ext.py.
_MAX_RAW_CORRECTION = 0.004
# First-order smoothing time constant (s) -- avoids abrupt jumps in the trim.
_SMOOTH_TAU_S = 0.4
# Rate-of-change limit on the applied correction (1/m per 20 Hz tick), independent of the
# smoothing filter above. The filter alone still has its *fastest* slew immediately after a
# target jump -- its per-tick step is proportional to how far the target moved, so a large,
# sudden confidence swing (e.g. crossing into/out of a too-wide merge lane, or lane lines
# dropping out entirely while the model's own path and the laneline center disagree) can still
# produce a big single-tick correction change even though it's technically "smoothed". This caps
# that step explicitly, the same way path_angle's own soft ROC (lateral_angle_ext.py) and
# curvature mode's LC_path_angle_ROC (lateral_curv_ext.py) rate-limit their outputs. At this
# rate, crossing the full -_MAX_RAW_CORRECTION..+_MAX_RAW_CORRECTION span takes ~2.7s; a more
# typical confidence-transition jump (a fraction of that) resolves proportionally faster.
_CORRECTION_ROC_PER_TICK = 0.00015
class LaneCenterTrim:
def __init__(self):
self._correction = 0.0
def reset(self) -> None:
self._correction = 0.0
def update(self, kappa_cmd: float, model, v_ego: float, enabled: bool, offset: float,
gain: float, lat_active: bool, lane_change: bool) -> float:
"""Returns ``kappa_cmd``, nudged toward (lane-blend target + ``offset``) when active.
``offset`` (m): positive shifts the target right, negative left (same sign convention as
curvature mode's ``custom_path_offset_curv``). Applied whether or not lane lines are usable
-- see module docstring.
``gain`` (0.0-1.0): user-tunable authority -- how much of the (already magnitude-clipped)
raw correction is actually applied. 0 disables the trim's effect without disabling detection.
The applied correction is both exponentially smoothed (_SMOOTH_TAU_S) and rate-limited
(_CORRECTION_ROC_PER_TICK) -- see the constants above -- so a lane-line-confidence
transition (e.g. a merge lane too wide to be trusted, then narrowing back into range) eases
into its new target instead of snapping.
"""
if not enabled or not lat_active or lane_change or model is None:
self.reset()
return kappa_cmd
speed_factor = float(interp(v_ego, _SPEED_RAMP_BP, _SPEED_RAMP_V))
if speed_factor <= 0.0:
self.reset()
return kappa_cmd
if not (np.isfinite(offset) and np.isfinite(gain)): #stops not a number from getting to steering command; validation of number
self.reset()
return kappa_cmd
valid, raw = self._raw_correction(model, v_ego, offset)
if not valid:
# Only true failure case now: model.position itself is unusable, so there's no baseline
# to offset from at all (see _raw_correction). Lane-line-only failures fall back to the
# model-position blend instead of landing here.
self.reset()
return kappa_cmd
target = float(np.clip(raw, -_MAX_RAW_CORRECTION, _MAX_RAW_CORRECTION)) * float(np.clip(gain, 0.0, 1.0)) * speed_factor
alpha = 1.0 - np.exp(-0.05 / _SMOOTH_TAU_S) # BluePilot lateral tick is 20 Hz (dt=0.05s)
filtered = float(alpha * target + (1.0 - alpha) * self._correction)
# Rate-of-change limit -- see _CORRECTION_ROC_PER_TICK. Bounds the correction's own per-tick
# delta on top of the exponential filter above, so a lane-line-confidence transition can't
# snap the trim toward its new target in one or two frames.
self._correction = float(np.clip(filtered, self._correction - _CORRECTION_ROC_PER_TICK,
self._correction + _CORRECTION_ROC_PER_TICK))
return kappa_cmd + self._correction
@property
def correction(self) -> float:
"""Telemetry: last applied correction (1/m)."""
return self._correction
def _raw_correction(self, model, v_ego: float, offset: float) -> tuple[bool, float]:
try:
pos_x = np.asarray(model.position.x, dtype=float)
pos_y = np.asarray(model.position.y, dtype=float)
if pos_x.size < 2 or pos_x.size != pos_y.size:
return False, 0.0
if not (np.isfinite(pos_x).all() and np.isfinite(pos_y).all() and np.all(np.diff(pos_x) > 0)):
return False, 0.0
lookahead = float(np.clip(v_ego, _LOOKAHEAD_MIN_M, _LOOKAHEAD_MAX_M))
model_y = float(np.interp(lookahead, pos_x, pos_y))
except (AttributeError, IndexError, TypeError, ValueError):
return False, 0.0
# Laneline contribution, blended in by confidence. scale=0 (model-position-only, i.e. just
# the user's offset bias) whenever lines are missing, low-confidence, or structurally bad --
# see _laneline_blend for the confidence formula (ported from lateral_curv_ext.py).
scale, laneline_center_y = self._laneline_blend(model, lookahead)
target_y = model_y * (1.0 - scale) + laneline_center_y * scale
error = (target_y + offset) - model_y
raw = 2.0 * error / (lookahead ** 2)
return True, float(raw)
def _laneline_blend(self, model, lookahead: float) -> tuple[float, float]:
"""Returns (scale, laneline_center_y). scale=0 whenever lanelines can't be trusted (missing,
low-probability, structurally invalid) -- center_y is unused/meaningless in that case since
it's weighted out by scale in the caller's blend."""
try:
lane_lines = model.laneLines
probs = model.laneLineProbs
stds = model.laneLineStds # a line could exist but this verifies how sure it is of where it is
if len(lane_lines) < 3 or len(probs) < 3 or len(stds) < 3:
return 0.0, 0.0
left_x = np.asarray(lane_lines[1].x, dtype=float)
left_y = np.asarray(lane_lines[1].y, dtype=float)
right_x = np.asarray(lane_lines[2].x, dtype=float)
right_y = np.asarray(lane_lines[2].y, dtype=float)
if (left_x.size < 2 or left_x.size != left_y.size or
right_x.size < 2 or right_x.size != right_y.size):
return 0.0, 0.0
if not (np.isfinite(left_x).all() and np.isfinite(left_y).all() and
np.isfinite(right_x).all() and np.isfinite(right_y).all()):
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))
width = right - left
# Same confidence formula as lateral_curv_ext.py's path_offset blend: width-tolerance
# (penalizes implausibly wide/merging-looking detections) combined with per-line
# 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))
# 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)
return scale, center_y
except (AttributeError, IndexError, TypeError, ValueError):
return 0.0, 0.0
opendbc_repo/opendbc/bluepilot_lateral/core/lateral_angle_ext.py
+569
View File
@@ -0,0 +1,569 @@
"""
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. 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.** 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. 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 ...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
FORD_DBC_PATH_ANGLE_MAX = 0.5235
# PSCM d_ref (m) vs speed (m/s) — 6 points; above ~55.6 m/s use plateau + optional cap to 5 m.
_PSCM_DREF_SPEEDS_MS = (0.0, 4.17, 27.78, 41.67, 50.0, 55.56)
_PSCM_DREF_M = (0.5, 0.95, 1.4, 2.075, 2.75, 3.875)
# Default blend ratio validated on F-150 fleet data (0.5s lookup time).
_FORD_PATH_ANGLE_BLEND_RATIO_DEFAULT = 0.50
# Variable lookup time (VLT): curvature_lookup_time adapts to speed and curvature magnitude.
# t_lookup = t_base + t_extra_max × speed_factor(v) × kappa_factor(|κ|)
# t_base = liveDelay.lateralDelay + DT_MDL — always matches the planner's pre-compensation floor.
# Extra lookahead collapses toward zero at high speed (PSCM responds faster)
# and at large curvature (prevents blend importing a "start unwinding" signal too early).
_DT_MDL = 0.05 # model loop period (matches common/realtime.py)
_VLT_T_EXTRA_MAX = 0.10 # max extra lookahead above t_base
_VLT_V_LOW_MS = 25.0 * 0.44704 # 25 mph — full extra lookahead at or below this speed
_VLT_V_HIGH_MS = 55.0 * 0.44704 # 55 mph — no extra lookahead at or above this speed
_VLT_KAPPA_FULL = 0.005 # 1/m — full extra lookahead below this curvature (200m+ radius)
_VLT_KAPPA_TAPER = 0.020 # 1/m — no extra lookahead above this curvature (50m radius)
# Rate cap on path_angle magnitude DECREASE during PSCM LimitReached (rad/call = 0.40 rad/s).
# Both model and planner naturally drop path_angle ~0.36 rad/s at a sharp 90° apex, while the PSCM is
# physically pinned and cannot execute the rapidly falling desired angle. The resulting actual-vs-desired
# gap (up to 47° observed) causes a snap correction the moment the PSCM is released. This cap limits
# the desired-angle drop rate to what the PSCM can reasonably track, at the cost of holding the car
# slightly more in the curve during saturation.
# BluePilot: this strategy runs once per STEER_STEP (CarControllerParams.STEER_STEP=5), i.e. once
# every 5th 100Hz control tick = 20Hz, not every tick. The original 0.004 rad/call value (and its
# "50Hz" comment, corrected above) was authored 2026-05-11 on bp-sid-simple, which had already
# switched STEER_STEP 5->1 (true 100Hz) on 2026-04-22 -- so it was tuned at 100Hz real cadence
# even though its own comment mistakenly said 50Hz. Scaled x5 here to restore the same real-world
# 0.40 rad/s (23 deg/s) unwind rate on this branch's actual 20Hz cadence.
_PSCM_SAT_UNWIND_RATE = 0.02 # rad/call (0.02 * 20Hz = 0.40 rad/s)
# Post-override stall blip. Road test 2026-07-14 (route 886240741b067740/000000bd--feb980680f)
# showed that after driver-touch episodes the Mach-E PSCM keeps reporting InProgress but honors
# path_angle at only ~0.56x (healthy hands-free delivery on the same route: ~0.95 median). The
# current-curvature deviation clip below then pins kappa_cmd at measured + CURVATURE_ERROR, so the
# command can never lead the car enough to overcome the attenuation -- a stall equilibrium the
# driver reads as "not engaging" (wire path_angle flat at ~4 deg for 4.5 s while desired kappa
# climbed to 3x measured, EPS motor current ~0 A). A short mode-0 pulse -- the identical
# panda-clean wire pattern the human-turn override sends, no ford.h involvement -- resets the
# PSCM's authority, after which path_angle ramps back in from zero through the soft ROC.
_STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL # 20 Hz lateral tick (matches human_turn.py)
_STALL_GAP_MIN = 2.0 * CarControllerParams.CURVATURE_ERROR # desired must lead measured by 2x the clip tolerance
_STALL_HOLD_S = 0.5 # accumulated clip-binding time before a pulse fires
_STALL_BLIP_FRAMES = 6 # mode-0 pulse length (6 frames @ 20 Hz = 300 ms; PSCM acked mode 0 in ~150 ms on-road)
_STALL_COOLDOWN_S = 2.0 # re-arm delay after a pulse (release ramp + PSCM response time)
_STALL_MAX_BLIPS = 3 # give up on a stuck episode; devLim telemetry keeps recording the stall
# Proactive hand-off blip: any sustained driver press attenuates the PSCM (route 000000be seg 4:
# 3 s of sub-45-deg circle-exit steering left it at ~0x delivery, and the reactive detector's
# fire-after-the-stall-develops timing meant 2.4 s of dead-straight running into the next curve
# before the pulse landed). Firing the same pulse on the falling edge of a sustained press resets
# the PSCM while the car is straight and the command is small -- a 300 ms lateral gap right at
# hand-off, imperceptible, instead of a missed curve. The reactive detector above stays as backstop.
_PRESS_BLIP_MIN_S = 0.5 # press must last this long before its release earns a pulse
# The pulse releases steering for 300 ms; never fire it in a curve.
_BLIP_MAX_PATH_ANGLE = 0.10 # rad
def pscm_d_ref_m(v_ego_ms: float) -> float:
v = max(float(v_ego_ms), 0.0)
d = float(np.interp(v, _PSCM_DREF_SPEEDS_MS, _PSCM_DREF_M))
if v > _PSCM_DREF_SPEEDS_MS[-1]:
# Doc: d_ref table ends at 3.875 m; contribution saturates for high speed — cap at 5 m.
d = min(5.0, d)
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, 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 (fixed)
self.vlt_extra_max = _VLT_T_EXTRA_MAX
# 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
# 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 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_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 (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.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.
# 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
# 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
self.stall_blip_count = 0 # pulses fired this stall episode
self.angle_stall_blip_active = False
self.press_timer_s = 0.0 # continuous steeringPressed time, for the hand-off blip
def update_angle_params(self, params):
"""Sets per-platform gain defaults and reads user angle-tuning params."""
fp = getattr(self.CP, 'carFingerprint', '')
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 = ANGLE_GAIN_CAN
self.path_angle_gain_lowC_highV = low
self.path_angle_gain_highC_highV = high
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):
"""
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).
"""
v_ego = float(CS.out.vEgoRaw)
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
path_angle = 0.0
ramp_type = 0
lateral_uncertainty = 0.0
precision = 1
if not CC.latActive:
self.shared.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_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
# latActive), and ford.h latches the shadow from every LKA frame -- so the latched
# value must track reality here, not sit at a stale zero. Otherwise the first enabled
# LMC frame after (re-)engage races LKA's 33Hz latch against LMC's 20Hz enable bit and
# 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.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.shared.precision_type = 1
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=0.0,
)
# Human-turn override: sustained driver press + large wheel angle → force lateral inactive
# (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.
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.shared.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_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.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.
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.shared.precision_type = 1
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=0.0,
)
# Proactive hand-off blip: the falling edge of a sustained press earns an immediate mode-0
# pulse (see _PRESS_BLIP_MIN_S) -- resets the PSCM's press-induced attenuation right at
# hand-off, while the car is straight and the command small, instead of waiting for the
# reactive stall detector below to watch the car miss the next curve first.
if CS.out.steeringPressed:
self.press_timer_s += _STEER_DT
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.shared.path_angle_last) < _BLIP_MAX_PATH_ANGLE):
self.stall_blip_frames_left = _STALL_BLIP_FRAMES
self.press_timer_s = 0.0
# Stall-blip pulse in progress: hold lateral inactive (mode 0, all-zero signals -- the same
# wire pattern as the human-turn override, no ford.h involvement) for _STALL_BLIP_FRAMES so the
# PSCM drops its post-override attenuation, then release; path_angle ramps back in from zero
# through the soft ROC exactly like a human-turn release. Detection lives at the end of the
# normal flow below.
if self.stall_blip_frames_left > 0:
self.stall_blip_frames_left -= 1
self.angle_stall_blip_active = True
self.shared.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_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.inputs.get_current_curvature(CS)
self._desired_curvature_last = float(actuators.curvature)
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(
apply_curvature=0.0,
curvature_rate=0.0,
path_offset=0.0,
path_angle=0.0,
ramp_type=0,
precision_type=1,
lateralUncertainty=0.0,
)
self.angle_stall_blip_active = False
self.shared.precision_type = 1
precision = 1
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.
# Cap liveDelay at 0.15s for VLT purposes. liveDelay can calibrate up to ~420ms on some runs, which inflates
# VLT to 0.6s and pushes the model lookahead 5m into the curve. At that depth the model sees full peak
# 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.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.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
else:
_kappa_factor = float(interp(abs(desired_curvature), [_VLT_KAPPA_FULL, _VLT_KAPPA_TAPER], [1.0, 0.0]))
curvature_lookup_time = _t_base + self.vlt_extra_max * _speed_factor * _kappa_factor
self.bp_curvature_lookup_time = curvature_lookup_time
predicted_curvature = 0.0
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, MODEL_T_IDXS, curvatures)
)
b = float(self.path_angle_blend_ratio)
b = float(clip(b, 0.0, 1.0))
# Exit-biased blend: near the PSCM authority limit or while the planner is actively
# reducing curvature (exit detected), drop model prediction weight from 60% → ~15%.
# This lets the planner's natural unwind dominate instead of being diluted by a model
# prediction that still sees the curve (→ seg-14 slow unwind) or that snaps when its
# lookahead window crosses the curve exit (→ seg-17 snap + reverse PSCM hit).
# Normal gentle curves are unaffected: no PSCM limit, no falling desired → full b=0.60.
_pscm_lim = getattr(CS, 'lat_ctl_lim_stat', 0)
# In angle mode, LatCtlLim_D_Stat (→ lat_ctl_lim_stat) does not fire.
# Previously used angleState.saturated (CtrSat) as a proxy, but CtrSat fires whenever the car
# lags the commanded path_angle by > 2.5° — which happens during any normal curve entry.
# 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.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
# was tuned to trigger on planner unwind faster than 0.2 (1/m)/s. Scaled x5 here to restore
# that same real-world trigger rate on this branch's actual 20Hz cadence; unscaled it fired at
# 0.04 (1/m)/s, collapsing the model blend on mild straightening instead of genuine exits.
# Same bug class and fix as _PSCM_SAT_UNWIND_RATE and _soft_roc above.
_desired_falling = abs(desired_curvature) < abs(self._desired_curvature_last) - 0.010
_on_exit_near_limit = not _kappa_entering and (_pscm_lim >= 1 or _in_hard_sat or _desired_falling)
b_blend = float(clip(b * 0.25, 0.0, 1.0)) if _on_exit_near_limit else b
requested_curvature = predicted_curvature * b_blend + desired_curvature * (1.0 - b_blend)
self._desired_curvature_last = desired_curvature
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, LANE_CHANGE_FACTOR_BP, [LANE_CHANGE_FACTOR_LOW, self.lane_change_factor_high_ang]
)
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.inputs.model.meta.laneChangeDirection == 2 and requested_curvature > 0:
requested_curvature *= lane_change_factor
precision = 0
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
# (and therefore path_angle, and the shadow_curvature sent to ford.h) can legitimately lead the
# measured curvature by more than ford.h's angle-error tolerance during normal curve entry/exit
# -- the shadow-curvature deviation check (ford_shadow_curvature_error_check) would then block
# 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.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)? 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)]
)
self.high_gain_calc = interp(v_ego, [13.5, 26.82], [(1.30 * self.low_speed_curv_factor), (self.path_angle_gain_highC_highV * self.high_speed_curv_factor)])
# As the curve gets bigger, we will need a little boost to the signal to to not understeer
self.curvature_factor = interp(abs(kappa_cmd), [0.0007, 0.001], [self.low_gain_calc, self.high_gain_calc])
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).
# LimitClose (_pscm_lim >= 1 only): block magnitude increases — exit-biased blend provides unwind.
# Hard saturation (_in_hard_sat): block increases AND rate-limit decreases to _PSCM_SAT_UNWIND_RATE.
# 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.shared.path_angle_last
_last_mag = abs(_last)
_curr_mag = abs(path_angle)
if _curr_mag > _last_mag: # magnitude growing — block
path_angle = _last
elif _last_mag - _curr_mag > _PSCM_SAT_UNWIND_RATE: # decreasing too fast — rate-limit
_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.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))
# Soft ROC limit — unconditional, slightly tighter than ford.h, applied before the
# hardware bypass in ford.h is re-enabled. Lets us observe whether the limit would
# suppress control and tune it, while the PSCM still receives the clipped value.
# BluePilot: this strategy runs once per STEER_STEP (CarControllerParams.STEER_STEP=5), i.e.
# once every 5th 100Hz control tick = 20Hz, not every tick -- ported "verbatim from bp-sid-simple"
# (2026-06-13), which runs STEER_STEP=1 (true 100Hz, switched 2026-04-22). The y-values below are
# scaled x5 from the original [0.011, 0.011, 0.0085, 0.0018] to restore the same real-world rate
# (63/63/49/10 deg/s at v=9-10/15/25) on this branch's actual 20Hz cadence. See ford.h's
# FORD_PATH_ANGLE_LIMITS, which must mirror this scaling (x1.02 looser) to stay a true backstop.
_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.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
# Telemetry / state
self.bp_path_angle_gain_lowC_highV = self.path_angle_gain_lowC_highV
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.shared.path_angle_last = path_angle
self.bp_path_angle_final = path_angle
# 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
# kappa can't follow the wheel: the driver moves the measured curvature faster than the
# deviation clip tracks it, so the shadow can exit ford.h's error band mid-curve -- the one
# 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.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
# curvature mode applies), so only the rate-of-change portion remains to simulate here.
_equiv_curv_rl = apply_std_steer_angle_limits(kappa_cmd, self.sim_curvature_last, v_ego,
CS.out.steeringAngleDeg, CC.latActive, BP_ANGLE_LIMITS)
self.bp_curvature_rate_limited = bool(abs(_equiv_curv_rl - kappa_cmd) > 1e-9)
self.sim_curvature_last = float(_equiv_curv_rl)
# Post-override stall detection (mechanism in the module constants' comment). Fires the mode-0
# blip when, hands-free, desired curvature has led measured by more than 2x the deviation
# clip's tolerance while the clip was actually binding for _STALL_HOLD_S accumulated seconds.
# devLim flickers mid-stall (~63% duty on the diagnosis route), so off frames hold the
# accumulator rather than resetting it; a closed gap or driver press ends the episode.
self.stall_blip_cooldown_s = max(0.0, self.stall_blip_cooldown_s - _STEER_DT)
_stall_gap = desired_curvature - current_curvature
_stalled = (not CS.out.steeringPressed and not self.lane_change and v_ego > 9.0
and abs(_stall_gap) > _STALL_GAP_MIN
and abs(desired_curvature) > abs(current_curvature))
if _stalled:
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.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
else:
self.stall_blip_hold_s = 0.0
if CS.out.steeringPressed or abs(_stall_gap) < 0.5 * _STALL_GAP_MIN:
self.stall_blip_count = 0 # episode over: the car is tracking again or the driver took it
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.shared.precision_type,
lateralUncertainty=lateral_uncertainty,
)
opendbc_repo/opendbc/bluepilot_lateral/core/lateral_curv_ext.py
+448
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/core/lateral_inputs.py
+54
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/core/values_ext.py
+148
View File
@@ -0,0 +1,148 @@
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.
# 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.
# Index 10 (FORD_MONDEO_MK5) is reserved but unmapped here: that platform exists in
# BluePilot but not in this opendbc baseline. 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 this repo's CAR doesn't have (e.g.
# Mondeo MK5, which BP7 and StarPilot have; note the hosts with each); any name a host's CAR lacks
# must be on it (tests/test_platform_tables.py).
FORD_HOST_ONLY_PLATFORMS: frozenset[str] = frozenset()
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_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",
})
opendbc_repo/opendbc/bluepilot_lateral/host_types.py
+48
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/hosts/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/hooks.patch
+270
View File
@@ -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"
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/host.sh
Executable
+92
View File
@@ -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 <dir> [ref] <dir>/iq: the IQPilot folders iqdbc's code imports; <dir>/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 <dir>/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 <dir> build <dir>/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 <dir> write hooks.patch from <dir>/host: every change outside the library since IQPilot's
# commit, committed or not. Change a hook: tree, edit <dir>/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 <dir> [ref]" >&2
exit 2
;;
esac
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/known_failures.txt
+4
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/lateral.py
+21
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/hosts/iqpilot/safety_param.py
+14
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/lateral.py
+40
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot/safety_param.py
+37
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/lateral.py
+21
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/hosts/sunnypilot/safety_param.py
+15
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/params/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/params/manifest.py
+137
View File
@@ -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 <pkg>.bluepilot_lateral.params.manifest` prints the lines for a host's params_keys.h (<pkg>: 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()))
opendbc_repo/opendbc/bluepilot_lateral/params/param_store.py
+17
View File
@@ -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
opendbc_repo/opendbc/bluepilot_lateral/safety/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/safety/ford.h
+383
View File
@@ -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;
}
opendbc_repo/opendbc/bluepilot_lateral/safety/ford_declarations.h
+99
View File
@@ -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 <pkg>/safety/tests/libsafety/safety.c
// (the same flags <pkg>/safety/tests/libsafety/libsafety_py.py uses, plus FORD_BP_DEBUG; <pkg> is the
// host's opendbc package: opendbc, iqdbc).
#ifdef FORD_BP_DEBUG
#include <stdio.h>
#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);
opendbc_repo/opendbc/bluepilot_lateral/safety/lateral.h
+121
View File
@@ -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;
}
opendbc_repo/opendbc/bluepilot_lateral/safety/lateral_declarations.h
+60
View File
@@ -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);
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_coverage.sh
Executable
+43
View File
@@ -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 <pkg>/safety/, and the headers live in <pkg>/bluepilot_lateral/safety/. <pkg> 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 <pkg>/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!"
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/bluepilot_mutation.py
+47
View File
@@ -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 <pkg>/safety/, and the BluePilot headers live in
the library (<pkg>/bluepilot_lateral/safety/). This points the same run at them; the killer tests
are the ones mutation.py already discovers in <pkg>/safety/tests/, the BluePilot ones through the
loader there (test_bluepilot_lateral.py). Same options as mutation.py. <pkg> 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())
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/ford_bluepilot_common.py
+268
View File
@@ -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<slip>{_NUM})f, \.steer_ratio = (?P<sr>{_NUM})f, \.wheelbase = (?P<wb>{_NUM})f\}},\s*// (?P<idx>\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))
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot.py
+58
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_angle_mode.py
+99
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_limits.py
+463
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_matrix.py
+125
View File
@@ -0,0 +1,125 @@
#!/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)
self.SAFETY_PARAM = host.ford_param(self.SAFETY_PARAM, self.PARAM_SP) # StarPilot: the bits ride the Ford param
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()
opendbc_repo/opendbc/bluepilot_lateral/safety/tests/test_ford_bluepilot_pinion.py
+260
View File
@@ -0,0 +1,260 @@
#!/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_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, every Python index entry points at a C row.
# C may carry rows for platforms upstream has since dropped (row 10, Mondeo): unused, harmless.
# 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), "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()
opendbc_repo/opendbc/bluepilot_lateral/tests/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/tests/host.py
+79
View File
@@ -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)
opendbc_repo/opendbc/bluepilot_lateral/tests/test_bluepilot_upstream_copies.py
+102
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_core_imports.py
+59
View File
@@ -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 <host> 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(["<host>", *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 = ("<host>.bluepilot_lateral.hosts", *(f"<host>.{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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_host_types.py
+50
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_assist_data1.py
+49
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_lane_center_trim.py
+307
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_lat_ctl2_checksum.py
+48
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_inputs.py
+121
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_lateral_status.py
+89
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_param_store.py
+74
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_platform_tables.py
+37
View File
@@ -0,0 +1,37 @@
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_PINION_GEOMETRY_INDEX,
FORD_PINION_WHEELBASE,
)
TABLES = {
"FORD_PINION_GEOMETRY_INDEX": FORD_PINION_GEOMETRY_INDEX,
"FORD_PINION_WHEELBASE": FORD_PINION_WHEELBASE,
"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)
if __name__ == "__main__":
unittest.main()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_safety_param_bits.py
+86
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_settings_manifest.py
+100
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tests/test_vendor_tools.py
+93
View File
@@ -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()
opendbc_repo/opendbc/bluepilot_lateral/tools/check_vendored.sh
Executable
+25
View File
@@ -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 <host folder>
set -euo pipefail
if [ $# -ne 1 ]; then
echo "usage: $0 <folder>" >&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
opendbc_repo/opendbc/bluepilot_lateral/tools/make_hook_patch.sh
Executable
+24
View File
@@ -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[@]}"
opendbc_repo/opendbc/bluepilot_lateral/tools/test_on_host.sh
Executable
+35
View File
@@ -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 <sunnypilot/opendbc checkout> [base]
set -euo pipefail
if [ $# -lt 1 ]; then
echo "usage: $0 <sunnypilot/opendbc checkout> [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 .)
opendbc_repo/opendbc/bluepilot_lateral/tools/vendor.sh
Executable
+21
View File
@@ -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 <ref> (files the library doesn't have are removed), minus what
# .gitattributes marks export-ignore (.github/), plus VERSION: "<tag> <commit>" ("untagged" when <ref> 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 <ref, a release tag> <host folder, e.g. .../iqdbc/bluepilot_lateral>
set -euo pipefail
if [ $# -ne 2 ]; then
echo "usage: $0 <ref> <folder>" >&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)"