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

Ford angle steering and Panda routing

Browse Source
This commit is contained in:
firestar5683
2026-10-10 07:10:07 -06:00
parent 71b02a7975
commit 1279c092b1
84 changed files with 6336 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CREDITS.md
+4
View File
@@ -17,3 +17,7 @@ The Ford lateral strategies, extended CAN constructors, camera-radar filtering a
Relevant lineage includes the controller and integration work in [`db2bdff05`](https://github.com/BluePilotDev/bluepilot/commit/db2bdff05df103d71df62f45c2a3cb5211aba6e6) and [`d0aac605f`](https://github.com/BluePilotDev/bluepilot/commit/d0aac605f99d37e9da205e419f7989c1e9eaa386), extended CAN and safety work in [`8f8d6d15f`](https://github.com/BluePilotDev/bluepilot/commit/8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d), and manual-turn detection in [`97867c1eb`](https://github.com/BluePilotDev/bluepilot/commit/97867c1eb57b7472f6fc3de62f0fef576e5a5497).
The vehicle strategy and CAN/native owners preserve that attribution while adapting to the current controller interfaces and shared safety limits. Upstream contributors do not maintain or endorse this adaptation. Applicable notices are retained in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md).
## Optional Ford angle controller
The vendored `opendbc_repo/opendbc/bluepilot_lateral` angle core, codecs and Ford-only safety hooks are adapted directly from [BluePilot bp-7.0 e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a), principally by Alan Polk, with John Christman, Jacob Neulight, Nathan Ingraham, Praeuner and other BluePilot, sunnypilot and comma contributors. Exact source paths and intentionally stricter safety checks are recorded in that folder. Both upstream `LICENSE` and `LICENSE.md` notices are retained. Its layout follows Alan Polk's September 2026 library proposal; this is a StarPilot adapter, not a published BluePilot library release.
THIRD_PARTY_NOTICES.md
+10
View File
@@ -74,3 +74,13 @@ the exclusion of the other.
## BluePilot Ford support
The Ford lateral strategies, CAN constructors, camera-radar filtering and native checks include work adapted from BluePilot's `bp-7.0` source, through StarPilot `2efe6017bfe14cee4807833e05f9a7f6ba255cdb`. See [CREDITS.md](CREDITS.md#ford-support-adapted-from-bluepilot) for contributors and source revisions. The upstream extension-file and `LICENSE.md` notices reproduced above also apply to the Ford extension sources; they are retained without resolving the difference between their published license descriptions.
## Optional BluePilot angle library adapter
The angle-only adapter in `opendbc_repo/opendbc/bluepilot_lateral` is sourced directly from BluePilot bp-7.0 commit e1d051d7ba270261b4455068bd68f1a58db15a4a. The complete upstream MIT notice is retained in that folder's `LICENSE`, and the simultaneously published Custom MIT notice is retained verbatim in `LICENSE.md`. The applicable SUNNYPILOT acknowledgments reproduced above are also retained in its `CREDITS.md`. This preserves the published terms without resolving their scope or representing the adapted extension as unambiguously plain MIT.
The optional, default-disabled angle lane-centering trim additionally derives from
BluePilot bp-dev e22afa6be9b881fa784c92ebb316db47728a3d81. Its exact source and
contributors are recorded in the library CREDITS.md; the same root/opendbc MIT
and Custom MIT notices coexist unchanged at that revision. This adds attribution,
not a license-scope decision or permission grant.
opendbc_repo/opendbc/bluepilot_lateral/CREDITS.md
+35
View File
@@ -0,0 +1,35 @@
# BluePilot angle source
Adapted from BluePilotDev/bluepilot bp-7.0 at
[e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a),
principally Alan Polk, with John Christman, Jacob Neulight, Nathan Ingraham,
Praeuner, tonesto7, Haibin Wen and other BluePilot, sunnypilot and comma contributors.
Source paths: opendbc/sunnypilot/car/ford/lateral_angle_ext.py,
fordcan_ext.py, human_turn.py and opendbc/safety/modes/ford.h.
The simultaneous upstream LICENSE and LICENSE.md notices are retained.
This software is licensed under a custom license requiring permission for use.
This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use.
This is a StarPilot adapter to the September 2026 proposed library shape.
It is not a released BluePilot library, tag, or claim of BluePilot validation.
The existing curvature controller remains StarPilot-owned. Pinion curvature,
curvature-primary settings and the unsafe native reset bypass are not imported.
The exact upstream root notices are additionally retained as upstream-root-LICENSE
and upstream-root-LICENSE.md; LICENSE/ LICENSE.md retain the opendbc notices.
ORIGIN_HISTORY.tsv records the inspected path history with commit IDs/authors/dates.
The private compact upstream root clone preserves its reachable root history.
Pre-conversion opendbc submodule objects are not included in that root history.
Angle lane-centering trim is separately adapted from BluePilot bp-dev
[e22afa6be9b881fa784c92ebb316db47728a3d81](https://github.com/BluePilotDev/bluepilot/commit/e22afa6be9b881fa784c92ebb316db47728a3d81),
`opendbc/sunnypilot/car/ford/lane_center_trim.py` (blob
`5eedebd79e99493624f9d6af0750d1dd5713306c`). Alan Polk authored the initial
trim, model-position fallback, correction slew and default-strength changes;
ghbarker contributed reference/NaN guards and tests. The source explicitly credits
StarPilot/u/jc01rho for curvature-domain placement/geometry. The core adaptation
adds immutable host projection and conservative stale/driver-pressure suppression.
The pinned bp-7.0 controller baseline and default-disabled policy are unchanged.
Both simultaneous root/opendbc MIT and Custom MIT notices also exist unchanged at
this trim revision; all retained notices and permission acknowledgments apply
without assigning an exclusive license or claiming a new permission grant.
opendbc_repo/opendbc/bluepilot_lateral/LICENSE
+7
View File
@@ -0,0 +1,7 @@
Copyright (c) 2020, Comma.ai, Inc.
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
opendbc_repo/opendbc/bluepilot_lateral/LICENSE.md
+21
View File
@@ -0,0 +1,21 @@
# Custom MIT License
Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions:
1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s).
2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment:
"This software is licensed under a custom license requiring permission for use."
3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment:
"This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use."
4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Contact sunnypilot Support <support@sunnypilot.ai> for permission requests.
---
Haibin Wen, SUNNYPILOT LLC
opendbc_repo/opendbc/bluepilot_lateral/ORIGIN_HISTORY.tsv
+38
View File
@@ -0,0 +1,38 @@
9d190417bf74563abfa6f36996f1e673e9ee0a1b Praeuner 2026-08-13T22:26:15-05:00 Update dampening factor to .25
a881646ea5edc21c0db4c4e74076e0dc14671a4e alan-polk 2026-08-10T09:04:52-05:00 Merge pull request #176 from BluePilotDev/bp-no-stall
9012f76666a5c90764fcaec40832b9a607488c27 John Christman 2026-08-05T17:49:59-04:00 anti-stall
3610e3f18a0ad37d793d7ada61dbc739ab1077a3 Nathan Ingraham 2026-07-23T22:26:29-05:00 self.path_angle_gain_lowC_highV remains the platform-specific base gain. The toggle now controls separate self.user_dampening_factor. l ow_gain_calc uses: self.path_angle_gain_lowC_highV * self.user_dampening_factor UI placement, description, default, and 0.75–1.25 range remain unchanged.
1db52bc79e607ddb00214254dd4d732615e27fe4 Nathan Ingraham 2026-07-23T00:00:51-05:00 -Added “High Speed Dampening” directly below “High Speed Adjustment Factor”, range 0.75–1.25, step 0.01, and default 1.0.
5d994c24edf63f3b9004b2963f18ef4199e5d368 Jacob Neulight 2026-07-18T20:17:04-06:00 Ford safety: optional pinion-sourced angle_meas behind STEER_ANGLE_CURVATURE SP flag
699c17d9fde62c690eed9d68eed7d731744d5137 Jacob Neulight 2026-07-18T19:53:46-06:00 Ford: publish angle-mode shadow curvature from measured curvature during overrides
b7428fee4b116d4832f91c850d54cac2730de2ef alan-polk 2026-07-18T08:52:24-05:00 restore curvature safety checks to bp-6.0 logic.
97867c1eb57b7472f6fc3de62f0fef576e5a5497 alan-polk 2026-07-16T19:18:05-05:00 Human Turn Reset for Angle (#118)
7efee67e3e643d52b973895d6c6765c5cc12e314 alan-polk 2026-07-16T08:19:43-05:00 angle mode: rescale exit-detection threshold for 20Hz cadence
1958a3501ead10ae1f496f0d11e1e8cda6e2dbcc alan-polk 2026-07-14T15:22:38-05:00 Bp 7.0 variable cleanup (#111)
8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d alan-polk 2026-07-14T08:53:28-05:00 Bp 7.0 panda2 (#109)
11ac3b760671b489ca449d522f1edf240a232884 alan-polk 2026-07-09T07:53:21-05:00 angle mode: fix ROC values tuned for 100Hz cadence running at actual 20Hz
dbdcea747e65f574d326a55c9bf1a9d6c9485e22 alan-polk 2026-07-08T09:21:32-05:00 Revert "ford: add current-curvature deviation check to angle mode (panda-side)"
69a9714563622729ba304ac75354ba3fd37b0b39 alan-polk 2026-07-08T09:17:09-05:00 ford: add current-curvature deviation check to angle mode (panda-side)
be7f86a5d0dc1ea3b3f0052d4b54ff0b8a70f843 John Christman 2026-07-06T21:53:55-04:00 cast on debug
5c1fbb07dd4d7d950a01cb2cd1faddd90d41e632 alan-polk 2026-07-06T14:20:49-05:00 controllerStateBP: add angleRateLimited + curvatureRateLimited diagnostics
06d654e5f48ea8fcffce6e98e48e3a30450b5097 alan-polk 2026-06-25T23:28:51-05:00 Revert "temp blend ratio change" — restore predicted-curvature blend to 0.50
540e69fcf4861a2ce5d7ca41467effc231638e27 alan-polk 2026-06-22T08:35:44-05:00 temp blend ratio change
e3819c7e630d85cfb44858840124fe7f72f1762d alan-polk 2026-06-13T20:47:09-05:00 ford.h: don't block angle mode on the inactive curvature signal
5d343738e30267688d0ffa6eebe2ec3ca57b10ee alan-polk 2026-06-13T18:28:30-05:00 ford.h: widen path_angle ROC to 2% looser than lateral_angle_ext _soft_roc
d0aac605f99d37e9da205e419f7989c1e9eaa386 alan-polk 2026-06-13T11:15:50-05:00 BluePilot: add angle-primary lateral control mode
51d5de8991508c862e7c51333e5055425b2b8d23 alan-polk 2026-04-11T12:00:10-05:00 fixing missing punctation in ford.h
8194ceb7e0bbc112c73d393a8f0add3261f6592d alan-polk 2026-04-11T11:51:49-05:00 make safety limits symetrical but control limits asymetrical. Add 1.5s duration to detect human turn
db2bdff05df103d71df62f45c2a3cb5211aba6e6 alan-polk 2026-04-09T23:47:30-05:00 move all external logic to lateral_curv_ext.py. Apply some order of operations fixes to try and eleminate panda blocking messages.
1a8ac23b5bd5d7952c15eb913c2b1e39f14ad92e Blue Genie 2026-04-08T15:10:24-05:00 Adjust panda limits
892be076daf898d806983f847c72357accff7312 Blue Genie 2026-04-08T11:45:28-05:00 Add logging functionality
ce7d27230657159332f64a253a0faea6cf8dcc95 alan-polk 2026-04-08T11:40:02-05:00 fix rate of change limits
021b5146e80ac58fe2e81dbd848bc7cd409ef08e alan-polk 2026-04-07T16:24:38-05:00 align ford.h limits with dbc and carcontroller
7f9212f7f8dc22c4a0a22443158554f4d3652a3c tonesto7 2026-04-05T21:28:37-04:00 Merge bp-6.1 upstream changes into bp-6.0-dev (#124)
dd1a9d3fd8f60462d3d8ac6de3e7860e70e1f667 alan-polk 2025-12-12T12:22:31-06:00 convert opendbc from a submodule to a subfolder
10101012d8a4cc37951a557023824cdd995443a6 alan-polk 2026-08-10T17:23:04Z BluePilot: angle-mode lane centering trim (advanced lane positioning)
560e80e9d593badca08aa60e8ddb692e58033425 alan-polk 2026-08-11T12:50:00Z BluePilot: angle-mode lane centering falls back to model position, not zero
94bf8144d4a683e9f3408f3babf892878f3b81f4 ghbarker 2026-08-12T01:31:33Z ford ALP: restore StarPilot guards (narrow lane, laneline std, NaN params) and cap applied correction at 0.0015
80825b81568ad684fe4ec37a797d3cbaa004fdcf ghbarker 2026-08-13T01:04:02Z split applied-correction ceiling into its own PR per review
e5b92cc48b34ea43ac756a96a96687c9851a188d ghbarker 2026-08-13T01:05:07Z add tests for the restored guards
af4bc410c9ef417ba723212ceaa2a77758dd8ecb alan-polk 2026-08-14T03:24:28Z BluePilot: rate-limit the lane centering correction, not just filter it
8eea9eb68e0c4126ee1c52f251e274d5865b6743 alan-polk 2026-08-19T13:33:00Z BluePilot: lower angle-mode lane centering default strength to 0.25
Can't render this file because it contains an unexpected character in line 14 and column 85.
opendbc_repo/opendbc/bluepilot_lateral/README.md
+81
View File
@@ -0,0 +1,81 @@
# BluePilot Ford angle control
StarPilot adaptation of BluePilotDev/bluepilot `bp-7.0`, pinned to
[e1d051d7ba270261b4455068bd68f1a58db15a4a](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a).
The layout follows Alan Polk's September 2026 library proposal. This is an
adaptation, not an upstream library release.
Existing curvature control remains the default. Explicit `FordLateralMode=1`
selects angle control next drive; the saved choice persists when unavailable.
Only this selector is exposed. Fixed defaults and bounds live in
`params/manifest.json`; pinion-yaw correction and curvature-primary tuning are
outside this library. Angle lane-centering trim is available as a default-disabled
core capability, separately sourced from BluePilot bp-dev
`e22afa6be9b881fa784c92ebb316db47728a3d81`; it adds no public settings.
`core/` accepts scalar/model inputs without openpilot, Params or messaging
imports. `can/` owns encoding, `safety/ford_bp.h` owns native enforcement, and
`hosts/starpilot.py` connects the host. `params/protocol.json` generates the wire
constants. The folder is vendored without submodules, LFS or an external runtime.
## Admission
Classic: Bronco Sport Mk1, Escape Mk4, Focus Mk4, Maverick Mk1, Explorer Mk6.
CAN FD: Escape Mk4.5, Expedition Mk4, F150 Mk14, F150 Lightning Mk1, Ranger Mk2,
Mustang Mach-E Mk1.
Exact classic safety parameters 32/33 map to angle 128/129; CAN FD 18/19 and
66/67 map to 130/131. Parameter 131 requires DEBUG; RELEASE retains stock 130.
Longitudinal, radar, fingerprint and parser policy remain host-owned. Admission
uses the final CarParams contract, not brand or steering-control enum alone.
AOL, alternative experiences, Transit, Edge, Mondeo, SecOC, passive, dashcam,
notCar and unsupported static flags are excluded. Auxiliary noOutput requires
parameter zero; production deployments with two Pandas remain unqualified until
actual startup, bus routing and public acknowledgments are verified.
## Safety design
The extension uses small Ford hooks without changing shared safety headers or
existing curvature namespaces. Active steering requires ordinary controls
permission, healthy physical RX and a fresh matching public Panda acknowledgment
of safety parameter, alternative experience, controls, RX validity, heartbeat and
faults. Cruise, gas/brake, checksum, counter, quality and freshness checks apply.
Neutral mode cannot grant permission.
The BluePilot reset-bypass latch is omitted. Rejected frames preserve accepted
actuator history and withdraw the shadow lease; accepted neutral commands or
permission withdrawal reset history. Lost counters recover through neutral
resynchronization. Version or checksum failure cannot fall through to curvature
control.
Active frames carry neutral curvature, offset and curvature rate, with checked
mode, ramp, precision, path angle, counter, checksum and cadence. Native enforcement
checks the shadow against the actual path across gains 0.95–1.3, bounds both by
`min(0.02, 2.4114/v²)`, and bounds the full actual-curvature interval against
measured curvature above 10 m/s. Actual path also obeys the 3.5886 m/s³ equivalent
jerk limit and rolling message-rate limit. Core quantization and timing margins
keep packed commands inside these bounds; an impossible intersection is neutral.
The versioned 0x3CA announcement preserves StarPilot's existing byte 4 bit 1
curvature flag and requires it clear in angle mode. It carries a counter, signed
shadow curvature, protocol version and checksum; its lease expires after 60 ms.
The shadow derives from the final path after rate limiting and saturation.
Control computes at 20 Hz, announces through 33 Hz LKA, then sends the latest
announced result at 20 Hz, adding at most one steering tick. Permission withdrawal
sends neutral immediately. Human-turn recovery uses bounded neutral blips without
a native bypass. The retained Dom factory steering delay is 0.22 s; pinned
BluePilot uses 0.20 s.
These stricter native limits and versioned metadata intentionally differ from
BluePilot 7.0. Identical bytes or safety decisions are not claimed for affected
commands. Runtime and physical ECU qualification remain pending.
## Attribution and notices
See `CREDITS.md`, `ORIGIN_HISTORY.tsv` and the project `THIRD_PARTY_NOTICES.md`.
Both upstream MIT and Custom MIT notices are retained verbatim in `LICENSE`,
`LICENSE.md`, `upstream-root-LICENSE` and `upstream-root-LICENSE.md`. The maintainer
reports author permission to use this adaptation under MIT. Both published notices
remain intact.
opendbc_repo/opendbc/bluepilot_lateral/__init__.py
+34
View File
@@ -0,0 +1,34 @@
"""Adapter to the proposed BluePilot library; angle only, exact source pinned."""
from opendbc.bluepilot_lateral.can.ford import lateral, lka
from opendbc.bluepilot_lateral.core.angle import Command
from opendbc.bluepilot_lateral.hosts.starpilot import Host, qualified
class BluePilotLateral:
def __init__(self, CP, *, host=None):
if not qualified(CP):
raise ValueError("Angle mode is not configured")
self.host = host or Host(CP)
self.canfd = bool(CP.flags & 1)
self.pending = Command()
self.announced = Command()
self.announced_ns = 0
self.lka_counter = 0
self.apply_curvature_last = 0.0
def step_lateral(self, CC, CS, packer, CAN, frame, now_ns):
self.pending = self.host.update(CC, CS)
command = self.announced
# A pending permission withdrawal is immediate; an increase waits until
# the 33Hz side channel has announced the matching final actuator.
if not self.pending.active or not 0 < self.announced_ns <= now_ns <= self.announced_ns + 60_000_000:
command = Command()
self.announced = command
return [lateral(packer, CAN.main, command, canfd=self.canfd, counter=frame // 5)]
def step_lka(self, packer, CAN, now_ns):
self.announced = self.pending
self.announced_ns = now_ns
message = lka(packer, CAN.main, self.announced.shadow_curvature, self.lka_counter)
self.lka_counter = (self.lka_counter + 1) % 8
return [message]
opendbc_repo/opendbc/bluepilot_lateral/can/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/can/ford.py
+37
View File
@@ -0,0 +1,37 @@
"""Ford angle codecs, adapted from BluePilot fordcan_ext.py at e1d051d7.
Core commands use planner signs; path and shadow are negated on the wire.
The v1 side channel retains byte4 bit1 for StarPilot's curvature announcement.
"""
from opendbc.bluepilot_lateral.params import protocol as p
def checksum4(dat):
return (15 - (sum(dat[:7]) & 15)) & 15
def lka(packer, bus, shadow, counter):
addr, original, _ = packer.make_can_msg("Lane_Assist_Data1", bus, {"LaRefAng_No_Req": 0., "LaCurvature_No_Calc": 0.})
dat = bytearray(original)
raw = max(-20000, min(20000, round(-shadow / p.SHADOW_SCALE))) & 0xFFFF
dat[4] |= p.ANGLE_MASK | ((counter % p.COUNTER_MODULUS) << p.COUNTER_SHIFT)
dat[5], dat[6] = raw >> 8, raw & 255
dat[7] = p.VERSION | (checksum4(dat) << p.CHECKSUM_SHIFT)
return addr, bytes(dat), bus
def lateral(packer, bus, command, *, canfd, counter):
values = {"HandsOffCnfm_B_Rq": 0, "LatCtlRampType_D_Rq": 2 if command.active else 0,
"LatCtlPrecision_D_Rq": command.precision, "LatCtlPathOffst_L_Actl": 0.0,
"LatCtlPath_An_Actl": -command.path_angle, "LatCtlCurv_No_Actl": 0.0}
if canfd:
values.update({"LatCtl_D2_Rq": int(command.active), "LatCtlCrv_NoRate2_Actl": 0.0,
"LatCtlPath_No_Cnt": counter % 16, "LatCtlPath_No_Cs": 0})
dat = packer.make_can_msg("LateralMotionControl2", bus, values)[1]
fields = ((dat[2] << 3) | (dat[3] >> 5), (dat[6] << 3) | (dat[7] >> 5),
((dat[3] & 31) << 6) | (dat[4] >> 2), ((dat[4] & 3) << 8) | dat[5])
total = int(command.active) + counter % 16 + sum(v + (v >> 8) for v in fields)
values["LatCtlPath_No_Cs"] = 255 - (total & 255)
return packer.make_can_msg("LateralMotionControl2", bus, values)
values.update({"LatCtl_D_Rq": int(command.active), "LatCtlRng_L_Max": 0, "LatCtlCurv_NoRate_Actl": 0.0})
return packer.make_can_msg("LateralMotionControl", bus, values)
opendbc_repo/opendbc/bluepilot_lateral/core/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/core/angle.py
+198
View File
@@ -0,0 +1,198 @@
"""Angle-primary Ford steering adapted from Alan Polk's BluePilot bp-7.0.
Source e1d051d7ba270261b4455068bd68f1a58db15a4a lateral_angle_ext.py.
See ../LICENSE, ../LICENSE.md and ../CREDITS.md. This core has no vehicle,
openpilot, Params, messaging or model-constant imports.
"""
from dataclasses import dataclass
import math
from ..params import limits as bounds
from .trim import LaneCenterTrim, LaneGeometry, TrimConfig
def clip(value, low, high):
return max(low, min(high, value))
def interp(value, xs, ys):
if value <= xs[0]:
return ys[0]
for i in range(1, len(xs)):
if value < xs[i]:
return ys[i - 1] + (ys[i] - ys[i - 1]) * (value - xs[i - 1]) / (xs[i] - xs[i - 1])
return ys[-1]
@dataclass(frozen=True)
class Inputs:
active: bool
speed: float
measured_curvature: float
desired_curvature: float
steering_pressed: bool = False
steering_angle: float = 0.0
yaw_predictions: tuple = ()
time_indices: tuple = ()
lateral_delay: float = 0.15
lane_change: bool = False
lane_direction: int = 0
limit_status: int = 0
geometry: LaneGeometry | None = None
@dataclass(frozen=True)
class Command:
active: bool = False
path_angle: float = 0.0
shadow_curvature: float = 0.0
precision: int = 1
class AngleController:
def __init__(self, *, low_gain=1.0, high_gain=1.15, trim_config=None):
self.trim_config = TrimConfig() if trim_config is None else trim_config
self.trim = LaneCenterTrim()
self.low_gain = low_gain
self.high_gain = high_gain
self.path_last = 0.0
self.speed_last = 0.1
self.desired_last = 0.0
self.human_timer = 0.0
self.press_last = False
self.preturned = False
self.press_timer = 0.0
self.blip_frames = 0
self.blip_cooldown = 0.0
self.blip_hold = 0.0
self.blip_count = 0
def reset(self):
self.trim.reset()
self.path_last = 0.0
self.speed_last = 0.1
self.desired_last = 0.0
self.human_timer = 0.0
self.press_last = False
self.preturned = False
self.press_timer = 0.0
self.blip_frames = 0
self.blip_cooldown = 0.0
self.blip_hold = 0.0
self.blip_count = 0
def step(self, i: Inputs) -> Command:
values = (i.speed, i.measured_curvature, i.desired_curvature, i.steering_angle, i.lateral_delay)
if not i.active or not all(math.isfinite(v) for v in values) or i.speed < 0.0:
self.reset()
return Command()
if i.steering_pressed and not self.press_last:
self.preturned = abs(i.steering_angle) > 45.0
self.press_last = i.steering_pressed
self.human_timer = self.human_timer + 0.05 if i.steering_pressed and abs(i.steering_angle) > 45.0 else 0.0
if self.human_timer + 1e-9 >= (3.0 if self.preturned else 1.5):
self.trim.reset()
self.path_last = 0.0
self.desired_last = i.desired_curvature
self.blip_frames = self.blip_count = 0
self.blip_hold = self.blip_cooldown = self.press_timer = 0.0
return Command()
if i.steering_pressed:
self.press_timer += 0.05
else:
if self.press_timer >= 0.5 and self.blip_cooldown <= 0.0 and self.blip_frames <= 0 and abs(self.path_last) < 0.1:
self.blip_frames = 6
self.press_timer = 0.0
if self.blip_frames > 0:
self.blip_frames -= 1
self.trim.reset()
self.path_last = 0.0
self.desired_last = i.desired_curvature
if self.blip_frames == 0:
self.blip_cooldown = 2.0
return Command()
base = clip(i.lateral_delay, 0.1, 0.15) + 0.05
predictions_valid = (len(i.yaw_predictions) == len(i.time_indices) and len(i.time_indices) >= 17 and
all(math.isfinite(v) for v in (*i.yaw_predictions, *i.time_indices)) and
all(b > a for a, b in zip(i.time_indices, i.time_indices[1:], strict=False)))
curves = tuple(y / max(0.01, i.speed) for y in i.yaw_predictions) if predictions_valid else ()
entering = bool(curves) and abs(interp(base, i.time_indices, curves)) > abs(i.desired_curvature)
factor = 1.0 if entering else interp(abs(i.desired_curvature), (0.005, 0.020), (1.0, 0.0))
lookup = base + bounds.VLT_EXTRA * interp(i.speed, (11.176, 24.5872), (1.0, 0.0)) * factor
predicted = interp(lookup, i.time_indices, curves) if curves else i.desired_curvature
saturated = i.limit_status >= 2 or self.path_last >= 0.5235 * 0.9 or self.path_last <= -0.5 * 0.9
falling = abs(i.desired_curvature) < abs(self.desired_last) - 0.010
blend = bounds.BLEND * 0.25 if not entering and (i.limit_status >= 1 or saturated or falling) else bounds.BLEND
requested = predicted * blend + i.desired_curvature * (1.0 - blend)
self.desired_last = i.desired_curvature
precision = 0 if i.lane_change and ((i.lane_direction == 1 and requested < 0) or
(i.lane_direction == 2 and requested > 0)) else 1
# BluePilot's default lane-change factors are both 1.0.
# Trim requested curvature before every existing actuator/safety limiter.
requested = self.trim.update(requested, i.speed, i.geometry, self.trim_config,
active=i.active, lane_change=i.lane_change, steering_pressed=i.steering_pressed)
kappa = clip(requested, i.measured_curvature - 0.002, i.measured_curvature + 0.002) if i.speed > 9.0 else requested
low = interp(i.speed, (13.5, 26.82), (1.0, self.low_gain))
high = interp(i.speed, (13.5, 26.82), (1.3, self.high_gain))
gain = interp(abs(kappa), (0.0007, 0.001), (low, high))
path = kappa * i.speed * gain
if saturated:
last_mag, magnitude = abs(self.path_last), abs(path)
if magnitude > last_mag:
path = self.path_last
elif last_mag - magnitude > 0.02:
path = math.copysign(last_mag - 0.02, self.path_last)
elif i.limit_status >= 1:
path = clip(path, -abs(self.path_last), abs(self.path_last))
roc = interp(i.speed, (10.0, 15.0, 25.0), (0.055, 0.0425, 0.009))
path = clip(clip(path, -0.5235, 0.5), self.path_last - roc, self.path_last + roc)
# Tightening relative to bp-7.0: the shadow describes the final actuator,
# including ROC/saturation. A truthful derived curvature must fit both the
# measured-curvature interval and the modern ISO envelope. If those bands
# cannot meet the previous actuator ROC, send a neutral mode-0 frame.
effective_speed = max(i.speed, 0.1)
cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(i.speed, 1.0) ** 2)
low_kappa, high_kappa = -cap, cap
if i.speed > 9.0:
low_kappa = max(low_kappa, i.measured_curvature - 0.0018)
high_kappa = min(high_kappa, i.measured_curvature + 0.0018)
# Native cannot trust a host-selected feel gain. Intersect every admitted
# gain (0.95..1.3), so the worst equivalent curvature remains bounded.
lower_gain = bounds.GAIN_MAX if low_kappa >= 0.0 else bounds.GAIN_MIN
upper_gain = bounds.GAIN_MIN if high_kappa >= 0.0 else bounds.GAIN_MAX
lower = max(-bounds.PATH_MAX, low_kappa * effective_speed * lower_gain, self.path_last - roc)
upper = min(-bounds.PATH_MIN, high_kappa * effective_speed * upper_gain, self.path_last + roc)
# Bound the actual-angle equivalent derivative at the worst admitted gain.
# 45ms core margin remains inside the native elapsed-time/50ms cap.
jerk_delta = bounds.LATERAL_JERK / max(i.speed - 1.0, 1.0) ** 2 * bounds.CORE_STEER_PERIOD_S * bounds.GAIN_MIN * effective_speed
jerk_center = self.path_last / self.speed_last * effective_speed
lower = max(lower, jerk_center - jerk_delta)
upper = min(upper, jerk_center + jerk_delta)
lower += bounds.PATH_ROUNDING_MARGIN
upper -= bounds.PATH_ROUNDING_MARGIN
if lower > upper:
self.trim.reset()
self.path_last = 0.0
return Command()
path = clip(path, lower, upper)
self.path_last = path
self.speed_last = effective_speed
shadow = path / (effective_speed * gain)
self.blip_cooldown = max(0.0, self.blip_cooldown - 0.05)
gap = i.desired_curvature - i.measured_curvature
stalled = (not i.steering_pressed and not i.lane_change and i.speed > 9.0 and abs(gap) > 0.004 and
abs(i.desired_curvature) > abs(i.measured_curvature))
if stalled:
if abs(kappa - requested) > 1e-9 and self.blip_cooldown <= 0.0:
self.blip_hold += 0.05
if self.blip_hold >= 0.5 and self.blip_count < 3 and abs(path) < 0.1:
self.blip_frames = 6
self.blip_hold = 0.0
self.blip_count += 1
else:
self.blip_hold = 0.0
if i.steering_pressed or abs(gap) < 0.002:
self.blip_count = 0
return Command(True, path, shadow, precision)
opendbc_repo/opendbc/bluepilot_lateral/core/trim.py
+104
View File
@@ -0,0 +1,104 @@
"""Pure angle lane-centering trim adapted from BluePilot bp-dev e22afa6b.
Alan Polk and ghbarker; the source also credits StarPilot/u/jc01rho.
See ../CREDITS.md, ../LICENSE and ../LICENSE.md. Geometry/confidence/fallback,
filter and slew follow lane_center_trim.py; this module receives immutable arrays
instead of a host model. Stale geometry and driver pressure additionally suppress
trim, and nonfinite confidence cannot create correction.
"""
from dataclasses import dataclass
import math
from ..params.trim import DEFAULTS, BOUNDS
def clip(value, low, high):
return max(low, min(high, value))
def interp(value, xs, ys):
if value <= xs[0]:
return ys[0]
for index in range(1, len(xs)):
if value < xs[index]:
return ys[index - 1] + (ys[index] - ys[index - 1]) * (value - xs[index - 1]) / (xs[index] - xs[index - 1])
return ys[-1]
@dataclass(frozen=True)
class TrimConfig:
enabled: bool = DEFAULTS["enabled"]
offset_m: float = DEFAULTS["offset_m"]
gain: float = DEFAULTS["gain"]
@dataclass(frozen=True)
class LaneGeometry:
position_x: tuple = ()
position_y: tuple = ()
left_x: tuple = ()
left_y: tuple = ()
right_x: tuple = ()
right_y: tuple = ()
probabilities: tuple = ()
standard_deviations: tuple = ()
fresh: bool = False
def sample(xs, ys, distance):
if (not isinstance(xs, tuple) or not isinstance(ys, tuple) or len(xs) < 2 or len(xs) != len(ys) or
not all(math.isfinite(v) for v in (*xs, *ys)) or not all(b > a for a, b in zip(xs, xs[1:], strict=False))):
return None
return interp(distance, xs, ys)
class LaneCenterTrim:
def __init__(self):
self.correction = 0.0
def reset(self):
self.correction = 0.0
def update(self, requested, speed, geometry, config, *, active, lane_change, steering_pressed=False):
try:
valid = (isinstance(config, TrimConfig) and config.enabled is True and active and not lane_change and
not steering_pressed and isinstance(geometry, LaneGeometry) and geometry.fresh is True and
math.isfinite(requested) and math.isfinite(speed) and speed >= 0.0 and
math.isfinite(config.offset_m) and BOUNDS["offset_m"][0] <= config.offset_m <= BOUNDS["offset_m"][1] and
math.isfinite(config.gain) and BOUNDS["gain"][0] <= config.gain <= BOUNDS["gain"][1])
authority = interp(speed, BOUNDS["speed_mps"], BOUNDS["speed_authority"]) if valid else 0.0
if not valid or authority <= 0.0:
self.reset()
return requested
distance = clip(speed, *BOUNDS["lookahead_m"])
model_y = sample(geometry.position_x, geometry.position_y, distance)
if model_y is None:
self.reset()
return requested
# Bad lane-only geometry retains the source's model-position/offset fallback.
scale, center = self.lane_blend(geometry, distance)
error = scale * (center - model_y) + config.offset_m
raw = 2.0 * error / distance ** 2
target = clip(raw, -BOUNDS["raw_correction"], BOUNDS["raw_correction"]) * config.gain * authority
alpha = 1.0 - math.exp(-BOUNDS["period_s"] / BOUNDS["tau_s"])
filtered = alpha * target + (1.0 - alpha) * self.correction
self.correction = clip(filtered, self.correction - BOUNDS["correction_delta"], self.correction + BOUNDS["correction_delta"])
return requested + self.correction
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
self.reset()
return requested
def lane_blend(self, geometry, distance):
try:
left = sample(geometry.left_x, geometry.left_y, distance)
right = sample(geometry.right_x, geometry.right_y, distance)
probs, stds = geometry.probabilities, geometry.standard_deviations
if (left is None or right is None or not isinstance(probs, tuple) or not isinstance(stds, tuple) or
len(probs) != 2 or len(stds) != 2 or not all(math.isfinite(p) and 0.0 <= p <= 1.0 for p in probs) or
not all(math.isfinite(s) and s >= 0.0 for s in stds)):
return 0.0, 0.0
confidence = min(*probs, interp(right - left, BOUNDS["width_m"], BOUNDS["width_tolerance"]),
interp(max(stds), BOUNDS["std"], BOUNDS["std_tolerance"]))
scale = clip(interp(confidence, BOUNDS["confidence"], (0.0, 1.0)), 0.0, 1.0)
return scale, 0.5 * (left + right)
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return 0.0, 0.0
opendbc_repo/opendbc/bluepilot_lateral/hosts/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py
+101
View File
@@ -0,0 +1,101 @@
"""StarPilot's CP adapter; model transport and saved Params stay in the host.
Only exact final CP words derived from the existing five classic/six CANFD
extended profiles can select angle. AOL and Transit keep their current owner.
"""
from opendbc.car import structs
from opendbc.car.ford.values import CAR, FordFlags
from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs
from opendbc.bluepilot_lateral.core.trim import LaneGeometry
from opendbc.bluepilot_lateral.params import limits as bounds
ANGLE_FLAG = 128
CLASSIC = frozenset({CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_FOCUS_MK4,
CAR.FORD_MAVERICK_MK1, CAR.FORD_EXPLORER_MK6})
CANFD = frozenset({CAR.FORD_ESCAPE_MK4_5, CAR.FORD_EXPEDITION_MK4, CAR.FORD_F_150_MK14,
CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_RANGER_MK2, CAR.FORD_MUSTANG_MACH_E_MK1})
TRUCKS = CANFD - {CAR.FORD_ESCAPE_MK4_5, CAR.FORD_MUSTANG_MACH_E_MK1}
def mode_word(cp, *, selected):
if (cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar or cp.alternativeExperience != 0 or
cp.steerControlType != structs.CarParams.SteerControlType.angle or
cp.pcmCruise == cp.openpilotLongitudinalControl or len(cp.safetyConfigs) not in (1, 2)):
return None
if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or
cp.safetyConfigs[0].safetyParam != 0):
return None
fd = cp.carFingerprint in CANFD
if ((fd and (not cp.flags & FordFlags.CANFD or cp.flags & ~int(FordFlags.CANFD | FordFlags.HAS_BSM))) or
(not fd and (cp.carFingerprint not in CLASSIC or cp.flags & ~int(FordFlags.HAS_BSM)))):
return None
safety = cp.safetyConfigs[-1]
long = int(cp.openpilotLongitudinalControl)
original = (18 if cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 else 66) + long if fd else 32 + long
expected = ANGLE_FLAG + (2 if fd else 0) + long if selected else original
return expected if safety.safetyModel == structs.CarParams.SafetyModel.ford and safety.safetyParam == expected else None
def supported(cp):
return mode_word(cp, selected=False) is not None or mode_word(cp, selected=True) is not None
def qualified(cp):
return mode_word(cp, selected=True) is not None
def select(cp, raw):
# Absence/invalid bytes stay at current curvature. Never rewrite saved bytes.
chosen = (type(raw) is int and raw == 1) or (type(raw) in (bytes, str) and raw in (b"1", "1"))
if not chosen or mode_word(cp, selected=False) is None:
return False
cp.safetyConfigs[-1].safetyParam = ANGLE_FLAG + (2 if cp.flags & FordFlags.CANFD else 0) + int(cp.openpilotLongitudinalControl)
cp.steerActuatorDelay = 0.22
return True
def model_geometry(model, *, fresh=False):
"""Project a caller-qualified snapshot to immutable core arrays; no model imports."""
if model is None or fresh is not True:
return None
try:
position_x, position_y = tuple(model.position.x), tuple(model.position.y)
except (AttributeError, TypeError, ValueError):
return None
try:
return LaneGeometry(position_x=position_x, position_y=position_y, fresh=True, left_x=tuple(model.laneLines[1].x), left_y=tuple(model.laneLines[1].y),
right_x=tuple(model.laneLines[2].x), right_y=tuple(model.laneLines[2].y),
probabilities=(model.laneLineProbs[1], model.laneLineProbs[2]),
standard_deviations=(model.laneLineStds[1], model.laneLineStds[2]))
except (AttributeError, IndexError, TypeError, ValueError):
return LaneGeometry(position_x=position_x, position_y=position_y, fresh=True)
class Host:
def __init__(self, cp):
if not qualified(cp):
raise ValueError("BluePilot angle requires an exact admitted Ford safety word")
low, high = (0.95, 0.95) if cp.carFingerprint in TRUCKS else ((1.0, 1.05) if cp.carFingerprint in CANFD else (1.0, 1.15))
self.cp = cp
self.controller = AngleController(low_gain=low, high_gain=high)
self.inputs = None
def update(self, CC, CS):
if self.inputs is not None:
self.inputs.update()
snapshot = self.inputs.lateral_snapshot(CS.out.vEgoRaw) if self.inputs is not None else None
model, times, delay, _ = snapshot if snapshot is not None else (None, (), 0.15, True)
ready = (self.inputs is not None and self.inputs.native_permission(self.cp, max_age_ns=bounds.PANDA_AGE_NS) and
CC.enabled and CC.latActive and CS.out.cruiseState.enabled and CS.out.canValid and
not CS.out.canTimeout and not CS.out.gasPressed and not CS.out.brakePressed and
not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent and not CS.out.vehicleSensorsInvalid)
meta = model.meta if model is not None else None
return self.controller.step(Inputs(
active=ready, speed=CS.out.vEgoRaw, measured_curvature=-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1),
desired_curvature=CC.actuators.curvature, steering_pressed=CS.out.steeringPressed,
steering_angle=CS.out.steeringAngleDeg, yaw_predictions=tuple(model.orientationRate.z) if model is not None else (),
time_indices=tuple(times), lateral_delay=delay,
lane_change=int(getattr(meta.laneChangeState, "raw", meta.laneChangeState)) in (1, 2, 3) if meta else False,
lane_direction=int(getattr(meta.laneChangeDirection, "raw", meta.laneChangeDirection)) if meta else 0,
limit_status=getattr(CS, "lat_ctl_lim_stat", 0),
geometry=model_geometry(model, fresh=snapshot is not None)))
opendbc_repo/opendbc/bluepilot_lateral/params/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/params/generate.py
+41
View File
@@ -0,0 +1,41 @@
"""Generate protocol constants without loading a host or vehicle module."""
import json
from pathlib import Path
root = Path(__file__).parent
spec = json.loads((root / "protocol.json").read_text())
(root / "protocol.py").write_text('"""Generated from protocol.json; layout preserves the existing curvature flag."""\n' +
"".join(f"{key.upper()} = {value!r}\n" for key, value in spec.items()))
(root / "protocol.h").write_text("#pragma once\n// Generated from protocol.json; do not allocate bit 1 to angle metadata.\n" +
"".join(f"#define FORD_BP_{key.upper()} {int(value)}U\n" for key, value in spec.items() if key != "shadow_scale"))
manifest = json.loads((root / "manifest.json").read_text())
defaults, bounds = manifest["defaults"], manifest["bounds"]
limits = {"BLEND": defaults["blend"], "VLT_EXTRA": defaults["vlt_extra_max"],
"PATH_MIN": bounds["path_angle_wire"][0], "PATH_MAX": bounds["path_angle_wire"][1],
"GAIN_MIN": bounds["gain"][0], "GAIN_MAX": bounds["gain"][1],
"CURVATURE_MAX": bounds["curvature"], "CURVATURE_ERROR": bounds["curvature_error"],
"LATERAL_ACCEL": bounds["lateral_accel"], "RX_AGE_US": bounds["rx_age_us"],
"SHADOW_AGE_US": bounds["shadow_age_us"], "PATH_ROUNDING_MARGIN": bounds["path_rounding_margin"],
"SHADOW_ROUNDING_MARGIN": bounds["shadow_rounding_margin"], "LATERAL_JERK": bounds["lateral_jerk"],
"RT_INTERVAL_US": bounds["rt_interval_us"], "STEER_PERIOD_US": bounds["steer_period_us"],
"CORE_STEER_PERIOD_S": bounds["core_steer_period_s"], "PANDA_AGE_NS": bounds["panda_age_ns"]}
(root / "limits.py").write_text('"""Generated from manifest.json; host and core use this manifest contract."""\n' +
"".join(f"{key} = {value!r}\n" for key, value in limits.items()))
(root / "limits.h").write_text("#pragma once\n// Generated from manifest.json; native and Python bounds share a source.\n" +
"".join(f"#define FORD_BP_{key} {str(value) + 'F' if isinstance(value, float) else str(value) + 'U'}\n"
for key, value in limits.items()))
# Optional angle trim settings are Python-only; native safety limits stay unchanged.
trim = manifest["angle_lane_trim"]
types = {"enabled": "bool", "offset_m": "float", "gain": "float"}
defaults_fields = "".join(f" {key}: {types[key]}\n" for key in trim["defaults"])
bounds_fields = "".join(f" {key}: {'list[float]' if isinstance(value, list) else 'float'}\n" for key, value in trim["bounds"].items())
(root / "trim.py").write_text('"""Generated Python-only angle trim defaults; no native bound or registry changes."""\n' +
"from typing import TypedDict\n\n\n" +
"class TrimDefaults(TypedDict):\n" + defaults_fields + "\n\n" +
"class TrimBounds(TypedDict):\n" + bounds_fields + "\n\n" +
f"SOURCE_REVISION = {trim['source_revision']!r}\n" +
f"DEFAULTS: TrimDefaults = {trim['defaults']!r}\n" +
"BOUNDS: TrimBounds = {\n" +
"".join(f" {key!r}: {value!r},\n" for key, value in trim["bounds"].items()) + "}\n")
opendbc_repo/opendbc/bluepilot_lateral/params/limits.h
+20
View File
@@ -0,0 +1,20 @@
#pragma once
// Generated from manifest.json; native and Python bounds share a source.
#define FORD_BP_BLEND 0.5F
#define FORD_BP_VLT_EXTRA 0.1F
#define FORD_BP_PATH_MIN -0.5F
#define FORD_BP_PATH_MAX 0.5235F
#define FORD_BP_GAIN_MIN 0.95F
#define FORD_BP_GAIN_MAX 1.3F
#define FORD_BP_CURVATURE_MAX 0.02F
#define FORD_BP_CURVATURE_ERROR 0.002F
#define FORD_BP_LATERAL_ACCEL 2.4114F
#define FORD_BP_RX_AGE_US 100000U
#define FORD_BP_SHADOW_AGE_US 60000U
#define FORD_BP_PATH_ROUNDING_MARGIN 0.0005F
#define FORD_BP_SHADOW_ROUNDING_MARGIN 1e-06F
#define FORD_BP_LATERAL_JERK 3.5886F
#define FORD_BP_RT_INTERVAL_US 250000U
#define FORD_BP_STEER_PERIOD_US 50000U
#define FORD_BP_CORE_STEER_PERIOD_S 0.045F
#define FORD_BP_PANDA_AGE_NS 300000000U
opendbc_repo/opendbc/bluepilot_lateral/params/limits.py
+19
View File
@@ -0,0 +1,19 @@
"""Generated from manifest.json; host and core use this manifest contract."""
BLEND = 0.5
VLT_EXTRA = 0.1
PATH_MIN = -0.5
PATH_MAX = 0.5235
GAIN_MIN = 0.95
GAIN_MAX = 1.3
CURVATURE_MAX = 0.02
CURVATURE_ERROR = 0.002
LATERAL_ACCEL = 2.4114
RX_AGE_US = 100000
SHADOW_AGE_US = 60000
PATH_ROUNDING_MARGIN = 0.0005
SHADOW_ROUNDING_MARGIN = 1e-06
LATERAL_JERK = 3.5886
RT_INTERVAL_US = 250000
STEER_PERIOD_US = 50000
CORE_STEER_PERIOD_S = 0.045
PANDA_AGE_NS = 300000000
opendbc_repo/opendbc/bluepilot_lateral/params/manifest.json
+109
View File
@@ -0,0 +1,109 @@
{
"source_revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a",
"settings": [
{
"key": "FordLateralMode",
"type": "enum",
"default": 0,
"values": {
"0": "Current curvature",
"1": "BluePilot angle"
},
"label": "Ford steering controller",
"description": "Select the steering controller for supported Ford profiles. Takes effect on the next drive.",
"modes": [
"angle"
]
}
],
"defaults": {
"blend": 0.5,
"vlt_extra_max": 0.1,
"low_speed_factor": 1.0,
"high_speed_factor": 1.0,
"dampening": 1.0,
"lane_change_factor": 1.0
},
"bounds": {
"path_angle_wire": [
-0.5,
0.5235
],
"gain": [
0.95,
1.3
],
"curvature_error": 0.002,
"lateral_accel": 2.4114,
"rx_age_us": 100000,
"shadow_age_us": 60000,
"steer_period_us": 50000,
"curvature": 0.02,
"path_rounding_margin": 0.0005,
"shadow_rounding_margin": 1e-06,
"lateral_jerk": 3.5886,
"rt_interval_us": 250000,
"core_steer_period_s": 0.045,
"panda_age_ns": 300000000
},
"angle_lane_trim": {
"source_revision": "e22afa6be9b881fa784c92ebb316db47728a3d81",
"defaults": {
"enabled": false,
"offset_m": 0.0,
"gain": 0.25
},
"bounds": {
"offset_m": [
-0.5,
0.5
],
"gain": [
0.0,
1.0
],
"width_m": [
2.4,
2.8,
3.75,
4.25
],
"width_tolerance": [
0.0,
0.81,
0.81,
0.59
],
"std": [
0.3,
0.5
],
"std_tolerance": [
0.81,
0.0
],
"confidence": [
0.6,
0.8
],
"speed_mps": [
0.0,
9.0,
15.0
],
"speed_authority": [
0.0,
0.0,
1.0
],
"lookahead_m": [
8.0,
35.0
],
"raw_correction": 0.004,
"tau_s": 0.4,
"period_s": 0.05,
"correction_delta": 0.00015
}
}
}
opendbc_repo/opendbc/bluepilot_lateral/params/protocol.h
+10
View File
@@ -0,0 +1,10 @@
#pragma once
// Generated from protocol.json; do not allocate bit 1 to angle metadata.
#define FORD_BP_VERSION 1U
#define FORD_BP_ANGLE_MASK 1U
#define FORD_BP_EXTENDED_CURVATURE_MASK 2U
#define FORD_BP_COUNTER_SHIFT 2U
#define FORD_BP_COUNTER_MASK 28U
#define FORD_BP_COUNTER_MODULUS 8U
#define FORD_BP_VERSION_MASK 15U
#define FORD_BP_CHECKSUM_SHIFT 4U
opendbc_repo/opendbc/bluepilot_lateral/params/protocol.json
+11
View File
@@ -0,0 +1,11 @@
{
"version": 1,
"angle_mask": 1,
"extended_curvature_mask": 2,
"counter_shift": 2,
"counter_mask": 28,
"counter_modulus": 8,
"shadow_scale": 1e-06,
"version_mask": 15,
"checksum_shift": 4
}
opendbc_repo/opendbc/bluepilot_lateral/params/protocol.py
+10
View File
@@ -0,0 +1,10 @@
"""Generated from protocol.json; layout preserves the existing curvature flag."""
VERSION = 1
ANGLE_MASK = 1
EXTENDED_CURVATURE_MASK = 2
COUNTER_SHIFT = 2
COUNTER_MASK = 28
COUNTER_MODULUS = 8
SHADOW_SCALE = 1e-06
VERSION_MASK = 15
CHECKSUM_SHIFT = 4
opendbc_repo/opendbc/bluepilot_lateral/params/trim.py
+45
View File
@@ -0,0 +1,45 @@
"""Generated Python-only angle trim defaults; no native bound or registry changes."""
from typing import TypedDict
class TrimDefaults(TypedDict):
enabled: bool
offset_m: float
gain: float
class TrimBounds(TypedDict):
offset_m: list[float]
gain: list[float]
width_m: list[float]
width_tolerance: list[float]
std: list[float]
std_tolerance: list[float]
confidence: list[float]
speed_mps: list[float]
speed_authority: list[float]
lookahead_m: list[float]
raw_correction: float
tau_s: float
period_s: float
correction_delta: float
SOURCE_REVISION = 'e22afa6be9b881fa784c92ebb316db47728a3d81'
DEFAULTS: TrimDefaults = {'enabled': False, 'offset_m': 0.0, 'gain': 0.25}
BOUNDS: TrimBounds = {
'offset_m': [-0.5, 0.5],
'gain': [0.0, 1.0],
'width_m': [2.4, 2.8, 3.75, 4.25],
'width_tolerance': [0.0, 0.81, 0.81, 0.59],
'std': [0.3, 0.5],
'std_tolerance': [0.81, 0.0],
'confidence': [0.6, 0.8],
'speed_mps': [0.0, 9.0, 15.0],
'speed_authority': [0.0, 0.0, 1.0],
'lookahead_m': [8.0, 35.0],
'raw_correction': 0.004,
'tau_s': 0.4,
'period_s': 0.05,
'correction_delta': 0.00015,
}
opendbc_repo/opendbc/bluepilot_lateral/safety/ford_bp.h
+227
View File
@@ -0,0 +1,227 @@
#pragma once
// Angle-primary Ford extension adapted from Alan Polk's bp-7.0 e1d051d7.
// See ../CREDITS.md, ../LICENSE and ../LICENSE.md. No reset bypass.
#include "opendbc/bluepilot_lateral/params/protocol.h"
#include "opendbc/bluepilot_lateral/params/limits.h"
static bool ford_bp_enabled = false;
static bool ford_bp_canfd = false;
static bool ford_bp_shadow_seen = false;
static bool ford_bp_lka_counter_seen = false;
static uint8_t ford_bp_lka_counter = 0U;
static int ford_bp_shadow_raw = 0;
static uint32_t ford_bp_shadow_us = 0U;
static int ford_bp_path_last = 0;
static bool ford_bp_control_counter_seen = false;
static uint8_t ford_bp_control_counter = 0U;
static bool ford_bp_control_seen = false;
static uint32_t ford_bp_control_us = 0U;
static float ford_bp_equivalent_last = 0.0F;
static unsigned int ford_bp_rt_msgs = 0U;
static unsigned int ford_bp_rt_msgs_prev = 0U;
static uint32_t ford_bp_rt_us = 0U;
static bool ford_bp_rx_seen[7];
static uint32_t ford_bp_rx_us[7];
static void ford_bp_invalidate_shadow(void) {
ford_bp_shadow_seen = false;
ford_bp_shadow_raw = 0;
}
static void ford_bp_withdraw(void) {
ford_bp_invalidate_shadow();
ford_bp_path_last = 0;
ford_bp_equivalent_last = 0.0F;
}
static bool ford_bp_configure(uint16_t param) {
ford_bp_enabled = ((param == 128U) || (param == 129U) || (param == 130U)) && (alternative_experience == 0);
#ifdef ALLOW_DEBUG
ford_bp_enabled |= (param == 131U) && (alternative_experience == 0);
#endif
ford_bp_canfd = (param & 2U) != 0U;
ford_bp_lka_counter_seen = false;
ford_bp_control_counter_seen = false;
ford_bp_control_seen = false;
ford_bp_lka_counter = 0U;
ford_bp_control_counter = 0U;
ford_bp_shadow_us = 0U;
ford_bp_control_us = 0U;
ford_bp_rt_msgs = 0U;
ford_bp_rt_msgs_prev = 0U;
ford_bp_rt_us = microsecond_timer_get();
ford_bp_withdraw();
for (unsigned int i = 0U; i < 7U; i++) {
ford_bp_rx_seen[i] = false;
ford_bp_rx_us[i] = 0U;
}
return ((param & 128U) == 0U) || ford_bp_enabled;
}
static bool ford_bp_health(uint32_t now);
static void ford_bp_rx(const CANPacket_t *msg) {
if (ford_bp_enabled && (msg->bus == 0U) && (GET_LEN(msg) == 8U)) {
const unsigned int ids[7] = {0x415U, 0x202U, 0x91U, 0x165U, 0x204U, 0x213U, 0x83U};
for (unsigned int i = 0U; i < 7U; i++) {
if (msg->addr == ids[i]) {
ford_bp_rx_seen[i] = true;
ford_bp_rx_us[i] = microsecond_timer_get();
}
}
// Angle-specific source freshness is stricter than the generic1s RX tick.
// Expose that withdrawal through the ordinary public controls acknowledgment.
if (controls_allowed && !ford_bp_health(microsecond_timer_get())) {
controls_allowed = false;
}
// Permission losses remove the old actuator history before any later arm.
if (!controls_allowed || brake_pressed || gas_pressed) {
ford_bp_withdraw();
}
}
}
static bool ford_bp_health(uint32_t now) {
bool valid = !safety_rx_checks_invalid && !relay_malfunction;
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
const RxStatus *status = &current_safety_config.rx_checks[i].status;
valid &= status->msg_seen && !status->lagging && status->valid_checksum &&
status->valid_quality_flag && (status->wrong_counters < MAX_WRONG_COUNTERS);
}
const unsigned int count = ford_stock_switch ? 7U : 6U;
for (unsigned int i = 0U; i < count; i++) {
const uint32_t age = (i == 3U || i == 6U) ? (2U * FORD_BP_RX_AGE_US) : FORD_BP_RX_AGE_US;
valid &= ford_bp_rx_seen[i] && (safety_get_ts_elapsed(now, ford_bp_rx_us[i]) <= age);
}
return valid;
}
static bool ford_bp_lka(const CANPacket_t *msg) {
const uint32_t now = microsecond_timer_get();
const unsigned int counter = (msg->data[4] & FORD_BP_COUNTER_MASK) >> FORD_BP_COUNTER_SHIFT;
unsigned int sum = 0U;
for (unsigned int i = 0U; i < 7U; i++) { sum += msg->data[i]; }
const unsigned int checksum = (15U - (sum & 15U)) & 15U;
// Preserve every DBC-owned bit of the packer's neutral LKA body.
const unsigned int angle = ((msg->data[2] & 0xFU) << 8) | msg->data[3];
const unsigned int curvature = (msg->data[1] << 4) | (msg->data[2] >> 4);
bool valid = !relay_malfunction && (msg->bus == 0U) && (GET_LEN(msg) == 8U) && (msg->data[0] == 0U) &&
(angle == 2048U) && (curvature == 2048U) &&
((msg->data[4] & 0xE3U) == FORD_BP_ANGLE_MASK) &&
((msg->data[7] & FORD_BP_VERSION_MASK) == FORD_BP_VERSION) &&
((msg->data[7] >> FORD_BP_CHECKSUM_SHIFT) == checksum);
const int shadow = to_signed((msg->data[5] << 8) | msg->data[6], 16);
const bool resync = (shadow == 0) && !controls_allowed;
if (ford_bp_lka_counter_seen && !resync) {
valid &= counter == ((ford_bp_lka_counter + 1U) % FORD_BP_COUNTER_MODULUS);
valid &= safety_get_ts_elapsed(now, ford_bp_shadow_us) >= 20000U;
}
valid &= SAFETY_ABS(shadow) <= 20000;
if (valid) {
ford_bp_lka_counter = counter;
ford_bp_lka_counter_seen = true;
ford_bp_shadow_raw = shadow;
ford_bp_shadow_seen = !resync;
ford_bp_shadow_us = now;
} else {
ford_bp_invalidate_shadow();
}
return valid;
}
static bool ford_bp_lateral(const CANPacket_t *msg) {
const bool fd = msg->addr == 0x3D6U;
const unsigned int mode = fd ? ((msg->data[0] >> 4) & 7U) : ((msg->data[4] >> 2) & 7U);
const unsigned int curvature = fd ? ((msg->data[2] << 3) | (msg->data[3] >> 5)) :
((msg->data[0] << 3) | (msg->data[1] >> 5));
const unsigned int rate = fd ? (((unsigned int)msg->data[6] << 3) | (msg->data[7] >> 5)) :
(((msg->data[1] & 31U) << 8) | msg->data[2]);
const unsigned int path_raw = fd ? (((msg->data[3] & 31U) << 6) | (msg->data[4] >> 2)) :
(((unsigned int)msg->data[3] << 3) | (msg->data[4] >> 5));
const unsigned int offset = fd ? (((msg->data[4] & 3U) << 8) | msg->data[5]) :
(((unsigned int)msg->data[5] << 2) | (msg->data[6] >> 6));
const unsigned int precision = fd ? ((msg->data[0] >> 2) & 3U) : (msg->data[4] & 3U);
const unsigned int ramp = fd ? (msg->data[0] & 3U) : ((msg->data[6] >> 4) & 3U);
const bool active = mode == 1U;
const int path = (int)path_raw - 1000;
const uint32_t now = microsecond_timer_get();
bool valid = !relay_malfunction && (msg->bus == 0U) && (GET_LEN(msg) == 8U) && (fd == ford_bp_canfd) && (mode <= 1U) &&
(curvature == 1000U) && (rate == (fd ? 1024U : 4096U)) && (offset == 512U) &&
(precision <= 1U) && (ramp == (active ? 2U : 0U));
const unsigned int counter = (msg->data[7] >> 1) & 15U;
if (fd) {
unsigned int checksum = mode + counter;
const unsigned int fields[4] = {curvature, rate, path_raw, offset};
for (unsigned int i = 0U; i < 4U; i++) { checksum += fields[i] + (fields[i] >> 8); }
valid &= msg->data[1] == (0xFFU - (checksum & 0xFFU));
valid &= ((msg->data[0] & 0x80U) == 0U) && ((msg->data[7] & 1U) == 0U);
if (ford_bp_control_counter_seen && active) { valid &= counter == ((ford_bp_control_counter + 1U) % 16U); }
} else {
valid &= ((msg->data[6] & 0xFU) == 0U) && (msg->data[7] == 0U);
}
const float speed_for_equivalent = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 0.1F);
const float equivalent = path / (2000.0F * speed_for_equivalent);
// Time-window expiration is independent of packet acceptance: denied floods
// cannot latch the old count forever or erase accepted actuator history.
const uint32_t rt_elapsed = safety_get_ts_elapsed(now, ford_bp_rt_us);
if (rt_elapsed >= FORD_BP_RT_INTERVAL_US) {
ford_bp_rt_msgs = 0U;
ford_bp_rt_msgs_prev = 0U;
ford_bp_rt_us = now;
} else if (rt_elapsed >= (FORD_BP_RT_INTERVAL_US / 2U)) {
ford_bp_rt_msgs_prev = ford_bp_rt_msgs;
ford_bp_rt_msgs = 0U;
ford_bp_rt_us = now;
}
const unsigned int rt_count = ford_bp_rt_msgs + ford_bp_rt_msgs_prev;
valid &= rt_count <= 7U; // Current20Hz curvature RT allowance; accepted frames own this count.
if (active) {
static const struct lookup_t roc = {{10.0F, 15.0F, 25.0F}, {0.0561F, 0.04335F, 0.00918F}};
const float speed_min = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 0.1F);
const float speed_max = SAFETY_MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 0.1F);
const int delta = (int)(safety_interpolate(roc, speed_min - 1.0F) * 2000.0F) + 1;
const float shadow = ford_bp_shadow_raw * 0.000001F;
const float shadow_can = shadow * 50000.0F;
const float path_abs = SAFETY_ABS(path) / 2000.0F;
const float path_curvature_low = path_abs / (speed_max * FORD_BP_GAIN_MAX);
const float path_curvature_high = path_abs / (speed_min * FORD_BP_GAIN_MIN);
const float physical_cap = SAFETY_MIN(FORD_BP_CURVATURE_MAX, FORD_BP_LATERAL_ACCEL / (SAFETY_MAX(speed_max, 1.0F) * SAFETY_MAX(speed_max, 1.0F)));
const float relation_low = SAFETY_ABS(shadow) * speed_min * FORD_BP_GAIN_MIN * 2000.0F;
const float relation_high = SAFETY_ABS(shadow) * speed_max * FORD_BP_GAIN_MAX * 2000.0F;
valid &= lateral_controls_allowed() && controls_allowed && ford_bp_health(now) && ford_bp_shadow_seen &&
(safety_get_ts_elapsed(now, ford_bp_shadow_us) <= FORD_BP_SHADOW_AGE_US) &&
(path >= -1000) && (path <= 1047) && (SAFETY_ABS(path - ford_bp_path_last) <= delta) &&
((path == 0) || ((ford_bp_shadow_raw != 0) && ((path > 0) == (ford_bp_shadow_raw > 0)))) &&
(SAFETY_ABS(path) >= (relation_low - 2.0F)) && (SAFETY_ABS(path) <= (relation_high + 2.0F)) &&
(SAFETY_ABS(shadow) <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN)) && (path_curvature_high <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN));
if (speed_max > 10.0F) {
valid &= (shadow_can >= (curvature_state.meas.min - 100)) && (shadow_can <= (curvature_state.meas.max + 100));
// Independent actual-actuator check; an invented shadow cannot launder path angle.
const float signed_low = path < 0 ? -path_curvature_high : path_curvature_low;
const float signed_high = path < 0 ? -path_curvature_low : path_curvature_high;
valid &= (signed_low * 50000.0F >= (curvature_state.meas.min - 100)) &&
(signed_high * 50000.0F <= (curvature_state.meas.max + 100));
}
const uint32_t elapsed = ford_bp_control_seen ? safety_get_ts_elapsed(now, ford_bp_control_us) : FORD_BP_STEER_PERIOD_US;
const float jerk_speed = SAFETY_MAX(speed_min - 1.0F, 1.0F);
const float jerk_delta = FORD_BP_LATERAL_JERK / (jerk_speed * jerk_speed) *
(SAFETY_MIN(elapsed, FORD_BP_STEER_PERIOD_US) * 0.000001F) * FORD_BP_GAIN_MIN;
valid &= SAFETY_ABS(equivalent - ford_bp_equivalent_last) <= (jerk_delta + FORD_BP_SHADOW_ROUNDING_MARGIN);
if (ford_bp_control_seen) { valid &= elapsed >= 40000U; }
} else {
valid &= path == 0;
}
if (valid) {
ford_bp_rt_msgs += 1U;
ford_bp_equivalent_last = active ? equivalent : 0.0F;
ford_bp_path_last = active ? path : 0;
ford_bp_control_counter = counter;
ford_bp_control_counter_seen = true;
ford_bp_control_seen = true;
ford_bp_control_us = now;
if (!active) { ford_bp_shadow_seen = false; }
} else {
ford_bp_invalidate_shadow();
}
return valid;
}
opendbc_repo/opendbc/bluepilot_lateral/tests/__init__.py
View File
opendbc_repo/opendbc/bluepilot_lateral/tests/reference.py
+101
View File
@@ -0,0 +1,101 @@
"""Hash-bound upstream angle execution and current-curvature controller oracle.
Only imports are removed from the upstream angle/human module AST; every other
statement and function remains exact. Glue supplies current ABI types, source
fixed defaults and the source's no-op lazy-init/yaw-default adapter. No BluePilot
curvature strategy is loaded. Its diagnostic limiter cannot affect angle output.
"""
import ast
from collections import namedtuple
import hashlib
import json
from pathlib import Path
from types import SimpleNamespace
import numpy as np
from opendbc.car import structs, DT_CTRL
from opendbc.car.ford.values import CAR, CarControllerParams
from opendbc.car.ford.fordcan import CanBus
from opendbc.car.lateral import AngleSteeringLimits, apply_std_steer_angle_limits
from openpilot.selfdrive.modeld.constants import ModelConstants
ROOT = Path(__file__).with_name("reference")
MANIFEST = json.loads((ROOT / "manifest.json").read_text())
def source(name):
raw = (ROOT / (name + ".py.txt")).read_bytes()
if hashlib.sha256(raw).hexdigest() != MANIFEST[name]["sha256"]:
raise AssertionError("Reference source differs from its pinned manifest: " + name)
return ast.parse(raw.decode(), filename=MANIFEST[name]["origin"])
def execute(tree, namespace, *, names=None):
body = []
for node in tree.body:
if isinstance(node, (ast.Import, ast.ImportFrom)):
continue
if names is not None:
assigned = [target.id for target in node.targets if isinstance(target, ast.Name)] if isinstance(node, ast.Assign) else []
if getattr(node, "name", None) not in names and not any(name in names for name in assigned):
continue
body.append(node)
exec(compile(ast.Module(body=body, type_ignores=[]), "<pinned source>", "exec"), namespace)
def angle():
LateralResult = namedtuple("LateralResult", "apply_curvature curvature_rate path_offset path_angle ramp_type precision_type lateralUncertainty")
namespace = {"np": np, "clip": np.clip, "interp": np.interp, "DT_CTRL": DT_CTRL,
"CAR": CAR, "CarControllerParams": CarControllerParams, "ModelConstants": ModelConstants,
"LateralResult": LateralResult, "AngleSteeringLimits": AngleSteeringLimits,
"apply_std_steer_angle_limits": apply_std_steer_angle_limits}
execute(source("human"), namespace)
execute(source("values"), namespace, names={"_BP_ANGLE_RATE_UP", "_BP_ANGLE_RATE_DOWN", "BP_ANGLE_LIMITS"})
execute(source("angle"), namespace)
return namespace["LateralAngleExt"]
def builders():
namespace = {"CanBus": CanBus, "structs": structs}
execute(source("fordcan"), namespace, names={"calculate_lat_ctl2_checksum"})
execute(source("builders"), namespace, names={"create_lka_msg", "create_lat_ctl_msg", "create_lat_ctl2_msg", "_BP_LKA_SHADOW_CURVATURE_SCALE"})
return namespace
def curvature_controller():
# Full exact pre-angle production controller, with unchanged current imports.
namespace = {"__name__": "ford_curvature_pre_angle_reference"}
exec(compile(source("starpilot_curvature_controller"), "<current pre-angle controller>", "exec"), namespace)
return namespace["CarController"]
class Angle:
def __init__(self, identity):
self.controller = angle()()
self.controller.CP = SimpleNamespace(carFingerprint=identity)
self.controller._ensure_lateral_curv_initialized = lambda _cp: None # Exact source guard is a no-op.
self.controller.get_current_curvature = lambda cs: -cs.out.yawRate / max(cs.out.vEgoRaw, .1)
self.controller.path_angle_last = 0.
self.controller.lp = None # Read but unused by the source angle strategy.
self.controller.lane_change_factor_bp = [0., 50.]
self.controller.lane_change_factor_low = 1.
self.controller.model = None
self.controller.update_angle_params(None)
def step(self, inputs):
c = self.controller
c.sm = {"liveDelay": SimpleNamespace(lateralDelay=inputs.lateral_delay)}
c.model = SimpleNamespace(orientationRate=SimpleNamespace(z=list(inputs.yaw_predictions)),
meta=SimpleNamespace(laneChangeState=1 if inputs.lane_change else 0,
laneChangeDirection=inputs.lane_direction)) if inputs.yaw_predictions else None
cc = SimpleNamespace(latActive=inputs.active)
cs = SimpleNamespace(out=SimpleNamespace(vEgoRaw=inputs.speed, yawRate=-inputs.measured_curvature * inputs.speed,
steeringPressed=inputs.steering_pressed, steeringAngleDeg=inputs.steering_angle),
lat_ctl_lim_stat=inputs.limit_status)
return c.update_angle_strategy(cc, cs, SimpleNamespace(curvature=inputs.desired_curvature), c.CP)
def trim():
namespace = {"np": np, "interp": np.interp}
execute(source("bpdev_trim"), namespace)
return namespace["LaneCenterTrim"]
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/angle.py.txt
+577
View File
@@ -0,0 +1,577 @@
"""
BluePilot: Ford CAN-FD path-angle–primary lateral control (developer).
Steering intent is c1 from κ → θ. Converts planner/model curvature into ``path_angle`` using
PSCM short lookahead d_ref and y ≈ ½κ x² ⇒ path_angle = ½ κ d_ref (see
``bluepilot/agent_info/20_FORD_PSCM_KNOWLEDGE_PACK.md``). Predicted curvature (modelV2) is
blended with ``actuators.curvature`` per ``FordPathAngleBlendRatio`` (0 = planner only,
1 = model only).
**c0 (path_offset) is always zero on the wire, unconditionally.** Angle mode has no centering
trim -- an earlier port attempt piped a small additive trim onto path_angle through the
curv-mode ``LC_PID_controller``, but it never actually tracked lane center correctly in this
mode and was removed; only the DBC-required zero c0 remains.
**Human-turn override**: while the driver manually turns (same sustained-press + angle criteria
as ``lateral_curv_ext``, via the shared ``HumanTurnDetector``), lateral is forced inactive (mode
0, all-zero signals) instead of winding path_angle into a stale command the PSCM has to reconcile
on release -- on the Mach-E's PSCM that reconciliation cost 2-3 s of dead time before control
resumed. Mode 0 is panda-clean by construction: every ford.h check has a legitimate
!steer_control_enabled branch, so no reset-bypass latch involvement. On release, path_angle ramps
back in from zero through the soft ROC below (no jump seed) -- generous at human-turn speeds, and
admitted by ford.h's path_angle ROC check (2% looser) without any bypass.
"""
import numpy as np
from numpy import clip, interp
from opendbc.car import DT_CTRL
from opendbc.car.lateral import apply_std_steer_angle_limits
from opendbc.car.ford.values import CAR, CarControllerParams
from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralResult
from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector
from opendbc.sunnypilot.car.ford.values_ext import BP_ANGLE_LIMITS
from selfdrive.modeld.constants import ModelConstants
# Hard-coded per-platform gain defaults.
# CAN vehicles (Escape MK4, Bronco Sport, Explorer, Maverick, Edge)
_GAIN_CAN = (1.00, 1.15)
# CAN-FD body-on-frame trucks (F-150, Lightning, Expedition, Ranger)
_GAIN_CANFD_BOF = (0.95, 0.95)
# CAN-FD unibody SUVs (Mustang Mach-E, Escape MK4.5)
_GAIN_CANFD_SUV = (1.00, 1.05)
_CANFD_BOF_CARS = frozenset({
CAR.FORD_F_150_MK14,
CAR.FORD_F_150_LIGHTNING_MK1,
CAR.FORD_EXPEDITION_MK4,
CAR.FORD_RANGER_MK2,
})
_CANFD_SUV_CARS = frozenset({
CAR.FORD_MUSTANG_MACH_E_MK1,
CAR.FORD_ESCAPE_MK4_5,
})
# 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
class LateralAngleExt:
def __init__(self, CP=None, CP_SP=None):
# Predicted-curvature blend for path_angle: pred * b + desired * (1-b); b from ``FordPathAngleBlendRatio``
self.path_angle_blend_ratio = _FORD_PATH_ANGLE_BLEND_RATIO_DEFAULT
# Max extra VLT above t_base; from ``FordVLTExtraMax`` param
self.vlt_extra_max = _VLT_T_EXTRA_MAX
# Telemetry: final path_angle (rad) after limits (see bp_card_publisher)
self.bp_path_angle_final = 0.0
# High-speed gain factors: set per-platform via carFingerprint in update_angle_params.
self.path_angle_gain_lowC_highV = 1.0 # dampening at high speed, low curvature
self.path_angle_gain_highC_highV = 1.0 # gain at high speed, high curvature
self.bp_path_angle_gain_lowC_highV = 1.0
self.bp_path_angle_gain_highC_highV = 1.0
# User-tunable "feel" multipliers: read from the angle-tuning Params below.
self.low_speed_curv_factor = 1.0
self.high_speed_curv_factor = 1.0
self.user_dampening_factor = 1.0
self.bp_low_speed_curv_factor = 1.0
self.bp_high_speed_curv_factor = 1.0
# BluePilot: angle mode's own lane-change scaling factor, independent of curvature mode's
# lane_change_factor_high_curv -- angle needs a boost (>1) where curvature needs a cut (<1).
self.lane_change_factor_high_ang = 1.0
# Telemetry: variable curvature lookup time used this frame (s)
self.bp_curvature_lookup_time = _VLT_T_EXTRA_MAX + 0.3725 # warm start at ~0.5s
# BluePilot: error-clipped kappa path_angle was derived from -- carcontroller.py reads this as
# shadow_curvature for ford.h's angle-mode deviation check. Actively consumed, not telemetry.
self.bp_kappa_cmd = 0.0
# BluePilot: rate-limit diagnostics (controllerStateBP)
self.bp_angle_rate_limited = False # path_angle soft-ROC clip actually bit this frame
self.bp_curvature_rate_limited = False # equivalent curvature would be rate-limited by curv-mode logic (sim)
self.bp_curvature_deviation_limited = False # current_curvature error-clip constrained kappa_cmd this frame
self.sim_curvature_last = 0.0 # shadow curvature-mode last for the curvatureRateLimited sim
# Exit detection: track previous desired curvature to sense when planner is actively reducing
self._desired_curvature_last = 0.0
# Human-turn override: while the driver manually turns, lateral is forced inactive (mode 0,
# all-zero signals) instead of winding path_angle into a stale command the PSCM can't cleanly
# reconcile on release (2-3 s re-engage dead time observed on Mach-E). See module docstring.
# Note: in CarController this attribute is shared with LateralCurvExt (same mixin instance) --
# only one lateral strategy runs per frame, so a single detector serves both.
self.human_turn_detector = HumanTurnDetector()
self.angle_human_turn_active = False # read by carcontroller to force mode 0
# Post-override stall blip state (see module constants). angle_stall_blip_active is read by
# carcontroller to force mode 0, exactly like angle_human_turn_active.
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."""
self._ensure_lateral_curv_initialized(self.CP)
fp = getattr(self.CP, 'carFingerprint', '')
if fp in _CANFD_BOF_CARS:
low, high = _GAIN_CANFD_BOF
elif fp in _CANFD_SUV_CARS:
low, high = _GAIN_CANFD_SUV
else:
low, high = _GAIN_CAN
self.path_angle_gain_lowC_highV = low
self.path_angle_gain_highC_highV = high
if params is not None and hasattr(params, "get"):
for attr, key, min_value, max_value in (
("low_speed_curv_factor", "FordLowSpeedFactor_ang", 0.5, 1.5),
("high_speed_curv_factor", "FordHighSpeedFactor_ang", 0.5, 1.5),
("user_dampening_factor", "FordHighSpeedDampening_ang", 0.25, 1.25),
):
try:
raw = params.get(key, return_default=True)
if raw is not None and raw != b"":
setattr(self, attr, float(clip(
float(raw.decode("utf-8", errors="replace") if isinstance(raw, bytes) else raw), min_value, max_value)))
except Exception:
pass
try:
raw = params.get("lane_change_factor_high_ang", return_default=True)
if raw is not None and raw != b"":
self.lane_change_factor_high_ang = float(clip(
float(raw.decode("utf-8", errors="replace") if isinstance(raw, bytes) else raw), 0.85, 1.50))
except Exception:
pass
def update_angle_strategy(self, CC, CS, actuators, CP):
"""
Curvature from planner (+ optional predicted blend) → path_angle via ½·κ·d_ref.
c0 (path_offset) is always zero on the wire -- no centering trim in angle mode. c2 and c3 are zero.
Blended κ is not passed through Ford c2 rate / DBC limits (those target the curvature actuator).
"""
self._ensure_lateral_curv_initialized(CP)
v_ego = float(CS.out.vEgoRaw)
d_ref = pscm_d_ref_m(v_ego)
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.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.apply_curvature_last = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_limited = False
self.bp_curvature_deviation_limited = False
self.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.get_current_curvature(CS)
self.human_turn_detector.reset()
self.angle_human_turn_active = False
self.stall_blip_hold_s = 0.0
self.stall_blip_frames_left = 0
self.stall_blip_cooldown_s = 0.0
self.stall_blip_count = 0
self.angle_stall_blip_active = False
self.press_timer_s = 0.0
self.precision_type = 1
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
# (carcontroller drops mode to 0; all signals are zero on the wire) so path_angle can't wind
# into a stale command while the driver turns. Always on in angle mode (no param gate) -- the
# curv-suffixed human-turn toggle belongs to curvature mode's reset strategy, and the Mach-E
# PSCM re-engage stall this prevents is not something a user should be able to opt out of.
# On release, no jump seed: path_angle_last is 0, so the normal flow below ramps the command
# back in through the soft ROC -- generous at human-turn speeds, no panda bypass involved.
self.angle_human_turn_active = self.human_turn_detector.update(
True, CS.out.steeringPressed, CS.out.steeringAngleDeg)
if self.angle_human_turn_active:
self.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.apply_curvature_last = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_limited = False
self.bp_curvature_deviation_limited = False
self.sim_curvature_last = 0.0
# Truthful shadow during the override (mirrors the inactive path -- see the comment
# there): the driver is steering, so the honest command is the car's actual curvature,
# and the panda-latched shadow stays current for the re-engage frame.
self.bp_kappa_cmd = self.get_current_curvature(CS)
# Keep exit detection current so resume doesn't compare against a stale pre-turn value.
self._desired_curvature_last = float(actuators.curvature)
# 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.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.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.path_angle_last = 0.0
self.bp_path_angle_final = 0.0
self.apply_curvature_last = 0.0
self.bp_angle_rate_limited = False
self.bp_curvature_rate_limited = False
self.bp_curvature_deviation_limited = False
self.sim_curvature_last = 0.0
# Truthful shadow during the blip (see the inactive-path comment).
self.bp_kappa_cmd = self.get_current_curvature(CS)
self._desired_curvature_last = float(actuators.curvature)
self.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.precision_type = 1
precision = 1
LP = self.lp
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.sm['liveDelay'].lateralDelay, 0.1, 0.15)) + _DT_MDL
_speed_factor = float(interp(v_ego, [_VLT_V_LOW_MS, _VLT_V_HIGH_MS], [1.0, 0.0]))
# Direction-aware kappa factor: on curve ENTRY (model shows more curvature at t_base than planner now),
# keep full lookahead so pre-steering begins early. On exit/apex, taper by magnitude to prevent unwind.
_kappa_at_t_base = 0.0
if self.model is not None and len(self.model.orientationRate.z) >= 17:
_curvatures_ref = np.array(self.model.orientationRate.z) / max(0.01, v_ego)
_kappa_at_t_base = abs(float(interp(_t_base, ModelConstants.T_IDXS, _curvatures_ref)))
_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.model is not None and len(self.model.orientationRate.z) >= 17:
curvatures = np.array(self.model.orientationRate.z) / max(0.01, v_ego)
predicted_curvature = float(
interp(curvature_lookup_time, ModelConstants.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.path_angle_last >= FORD_DBC_PATH_ANGLE_MAX * 0.90 or
self.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.model is not None:
self.lane_change = self.model.meta.laneChangeState in (1, 2, 3)
else:
self.lane_change = False
lane_change_factor = interp(
v_ego, self.lane_change_factor_bp, [self.lane_change_factor_low, self.lane_change_factor_high_ang]
)
if self.lane_change and self.model is not None:
if self.model.meta.laneChangeDirection == 1 and requested_curvature < 0:
requested_curvature *= lane_change_factor
precision = 0
elif self.model.meta.laneChangeDirection == 2 and requested_curvature > 0:
requested_curvature *= lane_change_factor
precision = 0
self.precision_type = precision
# Use planner / predicted κ directly for the κ → path_angle map; we are not sending κ on CAN.
kappa_cmd = float(requested_curvature)
# 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.get_current_curvature(CS)
self.bp_curvature_deviation_limited = False
if v_ego > 9:
_kappa_cmd_pre_error_clip = kappa_cmd
kappa_cmd = float(clip(kappa_cmd, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR))
# BluePilot: did this clip actually constrain kappa_cmd this frame (deviation from measured,
# not rate-of-change -- see carcontroller.py)?
self.bp_curvature_deviation_limited = bool(abs(kappa_cmd - _kappa_cmd_pre_error_clip) > 1e-9)
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.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.path_angle_last), abs(self.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.path_angle_last - _soft_roc,
self.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.path_angle_last = path_angle
self.bp_path_angle_final = path_angle
self.apply_curvature_last = 0.0
# BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller.py reads this
# 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.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.bp_curvature_deviation_limited and self.stall_blip_cooldown_s <= 0.0:
self.stall_blip_hold_s += _STEER_DT
if (self.stall_blip_hold_s >= _STALL_HOLD_S and self.stall_blip_count < _STALL_MAX_BLIPS
and abs(self.path_angle_last) < _BLIP_MAX_PATH_ANGLE):
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.precision_type,
lateralUncertainty=lateral_uncertainty,
)
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/bpdev_trim.py.txt
+203
View File
@@ -0,0 +1,203 @@
"""
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. This mirrors where a GM-focused fork (StarPilot,
u/jc01rho -- ``selfdrive/controls/lib/drive_helpers.py::LaneCenteringController`` on
``feat/lane-centering-camera-offset``) places their own lane-centering trim: on the universal
``desired_curvature`` signal, upstream of any car-specific actuator conversion. The geometry here
(``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
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))
std_tolerance = float(np.interp(max(float(stds[1]), float(stds[2])), _STD_TOLERANCE_BP, _STD_TOLERANCE_V)) #StarPilot stopped at 0.3, this fades the std through the table
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/tests/reference/builders.py.txt
+377
View File
@@ -0,0 +1,377 @@
"""
BluePilot Ford CAN message builder extensions.
Provides extended versions of the stock fordcan.py message builders with additional
parameters for BluePilot features:
- Dynamic ramp_type and precision_type for lateral control
- Split brake_actuate / precharge_actuate for smoother deceleration
- BlueCruise cluster HUD status, TJA warnings, and hands-free messaging
- Independent left/right lane line logic for LKAS UI
- ICBM button injection for intelligent cruise button management
Each function mirrors the stock fordcan.py function it replaces, with an extended
parameter list. When stock carcontroller.py is refactored, it will import these
functions instead of the stock versions.
"""
from opendbc.car import structs
from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum
HUDControl = structs.CarControl.HUDControl
_BP_LKA_SHADOW_CURVATURE_SCALE = 1e-6 # 1/meter per raw unit, matches ford.h's decode
def create_lka_msg(packer, CAN: CanBus, lat_active: bool, hud_control,
angle_mode_engaged: bool = False, shadow_curvature: float = 0.0):
"""
Creates a CAN message for the Ford LKA Command.
BluePilot extension: accepts lat_active and hud_control parameters for future
lane departure warning integration (unused so far).
Also carries angle_mode_engaged + shadow_curvature packed into bits with no DBC signal mapped to
them -- confirmed unused (always 0, no cabana signal) on real F-150 dashcam routes. This message
is one openpilot itself originates every cycle, and ford_tx_hook already reads other fields
(LkaActvStats_D2_Req) directly out of these same bytes synchronously, in the same tx_hook call --
no separate CAN ID, no RX round-trip needed (panda does not self-receive its own TX, confirmed
2026-07-09 investigating a controlsMismatch caused by an earlier dedicated-message design).
Byte layout (bits not covered by any Lane_Assist_Data1 DBC signal):
byte 4 bit 0: angle_mode_engaged
byte 4 bits 1-4: reserved (future bools)
byte 5-6: shadow_curvature (int16, scale 1e-6 1/m)
byte 7: reserved (future value)
Must match the decode in ford.h's FORD_Lane_Assist_Data1 tx_hook check exactly.
Frequency is 33Hz.
"""
addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
dat = bytearray(dat)
shadow_curvature_raw = int(round(shadow_curvature / _BP_LKA_SHADOW_CURVATURE_SCALE))
shadow_curvature_raw = max(-32768, min(32767, shadow_curvature_raw)) & 0xFFFF
dat[4] |= 1 if angle_mode_engaged else 0
dat[5] = (shadow_curvature_raw >> 8) & 0xFF
dat[6] = shadow_curvature_raw & 0xFF
return addr, bytes(dat), bus
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, ramp_type: int, precision_type: int,
path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
"""
Creates a CAN message for the Ford TJA/LCA Command (non-CAN FD).
BluePilot extension: dynamic ramp_type and precision_type parameters.
Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise).
Ford lane centering uses a third-order polynomial to describe the road centerline:
c0 (path_offset): lateral offset between vehicle and centerline (positive is right)
c1 (path_angle): heading angle between vehicle and centerline (positive is right)
c2 (curvature): curvature of the centerline (positive is left)
c3 (curvature_rate): rate of change of curvature
Frequency is 20Hz.
"""
values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
# 3=InterventionRight, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": precision_type, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int,
path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
"""
Creates a CAN message for the Ford Lane Centering command (CAN FD).
BluePilot extension: dynamic ramp_type and precision_type parameters.
Stock hardcodes ramp_type=0 (Slow) and precision_type=1 (Precise).
This message replaces LateralMotionControl on CAN FD platforms and includes
counter and checksum fields.
Frequency is 20Hz.
"""
values = {
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
# 3=SafeRampOut, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": ramp_type, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": precision_type, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
"LatCtlPath_No_Cnt": counter, # [0|15]
"LatCtlPath_No_Cs": 0, # [0|255]
}
# Calculate checksum (reuse stock function)
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, accel_pred: float,
stopping: bool, brake_actuate: bool, precharge_actuate: bool, v_ego_kph: float):
"""
Creates a CAN message for the Ford ACC Command.
BluePilot extension: split brake control into brake_actuate and precharge_actuate
(each with independent hysteresis thresholds) for smoother deceleration. Also
accepts accel_pred as a parameter instead of hardcoding -5.0.
Precharge engages slightly before full brake for smoother initial decel feel.
Both use configurable hysteresis to avoid binary on/off feel.
Frequency is 50Hz.
"""
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
"AccPrpl_A_Pred": accel_pred, # Predicted accel (from carcontroller, not hardcoded)
"AccResumEnbl_B_Rq": 1 if long_active else 0,
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
"AccBrkPrchg_B_Rq": 1 if precharge_actuate else 0, # Pre-charge brake request (independent hysteresis)
"AccBrkDecel_B_Rq": 1 if brake_actuate else 0, # Deceleration request (independent hysteresis)
"AccStopStat_B_Rq": 1 if stopping else 0,
}
return packer.make_can_msg("ACCDATA", CAN.main, values)
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool,
standstill: bool, hud_control, stock_values: dict, send_hands_free_msg: bool,
send_ui: bool, send_bars: bool, tja_warn: int, tja_msg: int):
"""
Creates a CAN message for the Ford IPC adaptive cruise, FCW and TJA status.
BluePilot extension: replaces stock show_distance_bars with explicit send_ui,
send_bars, and TJA parameters. Adds BlueCruise status 7 for hands-free cluster
UI. TJA warn/msg are set from DM state computation rather than stock passthrough.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 5Hz.
"""
# Tja_D_Stat: TJA status for cluster display
if enabled:
if hud_control.leftLaneDepart:
status = 3 # ActiveInterventionLeft
elif hud_control.rightLaneDepart:
status = 4 # ActiveInterventionRight
elif send_hands_free_msg:
status = 7 # BlueCruise UI in the cluster
else:
status = 2 # Active
elif main_on:
if hud_control.leftLaneDepart:
status = 5 # ActiveWarningLeft
elif hud_control.rightLaneDepart:
status = 6 # ActiveWarningRight
else:
status = 1 # Standby
elif standstill:
status = 0 # Off
else:
status = 1 # Standby
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
# TjaWarn_D_Rq and TjaMsgTxt_D_Dsply are set explicitly below, not passed through
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
"TjaWarn_D_Rq": tja_warn, # TJA warning (from DM state, not stock passthrough)
"TjaMsgTxt_D_Dsply": tja_msg, # TJA text (from DM state, not stock passthrough)
})
if CP.openpilotLongitudinalControl:
values.update({
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
"AccMsgTxt_D2_Rq": 0, # ACC text
"AccTGap_B_Dsply": 1 if send_bars else 0, # Show time gap control UI
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
"AccStopMde_B_Dsply": 1 if standstill else 0,
"AccWarn_D_Dsply": 0, # ACC warning
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
})
# Forward FCW alert from IPMA
if fcw_alert:
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
values["FcwAudioWarn_B_Rq"] = 1 # FCW audio alert
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, hands: int,
hud_control, stock_values: dict):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
BluePilot extension: replaces stock steer_alert bool with hands int (0-3),
and uses independent left/right lane line status logic.
hands values:
0 = HandsOn
1 = Level1 (warning without chime)
2 = Level2 (warning with chime)
3 = Suppressed
LaActvStats_D_Dsply value table (left \\ right):
Right → | Intervene | Warning | Suppress | Available | None
Intervene | 24 | 19 | 14 | 9 | 4
Warning | 23 | 18 | 13 | 8 | 3
Suppress | 22 | 17 | 12 | 7 | 2
Available | 21 | 16 | 11 | 6 | 1
None | 20 | 15 | 10 | 5 | 0
Stock functionality is maintained by passing through unmodified signals.
Frequency is 1Hz.
"""
lines = 0
if hud_control is not None:
# Determine left lane status independently
if hud_control.leftLaneDepart:
left_status = 4 # Intervene (Yellow)
elif hud_control.leftLaneVisible:
left_status = 1 # Available
else:
left_status = 2 # Suppress
# Determine right lane status independently
if hud_control.rightLaneDepart:
right_status = 20 # Intervene (Yellow)
elif hud_control.rightLaneVisible:
right_status = 5 # Available
else:
right_status = 10 # Suppress
# Combine left and right lane status
lines = left_status + right_status
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lane lines) [0|31]
"LaHandsOff_D_Dsply": hands, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
})
return packer.make_can_msg("IPMA_Data", CAN.main, values)
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False,
tja_toggle=False, icbm_button=None):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
BluePilot extension: adds icbm_button parameter for Intelligent Cruise Button
Management. When set, the specified CAN signal is set to 1 in the outgoing
message, enabling openpilot to emulate cruise button presses for speed adjustment.
Args:
icbm_button: Optional string signal name (e.g., "CcAslButtnSetIncPress",
"CcAslButtnSetDecPress") for ICBM button injection.
Frequency is 10Hz.
"""
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
# ICBM button support — set the specified button signal to 1
if icbm_button is not None:
values[icbm_button] = 1
return packer.make_can_msg("Steering_Data_FD1", bus, values)
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/fordcan.py.txt
+342
View File
@@ -0,0 +1,342 @@
from opendbc.car import CanBusBase, structs
HUDControl = structs.CarControl.HUDControl
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
@property
def main(self) -> int:
return self.offset
@property
def radar(self) -> int:
return self.offset + 1
@property
def camera(self) -> int:
return self.offset + 2
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)
def create_lka_msg(packer, CAN: CanBus):
"""
Creates an empty CAN message for the Ford LKA Command.
This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout.
Frequency is 33Hz.
"""
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float):
"""
Creates a CAN message for the Ford TJA/LCA Command.
This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway
driving. It is not subject to the PSCM lockout.
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
by the following coefficients:
c0: lateral offset between the vehicle and the centerline (positive is right)
c1: heading angle between the vehicle and the centerline (positive is right)
c2: curvature of the centerline (positive is left)
c3: rate of change of curvature of the centerline
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
angle cannot be easily controlled.
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
using tools such as Forscan.
Frequency is 20Hz.
"""
values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
# 3=InterventionRight, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
# Makes no difference with curvature control
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
# The stock system always uses comfortable
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
"""
Create a CAN message for the new Ford Lane Centering command.
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
additional signals for a counter and checksum.
Frequency is 20Hz.
"""
values = {
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
# 3=SafeRampOut, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
"LatCtlPath_No_Cnt": counter, # [0|15]
"LatCtlPath_No_Cs": 0, # [0|255]
}
# calculate checksum
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
"""
Creates a CAN message for the Ford ACC Command.
This command can be used to enable ACC, to set the ACC gas/brake/decel values
and to disable ACC.
Frequency is 50Hz.
"""
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
# No observed acceleration seen from this signal alone. During stock system operation, it appears to
# be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
"AccResumEnbl_B_Rq": 1 if long_active else 0,
# No observed acceleration seen from this signal alone
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
# TODO: we may be able to improve braking response by utilizing pre-charging better
# When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
"AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active
"AccStopStat_B_Rq": 1 if stopping else 0,
}
return packer.make_can_msg("ACCDATA", CAN.main, values)
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
show_distance_bars: bool, hud_control, stock_values: dict):
"""
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
assist status.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 5Hz.
"""
# Tja_D_Stat
if enabled:
if hud_control.leftLaneDepart:
status = 3 # ActiveInterventionLeft
elif hud_control.rightLaneDepart:
status = 4 # ActiveInterventionRight
else:
status = 2 # Active
elif main_on:
if hud_control.leftLaneDepart:
status = 5 # ActiveWarningLeft
elif hud_control.rightLaneDepart:
status = 6 # ActiveWarningRight
else:
status = 1 # Standby
else:
status = 0 # Off
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
"TjaWarn_D_Rq", # TJA warning
"TjaMsgTxt_D_Dsply", # TJA text
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
})
if CP.openpilotLongitudinalControl:
values.update({
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
"AccMsgTxt_D2_Rq": 0, # ACC text
"AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
"AccStopMde_B_Dsply": 1 if standstill else 0,
"AccWarn_D_Dsply": 0, # ACC warning
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
})
# Forwards FCW alert from IPMA
if fcw_alert:
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
stock_values: dict):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
Show the LKAS status with the "driver assist" lines in the IPC.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 1Hz.
"""
# LaActvStats_D_Dsply
# R Intvn Warn Supprs Avail No
# L
# Intvn 24 19 14 9 4
# Warn 23 18 13 8 3
# Supprs 22 17 12 7 2
# Avail 21 16 11 6 1
# No 20 15 10 5 0
#
# TODO: test suppress state
if enabled:
lines = 0 # NoLeft_NoRight
if hud_control.leftLaneDepart:
lines += 4
elif hud_control.leftLaneVisible:
lines += 1
if hud_control.rightLaneDepart:
lines += 20
elif hud_control.rightLaneVisible:
lines += 5
elif main_on:
lines = 0
else:
if hud_control.leftLaneDepart:
lines = 3 # WarnLeft_NoRight
elif hud_control.rightLaneDepart:
lines = 15 # NoLeft_WarnRight
else:
lines = 30 # LA_Off
hands_on_wheel_dsply = 1 if steer_alert else 0
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
})
return packer.make_can_msg("IPMA_Data", CAN.main, values)
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
Includes cruise control buttons, turn lights and more.
Frequency is 10Hz.
"""
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
return packer.make_can_msg("Steering_Data_FD1", bus, values)
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/human.py.txt
+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 opendbc.car import DT_CTRL
from opendbc.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/tests/reference/manifest.json
+37
View File
@@ -0,0 +1,37 @@
{
"angle": {
"origin": "opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py",
"sha256": "18fa5290edade52137c7c6705a2eb2d42c94d39d1fa649eae06b7828c68207b3",
"revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a"
},
"human": {
"origin": "opendbc_repo/opendbc/sunnypilot/car/ford/human_turn.py",
"sha256": "82735e348c5ce3e7b2c6a28ea3e298cca71ae8867433c270f7b54a9ee315ef37",
"revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a"
},
"builders": {
"origin": "opendbc_repo/opendbc/sunnypilot/car/ford/fordcan_ext.py",
"sha256": "65ad2114db7210371fc5de06de962c0e29ce396a651c1dd2d58b07a80555a1bf",
"revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a"
},
"fordcan": {
"origin": "opendbc_repo/opendbc/car/ford/fordcan.py",
"sha256": "0c35b8663afcaa3a58ab028a56f076c2c9f2f0296416c0abaf5905c674672956",
"revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a"
},
"values": {
"origin": "opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py",
"sha256": "74802d82314eaa27eb610810d2815d3da77003c4a1c62e0437e57e785ba25f52",
"revision": "e1d051d7ba270261b4455068bd68f1a58db15a4a"
},
"starpilot_curvature_controller": {
"origin": "opendbc_repo/opendbc/car/ford/carcontroller.py",
"sha256": "c131d1d5f4231004e00f46968988b5d5f379ec22ef8c44d7f67ded8464344855",
"revision": "d2ae98eb3959c88011ed537e7cbc033d91fd3efa"
},
"bpdev_trim": {
"origin": "opendbc_repo/opendbc/sunnypilot/car/ford/lane_center_trim.py",
"sha256": "1affc0abeaeeabaefca4741b0d75d8f249590c157340c5a9760a395d28f0a621",
"revision": "e22afa6be9b881fa784c92ebb316db47728a3d81"
}
}
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/starpilot_curvature_controller.py.txt
+329
View File
@@ -0,0 +1,329 @@
import math
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs
from opendbc.car.ford import fordcan
from opendbc.car.ford.manual_turn import ManualTurnLatch
from opendbc.car.ford.aol import qualified as qualified_aol
from opendbc.car.ford.stock_cruise import FordStockCruiseButton, qualified as stock_switch_qualified
from opendbc.car.ford.curvature_preview import blend_curvature
from opendbc.car.ford import mache_can
from opendbc.car.ford.extended_classic_can import create_extended_classic_lat_ctl_msg
from opendbc.car.ford.classic_lateral import create_controller as create_classic_controller, bounded_command as classic_bounded_command
from opendbc.car.ford.generic_canfd_can import create_extended_canfd_msg
from opendbc.car.ford.generic_canfd_lateral import (
GenericCanfdLateralController, qualified as generic_canfd_qualified, bounded_command as generic_canfd_bounded_command,
)
from opendbc.car.ford.mache_lateral import MachELateralController, FordLateralResult, bounded_command, qualified as mache_qualified
from opendbc.car.ford.values import CarControllerParams, FordFlags, FordSafetyFlags, CAR
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
LongCtrlState = structs.CarControl.Actuators.LongControlState
VisualAlert = structs.CarControl.HUDControl.VisualAlert
def anti_overshoot(apply_curvature, apply_curvature_last, v_ego):
diff = 0.1
tau = 5 # 5s smooths over the overshoot
dt = DT_CTRL * CarControllerParams.STEER_STEP
alpha = 1 - np.exp(-dt / tau)
lataccel = apply_curvature * (v_ego ** 2)
last_lataccel = apply_curvature_last * (v_ego ** 2)
last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff)
last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel
output_curvature = last_lataccel / (max(v_ego, 1) ** 2)
return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature]))
def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float:
if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping):
return accel
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
accel -= creep_accel
return float(accel)
def apply_brake_rate_limit(accel: float, gas: float, accel_last: float, v_ego: float, car_fingerprint: str,
*, stopping: bool, standstill: bool, fcw: bool) -> tuple[float, float]:
mild_mach_e_braking = (
car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and
-1.0 < gas < 0.0 and v_ego >= 5.0 and not (stopping or standstill or fcw)
)
if mild_mach_e_braking:
accel = max(accel, min(accel_last, 0.0) - 2.0 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL)
gas = accel
else:
accel = max(accel, accel_last - 3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL)
return float(accel), float(gas)
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.packer = CANPacker(dbc_names[Bus.pt])
self.CAN = fordcan.CanBus(CP)
self.manual_turn = ManualTurnLatch() if CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not CP.flags & FordFlags.LKA_STEERING else None
self.stock_cruise_button = FordStockCruiseButton()
self.manual_turn_inputs = None
self.mache_lateral = MachELateralController(CP) if mache_qualified(CP) else None
self.mache_extended_announced = False
self.generic_canfd = generic_canfd_qualified(CP)
self.classic_lateral = GenericCanfdLateralController(CP) if self.generic_canfd else create_classic_controller(CP)
self.classic_extended_announced = False
self.classic_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and
s.safetyParam & (FordSafetyFlags.CLASSIC_EXTENDED | FordSafetyFlags.GENERIC_CANFD_EXTENDED) for s in CP.safetyConfigs)
self.mache_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and
s.safetyParam & FordSafetyFlags.MACH_E_EXTENDED for s in CP.safetyConfigs)
self.apply_curvature_last = 0
self.anti_overshoot_curvature_last = 0
self.accel = 0.0
self.gas = 0.0
self.brake_request = False
self.main_on_last = False
self.lkas_enabled_last = False
self.steer_alert_last = False
self.lead_distance_bars_last = None
self.distance_bar_frame = 0
self.apply_lka_angle_last = 0.
self.apply_lka_curvature_last = 0.
def update(self, CC, CS, now_nanos):
can_sends = []
if (self.mache_lateral is not None or self.classic_lateral is not None) and self.manual_turn_inputs is not None:
self.manual_turn_inputs.update()
self.manual_turn_inputs.apply_blend_settings(self)
actuators = CC.actuators
hud_control = CC.hudControl
main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
### acc buttons ###
stock_cancel = False
stock_resume = False
if stock_switch_qualified(self.CP):
stock_cancel, stock_resume = self.stock_cruise_button.update(
bool(CS.buttons_stock_values["CcAslButtnCnclResPress"]),
CS.out.cruiseState.available, CS.out.cruiseState.enabled)
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
elif (stock_cancel or stock_resume) and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(
self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume))
can_sends.append(fordcan.create_button_msg(
self.packer, self.CAN.main, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
# if stock lane centering isn't off, send a button press to toggle it off
# the stock system checks for steering pressed, and eventually disengages cruise control
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
### lateral control ###
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING:
# Preserve the stock LMC heartbeat without requesting lateral action.
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, False, 0., 0., 0., 0.,
stock_lmc=CS.lateral_motion_control))
elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.mache_lateral is not None:
permission = getattr(self.manual_turn_inputs, "assist_permission", None)
self.mache_lateral.set_assist_permission(*(permission() if permission is not None else ((), False)))
inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None
if inputs is not None:
self.mache_lateral.set_inputs(*inputs)
else:
# Withdraw stale preview while retaining current actuator/manual-turn control.
self.mache_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True)
previous = self.mache_lateral.curvature_last
previous_path_angle = self.mache_lateral.path_angle_last
demanded = self.mache_lateral.update(CC, CS, actuators) if self.mache_extended_announced else FordLateralResult()
lateral = bounded_command(self.mache_lateral, demanded, previous, CS.out.vEgoRaw, previous_path_angle)
self.mache_lateral_demand = demanded
self.apply_curvature_last = lateral.curvature
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(mache_can.create_lat_ctl2_msg(
self.packer, self.CAN, int(lateral.active), lateral.ramp_type, lateral.precision_type,
-lateral.curvature, -lateral.curvature_rate, counter, -lateral.path_angle))
elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.classic_lateral is not None:
inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None
if inputs is not None:
self.classic_lateral.set_inputs(*inputs)
else:
self.classic_lateral.set_inputs(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True)
previous = self.classic_lateral.curvature_last
demanded = self.classic_lateral.update(CC, CS, actuators) if self.classic_extended_announced else FordLateralResult()
bound = generic_canfd_bounded_command if self.generic_canfd else classic_bounded_command
lateral = bound(self.classic_lateral, demanded, previous, CS.out.vEgoRaw,
-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1))
self.classic_lateral_demand = demanded
self.apply_curvature_last = lateral.curvature
if self.generic_canfd:
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(create_extended_canfd_msg(
self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type,
-lateral.curvature, -lateral.curvature_rate, counter))
else:
can_sends.append(create_extended_classic_lat_ctl_msg(
self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type,
-lateral.curvature, -lateral.curvature_rate))
elif (self.frame % CarControllerParams.STEER_STEP) == 0:
lateral_active = CC.latActive and not self.mache_profile and not self.classic_profile
manual_turn = False
if self.manual_turn is not None and self.manual_turn_inputs is not None:
detection_enabled, lane_change, model_ready = self.manual_turn_inputs.update()
manual_turn = self.manual_turn.update(CC, CS, float(actuators.curvature), detection_enabled, lane_change, model_ready)
if manual_turn:
lateral_active = False
self.apply_curvature_last = 0.
# Bronco and some other cars consistently overshoot curv requests
# Apply some deadzone + smoothing convergence to avoid oscillations
if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
apply_curvature = self.anti_overshoot_curvature_last
else:
apply_curvature = actuators.curvature
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
if self.manual_turn is not None and self.manual_turn_inputs is not None and lateral_active:
prediction = getattr(self.manual_turn_inputs, "preview_curvature", None)
preview = prediction(CS.out.vEgoRaw) if prediction is not None else None
if preview is not None:
apply_curvature = blend_curvature(apply_curvature, preview, current_curvature)
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
if CS.out.vEgoRaw > 9:
apply_curvature = float(np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR))
apply_curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw,
0., lateral_active, CarControllerParams.STEER_STEP)
self.apply_curvature_last = 0. if manual_turn else apply_curvature
if self.CP.flags & FordFlags.CANFD:
# TODO: extended mode
# Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02 m^-1)
# can actuate the steering for a large portion of any lateral movements. However, in order to get further control on
# steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes.
# A detailed explanation on ford control can be found here:
# https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706
mode = 1 if lateral_active else 0
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, lateral_active, 0., 0., -self.apply_curvature_last, 0.))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
if self.CP.flags & FordFlags.LKA_STEERING:
source_age_ns = now_nanos - CS.lkas_available_ts_nanos
lka_active = (CC.latActive and CS.lkas_available and 0 <= source_age_ns <= 100_000_000 and
(CS.out.cruiseState.enabled or qualified_aol(self.CP, marked_only=True)) and
not CS.out.steerFaultTemporary and
not CS.out.vehicleSensorsInvalid)
if lka_active:
desired_angle = float(np.clip(actuators.steeringAngleDeg - CS.out.steeringAngleDeg, -5.8, 5.8))
self.apply_lka_angle_last = float(np.clip(desired_angle, self.apply_lka_angle_last - 1.,
self.apply_lka_angle_last + 1.))
desired_curvature = -actuators.curvature
current_curvature = CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
if CS.out.vEgoRaw > 9.:
desired_curvature = float(np.clip(desired_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR))
self.apply_lka_curvature_last = CarControllerParams.LKA_CURVATURE_LIMITS.apply_limits(
desired_curvature, self.apply_lka_curvature_last, CS.out.vEgoRaw, 0., True, CarControllerParams.LKA_STEP)
else:
self.apply_lka_angle_last = 0.
self.apply_lka_curvature_last = 0.
self.apply_curvature_last = -self.apply_lka_curvature_last
direction = 2 if self.apply_lka_angle_last >= 0. else 4
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, lka_active, self.apply_lka_angle_last,
direction, self.apply_lka_curvature_last, transit=True))
elif self.mache_lateral is not None:
can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN))
self.mache_extended_announced = True
elif self.classic_lateral is not None:
can_sends.append(mache_can.create_lka_msg(self.packer, self.CAN))
self.classic_extended_announced = True
else:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
### longitudinal control ###
# send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
accel = actuators.accel
gas = accel
stopping = actuators.longControlState == LongCtrlState.stopping
if CC.longActive:
accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint,
standstill=CS.out.standstill, stopping=stopping)
accel, gas = apply_brake_rate_limit(
accel, gas, self.accel, CS.out.vEgo, self.CP.carFingerprint,
stopping=stopping, standstill=CS.out.standstill,
fcw=fcw_alert or CS.out.stockFcw or CS.out.stockAeb,
)
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
# Both gas and accel are in m/s^2, accel is used solely for braking
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
gas = CarControllerParams.INACTIVE_GAS
# PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
accel_due_to_pitch = 0.0
if len(CC.orientationNED) == 3:
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
accel_pitch_compensated = accel + accel_due_to_pitch
if accel_pitch_compensated > 0.3 or not CC.longActive:
self.brake_request = False
elif accel_pitch_compensated < 0.0:
self.brake_request = True
# TODO: look into using the actuators packet to send the desired speed
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
self.accel = accel
self.gas = gas
### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# send lkas ui msg at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
# send acc ui msg at 5Hz or if ui state changes
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
send_ui = True
self.distance_bar_frame = self.frame
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
show_distance_bars = self.frame - self.distance_bar_frame < 400
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
hud_control, CS.acc_tja_status_stock_values))
self.main_on_last = main_on
self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert
self.lead_distance_bars_last = hud_control.leadDistanceBars
new_actuators = actuators.as_builder()
new_actuators.curvature = self.apply_curvature_last
if self.CP.flags & FordFlags.LKA_STEERING:
new_actuators.steeringAngleDeg = CS.out.steeringAngleDeg + self.apply_lka_angle_last
new_actuators.accel = self.accel
new_actuators.gas = self.gas
self.frame += 1
return new_actuators, can_sends
opendbc_repo/opendbc/bluepilot_lateral/tests/reference/values.py.txt
+125
View File
@@ -0,0 +1,125 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from collections import namedtuple
from opendbc.car import structs
from opendbc.car.docs_definitions import CarParts, Device
from opendbc.car.ford.values import CAR
from opendbc.car.lateral import AngleSteeringLimits
ButtonType = structs.CarState.ButtonEvent.Type
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
# Ford cruise control buttons are in the Steering_Data_FD1 message (CAN ID 131)
# These signals are 1-bit flags: 1 = pressed, 0 = not pressed
#
# Note: Some buttons are combo buttons that emit multiple ButtonEvent types:
# - CcAslButtnSetIncPress emits both accelCruise (type 3) and setCruise (type 9)
# - CcAslButtnCnclResPress emits both cancel (type 5) and resumeCruise (type 10)
#
# There is also a separate CcAslButtnSetPress signal for the standalone "Set" button,
# but based on user mapping, setCruise is mapped to the combo button instead.
BUTTONS = [
# Combo button: Set + Increase (emits accelCruise when enabled, setCruise when disabled)
Button(ButtonType.accelCruise, "Steering_Data_FD1", "CcAslButtnSetIncPress", [1]),
Button(ButtonType.setCruise, "Steering_Data_FD1", "CcAslButtnSetIncPress", [1]),
# Combo button: Set + Decrease (emits decelCruise when enabled, setCruise when disabled)
Button(ButtonType.decelCruise, "Steering_Data_FD1", "CcAslButtnSetDecPress", [1]),
Button(ButtonType.setCruise, "Steering_Data_FD1", "CcAslButtnSetDecPress", [1]),
# Combo button: Cancel/Resume (emits cancel when enabled, resumeCruise when disabled)
Button(ButtonType.cancel, "Steering_Data_FD1", "CcAslButtnCnclResPress", [1]),
Button(ButtonType.resumeCruise, "Steering_Data_FD1", "CcAslButtnCnclResPress", [1]),
# Main cruise button (on/off toggle)
Button(ButtonType.mainCruise, "Steering_Data_FD1", "CcButtnOnOffPress", [1]),
]
class FordSafetyFlagsSP:
"""Sunnypilot-level safety flags for Ford.
Carried in CP_SP.safetyParam and delivered to the safety firmware as
current_safety_param_sp (the separate SP uint16, USB control 0xdf) -- NOT the main
safetyConfigs[].safetyParam. ford_init reads it with GET_FLAG(current_safety_param_sp,
...), same pattern as Subaru STOP_AND_GO (subaru_common.h). Plain int constants, not
IntFlag: CP_SP.safetyParam must stay a plain int through capnp serialization in card.
"""
STEER_ANGLE_CURVATURE = 1
# Geometry-table index for the steering-angle curvature measurement, packed into
# CP_SP.safetyParam bits 1-4 when STEER_ANGLE_CURVATURE is set. Must match the
# ford_pinion_geometry table in safety/modes/ford.h row for row (enforced by
# test_ford.py's geometry-consistency test against CarSpecs + calc_slip_factor).
# Index 0 is reserved as invalid: the firmware treats flag-set-but-no-index as feature
# off, so a half-configured param can never select the wrong geometry silently.
# FORD_EDGE_MK2 is deliberately absent: ALT_STEER_ANGLE platforms read a RELATIVE pinion
# angle (SteeringPinion_Data_Alt + learned offset) and lack the absolute measurement
# this feature needs -- the toggle no-ops there and yaw behavior is kept.
FORD_PINION_GEOMETRY_SHIFT = 1
FORD_PINION_GEOMETRY_INDEX = {
CAR.FORD_BRONCO_SPORT_MK1: 1,
CAR.FORD_ESCAPE_MK4: 2,
CAR.FORD_ESCAPE_MK4_5: 3,
CAR.FORD_EXPEDITION_MK4: 4,
CAR.FORD_EXPLORER_MK6: 5,
CAR.FORD_FOCUS_MK4: 6,
CAR.FORD_F_150_LIGHTNING_MK1: 7,
CAR.FORD_F_150_MK14: 8,
CAR.FORD_MAVERICK_MK1: 9,
CAR.FORD_MONDEO_MK5: 10,
CAR.FORD_MUSTANG_MACH_E_MK1: 11,
CAR.FORD_RANGER_MK2: 12,
}
# BluePilot: Max curvature for steering command (m^-1), from DBC file limits
CURVATURE_MAX = 0.02
# BluePilot: Curvature rate limits — 3-point breakpoints for smoother lateral control.
# Upstream opendbc uses 2-point ([5, 25]) with more conservative values.
# These allow higher rates at low speed for responsiveness, lower rates at mid-speed
# for comfort, and very low rates at highway speed for stability.
#
# Control (Python) uses stricter windup than unwind so OP stays inside panda when apply_std
# picks the wrong table vs steer_angle_cmd_checks. Safety firmware uses looser symmetric ROCs
# (former “down” table for both up/down) — see ford.h FORD_LIMITS.
# Tests: test_ford.py ANGLE_RATE_* match ford.h, not the stricter BP_ANGLE_LIMITS up row.
_BP_ANGLE_RATE_UP = ([5, 16, 25], [0.0025, 0.0012, 0.00008])
_BP_ANGLE_RATE_DOWN = ([5, 16, 25], [0.0025, 0.0014, 0.00018])
BP_ANGLE_LIMITS = AngleSteeringLimits(
0.02, # Max curvature for steering command, m^-1
_BP_ANGLE_RATE_UP,
_BP_ANGLE_RATE_DOWN,
)
def apply_bp_device_mount(car_docs, CP):
"""BluePilot: Select comma3 mount type per vehicle.
Most Ford vehicles use the angled mount due to windshield angle.
Vehicles not in this list use the standard mount.
"""
from opendbc.car.ford.values import CAR, CarHarness, FordFlags
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
if CP.carFingerprint in (
CAR.FORD_BRONCO_SPORT_MK1,
CAR.FORD_MAVERICK_MK1,
CAR.FORD_F_150_MK14,
CAR.FORD_F_150_LIGHTNING_MK1,
CAR.FORD_ESCAPE_MK4_5,
CAR.FORD_MUSTANG_MACH_E_MK1,
CAR.FORD_RANGER_MK2,
CAR.FORD_EDGE_MK2,
):
car_docs.car_parts = CarParts([Device.threex_angled_mount, harness])
else:
car_docs.car_parts = CarParts([Device.threex, harness])
opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_core.py
+213
View File
@@ -0,0 +1,213 @@
"""Pure angle host-independent finite inputs and explicit override/blip reset."""
import math
import unittest
from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs, Command
from opendbc.bluepilot_lateral.params import limits
from opendbc.bluepilot_lateral.tests import reference
from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, TRUCKS
from opendbc.bluepilot_lateral.can.ford import lateral, lka
from opendbc.can import CANPacker
from types import SimpleNamespace
from dataclasses import replace
from opendbc.bluepilot_lateral.core.trim import LaneCenterTrim, LaneGeometry, TrimConfig
from opendbc.bluepilot_lateral.hosts.starpilot import model_geometry
from openpilot.selfdrive.modeld.constants import ModelConstants
class TestAngleCore(unittest.TestCase):
def test_finite_permission_and_inactive_state_clear_pending_history(self):
for value in (math.nan, math.inf, -math.inf):
for field in ("speed", "desired_curvature", "measured_curvature", "steering_angle", "lateral_delay"):
core = AngleController()
inputs = Inputs(active=True, speed=15., measured_curvature=0., desired_curvature=.001)
self.assertEqual(core.step(replace(inputs, **{field: value})), Command())
core = AngleController()
for _ in range(10):
self.assertTrue(core.step(Inputs(True, 15., 0., .001)).active)
self.assertEqual(core.step(Inputs(False, 15., 0., .001)), Command())
self.assertEqual(core.path_last, 0.)
def test_actual_equivalent_jerk_and_cap_both_signs_and_speeds(self):
for speed in (1., 9., 10., 15., 25., 40., 55.):
for direction in (-1, 1):
core = AngleController(low_gain=.95, high_gain=.95)
previous = 0.
for _ in range(60):
command = core.step(Inputs(True, speed, 0., direction * .002))
if command.active:
equivalent = command.path_angle / speed / limits.GAIN_MIN
self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2))
self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05)
previous = equivalent
else:
previous = 0.
def test_human_turn_and_release_blips_are_neutral_then_bounded(self):
core = AngleController()
for _ in range(61):
result = core.step(Inputs(True, 15., 0., .001, steering_pressed=True, steering_angle=60.))
self.assertFalse(result.active)
self.assertEqual(result.path_angle, 0.)
core = AngleController()
for _ in range(12):
core.step(Inputs(True, 15., 0., .001, steering_pressed=True))
for _ in range(6):
self.assertEqual(core.step(Inputs(True, 15., 0., .001)), Command())
resumed = core.step(Inputs(True, 15., 0., .001))
self.assertTrue(resumed.active)
self.assertLess(abs(resumed.path_angle), .02)
def test_pinned_bp7_pure_angle_and_actual_control_bytes_with_documented_limits(self):
builders = reference.builders()
bus = SimpleNamespace(main=0)
differences = set()
packer = CANPacker("ford_lincoln_base_pt")
for identity in sorted(CLASSIC | CANFD):
gains = (.95, .95) if identity in TRUCKS else ((1., 1.05) if identity in CANFD else (1., 1.15))
for speed in (5., 15., 25., 40.):
for direction in (-1, 1):
adapted, upstream = AngleController(low_gain=gains[0], high_gain=gains[1]), reference.Angle(identity)
for tick in range(60):
# Tiny tracking curves stay clear of all intentional envelope changes.
desired = direction * .00005 * (1. + .2 * math.sin(tick / 8.))
inputs = Inputs(True, speed, desired, desired,
yaw_predictions=tuple(desired * speed for _ in ModelConstants.T_IDXS),
time_indices=tuple(ModelConstants.T_IDXS), lateral_delay=.15,
lane_change=tick >= 30, lane_direction=1 if direction < 0 else 2)
result = upstream.step(inputs)
command = adapted.step(inputs)
self.assertTrue(command.active)
self.assertAlmostEqual(command.path_angle, result.path_angle, places=12)
self.assertEqual(command.precision, result.precision_type)
fd = identity in CANFD
actual = lateral(packer, 0, command, canfd=fd, counter=tick % 16)
expected = (builders["create_lat_ctl2_msg"](packer, bus, 1, result.ramp_type, result.precision_type,
0., -result.path_angle, 0., 0., tick % 16) if fd else
builders["create_lat_ctl_msg"](packer, bus, True, result.ramp_type, result.precision_type,
0., -result.path_angle, 0., 0.))
self.assertEqual(actual, expected)
old_shadow = builders["create_lka_msg"](packer, bus, True, SimpleNamespace(), True, upstream.controller.bp_kappa_cmd)
new_shadow = lka(packer, 0, command.shadow_curvature, tick % 8)
# bp7 packs an empty body as raw zeros; the strict native contract requires physical zeros.
self.assertEqual(old_shadow[1][:4], b"\x00\x00\x00\x00")
self.assertEqual(new_shadow[1][:4], b"\x00\x80\x08\x00")
differences.update(("physical_neutral_dbc_offsets", "versioned_side_channel"))
# Aggressive entry demonstrates the required stricter native-equivalent derivative/cap.
adapted, upstream = AngleController(low_gain=gains[0], high_gain=gains[1]), reference.Angle(identity)
previous = 0.
for _tick in range(8):
inputs = Inputs(True, speed, 0., direction * .02,
yaw_predictions=tuple(direction * .02 * speed for _ in ModelConstants.T_IDXS),
time_indices=tuple(ModelConstants.T_IDXS), lateral_delay=.15)
result, command = upstream.step(inputs), adapted.step(inputs)
if command.active:
equivalent = command.path_angle / max(speed, .1) / limits.GAIN_MIN
self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2))
self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05)
previous = equivalent
if abs(command.path_angle - result.path_angle) > 1e-7:
differences.add("native_cap_jerk_deviation_packing")
else:
previous = 0.
self.assertEqual(differences, {"physical_neutral_dbc_offsets", "versioned_side_channel", "native_cap_jerk_deviation_packing"})
class TestAngleTrim(unittest.TestCase):
@staticmethod
def geometry(direction=1., *, width=3.3, probability=.9):
xs = (0., 10., 40.)
return LaneGeometry(xs, (direction * .3,) * 3, xs, (-width / 2,) * 3,
xs, (width / 2,) * 3, (probability,) * 2, (.1,) * 2, True)
@staticmethod
def model(g):
return SimpleNamespace(position=SimpleNamespace(x=g.position_x, y=g.position_y),
laneLines=[SimpleNamespace(x=(), y=()), SimpleNamespace(x=g.left_x, y=g.left_y),
SimpleNamespace(x=g.right_x, y=g.right_y)],
laneLineProbs=[0., *g.probabilities], laneLineStds=[0., *g.standard_deviations])
def test_disabled_trim_preserves_all_eleven_default_commands_and_wire_bytes(self):
packer = CANPacker("ford_lincoln_base_pt")
for identity in sorted(CLASSIC | CANFD):
low, high = (.95, .95) if identity in TRUCKS else ((1., 1.05) if identity in CANFD else (1., 1.15))
for speed in (5., 15., 25., 40.):
for direction in (-1, 1):
baseline, supplied = AngleController(low_gain=low, high_gain=high), AngleController(low_gain=low, high_gain=high)
self.assertFalse(supplied.trim_config.enabled)
for tick in range(30):
i = Inputs(True, speed, 0., direction * .0002)
a, b = baseline.step(i), supplied.step(replace(i, geometry=self.geometry(direction)))
self.assertEqual(a, b)
self.assertEqual(lateral(packer, 0, a, canfd=identity in CANFD, counter=tick % 16),
lateral(packer, 0, b, canfd=identity in CANFD, counter=tick % 16))
self.assertEqual(lka(packer, 0, a.shadow_curvature, tick % 8), lka(packer, 0, b.shadow_curvature, tick % 8))
self.assertEqual(supplied.trim.correction, 0.)
def test_fresh_geometry_matches_exact_currentdev_trim_and_model_fallback(self):
for speed in (5., 9., 12., 15., 25., 40.):
for direction in (-1, 1):
adapted, upstream = LaneCenterTrim(), reference.trim()()
config = TrimConfig(True, direction * .1, .25)
for tick in range(90):
g = self.geometry(direction, width=3.3 if tick < 30 or tick >= 60 else 1.5,
probability=.9 if tick < 60 else .3)
actual = adapted.update(.0001, speed, g, config, active=True, lane_change=False)
expected = upstream.update(.0001, self.model(g), speed, True, config.offset_m, config.gain, True, False)
self.assertAlmostEqual(actual, expected, places=14)
self.assertAlmostEqual(adapted.correction, upstream.correction, places=14)
projected = model_geometry(self.model(g), fresh=True)
self.assertEqual(projected, g)
self.assertIsNone(model_geometry(self.model(g), fresh=False))
def test_invalid_stale_driver_and_lane_change_suppress_trim_and_reset_history(self):
original = self.geometry()
config = TrimConfig(True, 0., .25)
for geometry in (replace(original, fresh=False), replace(original, position_x=(0., 0., 40.)),
replace(original, position_y=(0., math.nan, 0.)), replace(original, position_y=(0.,)), None):
trim = LaneCenterTrim()
self.assertNotEqual(trim.update(0., 15., original, config, active=True, lane_change=False), 0.)
self.assertEqual(trim.update(.001, 15., geometry, config, active=True, lane_change=False), .001)
self.assertEqual(trim.correction, 0.)
for changes in ({"active": False}, {"lane_change": True}, {"steering_pressed": True}):
trim = LaneCenterTrim()
trim.update(0., 15., original, config, active=True, lane_change=False)
flags = dict(active=True, lane_change=False, steering_pressed=False)
flags.update(changes)
self.assertEqual(trim.update(.001, 15., original, config, **flags), .001)
self.assertEqual(trim.correction, 0.)
for invalid in (TrimConfig(True, math.nan, .25), TrimConfig(True, 0., math.inf), TrimConfig(True, .6, .25)):
self.assertEqual(LaneCenterTrim().update(.001, 15., original, invalid, active=True, lane_change=False), .001)
# Bad lane-only references fall back to the model+offset, not a stale lane correction.
bad = replace(original, probabilities=(math.nan, .9))
without_lines = replace(original, left_x=(), right_x=())
a, b = LaneCenterTrim(), LaneCenterTrim()
self.assertEqual(a.update(0., 15., bad, TrimConfig(True, .1, .25), active=True, lane_change=False),
b.update(0., 15., without_lines, TrimConfig(True, .1, .25), active=True, lane_change=False))
core = AngleController(trim_config=config)
core.step(Inputs(True, 15., 0., 0., geometry=original))
self.assertNotEqual(core.trim.correction, 0.)
self.assertEqual(core.step(Inputs(False, 15., 0., 0., geometry=original)), Command())
self.assertEqual(core.trim.correction, 0.)
def test_enabled_trim_remains_inside_final_curvature_jerk_and_shadow_bounds(self):
for speed in (9., 10., 15., 25., 40.):
for direction in (-1, 1):
core = AngleController(low_gain=.95, high_gain=.95, trim_config=TrimConfig(True, direction * .5, 1.))
previous, previous_trim, previous_active = 0., 0., False
for _tick in range(100):
command = core.step(Inputs(True, speed, 0., direction * .02, geometry=self.geometry(-direction)))
if command.active and previous_active:
self.assertLessEqual(abs(core.trim.correction - previous_trim), .00015 + 1e-12)
previous_trim, previous_active = core.trim.correction, command.active
if command.active:
equivalent = command.path_angle / speed / limits.GAIN_MIN
self.assertLessEqual(abs(equivalent), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2))
self.assertLessEqual(abs(equivalent - previous), limits.LATERAL_JERK / max(speed - 1., 1.) ** 2 * .05)
self.assertLessEqual(abs(command.shadow_curvature), min(limits.CURVATURE_MAX, limits.LATERAL_ACCEL / max(speed, 1.) ** 2))
# Shadow comes from the final path; no independent trim/shadow command can bypass bounds.
ratio = command.path_angle / (speed * command.shadow_curvature) if command.shadow_curvature else 1.
self.assertTrue(limits.GAIN_MIN - 1e-12 <= ratio <= limits.GAIN_MAX + 1e-12)
previous = equivalent
else:
previous = 0.
self.assertEqual(core.trim.correction, 0.)
opendbc_repo/opendbc/bluepilot_lateral/tests/test_angle_native.py
+307
View File
@@ -0,0 +1,307 @@
"""Public packed angle/native boundaries; uses the runner-selected safety library."""
from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs, Command
from opendbc.bluepilot_lateral.can.ford import lateral, lka, checksum4
from opendbc.safety.tests import common
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.car.structs import CarParams
from opendbc.can import CANPacker
class TestFordAngleNative(common.SafetyTestBase):
DBC = "ford_lincoln_base_pt"
SAFETY_MODEL = CarParams.SafetyModel.ford
SAFETY_PARAM = 128
def setUp(self):
super().setUp()
self.debug = self.safety.set_safety_hooks(CarParams.SafetyModel.allOutput, 0) == 0
self.safety.set_safety_hooks(self.SAFETY_MODEL, self.SAFETY_PARAM)
self.safety.init_tests()
self.now = 1_000_000
self.lka_count = 0
self.control_count = 0
self.word = self.SAFETY_PARAM
def packet(self, msg):
addr, dat, bus = msg
return libsafety_py.make_CANPacket(addr, bus, dat)
def rx(self, name, values, *, bus=0):
self.assertTrue(self._rx(self.packer.make_can_msg_safety(name, bus, values)))
def warm(self, speed=15.0, curvature=0.0):
self.safety.set_timer(self.now)
for _ in range(6):
self.rx("BrakeSysFeatures", {"Veh_V_ActlBrk": speed * 3.6, "VehVActlBrk_D_Qf": 3})
self.rx("EngVehicleSpThrottle2", {"Veh_V_ActlEng": speed * 3.6, "VehVActlEng_D_Qf": 3})
self.rx("Yaw_Data_FD1", {"VehYaw_W_Actl": curvature * speed, "VehYawWActl_D_Qf": 3})
self.rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 0})
self.rx("DesiredTorqBrk", {"VehStop_D_Stat": 0})
self.rx("Steering_Data_FD1", {"CcAslButtnCnclResPress": 0})
if not self.safety.get_controls_allowed():
self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 0})
self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 5})
self.assertTrue(self.safety.get_controls_allowed())
def reset_word(self, word):
self.safety.set_alternative_experience(0)
self.safety.set_safety_hooks(self.SAFETY_MODEL, word)
self.word = word
self.lka_count = self.control_count = 0
self.now += 1_000_000
self.warm()
def shadow(self, command):
msg = lka(self.packer, 0, command.shadow_curvature, self.lka_count)
self.lka_count = (self.lka_count + 1) % 8
return msg
def control(self, command):
msg = lateral(self.packer, 0, command, canfd=bool(self.word & 2), counter=self.control_count)
self.control_count = (self.control_count + 1) % 16
return msg
def send(self, msg, expected=True):
self.assertEqual(bool(self._tx(self.packet(msg))), expected)
def tick(self, command):
self.now += 50_000
self.warm()
self.send(self.shadow(command))
self.send(self.control(command))
def test_real_packer_physical_neutral_shadow_and_corruptions(self):
packer = CANPacker(self.DBC)
for word in (128, 130):
for direction in (-1, 1):
for fault in ("angle", "curvature", "checksum", "version", "counter", "replay"):
with self.subTest(word=word, direction=direction, fault=fault):
self.reset_word(word)
command = Command(True, direction * 0.005, direction * 0.005 / 15.0)
# Real RX and PCM edges in warm() establish permission, with no allowance setter.
self.assertTrue(self.safety.get_controls_allowed())
good = lka(packer, 0, command.shadow_curvature, 0)
addr, data, bus = good
self.assertEqual((addr, bus), (0x3CA, 0))
self.assertEqual(data[:4], b"\x00\x80\x08\x00")
self.assertEqual(((data[2] & 15) << 8) | data[3], 2048)
self.assertEqual((data[1] << 4) | (data[2] >> 4), 2048)
self.assertEqual(data[7] & 15, 1)
self.assertEqual(data[7] >> 4, checksum4(data))
self.assertEqual(int.from_bytes(data[5:7], "big", signed=True), round(-command.shadow_curvature * 1e6))
self.send(good)
self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=0))
self.now += 50_000
self.warm()
next_good = lka(packer, 0, command.shadow_curvature, 1)
mutated = bytearray(next_good[1])
if fault == "angle":
mutated[3] ^= 1
elif fault == "curvature":
mutated[1] ^= 1
elif fault == "checksum":
mutated[7] ^= 16
elif fault == "version":
mutated[7] ^= 1
elif fault == "counter":
mutated[4] ^= 4
if fault in ("angle", "curvature", "counter"):
mutated[7] = 1 | (checksum4(mutated) << 4)
bad = good if fault == "replay" else (addr, bytes(mutated), bus)
self.send(bad, False)
self.assertTrue(self.safety.get_controls_allowed())
self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=1), False)
# A new valid announcement restores the same bounded path without relaxing native checks.
self.now += 50_000
self.warm()
self.send(next_good)
self.send(lateral(packer, 0, command, canfd=bool(word & 2), counter=1))
def test_actual_core_packer_boundary_all_namespaces(self):
words = [128, 129, 130]
if self.debug:
words.append(131)
for word in words:
for direction in (-1, 1):
self.reset_word(word)
core = AngleController(low_gain=0.95, high_gain=0.95)
active_seen = False
for _ in range(40):
command = core.step(Inputs(True, 15.0, 0.0, direction * 0.02))
active_seen |= command.active
self.tick(command)
self.assertTrue(active_seen)
self.tick(Command())
def test_cold_and_stale_shadow_require_new_valid_announcement(self):
command = Command(True, 0.005, 0.005 / 15.0)
self.warm()
self.send(self.control(command), False)
self.send(self.shadow(command))
self.send(self.control(command))
self.now += 60_001
self.warm()
self.send(self.control(command), False)
self.now += 50_000
self.warm()
self.send(self.shadow(command))
self.send(self.control(command))
def test_invalid_shadow_version_checksum_and_dbc_bits_withdraw(self):
command = Command(True, 0.005, 0.005 / 15.0)
for index, mask in ((7, 1), (7, 16), (0, 32), (4, 2), (4, 32)):
self.reset_word(128)
valid = self.shadow(command)
addr, data, bus = valid
mutated = bytearray(data)
mutated[index] ^= mask
if index != 7:
mutated[7] = 1 | (checksum4(mutated) << 4)
self.send((addr, bytes(mutated), bus), False)
self.send(self.control(command), False)
self.send(valid)
self.send(self.control(command))
def test_replay_wrong_bus_and_length_cannot_refresh_shadow(self):
command = Command(True, 0.005, 0.005 / 15.0)
self.warm()
msg = self.shadow(command)
self.send(msg)
self.send(msg, False)
self.send(self.control(command), False)
for bus, size in ((2, 8), (0, 7), (0, 16)):
addr, dat, _ = self.shadow(command)
self.send((addr, (dat + b"\x00" * 8)[:size], bus), False)
self.now += 30_000
self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 0})
self.send(self.shadow(Command()))
self.now += 30_000
self.warm()
self.send(self.shadow(command))
self.send(self.control(command))
def test_native_actual_path_cannot_be_laundered_by_shadow(self):
for command in (Command(True, 0.0005, 0.0), Command(True, -0.0005, 0.0), Command(True, 0.5, 0.0), Command(True, -0.01, 0.001),
Command(True, 0.04, 0.002), Command(True, 0.10, 0.005)):
self.reset_word(128)
self.send(self.shadow(command))
self.send(self.control(command), False)
# A rejected command does not seed an excessive subsequent ROC.
self.reset_word(128)
large = Command(True, 0.20, 0.01)
self.send(self.shadow(large))
self.send(self.control(large), False)
self.now += 50_000
self.warm()
small = Command(True, 0.005, 0.005 / 15.0)
self.send(self.shadow(small))
self.send(self.control(small))
def test_permission_and_rx_withdrawals(self):
command = Command(True, 0.005, 0.005 / 15.0)
for source in ("brake", "gas", "cruise", "speed_quality", "yaw_checksum", "stale"):
self.reset_word(128)
self.send(self.shadow(command))
self.send(self.control(command))
self.now += 50_000
self.warm()
self.send(self.shadow(command))
if source == "brake":
self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 2, "CcStat_D_Actl": 5})
elif source == "gas":
self.rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 50})
elif source == "cruise":
self.rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 3})
elif source == "speed_quality":
self.assertFalse(self._rx(self.packer.make_can_msg_safety("BrakeSysFeatures", 0, {"VehVActlBrk_D_Qf": 0})))
elif source == "yaw_checksum":
packet = self.packer.make_can_msg_safety("Yaw_Data_FD1", 0, {"VehYawWActl_D_Qf": 3})
packet[0].data[4] ^= 1
self.assertFalse(self._rx(packet))
else:
self.now += 200_001
self.safety.set_timer(self.now)
self.send(self.control(command), False)
self.send(self.control(Command()))
def test_fd_bad_crc_counter_and_mode_neutral_recovery(self):
self.reset_word(130)
command = Command(True, 0.005, 0.005 / 15.0)
self.send(self.shadow(command))
good = self.control(command)
addr, dat, bus = good
bad = bytearray(dat)
bad[1] ^= 1
self.send((addr, bytes(bad), bus), False)
self.send(good, False) # bad CRC withdrew shadow
self.now += 50_000
self.warm()
self.send(self.shadow(command))
self.send(self.control(Command())) # valid mode0 resynchronizes skipped counters
self.now += 50_000
self.warm()
self.send(self.shadow(command))
self.send(self.control(command))
self.now += 50_000
self.warm()
self.send(self.shadow(command))
self.control_count = (self.control_count + 1) % 16
self.send(self.control(command), False)
def test_adjacent_profile_version_and_release_long_denials(self):
command = Command(True, 0.005, 0.005 / 15.0)
for word in (0, 2, 4, 8, 10, 12, 18, 32, 66, 132, 134, 136, 160, 194, 256):
self.reset_word(word)
announcement = self.shadow(command)
if word & 128:
self.send(announcement, False)
else:
self._tx(self.packet(announcement)) # Existing neutral metadata is not angle authority.
self.send(self.control(command), False)
self.reset_word(131)
expected = bool(self.debug)
self.send(self.shadow(command), expected)
self.send(self.control(command), expected)
def test_rejected_metadata_does_not_erase_last_accepted_path_and_neutral_resync(self):
self.reset_word(128)
# At25m/s ROC is tight enough that erasing the accepted path would reject a same-path retry.
core = AngleController(low_gain=.95, high_gain=.95)
for _ in range(30):
self.now += 50_000
self.warm(speed=25., curvature=.001)
command = core.step(Inputs(True, 25., -.001, -.001))
self.send(self.shadow(command))
self.send(self.control(command))
self.assertGreater(abs(command.path_angle), .01)
self.now += 50_000
self.warm(speed=25., curvature=.001)
good = self.shadow(command)
addr, data, bus = good
bad = bytearray(data)
bad[7] ^= 16
self.send((addr, bytes(bad), bus), False)
self.send(good)
self.send(self.control(command))
self.now += 50_000
self.warm(speed=25., curvature=.001)
self.send(self.control(Command()))
self.now += 50_000
self.warm(speed=25., curvature=.001)
recovery = AngleController(low_gain=.95, high_gain=.95).step(Inputs(True, 25., -.001, -.001))
self.send(self.shadow(recovery))
self.send(self.control(recovery))
def test_rt_rate_denial_expires_and_neutral_can_recover(self):
self.reset_word(130)
for _ in range(8):
self.send(self.control(Command()))
self.send(self.control(Command()), False)
self.now += 250_000
self.warm()
self.send(self.control(Command()))
self.now += 50_000
self.warm()
command = Command(True, .005, .005 / 15.)
self.send(self.shadow(command))
self.send(self.control(command))
opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE
+7
View File
@@ -0,0 +1,7 @@
Copyright (c) 2018, Comma.ai, Inc.
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
opendbc_repo/opendbc/bluepilot_lateral/upstream-root-LICENSE.md
+21
View File
@@ -0,0 +1,21 @@
# Custom MIT License
Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions:
1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s).
2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment:
"This software is licensed under a custom license requiring permission for use."
3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment:
"This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use."
4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Contact sunnypilot Support <support@sunnypilot.ai> for permission requests.
---
Haibin Wen, SUNNYPILOT LLC
opendbc_repo/opendbc/car/ford/carcontroller.py
+29 -3
View File
@@ -1,6 +1,8 @@
import math
import numpy as np
from opendbc.can import CANPacker
from opendbc.bluepilot_lateral import BluePilotLateral
from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs
from opendbc.car.ford import fordcan
from opendbc.car.ford.manual_turn import ManualTurnLatch
@@ -15,6 +17,7 @@ from opendbc.car.ford.generic_canfd_lateral import (
GenericCanfdLateralController, qualified as generic_canfd_qualified, bounded_command as generic_canfd_bounded_command,
)
from opendbc.car.ford.mache_lateral import MachELateralController, FordLateralResult, bounded_command, qualified as mache_qualified
from opendbc.car.ford.new_port_curvature import create_controller as create_new_port_curvature, bounded_command as new_port_bounded_command
from opendbc.car.ford.values import CarControllerParams, FordFlags, FordSafetyFlags, CAR
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
@@ -66,6 +69,7 @@ class CarController(CarControllerBase):
self.packer = CANPacker(dbc_names[Bus.pt])
self.CAN = fordcan.CanBus(CP)
self.bp_lat = BluePilotLateral(CP) if angle_qualified(CP) else None
self.manual_turn = ManualTurnLatch() if CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not CP.flags & FordFlags.LKA_STEERING else None
self.stock_cruise_button = FordStockCruiseButton()
self.manual_turn_inputs = None
@@ -73,6 +77,7 @@ class CarController(CarControllerBase):
self.mache_extended_announced = False
self.generic_canfd = generic_canfd_qualified(CP)
self.classic_lateral = GenericCanfdLateralController(CP) if self.generic_canfd else create_classic_controller(CP)
self.new_port_curvature = create_new_port_curvature(CP)
self.classic_extended_announced = False
self.classic_profile = any(s.safetyModel == structs.CarParams.SafetyModel.ford and
s.safetyParam & (FordSafetyFlags.CLASSIC_EXTENDED | FordSafetyFlags.GENERIC_CANFD_EXTENDED) for s in CP.safetyConfigs)
@@ -93,7 +98,7 @@ class CarController(CarControllerBase):
def update(self, CC, CS, now_nanos):
can_sends = []
if (self.mache_lateral is not None or self.classic_lateral is not None) and self.manual_turn_inputs is not None:
if (self.mache_lateral is not None or self.classic_lateral is not None or self.new_port_curvature is not None) and self.manual_turn_inputs is not None:
self.manual_turn_inputs.update()
self.manual_turn_inputs.apply_blend_settings(self)
@@ -130,7 +135,10 @@ class CarController(CarControllerBase):
### lateral control ###
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING:
if (self.frame % CarControllerParams.STEER_STEP) == 0 and self.bp_lat is not None:
can_sends += self.bp_lat.step_lateral(CC, CS, self.packer, self.CAN, self.frame, now_nanos)
self.apply_curvature_last = self.bp_lat.apply_curvature_last
elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.CP.flags & FordFlags.LKA_STEERING:
# Preserve the stock LMC heartbeat without requesting lateral action.
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, False, 0., 0., 0., 0.,
stock_lmc=CS.lateral_motion_control))
@@ -175,6 +183,22 @@ class CarController(CarControllerBase):
can_sends.append(create_extended_classic_lat_ctl_msg(
self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type,
-lateral.curvature, -lateral.curvature_rate))
elif (self.frame % CarControllerParams.STEER_STEP) == 0 and self.new_port_curvature is not None:
inputs = self.manual_turn_inputs.lateral_snapshot(CS.out.vEgoRaw) if self.manual_turn_inputs is not None else None
self.new_port_curvature.set_inputs(*(inputs if inputs is not None else
(None, (), 0.2, self.manual_turn_inputs.enabled if self.manual_turn_inputs is not None else True)))
previous = self.new_port_curvature.curvature_last
demanded = self.new_port_curvature.update(CC, CS, actuators)
lateral = new_port_bounded_command(self.new_port_curvature, demanded, previous, CS.out.vEgoRaw,
-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1))
self.new_port_curvature_demand = demanded
self.apply_curvature_last = lateral.curvature
if self.CP.flags & FordFlags.CANFD:
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, int(lateral.active), 0., 0.,
-lateral.curvature, 0., counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, lateral.active, 0., 0., -lateral.curvature, 0.))
elif (self.frame % CarControllerParams.STEER_STEP) == 0:
lateral_active = CC.latActive and not self.mache_profile and not self.classic_profile
manual_turn = False
@@ -222,7 +246,9 @@ class CarController(CarControllerBase):
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
if self.CP.flags & FordFlags.LKA_STEERING:
if self.bp_lat is not None:
can_sends += self.bp_lat.step_lka(self.packer, self.CAN, now_nanos)
elif self.CP.flags & FordFlags.LKA_STEERING:
source_age_ns = now_nanos - CS.lkas_available_ts_nanos
lka_active = (CC.latActive and CS.lkas_available and 0 <= source_age_ns <= 100_000_000 and
(CS.out.cruiseState.enabled or qualified_aol(self.CP, marked_only=True)) and
opendbc_repo/opendbc/car/ford/explorer_lateral.py
+9 -2
View File
@@ -6,6 +6,7 @@ THIRD_PARTY_NOTICES.md.
"""
from dataclasses import replace
from typing import Protocol
import numpy as np
@@ -23,7 +24,7 @@ class ExplorerLateralController(FordLateralController):
super().__init__(CP)
self.manual_turn_detected = False
def _manual_turn(self, CC, CS, desired, driver_assisting) -> bool:
def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool:
self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting)
return self.manual_turn_detected
@@ -47,7 +48,13 @@ def qualified(CP) -> bool:
return safety.safetyModel == structs.CarParams.SafetyModel.ford and safety.safetyParam == expected
def bounded_command(owner: ExplorerLateralController, demanded: FordLateralResult,
class CurvatureOwner(Protocol):
manual_turn_detected: bool
curvature_last: float
path_angle_last: float
def bounded_command(owner: CurvatureOwner, demanded: FordLateralResult,
previous: float, speed: float, measured: float, *, absolute_cap: float | None = None) -> FordLateralResult:
"""Keep classic demand inside the modern common ISO/jerk envelope."""
limits = CarControllerParams.CURVATURE_LIMITS
opendbc_repo/opendbc/car/ford/interface.py
+1 -1
View File
@@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
ret.radarUnavailable = Bus.radar not in DBC[candidate]
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.2
ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.22
ret.steerLimitTimer = 1.0
ret.steerAtStandstill = True
opendbc_repo/opendbc/car/ford/new_port_curvature.py
+69
View File
@@ -0,0 +1,69 @@
"""Reuse the current Ford curvature strategy for exact Edge/Mondeo callers.
Only scalar curvature crosses the existing native boundary; no protocol extension.
The reused strategy retains its required provenance and notices in lateral_strategy.py.
"""
from dataclasses import replace
import math
from opendbc.car import structs
from opendbc.car.ford.explorer_lateral import bounded_command as shared_bound
from opendbc.car.ford.lateral_strategy import FordLateralController, FordLateralResult
from opendbc.car.ford.values import CAR, FordFlags
PROFILES = {
CAR.FORD_EDGE_MK2: (int(FordFlags.NEW_PORT | FordFlags.ALT_STEER_ANGLE), 8),
CAR.FORD_MONDEO_MK5: (int(FordFlags.NEW_PORT | FordFlags.CANFD), 10),
}
def qualified(cp):
profile = PROFILES.get(cp.carFingerprint)
if (profile is None or cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar or
cp.alternativeExperience not in (0, 32) or cp.pcmCruise == cp.openpilotLongitudinalControl or
cp.steerControlType != structs.CarParams.SteerControlType.angle or
len(cp.safetyConfigs) not in (1, 2) or (cp.alternativeExperience == 32 and len(cp.safetyConfigs) != 1)):
return False
flags, word = profile
if (int(cp.flags) & ~int(FordFlags.HAS_BSM)) != flags:
return False
if len(cp.safetyConfigs) == 2 and (cp.safetyConfigs[0].safetyModel != structs.CarParams.SafetyModel.noOutput or
cp.safetyConfigs[0].safetyParam != 0):
return False
safety = cp.safetyConfigs[-1]
return safety.safetyModel == structs.CarParams.SafetyModel.ford and safety.safetyParam == word + int(cp.openpilotLongitudinalControl)
class NewPortCurvatureController(FordLateralController):
def __init__(self, cp):
if not qualified(cp):
raise ValueError("Edge/Mondeo curvature requires its exact current native profile")
super().__init__(cp)
self.manual_turn_detected = False
def _manual_turn(self, CC, CS, desired: float, driver_assisting: bool = False) -> bool:
self.manual_turn_detected = super()._manual_turn(CC, CS, desired, driver_assisting)
return self.manual_turn_detected
def update(self, CC, CS, actuators):
out = CS.out
if (not CC.latActive or not out.canValid or out.canTimeout or out.steerFaultTemporary or
out.steerFaultPermanent or out.vehicleSensorsInvalid or
not all(math.isfinite(value) for value in (out.vEgoRaw, out.yawRate, out.steeringAngleDeg, actuators.curvature))):
self.human_turn.reset()
self.curvature_samples.clear()
self.curvature_last = self.path_angle_last = 0.0
self.manual_turn_detected = False
return FordLateralResult()
return super().update(CC, CS, actuators)
def create_controller(cp):
return NewPortCurvatureController(cp) if qualified(cp) else None
def bounded_command(owner, demanded, previous, speed, measured):
cap = (3.0 - 9.81 * .06) / max(speed, 1.) ** 2 if owner.CP.flags & FordFlags.CANFD else None
command = shared_bound(owner, demanded, previous, speed, measured, absolute_cap=cap)
# These profiles keep their current curvature-only native/codec contract.
return replace(command, curvature_rate=0.0, path_angle=0.0, ramp_type=0, precision_type=1)
opendbc_repo/opendbc/car/ford/stock_cruise.py
+3
View File
@@ -27,6 +27,9 @@ class FordStockCruiseButton:
def qualified(CP) -> bool:
from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified
if angle_qualified(CP):
return not CP.openpilotLongitudinalControl
if CP.alternativeExperience == 32 and len(CP.safetyConfigs) != 1:
return False
"""Existing stock profiles with the paired physical-driver button contract."""
opendbc_repo/opendbc/car/ford/tests/test_new_port_curvature.py
+187
View File
@@ -0,0 +1,187 @@
"""Current-strategy Edge/Mondeo wiring and current native scalar-curvature proof.
Source-only authored cases; runtime must bind actual DEBUG/RELEASE libraries.
"""
import os
from pathlib import Path
import unittest
from unittest.mock import patch
from openpilot.cereal import messaging
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.starpilot.tests import test_vehicle_preferences as startup
from openpilot.starpilot.lateral.tests import test_lane_runtime as lane
from opendbc.car import gen_empty_fingerprint, structs
from opendbc.car.ford.new_port_curvature import qualified, create_controller
from opendbc.car.ford.tests.test_three_ports import params as factory
from opendbc.car.ford.values import CAR, FordFlags
from opendbc.safety.tests import test_ford_aol as physical
from opendbc.safety.tests.libsafety import libsafety_py
PROFILES = (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5)
class TestNewPortCurvature(unittest.TestCase):
def test_exact_existing_factory_profiles_and_no_namespace_or_topology_inference(self):
for identity in PROFILES:
for alpha, release in ((False, False), (True, False), (False, True), (True, True)):
cp = factory(identity, alpha, release)
self.assertTrue(qualified(cp))
self.assertIsNotNone(create_controller(cp))
original = cp.to_bytes()
create_controller(cp)
self.assertEqual(cp.to_bytes(), original)
for field, value in (("passive", True), ("dashcamOnly", True), ("notCar", True), ("alternativeExperience", 64)):
with structs.CarParams.from_bytes(original) as prior:
changed = prior.as_builder()
setattr(changed, field, value)
self.assertFalse(qualified(changed))
for word in (0, 2, 12, 18, 32, 66, 128, 130, 65535):
with structs.CarParams.from_bytes(original) as prior:
changed = prior.as_builder()
changed.safetyConfigs[-1].safetyParam = word
self.assertIsNone(create_controller(changed))
cp.alternativeExperience = 32
self.assertTrue(qualified(cp))
cp.safetyConfigs = [cp.safetyConfigs[-1], cp.safetyConfigs[-1]]
self.assertFalse(qualified(cp))
self.assertIsNone(create_controller(factory(CAR.FORD_TRANSIT_MK5)))
self.assertIsNone(create_controller(factory(CAR.FORD_BRONCO_SPORT_MK1)))
def test_saved_card_controls_actual_parser_provider_sender_and_native_both_signs(self):
safety = libsafety_py.libsafety
for identity in PROFILES:
for direction in (-1, 1):
with self.subTest(identity=identity, direction=direction), OpenpilotPrefix(), \
patch.dict(os.environ, {"REPLAY": "1", "SIMULATION": "1"}), \
patch("openpilot.selfdrive.controls.controlsd.messaging.PubMaster"):
saved = Params()
helper = startup.TestVehicleStartupPreferences()
helper.setUp()
helper.params = saved
native = physical.TestFordAolDriverIntent()
try:
for key, value in (("OpenpilotEnabledToggle", True), ("SafeMode", False), ("AlwaysOnLateral", False),
("AlphaLongitudinalEnabled", False), ("IsReleaseBranch", False)):
saved.put_bool(key, value, block=True)
observed = gen_empty_fingerprint()
observed[0][0x5A] = 8
observed[2].update({0x3D6: 8, 0x186: 8})
host, constructed, cp = helper.start(identity, key="FordHumanTurnDetection", observed=observed,
capture=lambda ci: (ci.CP.safetyConfigs[-1].safetyParam, ci.CC.new_port_curvature is not None))
self.assertEqual(constructed, [(8 if identity == CAR.FORD_EDGE_MK2 else 10, True)])
self.assertEqual(host.CI.CP.to_dict(), cp.to_dict())
self.assertTrue(qualified(cp))
owner = host.CI.CC.new_port_curvature
inputs = host.CI.CC.manual_turn_inputs
self.assertIsNotNone(inputs)
inputs.sm = messaging.SubMaster(["modelV2", "lateralDelay"])
host.sm = messaging.SubMaster(["carControl"])
controls = Controls()
native.setUp()
native.word, native.tick = cp.safetyConfigs[-1].safetyParam, 0
safety.set_alternative_experience(0)
self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, native.word), 0)
safety.init_tests()
received, sent, active_curvatures = [], [], []
def rx(name, values, *, native=native, received=received):
self.assertLessEqual(set(values), set(native.packer.dbc.name_to_msg[name].sigs))
frame = native.packer.make_can_msg(name, 0, values)
addr, data, bus = frame
self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data)))
received.append(frame)
host.publish_sendcan = lambda frames, valid=True, sent=sent: sent.extend(frames) if valid else self.fail("Invalid Card trace")
host.ci_initialized = True
host.CI.update([])
saw_human_zero = saw_fault_zero = saw_stale_model = saw_rate_neutral = False
for tick in range(600):
received.clear()
with patch.object(native, "rx", rx):
native.pump(1, main=0 if tick < 30 else 4, speed=15.)
now = (1_000_000 + native.tick * 10_000) * 1000
pressed = 100 <= tick < 430
fault = 460 <= tick < 480
extra: list[tuple[str, int, dict[str, float]]] = [("Cluster_Info1_FD1", 0, {"AccEnbl_B_RqDrv": 1}),
("BodyInfo_3_FD1", 0, {}), ("RCMStatusMessage2_FD1", 0, {"FirstRowBuckleDriver": 1}),
("INSTRUMENT_PANEL", 0, {}), ("IPMA_Data", 2, {}), ("ACCDATA", 2, {}),
("ACCDATA_2", 2, {}), ("ACCDATA_3", 2, {}),
("EPAS_INFO", 0, {"SteeringColumnTorque": 2 if pressed else 0, "EPAS_Failure": 2 if fault else 0})]
if cp.flags & FordFlags.ALT_STEER_ANGLE:
extra += [("ParkAid_Data", 0, {"ExtSteeringAngleReq2": 60 if pressed else 0, "EPASExtAngleStatReq": 0, "ApaSys_D_Stat": 0}),
("SteeringPinion_Data_Alt", 0, {"StePinRelInit_An_Sns": 60 if pressed else 0})]
else:
extra += [("SteeringPinion_Data", 0, {"StePinComp_An_Est": 60 if pressed else 0, "StePinCompAnEst_D_Qf": 3})]
host_sources = [native.packer.make_can_msg(name, bus, values) for name, bus, values in extra]
for addr, data, bus in host_sources:
self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, data)))
received.extend(host_sources)
state = host.CI.update([(now, list(received))])
if tick < 40:
continue
self.assertTrue(state.canValid)
self.assertFalse(state.canTimeout)
self.assertTrue(safety.get_controls_allowed())
with patch("openpilot.starpilot.controller_extensions.time.monotonic_ns", return_value=now), \
patch("openpilot.selfdrive.car.card.time.monotonic", return_value=now / 1e9), \
patch.object(inputs.sm, "update", return_value=None):
lane.feed(controls, now, tick, speed=15.)
car_state = messaging.new_message("carState", valid=True, logMonoTime=now)
car_state.carState = state
controls.sm.update_msgs(now / 1e9, [car_state.as_reader()])
preview = messaging.new_message("modelV2", valid=True, logMonoTime=now)
preview.modelV2 = lane.model()
preview.modelV2.action.desiredCurvature = direction * .0002
predicted = .012 + (tick - 430) * .0002 if 430 <= tick < 460 else .0012
preview.modelV2.orientationRate.z = [direction * predicted * 15.] * 33
delay = messaging.new_message("lateralDelay", valid=True, logMonoTime=now)
delay.lateralDelay.lateralDelay = .38
controls.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader()])
if tick < 490:
inputs.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader()])
command, _ = controls.state_control()
self.assertTrue(command.enabled)
control_event = messaging.new_message("carControl", valid=True, logMonoTime=now)
control_event.carControl = command
host.sm.update_msgs(now / 1e9, [control_event.as_reader()])
host.can_log_mono_time = now
sent.clear()
host.controls_update(state, command)
if tick > 510:
self.assertIsNone(owner.model)
saw_stale_model = True
for addr, data, bus in sent:
if addr not in (0x3CA, 0x3D3, 0x3D6):
continue
self.assertTrue(safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, data)), (identity, tick, hex(addr)))
if addr == 0x3CA:
self.assertEqual(data[4] & 3, 0) # Existing neutral body, no extension announcement.
elif addr in (0x3D3, 0x3D6):
fd = addr == 0x3D6
mode = (data[0] >> 4) & 7 if fd else (data[4] >> 2) & 7
curvature = ((data[2] << 3) | (data[3] >> 5)) - 1000 if fd else ((data[0] << 3) | (data[1] >> 5)) - 1000
rate = (data[6] << 3) | (data[7] >> 5) if fd else ((data[1] & 31) << 8) | data[2]
self.assertEqual(rate, 1024 if fd else 4096)
if 430 <= tick < 460 and host.CI.CC.new_port_curvature_demand.curvature_rate != 0:
saw_rate_neutral = True
if 400 <= tick < 425:
self.assertEqual((mode, curvature), (0, 0))
saw_human_zero = True
if fault:
self.assertEqual((mode, curvature), (0, 0))
saw_fault_zero = True
if 70 <= tick < 100 and mode and curvature:
active_curvatures.append(curvature)
self.assertAlmostEqual(owner._curvature_lookahead(), .38, places=6)
self.assertGreater(direction * host.CI.CC.new_port_curvature_demand.curvature, .0002)
self.assertTrue(active_curvatures)
self.assertTrue(all(value * direction < 0 for value in active_curvatures))
self.assertTrue(saw_human_zero and saw_fault_zero and saw_stale_model and saw_rate_neutral)
self.assertEqual(Path(saved.get_param_path("FordHumanTurnDetection")).read_bytes(), b"1")
finally:
helper.doCleanups()
safety.set_alternative_experience(0)
safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
opendbc_repo/opendbc/car/ford/tests/test_three_ports.py
+5 -2
View File
@@ -55,6 +55,7 @@ class TestFordThreePorts(unittest.TestCase):
with self.subTest(candidate=candidate):
stock = params(candidate)
self.assertFalse(stock.openpilotLongitudinalControl)
self.assertEqual(stock.steerActuatorDelay, 0.05 if candidate == CAR.FORD_TRANSIT_MK5 else 0.22)
self.assertTrue(stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.NEW_PORT)
self.assertFalse(stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL)
requested = params(candidate, alpha=True)
@@ -62,14 +63,16 @@ class TestFordThreePorts(unittest.TestCase):
self.assertTrue(requested.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL)
if candidate == CAR.FORD_MONDEO_MK5:
self.assertFalse(params(candidate, alpha=True, release=True).openpilotLongitudinalControl)
self.assertEqual(params(CAR.FORD_MUSTANG_MACH_E_MK1).steerActuatorDelay, 0.22)
self.assertFalse(params(CAR.FORD_BRONCO_SPORT_MK1).openpilotLongitudinalControl)
self.assertTrue(params(CAR.FORD_BRONCO_SPORT_MK1, alpha=True).openpilotLongitudinalControl)
def test_real_edge_angle_and_mondeo_gear_parsers(self):
for candidate, source_name, source_values in (
cases: tuple[tuple[CAR, str, dict[str, float]], ...] = (
(CAR.FORD_EDGE_MK2, "TransGearData", {"GearLvrPos_D_Actl": 4}),
(CAR.FORD_MONDEO_MK5, "Gear_Shift_by_Wire_FD1", {"TrnRng_D_RqGsm": 4}),
):
)
for candidate, source_name, source_values in cases:
with self.subTest(candidate=candidate):
cp = params(candidate)
self.assertEqual(cp.transmissionType, structs.CarParams.TransmissionType.automatic)
opendbc_repo/opendbc/car/ford/values.py
+1
View File
@@ -48,6 +48,7 @@ class FordSafetyFlags(IntFlag):
MACH_E_EXTENDED = 16
CLASSIC_EXTENDED = 32
EXPLORER_EXTENDED = CLASSIC_EXTENDED # Existing public profile spelling.
BLUEPILOT_ANGLE = 128
GENERIC_CANFD_EXTENDED = 64
opendbc_repo/opendbc/car/hyundai/canfd_owner.py
+36
View File
@@ -0,0 +1,36 @@
"""Selected safety slot for the exact first-generation captured CANFD owner."""
from opendbc.car.structs import CarParams
from opendbc.car.hyundai.values import CAR, HyundaiFlags
def topology_index(cp):
if len(cp.safetyConfigs) == 1:
return 0 if cp.safetyConfigs[0].safetyModel == CarParams.SafetyModel.hyundaiCanfd else None
if len(cp.safetyConfigs) != 2:
return None
auxiliary, owner = cp.safetyConfigs
required = HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEER_MSG
excluded = (HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.CANFD_LKA_STEER_MSG_ALT |
HyundaiFlags.CANFD_ALT_BUTTONS | HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CCNC | HyundaiFlags.HYBRID)
if (cp.brand != 'hyundai' or cp.carFingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) or
cp.passive or cp.dashcamOnly or cp.notCar or cp.steerControlType != CarParams.SteerControlType.torque or
cp.flags & required != required or cp.flags & excluded or
auxiliary.safetyModel != CarParams.SafetyModel.noOutput or auxiliary.safetyParam != 0 or
owner.safetyModel != CarParams.SafetyModel.hyundaiCanfd):
return None
return 1
def config_index(cp):
owner = topology_index(cp)
if owner != 1:
return owner
raw = int(cp.safetyConfigs[owner].safetyParam)
if cp.openpilotLongitudinalControl and not cp.pcmCruise:
if raw == 0x15 and cp.alternativeExperience == 0:
return 1
if cp.carFingerprint == CAR.KIA_EV6 and raw == 0x815 and cp.alternativeExperience == 32:
return 1
elif not cp.openpilotLongitudinalControl and cp.pcmCruise and cp.alternativeExperience == 0 and raw in (0x11, 0x811):
return 1
return None
opendbc_repo/opendbc/car/hyundai/canfd_stock_aol.py
+6 -4
View File
@@ -3,6 +3,7 @@
from opendbc.car.structs import CarParams
from opendbc.car.hyundai.values import CAR, HyundaiFlags
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.canfd_owner import config_index
STOCK_AOL_MARKER = 0x0800
STOCK_EV_CARS = frozenset(
@@ -29,6 +30,7 @@ STOCK_AOL_WORDS = frozenset(
def _base_word(cp, *, experiences=(0,)):
owner = config_index(cp)
allowed = (
HyundaiFlags.CANFD
| HyundaiFlags.EV
@@ -59,8 +61,7 @@ def _base_word(cp, *, experiences=(0,)):
or cp.steerControlType != CarParams.SteerControlType.torque
or not cp.flags & HyundaiFlags.CANFD
or int(cp.flags) & ~int(allowed)
or len(cp.safetyConfigs) != 1
or cp.safetyConfigs[0].safetyModel != CarParams.SafetyModel.hyundaiCanfd
or owner is None
):
return None
# EV identity is static; hybrid fuel is selected by the actual 0xFA fingerprint.
@@ -74,7 +75,8 @@ def _base_word(cp, *, experiences=(0,)):
if (ev and hybrid) or (alt_lka and not lka) or (lka and camera):
return None
bus = CanBus(cp)
if (bus.ACAN, bus.ECAN, bus.CAM) != ((0, 1, 2) if lka else (1, 0, 2)):
offset = 4 * owner
if (bus.ACAN - offset, bus.ECAN - offset, bus.CAM - offset) != ((0, 1, 2) if lka else (1, 0, 2)):
return None
word = (1 if ev else 2 if hybrid else 0) | (16 if lka else 0) | (128 if alt_lka else 0)
word |= (32 if alt_buttons else 0) | (8 if camera else 0)
@@ -89,7 +91,7 @@ def qualified(cp, *, marked_only=False):
word = _base_word(cp)
if word is None or cp.openpilotLongitudinalControl or not cp.pcmCruise:
return False
return cp.safetyConfigs[0].safetyParam in ((word | STOCK_AOL_MARKER,) if marked_only else (word, word | STOCK_AOL_MARKER))
return cp.safetyConfigs[config_index(cp)].safetyParam in ((word | STOCK_AOL_MARKER,) if marked_only else (word, word | STOCK_AOL_MARKER))
LONG_AOL_WORDS = frozenset((0x0815, 0x0895))
opendbc_repo/opendbc/car/hyundai/captured_adrv_startup.py
+13 -4
View File
@@ -6,21 +6,30 @@ from opendbc.can.parser import MAX_BAD_COUNTER
from opendbc.car.hyundai.ecu_startup import HyundaiECUStartup, Outcome
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.car.hyundai.canfd_owner import topology_index
def first_generation(cp):
owner = topology_index(cp)
if owner is None:
return False
buses = CanBus(cp)
offset = 4 * owner
excluded = (HyundaiFlags.CANFD_ANGLE_STEERING | HyundaiFlags.CANFD_LKA_STEER_MSG_ALT |
HyundaiFlags.CANFD_ALT_BUTTONS | HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CCNC |
HyundaiFlags.HYBRID)
return (cp.flags & HyundaiFlags.CANFD and
cp.flags & HyundaiFlags.EV and cp.flags & HyundaiFlags.CANFD_LKA_STEER_MSG and
not cp.flags & excluded and CanBus(cp).ACAN == 0 and CanBus(cp).ECAN == 1 and
not cp.flags & excluded and (buses.ACAN, buses.ECAN, buses.CAM) == (offset, offset + 1, offset + 2) and
not cp.passive and not cp.dashcamOnly and not cp.notCar)
class CapturedADRVStartup(HyundaiECUStartup):
def __init__(self, cp, callbacks, *, label, template_type):
super().__init__(cp, callbacks, address=0x730, bus=1, label=label)
buses = CanBus(cp)
super().__init__(cp, callbacks, address=0x730, bus=buses.ECAN, label=label)
self.template_bus = buses.ACAN
self.camera_bus = buses.CAM
self.template_type = template_type
self.template = None
@@ -35,7 +44,7 @@ class CapturedADRVStartup(HyundaiECUStartup):
if not floor < stamp <= now or now - stamp > 150_000_000:
continue
for msg in packet:
if msg.src != 0 or msg.address != 0x51:
if msg.src != self.template_bus or msg.address != 0x51:
continue
try:
candidate = self.template_type.capture(bytes(msg.dat))
@@ -66,7 +75,7 @@ class CapturedADRVStartup(HyundaiECUStartup):
return False
pt = ci.can_parsers[Bus.pt]
cam = ci.can_parsers[Bus.cam]
if pt.bus != 1 or cam.bus != 2:
if pt.bus != self.bus or cam.bus != self.camera_bus:
return False
names = ['ACCELERATOR', 'TCS', 'WHEEL_SPEEDS', 'MDPS', 'CRUISE_BUTTONS', ci.CS.gear_msg_canfd]
if not ci.CP.openpilotLongitudinalControl:
opendbc_repo/opendbc/car/hyundai/ecu_startup.py
+22 -7
View File
@@ -1,14 +1,26 @@
"""Hyundai prepublication ECU transaction; sent disable is not confirmed ownership."""
from enum import Enum
from collections.abc import Callable
from typing import TypedDict, cast
from copy import deepcopy
import time
from opendbc.car import structs
from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.hyundai.canfd_owner import config_index
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
class PreparedSafetyConfig(TypedDict):
safetyParam: int
class PreparedParams(TypedDict):
safetyConfigs: list[PreparedSafetyConfig]
alternativeExperience: int
class Outcome(Enum):
STOCK_UNTOUCHED = 'stock_untouched'
SENT_UNCONFIRMED = 'sent_unconfirmed'
@@ -60,7 +72,8 @@ class HyundaiECUStartup:
self.callbacks[1](frames)
def _restore(self):
if not self.admission():
admission = cast(Callable[[], bool], self.admission)
if not admission():
return False
recv, send = self.callbacks
session = self._query(send, recv, self.bus, [(self.address, None)], [b'\x10\x03'], [b'\x50\x03'])
@@ -126,11 +139,12 @@ class HyundaiECUStartup:
return
from opendbc.car.hyundai.ev6_aol import qualified as ev6_aol_qualified
if self.outcome is Outcome.SENT_UNCONFIRMED and ev6_aol_qualified(ci.CP, marked_only=True):
expected = deepcopy(self.prepared_cp)
expected = deepcopy(cast(PreparedParams, self.prepared_cp))
configs = expected.get('safetyConfigs', [])
if len(configs) != 1 or configs[0]['safetyParam'] != 0x15 or expected['alternativeExperience'] != 0:
owner = config_index(ci.CP)
if owner is None or len(configs) != owner + 1 or configs[owner]['safetyParam'] != 0x15 or expected['alternativeExperience'] != 0:
raise RuntimeError('EV6 prepared LONG profile cannot be finalized')
configs[0]['safetyParam'] |= 0x0800
configs[owner]['safetyParam'] |= 0x0800
expected['alternativeExperience'] = 32
if ci.CP.to_dict() != expected:
raise RuntimeError('EV6 AOL finalization changed unrelated CarParams')
@@ -140,11 +154,12 @@ class HyundaiECUStartup:
if (self.outcome not in (Outcome.STOCK_UNTOUCHED, Outcome.STOCK_RESTORED) or
not stock_aol_qualified(ci.CP, marked_only=True)):
raise RuntimeError('Hyundai startup does not admit this AOL configuration')
expected = deepcopy(self.prepared_cp)
expected = deepcopy(cast(PreparedParams, self.prepared_cp))
configs = expected.get('safetyConfigs', [])
if len(configs) != 1 or configs[0]['safetyParam'] not in (0x11, 0x91):
owner = config_index(ci.CP)
if owner is None or len(configs) != owner + 1 or configs[owner]['safetyParam'] not in (0x11, 0x91):
raise RuntimeError('Hyundai startup stock profile cannot be finalized')
configs[0]['safetyParam'] |= 0x0800
configs[owner]['safetyParam'] |= 0x0800
if ci.CP.to_dict() != expected:
raise RuntimeError('Hyundai AOL finalization changed unrelated CarParams')
self.prepared_cp = expected
opendbc_repo/opendbc/car/hyundai/ev6_aol.py
+2 -1
View File
@@ -1,5 +1,6 @@
"""First-generation EV6 physical authorization for the existing torque lease."""
from opendbc.car.hyundai.canfd_stock_aol import _base_word
from opendbc.car.hyundai.canfd_owner import config_index
from opendbc.car.hyundai.ev6_startup import eligible
from opendbc.car.hyundai.values import Buttons
@@ -9,7 +10,7 @@ def qualified(cp, *, marked_only=False):
return False
if _base_word(cp, experiences=(0, 32)) != 0x11:
return False
word = cp.safetyConfigs[0].safetyParam
word = cp.safetyConfigs[config_index(cp)].safetyParam
return (word == 0x815 and cp.alternativeExperience == 32) or (
not marked_only and word == 0x15 and cp.alternativeExperience == 0)
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+11 -6
View File
@@ -1,4 +1,5 @@
import math
from typing import cast
import numpy as np
from opendbc.car import CanBusBase, CanData
@@ -213,8 +214,8 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
object_distance, object_relative, object_valid, object_status = 1., 0., 0, 2
else:
visible = bool(lead_visible)
object_distance = float(np.clip(lead_distance if visible else 0., 0., 204.7))
object_relative = float(np.clip(lead_rel_speed if visible else 0., -16.4, 34.7))
object_distance = float(np.clip(cast(float, lead_distance) if visible else 0., 0., 204.7))
object_relative = float(np.clip(cast(float, lead_rel_speed) if visible else 0., -16.4, 34.7))
object_valid = int(not visible)
object_status = 0 if not (enabled and visible) else 1 if gas_override else 2
@@ -392,11 +393,15 @@ def create_adrv_messages(packer, CAN, frame, *, template=None, drive_gear=False,
values = {
}
from opendbc.car.hyundai.gv70_template import GV70Template
if isinstance(template, GV70Template):
ret.append(template.frame(frame, drive_gear, CAN.ACAN, speed=speed))
if template is not None:
if type(CAN.ACAN) is not int or type(CAN.offset) is not int:
raise ValueError('Captured ADRV requires integer CAN routing')
local_bus = CAN.ACAN - CAN.offset
packet = (template.frame(frame, drive_gear, local_bus, speed=speed) if isinstance(template, GV70Template) else
template.frame(frame, drive_gear, local_bus))
ret.append(CanData(packet.address, packet.dat, CAN.ACAN))
else:
ret.append(template.frame(frame, drive_gear, CAN.ACAN) if template is not None else
packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
ret.extend(create_fca_warning_light(packer, CAN, frame))
opendbc_repo/opendbc/car/hyundai/interface.py
+2
View File
@@ -150,6 +150,8 @@ class CarInterface(CarInterfaceBase):
else:
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
if candidate in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and CAN.offset >= 8:
ret.dashcamOnly = True
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiCanfd), ]
if CAN.ECAN >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
opendbc_repo/opendbc/car/hyundai/tests/test_captured_adrv_routing.py
+73
View File
@@ -0,0 +1,73 @@
"""Captured ADRV encoding keeps local validation and Panda routing separate."""
import unittest
from opendbc.can import CANPacker
from opendbc.car import Bus, CanData, structs
from opendbc.car.hyundai.ev6_template import EV6Template
from opendbc.car.hyundai.gv70_template import GV70Template
from opendbc.car.hyundai.hyundaicanfd import CanBus, create_adrv_messages, hkg_can_fd_checksum
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags
CAPTURE = bytes.fromhex('88ed2e091700ffff5e0d0000012006ff021c2200000000000800000010000000')
def buses(pandas: int, *, lka: bool = True) -> CanBus:
configs = [structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.noOutput) for _ in range(pandas - 1)]
configs.append(structs.CarParams.SafetyConfig(safetyModel=structs.CarParams.SafetyModel.hyundaiCanfd))
cp = structs.CarParams(safetyConfigs=configs, flags=int(HyundaiFlags.CANFD_LKA_STEER_MSG) if lka else 0)
return CanBus(cp)
class TestCapturedADRVRouting(unittest.TestCase):
def setUp(self):
self.packer = CANPacker(DBC[CAR.KIA_EV6][Bus.pt])
def test_ev6_and_gv70_use_local_encoding_and_logical_owner_bus(self):
for pandas in (1, 2):
can = buses(pandas)
self.assertEqual((can.offset, can.ACAN, can.ECAN), (4 * (pandas - 1), 4 * (pandas - 1), 4 * (pandas - 1) + 1))
for template in (EV6Template.capture(CAPTURE), GV70Template.capture(CAPTURE)):
for frame in (0, 7, 255, 256):
for drive in (False, True):
for speed in (None, 12.345):
with self.subTest(pandas=pandas, template=type(template).__name__, frame=frame, drive=drive, speed=speed):
local = (template.frame(frame, drive, 0, speed=speed) if isinstance(template, GV70Template) else
template.frame(frame, drive, 0))
sent = create_adrv_messages(self.packer, can, frame, template=template, drive_gear=drive, speed=speed)
self.assertEqual(sent[0], CanData(local.address, local.dat, can.ACAN))
self.assertEqual(int.from_bytes(sent[0].dat[:2], 'little'), hkg_can_fd_checksum(0x51, None, bytearray(sent[0].dat)))
self.assertEqual(template.data, CAPTURE)
self.assertTrue(all(packet[2] == can.ECAN for packet in sent[1:]))
if pandas == 2:
self.assertNotEqual(sent[0].src, 0)
def test_default_packer_sender_preserves_logical_bus_for_both_harnesses(self):
expected_packer = CANPacker(DBC[CAR.KIA_EV6][Bus.pt])
for pandas in (1, 2):
for lka in (False, True):
can = buses(pandas, lka=lka)
expected = expected_packer.make_can_msg('ADRV_0x51', can.ACAN, {})
self.assertEqual(create_adrv_messages(self.packer, can, 7)[0], expected)
def test_wrong_local_bus_and_noninteger_routing_remain_rejected(self):
for pandas in (1, 2):
for template in (EV6Template.capture(CAPTURE), GV70Template.capture(CAPTURE)):
with self.subTest(pandas=pandas, template=type(template).__name__, local_bus=1):
with self.assertRaises(ValueError):
create_adrv_messages(self.packer, buses(pandas, lka=False), 7, template=template, drive_gear=True)
for field, value in (('_a', False), ('_a', 0.0), ('offset', False), ('offset', 0.0)):
can = buses(pandas)
setattr(can, field, value)
with self.subTest(pandas=pandas, template=type(template).__name__, field=field, value=value):
with self.assertRaises(ValueError):
create_adrv_messages(self.packer, can, 7, template=template, drive_gear=True)
def test_template_physical_bus_and_drive_guards_remain_closed(self):
for pandas in (1, 2):
for template in (EV6Template.capture(CAPTURE), GV70Template.capture(CAPTURE)):
for drive in (0, 1):
with self.assertRaises(ValueError):
create_adrv_messages(self.packer, buses(pandas), 7, template=template, drive_gear=drive)
for bus in (False, 1, 4):
with self.assertRaises(ValueError):
template.frame(7, True, bus)
opendbc_repo/opendbc/safety/modes/ford.h
+16 -6
View File
@@ -149,6 +149,8 @@ static bool ford_lka_curvature_checks(int desired_curvature, bool active) {
return violation;
}
#include "opendbc/bluepilot_lateral/safety/ford_bp.h"
static void ford_rx_hook(const CANPacket_t *msg) {
ford_aol_rx(msg);
// Update in motion state from standstill signal
@@ -217,6 +219,7 @@ static void ford_rx_hook(const CANPacket_t *msg) {
// Physical combined cancel/resume switch, not the outgoing resume signal.
ford_cancel_resume_button = (msg->data[2] & 0x20U) != 0U;
}
ford_bp_rx(msg);
}
static bool ford_tx_hook(const CANPacket_t *msg) {
@@ -284,7 +287,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
}
// Safety check for Lane_Assist_Data1 action
if (msg->addr == FORD_Lane_Assist_Data1) {
if ((msg->addr == FORD_Lane_Assist_Data1) && !ford_bp_enabled) {
if (ford_mach_e_extended || ford_explorer_extended) {
tx &= (msg->data[4] & 0x1U) == 0U;
}
@@ -348,7 +351,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
}
// Safety check for LateralMotionControl action
if (msg->addr == FORD_LateralMotionControl) {
if ((msg->addr == FORD_LateralMotionControl) && !ford_bp_enabled) {
// Signal: LatCtl_D_Rq
bool steer_control_enabled = ((msg->data[4] >> 2) & 0x7U) != 0U;
unsigned int raw_curvature = (msg->data[0] << 3) | (msg->data[1] >> 5);
@@ -388,7 +391,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
}
// Safety check for LateralMotionControl2 action
if (msg->addr == FORD_LateralMotionControl2) {
if ((msg->addr == FORD_LateralMotionControl2) && !ford_bp_enabled) {
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
unsigned int raw_curvature = (msg->data[2] << 3) | (msg->data[3] >> 5);
@@ -472,6 +475,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
}
}
if (ford_bp_enabled) {
if (msg->addr == FORD_Lane_Assist_Data1) { tx &= ford_bp_lka(msg); }
if ((msg->addr == FORD_LateralMotionControl) || (msg->addr == FORD_LateralMotionControl2)) { tx &= ford_bp_lateral(msg); }
}
ford_aol_tx(msg, tx);
return tx;
}
@@ -571,11 +578,13 @@ static safety_config ford_init(uint16_t param) {
#ifdef ALLOW_DEBUG
ford_mach_e_extended |= (param == 19U) && (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param)));
#endif
ford_stock_switch = (((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))) &&
const bool bp_config_valid = ford_bp_configure(param);
ford_stock_switch = ((((unsigned int)alternative_experience == 0U) || (((unsigned int)alternative_experience == 32U) && ford_aol_param_valid(param))) &&
((param == 2U) || (param == 8U) || (param == 10U) || (param == 12U) ||
((param == 18U) && ford_mach_e_extended) ||
((param == 32U) && ford_explorer_extended) ||
((param == 66U) && ford_generic_canfd_extended));
((param == 66U) && ford_generic_canfd_extended))) ||
(ford_bp_enabled && ((param & 1U) == 0U));
ford_cancel_resume_button = false;
ford_mach_e_path_angle_last = 0;
ford_mach_e_announced = false;
@@ -615,6 +624,7 @@ static safety_config ford_init(uint16_t param) {
if (ford_explorer_extended && (param == 32U)) {
SET_TX_MSGS(FORD_STOCK_TX_MSGS, ret);
}
if (ford_bp_enabled && (param == 128U)) { SET_TX_MSGS(FORD_STOCK_TX_MSGS, ret); }
if (ford_stock_switch) {
if (ford_lka_steering) {
SET_RX_CHECKS(ford_stock_lka_rx_checks, ret);
@@ -622,7 +632,7 @@ static safety_config ford_init(uint16_t param) {
SET_RX_CHECKS(ford_stock_rx_checks, ret);
}
}
if ((mach_e_namespace && !ford_mach_e_extended) ||
if (!bp_config_valid || (mach_e_namespace && !ford_mach_e_extended) ||
(explorer_namespace && !ford_explorer_extended) ||
(generic_canfd_namespace && !ford_generic_canfd_extended)) {
ret.tx_msgs = NULL;
openpilot/cereal/custom.capnp
+16
View File
@@ -798,6 +798,22 @@ struct AolAxisState @0xfc6241ed8877b611 {
requestedLongitudinal @11 :Bool;
pandaSerial @12 :Text;
axisSessionId @13 :Text;
# Exact source Event stamp uses pandad BOOTTIME, for provenance only.
# Inventory shares the wire's CLOCK_MONOTONIC observed/expiry lease.
sourcePandaStatesMonoTime @14 :UInt64;
pandaInventory @15 :List(PandaSlot);
struct PandaSlot {
slotIndex @0 :UInt16;
hardwareSerial @1 :Text;
safetyModel @2 :UInt16;
safetyParam @3 :UInt16;
alternativeExperience @4 :UInt16;
controlsAllowed @5 :Bool;
safetyRxChecksInvalid @6 :Bool;
heartbeatLost @7 :Bool;
faults @8 :UInt32;
}
}
struct IntentWire {
openpilot/common/params_keys.h
+1
View File
@@ -29,6 +29,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"HondaBoschARadar", {PERSISTENT, BOOL, "1"}},
{"HondaGasFactorParams", {PERSISTENT, FLOAT, "1.0"}},
{"HondaWindFactorParams", {PERSISTENT, FLOAT, "1.0"}},
{"FordLateralMode", {PERSISTENT, INT, "0"}},
{"FordHumanTurnDetection", {PERSISTENT, BOOL, "1"}},
{"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4"}},
{"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4"}},
openpilot/selfdrive/car/card.py
+39 -4
View File
@@ -52,6 +52,9 @@ from opendbc.car.ford.aol import (
temporary_restriction as ford_temporary_restriction,
)
from opendbc.car.hyundai.canfd_angle_aol import native_observation, temporary_restriction, qualified as qualified_angle_aol
from opendbc.car.hyundai.canfd_owner import config_index as hyundai_canfd_config_index
from opendbc.car.hyundai.captured_adrv_startup import CapturedADRVStartup
from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config
from openpilot.starpilot.aol.runtime import current_native
from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for
from openpilot.starpilot.aol.wire import IntentState, encode_intent
@@ -346,7 +349,10 @@ class Car:
self.aol_sequence = 0
self.aol_process_fault_context = AolProcessFaultContext() if self.aol_replay else None
if self.aol_qualified:
self.CP.safetyConfigs[0].safetyParam |= aol_policy.safety_param_addition
config = hyundai_aol_safety_config(self.CP) if self.CP.brand == 'hyundai' else self.CP.safetyConfigs[0]
if config is None:
raise RuntimeError('AOL requires the exact vehicle safety configuration')
config.safetyParam |= aol_policy.safety_param_addition
self.CP.alternativeExperience |= aol_policy.alternative_experience_addition
self.vehicle_startup.finalize_aol_configuration(self.CI)
self.aol_transport_feedback = TransportPauseFeedback(self.CP) if self.aol_replay else None
@@ -446,12 +452,15 @@ class Car:
self.timing_mark('can_receive_start')
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
self.timing_mark('can_receive_end')
can_received_ns = time.monotonic_ns()
can_list = can_capnp_to_list(can_strs)
self.timing_mark('can_decode_end')
# Update carState from CAN
CS = self.CI.update(can_list)
self.timing_mark('interface_update_end')
cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME)
if getattr(self.CI.CS, 'ev6_aol_source_healthy', False) else 0)
self.observe_ioniq6_long_authority(can_list, CS)
self.timing_mark('ioniq_authority_end')
media_owner = getattr(self, 'ioniq6_media', None)
@@ -480,6 +489,8 @@ class Car:
self.slc_receipts = []
now_ns = int(self.can_log_mono_time) if REPLAY and hasattr(self, 'can_log_mono_time') else time.monotonic_ns()
self._aol_can_observation = (CS, can_received_ns,
max((stamp for stamp, _ in can_list), default=0), cancel_boot_ns)
self.refresh_slc_configuration(now_ns)
evidence = getattr(self.CI.CS, 'dashboard_limit', None) if self.slc_replay else None
observation = evidence.observation if evidence is not None else None
@@ -520,7 +531,7 @@ class Car:
if getattr(self, 'aol_transport_feedback', None) is None:
self.aol_transport_feedback = TransportPauseFeedback(self.CP)
transport_reset = self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
latched=self.aol_card_intent.allowed_latch)
latched=self.aol_card_intent.allowed_latch, cancel_source_ns=self.aol_cancel_source(CS, now_ns))
fault_active = True if transport_reset else None
event_ns = int(self.sm.logMonoTime['onroadEvents'])
if (getattr(self.aol_card_intent, 'requires_fault_observation', self.aol_card_intent.explicit_latch) and
@@ -705,12 +716,34 @@ class Car:
self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode,
resume=self.vehicle_startup.consume_cruise_resume())
def aol_cancel_source(self, CS, now_ns: int) -> int:
"""Bind Cancel to this parsed CAN batch and its local monotonic receipt."""
from opendbc.car.hyundai.values import Buttons
state = getattr(getattr(self, 'CI', None), 'CS', None)
observation = getattr(self, '_aol_can_observation', None)
if (state is None or observation is None or observation[0] is not CS or
not getattr(state, 'ev6_aol_source_healthy', False)):
return 0
_, received_ns, can_source_ns, boot_ns = observation
stamp = int(state.ev6_aol_sample_stamp_ns)
# Parser source and CAN batch use the producer clock; compare only those.
# The fixed intent-retention deadline uses the local MONOTONIC receipt.
if (not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000 or
not 0 < stamp <= can_source_ns <= boot_ns or
boot_ns - stamp > min(int(state.ev6_aol_timeout_ns), 30_000_000) or
can_source_ns - stamp > min(int(state.ev6_aol_timeout_ns), 30_000_000) or
not any(e.type == structs.CarState.ButtonEvent.Type.cancel and e.pressed for e in CS.buttonEvents) or
not any(cruise == Buttons.CANCEL for _, _, cruise in state.ev6_aol_samples)):
return 0
return received_ns
def aol_disarming_fault(self, CS, event_ns: int, now_ns: int) -> bool:
from openpilot.starpilot.car.gm.aol import intent_disarming_fault
if getattr(self, 'aol_transport_feedback', None) is None:
self.aol_transport_feedback = TransportPauseFeedback(self.CP)
if self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
latched=getattr(getattr(self, 'aol_card_intent', None), 'allowed_latch', True)):
latched=getattr(getattr(self, 'aol_card_intent', None), 'allowed_latch', True),
cancel_source_ns=self.aol_cancel_source(CS, now_ns)):
return True
return intent_disarming_fault(self.CP, self.sm['onroadEvents'], CS,
temporary_ui_process_failure=self.aol_process_fault_context.temporary_ui_failure(event_ns, now_ns),
@@ -1029,10 +1062,12 @@ class Car:
sm = self.sm
stamp, receipt = int(sm.logMonoTime['pandaStates']), int(sm.recv_time['pandaStates'] * 1e9)
pandas = sm['pandaStates']
owner = getattr(self.vehicle_startup, 'owner', None)
count = 2 if isinstance(owner, CapturedADRVStartup) and hyundai_canfd_config_index(owner.cp) == 1 else 1
return bool(not self.params.get_bool('IsOffroad') and not self.params.get_bool('ControlsReady') and
sm.seen['pandaStates'] and sm.valid['pandaStates'] and sm.alive['pandaStates'] and
0 < stamp <= boot and boot - stamp <= 300_000_000 and
0 < receipt <= now and now - receipt <= 300_000_000 and len(pandas) == 1 and
0 < receipt <= now and now - receipt <= 300_000_000 and len(pandas) == count and
all(ps.safetyModel == structs.CarParams.SafetyModel.elm327 and ps.safetyParam == 1 and
not ps.controlsAllowed and not ps.safetyRxChecksInvalid and
(ps.ignitionLine or ps.ignitionCan) for ps in pandas))
openpilot/selfdrive/pandad/SConscript
+3
View File
@@ -16,3 +16,6 @@ if arch != "Darwin":
if GetOption('extras') or any(target.startswith('openpilot/selfdrive/pandad/tests/') for target in COMMAND_LINE_TARGETS):
env.Program('tests/test_pandad_canprotocol', ['tests/test_pandad_canprotocol.cc'], LIBS=[panda] + libs)
if GetOption('extras') or any(target.startswith('openpilot/selfdrive/pandad/tests/') for target in COMMAND_LINE_TARGETS):
env.Program('tests/test_panda_owner', ['tests/test_panda_owner.cc', 'panda_safety.cc'], LIBS=[panda] + libs)
openpilot/selfdrive/pandad/aol_wire.h
+39 -1
View File
@@ -12,6 +12,24 @@
// Inner bounded, flat Cap'n Proto payload for Event.aolSafetyWire @124 :Data.
// The outer Event validity remains a separate native status check.
struct AolPandaSlot {
uint16_t slot_index = 0;
std::string hardware_serial;
uint16_t safety_model = 0;
uint16_t safety_param = 0;
uint16_t alternative_experience = 0;
bool controls_allowed = false;
bool safety_rx_checks_invalid = false;
bool heartbeat_lost = false;
uint32_t faults = 0;
};
struct AolPandaStateSnapshot {
// Source Event BOOTTIME stamp: provenance only, not the wire's lease clock.
uint64_t source_mono_time = 0;
std::vector<AolPandaSlot> slots;
};
struct AolSafetyWireFields {
uint16_t protocol_version = 0;
bool compatible = false;
@@ -25,15 +43,20 @@ struct AolSafetyWireFields {
bool requested_longitudinal = false;
std::string panda_serial;
std::string axis_session_id;
AolPandaStateSnapshot inventory;
};
inline std::vector<uint8_t> encode_aol_safety_wire(const AolSafetyWireFields &fields) {
if (fields.panda_serial.size() > 96 || fields.axis_session_id.size() > 96 ||
fields.valid_until_mono_time < fields.observed_mono_time) return {};
if (fields.inventory.source_mono_time == 0U || fields.inventory.slots.empty() || fields.inventory.slots.size() > 2U) return {};
for (const auto &slot : fields.inventory.slots) {
if (slot.hardware_serial.empty() || slot.hardware_serial.size() > 96U) return {};
}
capnp::MallocMessageBuilder builder;
auto out = builder.initRoot<cereal::AolAxisState::SafetyWire>();
out.setKind(1U);
out.setVersion(1U);
out.setVersion(2U);
out.setProtocolVersion(fields.protocol_version);
out.setCompatible(fields.compatible);
out.setObservedMonoTime(fields.observed_mono_time);
@@ -46,6 +69,21 @@ inline std::vector<uint8_t> encode_aol_safety_wire(const AolSafetyWireFields &fi
out.setRequestedLongitudinal(fields.requested_longitudinal);
out.setPandaSerial(fields.panda_serial);
out.setAxisSessionId(fields.axis_session_id);
out.setSourcePandaStatesMonoTime(fields.inventory.source_mono_time);
auto inventory = out.initPandaInventory(fields.inventory.slots.size());
for (size_t i = 0; i < fields.inventory.slots.size(); ++i) {
const auto &slot = fields.inventory.slots[i];
auto entry = inventory[i];
entry.setSlotIndex(slot.slot_index);
entry.setHardwareSerial(slot.hardware_serial);
entry.setSafetyModel(slot.safety_model);
entry.setSafetyParam(slot.safety_param);
entry.setAlternativeExperience(slot.alternative_experience);
entry.setControlsAllowed(slot.controls_allowed);
entry.setSafetyRxChecksInvalid(slot.safety_rx_checks_invalid);
entry.setHeartbeatLost(slot.heartbeat_lost);
entry.setFaults(slot.faults);
}
auto flat = capnp::messageToFlatArray(builder);
auto bytes = flat.asBytes();
if (bytes.size() > 512) return {};
openpilot/selfdrive/pandad/main.cc
+3 -2
View File
@@ -16,7 +16,8 @@ int main(int argc, char *argv[]) {
assert(err == 0);
}
std::string serial = (argc > 1) ? argv[1] : "";
pandad_main_thread(serial);
std::vector<std::string> serials;
for (int i = 1; i < argc; ++i) serials.emplace_back(argv[i]);
pandad_main_thread(serials);
return 0;
}
openpilot/selfdrive/pandad/panda.cc
+4 -4
View File
@@ -14,7 +14,7 @@
const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr;
Panda::Panda(std::string serial) {
Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
handle = open_panda_handle(serial, [](const std::string &requested) {
auto spi = std::make_unique<PandaSpiHandle>(requested);
LOGW("connected to %s over SPI", requested.c_str());
@@ -206,7 +206,7 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
for (const auto &cmsg : can_data_list) {
// check if the message is intended for this panda
uint8_t bus = cmsg.getSrc();
if (bus >= PANDA_BUS_OFFSET) {
if (bus < bus_offset || bus >= bus_offset + PANDA_BUS_OFFSET) {
continue;
}
auto can_data = cmsg.getDat();
@@ -218,7 +218,7 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
header.addr = cmsg.getAddress();
header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
header.data_len_code = data_len_code;
header.bus = bus;
header.bus = bus - bus_offset;
header.checksum = 0;
memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
@@ -294,7 +294,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_fra
can_frame &canData = out_vec.emplace_back();
canData.address = header.addr;
canData.src = header.bus;
canData.src = header.bus + bus_offset;
if (header.rejected) {
canData.src += CAN_REJECTED_BUS_OFFSET;
}
openpilot/selfdrive/pandad/panda.h
+5 -4
View File
@@ -49,7 +49,8 @@ private:
std::unique_ptr<PandaCommsHandle> handle;
public:
Panda(std::string serial);
Panda(std::string serial, uint32_t bus_offset=0);
const uint32_t bus_offset;
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
@@ -93,9 +94,9 @@ protected:
uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
uint32_t receive_buffer_size = 0;
Panda() {}
Panda(std::unique_ptr<PandaCommsHandle> comms, cereal::PandaState::PandaType type)
: handle(std::move(comms)), hw_type(type) {}
Panda(uint32_t bus_offset=0) : bus_offset(bus_offset) {}
Panda(std::unique_ptr<PandaCommsHandle> comms, cereal::PandaState::PandaType type, uint32_t offset=0)
: handle(std::move(comms)), bus_offset(offset), hw_type(type) {}
void pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
std::function<void(uint8_t *, size_t)> write_func);
bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
openpilot/selfdrive/pandad/panda_safety.cc
+56 -8
View File
@@ -23,15 +23,17 @@ void PandaSafety::updateMultiplexingMode() {
// Initialize to ELM327 without OBD multiplexing for initial fingerprinting
if (!initialized_) {
prev_obd_multiplexing_ = false;
panda_->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
for (auto *panda : pandas_) panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
initialized_ = true;
}
// Switch between multiplexing modes based on the OBD multiplexing request
bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled");
if (obd_multiplexing_requested != prev_obd_multiplexing_) {
const uint16_t safety_param = obd_multiplexing_requested ? 0U : 1U;
panda_->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
for (size_t i = 0; i < pandas_.size(); ++i) {
const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
}
prev_obd_multiplexing_ = obd_multiplexing_requested;
params_.putBool("ObdMultiplexingChanged", true);
}
@@ -61,10 +63,56 @@ void PandaSafety::setSafetyMode(const std::string &params_string) {
auto safety_configs = car_params.getSafetyConfigs();
uint16_t alternative_experience = car_params.getAlternativeExperience();
cereal::CarParams::SafetyModel safety_model = safety_configs[0].getSafetyModel();
uint16_t safety_param = safety_configs[0].getSafetyParam();
alternative_experience_ = alternative_experience;
safety_config_count_ = safety_configs.size();
safety_slots_.clear();
for (size_t i = 0; i < pandas_.size(); ++i) {
auto model = cereal::CarParams::SafetyModel::SILENT;
uint16_t param = 0U;
if (i < safety_configs.size()) {
model = safety_configs[i].getSafetyModel();
param = safety_configs[i].getSafetyParam();
}
safety_slots_.emplace_back(model, param);
LOGW("Panda %zu: setting safety model: %d, param: %d, alternative experience: %d", i, (int)model, param, alternative_experience);
pandas_[i]->set_alternative_experience(alternative_experience);
pandas_[i]->set_safety_model(model, param);
}
}
LOGW("setting safety model: %d, param: %d, alternative experience: %d", (int)safety_model, safety_param, alternative_experience);
panda_->set_alternative_experience(alternative_experience);
panda_->set_safety_model(safety_model, safety_param);
Panda *PandaSafety::aolOwner() {
if (pandas_.size() == 1) return pandas_[0];
if (pandas_.size() != 2 || !safety_configured_ || safety_config_count_ != pandas_.size() ||
safety_slots_.size() != pandas_.size()) return nullptr;
if (pandas_[0]->hw_serial().empty() || pandas_[1]->hw_serial().empty() ||
pandas_[0]->hw_serial() == pandas_[1]->hw_serial()) return nullptr;
Panda *owner = nullptr;
for (size_t i = 0; i < pandas_.size(); ++i) {
auto *panda = pandas_[i];
if (!panda->connected() || !panda->comms_healthy()) return nullptr;
const auto health = panda->get_state();
if (!health || health->alternative_experience_pkt != alternative_experience_) return nullptr;
const auto [model, param] = safety_slots_[i];
const bool passive = model == cereal::CarParams::SafetyModel::NO_OUTPUT || model == cereal::CarParams::SafetyModel::SILENT;
if (passive) {
if (param != 0U || (health->safety_mode_pkt != (uint8_t)cereal::CarParams::SafetyModel::NO_OUTPUT &&
health->safety_mode_pkt != (uint8_t)cereal::CarParams::SafetyModel::SILENT) ||
health->safety_param_pkt != 0U || (health->flags_pkt & HEALTH_FLAG_CONTROLS_ALLOWED) != 0U) return nullptr;
} else {
if (owner != nullptr || health->safety_mode_pkt != (uint8_t)model || health->safety_param_pkt != param) return nullptr;
owner = panda;
}
}
return owner;
}
bool PandaSafety::aolStatusMatches(Panda *owner, const aol_safety_health_t &status) const {
if (pandas_.size() == 1) return owner == pandas_[0];
if (pandas_.size() != 2 || safety_config_count_ != pandas_.size() || safety_slots_.size() != pandas_.size()) return false;
for (size_t i = 0; i < pandas_.size(); ++i) {
if (pandas_[i] == owner) {
return status.safety_mode == (uint8_t)safety_slots_[i].first && status.safety_param == safety_slots_[i].second;
}
}
return false;
}
openpilot/selfdrive/pandad/pandad.cc
+206 -113
View File
@@ -9,6 +9,7 @@
#include "openpilot/starpilot/car/hyundai/aol_policy.h"
#include "starpilot/car/gm/aol_policy.h"
#include <algorithm>
#include <array>
#include <bitset>
#include <cassert>
@@ -33,18 +34,20 @@
ExitHandler do_exit;
bool check_connected(Panda *panda) {
if (!panda->connected()) {
do_exit = true;
return false;
bool check_all_connected(const std::vector<Panda *> &pandas) {
for (const auto& panda : pandas) {
if (!panda->connected()) {
do_exit = true;
return false;
}
}
return true;
}
Panda *connect(std::string serial) {
Panda *connect(std::string serial, uint32_t index) {
std::unique_ptr<Panda> panda;
try {
panda = std::make_unique<Panda>(serial);
panda = std::make_unique<Panda>(serial, index * PANDA_BUS_OFFSET);
} catch (std::exception &e) {
return nullptr;
}
@@ -66,7 +69,7 @@ Panda *connect(std::string serial) {
return panda.release();
}
void can_send_thread(Panda *panda, bool fake_send) {
void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
util::set_thread_name("pandad_can_send");
AlignedBuffer aligned_buf;
@@ -76,7 +79,7 @@ void can_send_thread(Panda *panda, bool fake_send) {
subscriber->setTimeout(100);
// run as fast as messages come in
while (!do_exit && check_connected(panda)) {
while (!do_exit && check_all_connected(pandas)) {
std::unique_ptr<Message> msg(subscriber->receive());
if (!msg) {
continue;
@@ -87,20 +90,26 @@ void can_send_thread(Panda *panda, bool fake_send) {
// Don't send if older than 1 second
if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str());
panda->can_send(event.getSendcan());
LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str());
for (const auto& panda : pandas) {
LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str());
panda->can_send(event.getSendcan());
LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str());
}
} else {
LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime());
}
}
}
void can_recv(Panda *panda, PubMaster *pm) {
void can_recv(std::vector<Panda *> &pandas, PubMaster *pm) {
static std::vector<can_frame> raw_can_data;
{
bool comms_healthy = true;
raw_can_data.clear();
bool comms_healthy = panda->can_receive(raw_can_data);
for (const auto& panda : pandas) {
comms_healthy &= panda->can_receive(raw_can_data);
}
MessageBuilder msg;
auto evt = msg.initEvent();
@@ -173,75 +182,104 @@ void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const
cs.setCanCoreResetCnt(can_health.can_core_reset_cnt);
}
std::optional<bool> send_panda_states(PubMaster *pm, Panda *panda, bool is_onroad, bool spoofing_started) {
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool is_onroad, bool spoofing_started,
AolPandaStateSnapshot *snapshot = nullptr) {
if (snapshot) *snapshot = {};
bool ignition_local = false;
std::vector<health_t> states;
std::vector<std::array<can_health_t, PANDA_CAN_CNT>> can_states;
// build msg
MessageBuilder msg;
auto evt = msg.initEvent();
auto pss = evt.initPandaStates(1);
auto pss = evt.initPandaStates(pandas.size());
auto health_opt = panda->get_state();
if (!health_opt) {
return std::nullopt;
}
health_t health = *health_opt;
std::array<can_health_t, PANDA_CAN_CNT> can_health{};
for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
auto can_health_opt = panda->get_can_state(i);
if (!can_health_opt) {
for (auto *panda : pandas) {
auto health_opt = panda->get_state();
if (!health_opt) {
return std::nullopt;
}
can_health[i] = *can_health_opt;
health_t health = *health_opt;
std::array<can_health_t, PANDA_CAN_CNT> can_health{};
for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
auto can_health_opt = panda->get_can_state(i);
if (!can_health_opt) {
return std::nullopt;
}
can_health[i] = *can_health_opt;
}
if (spoofing_started) {
health.flags_pkt |= HEALTH_FLAG_IGNITION_LINE;
}
if (pandas.size() == 2 && pandas[0]->hw_type == cereal::PandaState::PandaType::DOS &&
pandas[1]->hw_type == cereal::PandaState::PandaType::RED_PANDA && panda == pandas[0]) {
health.flags_pkt &= ~HEALTH_FLAG_IGNITION_LINE;
}
ignition_local |= (health.flags_pkt & (HEALTH_FLAG_IGNITION_LINE | HEALTH_FLAG_IGNITION_CAN)) != 0U;
states.push_back(health);
can_states.push_back(can_health);
}
if (spoofing_started) {
health.flags_pkt |= HEALTH_FLAG_IGNITION_LINE;
}
for (size_t i = 0; i < pandas.size(); ++i) {
auto *panda = pandas[i];
const auto &health = states[i];
const auto &can_health = can_states[i];
bool ignition_local = (health.flags_pkt & (HEALTH_FLAG_IGNITION_LINE | HEALTH_FLAG_IGNITION_CAN)) != 0U;
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
bool power_save_desired = !ignition_local;
if (((health.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U) != power_save_desired) {
panda->set_power_saving(power_save_desired);
}
bool power_save_desired = !ignition_local;
if (((health.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U) != power_save_desired) {
panda->set_power_saving(power_save_desired);
}
// set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect
bool should_close_relay = !ignition_local || !is_onroad;
if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
// set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect
bool should_close_relay = !ignition_local || !is_onroad;
if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
if (!panda->comms_healthy()) {
evt.setValid(false);
}
if (!panda->comms_healthy()) {
evt.setValid(false);
}
auto ps = pss[i];
fill_panda_state(ps, panda->hw_type, health);
auto ps = pss[0];
fill_panda_state(ps, panda->hw_type, health);
auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
fill_panda_can_state(cs[j], can_health[j]);
}
auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
fill_panda_can_state(cs[j], can_health[j]);
}
// Convert faults bitset to capnp list
std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
auto faults = ps.initFaults(fault_bits.count());
// Convert faults bitset to capnp list
std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
auto faults = ps.initFaults(fault_bits.count());
size_t j = 0;
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) {
if (fault_bits.test(f)) {
faults.set(j, cereal::PandaState::FaultType(f));
j++;
size_t j = 0;
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) {
if (fault_bits.test(f)) {
faults.set(j, cereal::PandaState::FaultType(f));
j++;
}
}
}
if (snapshot && evt.getValid()) {
snapshot->source_mono_time = evt.getLogMonoTime();
for (size_t i = 0; i < pandas.size(); ++i) {
const auto &health = states[i];
snapshot->slots.push_back({static_cast<uint16_t>(i), pandas[i]->hw_serial(), health.safety_mode_pkt,
health.safety_param_pkt, health.alternative_experience_pkt,
(health.flags_pkt & HEALTH_FLAG_CONTROLS_ALLOWED) != 0U,
(health.flags_pkt & HEALTH_FLAG_SAFETY_RX_CHECKS_INVALID) != 0U,
(health.flags_pkt & HEALTH_FLAG_HEARTBEAT_LOST) != 0U, health.faults_pkt});
}
}
pm->send("pandaStates", msg);
return ignition_local;
}
@@ -270,22 +308,48 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
pm->send("peripheralState", msg);
}
void process_panda_state(Panda *panda, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) {
auto ignition_opt = send_panda_states(pm, panda, is_onroad, spoofing_started);
if (!ignition_opt) {
LOGE("Failed to get ignition_opt");
return;
void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engaged, bool is_onroad,
bool spoofing_started, AolPandaStateSnapshot *snapshot = nullptr) {
if (snapshot) *snapshot = {};
std::vector<std::string> connected_serials;
for (Panda *p : pandas) {
connected_serials.push_back(p->hw_serial());
}
// check if we should have pandad reconnect
if (!ignition_opt.value()) {
if (!panda->comms_healthy()) {
LOGE("Reconnecting, communication to panda not healthy");
do_exit = true;
{
auto ignition_opt = send_panda_states(pm, pandas, is_onroad, spoofing_started, snapshot);
if (!ignition_opt) {
LOGE("Failed to get ignition_opt");
return;
}
// check if we should have pandad reconnect
if (!ignition_opt.value()) {
bool comms_healthy = true;
for (const auto &panda : pandas) {
comms_healthy &= panda->comms_healthy();
}
if (!comms_healthy) {
LOGE("Reconnecting, communication to pandas not healthy");
do_exit = true;
} else {
// check for new pandas
for (std::string &s : Panda::list()) {
if (!std::count(connected_serials.begin(), connected_serials.end(), s)) {
LOGW("Reconnecting to new panda: %s", s.c_str());
do_exit = true;
break;
}
}
}
}
for (const auto &panda : pandas) {
panda->send_heartbeat(engaged);
}
}
panda->send_heartbeat(engaged);
}
void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control, bool is_onroad) {
@@ -369,7 +433,7 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control,
}
}
void pandad_run(Panda *panda) {
void pandad_run(std::vector<Panda *> &pandas) {
const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr;
const bool spoofing_started = getenv("STARTED") != nullptr;
const bool fake_send = getenv("FAKESEND") != nullptr;
@@ -377,12 +441,14 @@ void pandad_run(Panda *panda) {
const bool aol_replay = aol_env != nullptr && strcmp(aol_env, "1") == 0;
// Start helper thread for event-driven sendcan.
std::thread send_thread(can_send_thread, panda, fake_send);
std::thread send_thread(can_send_thread, pandas, fake_send);
RateKeeper rk("pandad", 100);
SubMaster sm({"selfdriveState", "deviceState", "aolAxisState", "aolIntentWire"});
PubMaster pm({"can", "pandaStates", "peripheralState", "aolSafetyWire"});
PandaSafety panda_safety(panda);
PandaSafety panda_safety(pandas);
Panda *peripheral_panda = pandas[0];
Panda *previous_aol_owner = nullptr;
bool engaged = false;
bool is_onroad = false;
constexpr AolSafetyProfile aol_profiles[] = {HONDA_AOL_PROFILE, HONDA_STOCK_AOL_PROFILE, HONDA_NIDEC_AOL_PROFILE, HYUNDAI_AOL_PROFILE, HYUNDAI_CLASSIC_AOL_PROFILE, HYUNDAI_LEGACY_AOL_PROFILE, GM_AOL_PROFILE, FORD_AOL_PROFILE, MAZDA_AOL_PROFILE, TESLA_PREAP_AOL_PROFILE, TESLA_SCREEN_AOL_PROFILE, TOYOTA_AOL_PROFILE};
@@ -390,12 +456,12 @@ void pandad_run(Panda *panda) {
AolAxisNegotiator aol_negotiator(aol_registry);
// Main loop: receive CAN first, then process lower priority panda and peripheral state.
while (!do_exit && check_connected(panda)) {
can_recv(panda, &pm);
while (!do_exit && check_all_connected(pandas)) {
can_recv(pandas, &pm);
// Process peripheral state at 20 Hz
if (rk.frame() % 5 == 0) {
process_peripheral_state(panda, &pm, no_fan_control, is_onroad);
process_peripheral_state(peripheral_panda, &pm, no_fan_control, is_onroad);
}
// Process panda state at 10 Hz
@@ -406,7 +472,13 @@ void pandad_run(Panda *panda) {
// Keep the ordinary pandad/CAN BOOTTIME clock unchanged.
uint64_t now_ns = aol_monotonic_ns();
const bool axis_received = sm.allAliveAndValid({"aolAxisState"});
auto aol_status = (aol_replay || axis_received) ? panda->get_aol_safety_state() : std::nullopt;
Panda *panda = panda_safety.aolOwner();
if (panda != previous_aol_owner) {
aol_negotiator = AolAxisNegotiator(aol_registry);
previous_aol_owner = panda;
}
auto aol_status = (panda && (aol_replay || axis_received)) ? panda->get_aol_safety_state() : std::nullopt;
if (aol_status && !panda_safety.aolStatusMatches(panda, *aol_status)) aol_status.reset();
const bool aol_runtime = aol_runtime_enabled(aol_replay, aol_status, aol_registry);
if (sm.updated("deviceState")) {
is_onroad = sm["deviceState"].getDeviceState().getStarted();
@@ -430,12 +502,14 @@ void pandad_run(Panda *panda) {
}
const uint8_t aol_expected_mode = aol_status ? aol_status->safety_mode : 0U;
auto plan = aol_negotiator.prepare(aol_runtime ? aol_status : std::nullopt, axis_input, now_ns, aol_expected_mode);
bool aol_write_ok = plan.capable && panda->set_aol_axis_request(plan.request_mask);
bool aol_write_ok = panda && plan.capable && panda->set_aol_axis_request(plan.request_mask);
if (aol_write_ok) {
engaged = engaged || (plan.request_mask & 0x3U) != 0U || plan.retain_lateral_arm;
}
process_panda_state(panda, &pm, engaged, is_onroad, spoofing_started);
auto aol_after = aol_write_ok ? panda->get_aol_safety_state() : std::nullopt;
AolPandaStateSnapshot inventory;
process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started, &inventory);
auto aol_after = (aol_write_ok && panda_safety.aolOwner() == panda) ? panda->get_aol_safety_state() : std::nullopt;
if (aol_after && !panda_safety.aolStatusMatches(panda, *aol_after)) aol_after.reset();
auto outcome = aol_negotiator.complete(plan, aol_write_ok, aol_after);
if (aol_runtime) {
MessageBuilder axis_status_msg;
@@ -443,7 +517,7 @@ void pandad_run(Panda *panda) {
axis_status_event.setLogMonoTime(now_ns);
bool status_current = outcome.compatible;
auto reported_status = outcome.status;
const std::string serial = panda->hw_serial();
const std::string serial = panda ? panda->hw_serial() : "";
const AolSafetyWireFields fields = {
static_cast<uint16_t>(status_current ? AOL_SAFETY_PROTOCOL_VERSION : 0U), status_current,
now_ns, now_ns + 200000000ULL,
@@ -452,7 +526,7 @@ void pandad_run(Panda *panda) {
status_current && reported_status && ((reported_status->permission_mask & 0x1U) != 0U),
status_current && reported_status && ((reported_status->permission_mask & 0x2U) != 0U),
status_current && reported_status && ((reported_status->request_mask & 0x1U) != 0U),
status_current && reported_status && ((reported_status->request_mask & 0x2U) != 0U), serial, outcome.session,
status_current && reported_status && ((reported_status->request_mask & 0x2U) != 0U), serial, outcome.session, inventory,
};
auto payload = encode_aol_safety_wire(fields);
const bool wire_valid = status_current && !payload.empty();
@@ -469,57 +543,76 @@ void pandad_run(Panda *panda) {
// Send out peripheralState at 2Hz
if (rk.frame() % 50 == 0) {
send_peripheral_state(panda, &pm);
send_peripheral_state(peripheral_panda, &pm);
}
// Forward logs from panda to cloudlog if available
std::string log = panda->serial_read();
if (!log.empty()) {
if (log.find("Register 0x") != std::string::npos) {
// Log register divergent faults as errors
LOGE("%s", log.c_str());
} else {
LOGD("%s", log.c_str());
for (auto *panda : pandas) {
std::string log = panda->serial_read();
if (!log.empty()) {
if (log.find("Register 0x") != std::string::npos) {
// Log register divergent faults as errors
LOGE("%s", log.c_str());
} else {
LOGD("%s", log.c_str());
}
}
}
rk.keepTime();
}
// Close relay on exit to prevent a fault
if (is_onroad && !engaged) {
if (panda->connected()) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
for (auto *panda : pandas) {
if (panda->connected()) panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
}
send_thread.join();
}
void pandad_main_thread(std::string serial) {
if (serial.empty()) {
auto serials = Panda::list();
void pandad_main_thread(std::vector<std::string> serials) {
if (serials.size() == 0) {
serials = Panda::list();
if (serials.empty()) {
if (serials.size() == 0) {
LOGW("no pandas found, exiting");
return;
}
serial = serials[0];
if (serials.size() > 1) {
// CP slot order is owned by the Python wrapper's hardware/type/serial sort.
// Enumeration order alone cannot determine multi-Panda bus ownership.
LOGE("multiple pandas require ordered serial arguments from pandad wrapper");
return;
}
}
LOGW("connecting to panda: %s", serial.c_str());
std::string serials_str;
for (int i = 0; i < serials.size(); i++) {
serials_str += serials[i];
if (i < serials.size() - 1) serials_str += ", ";
}
LOGW("connecting to pandas: %s", serials_str.c_str());
Panda *panda = nullptr;
while (!do_exit) {
panda = connect(serial);
if (panda) break;
util::sleep_for(100);
// connect to all provided serials
std::vector<Panda *> pandas;
for (int i = 0; i < serials.size() && !do_exit; /**/) {
Panda *p = connect(serials[i], i);
if (!p) {
util::sleep_for(100);
continue;
}
pandas.push_back(p);
++i;
}
if (!do_exit) {
LOGW("connected to panda");
pandad_run(panda);
LOGW("connected to all pandas");
pandad_run(pandas);
}
delete panda;
for (Panda *panda : pandas) {
delete panda;
}
}
openpilot/selfdrive/pandad/pandad.h
+8 -3
View File
@@ -5,12 +5,14 @@
#include "common/params.h"
#include "selfdrive/pandad/panda.h"
void pandad_main_thread(std::string serial);
void pandad_main_thread(std::vector<std::string> serials);
class PandaSafety {
public:
PandaSafety(Panda *panda) : panda_(panda) {}
PandaSafety(const std::vector<Panda *> &pandas) : pandas_(pandas) {}
void configureSafetyMode(bool is_onroad);
Panda *aolOwner();
bool aolStatusMatches(Panda *owner, const aol_safety_health_t &status) const;
private:
void updateMultiplexingMode();
@@ -21,6 +23,9 @@ private:
bool log_once_ = false;
bool safety_configured_ = false;
bool prev_obd_multiplexing_ = false;
Panda *panda_;
std::vector<Panda *> pandas_;
std::vector<std::pair<cereal::CarParams::SafetyModel, uint16_t>> safety_slots_;
uint16_t alternative_experience_ = 0;
size_t safety_config_count_ = 0;
Params params_;
};
openpilot/selfdrive/pandad/pandad.py
+8 -3
View File
@@ -108,13 +108,18 @@ def main() -> None:
panda_serials = Panda.list()
if len(panda_serials):
assert len(panda_serials) == 1
cloudlog.info(f"{len(panda_serials)} panda found, connecting - {panda_serials}")
flash_panda(panda_serials[0])
for serial in panda_serials:
flash_panda(serial)
ordering = []
for serial in panda_serials:
with Panda(serial) as panda:
ordering.append((not panda.is_internal(), panda.get_type(), panda.get_usb_serial()))
panda_serials = [serial for _, _, serial in sorted(ordering)]
# run real pandad
os.environ['MANAGER_DAEMON'] = 'pandad'
process = subprocess.Popen(["./pandad", panda_serials[0]], cwd=os.path.join(BASEDIR, "openpilot/selfdrive/pandad"))
process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "openpilot/selfdrive/pandad"))
process.wait()
# TODO: wrap all panda exceptions in a base panda exception
except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
openpilot/selfdrive/pandad/tests/test_aol_wire.cc
+35 -10
View File
@@ -8,22 +8,35 @@
static constexpr uint64_t now = 1000000000ULL;
int main() {
const AolSafetyWireFields fields = {1, true, 100, 200, 5, 34, true, false, true, false, "panda", "axis"};
const AolSafetyWireFields fields = {1, true, 100, 200, 5, 34, true, false, true, false, "panda", "axis",
{now + 9000000000ULL, {{0, "panda", 5, 34, 0, true, false, false, 0}}}};
auto bytes = encode_aol_safety_wire(fields);
assert(!bytes.empty() && bytes.size() <= 512);
capnp::MallocMessageBuilder event_builder;
auto event = event_builder.initRoot<cereal::Event>();
event.setAolSafetyWire(kj::arrayPtr(bytes.data(), bytes.size()));
assert(event.isAolSafetyWire());
std::string hex;
char pair[3];
for (uint8_t value : bytes) {
std::snprintf(pair, sizeof(pair), "%02x", value);
hex += pair;
}
assert(hex == "00000000090000000000000004000200010b0100010005006400000000000000"
"c80000000000000022000000000000000500000032000000050000002a000000"
"70616e64610000006178697300000000");
auto aligned = kj::heapArray<capnp::word>(bytes.size() / sizeof(capnp::word));
std::memcpy(aligned.begin(), bytes.data(), bytes.size());
capnp::FlatArrayMessageReader current(aligned.asPtr());
auto wire = current.getRoot<cereal::AolAxisState::SafetyWire>();
assert(wire.getVersion() == 2U && wire.getPandaSerial() == "panda");
assert(wire.getSourcePandaStatesMonoTime() == now + 9000000000ULL);
assert(wire.getPandaInventory().size() == 1U);
assert(wire.getPandaInventory()[0].getHardwareSerial() == "panda");
assert(wire.getPandaInventory()[0].getSafetyParam() == 34U);
// Historic exact v1 bytes remain readable after adding the custom fields.
const uint64_t legacy_words[] = {
0x0000000900000000ULL, 0x0002000400000000ULL, 0x0005000100010b01ULL,
0x0000000000000064ULL, 0x00000000000000c8ULL, 0x0000000000000022ULL,
0x0000003200000005ULL, 0x0000002a00000005ULL, 0x00000061646e6170ULL,
0x0000000073697861ULL
};
capnp::FlatArrayMessageReader legacy(kj::arrayPtr(reinterpret_cast<const capnp::word *>(legacy_words),
sizeof(legacy_words) / sizeof(capnp::word)));
auto old_wire = legacy.getRoot<cereal::AolAxisState::SafetyWire>();
assert(old_wire.getVersion() == 1U && old_wire.getPandaSerial() == "panda");
assert(old_wire.getSourcePandaStatesMonoTime() == 0U && old_wire.getPandaInventory().size() == 0U);
for (uint8_t value : bytes) std::printf("%02x", value);
std::printf("\n");
auto bad = fields;
@@ -32,6 +45,18 @@ int main() {
bad = fields;
bad.valid_until_mono_time = 99;
assert(encode_aol_safety_wire(bad).empty());
bad = fields;
bad.inventory = {};
assert(encode_aol_safety_wire(bad).empty());
bad = fields;
bad.inventory.slots[0].hardware_serial = std::string(97, 'x');
assert(encode_aol_safety_wire(bad).empty());
bad = fields;
bad.inventory.slots.push_back({1, "auxiliary", 19, 0, 0, false, false, false, 0});
auto pair_bytes = encode_aol_safety_wire(bad);
assert(!pair_bytes.empty() && pair_bytes.size() <= 512U);
bad.inventory.slots.push_back({2, "third", 19, 0, 0, false, false, false, 0});
assert(encode_aol_safety_wire(bad).empty());
capnp::MallocMessageBuilder intent_builder;
auto intent = intent_builder.initRoot<cereal::AolAxisState::IntentWire>();
intent.setKind(2);
openpilot/selfdrive/pandad/tests/test_panda_owner.cc
+145
View File
@@ -0,0 +1,145 @@
#include <cstring>
#include <unistd.h>
#include <cstdlib>
#include <filesystem>
#include <memory>
#include <vector>
#include "common/tests/native_test.h"
#include "openpilot/cereal/messaging/messaging.h"
#include "selfdrive/pandad/pandad.h"
#include "selfdrive/pandad/aol_protocol.h"
class OwnerHandle : public PandaCommsHandle {
public:
health_t health{};
aol_safety_health_t aol{};
std::vector<uint16_t> requests;
explicit OwnerHandle(std::string serial) {
hw_serial = serial;
aol.magic = AOL_SAFETY_PROTOCOL_MAGIC;
aol.version = AOL_SAFETY_PROTOCOL_VERSION;
}
int control_write(uint8_t request, uint16_t p1, uint16_t p2, unsigned int) override {
if (request == 0xdc) {
health.safety_mode_pkt = p1; health.safety_param_pkt = p2;
aol.safety_mode = p1; aol.safety_param = p2;
} else if (request == 0xdf) {
health.alternative_experience_pkt = p1;
} else if (request == 0xf5) {
requests.push_back(p1); aol.request_mask = p1;
}
return 0;
}
int control_read(uint8_t request, uint16_t, uint16_t, unsigned char *data, uint16_t length, unsigned int) override {
if (request == 0xd2 && length == sizeof(health)) { memcpy(data, &health, length); return length; }
if (request == 0xd5 && length == sizeof(aol)) { memcpy(data, &aol, length); return length; }
return -1;
}
int bulk_write(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
int bulk_read(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
void cleanup() override {}
};
class OwnerPanda : public Panda {
public:
OwnerPanda(std::unique_ptr<OwnerHandle> h, uint32_t offset) : Panda(std::move(h), cereal::PandaState::PandaType::RED_PANDA, offset) {}
};
void test_actual_config_slots_and_owner_handle() {
using Mode = cereal::CarParams::SafetyModel;
const std::string prefix = "test-panda-owner-" + std::to_string(getpid());
const char *old = getenv("OPENPILOT_PREFIX");
const std::string old_prefix = old ? old : "";
setenv("OPENPILOT_PREFIX", prefix.c_str(), 1);
Params params;
struct Restore {
Params &params;
std::string old;
~Restore() {
for (auto key : {"CarParams", "FirmwareQueryDone", "ControlsReady", "ObdMultiplexingChanged"}) params.remove(key);
if (old.empty()) unsetenv("OPENPILOT_PREFIX"); else setenv("OPENPILOT_PREFIX", old.c_str(), 1);
}
} restore{params, old_prefix};
auto aux_handle = std::make_unique<OwnerHandle>("auxiliary");
auto owner_handle = std::make_unique<OwnerHandle>("owner");
auto extra_handle = std::make_unique<OwnerHandle>("extra");
auto *aux = aux_handle.get(); auto *owner = owner_handle.get(); auto *extra = extra_handle.get();
OwnerPanda auxiliary(std::move(aux_handle), 0), primary(std::move(owner_handle), 4), excess(std::move(extra_handle), 8);
std::vector<Panda *> pandas{&auxiliary, &primary, &excess};
PandaSafety safety(pandas);
CHECK(safety.aolOwner() == nullptr);
capnp::MallocMessageBuilder message;
auto cp = message.initRoot<cereal::CarParams>();
cp.setAlternativeExperience(32);
auto slots = cp.initSafetyConfigs(2);
slots[0].setSafetyModel(Mode::NO_OUTPUT); slots[0].setSafetyParam(0);
slots[1].setSafetyModel(Mode::HYUNDAI_CANFD); slots[1].setSafetyParam(0x809);
auto words = capnp::messageToFlatArray(message);
auto bytes = words.asBytes();
params.put("CarParams", reinterpret_cast<const char *>(bytes.begin()), bytes.size());
params.putBool("FirmwareQueryDone", true); params.putBool("ControlsReady", true);
safety.configureSafetyMode(true);
CHECK(aux->health.safety_mode_pkt == (uint8_t)Mode::NO_OUTPUT);
CHECK(owner->health.safety_mode_pkt == (uint8_t)Mode::HYUNDAI_CANFD);
CHECK(owner->health.safety_param_pkt == 0x809);
CHECK(extra->health.safety_mode_pkt == (uint8_t)Mode::SILENT);
CHECK(aux->health.alternative_experience_pkt == 32 && owner->health.alternative_experience_pkt == 32);
CHECK(safety.aolOwner() == nullptr);
std::vector<Panda *> exact{&auxiliary, &primary};
PandaSafety owner_safety(exact); owner_safety.configureSafetyMode(true);
CHECK(owner_safety.aolOwner() == &primary);
CHECK(owner_safety.aolStatusMatches(&primary, owner->aol));
auto foreign = owner->aol; foreign.safety_param ^= 1;
CHECK(!owner_safety.aolStatusMatches(&primary, foreign));
CHECK(!owner_safety.aolStatusMatches(&auxiliary, owner->aol));
aux->hw_serial = "owner"; CHECK(owner_safety.aolOwner() == nullptr); aux->hw_serial = "auxiliary";
for (auto *peer : {aux}) {
peer->health.flags_pkt |= HEALTH_FLAG_CONTROLS_ALLOWED;
CHECK(owner_safety.aolOwner() == nullptr);
peer->health.flags_pkt = 0;
peer->health.safety_param_pkt = 0x809;
CHECK(owner_safety.aolOwner() == nullptr);
peer->health.safety_param_pkt = 0;
peer->comms_healthy = false;
CHECK(owner_safety.aolOwner() == nullptr);
peer->comms_healthy = true;
peer->connected = false;
CHECK(owner_safety.aolOwner() == nullptr);
peer->connected = true;
}
owner->health.safety_param_pkt = 0x808; CHECK(owner_safety.aolOwner() == nullptr);
owner->health.safety_param_pkt = 0x809;
owner->health.alternative_experience_pkt = 0; CHECK(owner_safety.aolOwner() == nullptr);
owner->health.alternative_experience_pkt = 32;
slots[0].setSafetyParam(1);
{ auto changed = capnp::messageToFlatArray(message); auto data = changed.asBytes();
params.put("CarParams", reinterpret_cast<const char *>(data.begin()), data.size()); }
owner_safety.configureSafetyMode(false); owner_safety.configureSafetyMode(true);
aux->health.safety_param_pkt = 0;
CHECK(owner_safety.aolOwner() == nullptr);
slots[0].setSafetyParam(0);
{ auto changed = capnp::messageToFlatArray(message); auto data = changed.asBytes();
params.put("CarParams", reinterpret_cast<const char *>(data.begin()), data.size()); }
owner_safety.configureSafetyMode(false); owner_safety.configureSafetyMode(true);
CHECK(owner_safety.aolOwner() == &primary);
const AolSafetyProfile profiles[] = {{(uint8_t)Mode::HYUNDAI_CANFD, [](uint16_t p) { return p == 0x809; }, true}};
AolProfileRegistry registry{profiles, 1}; AolAxisNegotiator negotiator(registry);
owner->aol.capability_flags = 1;
aux->aol = owner->aol; aux->aol.permission_mask = 3; aux->aol.request_mask = 3;
AolAxisInput axis{true, true, "session", 1, 200000001, 1, true, false};
for (int i = 0; i < 2; ++i) {
Panda *selected = owner_safety.aolOwner(); CHECK(selected == &primary);
auto before = selected->get_aol_safety_state();
auto plan = negotiator.prepare(before, axis, 2, before->safety_mode);
CHECK(plan.request_mask == (i == 0 ? 0 : 1));
CHECK(selected->set_aol_axis_request(plan.request_mask));
auto outcome = negotiator.complete(plan, true, selected->get_aol_safety_state());
CHECK(outcome.compatible); CHECK(outcome.status->permission_mask == 0);
CHECK(selected->hw_serial() == "owner");
}
CHECK(aux->requests.empty() && extra->requests.empty());
CHECK(owner->requests == std::vector<uint16_t>({0, 1}));
owner_safety.configureSafetyMode(false); CHECK(owner_safety.aolOwner() == nullptr);
PandaSafety single({&primary}); CHECK(single.aolOwner() == &primary);
}
int main() { return run_native_test(test_actual_config_slots_and_owner_handle); }
openpilot/selfdrive/pandad/tests/test_pandad_canprotocol.cc
+66 -6
View File
@@ -1,4 +1,5 @@
#include <climits>
#include <array>
#include <filesystem>
#include <fstream>
#include <cstdlib>
@@ -8,7 +9,7 @@
#include "selfdrive/pandad/panda.h"
struct PandaTest : public Panda {
PandaTest(int can_list_size, cereal::PandaState::PandaType hw_type);
PandaTest(int can_list_size, cereal::PandaState::PandaType hw_type, uint32_t offset=0);
void test_can_send();
void test_can_recv(uint32_t chunk_size = 0);
void test_chunked_can_recv();
@@ -20,7 +21,7 @@ struct PandaTest : public Panda {
capnp::List<cereal::CanData>::Reader can_data_list;
};
PandaTest::PandaTest(int can_list_size_, cereal::PandaState::PandaType hw_type_) : can_list_size(can_list_size_), Panda() {
PandaTest::PandaTest(int can_list_size_, cereal::PandaState::PandaType hw_type_, uint32_t offset) : can_list_size(can_list_size_), Panda(offset) {
this->hw_type = hw_type_;
int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 9);
// prepare test data
@@ -38,7 +39,7 @@ PandaTest::PandaTest(int can_list_size_, cereal::PandaState::PandaType hw_type_)
uint32_t id = i % data_limit;
const std::string &dat = test_data[dlc_to_len[id]];
can.setAddress(i);
can.setSrc(i % 3);
can.setSrc(i % 3 + bus_offset);
can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
total_pakets_size += sizeof(can_header) + dat.size();
}
@@ -96,6 +97,7 @@ void PandaTest::test_can_recv(uint32_t rx_chunk_size) {
CHECK(frames.size() == can_list_size);
for (int i = 0; i < frames.size(); ++i) {
CHECK(frames[i].address == i);
CHECK(frames[i].src == i % 3 + bus_offset);
CHECK(test_data.find(frames[i].dat.size()) != test_data.end());
const std::string &dat = test_data[frames[i].dat.size()];
CHECK(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0);
@@ -155,22 +157,80 @@ void test_firmware_selection() {
CHECK(!FirmwarePanda(Type::DOS, 0x11).up_to_date());
}
class RoutingPanda : public Panda {
public:
explicit RoutingPanda(uint32_t offset) : Panda(offset) {}
using Panda::pack_can_buffer;
using Panda::unpack_can_buffer;
using Panda::calculate_checksum;
};
void test_disjoint_bus_blocks_and_return_flags() {
MessageBuilder message;
auto list = message.initEvent().initSendcan(12);
for (uint32_t bus = 0; bus < 12; ++bus) {
list[bus].setAddress(0x100 + bus);
list[bus].setSrc(bus);
const std::array<uint8_t, 8> data{(uint8_t)bus, 1, 2, 3, 4, 5, 6, 7};
list[bus].setDat(kj::arrayPtr(data.data(), data.size()));
}
for (uint32_t offset : {0U, 4U, 8U}) {
RoutingPanda panda(offset);
std::vector<uint8_t> bytes;
panda.pack_can_buffer(list.asReader(), [&](uint8_t *data, size_t size) { bytes.insert(bytes.end(), data, data + size); });
const size_t packet_size = sizeof(can_header) + 8;
CHECK(bytes.size() == 4 * packet_size);
for (uint32_t local = 0; local < 4; ++local) {
can_header header;
memcpy(&header, bytes.data() + local * packet_size, sizeof(header));
CHECK(header.bus == local && header.addr == 0x100 + offset + local);
}
for (uint32_t flags : {0U, CAN_RETURNED_BUS_OFFSET, CAN_REJECTED_BUS_OFFSET}) {
auto encoded = bytes;
for (uint32_t local = 0; local < 4; ++local) {
can_header header;
auto *packet = encoded.data() + local * packet_size;
memcpy(&header, packet, sizeof(header));
header.returned = flags == CAN_RETURNED_BUS_OFFSET;
header.rejected = flags == CAN_REJECTED_BUS_OFFSET;
header.checksum = 0;
memcpy(packet, &header, sizeof(header));
header.checksum = panda.calculate_checksum(packet, packet_size);
memcpy(packet, &header, sizeof(header));
}
uint32_t size = encoded.size(); std::vector<can_frame> frames;
CHECK(panda.unpack_can_buffer(encoded.data(), size, frames));
CHECK(frames.size() == 4);
for (uint32_t local = 0; local < 4; ++local) {
CHECK(frames[local].src == offset + local + flags);
CHECK(frames[local].address == 0x100 + offset + local);
CHECK((uint8_t)frames[local].dat[0] == offset + local);
}
}
}
}
void test_can_protocol() {
test_disjoint_bus_blocks_and_return_flags();
test_firmware_selection();
for (auto hw_type : {cereal::PandaState::PandaType::DOS, cereal::PandaState::PandaType::RED_PANDA}) {
for (uint32_t offset : {0U, 4U, 8U}) {
for (int can_list_size : {1, 3, 5, 9, 10, 18, 19, 20, 30, 60, 100, 200}) {
PandaTest send_test(can_list_size, hw_type);
PandaTest send_test(can_list_size, hw_type, offset);
send_test.test_can_send();
PandaTest receive_test(can_list_size, hw_type);
PandaTest receive_test(can_list_size, hw_type, offset);
receive_test.test_can_recv();
PandaTest chunked_receive_test(can_list_size, hw_type);
PandaTest chunked_receive_test(can_list_size, hw_type, offset);
for (uint32_t chunk_size : {1U, 63U, 64U, 65U}) chunked_receive_test.test_can_recv(chunk_size);
}
}
}
}
int main() {
return run_native_test(test_can_protocol);
}
openpilot/starpilot/aol/runtime.py
+41
View File
@@ -69,6 +69,42 @@ def current_intent(sm, *, car_state_ns: int, now_ns: int, previous: IntentState
return intent
def native_inventory_matches_cp(state, CP) -> bool:
"""Bind the selected serial to the ordered same-read configuration inventory.
Historical v1 wires retain single-Panda compatibility only; they carry no
inventory qualification. Multi-Panda admission requires coherent production v2
with the exact configuration and one owner.
The source PandaStates stamp is BOOTTIME provenance, never a monotonic lease.
"""
if not state.pandaInventory:
return state.sourcePandaStatesMonoTime == 0 and len(CP.safetyConfigs) == 1
try:
slots = state.pandaInventory
configs = CP.safetyConfigs
if state.sourcePandaStatesMonoTime <= 0 or not 1 <= len(slots) <= 2 or len(slots) != len(configs):
return False
serials = [slot.hardwareSerial for slot in slots]
if any(not serial for serial in serials) or len(set(serials)) != len(serials):
return False
owners = []
for index, (slot, config) in enumerate(zip(slots, configs, strict=True)):
if slot.safetyRxChecksInvalid or slot.heartbeatLost or slot.faults:
return False
if (slot.slotIndex != index or slot.safetyModel != int(config.safetyModel.raw) or
slot.safetyParam != int(config.safetyParam) or slot.alternativeExperience != int(CP.alternativeExperience)):
return False
if str(config.safetyModel) in ('silent', 'noOutput'):
if slot.safetyParam != 0 or slot.controlsAllowed:
return False
else:
owners.append(slot)
return bool(len(owners) == 1 and owners[0].hardwareSerial == state.pandaSerial and
owners[0].safetyModel == state.safetyModel and owners[0].safetyParam == state.safetyParam)
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
return False
def current_native(sm, CP, *, now_ns: int, axis_session_id: str | None = None):
if not (sm.valid[SAFETY_SERVICE] and sm.alive[SAFETY_SERVICE] and sm.seen[SAFETY_SERVICE]):
return None
@@ -76,12 +112,17 @@ def current_native(sm, CP, *, now_ns: int, axis_session_id: str | None = None):
if state is None:
return None
if (not native_matches_cp(CP, int(state.safetyModel), int(state.safetyParam)) or
not native_inventory_matches_cp(state, CP) or
not state.compatible or int(state.protocolVersion) != 1 or not state.pandaSerial or not state.axisSessionId or
(axis_session_id is not None and str(state.axisSessionId) != axis_session_id) or
int(sm.logMonoTime[SAFETY_SERVICE]) > now_ns or
now_ns - int(sm.logMonoTime[SAFETY_SERVICE]) > SAFETY_MAX_AGE_NS or
int(state.observedMonoTime) > now_ns or now_ns > int(state.validUntilMonoTime)):
return None
if state.pandaInventory and (int(sm.logMonoTime[SAFETY_SERVICE]) != state.observedMonoTime or
state.observedMonoTime <= 0 or
state.validUntilMonoTime - state.observedMonoTime > SAFETY_MAX_AGE_NS):
return None
return state
openpilot/starpilot/aol/tests/test_panda_inventory.py
+498
View File
@@ -0,0 +1,498 @@
"""Inventory consumers use coherent producer health and preserve axis guards.
Synthetic IPC exercises actual SelfdriveD, Controls and Card callers. Physical
USB and production main enumeration/reconnect remain separate qualifications.
"""
from dataclasses import replace
from unittest import mock
import pytest
from openpilot.cereal import custom, log, messaging
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.car.tests.test_hyundai_aol import candidate
from openpilot.starpilot.aol.runtime import current_native, native_inventory_matches_cp
from openpilot.starpilot.aol.transport_pause import healthy_transport
from openpilot.starpilot.aol.vehicle import native_matches_cp, policy_for
from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, decode_safety, encode_safety
from openpilot.starpilot.aol.tests import test_runtime as runtime_tests
from openpilot.starpilot.aol.tests.test_runtime import car_state
NOW = 10_000_000_000
BOOT_OFFSET = 9_000_000_000
def configured_cp():
_, cp = candidate(True)
cp.safetyConfigs[-1].safetyParam |= 0x0800
policy = policy_for(cp)
cp.safetyConfigs[-1].safetyParam |= policy.safety_param_addition
cp.alternativeExperience |= policy.alternative_experience_addition
assert native_matches_cp(cp, cp.safetyConfigs[-1].safetyModel.raw, cp.safetyConfigs[-1].safetyParam)
return cp
def bound_state(cp, *, now=NOW, serial='authority-panda', session='authority-session'):
slots = tuple(PandaSlot(i, serial if i == len(cp.safetyConfigs) - 1 else 'auxiliary-panda',
int(config.safetyModel.raw), int(config.safetyParam), int(cp.alternativeExperience),
str(config.safetyModel) not in ('silent', 'noOutput'))
for i, config in enumerate(cp.safetyConfigs))
return SafetyState(1, True, now, now + 200_000_000,
slots[-1].safetyModel, slots[-1].safetyParam, True, True, True, True,
serial, session, now + BOOT_OFFSET, slots)
def sm_for(cp, state, *, stamp=None, panda_stamp=None):
services = ['aolSafetyWire', 'pandaStates']
sm = messaging.SubMaster(services, ignore_avg_freq=services)
native = messaging.new_message('aolSafetyWire', 0, valid=True,
logMonoTime=state.observedMonoTime if stamp is None else stamp)
native.aolSafetyWire = encode_safety(state)
pandas = messaging.new_message('pandaStates', len(cp.safetyConfigs), valid=True,
logMonoTime=state.sourcePandaStatesMonoTime if panda_stamp is None else panda_stamp)
for panda, config in zip(pandas.pandaStates, cp.safetyConfigs, strict=True):
panda.safetyModel, panda.safetyParam = config.safetyModel, config.safetyParam
panda.alternativeExperience = cp.alternativeExperience
panda.controlsAllowed = str(config.safetyModel) not in ('silent', 'noOutput')
sm.update_msgs((NOW - 1_000_000) / 1e9, [native.as_reader(), pandas.as_reader()])
return sm
@pytest.mark.parametrize('mutation', ['serial', 'slot', 'model', 'param', 'ae', 'empty_serial',
'rx_invalid', 'heartbeat', 'fault'])
def test_present_inventory_rejects_borrowed_or_malformed_owner(mutation):
cp = configured_cp()
state = bound_state(cp)
assert current_native(sm_for(cp, state), cp, now_ns=NOW, axis_session_id=state.axisSessionId) == state
changes = {'slot': {'slotIndex': 1}, 'model': {'safetyModel': 19}, 'param': {'safetyParam': 0},
'ae': {'alternativeExperience': cp.alternativeExperience ^ 1}, 'empty_serial': {'hardwareSerial': ''},
'rx_invalid': {'safetyRxChecksInvalid': True}, 'heartbeat': {'heartbeatLost': True}, 'fault': {'faults': 1}}
bad = replace(state, pandaSerial='borrowed-same-profile') if mutation == 'serial' else replace(
state, pandaInventory=(replace(state.pandaInventory[0], **changes[mutation]),))
assert current_native(sm_for(cp, bad), cp, now_ns=NOW, axis_session_id=state.axisSessionId) is None
@pytest.mark.parametrize('boundary', ['at_200ms', 'past_200ms', 'future', 'wrong_session', 'wrong_event', 'extended_lease'])
def test_inventory_uses_original_monotonic_lease(boundary):
cp = configured_cp()
state = bound_state(cp)
now = NOW + (200_000_000 if boundary == 'at_200ms' else 200_000_001 if boundary == 'past_200ms' else 0)
if boundary == 'future':
state = bound_state(cp, now=NOW + 1)
if boundary == 'extended_lease':
state = replace(state, validUntilMonoTime=NOW + 200_000_001)
sm = sm_for(cp, state, stamp=NOW - 1 if boundary == 'wrong_event' else None)
native = current_native(sm, cp, now_ns=now,
axis_session_id='foreign-session' if boundary == 'wrong_session' else state.axisSessionId)
assert (native is not None) == (boundary == 'at_200ms')
@pytest.mark.parametrize('arrival', ['older', 'newer', 'boot_offset', 'latest_fault', 'latest_invalid'])
def test_coherent_health_avoids_independently_conflated_stamp_race(arrival):
cp = configured_cp()
state = bound_state(cp)
stamp = NOW - 100_000_000 if arrival == 'older' else NOW + 1 if arrival == 'newer' else NOW + BOOT_OFFSET
sm = sm_for(cp, state, panda_stamp=stamp)
if arrival == 'latest_fault':
message = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=stamp)
message.pandaStates[0] = sm['pandaStates'][0]
message.pandaStates[0].heartbeatLost = True
sm.update_msgs(NOW / 1e9, [message.as_reader()])
elif arrival == 'latest_invalid':
sm.valid['pandaStates'] = False
healthy = healthy_transport(sm, cp, car_state(), state.axisSessionId, NOW)
assert healthy == (arrival not in ('latest_fault', 'latest_invalid'))
def test_legacy_and_strict_version_rollout():
cp = configured_cp()
state = bound_state(cp)
legacy = replace(state, sourcePandaStatesMonoTime=0, pandaInventory=())
assert decode_safety(encode_safety(legacy)) == legacy
assert current_native(sm_for(cp, legacy, panda_stamp=NOW), cp, now_ns=NOW) == legacy
assert not legacy.pandaInventory # Historical compatibility is not inventory qualification.
for version in (1, 2):
wire = custom.AolAxisState.SafetyWire.new_message(kind=1, version=version)
if version == 1:
wire.sourcePandaStatesMonoTime = NOW
assert decode_safety(wire.to_bytes()) is None
with pytest.raises(ValueError):
encode_safety(replace(state, sourcePandaStatesMonoTime=0))
def test_two_slot_identity_does_not_bypass_existing_port_admission():
cp = configured_cp()
original = cp.safetyConfigs[0].to_dict()
cp.safetyConfigs = [{'safetyModel': 'noOutput', 'safetyParam': 0}, original]
state = bound_state(cp)
assert native_inventory_matches_cp(state, cp)
assert not native_matches_cp(cp, state.safetyModel, state.safetyParam)
assert current_native(sm_for(cp, state), cp, now_ns=NOW) is None
for slots in ((state.pandaInventory[1], state.pandaInventory[0]),
(replace(state.pandaInventory[0], hardwareSerial=state.pandaSerial), state.pandaInventory[1]),
(replace(state.pandaInventory[0], controlsAllowed=True), state.pandaInventory[1]),
(state.pandaInventory[1],)):
assert not native_inventory_matches_cp(replace(state, pandaInventory=slots), cp)
@pytest.mark.parametrize('failure', ['none', 'serial', 'ae', 'heartbeat', 'critical', 'stale_intent', 'transport_pause'])
def test_actual_sd_controls_card_inventory_callers(failure):
case = runtime_tests.IpcAxisContractTests(methodName='runTest')
try:
with OpenpilotPrefix():
sd, controls, step, card, feedback = case._transport_callers()
real_encode = encode_safety
corrupt = [False]
def production_shaped_wire(value):
state = bound_state(sd.CP, now=value.observedMonoTime, serial=value.pandaSerial, session=value.axisSessionId)
state = replace(state, lateralAllowed=value.lateralAllowed, longitudinalAllowed=value.longitudinalAllowed,
requestedLateral=value.requestedLateral, requestedLongitudinal=value.requestedLongitudinal)
if corrupt[0] and failure == 'serial':
state = replace(state, pandaSerial='borrowed-same-profile')
if corrupt[0] and failure in ('ae', 'heartbeat'):
slot = replace(state.pandaInventory[0], **({'alternativeExperience': sd.CP.alternativeExperience ^ 1}
if failure == 'ae' else {'heartbeatLost': True}))
state = replace(state, pandaInventory=(slot,))
return real_encode(state)
with mock.patch('openpilot.starpilot.aol.tests.test_runtime.encode_safety', production_shaped_wire):
axis, _, command = step(0)
assert axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive
assert not feedback(NOW, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
assert card.aol_card_intent.allowed_latch
corrupt[0] = True
axis, _, command = step(8 if failure == 'transport_pause' else 4 if failure == 'stale_intent' else 1,
old_intent=failure == 'stale_intent', timeout=failure == 'transport_pause',
critical_event=log.OnroadEvent.EventName.canError if failure == 'critical' else None)
now = int(sd.pm.messages['aolAxisState'].logMonoTime)
fault = feedback(now, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
if failure == 'none':
assert axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive
assert not fault and card.aol_card_intent.allowed_latch
elif failure == 'transport_pause':
assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
assert not fault and card.aol_card_intent.allowed_latch
assert str(axis.faultReason) == 'transportPause'
else:
assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
assert fault and not card.aol_card_intent.allowed_latch
assert str(axis.faultReason) == 'critical'
finally:
case.doCleanups()
class _CancelObservationReady(Exception):
pass
class _Ev6CancelCaller:
"""Real parser/Card receipt and physical intent, with synthetic native IPC.
Halt state_update after its real CAN observation; SLC/radar are unrelated to
this regression. The joined production actor separately proves native TX.
"""
def __init__(self):
import time
from types import SimpleNamespace
from opendbc.car.hyundai.interface import CarInterface
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car.card import Car
from openpilot.starpilot.aol.intent import AolSettings, AolProcessFaultContext, AOL_TOGGLE
from openpilot.starpilot.aol.tests.test_ev6_physical_authorization import ev6_params
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
from openpilot.starpilot.car.hyundai.ev6_intent import Ev6CardIntent
self.clock = time
self.cp = ev6_params()
assert self.cp.safetyConfigs[-1].safetyParam == 0x15
self.cp.safetyConfigs[-1].safetyParam = 0x815
self.cp.alternativeExperience = 32
self.card = Car.__new__(Car)
self.card.CP, self.card.CI = self.cp, CarInterface(self.cp)
self.card.CI.update([])
self.packers = {bus: CANPacker(parser.dbc.name) for bus, parser in self.card.CI.can_parsers.items()}
self.counters = {}
self.drive = next(value for value, name in self.card.CI.CS.shifter_values.items() if name == 'D')
self.card.RI = SimpleNamespace(update=lambda _: None)
self.card.can_sock = object()
self.card.timing_mark = lambda _: None
self.card.observe_ioniq6_long_authority = lambda *_: None
self.card.update_vehicle_state_context = lambda _: None
self.card.can_rcv_cum_timeout_counter = 0
services = ['aolAxisState', 'aolSafetyWire', 'pandaStates', 'onroadEvents']
self.card.sm = messaging.SubMaster(services, ignore_avg_freq=services)
self.card.aol_transport_feedback = TransportPauseFeedback(self.cp)
self.card.aol_process_fault_context = AolProcessFaultContext()
self.card.aol_card_intent = Ev6CardIntent(AolSettings(True, 0., AOL_TOGGLE, 0, (0, 0, 0), (0, 0, 0)))
self.card.refresh_slc_configuration = self._stop_after_observation
self.sequence = 0
self.last_now = 0
@staticmethod
def _stop_after_observation(_):
raise _CancelObservationReady
def parse(self, button=0, *, stamp_delta=0, lkas=0):
common = {'GEAR': self.drive, 'DRIVER_SEATBELT': 1, 'DriverBraking': 0, 'ACCEnable': 0, 'ACC_REQ': 0,
'WHL_SpdFLVal': 45., 'WHL_SpdFRVal': 45., 'WHL_SpdRLVal': 45., 'WHL_SpdRRVal': 45.,
'ACCMode': 0, 'MainMode_ACC': 1, 'VSetDis': 60, 'DISTANCE_UNIT': 0,
'MDPS_StrTqSnsrVal': 0, 'MDPS_OutTqVal': 0, 'MDPS_LkaFailSta': 0}
frames = []
for bus, parser in self.card.CI.can_parsers.items():
for address in sorted(parser.addresses):
message = parser.dbc.addr_to_msg[address]
values = {name: value for name, value in common.items() if name in message.sigs}
if message.name == 'CRUISE_BUTTONS':
values.update(ADAPTIVE_CRUISE_MAIN_BTN=0, LDA_BTN=lkas, CRUISE_BUTTONS=button)
key = (parser.bus, message.name)
counter = self.counters.get(key, 0)
self.counters[key] = counter + 1
if 'COUNTER' in message.sigs:
values['COUNTER'] = counter % (16 if message.name == 'CRUISE_BUTTONS' else 256)
frames.append(self.packers[bus].make_can_msg(message.name, parser.bus, values))
can = messaging.new_message('can', len(frames), valid=True,
logMonoTime=self.clock.clock_gettime_ns(self.clock.CLOCK_BOOTTIME) + stamp_delta)
for target, (address, data, bus) in zip(can.can, frames, strict=True):
target.address, target.dat, target.src = address, data, bus
with mock.patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[can.to_bytes()]):
with pytest.raises(_CancelObservationReady):
self.card.state_update()
self.cs = self.card._aol_can_observation[0]
self.last_now = self.clock.monotonic_ns()
assert self.cs.canValid and not self.cs.canTimeout
return self.cs
def intent(self, *, fault=False, active=False, now=None):
now = self.last_now if now is None else now
owner = self.card.aol_card_intent
owner.observe_physical_samples(self.card.CI.CS, now_ns=now)
owner.update(self.cs, now_ns=now, fault_active=fault, standard_active=active, standard_enabled=active)
return owner
def arm(self):
from opendbc.car.hyundai.values import Buttons
self.parse(Buttons.NONE)
assert not self.intent().allowed_latch
self.parse(Buttons.SET_DECEL)
assert not self.intent().allowed_latch
self.parse(Buttons.NONE)
assert not self.intent().allowed_latch
assert self.intent(active=True).allowed_latch
assert self.card.aol_card_intent.physical.authorized
def evidence(self, *, partial=False, lat_only=False, now=None, mutation=None, event=None):
now = self.clock.monotonic_ns() if now is None else now
self.last_now = now
self.sequence += 1
state = bound_state(self.cp, now=now, session='ev6-cancel-session')
state = replace(state, requestedLongitudinal=not lat_only, longitudinalAllowed=not partial and not lat_only,
sourcePandaStatesMonoTime=self.clock.clock_gettime_ns(self.clock.CLOCK_BOOTTIME),
pandaInventory=tuple(replace(p, controlsAllowed=not partial and not lat_only) for p in state.pandaInventory))
if mutation == 'owner':
state = replace(state, pandaSerial='replacement',
pandaInventory=tuple(replace(p, hardwareSerial='replacement') for p in state.pandaInventory))
if mutation in ('ae', 'rx', 'heartbeat', 'native_fault'):
changes = {'ae': {'alternativeExperience': 0}, 'rx': {'safetyRxChecksInvalid': True},
'heartbeat': {'heartbeatLost': True}, 'native_fault': {'faults': 1}}
state = replace(state, pandaInventory=tuple(replace(p, **changes[mutation]) for p in state.pandaInventory))
if mutation == 'native_stale':
state = replace(state, observedMonoTime=now - 200_000_001, validUntilMonoTime=now - 1)
if mutation == 'lat_denied':
state = replace(state, lateralAllowed=False)
native = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=state.observedMonoTime)
native.aolSafetyWire = encode_safety(state)
pandas = messaging.new_message('pandaStates', len(self.cp.safetyConfigs), valid=True,
logMonoTime=state.sourcePandaStatesMonoTime)
for panda, config in zip(pandas.pandaStates, self.cp.safetyConfigs, strict=True):
panda.safetyModel, panda.safetyParam = config.safetyModel, config.safetyParam
panda.alternativeExperience = self.cp.alternativeExperience
panda.controlsAllowed = not partial and not lat_only
if mutation == 'latest_fault':
pandas.pandaStates[-1].heartbeatLost = True
axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=now)
axis.aolAxisState = {'sessionId': state.axisSessionId, 'sequence': self.sequence,
'sourceCarStateMonoTime': now, 'observedMonoTime': now, 'validUntilMonoTime': now + 30_000_000,
'mode': 'lateralOnly' if lat_only else 'combined', 'lateralActive': True, 'longitudinalActive': not lat_only,
'qualified': True, 'desiredLateral': True, 'desiredLongitudinal': not lat_only,
'nativeAcknowledged': True, 'faultReason': 'none', 'faultSessionId': state.axisSessionId}
if mutation == 'session':
axis.aolAxisState.sessionId = axis.aolAxisState.faultSessionId = 'new-session'
if mutation == 'sequence':
axis.aolAxisState.sequence = 1
if mutation == 'axis_stale':
axis.logMonoTime = axis.aolAxisState.observedMonoTime = now - 30_000_001
axis.aolAxisState.validUntilMonoTime = now - 1
events = messaging.new_message('onroadEvents', int(event is not None), valid=True, logMonoTime=now)
if event is not None:
events.onroadEvents[0].name = event
events.onroadEvents[0].immediateDisable = True
self.card.sm.update_msgs(now / 1e9, [native.as_reader(), pandas.as_reader(), axis.as_reader(), events.as_reader()])
self.native = state
return now
def fault(self, *, now=None):
now = self.last_now if now is None else now
result = self.card.aol_disarming_fault(self.cs, int(self.card.sm.logMonoTime['onroadEvents']), now)
self.intent(fault=result, active=True, now=now)
return result
def baseline(self):
self.arm()
self.parse()
self.evidence()
assert not self.fault()
feedback = self.card.aol_transport_feedback
assert feedback.session and feedback.lateral_baseline_ns and feedback.lateral_inventory
assert self.card.aol_card_intent.allowed_latch
def assert_no_exemption(self):
feedback = self.card.aol_transport_feedback
assert not feedback.qualified and not feedback.awaiting and not feedback.paused
@pytest.mark.parametrize('order', ['wire_before_press', 'press_before_wire'])
def test_ev6_cancel_real_card_async_orders_retain_only_existing_intent(order):
from opendbc.car.hyundai.values import Buttons
from openpilot.starpilot.aol.runtime import ordinary_axis_acknowledged
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
if order == 'wire_before_press':
caller.evidence(partial=True)
caller.parse(Buttons.CANCEL)
else:
caller.parse(Buttons.CANCEL)
caller.evidence()
assert caller.card.aol_cancel_source(caller.cs, caller.last_now) > 0
assert not caller.fault()
first = caller.card.aol_transport_feedback.longitudinal_cancel_ns
assert first > 0
caller.assert_no_exemption()
caller.parse(Buttons.NONE)
caller.evidence(partial=True)
assert not caller.fault()
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == first
assert caller.card.aol_card_intent.allowed_latch and caller.card.aol_card_intent.output(caller.cs)[0]
assert not ordinary_axis_acknowledged(caller.card.sm, caller.cp, now_ns=caller.last_now)
caller.parse()
caller.evidence(lat_only=True)
assert not caller.fault()
assert ordinary_axis_acknowledged(caller.card.sm, caller.cp, now_ns=caller.last_now)
assert not caller.native.requestedLongitudinal and not caller.native.longitudinalAllowed
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == 0
caller.assert_no_exemption()
assert caller.card.aol_card_intent.allowed_latch
@pytest.mark.parametrize('mutation', ['cp', 'owner', 'ae', 'session', 'sequence'])
def test_ev6_cancel_rejects_changed_identity_and_continuity(mutation):
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
caller.parse(Buttons.CANCEL)
if mutation == 'cp':
caller.cp.safetyConfigs[-1].safetyParam = 0x15
caller.evidence(partial=True, mutation=mutation)
assert caller.fault()
caller.assert_no_exemption()
assert not caller.card.aol_card_intent.allowed_latch
@pytest.mark.parametrize('source', ['queued_old', 'future_batch', 'old_local', 'wrong_cs', 'native_stale', 'axis_stale'])
def test_ev6_cancel_requires_current_parser_batch_and_two_clock_receipts(source):
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
caller.parse(Buttons.CANCEL, stamp_delta=-40_000_000 if source == 'queued_old' else 1_000_000_000 if source == 'future_batch' else 0)
if source == 'wrong_cs':
caller.cs = caller.cs.as_reader().as_builder()
now = caller.clock.monotonic_ns() + (30_000_001 if source == 'old_local' else 0)
caller.evidence(partial=True, now=now, mutation=source)
if source in ('queued_old', 'future_batch', 'old_local', 'wrong_cs'):
assert caller.card.aol_cancel_source(caller.cs, now) == 0
assert caller.fault()
caller.assert_no_exemption()
assert not caller.card.aol_card_intent.allowed_latch
def test_ev6_cancel_deadline_cannot_be_renewed_by_duplicate_held_or_release():
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
caller.parse(Buttons.CANCEL)
caller.evidence(partial=True)
assert not caller.fault()
first = caller.card.aol_transport_feedback.longitudinal_cancel_ns
assert not caller.fault() # Identical Card observation is not a new occurrence.
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == first
for button in (Buttons.CANCEL, Buttons.NONE):
caller.parse(button)
caller.evidence(partial=True)
assert not caller.fault()
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == first
# Fresh typed IPC keeps the independent 30ms baseline current. Explicit
# input times exercise the deadline without a global clock override/sleep.
for offset in range(20_000_000, 180_000_001, 20_000_000):
caller.evidence(partial=True, now=first + offset)
assert not caller.card.aol_disarming_fault(caller.cs, caller.last_now, caller.last_now)
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == first
caller.assert_no_exemption()
assert first + 200_000_001 - caller.card.aol_transport_feedback.lateral_baseline_ns <= 30_000_000
caller.evidence(partial=True, now=first + 200_000_001)
assert caller.fault()
caller.assert_no_exemption()
assert not caller.card.aol_card_intent.allowed_latch
@pytest.mark.parametrize('mutation', ['lat_denied', 'rx', 'heartbeat', 'native_fault', 'latest_fault', 'steer', 'acc'])
def test_ev6_cancel_lateral_and_hard_fault_vetoes_remain_closed(mutation):
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
caller.parse(Buttons.CANCEL)
if mutation == 'steer':
caller.cs.steerFaultPermanent = True
if mutation == 'acc':
caller.cs.accFaulted = True
caller.evidence(partial=True, mutation=mutation)
assert caller.fault()
caller.assert_no_exemption()
assert not caller.card.aol_card_intent.allowed_latch
@pytest.mark.parametrize('case', ['no_baseline', 'non_cancel'])
def test_ev6_cancel_needs_physical_occurrence_and_accepted_lateral_baseline(case):
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
if case == 'no_baseline':
caller.arm()
else:
caller.baseline()
caller.parse(Buttons.CANCEL if case == 'no_baseline' else Buttons.NONE)
caller.evidence(partial=True)
fault = caller.fault()
assert caller.card.aol_transport_feedback.longitudinal_cancel_ns == 0
caller.assert_no_exemption()
if case == 'non_cancel':
assert fault and not caller.card.aol_card_intent.allowed_latch
assert not caller.native.longitudinalAllowed
@pytest.mark.parametrize('event', [log.OnroadEvent.EventName.controlsMismatch, log.OnroadEvent.EventName.canError],
ids=['controlsMismatch', 'canError'])
def test_ev6_cancel_never_exempts_controls_mismatch_or_critical_events(event):
from opendbc.car.hyundai.values import Buttons
with OpenpilotPrefix():
caller = _Ev6CancelCaller()
caller.baseline()
caller.parse(Buttons.CANCEL)
caller.evidence(partial=True, event=event)
assert caller.fault()
caller.assert_no_exemption()
assert not caller.card.aol_card_intent.allowed_latch
openpilot/starpilot/aol/transport_pause.py
+113 -5
View File
@@ -1,7 +1,7 @@
"""Reason-qualified intent retention; this module never grants an axis."""
from openpilot.cereal import log
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, current_native
from openpilot.starpilot.aol.wire import decode_intent
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, current_native, native_inventory_matches_cp
from openpilot.starpilot.aol.wire import SafetyState, decode_intent
def cp_identity(cp):
@@ -20,9 +20,23 @@ def healthy_transport(sm, cp, cs, session, now_ns, native=None):
native.requestedLateral and not native.lateralAllowed or
native.requestedLongitudinal and not native.longitudinalAllowed):
return False
coherent = bool(native.pandaInventory)
if coherent and (int(sm.logMonoTime['aolSafetyWire']) != native.observedMonoTime or
not 0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime or
now_ns - native.observedMonoTime > SAFETY_MAX_AGE_NS or
native.validUntilMonoTime - native.observedMonoTime > SAFETY_MAX_AGE_NS):
return False
if coherent and (not native_inventory_matches_cp(native, cp) or
any(p.safetyRxChecksInvalid or p.heartbeatLost or p.faults or
(native.requestedLongitudinal and str(config.safetyModel) not in ('silent', 'noOutput') and
not p.controlsAllowed)
for p, config in zip(native.pandaInventory, cp.safetyConfigs, strict=True))):
return False
# v2 carries exact producer-read health under its monotonic 200ms lease.
# The independently conflated BOOTTIME PandaStates is a latest veto only.
stamp = int(sm.logMonoTime['pandaStates'])
if not (sm.seen['pandaStates'] and sm.valid['pandaStates'] and sm.alive['pandaStates'] and
0 < stamp <= now_ns and now_ns - stamp <= SAFETY_MAX_AGE_NS):
stamp > 0 and (coherent or stamp <= now_ns and now_ns - stamp <= SAFETY_MAX_AGE_NS)):
return False
pandas = sm['pandaStates']
if not cp.safetyConfigs or len(pandas) != len(cp.safetyConfigs):
@@ -93,8 +107,60 @@ class TransportPauseFeedback:
self.pending_event = 0
self.awaiting = False
self.failed = False
self.lateral_baseline_ns = 0
self.lateral_inventory = None
self.lateral_native_ns = 0
self.longitudinal_cancel_ns = 0
self.last_cancel_source_ns = 0
def observe(self, sm, cp, cs, now_ns, *, latched=True):
def _longitudinal_cancel_current(self, sm, cp, cs, axis, now_ns, *, latched, cancel_source_ns, withdrawal_required=True) -> SafetyState | None:
"""Retain existing intent during Cancel's bounded LONG withdrawal handshake."""
from opendbc.car.hyundai.ev6_aol import qualified
from opendbc.car.structs import car
if (not latched or self.failed or self.paused or self.pending_event or
not self.session or str(axis.sessionId) != self.session or not qualified(cp, marked_only=True) or
not self.lateral_baseline_ns or not 0 <= now_ns - self.lateral_baseline_ns <= AXIS_MAX_AGE_NS or
str(axis.faultReason) != 'none' or not axis.nativeAcknowledged or
not axis.desiredLateral or not axis.lateralActive or cs.accFaulted or
not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent):
return None
cancel = any(e.type == car.CarState.ButtonEvent.Type.cancel and e.pressed for e in cs.buttonEvents)
fresh_cancel = bool(cancel and self.last_cancel_source_ns < cancel_source_ns <= now_ns and
now_ns - cancel_source_ns <= AXIS_MAX_AGE_NS)
if not self.longitudinal_cancel_ns and not fresh_cancel:
return None
if not withdrawal_required and (self.longitudinal_cancel_ns or not fresh_cancel):
return None
if self.longitudinal_cancel_ns and not 0 <= now_ns - self.longitudinal_cancel_ns <= SAFETY_MAX_AGE_NS:
return None
event_ns = int(sm.logMonoTime['onroadEvents'])
if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or
not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000 or
any(e.immediateDisable or e.softDisable for e in sm['onroadEvents'])):
return None
native = current_native(sm, cp, now_ns=now_ns, axis_session_id=self.session)
if (native is None or not native.pandaInventory or not native.requestedLateral or not native.lateralAllowed or
not native.requestedLongitudinal or withdrawal_required and native.longitudinalAllowed or
native.observedMonoTime < self.lateral_native_ns or
not native_inventory_matches_cp(native, cp) or
(native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.lateral_inventory):
return None
stamp = int(sm.logMonoTime['pandaStates'])
pandas = sm['pandaStates']
if (not sm.seen['pandaStates'] or not sm.valid['pandaStates'] or not sm.alive['pandaStates'] or stamp <= 0 or
len(pandas) != len(cp.safetyConfigs)):
return None
# controlsAllowed represents the withdrawn normal/LONG axis here. Preserve
# every latest raw configuration/fault veto and exact producer-read inventory.
current_pandas = all(int(p.safetyModel.raw) == int(c.safetyModel.raw) and p.safetyParam == c.safetyParam and
p.alternativeExperience == cp.alternativeExperience and not p.safetyRxChecksInvalid and
not p.heartbeatLost and not p.faults and
(str(c.safetyModel) not in ('silent', 'noOutput') or not p.controlsAllowed)
for p, c in zip(pandas, cp.safetyConfigs, strict=True))
return native if current_pandas else None
def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0):
self.qualified = False
self.awaiting = False
try:
@@ -109,20 +175,44 @@ class TransportPauseFeedback:
axis.qualified and axis.sessionId and axis.sequence > 0 and stamp == axis.observedMonoTime and
0 < stamp <= now_ns <= axis.validUntilMonoTime and now_ns - stamp <= AXIS_MAX_AGE_NS and
axis.validUntilMonoTime - stamp <= AXIS_MAX_AGE_NS and axis.faultSessionId == axis.sessionId and
cp_identity(cp) == self.cp and healthy_transport(sm, cp, cs, str(axis.sessionId), now_ns))
cp_identity(cp) == self.cp)
continuity = bool(not self.session or axis.sessionId == self.session and
(axis.sequence > self.sequence or axis.sequence == self.sequence and signature == self.signature))
baseline = bool(reason == 'none' and axis.nativeAcknowledged and
0 < axis.sourceCarStateMonoTime <= stamp and
stamp - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS)
transport_healthy = healthy_transport(sm, cp, cs, str(axis.sessionId), now_ns)
cancel_native = (self._longitudinal_cancel_current(sm, cp, cs, axis, now_ns, latched=latched,
cancel_source_ns=cancel_source_ns)
if current and continuity and baseline and not transport_healthy else None)
current = bool(current and (transport_healthy or cancel_native is not None))
if current and baseline and not latched:
self.session = ''
self.pending_event = 0
self.failed = False
self.longitudinal_cancel_ns = 0
continuity = True
if self.failed:
self.longitudinal_cancel_ns = 0
return True
if cancel_native is not None:
# This is neither transport-pause provenance nor an event exemption.
# Runtime still requires an exact independent native ACK for each axis.
if not self.longitudinal_cancel_ns:
self.longitudinal_cancel_ns = int(cancel_source_ns)
self.last_cancel_source_ns = int(cancel_source_ns)
self.lateral_baseline_ns = now_ns
self.lateral_native_ns = int(cancel_native.observedMonoTime)
self.sequence = int(axis.sequence)
self.signature = signature
return False
if transport_healthy:
native = current_native(sm, cp, now_ns=now_ns, axis_session_id=str(axis.sessionId))
if (not latched or native is not None and not native.requestedLongitudinal or
self.longitudinal_cancel_ns and now_ns - self.longitudinal_cancel_ns > SAFETY_MAX_AGE_NS):
self.longitudinal_cancel_ns = 0
if not current or not continuity or reason not in ('none', 'transportPause'):
self.longitudinal_cancel_ns = 0
hard = (self.paused or bool(self.pending_event) or reason == 'critical' or
bool(self.session and (latched or not continuity)))
self.paused = False
@@ -159,7 +249,24 @@ class TransportPauseFeedback:
self.paused = False
self.failed = True
return True
# Remember a real physical occurrence before its native wire arrives.
# Duplicate Card observations and held/released samples cannot renew it.
cancel_pending = self._longitudinal_cancel_current(sm, cp, cs, axis, now_ns, latched=latched,
cancel_source_ns=cancel_source_ns, withdrawal_required=False)
if cancel_pending is not None:
self.longitudinal_cancel_ns = int(cancel_source_ns)
self.last_cancel_source_ns = int(cancel_source_ns)
self.session = str(axis.sessionId)
native = current_native(sm, cp, now_ns=now_ns, axis_session_id=self.session)
lateral_baseline = bool(axis.desiredLateral and axis.lateralActive and native is not None and
native.pandaInventory and native.requestedLateral and native.lateralAllowed)
self.lateral_baseline_ns = now_ns if lateral_baseline else 0
if lateral_baseline and native is not None:
self.lateral_native_ns = int(native.observedMonoTime)
self.lateral_inventory = (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory))
else:
self.lateral_native_ns = 0
self.lateral_inventory = None
self.paused = False
else:
provenance = bool(known and not axis.lateralActive and not axis.longitudinalActive and
@@ -189,6 +296,7 @@ class TransportPauseFeedback:
self.signature = signature
return False
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
self.longitudinal_cancel_ns = 0
hard = self.paused or bool(self.pending_event) or bool(self.session and latched)
self.paused = False
self.pending_event = 0
openpilot/starpilot/aol/vehicle.py
+4
View File
@@ -27,6 +27,10 @@ def native_profile_supported(model: int, param: int) -> bool:
def native_matches_cp(CP, model: int, param: int) -> bool:
try:
if CP.brand == 'hyundai':
config = hyundai.aol_safety_config(CP)
return bool(config is not None and hyundai.native_accepts_cp(CP, model, param) and
model == int(config.safetyModel.raw) and param == int(config.safetyParam))
return bool(any(port.native_accepts_cp(CP, model, param) for port in _PORTS.values()) and
model == int(CP.safetyConfigs[0].safetyModel.raw) and
param == int(CP.safetyConfigs[-1].safetyParam))
openpilot/starpilot/aol/wire.py
+44 -6
View File
@@ -20,6 +20,21 @@ WIRE_VERSION = 1
MAX_WIRE_BYTES = 512
MAX_WIRE_WORDS = (MAX_WIRE_BYTES - 8) // 8
MAX_ID_BYTES = 96
SAFETY_INVENTORY_VERSION = 2
MAX_PANDA_SLOTS = 2
@dataclass(frozen=True)
class PandaSlot:
slotIndex: int
hardwareSerial: str
safetyModel: int
safetyParam: int
alternativeExperience: int
controlsAllowed: bool = False
safetyRxChecksInvalid: bool = False
heartbeatLost: bool = False
faults: int = 0
@dataclass(frozen=True)
@@ -36,6 +51,8 @@ class SafetyState:
requestedLongitudinal: bool
pandaSerial: str
axisSessionId: str
sourcePandaStatesMonoTime: int = 0
pandaInventory: tuple[PandaSlot, ...] = ()
@dataclass(frozen=True)
@@ -59,7 +76,7 @@ def _bounded_id(value: str) -> str:
return value
def _payload(raw_value, schema, kind: int):
def _payload(raw_value, schema, kind: int, versions=(WIRE_VERSION,)):
if not isinstance(raw_value, (bytes, bytearray, memoryview)):
return None
byte_count = raw_value.nbytes if isinstance(raw_value, memoryview) else len(raw_value)
@@ -73,7 +90,7 @@ def _payload(raw_value, schema, kind: int):
if segment_count_minus_one != 0 or segment_words > MAX_WIRE_WORDS or len(raw) != 8 + 8 * segment_words:
return None
with schema.from_bytes(raw, traversal_limit_in_words=MAX_WIRE_WORDS, nesting_limit=4) as decoded:
if int(decoded.kind) != kind or int(decoded.version) != WIRE_VERSION:
if int(decoded.kind) != kind or int(decoded.version) not in versions:
return None
return decoded.to_dict()
except (ValueError, OverflowError, RuntimeError, UnicodeDecodeError, capnp.KjException):
@@ -97,15 +114,25 @@ def _memoized(decode):
@_memoized
def decode_safety(raw: bytes) -> SafetyState | None:
value = _payload(raw, custom.AolAxisState.SafetyWire, SAFETY_KIND)
value = _payload(raw, custom.AolAxisState.SafetyWire, SAFETY_KIND, (WIRE_VERSION, SAFETY_INVENTORY_VERSION))
if value is None:
return None
try:
source_ns = int(value.get('sourcePandaStatesMonoTime', 0))
inventory = tuple(PandaSlot(int(slot['slotIndex']), _bounded_id(str(slot.get('hardwareSerial', ''))),
int(slot['safetyModel']), int(slot['safetyParam']), int(slot['alternativeExperience']),
bool(slot['controlsAllowed']), bool(slot['safetyRxChecksInvalid']),
bool(slot['heartbeatLost']), int(slot['faults']))
for slot in value.get('pandaInventory', ()))
if (int(value['version']) == WIRE_VERSION and (source_ns or inventory) or
int(value['version']) == SAFETY_INVENTORY_VERSION and
(source_ns <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS)):
return None
result = SafetyState(int(value['protocolVersion']), bool(value['compatible']), int(value['observedMonoTime']),
int(value['validUntilMonoTime']), int(value['safetyModel']), int(value['safetyParam']),
bool(value['lateralAllowed']), bool(value['longitudinalAllowed']), bool(value['requestedLateral']),
bool(value['requestedLongitudinal']), _bounded_id(str(value.get('pandaSerial', ''))),
_bounded_id(str(value.get('axisSessionId', ''))))
_bounded_id(str(value.get('axisSessionId', ''))), source_ns, inventory)
if result.validUntilMonoTime < result.observedMonoTime:
return None
return result
@@ -148,14 +175,25 @@ def encode_intent(value: IntentState) -> bytes:
def encode_safety(value: SafetyState) -> bytes:
_bounded_id(value.pandaSerial)
_bounded_id(value.axisSessionId)
inventory = value.pandaInventory
inventory_version = bool(inventory or value.sourcePandaStatesMonoTime)
if inventory_version and (value.sourcePandaStatesMonoTime <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS):
raise ValueError('AOL Panda inventory is invalid')
for slot in inventory:
_bounded_id(slot.hardwareSerial)
message = custom.AolAxisState.SafetyWire.new_message(
kind=SAFETY_KIND, version=WIRE_VERSION, protocolVersion=value.protocolVersion,
kind=SAFETY_KIND, version=SAFETY_INVENTORY_VERSION if inventory_version else WIRE_VERSION,
protocolVersion=value.protocolVersion,
compatible=value.compatible, observedMonoTime=value.observedMonoTime,
validUntilMonoTime=value.validUntilMonoTime, safetyModel=value.safetyModel,
safetyParam=value.safetyParam, lateralAllowed=value.lateralAllowed,
longitudinalAllowed=value.longitudinalAllowed, requestedLateral=value.requestedLateral,
requestedLongitudinal=value.requestedLongitudinal, pandaSerial=value.pandaSerial,
axisSessionId=value.axisSessionId)
axisSessionId=value.axisSessionId, sourcePandaStatesMonoTime=value.sourcePandaStatesMonoTime,
pandaInventory=[{'slotIndex': slot.slotIndex, 'hardwareSerial': slot.hardwareSerial, 'safetyModel': slot.safetyModel,
'safetyParam': slot.safetyParam, 'alternativeExperience': slot.alternativeExperience,
'controlsAllowed': slot.controlsAllowed, 'safetyRxChecksInvalid': slot.safetyRxChecksInvalid,
'heartbeatLost': slot.heartbeatLost, 'faults': slot.faults} for slot in inventory])
data = message.to_bytes()
if len(data) > MAX_WIRE_BYTES:
raise ValueError('AOL safety payload exceeds wire bound')
openpilot/starpilot/car/ford/lateral_preferences.py
+19
View File
@@ -0,0 +1,19 @@
"""Saved Ford controller selection; no onroad CP mutation or saved-byte repair."""
from openpilot.starpilot.saved_source import read_saved
KEY = "FordLateralMode"
CHOICES = {0: "Current curvature", 1: "BluePilot angle"}
def read_choice(params):
raw, readable = read_saved(params, KEY, 8)
valid = readable and raw in (None, b"0", b"1")
return (1 if valid and raw == b"1" else 0), raw, readable, valid
def selected(params, *, enabled):
value, _, readable, valid = read_choice(params)
safe, safe_ok = read_saved(params, "SafeMode", 8)
aol, aol_ok = read_saved(params, "AlwaysOnLateral", 8)
return int(enabled and readable and valid and value == 1 and safe_ok and safe in (None, b"0") and
aol_ok and aol in (None, b"0"))
openpilot/starpilot/car/hyundai/aol.py
+12 -4
View File
@@ -17,6 +17,7 @@ from opendbc.car.hyundai.classic_scc_aol import (
native_accepts as classic_native_accepts,
native_profile_supported as classic_native_profile_supported,
)
from opendbc.car.hyundai.canfd_owner import config_index
from opendbc.car.hyundai.canfd_stock_aol import (
qualified as qualified_canfd_stock, qualified_long as qualified_canfd_long,
LONG_AOL_WORDS as CANFD_LONG_AOL_WORDS, STOCK_AOL_MARKER, STOCK_AOL_WORDS,
@@ -159,6 +160,13 @@ def _policy_for(CP) -> AolVehiclePolicy:
)
def aol_safety_config(CP):
if len(CP.safetyConfigs) == 1:
return CP.safetyConfigs[0]
owner = config_index(CP)
return CP.safetyConfigs[owner] if owner is not None else None
def policy_for(CP) -> AolVehiclePolicy:
policy = _policy_for(CP)
marked = (
@@ -169,8 +177,8 @@ def policy_for(CP) -> AolVehiclePolicy:
or (qualified_non_scc(CP) and CP.alternativeExperience == AOL_EXPERIENCE
and CP.safetyConfigs[0].safetyParam == aol_word(CP))
)
if policy.runtime_supported and marked and len(CP.safetyConfigs) == 1:
config = CP.safetyConfigs[0]
config = aol_safety_config(CP)
if policy.runtime_supported and marked and config is not None:
if native_accepts_cp(CP, int(config.safetyModel.raw), int(config.safetyParam)):
policy = replace(policy, ordinary_axis_ack_required=True, full_axis_runtime_required=True)
return policy
@@ -200,9 +208,9 @@ def native_accepts_cp(CP, model: int, param: int) -> bool:
if qualified_classic_scc(CP, marked_only=True):
return classic_native_accepts(CP, model, param)
if qualified_canfd_long(CP, marked_only=True):
return model == int(car.CarParams.SafetyModel.hyundaiCanfd) and param == CP.safetyConfigs[0].safetyParam
return model == int(car.CarParams.SafetyModel.hyundaiCanfd) and param == aol_safety_config(CP).safetyParam
if qualified_canfd_stock(CP, marked_only=True):
return model == int(car.CarParams.SafetyModel.hyundaiCanfd) and param == CP.safetyConfigs[0].safetyParam
return model == int(car.CarParams.SafetyModel.hyundaiCanfd) and param == aol_safety_config(CP).safetyParam
if qualified_non_scc(CP):
return CP.alternativeExperience == AOL_EXPERIENCE and model == int(car.CarParams.SafetyModel.hyundai) and param == aol_word(CP)
return (
openpilot/starpilot/controller_extensions.py
+36 -1
View File
@@ -37,7 +37,7 @@ class ManualTurnInputs:
return tuple(settings)
def apply_blend_settings(self, controller):
for name in ("mache_lateral", "classic_lateral"):
for name in ("mache_lateral", "classic_lateral", "new_port_curvature"):
owner = getattr(controller, name, None)
if owner is not None and self._applied_blends.get(owner) != self.blend_settings:
owner.set_blend_settings(*self.blend_settings)
@@ -88,6 +88,33 @@ class ManualTurnInputs:
delay = float(np.clip(value, 0.2, 0.4))
return self.sm["modelV2"], ModelConstants.T_IDXS, delay, self.enabled
def native_permission(self, cp, *, max_age_ns):
"""Exact current public Panda acknowledgment for the optional angle owner."""
if not self.track_assist_permission:
return False
try:
# Drain happens in update(); capture one SM snapshot before sampling clocks.
pandas = self.sm["pandaStates"]
stamp = int(self.sm.logMonoTime["pandaStates"])
receipt = int(self.sm.recv_time["pandaStates"] * 1e9)
now, boot = time.monotonic_ns(), time.clock_gettime_ns(time.CLOCK_BOOTTIME)
if (not self.sm.seen["pandaStates"] or not self.sm.all_checks(["pandaStates"]) or
not 0 < stamp <= boot <= stamp + max_age_ns or
not 0 < receipt <= now <= receipt + max_age_ns):
return False
if len(pandas) != len(cp.safetyConfigs):
return False
for i, (panda, config) in enumerate(zip(pandas, cp.safetyConfigs, strict=True)):
if (panda.safetyModel != config.safetyModel or panda.safetyParam != config.safetyParam or
panda.alternativeExperience != cp.alternativeExperience or panda.safetyRxChecksInvalid or
panda.heartbeatLost or len(panda.faults) or str(panda.faultStatus) != "none"):
return False
if bool(panda.controlsAllowed) != (i == len(pandas) - 1):
return False
return True
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
return False
def assist_permission(self):
if not self.track_assist_permission:
return (), False
@@ -164,11 +191,19 @@ def configure_controller(CI, params):
getattr(controller, "manual_turn", None) is not None):
controller.manual_turn_inputs = ManualTurnInputs(params, track_assist_permission=bool(cp.flags & FordFlags.CANFD))
from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified
if controller is not None and angle_qualified(cp) and getattr(controller, "bp_lat", None) is not None:
controller.bp_lat.host.inputs = ManualTurnInputs(params, track_assist_permission=True)
from opendbc.car.ford.classic_lateral import qualified as classic_qualified
from opendbc.car.ford.generic_canfd_lateral import qualified as generic_canfd_qualified
if controller is not None and (classic_qualified(cp) or generic_canfd_qualified(cp)) and getattr(controller, "classic_lateral", None) is not None:
controller.manual_turn_inputs = ManualTurnInputs(params)
from opendbc.car.ford.new_port_curvature import qualified as new_port_curvature_qualified
if controller is not None and new_port_curvature_qualified(cp) and getattr(controller, "new_port_curvature", None) is not None:
controller.manual_turn_inputs = ManualTurnInputs(params)
if controller is not None and cp.brand == "ford" and getattr(controller, "manual_turn_inputs", None) is not None:
controller.manual_turn_inputs.apply_blend_settings(controller)
openpilot/starpilot/galaxy/tests/test_ford_angle_settings.py
+113
View File
@@ -0,0 +1,113 @@
"""One typed Ford choice through the real Galaxy shared settings owner."""
from pathlib import Path
import tempfile
import unittest
from openpilot.common.params import Params
from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsChanged, SettingsGateway
from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
from opendbc.car.ford.tests.test_three_ports import params as ford_params
from opendbc.car.ford.values import CAR
from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, select
class FordContext:
def __init__(self, owner):
self.owner = owner
def sample(self) -> AuthorityContext:
return AuthorityContext(self.owner.parked, self.owner.cp, self.owner.cp.to_bytes())
class TestFordAngleSettings(unittest.TestCase):
def setUp(self):
temporary = tempfile.TemporaryDirectory()
self.addCleanup(temporary.cleanup)
self.params = Params(temporary.name)
self.params.put_bool("SafeMode", False, block=True)
self.params.put_bool("AlwaysOnLateral", False, block=True)
self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1)
self.parked = True
self.context = FordContext(self)
self.gateway = SettingsGateway(self.params, self.context)
self.addCleanup(self.gateway.close)
def page(self):
return self.gateway.page("torque", "ford-session", b"ford-generation")
def row(self, page):
rows = [(i, r) for i, r in enumerate(page["rows"]) if r["label"] == "Ford steering controller"]
self.assertEqual(len(rows), 1)
return rows[0]
def choose(self, label):
page = self.page()
index, row = self.row(page)
self.assertTrue(row["available"])
intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value=label)
return self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation")
def test_actual_final_profiles_one_dropdown_and_typed_explicit_persistence(self):
for identity in sorted(CLASSIC | CANFD):
for already_selected in (False, True):
self.cp = ford_params(identity)
if already_selected:
self.assertTrue(select(self.cp, 1))
page = self.page()
_, row = self.row(page)
self.assertEqual(row["choices"], ["Current curvature", "BluePilot angle"])
self.choose("BluePilot angle")
self.assertEqual(self.params.get("FordLateralMode"), 1)
self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"1")
self.choose("Current curvature")
self.assertEqual(self.params.get("FordLateralMode"), 0)
owner = FeatureSettingsOwner(self.params, lambda _: True, vehicle_fingerprint=lambda: self.cp.carFingerprint,
vehicle_params=lambda: self.cp)
rows = owner.snapshot("torque", parked=True, system_long=False, lateral_context=True, metric=False).rows
self.assertEqual(sum(r.key == "FordLateralMode" for r in rows), 1)
def test_excluded_or_modified_final_capability_has_no_angle_row(self):
for identity in (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5, CAR.FORD_TRANSIT_MK5):
self.cp = ford_params(identity)
self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"]))
for field, value in (("alternativeExperience", 32), ("passive", True), ("dashcamOnly", True), ("notCar", True)):
self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1)
setattr(self.cp, field, value)
self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"]))
self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1)
self.cp.safetyConfigs[-1].safetyParam = 0
self.assertFalse(any(r["label"] == "Ford steering controller" for r in self.page()["rows"]))
def test_stale_expected_dependency_vehicle_and_parked_changes_deny_commit(self):
for change in ("saved", "safe", "aol", "vehicle", "parked"):
self.cp = ford_params(CAR.FORD_BRONCO_SPORT_MK1)
self.parked = True
self.params.put("FordLateralMode", 0, block=True)
self.params.put_bool("SafeMode", False, block=True)
self.params.put_bool("AlwaysOnLateral", False, block=True)
page = self.page()
index, _ = self.row(page)
intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value="BluePilot angle")
if change == "saved":
Path(self.params.get_param_path("FordLateralMode")).write_bytes(b"invalid")
elif change in ("safe", "aol"):
self.params.put_bool("SafeMode" if change == "safe" else "AlwaysOnLateral", True, block=True)
elif change == "vehicle":
self.cp.safetyConfigs[-1].safetyParam = 128
else:
self.parked = False
with self.assertRaises(SettingsChanged):
self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation")
self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"invalid" if change == "saved" else b"0")
def test_invalid_saved_choice_can_only_repair_to_current_curvature(self):
path = Path(self.params.get_param_path("FordLateralMode"))
path.write_bytes(b"2")
page = self.page()
index, row = self.row(page)
self.assertEqual(row["value"], "Invalid saved choice")
with self.assertRaises(SettingsChanged):
self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", value="BluePilot angle")
intent = self.gateway.preview(page["view"], index, 0, "ford-session", b"ford-generation", reset_default=True)
self.gateway.confirm(intent["intent"], "ford-session", b"ford-generation")
self.assertEqual(path.read_bytes(), b"0")
openpilot/starpilot/tests/test_ford_angle.py
+395
View File
@@ -0,0 +1,395 @@
"""Actual saved owner/factory/Card/Controls/parser/sender/native qualification.
These tests must run with the runner-selected DEBUG and RELEASE safety library.
No authority, measurement, desired-history or angle-ready test setters are used.
"""
import os
import ctypes
import hashlib
from pathlib import Path
import unittest
from unittest.mock import patch
from contextlib import ExitStack
from types import SimpleNamespace
from openpilot.cereal import messaging
from openpilot.common.params import Params, ParamKeyType
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.starpilot.tests import test_vehicle_preferences as startup
from openpilot.starpilot.lateral.tests import test_lane_runtime as lane
from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
from opendbc.car import gen_empty_fingerprint, structs
from opendbc.car.ford.values import CAR, FordFlags, DBC
from opendbc.bluepilot_lateral.tests import reference
from openpilot.starpilot.controller_extensions import configure_controller
from opendbc.bluepilot_lateral.hosts.starpilot import CLASSIC, CANFD, qualified, select
from opendbc.safety.tests import test_ford_aol as physical
from opendbc.safety.tests.libsafety import libsafety_py
class TestFordAngleStartup(unittest.TestCase):
def setUp(self):
self.helper = startup.TestVehicleStartupPreferences()
self.helper.setUp()
self.addCleanup(self.helper.doCleanups)
self.params = self.helper.params
self.helper.raw("SafeMode", b"0")
self.helper.raw("AlwaysOnLateral", b"0")
self.params.put_bool("AlphaLongitudinalEnabled", False, block=True)
self.params.put_bool("IsReleaseBranch", False, block=True)
def start(self, identity, requested=True, *, auxiliary=False, **kwargs):
observed = gen_empty_fingerprint()
offset = 4 if auxiliary else 0
observed[offset][0x5A] = 8
observed[offset + 2].update({0x3D6: 8, 0x186: 8})
return self.helper.start(identity, key="FordLateralMode", requested=requested, observed=observed,
capture=lambda ci: (int(ci.CP.safetyConfigs[-1].safetyParam), ci.CC.bp_lat is not None), **kwargs)
def test_actual_factory_card_saved_choice_before_controller_all_eleven(self):
for identity in sorted(CLASSIC | CANFD):
for requested in (False, True):
with self.subTest(identity=identity, selected=requested):
host, constructed, published = self.start(identity, requested)
baseline = 18 if identity == CAR.FORD_MUSTANG_MACH_E_MK1 else 66 if identity in CANFD else 32
word = 130 if identity in CANFD else 128
self.assertEqual(constructed, [(word if requested else baseline, requested)])
self.assertEqual(published.safetyConfigs[-1].safetyParam, constructed[0][0])
self.assertEqual(qualified(published), requested)
self.assertEqual(host.CI.CP.to_dict(), published.to_dict())
if requested:
self.assertIsNotNone(host.CI.CC.bp_lat.host.inputs)
self.assertAlmostEqual(published.steerActuatorDelay, .22, places=6)
self.assertEqual(self.params.get("FordLateralMode"), int(requested))
self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes(), b"1" if requested else b"0")
def test_absent_corrupt_dependencies_and_typed_value_fail_closed(self):
self.assertEqual(self.params.get_type("FordLateralMode"), ParamKeyType.INT)
self.assertEqual(self.params.get_default_value("FordLateralMode"), 0)
for raw in (None, b"0", b"", b"1\n", b"true", b"2", b"1" * 20, b"1"):
self.helper.raw("FordLateralMode", raw)
selected = VehicleStartupPreferences.read(self.params, enabled=True)
self.assertEqual(selected.ford_lateral_mode, int(raw == b"1"))
self.assertEqual(Path(self.params.get_param_path("FordLateralMode")).read_bytes() if raw is not None else None, raw)
from opendbc.car.ford.tests.test_three_ports import params
for raw in (1, b"1", "1", True, 1.0, None, "01"):
cp = params(CAR.FORD_BRONCO_SPORT_MK1)
self.assertEqual(select(cp, raw), type(raw) is int or type(raw) in (bytes, str) and raw in (b"1", "1"))
for key in ("SafeMode", "AlwaysOnLateral"):
for raw in (b"1", b"", b"false", b"0\n"):
self.helper.raw(key, raw)
self.assertEqual(VehicleStartupPreferences.read(self.params, enabled=True).ford_lateral_mode, 0)
self.helper.raw(key, b"0")
self.assertEqual(VehicleStartupPreferences.read(self.params, enabled=False).ford_lateral_mode, 0)
def test_saved_selection_excluded_profiles_and_final_longitudinal_owner(self):
for identity in (CAR.FORD_EDGE_MK2, CAR.FORD_MONDEO_MK5, CAR.FORD_TRANSIT_MK5):
host, constructed, published = self.start(identity)
self.assertFalse(qualified(published))
self.assertFalse(constructed[0][1])
self.assertIsNone(host.CI.CC.bp_lat)
self.assertEqual(self.params.get("FordLateralMode"), 1)
for identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MUSTANG_MACH_E_MK1):
for release in (False, True):
self.params.put_bool("AlphaLongitudinalEnabled", True, block=True)
self.params.put_bool("IsReleaseBranch", release, block=True)
host, constructed, published = self.start(identity)
word = (130 if identity in CANFD else 128) + int(published.openpilotLongitudinalControl)
self.assertEqual(constructed[0][0], word)
self.assertEqual(published.safetyConfigs[-1].safetyParam, word)
self.assertEqual(published.pcmCruise, not published.openpilotLongitudinalControl)
if release and identity in CANFD:
self.assertEqual(word, 130)
class TestFordAngleJoined(unittest.TestCase):
def test_saved_card_controls_parser_and_actual_native_trace_both_signs(self):
safety = libsafety_py.libsafety
sources = ("BrakeSysFeatures", "EngVehicleSpThrottle2", "Yaw_Data_FD1", "EngBrakeData",
"EngVehicleSpThrottle", "DesiredTorqBrk", "Steering_Data_FD1")
faults = [("crc", source) for source in (sources[0], sources[2])] + \
[("quality", source) for source in sources[:3]] + \
[("counter", source) for source in (sources[0], sources[2])] + \
[("freshness", source) for source in sources] + \
[(name, None) for name in ("gas", "brake", "cruise", "relay", "buttons", "panda_stale",
"panda_model", "panda_param", "panda_ae", "panda_topology",
"panda_heartbeat", "panda_fault_status", "panda_controls")]
for identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_EXPLORER_MK6,
CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_F_150_MK14):
scenarios = [("angle", None), ("default0", None)]
if identity in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MUSTANG_MACH_E_MK1):
scenarios += [("aux", None)] + faults
for kind, source in scenarios:
for direction in (-1, 1):
with self.subTest(identity=identity, direction=direction, kind=kind, source=source), OpenpilotPrefix(), \
patch.dict(os.environ, {"REPLAY": "1", "SIMULATION": "1"}), \
patch("openpilot.selfdrive.controls.controlsd.messaging.PubMaster"):
params = Params()
helper = TestFordAngleStartup()
helper.setUp()
auxiliary = None
try:
helper.params = helper.helper.params = params
for key, value in (("SafeMode", False), ("AlwaysOnLateral", False), ("AlphaLongitudinalEnabled", False),
("IsReleaseBranch", False), ("OpenpilotEnabledToggle", True)):
params.put_bool(key, value, block=True)
selected = kind != "default0"
host, _, cp = helper.start(identity, selected, auxiliary=kind == "aux")
offset = 4 if kind == "aux" else 0
self.assertEqual(qualified(cp), selected)
controls = Controls()
native = physical.TestFordAolDriverIntent()
native.setUp()
native.word = 66 if cp.flags & FordFlags.CANFD else 32
native.tick = 0
self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam), 0)
safety.init_tests()
safety.set_alternative_experience(0)
auxiliary = None
auxiliary_handle = None
if kind == "aux":
self.assertEqual(len(cp.safetyConfigs), 2)
self.assertEqual(cp.safetyConfigs[0].safetyModel, structs.CarParams.SafetyModel.noOutput)
self.assertEqual(cp.safetyConfigs[0].safetyParam, 0)
self.assertEqual(host.CI.CC.CAN.main, 4)
self.assertEqual(host.CI.CC.CAN.camera, 6)
# Root supplies a distinct, hash-equal copy of the selected native flavor.
main_file, aux_file = Path(os.environ["FORD_MAIN_LIBRARY"]), Path(os.environ["FORD_AUX_LIBRARY"])
self.assertNotEqual(main_file.resolve(), aux_file.resolve())
self.assertEqual(hashlib.sha256(main_file.read_bytes()).digest(), hashlib.sha256(aux_file.read_bytes()).digest())
auxiliary = libsafety_py.ffi.dlopen(str(aux_file))
auxiliary_handle = ctypes.CDLL(str(aux_file))
self.assertEqual(auxiliary.set_safety_hooks(cp.safetyConfigs[0].safetyModel, cp.safetyConfigs[0].safetyParam), 0)
auxiliary.init_tests()
auxiliary.set_alternative_experience(0)
self.assertFalse(auxiliary.get_controls_allowed())
self.assertEqual(safety.get_current_safety_param(), cp.safetyConfigs[-1].safetyParam)
baseline = None
if not selected:
baseline = reference.curvature_controller()(DBC[identity], cp)
configure_controller(SimpleNamespace(CP=cp, CC=baseline), params)
providers = []
received, sent, validity = [], [], []
observed_allowed = False
observed_relay = False
observed_health = False
path_seen, resumed, withdraw_seen = [], False, False
paired_cancel, paired_resume = False, False
tx_rejected, invalid_rx = 0, 0
def bind(host, selected, baseline, providers, sent, validity):
providers.clear()
providers.append(host.CI.CC.bp_lat.host.inputs if selected else host.CI.CC.manual_turn_inputs)
if baseline is not None:
providers.append(baseline.manual_turn_inputs)
for owner in providers:
self.assertIsNotNone(owner)
services = ["modelV2", "lateralDelay"] + (["pandaStates"] if owner.track_assist_permission else [])
owner.sm = messaging.SubMaster(services)
host.sm = messaging.SubMaster(["carControl"])
host.publish_sendcan = lambda frames, valid=True: (sent.extend(frames), validity.append(valid))
host.ci_initialized = True
host.CI.update([])
bind(host, selected, baseline, providers, sent, validity)
for tick in range(340):
failing = 120 <= tick < 180
recovery = 180 <= tick < 220
received.clear()
def rx(name, values, *, failing=failing, kind=kind, source=source, tick=tick,
received=received, offset=offset, native=native):
nonlocal invalid_rx
if failing and kind == "freshness" and name == source:
return
values = dict(values)
if failing and name == source:
if kind == "quality":
signal = {sources[0]: "VehVActlBrk_D_Qf", sources[1]: "VehVActlEng_D_Qf", sources[2]: "VehYawWActl_D_Qf"}[name]
values[signal] = 0
elif kind == "counter":
values["VehVActlBrk_No_Cnt" if name == sources[0] else "VehRollYaw_No_Cnt"] = 0
if failing and kind == "gas" and name == "EngVehicleSpThrottle":
values["ApedPos_Pc_ActlArb"] = 50
if failing and kind == "brake" and name == "EngBrakeData":
values["BpedDrvAppl_D_Actl"] = 2
if kind == "buttons" and name == "Steering_Data_FD1":
values["CcAslButtnCnclResPress"] = int(119 <= tick < 140 or 159 <= tick < 180)
frame = native.packer.make_can_msg(name, 0, values)
if failing and kind == "crc" and name == source:
addr, data, bus = frame
data = bytearray(data)
data[3 if name == sources[0] else 4] ^= 1
frame = (addr, bytes(data), bus)
accepted = safety.safety_rx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]))
if not accepted:
invalid_rx += 1
if not (failing and kind in ("crc", "quality", "counter") and name == source):
self.assertTrue(accepted, (kind, source, tick, name))
received.append((frame[0], frame[1], frame[2] + offset))
# Same bytes/order: Panda-local0 maps to its actual host bus4 in the auxiliary factory topology.
main = 0 if tick < 30 or recovery and kind not in ("angle", "default0", "aux", "buttons", "panda_stale") else 4
if failing and kind == "cruise":
main = 3
if kind == "buttons":
main = 3 if 129 <= tick < 169 else main
if kind == "relay" and tick == 180:
# A relay fault is latched. Only a new actual factory/native session can recover.
host, _, replacement = helper.start(identity)
self.assertEqual(replacement.to_dict(), cp.to_dict())
self.assertEqual(safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam), 0)
safety.init_tests()
safety.set_alternative_experience(0)
bind(host, selected, baseline, providers, sent, validity)
with patch.object(native, "rx", rx):
native.pump(1, main=main, speed=15.)
now = (1_000_000 + native.tick * 10_000) * 1000
if kind == "relay" and tick == 120:
frame = native.packer.make_can_msg("LateralMotionControl2" if cp.flags & FordFlags.CANFD else "LateralMotionControl", 0, {})
safety.safety_rx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]))
received.append(frame)
self.assertTrue(safety.get_relay_malfunction())
safety.safety_tick()
extra: tuple[tuple[str, int, dict[str, float]], ...] = (("SteeringPinion_Data", 0, {"StePinCompAnEst_D_Qf": 3}),
("Cluster_Info1_FD1", 0, {"AccEnbl_B_RqDrv": 1}),
("BodyInfo_3_FD1", 0, {}), ("RCMStatusMessage2_FD1", 0, {"FirstRowBuckleDriver": 1}),
("INSTRUMENT_PANEL", 0, {}), ("IPMA_Data", 2, {}),
("ACCDATA", 2, {}), ("ACCDATA_2", 2, {}), ("ACCDATA_3", 2, {}))
received.extend(native.packer.make_can_msg(name, bus + offset, values) for name, bus, values in extra)
state = host.CI.update([(now, list(received))])
if tick < 40:
continue
if kind in ("angle", "default0", "aux"):
self.assertTrue(state.canValid and not state.canTimeout)
self.assertTrue(safety.get_controls_allowed())
with ExitStack() as stack:
stack.enter_context(patch("openpilot.starpilot.controller_extensions.time.monotonic_ns", return_value=now))
stack.enter_context(patch("openpilot.starpilot.controller_extensions.time.clock_gettime_ns", return_value=now))
stack.enter_context(patch("openpilot.selfdrive.car.card.time.monotonic", return_value=now / 1e9))
for owner in providers:
stack.enter_context(patch.object(owner.sm, "update", return_value=None))
events = []
if tick % 10 == 0 and not (failing and kind == "panda_stale"):
pandas = messaging.new_message("pandaStates", len(cp.safetyConfigs) + int(failing and kind == "panda_topology"),
valid=True, logMonoTime=now)
for i, _config in enumerate(cp.safetyConfigs):
p = pandas.pandaStates[i]
owner = safety if i == len(cp.safetyConfigs) - 1 else auxiliary
self.assertIsNotNone(owner)
p.safetyModel, p.safetyParam = owner.get_current_safety_mode(), owner.get_current_safety_param()
p.alternativeExperience = cp.alternativeExperience
p.controlsAllowed = owner.get_controls_allowed()
if owner is safety:
p.safetyRxChecksInvalid = not safety.safety_config_valid()
else:
assert auxiliary_handle is not None
p.safetyRxChecksInvalid = ctypes.c_bool.in_dll(auxiliary_handle, "safety_rx_checks_invalid").value
p.faultStatus = "faultPerm" if owner.get_relay_malfunction() else "none"
p.faults = ["relayMalfunction"] if owner.get_relay_malfunction() else []
if failing:
p = pandas.pandaStates[len(cp.safetyConfigs) - 1]
if kind == "panda_model":
p.safetyModel = structs.CarParams.SafetyModel.noOutput
elif kind == "panda_param":
p.safetyParam = 130 if p.safetyParam == 128 else 128
elif kind == "panda_ae":
p.alternativeExperience = 32
elif kind == "panda_heartbeat":
p.heartbeatLost = True
elif kind == "panda_fault_status":
p.faultStatus = "faultTemp"
elif kind == "panda_controls":
p.controlsAllowed = False
observed_allowed = bool(pandas.pandaStates[len(cp.safetyConfigs) - 1].controlsAllowed)
observed_relay = safety.get_relay_malfunction()
observed_health = safety.safety_config_valid()
events.append(pandas.as_reader())
lane.feed(controls, now, tick, speed=15., active=observed_allowed and not observed_relay,
enabled=observed_allowed and not observed_relay)
car_state = messaging.new_message("carState", valid=True, logMonoTime=now)
car_state.carState = state
controls.sm.update_msgs(now / 1e9, [car_state.as_reader()])
preview = messaging.new_message("modelV2", valid=True, logMonoTime=now)
preview.modelV2 = lane.model()
preview.modelV2.action.desiredCurvature = direction * .0008
preview.modelV2.orientationRate.z = [direction * .0008 * 15.] * 33
delay = messaging.new_message("lateralDelay", valid=True, logMonoTime=now)
delay.lateralDelay.lateralDelay = .1
controls.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader()])
for owner in providers:
owner.sm.update_msgs(now / 1e9, [preview.as_reader(), delay.as_reader(), *events] if owner.track_assist_permission else
[preview.as_reader(), delay.as_reader()])
command, _ = controls.state_control()
if kind in ("angle", "default0", "aux"):
self.assertTrue(command.enabled and command.latActive)
control_event = messaging.new_message("carControl", valid=True, logMonoTime=now)
control_event.carControl = command
host.sm.update_msgs(now / 1e9, [control_event.as_reader()])
host.can_log_mono_time = now
sent.clear()
validity.clear()
host.controls_update(state, command)
if baseline is not None:
_, expected = baseline.update(command, host.CI.CS, now)
self.assertEqual(sent, expected, (identity, direction, tick, "default0 current bytes"))
button_frames = [(bus, data) for addr, data, bus in sent if addr == 0x83]
if kind == "buttons" and button_frames:
self.assertEqual([bus for bus, _ in button_frames], [host.CI.CC.CAN.camera, host.CI.CC.CAN.main])
paired_cancel |= any(data[1] & 1 for _, data in button_frames)
paired_resume |= any(data[3] & 2 for _, data in button_frames)
for addr, data, bus in sent:
if addr not in (0x83, 0x3CA, 0x3D3, 0x3D6):
continue
packet = libsafety_py.make_CANPacket(addr, bus - offset, data)
if auxiliary is not None:
# The other actual configured Panda has no steering/longitudinal TX authority.
self.assertFalse(auxiliary.safety_tx_hook(packet))
self.assertFalse(auxiliary.get_controls_allowed())
accepted = bool(safety.safety_tx_hook(packet))
if addr == 0x83:
# Native retains its physical-context rules; a held redundant cancel after ACC-off is denied.
expected = not safety.get_relay_malfunction() and not (data[1] & 1 and not safety.get_cruise_engaged_prev())
self.assertEqual(accepted, expected)
continue
fd = addr == 0x3D6
mode = (data[0] >> 4) & 7 if fd else ((data[4] >> 2) & 7 if addr == 0x3D3 else data[0] >> 5)
if selected and addr in (0x3D3, 0x3D6):
bad_ack = failing and kind.startswith("panda_") and kind != "panda_stale"
if not observed_allowed or observed_relay or not observed_health or bad_ack or kind == "panda_stale" and 151 <= tick < 180:
self.assertEqual(mode, 0)
withdraw_seen |= kind not in ("angle", "default0", "aux")
if mode:
path = (((data[3] & 31) << 6) | (data[4] >> 2)) - 1000 if fd else ((data[3] << 3) | (data[4] >> 5)) - 1000
if path:
path_seen.append(path)
resumed |= tick >= 240
if not accepted:
tx_rejected += 1
# During actual faults native blocks immediately; the10Hz public acknowledgment can lag <=one period.
self.assertNotIn(kind, ("angle", "default0", "aux", "panda_stale", "buttons"))
self.assertTrue(120 <= tick < 230 and (not safety.get_controls_allowed() or not safety.safety_config_valid() or
safety.get_relay_malfunction()))
elif selected and addr in (0x3D3, 0x3D6) and mode:
self.assertTrue(safety.get_controls_allowed() and safety.safety_config_valid() and not safety.get_relay_malfunction())
if tick >= 260:
self.assertTrue(safety.get_controls_allowed() and safety.safety_config_valid() and not safety.get_relay_malfunction())
if selected:
self.assertTrue(path_seen, "Production sender never produced an active signed angle")
self.assertTrue(all(value * direction < 0 for value in path_seen))
self.assertTrue(resumed, "Actual factory/physical cruise rearm did not resume accepted angle")
if kind not in ("angle", "default0", "aux"):
self.assertTrue(withdraw_seen, "Fault did not withdraw production angle")
if kind in ("crc", "quality", "counter"):
self.assertGreater(invalid_rx, 0)
if kind == "buttons":
self.assertTrue(paired_cancel and paired_resume)
self.assertEqual(Path(params.get_param_path("FordLateralMode")).read_bytes(), b"1" if selected else b"0")
finally:
helper.doCleanups()
safety.set_alternative_experience(0)
safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
if auxiliary is not None:
auxiliary.set_alternative_experience(0)
auxiliary.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
openpilot/starpilot/ui/feature_settings_owner.py
+4
View File
@@ -53,6 +53,7 @@ from openpilot.starpilot.saved_source import read_saved
from openpilot.starpilot.ui.wheel_feature import WheelFeature, PREFIX as WHEEL_PREFIX
from openpilot.starpilot.longitudinal.output_max import KEY as OUTPUT_MAX_KEY
from openpilot.starpilot.ui.tesla_screen_feature import TeslaScreenFeature, KEYS as TESLA_SCREEN_KEYS
from openpilot.starpilot.ui.ford_lateral_feature import FordLateralFeature
from openpilot.starpilot.ui.gm_tune_feature import GmTuneFeature, GmTruckFeature, GmEvPresetFeature
from openpilot.starpilot.car.gm.tune_preferences import KEY as GM_TUNE_KEY
from openpilot.starpilot.car.gm.radar_recovery import KEY as RADAR_RECOVERY_KEY
@@ -800,6 +801,7 @@ class FeatureSettingsOwner:
vehicle_fingerprint=self.vehicle_fingerprint(), dependencies=learning.sources))
elif page == FeaturePage.TORQUE:
title = "Steering and Torque"
rows.extend(FordLateralFeature(self).rows(parked))
controller_row = self.controller.row()
if controller_row is not None:
rows.append(controller_row)
@@ -1120,6 +1122,8 @@ class FeatureSettingsOwner:
return self._apply_reverse_cruise(request)
if key in LATERAL_PAUSE_KEYS:
return self._apply_lateral_pause(request)
if key == "FordLateralMode":
return FordLateralFeature(self).apply(request)
if key == "TurnAssist":
return self._apply_turn_assist(request)
if key == 'LateralControllerSelection':
openpilot/starpilot/ui/ford_lateral_feature.py
+61
View File
@@ -0,0 +1,61 @@
"""One saved Ford enum shared by Galaxy and the native feature presentation."""
from opendbc.bluepilot_lateral.hosts.starpilot import supported
from openpilot.starpilot.car.ford.lateral_preferences import KEY, CHOICES, read_choice
from openpilot.starpilot.saved_source import read_saved
from openpilot.starpilot.saved_document import commit_exact
from openpilot.starpilot.ui.feature_settings_state import FeatureRow
class FordLateralFeature:
def __init__(self, owner):
self.owner = owner
def capability(self):
cp = self.owner.vehicle_params()
try:
if cp is None or not supported(cp):
return None
return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl),
bool(cp.pcmCruise), str(cp.steerControlType), bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar),
tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs))
except (AttributeError, TypeError, ValueError, OverflowError):
return None
def dependencies(self):
values = []
for key in ("SafeMode", "AlwaysOnLateral"):
raw, readable = read_saved(self.owner.params, key, 8)
if not readable or raw not in (None, b"0"):
return None
values.append((key, raw))
return tuple(values)
def rows(self, parked):
capability = self.capability()
if capability is None:
return ()
value, raw, readable, valid = read_choice(self.owner.params)
dependencies = self.dependencies()
return (FeatureRow(KEY, "Ford steering controller", CHOICES[value] if valid else "Invalid saved choice", raw,
choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and
readable and dependencies is not None, capability=capability,
dependencies=dependencies or (), repair_value="" if valid else CHOICES[0], default_value=CHOICES[0],
reason="Applies next drive. BluePilot angle is available with normal cruise control on supported Ford vehicles."),)
def apply(self, request):
value = next((code for code, label in CHOICES.items() if label == request.value), None)
if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction):
return False
def authorized():
capability = self.capability()
_, raw, readable, valid = read_choice(self.owner.params)
dependencies = self.dependencies()
return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and
self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and
capability == request.capability and dependencies is not None and dependencies == request.dependencies and
readable and raw == request.expected and (valid or value == 0))
result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected,
authorized=authorized, temp_prefix=".ford-lateral-")
return result.committed and result.verified
openpilot/starpilot/vehicle_preferences.py
+5 -1
View File
@@ -30,9 +30,11 @@ class VehicleStartupPreferences:
gm_camera_pedal: bool = True
toyota_filter: bool = True
gm_longitudinal_tune: int = 0
ford_lateral_mode: int = 0
@classmethod
def read(cls, params, *, enabled: bool):
from openpilot.starpilot.car.ford.lateral_preferences import selected as ford_mode
from openpilot.starpilot.car.gm.tune_preferences import selected_tune
from openpilot.starpilot.car.tesla.preap_preferences import stock_configuration
preap_stock = stock_configuration(params)
@@ -73,7 +75,7 @@ class VehicleStartupPreferences:
read_saved(params, 'AlwaysOnLateral', 8) == (b'1', True) and
read_saved(params, 'TeslaAOLScreenTap', 8) == (b'1', True))
screen_brake = read_saved(params, 'TeslaAOLDisengageOnBrake', 8) == (b'1', True)
return cls(tesla_screen=screen, tesla_screen_brake=screen_brake, toyota_auto_hold=toyota,
return cls(ford_lateral_mode=ford_mode(params, enabled=enabled), tesla_screen=screen, tesla_screen_brake=screen_brake, toyota_auto_hold=toyota,
gm_longitudinal_tune=selected_tune(params, enabled=enabled),
volt_sng=volt_sng, gm_auto_hold=gm_auto_hold, volt_one_pedal=volt_one_pedal,
gm_camera_pedal=bool(enabled and not disable_bolt and readable and safe in (None, b"0")),
@@ -144,6 +146,8 @@ class VehicleStartupPreferences:
apply_volt_one_pedal(cp, self.volt_one_pedal and not self.disable_bolt_long, self.gm_auto_hold)
if cp.brand == "toyota":
apply_toyota_auto_hold(cp, self.toyota_auto_hold)
from opendbc.bluepilot_lateral.hosts.starpilot import select as select_ford_lateral
select_ford_lateral(cp, self.ford_lateral_mode)
return cp
def finalize(self, cp) -> None: