Compare commits

..

27 Commits

Author SHA1 Message Date
whoisdomi f20731225f blueee 2026-09-29 10:40:09 -05:00
whoisdomi 244dccfae2 bluey gps 2026-09-29 09:45:40 -05:00
whoisdomi 48c4abb898 highway smoothing 2 2026-09-28 19:16:39 -05:00
whoisdomi 48c4ab68df Merge branch 'bluescreensonly3' of https://GitHub.com/firestar5683/StarPilot into bluescreensonly3 2026-09-28 14:20:34 -05:00
whoisdomi ee223ec1f2 GPS over phone bluetooth 2026-09-28 14:20:11 -05:00
StarPilot Build Bot eed130bf06 build 2026-09-28 13:14:32 +00:00
whoisdomi 25df1b9071 highway smoothing 2026-09-28 07:56:09 -05:00
whoisdomi 2f1ea6d60e gps ui 2026-09-28 07:21:28 -05:00
whoisdomi 2300a51edf kp slider 2026-09-27 21:14:43 -05:00
whoisdomi 6d3d2eb847 copy oem damping factor 2026-09-26 06:40:27 -05:00
whoisdomi f1db0546f4 gps ui 2026-09-25 15:27:11 -05:00
whoisdomi 0294c53e67 learner 2026-09-24 21:59:17 -05:00
whoisdomi a934fa6696 remove numb 2026-09-24 20:34:28 -05:00
whoisdomi d5ace15455 numb 2026-09-24 13:09:55 -05:00
whoisdomi eff2a7408d Ioniq 6 turn blips 2026-09-19 20:58:06 -05:00
whoisdomi ee87261cd5 Car Date/Time fallback when gps/wifi not available 2026-09-19 20:58:05 -05:00
whoisdomi 18a81d11e6 Ioniq 6 Date/Time DBC Signal
Added date/time signal from Ioniq 6 can
2026-09-19 20:58:03 -05:00
whoisdomi 7b96d6cbd1 stark 2 2026-09-19 08:33:48 -05:00
whoisdomi 13eb3754c0 mr stark left 2026-09-18 20:19:06 -05:00
whoisdomi 41f8e9288d Model Stuff 2026-09-18 14:41:15 -05:00
whoisdomi 0098beaf25 is small worth it 2026-09-17 17:58:00 -05:00
whoisdomi 7cbbac8f60 see sat 3 2026-09-17 17:44:32 -05:00
StarPilot Build Bot 03456fa4b2 build 2026-09-17 19:27:33 +00:00
whoisdomi e9b3180737 see sat 2 2026-09-17 13:56:01 -05:00
whoisdomi 947c6f6b10 see sats 2026-09-17 11:50:18 -05:00
whoisdomi e84eafd9d1 small before big 2 2026-09-17 11:34:33 -05:00
whoisdomi b06d8baa56 small before big 2026-09-17 11:34:00 -05:00
425 changed files with 7188 additions and 21124 deletions
-56
View File
@@ -1,56 +0,0 @@
name: Fleet controller safety
on:
workflow_dispatch:
pull_request:
paths:
- 'opendbc_repo/**'
- 'selfdrive/car/**'
- 'starpilot/car/**'
- 'starpilot/controls/**'
- 'cereal/**'
- '.github/workflows/fleet_safety.yaml'
permissions:
contents: read
jobs:
harness:
runs-on: ubuntu-24.04
steps:
- uses: actions/checkout@v4
- uses: actions/setup-python@v5
with:
python-version: '3.12'
- run: python -m pip install -r selfdrive/car/tests/fleet_requirements.txt
- name: Audit accounting and runner failures
run: >-
python -m pytest --noconftest -o addopts='' -q
selfdrive/car/tests/test_fleet_safety_core.py
selfdrive/car/tests/test_fleet_safety_runner.py
opendbc_repo/opendbc/safety/tests/safety_replay/test_replay_drive.py
recorded_fleet:
runs-on: ubuntu-24.04
timeout-minutes: 90
strategy:
fail-fast: false
matrix:
shard: [0, 1, 2, 3, 4, 5, 6, 7]
steps:
- uses: actions/checkout@v4
- uses: actions/setup-python@v5
with:
python-version: '3.12'
- run: python -m pip install -r selfdrive/car/tests/fleet_requirements.txt
- name: Current controllers against freshly compiled release safety
run: >-
python -m selfdrive.car.tests.fleet_safety --all --release
--shard-count 8 --shard-index ${{ matrix.shard }}
--out selfdrive/car/tests/fleet_results/ci
- uses: actions/upload-artifact@v4
if: always()
with:
name: fleet-safety-${{ matrix.shard }}
path: |
selfdrive/car/tests/fleet_results/ci/**/*.json
selfdrive/car/tests/fleet_results/ci/**/*.log
+26 -29
View File
@@ -1,19 +1,26 @@
# StarPilot
[![Ask DeepWiki](https://img.shields.io/badge/Ask-DeepWiki-blue?logo=readthedocs&logoColor=white)](https://deepwiki.com/StarPilotHQ/StarPilot)
[![Ask DeepWiki](https://deepwiki.com/badge.svg)](https://deepwiki.com/firestar5683/StarPilot)
[![Discord](https://img.shields.io/discord/1387432184121393333?label=Discord)](https://firestar.link/discord)
[![Last Updated](https://img.shields.io/github/last-commit/StarPilotHQ/StarPilot/StarPilot)](https://github.com/StarPilotHQ/StarPilot)
[![Last Updated](https://img.shields.io/github/last-commit/firestar5683/StarPilot/StarPilot)](https://github.com/firestar5683/StarPilot)
[![Wiki](https://img.shields.io/badge/Wiki-StarPilot-blue?logo=wiki)](https://wiki.firestar.link)
**StarPilot** is a custom fork of [comma.ai's openpilot](https://comma.ai/openpilot),
an open source driver assistance system.
Openpilot provides
* Automated Lane Centering
* Adaptive Cruise Control
* Lane Change Assist
* Driver Monitoring *without wheel nags*
StarPilot was formerly a GM targeted fork,
but [has expanded to offer Quality-Of-Life improvements for all](#features)!
StarPilot is built off of [FrogPilot](https://github.com/FrogAi/FrogPilot)
and supports the major features FrogPilot offers.
Ford-specific lateral-control and vehicle-support work includes substantial adaptations from
[BluePilot](https://github.com/BluePilotDev/bluepilot/tree/bp-7.0), principally developed by
[Alan Polk](https://github.com/alan-polk) with additional BluePilot contributors. StarPilot's
@@ -30,7 +37,6 @@ this adaptation. Detailed code lineage is recorded in [CREDITS.md](CREDITS.md#hy
StarPilot has a vibrant, welcoming community [discord](https://firestar.link/discord).
Stop by to chat or ask questions!
If your car isn't driving as well as you'd like, [open a ticket so we can help](https://firestar.link/logs)!
## Documentation
@@ -39,35 +45,27 @@ installation guides, and software configuration.
## Features
* **Full support for Comma C3, C3X, and C4**
* Full support for chestnut/eGPU on C3, C3X, and C4. C3/C3X may require special gps handling.
* Full support for Comma C3, C3X, and C4
* Model switcher with all of comma's tinygrad driving models
* StarPilot custom torque controller
* StarPilot custom longitudinal planner for both radar and VoACC (visual only, radar-less) vehicles
* **Custom tuning for an expanding list of vehicles**
* Firestar Lateral Method -
Auto tuning system to adapt openpilot to *your* car's unique lateral characteristics.
* Special longitudinal planner tuning for VoACC (visual only, radar-less) vehicles
* Custom-tuned torque controllers for an expanding list of cars.
* Galaxy: StarPilot's portal to configure your comma device using your phone from anywhere.
Download models, change settings, update software, diagnostics, manage dashcam video, and more.
* Navigation - Display routes and give the driving model hints on taking exits and turns.
* Always On Lateral (full time steering assist)
* Lane Centering - Enable to center between detected lane lines,
with graceful fallback to model E2E lateral outputs and safety breakout
* Force Stop 2.0 - Better stops at stop signs and traffic lights.
* Speed Limit Controller - Use dashboard, map data, and/or vision detection for speed limit signs
* Curve Speed Controller - Learns from how you take curves
* Conditional Experimental Mode (CEM)
* Conditional Chill Mode (CCM)
* Driving Profiles and Traffic Mode
* Custom themes
* Alert volume customization
* Comma Pedal Interceptor support
* Bluetooth support - Connect keyboards, controllers, steering wheel remotes, and more.
* Sentry mode - Record when motion is detected while parked.
* V-ASM - Detect vehicles in the adjacent/blind spots using the driver-facing camera.
* Additional car support - Toyota SDSU, ZSS, CAN filter; pre-AP tesla; ford; Polestar 2; and more.
Download models, change settings, update software, visualize live model outputs for tuning.
* Always On Lateral (full time steering assist)*
* Speed Limit Controller*
* Learning Curve Speed Controller*
* Conditional Experimental Mode (CEM)*
* Driving Profiles*
* Custom themes*
* Alert Volume Controller*
* Comma Pedal Interceptor support*
* Toyota SDSU support*
* ZSS support*
* High quality dashcam recordings*
* Enhanced tuning for CEM (dynamic experimental mode switching)
* And more!
\* [Inherited from FrogPilot](https://github.com/FrogAi/FrogPilot#openpilot-vs-frogpilot)
## GM-only Features
@@ -84,7 +82,6 @@ with graceful fallback to model E2E lateral outputs and safety breakout
* Native and cross compilation for Windows, Mac, and Ubuntu
* Custom AGNOS to support C3, C3X, and C4
* Raylib UI on C3, C3X, and C4
* To run UI on PC:
* `./c3` for large UI
* `./c4` for small UI
-28
View File
@@ -28,34 +28,6 @@ public sunnypilot repositories:
[`f95f996f5917dcbbf2e32fe51b606a24cf836af6`](https://github.com/sunnypilot/opendbc/commit/f95f996f5917dcbbf2e32fe51b606a24cf836af6).
- Historical reconstruction and contributors: [CREDITS.md](CREDITS.md#hyundai-kia-and-genesis-support-adapted-from-sunnypilot)
## Continental ARS510 radar
The optional RAV4 radar decoders and `toyota_tss2_ars510.dbc` are adapted from
<https://github.com/ds-sebastian/ars510-radar> at
`fb5b29c2b5999f34afeb6f7ac494a73949d66c67`.
MIT License
Copyright (c) 2026 ARS510 radar contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
## Applicable upstream license notices
The upstream repositories and branches identified above publish both `LICENSE` and `LICENSE.md`.
-2
View File
@@ -237,8 +237,6 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin
cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m
approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off
slcPresentedSpeedLimitSource @43 :Text; # source of the shown accepted or pending posted limit
slcIsLimitingMaxSet @44 :Bool; # SLC target is below the configured Max Set
}
struct StarPilotRadarState @0xb86e6369214c01c8 {
Binary file not shown.
+1
View File
@@ -132,6 +132,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
audioFeedback @97;
bigModelLoading @100;
bigModelFailed @102;
bigModelPending @103;
soundsUnavailableDEPRECATED @47;
stockLkasDEPRECATED @98;
Binary file not shown.
+4 -10
View File
@@ -177,6 +177,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"UsbGpuActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"UsbGpuCompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"UsbGpuLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"UsbGpuPending", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"Version", {PERSISTENT, STRING}},
@@ -198,7 +199,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"AggressiveJerkSpeedDecrease", {PERSISTENT, FLOAT, "50.0", "50.0", 3}},
{"AlertVolumeControl", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"AllowImpossibleAcceleration", {PERSISTENT, BOOL, "0", "0", 3}},
{"AlwaysOnLateral", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
{"AlwaysOnLateral", {PERSISTENT, BOOL, "1", "0", 0, SETTINGS_SIMPLE}},
{"AlwaysOnLateralLKAS", {PERSISTENT, BOOL, "1", "0", 2}},
{"ApiCache_DriveStats", {PERSISTENT, JSON, "{}", "{}"}},
{"AutomaticallyDownloadModels", {PERSISTENT, BOOL, "1", "0", 1}},
@@ -389,7 +390,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
{"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"FordLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}},
{"SonataHybridLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}},
{"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}},
// These Ford curvature tuning concepts descend from BluePilot bp-7.0. StarPilot's key names and
// settings integration are local; see /CREDITS.md and /THIRD_PARTY_NOTICES.md for provenance.
@@ -432,6 +432,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"HideSpeedLimit", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"HideSteeringWheel", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"HigherBitrate", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"HighwayCorrectionGain", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
{"HolidayThemes", {PERSISTENT, BOOL, "1", "0", 0, SETTINGS_SIMPLE}},
{"HumanLaneChanges", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"IconPack", {PERSISTENT, STRING, "stock", "stock", 0}},
@@ -577,7 +578,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"PauseAOLOnBrake", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"PauseLateralOnSignal", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"PauseLateralSpeed", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
{"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
{"LateralResumeDelay",{PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
{"PedalsOnUI", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"PIPPreviewEnabled", {PERSISTENT, BOOL, "0", "0", 1}},
{"PIPPreviewMask", {PERSISTENT, JSON, "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", 2}},
@@ -631,7 +632,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ScreenBrightnessOnroadManual", {PERSISTENT, INT, "100", "100", 2, SETTINGS_SIMPLE}},
{"ScreenBrightnessOnroadOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
{"ScreenManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
{"KeepScreenOnGpuLoading", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"ScreenRecorder", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
{"ScreenTimeout", {PERSISTENT, INT, "30", "30", 2, SETTINGS_SIMPLE}},
{"ScreenTimeoutOnroad", {PERSISTENT, INT, "30", "10", 2, SETTINGS_SIMPLE}},
@@ -704,7 +704,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"StandbyMode", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"StandbyWakeButton", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"StandbyButtonPressTime", {CLEAR_ON_MANAGER_START | DONT_LOG, INT, "0", "0"}},
{"ScreenOffToggleCounter", {CLEAR_ON_MANAGER_START | DONT_LOG, INT, "0", "0"}},
{"StandbyWakeEngage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
{"StandbyWakeDisengage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
{"StandbyWakeInfoAlert", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
@@ -730,7 +729,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"SteerRatio", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
{"SteerRatioStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
{"EnableTorqueBarWidget", {PERSISTENT, BOOL, "1", "0", 0}},
{"StingerObjectShadow", {PERSISTENT, BOOL, "0", "0", 3}},
{"StockConfidenceBallWidget", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
{"StockDongleId", {PERSISTENT, STRING, "", ""}},
{"StopAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
@@ -744,11 +742,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
{"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"SubaruStopStartOff", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"SubaruAvhStartup", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
{"TeslaAOLDisengageOnBrake", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
{"TeslaAOLScreenTap", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
{"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
{"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}},
@@ -804,7 +799,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"GMAutoHold", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"VoltOnePedalMode", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"ToyotaAutoHold", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"ToyotaRadarMode", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
{"WarningImmediateVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
{"WarningSoftVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
{"WeatherPresets", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
Binary file not shown.
+1 -2
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
# 454 Supported Cars
# 453 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|Setup Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -265,7 +265,6 @@ A supported vehicle is one that just works when you install a comma device. All
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (with HDA II) 2025">Buy Here</a></sub></details>|||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025">Buy Here</a></sub></details>|||
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2026">Buy Here</a></sub></details>|||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 (without HDA II) 2025">Buy Here</a></sub></details>|||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
-86
View File
@@ -1,86 +0,0 @@
# Fleet offline audit — September 21, 2026
This is a first-pass fault and coverage inventory, not fleet driving clearance.
No hardware was accessed and no controller or safety policy was changed by this
audit. Other work was concurrently modifying the checkout; per-case source and
library hashes identify the tested implementations.
The full debug-library run visited all 345 platforms. It evaluated both recorded
and AOL-main scenarios for 287 registered routes, plus 108 missing-route entries:
| Result | Cases |
|---|---:|
| Pass within the stated scope | 103 |
| Failed checks, requiring triage | 123 |
| Missing coverage | 288 |
| Could not evaluate | 168 |
| Total | 682 |
These are case counts, not numbers of unsafe cars. Missing coverage includes all
108 platforms without routes and segments without active transitions. Evaluation
errors include unavailable recordings and identities that do not match the
registered platform after the existing fingerprint migration. Old logs must be
normalized explicitly, not quietly substituted for another car.
Full local evidence is under
`selfdrive/car/tests/fleet_results/full_fleet/results.json`, with per-shard build,
case and worker logs. Generated evidence is ignored by Git.
The follow-up full release-library run also completed 682 cases: **102 pass,
153 failed, 289 uncovered, 138 evaluation errors**. Evidence is under
`selfdrive/car/tests/fleet_results/release_fleet_checked/results.json`.
The two batches had different download availability and ran against a changing
working tree; their count difference is not an isolated debug-versus-release
experiment. Both correctly exit nonzero. The release batch is not fleet clearance.
## Confirmed distinctions from failure triage
**Hyundai Custin — controller/safety capability mismatch.** The registered
segment `0bbe367c98fa1538/2023-09-16--00-16-49/2` contains 600 camera-bus
messages at 0x53e, all eight bytes, none six. `CarInterfaceBase.get_starpilot_params`
in `opendbc_repo/opendbc/car/interfaces.py` enables HAS_LKAS12 by address alone.
Hyundai `CarState.update` and `CarController.update` then produce six-byte LKAS12
replacements. In `opendbc_repo/opendbc/safety/modes/hyundai.h`,
`hyundai_rx_all_hook` only enables replacement after receiving a six-byte camera
message, and `hyundai_tx_hook` correctly rejects the unsolicited replacement.
Both scenarios reject 5,799 such packets. A fresh-library probe also reproduced
the six-byte/eight-byte distinction. Repair requires a controller capability and
parser regression test; do not broaden the safety allowlist to hide the mismatch.
**Honda Civic Bosch — incompatible historical control requests.** All 5,997
recorded requests in the inspected 2020 fixture have enabled/latActive/longActive
false but resume true; all 6,000 cruise-state CAN messages are disabled. Current
controller output is RES_ACCEL at 0x296, which current safety correctly blocks.
The AOL probe suppresses resume and passes. Investigate historical command/schema
semantics before calling this a current steering defect.
**Ford Escape — mid-segment initialization artifact.** The first cruise-enabled
0x165 enables controls, but the immediately following 0x202 reaches
`speed_mismatch_check` before safety has nonzero speed history. Controls are
revoked; cruise stays enabled for the entire segment so `pcm_cruise_check` sees
no new rising edge. Relay health remains good. This reproduces with both recorded
and default alternative experience. A two-second scoring warmup does not repair
the latch. This needs recorded preroll/initialization coverage, not force-setting
`controls_allowed` or changing vehicle safety.
## Tesla and AOL scope
In the full debug-library run, the Model 3 route and the second Model Y route
passed both scenarios. The first Model Y route lacked a lateral transition;
its AOL case also lacked requested steering under AOL-only safety permission.
Model X was uncovered as a current dashcam-only configuration. The Model S HW1
and Pre-AP entries have no registered routes. None of these findings reproduces
or disproves the exact hackathon oscillation without its trace.
The separate actual StarPilotCard synthetic-input suite passed 964 checks with
72 explicit active-sequence gaps. All 345 disabled configurations were checked;
309 platforms completed both active modes. These tests check state-machine gates
and stable sequences, not the entire selfdrived-to-Panda feedback loop.
The test-harness regressions pass 34 tests. They cover pre-hook AOL authorization,
strict configuration/bus routing, rejected active packets, expected negative
checks, empty activity, worker crashes, stale reports and build failures.
See `FLEET_SAFETY_TESTING.md` for commands, CI scope and limitations. The workflow
has been added locally but not published or run on GitHub, and branch protection
has not been changed. The full fleet is not green.
-112
View File
@@ -1,112 +0,0 @@
# Offline fleet controller and safety checks
The runner in `selfdrive/car/tests/fleet_safety.py` enumerates every platform in
the current checkout and uses `opendbc.car.tests.routes`. It runs current
CarInterface/CarState/CarController code against recorded CAN and actuator
requests, then checks each newly generated CAN packet with freshly compiled
current safety hooks. It never connects to a Panda or starts vehicle processes.
Run from the repository root with the repository Python environment:
```sh
python -m selfdrive.car.tests.fleet_safety --inventory
python -m selfdrive.car.tests.fleet_safety --platform TESLA_MODEL_Y --release
python -m selfdrive.car.tests.fleet_safety --all --release
```
An isolated dependency set is in `selfdrive/car/tests/fleet_requirements.txt`.
The runner needs a C compiler. It does not use a previously staged libsafety.
Without `--release`, the safety library enables ALLOW_DEBUG, like the existing
safety unit tests. Release checks are needed as well: a debug-only hook must not
be mistaken for an available production configuration. Release host builds retain
unused-variable warnings without treating that specific diagnostic as an error.
For parallel runs, use separate output directories:
```sh
python -m selfdrive.car.tests.fleet_safety --all --release \
--shard-count 8 --shard-index 0 --out selfdrive/car/tests/fleet_results/shard_0
```
Run indices 0 through 7. Each has its own library copies, worker processes,
parameter namespace, logs and results. `--local-log` allows a local rlog for one
explicitly selected platform. Route IDs and old fingerprint aliases must match;
the harness does not silently treat another vehicle's log as that platform.
## What is checked
- Recorded commands under the current default feature configuration.
- A separate AOL MAIN-availability controller/safety probe, using recorded
actuator values with longitudinal requests and cruise button requests off.
- Actual per-Panda safety model, CP/FPCP safety-param OR, alternative-experience
OR, and strict four-bus routing, matching production configuration assembly.
- Incoming CAN, current CarState validity and safety receive health.
- Every emitted TX, including inactive-state packets; unexpected rejection fails.
- Pre-hook normal/AOL/longitudinal permissions, so a rejection that revokes
authorization cannot disappear from the failure accounting.
- Active requests, accepted active TX, engagement transitions, and AOL-only
safety authorization coverage. Sparse commands and absent transitions cannot
qualify as complete coverage.
There is a two-second unscored fixture startup interval. Controller and safety
history still receive messages during it. The harness does not force safety
authorization or clear a relay fault to manufacture a passing result.
## Results are deliberately strict
`pass` means the case satisfied these specific checks and coverage requirements.
`failed` means a hook/health check failed and needs investigation. `uncovered`
means the scenario was not demonstrated, including missing routes, dashcam-only
interfaces and segments without transitions. `error` means the case could not be
evaluated, such as download failure or mismatched fixture identity. Anything
other than pass makes the command exit nonzero. Existing `non_tested_cars`
exemptions remain visible coverage gaps.
Reports include frame counters, bounded rejected packet evidence, source hashes,
effective safety configurations and build provenance. Worker results carry a
unique execution ID; a crash, stale result or inconsistent exit code cannot be
reused as a pass. JSON and logs live under the ignored `fleet_results` directory.
The AOL probe is **not** a complete simulation of StarPilotCard, selfdrived,
controls mismatch handling or a vehicle ECU. Recorded commands may also reflect
historical settings different from current defaults. A blocked historical resume
request is not automatically a steering bug. Investigate each failure before
changing code. Do not widen safety permissions to make tests green.
## Continuous integration and remaining coverage
`starpilot/controls/tests/test_fleet_aol.py` separately exercises the actual
StarPilotCard state machine with isolated synthetic inputs for every platform.
It tests feature-off behavior, steady engagement, AOL-only operation, brake
pause, native/StarPilot immediate-disable alerts, calibration and gear gates.
It uses empty firmware/fingerprint fixtures, so optional vehicle configurations
are not covered by these sequences. Run it with a compatible built host runtime:
```sh
python -m pytest --noconftest -o addopts='' -q starpilot/controls/tests/test_fleet_aol.py
```
The first run passed 964 checks and explicitly skipped 72 active sequences:
309 platforms exercised both active modes; 36 platforms had two gaps each
(30 dashcam-only, one notCar, four Volvo policy exclusions, one Pre-AP external
authorization dependency). All 345 feature-off checks passed. A separate
Pre-AP authorization input boundary test is synthetic, not proof of actual
Panda authorization. These tests were run against the current working tree,
including concurrent Pre-AP changes; they do not certify an earlier commit.
The lightweight CI workflow below does not build the native runtime required
by this separate state-machine suite.
`.github/workflows/fleet_safety.yaml` adds harness tests and eight release-mode
recorded-route shards on relevant pull requests and manual runs. Missing coverage
is not converted to a skip or allowed failure. The current fleet is not green;
this workflow will expose that fact. It has not been executed on GitHub from this
local task. Requiring it for merge also needs repository branch protection; a
workflow file alone does not change repository settings.
As of the first September 21 inventory there are 345 platforms, 287 registered
routes across 237 platforms, and 108 platforms with no registered route. A route
entry does not guarantee valid, downloadable logs or all necessary maneuvers.
Every optional harness, longitudinal mode, safety parameter, firmware generation
and AOL configuration still needs explicit coverage. Offline checks reduce
blind spots; they do not certify every physical vehicle or reproduce an incident
whose CAN trace was not retained.
-36
View File
@@ -1,36 +0,0 @@
# Experimental RAV4 Radar
The optional Continental ARS510 parser supports the Toyota RAV4 2022 and RAV4
2023-2025 profiles, including hybrids. Earlier RAV4s, RAV4 Prime, and other Toyota
or Lexus profiles are unchanged. This is a beta feature, disabled by default.
In Galaxy, open **Vehicle > RAV4 Radar (Experimental)** while parked:
- **Off** preserves the existing vision-only behavior on these profiles.
- **ACC Target** publishes the radar's selected lead. Other leads remain vision-based.
- **Fused Tracks** decodes up to 20 objects, filters speed uncertainty, uses the
radar's ACC target to anchor its matched object's speed and distance, and gates
young, distant, or off-path objects before publishing them.
Restart the device after changing the mode. The equivalent persistent parameter
is `ToyotaRadarMode`: `0` for Off, `1` for ACC Target, and `2` for Fused Tracks.
The parser waits through the radar's delayed startup rather than requiring its
object frames in the initial fingerprint. Once running, missing scans clear the
tracks and report radar unavailability; healthy tracks are not cleared between scans.
This setting does not enable Alpha Long, change longitudinal or lateral tuning,
send new CAN commands, or alter the existing radar-disable procedure. Without a
radar CAN filter, Alpha Long still disables the factory AEB output. Receiving
radar objects does not restore AEB. Filter-equipped and unfiltered cars retain
their existing longitudinal and safety configurations.
Use only for supervised beta testing and remain ready to brake or steer. An
available parser is not evidence that a particular car is providing live objects.
Collect rlogs with normal lead following, braking, stops and takeoffs, curves,
cut-ins, and both stock ACC and Alpha Long configurations. Bookmark unexpected
braking or lead loss. Do not deliberately approach an obstacle to test AEB.
The parser is adapted from the [ARS510 radar reference](https://github.com/ds-sebastian/ars510-radar)
at `fb5b29c2b5999f34afeb6f7ac494a73949d66c67`; attribution and its MIT notice are
preserved in `THIRD_PARTY_NOTICES.md`. Protocol and offline replay validation do
not establish closed-loop performance on every trim, firmware, or harness.
+2 -2
View File
@@ -21,11 +21,11 @@ fi
export QCOM_PRIORITY=12
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="19.8.1"
export AGNOS_VERSION="19.6.20"
fi
if [ -z "$AGNOS_ACCEPTED_VERSIONS" ]; then
export AGNOS_ACCEPTED_VERSIONS="19.8.1 19.8.2"
export AGNOS_ACCEPTED_VERSIONS="$AGNOS_VERSION"
fi
export STAGING_ROOT="/data/safe_staging"
+1 -2
View File
@@ -1,6 +1,6 @@
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
# Support Information for 519 Known Cars
# Support Information for 518 Known Cars
|Make|Model|Package|Support Level|
|---|---|---|:---:|
@@ -287,7 +287,6 @@
|Kia|Forte Non-SCC 2021|No Smart Cruise Control (Non-SCC)|[Community](community)|
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Kia|K5 2021-24|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|[Upstream](#upstream)|
+11 -27
View File
@@ -4,7 +4,7 @@ from opendbc.can import CANPacker
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs
from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
from opendbc.car.ford import fordcan
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
from opendbc.car.ford.values import CarControllerParams, FordFlags
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
# This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md
# (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md.
@@ -64,29 +64,13 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
return apply_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
def apply_creep_compensation(accel: float, v_ego: float) -> float:
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)
@@ -181,7 +165,7 @@ class CarController(CarControllerBase):
can_sends.append(starpilot_fordcan.create_lat_ctl2_msg(
self.packer, self.CAN, 1 if lateral.active else 0,
lateral.ramp_type, lateral.precision_type,
-lateral.curvature, -lateral.curvature_rate, counter, -lateral.path_angle))
-lateral.curvature, -lateral.curvature_rate, counter))
else:
can_sends.append(starpilot_fordcan.create_lat_ctl_msg(
self.packer, self.CAN, lateral.active,
@@ -197,17 +181,16 @@ class CarController(CarControllerBase):
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)
# Compensate for engine creep at low speed.
# Either the ABS does not account for engine creep, or the correction is very slow
# TODO: verify this applies to EV/hybrid
accel = apply_creep_compensation(accel, CS.out.vEgo)
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,
)
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
# however even 3.5 m/s^3 causes some overshoot with a step response.
accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
@@ -227,6 +210,7 @@ class CarController(CarControllerBase):
elif accel_pitch_compensated < 0.0:
self.brake_request = True
stopping = CC.actuators.longControlState == LongCtrlState.stopping
# 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))
+1 -15
View File
@@ -29,9 +29,6 @@ class CarState(CarStateBase):
self.distance_button = 0
self.lc_button = 0
self.cancel_button = False
self.cancel_resume_pressed = False
self.cancel_resume_is_cancel = False
self.lkas_available = False
self.lateral_motion_control = None
self.lateral_control_status = None
@@ -159,14 +156,6 @@ class CarState(CarStateBase):
prev_lc_button = self.lc_button
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
prev_cancel_button = self.cancel_button
cancel_resume_pressed = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclResPress"])
if cancel_resume_pressed and not self.cancel_resume_pressed:
self.cancel_resume_is_cancel = ret.cruiseState.available and ret.cruiseState.enabled
self.cancel_resume_pressed = cancel_resume_pressed
self.cancel_button = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclPress"]) or (
cancel_resume_pressed and self.cancel_resume_is_cancel
)
# lock info
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
@@ -194,13 +183,10 @@ class CarState(CarStateBase):
except KeyError:
self.lateral_motion_control = None
button_events = [
ret.buttonEvents = [
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
]
if self.CP.openpilotLongitudinalControl:
button_events += create_button_events(self.cancel_button, prev_cancel_button, {1: ButtonType.cancel})
ret.buttonEvents = button_events
fp_ret = custom.StarPilotCarState.new_message()
fp_ret.brakeLights = ret.brakePressed
+1 -5
View File
@@ -8,7 +8,7 @@ from opendbc.car.ford.carcontroller import CarController
from opendbc.car.ford.carstate import CarState
from opendbc.car.ford.fordcan import CanBus
from opendbc.car.ford.radar_interface import RadarInterface
from opendbc.car.ford.values import CAR, CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
from opendbc.car.interfaces import CarInterfaceBase
TransmissionType = structs.CarParams.TransmissionType
@@ -40,8 +40,6 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.5]
ret.longitudinalTuning.kpV = [0.]
if candidate == CAR.FORD_MUSTANG_MACH_E_MK1:
ret.longitudinalTuning.kiV = [0.3]
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
@@ -65,8 +63,6 @@ class CarInterface(CarInterfaceBase):
if ret.flags & FordFlags.CANFD:
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
if candidate == CAR.FORD_MUSTANG_MACH_E_MK1:
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.MACH_E_CURVATURE.value
# TRON (SecOC) platforms are not supported
# LateralMotionControl2, ACCDATA are 16 bytes on these platforms
@@ -6,16 +6,15 @@ from hypothesis import settings, given, strategies as st
from parameterized import parameterized
import pytest
from opendbc.car import Bus, gen_empty_fingerprint, structs
from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, gen_empty_fingerprint
from opendbc.can import CANPacker
from opendbc.car.ford import fordcan
from opendbc.car.ford.carcontroller import CarController, FordStockCruiseButton, apply_brake_rate_limit, apply_creep_compensation
from opendbc.car.ford.carstate import CarState
from opendbc.car.ford.carcontroller import FordStockCruiseButton
from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps
from opendbc.car.structs import CarParams, CarState as CarStateStruct
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
from opendbc.car.ford.interface import CarInterface
from opendbc.car.ford.values import CAR, DBC, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
from opendbc.car.ford.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
@@ -39,135 +38,6 @@ def test_stock_cruise_button_ignores_press_with_cruise_master_off():
assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False)
@pytest.mark.parametrize("op_long", (False, True))
@pytest.mark.parametrize("standstill", (False, True))
@pytest.mark.parametrize("cruise_status", (4, 5))
@pytest.mark.parametrize("switch", ("CcAslButtnCnclResPress", "CcAslButtnCnclPress"))
def test_ford_cancel_event_does_not_require_pcm_disengagement(op_long, standstill, cruise_status, switch):
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], op_long, False, False, None)
state = CarState(CP, None)
parsers = state.get_can_parsers(CP)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
for frame, (pressed, status) in enumerate(((True, cruise_status), (True, 3), (False, 3)), start=1):
messages = [
packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {switch: int(pressed)}),
packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
packer.make_can_msg("DesiredTorqBrk", fordcan.CanBus(CP).main, {"VehStop_D_Stat": int(standstill)}),
]
parsers[Bus.pt].update([(frame * 100_000_000, messages)])
ret, _ = state.update(parsers, None)
assert ret.standstill == standstill
expected = [(CarStateStruct.ButtonEvent.Type.cancel, pressed)] if op_long and frame != 2 else []
assert [(event.type, event.pressed) for event in ret.buttonEvents] == expected
@pytest.mark.parametrize("initial_status", (0, 3))
def test_ford_resume_does_not_become_cancel_when_pcm_engages(initial_status):
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
state = CarState(CP, None)
parsers = state.get_can_parsers(CP)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
for frame, (pressed, status) in enumerate(((True, initial_status), (True, 4), (False, 4)), start=1):
messages = [
packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {"CcAslButtnCnclResPress": int(pressed)}),
packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
]
parsers[Bus.pt].update([(frame * 100_000_000, messages)])
ret, _ = state.update(parsers, None)
assert not ret.buttonEvents
def test_mach_e_does_not_apply_engine_creep_compensation():
for accel in (-1.0, -0.1, 0.0, 0.1):
assert apply_creep_compensation(accel, 0.5, CAR.FORD_MUSTANG_MACH_E_MK1,
standstill=False, stopping=False) == accel
assert apply_creep_compensation(0.0, 0.0, CAR.FORD_MUSTANG_MACH_E_MK1,
standstill=True, stopping=True) == -0.6
assert apply_creep_compensation(0.0, 0.5, CAR.FORD_F_150_MK14,
standstill=False, stopping=False) == -0.6
@pytest.mark.parametrize("last", (0.0, 0.35, 0.8))
def test_mach_e_mild_brake_transition_shapes_regen_and_brake_together(last):
accel, gas = apply_brake_rate_limit(-0.5, -0.5, last, 11.0, CAR.FORD_MUSTANG_MACH_E_MK1,
stopping=False, standstill=False, fcw=False)
assert accel == pytest.approx(-0.04)
assert gas == accel
@pytest.mark.parametrize("accel,last", ((-0.2, -0.5), (-0.5, -0.48), (-0.8, -0.4)))
def test_mach_e_mild_brake_ramp_does_not_delay_release(accel, last):
brake, gas = apply_brake_rate_limit(accel, accel, last, 11.0, CAR.FORD_MUSTANG_MACH_E_MK1,
stopping=False, standstill=False, fcw=False)
assert brake == pytest.approx(max(accel, last - 0.04))
assert gas == brake
@pytest.mark.parametrize("accel,speed,stopping,standstill,fcw", (
(-3.5, 11.0, False, False, False),
(-1.0, 11.0, False, False, False),
(-0.5, 4.9, False, False, False),
(-0.5, 11.0, True, False, False),
(-0.5, 11.0, False, True, False),
(-0.5, 11.0, False, False, True),
(0.2, 11.0, False, False, False),
))
def test_mach_e_strong_braking_stops_and_warnings_keep_original_ramp(accel, speed, stopping, standstill, fcw):
brake, gas = apply_brake_rate_limit(accel, accel, 0.0, speed, CAR.FORD_MUSTANG_MACH_E_MK1,
stopping=stopping, standstill=standstill, fcw=fcw)
assert brake == pytest.approx(max(accel, -0.07))
assert gas == accel
@pytest.mark.parametrize("fingerprint", [car for car in CAR if car != CAR.FORD_MUSTANG_MACH_E_MK1])
def test_other_fords_keep_original_brake_requests_and_integral_gain(fingerprint):
brake, gas = apply_brake_rate_limit(-0.5, -0.5, 0.35, 11.0, fingerprint,
stopping=False, standstill=False, fcw=False)
assert brake == pytest.approx(0.28)
assert gas == -0.5
CP = CarInterface.get_params(fingerprint, gen_empty_fingerprint(), [], True, False, False, None)
assert list(CP.longitudinalTuning.kiV) == [0.5]
def test_mach_e_integral_gain_preserves_delay_and_braking_authority():
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
assert list(CP.longitudinalTuning.kiV) == pytest.approx([0.3])
assert CP.longitudinalActuatorDelay == pytest.approx(0.15)
assert CarInterface.get_pid_accel_limits(CP, 11.0, 20.0)[0] == -3.5
def test_mach_e_can_brake_handoff_and_driver_override(monkeypatch):
monkeypatch.setattr("opendbc.car.ford.carcontroller.FordLateralController",
lambda _: SimpleNamespace(update_inputs=lambda: None))
monkeypatch.setattr(fordcan, "create_acc_ui_msg", lambda *args: (0, b"", 0))
monkeypatch.setattr(fordcan, "create_lkas_ui_msg", lambda *args: (0, b"", 0))
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ACCDATA", 50)], 0)
CC = structs.CarControl.new_message()
CC.enabled = True
CC.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid
CS = SimpleNamespace(out=structs.CarState.new_message(), acc_tja_status_stock_values={"Tja_D_Stat": 0})
CS.out.vEgo = 11.0
for frame, active, request, expected in ((2, True, -0.5, -0.04), (4, False, 0.0, 0.0),
(8, True, -0.5, -0.04), (14, True, -3.5, -0.11)):
controller.frame = frame
CC.longActive = active
CC.actuators.accel = request
output, messages = controller.update(CC.as_reader(), CS, frame * 10_000_000, None)
parser.update([(frame * 10_000_000, [msg for msg in messages if msg[0] == 0x186])])
values = parser.vl["ACCDATA"]
assert output.accel == pytest.approx(expected)
assert values["AccBrkTot_A_Rq"] == pytest.approx(expected, abs=0.01)
assert values["AccPrpl_A_Rq"] == pytest.approx(expected if active and request > -1.0 else -5.0, abs=0.01)
assert values["Cmbb_B_Enbl"] == int(active)
assert values["AccResumEnbl_B_Rq"] == int(active)
ECU_ADDRESSES = {
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
@@ -322,12 +192,10 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection():
assert not stock.openpilotLongitudinalControl
assert stock.pcmCruise
assert not (stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL)
assert stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_CURVATURE
assert enhanced.alphaLongitudinalAvailable
assert enhanced.openpilotLongitudinalControl
assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL
assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_CURVATURE
def test_mach_e_can_gps_decode():
-1
View File
@@ -50,7 +50,6 @@ class FordSafetyFlags(IntFlag):
LONG_CONTROL = 1
CANFD = 2
LKA_STEERING = 4
MACH_E_CURVATURE = 8
class FordFlags(IntFlag):
+1 -18
View File
@@ -415,18 +415,6 @@ def supports_bolt_acc_pedal_friction_experiment(CP) -> bool:
)
def update_bolt_acc_pedal_stop_hold(CP, CC, CS, holding: bool) -> bool:
return (
supports_bolt_acc_pedal_friction_experiment(CP) and
CC.longActive and
CC.actuators.longControlState == LongCtrlState.stopping and
not CC.cruiseControl.resume and
not CS.out.gasPressed and
not CS.out.brakePressed and
(holding or CS.out.standstill)
)
def get_bolt_acc_pedal_friction_brake(apply_brake, full_brake_accel, v_ego, params) -> int:
if apply_brake <= 0:
return 0
@@ -595,7 +583,6 @@ class CarController(CarControllerBase):
self.gm_auto_hold_enabled = False
self.bolt_acc_pedal_friction_release_frames = 0
self.bolt_acc_pedal_friction_low_speed_active = False
self.bolt_acc_pedal_stop_hold = False
self.truck_friction_brake_active = False
self.truck_follow_accel = 0.0
@@ -785,7 +772,6 @@ class CarController(CarControllerBase):
actuators = CC.actuators
self.aego = CS.out.aEgo
accel = actuators.accel
self.bolt_acc_pedal_stop_hold = update_bolt_acc_pedal_stop_hold(self.CP, CC, CS, self.bolt_acc_pedal_stop_hold)
press_regen_paddle = False
auto_hold_enabled = supports_volt_auto_hold(self.CP, self.gm_auto_hold_enabled)
volt_one_pedal_supported = supports_volt_one_pedal(
@@ -1191,9 +1177,6 @@ class CarController(CarControllerBase):
else:
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET))
if self.CP.enableGasInterceptorDEPRECATED:
if self.bolt_acc_pedal_stop_hold:
interceptor_gas_cmd = 0.0
self.pedal_steady = 0.0
can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx))
if bolt_acc_pedal_friction_experiment:
friction_brake_bus = get_friction_brake_bus(self.CP)
@@ -1214,7 +1197,7 @@ class CarController(CarControllerBase):
# cannot linger after a disengage or main-off event.
if should_send_bolt_acc_pedal_friction:
can_sends.append(gmcan.create_friction_brake_command(
self.packer_ch, friction_brake_bus, experiment_brake, idx, bolt_acc_pedal_friction_main_on and CC.longActive,
self.packer_ch, friction_brake_bus, experiment_brake, idx, bolt_acc_pedal_friction_main_on,
near_stop, at_full_stop, self.CP))
if self.CP.carFingerprint not in CC_ONLY_CAR:
friction_brake_bus = get_friction_brake_bus(self.CP)
@@ -70,7 +70,6 @@ from opendbc.car.gm.carcontroller import (
supports_bolt_acc_pedal_friction_experiment,
supports_volt_auto_hold,
supports_volt_one_pedal,
update_bolt_acc_pedal_stop_hold,
use_interceptor_sng_launch,
)
from opendbc.car.gm.gmcan import get_friction_brake_mode
@@ -315,150 +314,6 @@ def test_fixed_stopping_brake_stays_enabled_for_normal_acc_path():
assert not should_use_fixed_stopping_brake(CP, True, True, True)
def _bolt_stop_hold_case():
CP = SimpleNamespace(
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
openpilotLongitudinalControl=True,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
)
CC = SimpleNamespace(
longActive=True,
actuators=SimpleNamespace(longControlState=structs.CarControl.Actuators.LongControlState.stopping),
cruiseControl=SimpleNamespace(resume=False),
)
CS = SimpleNamespace(out=SimpleNamespace(standstill=False, gasPressed=False, brakePressed=False))
return CP, CC, CS
def test_bolt_pedal_stop_hold_preserves_approach_until_standstill():
CP, CC, CS = _bolt_stop_hold_case()
assert not update_bolt_acc_pedal_stop_hold(CP, CC, CS, False)
CS.out.standstill = True
assert update_bolt_acc_pedal_stop_hold(CP, CC, CS, False)
def test_bolt_pedal_stop_hold_stays_latched_when_grade_creep_clears_standstill():
CP, CC, CS = _bolt_stop_hold_case()
CS.out.standstill = True
holding = update_bolt_acc_pedal_stop_hold(CP, CC, CS, False)
CS.out.standstill = False
assert update_bolt_acc_pedal_stop_hold(CP, CC, CS, holding)
@pytest.mark.parametrize("release", ["long_off", "pid", "starting", "off", "resume", "gas", "brake"])
def test_bolt_pedal_stop_hold_releases_for_launch_or_driver_takeover(release):
CP, CC, CS = _bolt_stop_hold_case()
CS.out.standstill = True
if release == "long_off":
CC.longActive = False
elif release in ("pid", "starting", "off"):
CC.actuators.longControlState = getattr(structs.CarControl.Actuators.LongControlState, release)
elif release == "resume":
CC.cruiseControl.resume = True
elif release == "gas":
CS.out.gasPressed = True
else:
CS.out.brakePressed = True
assert not update_bolt_acc_pedal_stop_hold(CP, CC, CS, True)
@pytest.mark.parametrize("platform", [
CAR.CHEVROLET_BOLT_ACC_2022_2023,
CAR.CHEVROLET_BOLT_CC_2022_2023,
CAR.CHEVROLET_BOLT_CC_2018_2021,
CAR.CHEVROLET_BOLT_CC_2017,
CAR.CHEVROLET_VOLT,
CAR.CHEVROLET_MALIBU_HYBRID_CC,
CAR.CHEVROLET_SILVERADO,
])
def test_bolt_pedal_stop_hold_does_not_change_other_profiles(platform):
CP, CC, CS = _bolt_stop_hold_case()
CP.carFingerprint = platform
CS.out.standstill = True
assert not update_bolt_acc_pedal_stop_hold(CP, CC, CS, True)
@pytest.mark.parametrize("disabled", ["long", "pedal", "flag"])
def test_bolt_pedal_stop_hold_requires_active_pedal_long_configuration(disabled):
CP, CC, CS = _bolt_stop_hold_case()
CS.out.standstill = True
if disabled == "long":
CP.openpilotLongitudinalControl = False
elif disabled == "pedal":
CP.enableGasInterceptorDEPRECATED = False
else:
CP.flags = 0
assert not update_bolt_acc_pedal_stop_hold(CP, CC, CS, True)
def test_bolt_pedal_sender_zeros_stopped_offset_without_changing_brake_fade(monkeypatch):
import opendbc.car.gm.carcontroller as controller_module
monkeypatch.setattr(controller_module, "Params", lambda: SimpleNamespace(get_bool=lambda _key: False, get=lambda _key: None))
CP = structs.CarParams()
CP.carFingerprint = CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL
CP.openpilotLongitudinalControl = True
CP.enableGasInterceptorDEPRECATED = True
CP.flags = GMFlags.PEDAL_LONG.value
CP.networkLocation = CarParams.NetworkLocation.fwdCamera
CP.transmissionType = CarParams.TransmissionType.direct
CP.mass = 1669.0
CP.wheelbase = 2.63779
CP.vEgoStarting = 0.35
CP.vEgoStopping = 0.25
CP.stopAccel = -0.25
CP.radarUnavailable = True
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.095]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.07]
controller = CarController({}, CP)
controller.frame = 0
state = structs.CarState()
state.vEgo = 0.2
state.gearShifter = structs.CarState.GearShifter.drive
state.cruiseState.available = True
CS = SimpleNamespace(
out=state.as_reader(), auto_hold_armed=False, auto_hold_engaged=False, auto_hold_drive_time=0.0,
one_pedal_drive_time=0.0, regen_release_timer=0.0, loopback_lka_steering_cmd_updated=False,
loopback_lka_steering_cmd_ts_nanos=0, cam_lka_steering_cmd_counter=0, pt_lka_steering_cmd_counter=0,
pcm_acc_status=AccState.OFF, buttons_counter=0, cruise_buttons=0,
pscm_status={key: 0 for key in ("HandsOffSWDetectionMode", "HandsOffSWlDetectionStatus", "LKATorqueDeliveredStatus",
"LKADriverAppldTrq", "LKATorqueDelivered", "LKATotalTorqueDelivered", "RollingCounter", "PSCMStatusChecksum")},
)
control = structs.CarControl()
control.enabled = True
control.longActive = True
control.actuators.accel = -0.253
control.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping
toggles = SimpleNamespace(volt_sng=False)
def send():
controller.frame = (controller.frame + 3) // 4 * 4
_, frames = controller.update(control.as_reader(), CS, 2_000_000_000 + controller.frame * 10_000_000, toggles)
pedal = next(dat for address, dat, _bus in frames if address == 0x200)
brake = next(dat for address, dat, _bus in frames if address == 0x315)
assert brake[0] & 0xF == 0 and brake[1] == 0
return bool(pedal[4] & 0x80)
assert send()
state.vEgo = 0.05
state.standstill = True
CS.out = state.as_reader()
assert not send()
assert controller.pedal_steady == 0.0
state.vEgo = 0.244
state.standstill = False
CS.out = state.as_reader()
assert not send()
control.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
control.actuators.accel = 0.55
assert send()
assert not controller.bolt_acc_pedal_stop_hold
def test_stock_cancel_is_suppressed_when_acc_is_faulted():
CP = SimpleNamespace(carFingerprint=CAR.CHEVROLET_VOLT_CAMERA)
cs = _cs(True, AccState.FAULTED)
-60
View File
@@ -8,7 +8,6 @@ from typing import Any
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.ford.values import CAR as FORD_CAR
from opendbc.car.gm.values import CAR as GM_CAR
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN_CAR
CarGpsSample = dict[str, Any]
@@ -125,66 +124,12 @@ def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample
}
def parse_volkswagen_taos_can_gps(position: Mapping[str, float], motion: Mapping[str, float],
altitude: Mapping[str, float], status: Mapping[str, float]) -> CarGpsSample | None:
packet_ids = (position["GNSS_Nachrichtenpaket_ID1"], motion["GNSS_Nachrichtenpaket_ID2"],
altitude["GNSS_Nachrichtenpaket_ID4"], status["GNSS_Nachrichtenpaket_ID5"])
if len(set(packet_ids)) != 1:
return None
timestamp_ms = int(status["GNSS_UTC_Zeit"]) * 1000
if timestamp_ms == 0:
return None
latitude = float(position["GNSS_LatitudeMagnitude"])
longitude = float(position["GNSS_LongitudeMagnitude"])
coordinates_valid = (math.isfinite(latitude) and math.isfinite(longitude) and
0.0 <= latitude <= 90.0 and 0.0 <= longitude <= 180.0 and (latitude != 0.0 or longitude != 0.0))
if coordinates_valid:
latitude *= -1.0 if position["GNSS_LatitudeSouth"] else 1.0
longitude *= -1.0 if position["GNSS_LongitudeWest"] else 1.0
else:
latitude = longitude = 0.0
satellite_count = int(status["GNSS_Genutzte_Satelliten"])
hemisphere_validated = position["GNSS_LatitudeSouth"] == 0 and position["GNSS_LongitudeWest"] == 1
has_fix = (coordinates_valid and hemisphere_validated and position["GNSS_PositionStatus"] == 3 and status["GNSS_Empfaenger_Status"] == 1 and
bool(status["GNSS_GPS_in_Nutzung"] or status["GNSS_GLONASS_in_Nutzung"]) and 4 <= satellite_count <= 31)
speed = float(motion["GNSS_Speed"])
bearing = float(motion["GNSS_Bearing"])
speed_valid = math.isfinite(speed) and 0.0 <= speed <= 127.5
bearing_valid = math.isfinite(bearing) and 0.0 <= bearing < 360.0
speed = speed if has_fix and speed_valid else 0.0
bearing = bearing if has_fix and bearing_valid else 0.0
height = float(altitude["GNSS_Ortung_Hoehe"])
height_valid = math.isfinite(height) and -500.0 <= height <= 7686.0
heading_rad = math.radians(bearing)
return {
"latitude": latitude,
"longitude": longitude,
"altitude": height if has_fix and height_valid else 0.0,
"speed": speed,
"bearingDeg": bearing,
"horizontalAccuracy": 20.0,
"unixTimestampMillis": timestamp_ms,
"verticalAccuracy": 50.0 if height_valid else 500.0,
"bearingAccuracyDeg": 10.0 if has_fix and bearing_valid and speed > 1.0 else 180.0,
"speedAccuracy": 1.5 if speed_valid and bearing_valid else 100.0,
"hasFix": has_fix,
"satelliteCount": satellite_count if 0 <= satellite_count <= 31 else 0,
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0] if bearing_valid else [0.0, 0.0, 0.0],
}
FORD_MACH_E_GPS_MESSAGES = (
"APIMGPS_Data_Nav_1_FD1",
"APIMGPS_Data_Nav_2_FD1",
"APIMGPS_Data_Nav_3_FD1",
)
CHEVROLET_BOLT_GPS_MESSAGES = ("TCICOnStarGPSPosition",)
VOLKSWAGEN_TAOS_GPS_MESSAGES = ("GNSS_01", "GNSS_02", "GNSS_04", "GNSS_05")
CHEVROLET_BOLT_GPS_CARS = (
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
@@ -196,11 +141,6 @@ CHEVROLET_BOLT_GPS_CARS = (
CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
VOLKSWAGEN_CAR.VOLKSWAGEN_TAOS_MK1: CarGpsConfig(
brand="volkswagen",
messages=VOLKSWAGEN_TAOS_GPS_MESSAGES,
decoder=parse_volkswagen_taos_can_gps,
),
FORD_CAR.FORD_MUSTANG_MACH_E_MK1: CarGpsConfig(
brand="ford",
messages=FORD_MACH_E_GPS_MESSAGES,
@@ -1065,27 +1065,21 @@ FW_VERSIONS = {
CAR.HONDA_CITY_7G: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-T14-B030\x00\x00',
b'39990-T14-B510\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-T14-M110\x00\x00',
b'38897-T14-M210\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-T00-B830\x00\x00',
b'77959-T14-B810\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36161-T14-P050\x00\x00',
b'8S102-T14-P020\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-T14-B030\x00\x00',
b'57114-T14-M510\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
b'28101-63B-M420\x00\x00',
b'28101-63B-M510\x00\x00',
],
},
CAR.HONDA_PASSPORT_4G: {
+1 -1
View File
@@ -277,7 +277,7 @@ class CAR(Platforms):
flags=HondaFlags.BOSCH_RADARLESS,
)
HONDA_CITY_7G = HondaBoschPlatformConfig(
[HondaCarDocs("Honda City (Brazil only) 2023-25", "All")],
[HondaCarDocs("Honda City (Brazil only) 2023", "All")],
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.6, steerRatio=19.0, centerToFrontRatio=0.41, minSteerSpeed=23. * CV.KPH_TO_MS),
{Bus.pt: 'honda_bosch_radarless_generated'},
flags=HondaFlags.BOSCH_RADARLESS,
@@ -42,17 +42,9 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2
IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
EV9_CANFD_SCC_DECEL_STEP = 10.0 / 50.0
RAY_PEDAL_COMMAND_CAP = 0.70
RAY_PEDAL_CAP_SPEED_BP = [0.0, 12.0, 20.0, 25.0]
RAY_PEDAL_CAP_V = [0.55, 0.55, 0.65, RAY_PEDAL_COMMAND_CAP]
RAY_PEDAL_OFFSET_SPEED_BP = [0.0, 2.0, 4.0, 8.0, 12.0, 16.0, 20.0, 25.0]
RAY_PEDAL_OFFSET_V = [0.08, 0.13, 0.20, 0.32, 0.42, 0.51, 0.565, 0.60]
RAY_PEDAL_ACCEL_GAIN = 0.22
RAY_PEDAL_DECEL_GAIN = 0.55
RAY_PEDAL_RATE_UP = 0.02
RAY_PEDAL_COMMAND_CAP = 0.35 # Ray firmware voltage scaling is route-derived; validate before raising.
RAY_PEDAL_RATE_UP = 0.012 # per 25 Hz command (0.30 normalized pedal per second)
RAY_PEDAL_RATE_DOWN = 0.06
RAY_PEDAL_OVERSPEED_CUTOFF = 0.5
RAY_PEDAL_TAPER_BELOW_TARGET = 0.75
GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80]
GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
@@ -97,25 +89,6 @@ BLINDSPOT_WARNING_FLASH_ON_SAMPLES = 16
BLINDSPOT_WARNING_SOUND_SAMPLES = 36
def get_ray_pedal_target(accel: float, v_ego: float, set_speed: float) -> float:
if not all(np.isfinite(x) for x in (accel, v_ego, set_speed)) or set_speed < 1.0:
return 0.0
speed_error = set_speed - v_ego
if speed_error <= -RAY_PEDAL_OVERSPEED_CUTOFF:
return 0.0
pedal_offset = float(np.interp(v_ego, RAY_PEDAL_OFFSET_SPEED_BP, RAY_PEDAL_OFFSET_V))
pedal_cap = float(np.interp(v_ego, RAY_PEDAL_CAP_SPEED_BP, RAY_PEDAL_CAP_V))
pedal_gain = RAY_PEDAL_DECEL_GAIN if accel < 0.0 else RAY_PEDAL_ACCEL_GAIN
target = float(np.clip(pedal_offset + accel * pedal_gain, 0.0, pedal_cap))
if speed_error < 0.0:
target *= float(np.clip(0.65 * (1.0 + speed_error / RAY_PEDAL_OVERSPEED_CUTOFF), 0.0, 1.0))
elif speed_error < RAY_PEDAL_TAPER_BELOW_TARGET:
target *= 0.65 + 0.35 * speed_error / RAY_PEDAL_TAPER_BELOW_TARGET
return target
def egmp_dynamic_longitudinal_tuning(CP) -> bool:
return CP.carFingerprint in (CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV9, CAR.HYUNDAI_IONIQ_5_PE) or \
kia_ev6_gt_line_longitudinal_tuning(
@@ -795,8 +768,7 @@ class CarController(CarControllerBase):
lead_data = self._can_lead_data.update(lead_distance, lead_rel_speed, lead_visible)
# HUD messages
stinger_hud_enabled = CC.enabled or (self.CP.carFingerprint == CAR.KIA_STINGER_2022 and CC.latActive)
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(stinger_hud_enabled, self.car_fingerprint,
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
if blended_hda2:
@@ -837,12 +809,12 @@ class CarController(CarControllerBase):
# Button messages
if not self.long_active_ecu:
if self._ray_pedal and CC.enabled and CS.out.cruiseState.enabled:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
elif self._ray_pedal and CC.longActive and CS.out.cruiseState.enabled:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
self.last_button_frame = self.frame
elif self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
elif CC.cruiseControl.resume and not self._ray_pedal:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
@@ -857,14 +829,14 @@ class CarController(CarControllerBase):
if self._ray_pedal and self.frame % 4 == 0:
pedal_ready = CS.ray_pedal_valid and CS.ray_pedal_state == 0
pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and
not CS.out.gasPressed and not CS.out.brakePressed)
not CS.out.gasPressed and not CS.out.brakePressed and
not CS.out.cruiseState.enabled and CS.out.vEgo >= self.CP.minEnableSpeed)
if pedal_active:
target = get_ray_pedal_target(accel, CS.out.vEgo, hud_control.setSpeed)
if target <= 0.001:
self._ray_pedal_gas_last = 0.0
else:
next_gas = rate_limit(target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP)
self._ray_pedal_gas_last = min(next_gas, target)
target = float(np.clip(accel / CarControllerParams.ACCEL_MAX * RAY_PEDAL_COMMAND_CAP,
0.0, RAY_PEDAL_COMMAND_CAP))
self._ray_pedal_gas_last = rate_limit(
target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP,
)
else:
self._ray_pedal_gas_last = 0.0
can_sends.append(create_gas_interceptor_command(
@@ -966,7 +938,8 @@ class CarController(CarControllerBase):
CS.stock_lfa_msg if preserve_stock_lfa_status else None,
CS.stock_lkas_msg if preserve_stock_lkas else None,
lka_icon=lka_icon,
longitudinal_active=lfa_longitudinal_active))
longitudinal_active=lfa_longitudinal_active,
v_ego=getattr(getattr(CS, "out", None), "vEgo", None)))
direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault
inactive_steering_angle = float(np.clip(CS.angle_steering_angle,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
@@ -1053,12 +1026,7 @@ class CarController(CarControllerBase):
left_sound_active=left_warning.sound_active, right_sound_active=right_warning.sound_active,
)
else:
host_speed = getattr(CS.out, "vEgoRaw", None) \
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else None
adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame,
car_fingerprint=self.CP.carFingerprint,
drive_gear=drive_gear,
v_ego=host_speed)
adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)
can_sends.extend(adrv_messages)
# The front radar treats ADAS_DRV's 0x100 broadcast as its host heartbeat
# and stops publishing object tracks when it disappears.
@@ -26,33 +26,6 @@ ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
class Ev6AolArmingState:
def __init__(self, lkas_on_engage: bool):
self.lkas_on_engage = lkas_on_engage
self.main_on = False
self.lkas_on = False
self.prev_main_button = False
self.prev_lkas_button = False
self.prev_cruise_button = Buttons.NONE
def update(self, main_button: bool, lkas_button: bool, cruise_button: int):
if main_button and not self.prev_main_button:
self.main_on = not self.main_on
if lkas_button and not self.prev_lkas_button:
self.lkas_on = not self.lkas_on
if (self.lkas_on_engage and cruise_button != self.prev_cruise_button and
self.prev_cruise_button in (Buttons.SET_DECEL, Buttons.RES_ACCEL)):
self.lkas_on = True
self.prev_main_button = main_button
self.prev_lkas_button = lkas_button
self.prev_cruise_button = cruise_button
@property
def authorized(self) -> bool:
return self.main_on or self.lkas_on
IONIQ_6_BLINDSPOT_RIGHT_MASK = 0x08
IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10
CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1
@@ -165,13 +138,6 @@ class CarState(CarStateBase):
self.buttons_counter = 0
self.main_cruise_on = False
self.main_cruise_tracking = bool(getattr(FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING)
self.ev6_aol_arming = None
if CP.carFingerprint == CAR.KIA_EV6 and CP.openpilotLongitudinalControl and not CP.pcmCruise:
lkas_on_engage = any(
config.safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE
for config in (*CP.safetyConfigs, *FPCP.safetyConfigs)
)
self.ev6_aol_arming = Ev6AolArmingState(lkas_on_engage)
if CP.carFingerprint == CAR.KIA_RAY_EV:
self.ray_pedal_state = 5
self.ray_pedal_valid = False
@@ -215,10 +181,6 @@ class CarState(CarStateBase):
# Main button also can trigger an engagement on these cars
return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons)
@property
def ev6_aol_authorized(self) -> bool:
return self.ev6_aol_arming is not None and self.ev6_aol_arming.authorized
def update_main_cruise(self, ret: structs.CarState,
button_events: list[structs.CarState.ButtonEvent] | None = None) -> bool:
button_events = ret.buttonEvents if button_events is None else button_events
@@ -678,13 +640,6 @@ class CarState(CarStateBase):
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}),
*create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}),
*create_button_events(self.left_paddle, prev_left_paddle, {1: ButtonType.altButton2})]
if self.ev6_aol_arming is not None:
buttons = cp.vl_all[self.cruise_btns_msg_canfd]
for main, lkas, cruise in zip(buttons["ADAPTIVE_CRUISE_MAIN_BTN"], buttons["LDA_BTN"], buttons["CRUISE_BUTTONS"], strict=True):
self.ev6_aol_arming.update(bool(main), bool(lkas), int(cruise))
if not ret.cruiseState.available:
self.ev6_aol_arming.main_on = False
if self.CP.openpilotLongitudinalControl and (self.CP.carFingerprint == CAR.KIA_EV9 or self.main_cruise_tracking):
ret.cruiseState.available = self.update_main_cruise(ret)
@@ -1335,7 +1335,6 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708',
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708',
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ',
@@ -1557,7 +1556,6 @@ FW_VERSIONS = {
},
CAR.HYUNDAI_STARIA_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00US4 MFC AT AUS RHD 1.00 1.04 99211-CG000 210819',
b'\xf1\x00US4 MFC AT KOR LHD 1.00 1.06 99211-CG000 230524',
],
(Ecu.fwdRadar, 0x7d0, None): [
@@ -7,36 +7,27 @@ from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.crc import CRC16_XMODEM
from opendbc.car.hyundai.values import HyundaiFlags, CAR, CANFD_ALT_BUTTONS_RESUME_CAR
_adrv_0x51_templates: dict[CAR, bytes] = {}
# Ioniq 6 EPS damping, matched to the car's own LFA. With the stock ADAS driving, the Ioniq 6 steers
# through LFA (0x12A) and sends ONLY a damping value in LKAS_ALT (0x110 byte 8, Hyundai name
# Damping_Gain); both are scheduled purely on speed (drive 00000b41: no dependence on angle, torque or
# hands). openpilot sent a fixed 100 in LFA and never set LKAS_ALT's, so the EPS got 0 there. The
# breakpoints are the stock medians per 5 mph band (centers) while the stock system is steering,
# rising steadily to 163 (LFA) / 154 (LKAS_ALT) at 85-90 mph. openpilot's highway weave matched the
# stock system at 45-60 mph and grew past it above 60 mph, exactly where the stock damping pulls away.
# Below ~35 mph the previous values are kept (LFA floor 100, LKAS_ALT 0) so low-speed turn tuning
# is untouched. The panda only checks torque/steer_req on these messages, not the damping byte.
_DAMP_MPH = [37.5, 42.5, 47.5, 52.5, 57.5, 62.5, 67.5, 72.5, 77.5, 82.5, 87.5]
IONIQ_6_LFA_DAMP_BP = [mph * CV.MPH_TO_MS for mph in _DAMP_MPH]
IONIQ_6_LFA_DAMP_V = [100., 105., 107., 112., 116., 119., 133., 145., 151., 157., 163.]
IONIQ_6_LKAS_ALT_DAMP_BP = [mph * CV.MPH_TO_MS for mph in [35.0] + _DAMP_MPH[1:]]
IONIQ_6_LKAS_ALT_DAMP_V = [0., 125., 127., 132., 136., 139., 142., 144., 146., 150., 154.]
def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
if car_fingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN):
return
if dat is None:
_adrv_0x51_templates.pop(car_fingerprint, None)
elif len(dat) == 32 and any(dat[3:]):
_adrv_0x51_templates[car_fingerprint] = bytes(dat)
def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False,
v_ego: float | None = None):
template = _adrv_0x51_templates.get(car_fingerprint)
if template is None:
return packer.make_can_msg("ADRV_0x51", CAN.ACAN, {})
dat = bytearray(template)
dat[2] = (template[2] + frame + 1) & 0xFF
dat[3] = (dat[3] & ~0x1) | int(drive_gear)
if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN and v_ego is not None and np.isfinite(v_ego):
speed_raw = int(np.clip(round(v_ego * 100.0), 0, 65534))
dat[8:10] = speed_raw.to_bytes(2, "little")
crc = hkg_can_fd_checksum(0x51, None, dat)
dat[0] = crc & 0xFF
dat[1] = (crc >> 8) & 0xFF
return CanData(0x51, bytes(dat), CAN.ACAN)
def get_ioniq_6_damp_factors(v_ego: float) -> tuple[int, int]:
"""(LFA DAMP_FACTOR, LKAS_ALT DAMP_FACTOR) for the Ioniq 6 at this speed."""
lfa = float(np.interp(v_ego, IONIQ_6_LFA_DAMP_BP, IONIQ_6_LFA_DAMP_V))
lkas_alt = float(np.interp(v_ego, IONIQ_6_LKAS_ALT_DAMP_BP, IONIQ_6_LKAS_ALT_DAMP_V))
return int(round(lfa)), int(round(lkas_alt))
def _set_value(msg: bytearray, sig, ival: int) -> None:
@@ -131,7 +122,7 @@ def create_angle_adas_cmd(packer, CAN, apply_angle: float, lat_active: bool, tor
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle,
lfa_base_values=None, lkas_base_values=None, lka_icon=None,
longitudinal_active=None):
longitudinal_active=None, v_ego=None):
if lka_icon is None:
lka_icon = 2 if enabled else 1
if longitudinal_active is None:
@@ -173,6 +164,10 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
lfa_values["NEW_SIGNAL_2"] = 0
lfa_values["DAMP_FACTOR"] = 100 # can potentially tuned for better perf [3, 200]
if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and v_ego is not None and \
not CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
lfa_values["DAMP_FACTOR"], lkas_values["DAMP_FACTOR"] = get_ioniq_6_damp_factors(v_ego)
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT:
lkas_values["ADAS_StrAnglReqVal"] = apply_angle
lkas_values["LKAS_ANGLE_ACTIVE"] = 2 if lat_active else 1
@@ -819,14 +814,15 @@ def create_fca_warning_light(packer, CAN, frame):
return ret
def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False,
v_ego=None):
def create_adrv_messages(packer, CAN, frame, blended_hda2=False):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear, v_ego))
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
if blended_hda2:
return ret
+4 -26
View File
@@ -2,7 +2,6 @@ import time
# Provenance: portions of HKG angle integration are adapted from sunnypilot/opendbc's
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
from opendbc.car import get_safety_config, structs, uds
from opendbc.car.hyundai import hyundaicanfd
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
@@ -201,8 +200,6 @@ class CarInterface(CarInterfaceBase):
if ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ANGLE_STEERING.value
if candidate == CAR.KIA_SPORTAGE_HEV_2026:
ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.CANFD_NO_STOCK_LKA.value
if candidate == CAR.HYUNDAI_IONIQ_6:
# Keep lateral active through stops: zeroing torque at standstill dropped the
# stop-turn hold and forced a rate-limit re-ramp from zero on every pull-away
@@ -315,13 +312,8 @@ class CarInterface(CarInterfaceBase):
ret.pcmCruise = False
ret.radarUnavailable = True
ret.autoResumeSng = False
ret.minEnableSpeed = -1.0
ret.minEnableSpeed = 5.0 # pedal-only: no commanded friction brake/standstill hold
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
ret.longitudinalTuning.kpBP = [0.0, 5.0, 15.0, 35.0]
ret.longitudinalTuning.kpV = [0.095, 0.085, 0.065, 0.050]
ret.longitudinalTuning.kiBP = [0.0, 3.0, 6.0, 35.0]
ret.longitudinalTuning.kiV = [0.07, 0.10, 0.15, 0.24]
ret.longitudinalTuning.kfDEPRECATED = 1.0
# Car specific configuration overrides
@@ -338,8 +330,8 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.HYUNDAI_ELANTRA_2021:
ret.longitudinalActuatorDelay = 0.22
ret.stopAccel = -1.1
ret.stoppingDecelRate = 0.55
ret.stopAccel = -0.85
ret.stoppingDecelRate = 0.35
if candidate == CAR.HYUNDAI_ELANTRA_HEV_2024:
ret.longitudinalActuatorDelay = 0.22
@@ -399,25 +391,11 @@ class CarInterface(CarInterfaceBase):
skip_disable_ecu = True
if not skip_disable_ecu:
disable_can_recv = can_recv
if CP.carFingerprint in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and can_recv is not None:
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None)
base_can_recv = can_recv
adrv_bus = CanBus(CP).ACAN
def disable_can_recv(*args, **kwargs):
packets = base_can_recv(*args, **kwargs)
for packet in packets or []:
for msg in packet:
if msg.src == adrv_bus and msg.address == 0x51:
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, msg.dat)
return packets
# Try ECU disable. If it succeeds (IGN-ON mode), enable longitudinal.
# If it fails (READY mode returns NRC 0x22, or timeout), strip LONG safety flag
# so panda forwards stock SCC messages normally (lateral-only mode).
ecu_log(f"=== ECU DISABLE attempt: addr=0x{addr:x}, bus={bus} ===")
ecu_disabled = disable_ecu(disable_can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control,
ecu_disabled = disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control,
reset=bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED))
if CP.carFingerprint in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE):
@@ -51,11 +51,6 @@ RADAR_TRACK_CONFIGS = {
def get_radar_track_config(car_fingerprint, flags: int = 0) -> RadarTrackConfig | None:
if (car_fingerprint == CAR.HYUNDAI_PALISADE_2023 and
flags & HyundaiFlags.CAN_CANFD_BLENDED and flags & HyundaiFlags.CANFD_LKA_STEERING):
return RadarTrackConfig(GV70_RADAR_START_ADDR, GV70_RADAR_MSG_COUNT, "palisade_210", bus=0,
frequency=20, expected_length=32, dbc_name=GV70_RADAR_DBC)
if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
return RadarTrackConfig(GV70_RADAR_START_ADDR, GV70_RADAR_MSG_COUNT, "gv70_210", bus=0,
frequency=20, expected_length=32, dbc_name=GV70_RADAR_DBC)
@@ -78,7 +73,7 @@ def radar_tracks_available(radar_config: RadarTrackConfig | None, fingerprint) -
if radar_config is None:
return False
if radar_config.radar_type in ("gv70_210", "palisade_210"):
if radar_config.radar_type == "gv70_210":
return all(fingerprint[radar_config.bus].get(addr) == radar_config.expected_length
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.msg_count))
@@ -241,7 +236,7 @@ class RadarInterface(RadarInterfaceBase):
del self.pts[track_key]
continue
if radar_type in ("gv70_210", "palisade_210"):
if radar_type == "gv70_210":
for i in ("1", "2"):
track_key = addr * 2 + int(i) - 1
valid = msg[f"{i}_STATE"] in (3, 4)
@@ -21,7 +21,7 @@ from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY
preserve_stock_canfd_lfa_status, \
preserve_stock_canfd_lkas_status, \
suppress_redundant_gv70_brake_cancel
from opendbc.car.hyundai.carstate import CarState, Ev6AolArmingState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
get_canfd_cruise_available
from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_communication_control_request
from opendbc.car.hyundai import hyundaican, hyundaicanfd
@@ -130,73 +130,6 @@ def get_test_toggles() -> SimpleNamespace:
class TestHyundaiFingerprint:
@pytest.mark.parametrize("candidate, alpha_long, needs_arming", (
(CAR.KIA_EV6, True, True), (CAR.KIA_EV6, False, False),
(CAR.KIA_EV6_2025, True, False), (CAR.KIA_EV9, True, False),
(CAR.HYUNDAI_IONIQ_5, True, False), (CAR.HYUNDAI_IONIQ_6, True, False),
))
def test_ev6_aol_arming_is_vehicle_specific(self, candidate, alpha_long, needs_arming):
toggles = get_test_toggles()
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(candidate, gen_empty_fingerprint(), [], CP, toggles)
CS = CarState(CP, FPCP)
assert (CS.ev6_aol_arming is not None) == needs_arming
assert not CS.ev6_aol_authorized
@pytest.mark.parametrize("alt_buttons", (False, True))
@pytest.mark.parametrize("lkas_on_engage", (False, True))
def test_ev6_aol_uses_all_physical_button_samples(self, alt_buttons, lkas_on_engage):
toggles = get_test_toggles()
toggles.always_on_lateral_lkas = lkas_on_engage
fingerprint = gen_empty_fingerprint()
if not alt_buttons:
fingerprint[0][0x1CF] = 8
CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], True, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_EV6, fingerprint, [], CP, toggles)
CS = CarState(CP, FPCP)
parsers = CS.get_can_parsers(CP)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
buttons_msg = CS.cruise_btns_msg_canfd
frame = 0
def update(samples, acc_available=True):
nonlocal frame
frame += 1
msgs = [packer.make_can_msg(buttons_msg, CanBus(CP).ECAN, {
"ADAPTIVE_CRUISE_MAIN_BTN": main, "LDA_BTN": lkas, "CRUISE_BUTTONS": cruise,
}) for main, lkas, cruise in samples]
msgs.append(packer.make_can_msg("TCS", CanBus(CP).ECAN, {"ACCEnable": 0 if acc_available else 1}))
parsers[Bus.pt].update([(frame * 10_000_000, msgs)])
CS.update(parsers, toggles)
return CS.ev6_aol_authorized
assert not update([(0, 0, Buttons.NONE)])
assert update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
assert update([])
assert not update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
assert update([(1, 0, Buttons.NONE), (1, 0, Buttons.NONE)])
assert update([(0, 0, Buttons.NONE)])
assert not update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)])
assert update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)])
assert not update([], acc_available=False)
assert not update([])
assert not update([(0, 0, Buttons.SET_DECEL)])
assert update([(0, 0, Buttons.NONE)]) == lkas_on_engage
assert update([]) == lkas_on_engage
@pytest.mark.parametrize("button", (Buttons.SET_DECEL, Buttons.RES_ACCEL))
@pytest.mark.parametrize("lkas_on_engage", (False, True))
def test_ev6_aol_engagement_latch_matches_safety_flag(self, button, lkas_on_engage):
state = Ev6AolArmingState(lkas_on_engage)
state.update(False, False, button)
assert not state.authorized
state.update(False, False, Buttons.NONE)
assert state.authorized == lkas_on_engage
state.update(False, False, Buttons.CANCEL)
assert state.authorized == lkas_on_engage
state.update(False, True, Buttons.NONE)
assert state.authorized != lkas_on_engage
def test_egmp_communication_control_paths(self):
stock_request = bytes([0x28, 0x83, 0x01])
radar_keepalive_request = bytes([0x28, 0x01, 0x01])
@@ -215,60 +148,6 @@ class TestHyundaiFingerprint:
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
def test_ev6_adrv_0x51_replays_factory_payload(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.EV)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
can_bus = CanBus(CP)
factory = bytes.fromhex("88ed2e091700ffff5e0d0000012006ff021c2200000000000800000010000000")
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, factory)
try:
address, dat, bus = hyundaicanfd.create_adrv_0x51(packer, can_bus, 7, CAR.KIA_EV6, drive_gear=True)
_, parked_dat, _ = hyundaicanfd.create_adrv_0x51(packer, can_bus, 8, CAR.KIA_EV6, drive_gear=False)
_, other_dat, _ = hyundaicanfd.create_adrv_0x51(packer, can_bus, 7, CAR.HYUNDAI_IONIQ_6)
finally:
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, None)
assert address == 0x51
assert bus == can_bus.ACAN
assert dat[2] == (factory[2] + 8) & 0xFF
assert dat[3:] == factory[3:]
assert int.from_bytes(dat[:2], "little") == hkg_can_fd_checksum(address, None, bytearray(dat))
assert parked_dat[3] == factory[3] & ~0x1
assert parked_dat[4:] == factory[4:]
assert int.from_bytes(parked_dat[:2], "little") == hkg_can_fd_checksum(address, None, bytearray(parked_dat))
assert other_dat[3:] == bytes(29)
def test_ev6_init_captures_factory_adrv_0x51(self, monkeypatch):
fingerprint = gen_empty_fingerprint()
fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16
radar_config = get_radar_track_config(CAR.KIA_EV6)
fingerprint[radar_config.bus][radar_config.start_addr] = radar_config.expected_length
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, car_fw, True, False, False, get_test_toggles())
factory = bytes.fromhex("6b657d090900e1ff000000000020ffff00000000000000000800000010000000")
def can_recv(*, wait_for_one=True):
msg = SimpleNamespace(address=0x51, src=CanBus(CP).ACAN, dat=factory)
return [[msg]]
def fake_disable_ecu(capturing_can_recv, *_args, **_kwargs):
capturing_can_recv(wait_for_one=True)
return True
monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu)
CarInterface.init(CP, can_recv, None)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
try:
_, dat, _ = hyundaicanfd.create_adrv_0x51(packer, CanBus(CP), 0, CAR.KIA_EV6, drive_gear=True)
finally:
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, None)
assert dat[3:] == factory[3:]
def test_carnival_hev_low_speed_torque_rate_limits(self):
CP = CarInterface.get_params(CAR.KIA_CARNIVAL_HEV_4TH_GEN, gen_empty_fingerprint(), [],
False, False, False, None)
@@ -619,13 +498,6 @@ class TestHyundaiFingerprint:
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
assert CP.flags & HyundaiFlags.SEND_LFA
@pytest.mark.parametrize("candidate", list(CAR))
def test_no_stock_lka_safety_flag_is_sportage_only(self, candidate):
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None)
if CP.flags & HyundaiFlags.CANFD:
assert bool(CP.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.CANFD_NO_STOCK_LKA) == \
(candidate == CAR.KIA_SPORTAGE_HEV_2026)
def test_smart_mdps_allows_low_speed_steering(self):
candidate = CAR.HYUNDAI_IONIQ_EV_LTD
@@ -760,107 +632,6 @@ class TestHyundaiFingerprint:
assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED
assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANCEL_BTN_ENABLE
@pytest.mark.parametrize("alpha_long", [False, True])
def test_palisade_hda2_front_radar_is_available_with_complete_native_bank(self, alpha_long):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
no_radar = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, alpha_long, False, False, None)
assert no_radar.radarUnavailable
config = get_radar_track_config(no_radar.carFingerprint, no_radar.flags)
assert config.radar_type == "palisade_210"
assert config.start_addr == 0x210
assert config.msg_count == 16
assert config.expected_length == 32
assert config.frequency == 20
assert config.bus == CanBus(no_radar).ACAN == 0
for addr in range(0x210, 0x220):
fingerprint[0][addr] = 32
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, alpha_long, False, False, None)
assert not CP.radarUnavailable
assert CP.openpilotLongitudinalControl == no_radar.openpilotLongitudinalControl
assert CP.flags == no_radar.flags
assert CP.safetyConfigs[-1].safetyParam == no_radar.safetyConfigs[-1].safetyParam
assert DBC[CP.carFingerprint][Bus.pt] == "hyundai_palisade_2023_generated"
@pytest.mark.parametrize("missing", [0x210, 0x218, 0x21f])
def test_palisade_hda2_incomplete_radar_bank_stays_unavailable(self, missing):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
for addr in range(0x210, 0x220):
if addr != missing:
fingerprint[0][addr] = 32
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, [], False, False, False, None)
assert CP.radarUnavailable
@pytest.mark.parametrize("bus,length", [(0, 8), (0, 24), (1, 32), (2, 32), (128, 32), (130, 32)])
def test_palisade_hda2_wrong_radar_layout_or_forwarded_copy_stays_unavailable(self, bus, length):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
if bus < len(fingerprint):
for addr in range(0x210, 0x220):
fingerprint[bus][addr] = length
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, [], False, False, False, None)
assert CP.radarUnavailable
def test_palisade_front_radar_configuration_does_not_enable_non_hda2(self):
fingerprint = gen_empty_fingerprint()
for addr in range(0x210, 0x220):
fingerprint[0][addr] = 32
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, [], False, False, False, None)
assert not CP.flags & HyundaiFlags.CANFD_LKA_STEERING
assert get_radar_track_config(CP.carFingerprint, CP.flags) is None
assert CP.radarUnavailable
old_palisade = get_radar_track_config(CAR.HYUNDAI_PALISADE)
assert old_palisade.radar_type == "mando"
assert old_palisade.bus == 1
@pytest.mark.parametrize("data_hex,expected", [
("0da6690a412630ff79e0fcba3f05c0f90000000b2420410671b101d3bf0080fc",
[(6.05, -2.5, -0.7, -1.25, 0.05), (18.45, 1.35, -0.45, -0.7, 0.0)]),
("2cd9d10a381b30fe7c830195be00c0000000000a237830fa95c003f83e0f00fb",
[(44.6, 1.2, -3.63, 0.15, 0.0), (7.45, 3.0, -2.64, -1.0, 0.15)]),
])
def test_palisade_front_radar_decoder_matches_detailed_reference_frame(self, data_hex, expected):
CP = CarParams(carFingerprint=CAR.HYUNDAI_PALISADE_2023,
flags=int(HyundaiFlags.CAN_CANFD_BLENDED | HyundaiFlags.CANFD_LKA_STEERING), radarUnavailable=False)
radar = RadarInterface(CP)
data = bytes.fromhex(data_hex)
radar.rcp.update([(1, [(0x210, data, 0)])])
points = radar._update({0x210}).points
assert len(points) == 2
for point, values in zip(points, expected, strict=True):
assert point.measured
assert (point.dRel, point.yRel, point.vRel, point.aRel, point.yvRel) == pytest.approx(values)
def test_palisade_front_radar_emits_32_tracks_without_duplicate_camera_bus_copy(self):
CP = CarParams(carFingerprint=CAR.HYUNDAI_PALISADE_2023,
flags=int(HyundaiFlags.CAN_CANFD_BLENDED | HyundaiFlags.CANFD_LKA_STEERING), radarUnavailable=False)
radar = RadarInterface(CP)
packer = CANPacker(radar.radar_config.dbc_name)
messages = []
for addr in range(0x210, 0x220):
msg = packer.make_can_msg(f"RADAR_TRACK_{addr:x}", 0, {
"1_STATE": 3, "1_LONG_DIST": 25.0, "1_LAT_DIST": 0.5, "1_REL_SPEED": -2.0,
"2_STATE": 4, "2_LONG_DIST": 35.0, "2_LAT_DIST": -1.0, "2_REL_SPEED": -3.0,
})
data = bytearray(msg[1])
checksum = hkg_can_fd_checksum(addr, None, data)
data[0] = checksum & 0xff
data[1] = checksum >> 8
messages.append((addr, bytes(data), 0))
copies = [(addr, data, 2) for addr, data, _ in messages]
assert radar.update([(1, copies)]) is None
result = radar.update([(2, messages + copies)])
assert result is not None
assert len(result.points) == 32
assert not result.errors.canError
assert len({point.trackId for point in result.points}) == 32
for addr, _ in radar.track_addrs:
radar.rcp.vl[addr]["1_STATE"] = 0
radar.rcp.vl[addr]["2_STATE"] = 0
assert not radar._update(set()).points
def test_palisade_telluride_hda2_uses_mixed_can_layout(self):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
@@ -1254,31 +1025,6 @@ class TestHyundaiFingerprint:
assert not (FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12)
@pytest.mark.parametrize("length, expected", ((6, True), (8, False)))
def test_stinger_only_replaces_six_byte_lkas12(self, length, expected):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x53E] = length
CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], True, False, False, None)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, get_test_toggles())
assert bool(FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12) is expected
@pytest.mark.parametrize("alpha_long, main_aol, expected", (
(True, True, True), (True, False, True), (False, True, False),
))
def test_stinger_aol_latches_lkas_after_long_engagement(self, alpha_long, main_aol, expected):
toggles = get_test_toggles()
toggles.always_on_lateral_main = main_aol
fingerprint = gen_empty_fingerprint()
CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], alpha_long, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, toggles)
assert bool(FPCP.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE) is expected
sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], alpha_long, False, False, toggles)
sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, fingerprint, [], sonata_cp, toggles)
assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
def test_ray_ev_does_not_treat_eight_byte_485_as_lfa(self):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x485] = 8
@@ -1484,103 +1230,6 @@ class TestHyundaiFingerprint:
assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS
assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable
def test_sportage_hev_hda2_redneck_uses_stock_scc(self, monkeypatch):
class FakeParams:
def __init__(self, *args, **kwargs):
pass
@staticmethod
def get_bool(key):
return key == "RedneckCruise"
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
toggles = get_test_toggles()
fingerprint = gen_empty_fingerprint()
can_bus = CanBus(None, fingerprint, True)
fingerprint[can_bus.CAM][0x110] = 32
fingerprint[can_bus.ECAN][0x1CF] = 8
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
assert FPCP.redneckCruiseAvailable
assert not FPCP.pcmCruiseSpeed
assert CP.pcmCruise
assert not CP.openpilotLongitudinalControl
assert not CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
controller = CarInterface(CP, FPCP).CC
assert not controller.long_active_ecu
controller.frame = 30
CS = SimpleNamespace(redneck_send_button=1, buttons_counter=5)
msgs = controller._create_canfd_redneck_button_messages(CS)
assert len(msgs) == 20
assert all(msg[0] == 0x1CF and msg[2] == can_bus.ECAN for msg in msgs)
assert all(msg[1][2] & 0x7 == Buttons.RES_ACCEL for msg in msgs)
controller.frame = 60
CS.redneck_send_button = 2
msgs = controller._create_canfd_redneck_button_messages(CS)
assert len(msgs) == 20
assert all(msg[1][2] & 0x7 == Buttons.SET_DECEL for msg in msgs)
monkeypatch.setattr(FakeParams, "get_bool", staticmethod(lambda key: False))
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
assert FPCP.redneckCruiseAvailable
assert FPCP.pcmCruiseSpeed
assert CP.pcmCruise
assert not CP.openpilotLongitudinalControl
def test_sportage_redneck_rejects_unverified_button_layouts(self, monkeypatch):
class FakeParams:
def __init__(self, *args, **kwargs):
pass
@staticmethod
def get_bool(key):
return key == "RedneckCruise"
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
toggles = get_test_toggles()
for button_address, button_bus, button_length, lka_steering in (
(0x1AA, 1, 16, True),
(0x1CF, 0, 8, True),
(0x1CF, 0, 8, False),
(0x1CF, 1, 16, True),
):
fingerprint = gen_empty_fingerprint()
can_bus = CanBus(None, fingerprint, lka_steering)
if lka_steering:
fingerprint[can_bus.CAM][0x110] = 32
fingerprint[button_bus][button_address] = button_length
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
assert not FPCP.redneckCruiseAvailable
assert FPCP.pcmCruiseSpeed
assert CP.pcmCruise
assert not CP.openpilotLongitudinalControl
fingerprint = gen_empty_fingerprint()
can_bus = CanBus(None, fingerprint, True)
fingerprint[can_bus.CAM][0x110] = 32
fingerprint[can_bus.ECAN][0x1CF] = 8
fingerprint[can_bus.ECAN][0x1AA] = 16
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
assert not FPCP.redneckCruiseAvailable
fingerprint = gen_empty_fingerprint()
can_bus = CanBus(None, fingerprint, True)
fingerprint[can_bus.CAM][0x110] = 32
fingerprint[can_bus.ECAN][0x1CF] = 8
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
CP.openpilotLongitudinalControl = True
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
assert not FPCP.redneckCruiseAvailable
def test_hyundai_non_scc_without_redneck_keeps_stock_longitudinal_mode(self, monkeypatch):
class FakeParams:
def __init__(self, *args, **kwargs):
@@ -1920,8 +1569,8 @@ class TestHyundaiFingerprint:
CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.longitudinalActuatorDelay == pytest.approx(0.22)
assert CP.stopAccel == pytest.approx(-1.1)
assert CP.stoppingDecelRate == pytest.approx(0.55)
assert CP.stopAccel == pytest.approx(-0.85)
assert CP.stoppingDecelRate == pytest.approx(0.35)
def test_elantra_hev_2024_longitudinal_delay_matches_observed_response(self):
toggles = get_test_toggles()
@@ -1964,69 +1613,6 @@ class TestHyundaiFingerprint:
assert exact
assert matches == {candidate}
@pytest.mark.parametrize("alpha_long", (False, True))
@pytest.mark.parametrize("alt_buttons", (False, True))
@pytest.mark.parametrize("signal, value, button_type", (
("LDA_BTN", 1, ButtonType.lkas),
("ADAPTIVE_CRUISE_MAIN_BTN", 1, ButtonType.mainCruise),
("CRUISE_BUTTONS", Buttons.RES_ACCEL, ButtonType.accelCruise),
("CRUISE_BUTTONS", Buttons.SET_DECEL, ButtonType.decelCruise),
("CRUISE_BUTTONS", Buttons.CANCEL, ButtonType.cancel),
))
def test_k4_2025_2026_physical_button_events(self, alpha_long, alt_buttons, signal, value, button_type):
toggles = get_test_toggles()
fingerprint = gen_empty_fingerprint()
if not alt_buttons:
fingerprint[0][0x1CF] = 8
CP = CarInterface.get_params(CAR.KIA_K4_2025, fingerprint, [], alpha_long, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.KIA_K4_2025, fingerprint, [], CP, toggles)
car_state = CarState(CP, FPCP)
parsers = car_state.get_can_parsers(CP)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
def update(button_value, frame):
msg = packer.make_can_msg(car_state.cruise_btns_msg_canfd, CanBus(CP).ECAN, {signal: button_value})
parsers[Bus.pt].update([(frame * 10_000_000, [msg])])
return car_state.update(parsers, toggles)[0].buttonEvents
assert not update(0, 1)
pressed = update(value, 2)
assert [(event.type, event.pressed) for event in pressed] == [(button_type, True)]
released = update(0, 3)
assert [(event.type, event.pressed) for event in released] == [(button_type, False)]
@pytest.mark.parametrize("camera_fw", [
b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708',
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708',
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205',
])
@pytest.mark.parametrize("radar_fw", [
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ',
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG100 ',
])
def test_k4_2025_2026_fw_exact_matches(self, camera_fw, radar_fw):
car_fw = [
CarParams.CarFw(ecu=Ecu.fwdCamera, fwVersion=camera_fw, address=0x7c4, brand="hyundai"),
CarParams.CarFw(ecu=Ecu.fwdRadar, fwVersion=radar_fw, address=0x7d0, brand="hyundai"),
]
exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False)
assert exact
assert matches == {CAR.KIA_K4_2025}
def test_staria_2023_australian_route_fw_exact_matches(self):
route_fw = {
(Ecu.fwdCamera, 0x7c4): b'\xf1\x00US4 MFC AT AUS RHD 1.00 1.04 99211-CG000 210819',
(Ecu.fwdRadar, 0x7d0): b'\xf1\x00US4_ RDR ----- 1.00 1.00 99110-CG000 ',
}
car_fw = [
CarParams.CarFw(ecu=ecu, fwVersion=version, address=address, subAddress=0, brand="hyundai")
for (ecu, address), version in route_fw.items()
]
exact, matches = match_fw_to_car(car_fw, "KMFYFX71MPU095311", allow_fuzzy=False, log=False)
assert exact
assert matches == {CAR.HYUNDAI_STARIA_4TH_GEN}
def test_kona_ev_non_scc_has_no_dedicated_fw_coverage(self):
assert CAR.HYUNDAI_KONA_EV_NON_SCC not in FW_VERSIONS
@@ -0,0 +1,62 @@
import pytest
from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.structs import CarParams
from opendbc.car.hyundai import hyundaicanfd
from opendbc.car.hyundai.hyundaicanfd import CanBus, get_ioniq_6_damp_factors
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags
def _cp(fingerprint):
CP = CarParams.new_message()
CP.carFingerprint = fingerprint
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING |
HyundaiFlags.CANFD_LKA_STEERING_ALT)
CP.openpilotLongitudinalControl = True
return CP
def _damping(fingerprint, v_ego, torque=100):
CP = _cp(fingerprint)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
can_bus = CanBus(CP)
msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, True, True, torque, 0.0, v_ego=v_ego)
names = [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs]
assert names == [("LFA", can_bus.ECAN), ("LKAS_ALT", can_bus.ACAN)]
lfa = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN)
lfa.update([(1, [msgs[0]])])
lkas = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
lkas.update([(1, [msgs[1]])])
assert lfa.can_valid and lkas.can_valid
assert lfa.vl["LFA"]["TORQUE_REQUEST"] == torque
assert lkas.vl["LKAS_ALT"]["TORQUE_REQUEST"] == torque
return lfa.vl["LFA"]["DAMP_FACTOR"], lkas.vl["LKAS_ALT"]["DAMP_FACTOR"], lkas.vl["LKAS_ALT"]["Damping_Gain"]
@pytest.mark.parametrize(("mph", "expected"), [
(10, (100, 0)), # low speed unchanged: previous LFA 100 floor, LKAS_ALT 0
(30, (100, 0)),
(42.5, (105, 125)), # stock medians, 40-45 mph band
(50, (110, 130)), # halfway between the 45-50 (107/127) and 50-55 (112/132) bands
(72.5, (145, 144)), # stock medians, 70-75 mph band
(95, (163, 154)), # held beyond the last breakpoint (85-90 mph band)
])
def test_ioniq_6_damping_schedule(mph, expected):
assert get_ioniq_6_damp_factors(mph * CV.MPH_TO_MS) == expected
def test_ioniq_6_damping_is_encoded_on_both_steering_messages():
lfa, lkas_alt, damping_gain = _damping(CAR.HYUNDAI_IONIQ_6, 72.5 * CV.MPH_TO_MS)
assert (lfa, lkas_alt) == (145, 144)
assert damping_gain == lkas_alt # same bits: Hyundai's name for the LKAS_ALT damping byte
def test_ioniq_6_without_speed_keeps_previous_values():
assert _damping(CAR.HYUNDAI_IONIQ_6, None)[:2] == (100, 0)
def test_other_cars_unchanged():
assert _damping(CAR.HYUNDAI_IONIQ_5, 70 * CV.MPH_TO_MS)[:2] == (100, 0)
@@ -1,11 +1,10 @@
from types import SimpleNamespace
import numpy as np
import pytest
from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, gen_empty_fingerprint
from opendbc.car.hyundai.carcontroller import CarController, get_ray_pedal_target, RAY_PEDAL_COMMAND_CAP
from opendbc.car.hyundai.carcontroller import CarController
from opendbc.car.hyundai.carstate import CarState
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.values import CAR, DBC, HyundaiSafetyFlags
@@ -33,11 +32,8 @@ def test_ray_pedal_fingerprint_isolation(candidate, fingerprint, has_pedal):
if has_pedal:
assert not CP.pcmCruise
assert CP.safetyConfigs[-1].safetyParam == 0x9405
assert CP.minEnableSpeed == -1.0
assert CP.minEnableSpeed == 5.0
assert not CP.autoResumeSng
assert all(gain > 0 for gain in CP.longitudinalTuning.kpV)
assert all(gain > 0 for gain in CP.longitudinalTuning.kiV)
assert CP.longitudinalTuning.kfDEPRECATED == 1.0
FPCP = CarInterface.get_starpilot_params(candidate, fingerprint, [], CP, SimpleNamespace())
assert FPCP.canUsePedal
assert not FPCP.pcmCruiseSpeed
@@ -45,8 +41,6 @@ def test_ray_pedal_fingerprint_isolation(candidate, fingerprint, has_pedal):
else:
assert CP.pcmCruise
assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
assert list(CP.longitudinalTuning.kpV) == [0.0]
assert list(CP.longitudinalTuning.kiV) == [0.0]
def test_ray_pedal_safety_signature_is_unique_across_hyundai_platforms():
@@ -97,18 +91,12 @@ def test_ray_pedal_fault_clears_only_with_healthy_sensor_state():
assert not ret.accFaulted
@pytest.mark.parametrize("physical_rest", [
"010801f30cef", # September 15 calibration: raw 264/497.
"009b01ad0ef0", # October 9 route 0000007b segment 6: raw 155/429.
"00b601c4074b", # October 11 route 00000096 segment 28: raw 182/452.
])
def test_ray_driver_override_uses_physical_interceptor_tracks(physical_rest):
def test_ray_driver_override_uses_physical_interceptor_tracks():
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
state = CarState(CP, None)
parsers = state.get_can_parsers(CP)
native_gas = (0x371, bytes.fromhex("004e008000ae0700"), 0)
rest_data = bytes.fromhex(physical_rest)
physical_rest = (0x201, rest_data, 0)
physical_rest = (0x201, bytes.fromhex("010801f30cef"), 0)
for parser in parsers.values():
parser.update([(1_000_000_000, [native_gas, physical_rest])])
ret, _ = state.update(parsers, SimpleNamespace())
@@ -119,7 +107,7 @@ def test_ray_driver_override_uses_physical_interceptor_tracks(physical_rest):
physical_press = packer.make_can_msg("GAS_SENSOR", 0, {
"INTERCEPTOR_GAS": (310 - 264) * 0.672,
"INTERCEPTOR_GAS2": (593 - 497) * 0.332,
"STATE": 0, "COUNTER_PEDAL": ((rest_data[4] & 0xF) + 1) & 0xF,
"STATE": 0, "COUNTER_PEDAL": 13,
})
for parser in parsers.values():
parser.update([(1_020_000_000, [physical_press])])
@@ -140,100 +128,13 @@ def test_ray_without_pedal_keeps_native_gas_detection():
assert ret.gasPressed
@pytest.mark.parametrize(("speed", "cap"), [(0.0, 0.55), (12.0, 0.55), (16.0, 0.60), (20.0, 0.65), (25.0, 0.70), (40.0, 0.70)])
def test_ray_highway_headroom_preserves_low_speed_cap(speed, cap):
assert get_ray_pedal_target(3.5, speed, speed + 10.0) == pytest.approx(cap)
high_set_target = get_ray_pedal_target(3.5, speed, 145.0 / 3.6)
assert 0.0 < high_set_target <= cap + 1e-9 # high SET speed must not inhibit actuation
if speed < 39.0:
assert high_set_target == pytest.approx(cap)
@pytest.mark.parametrize("speed", [0.0, 2.0, 8.0, 12.0, 20.0, 25.0])
def test_ray_pedal_decel_mapping_is_continuous_and_monotonic(speed):
coast = get_ray_pedal_target(0.0, speed, speed + 10.0)
mild_decel = get_ray_pedal_target(-0.1, speed, speed + 10.0)
assert 0.0 < mild_decel < coast
assert coast - mild_decel == pytest.approx(0.055)
assert get_ray_pedal_target(-1.5, speed, speed + 10.0) == 0.0
targets = [get_ray_pedal_target(float(a), speed, speed + 10.0) for a in np.linspace(-3.5, 3.5, 701)]
assert all(0.0 <= x <= RAY_PEDAL_COMMAND_CAP for x in targets)
assert all(b >= a for a, b in zip(targets, targets[1:], strict=False))
assert abs(get_ray_pedal_target(-1e-6, speed, speed + 10.0) - coast) < 1e-6
@pytest.mark.parametrize("speed", [0.0, 12.0, 20.0, 25.0, 40.0])
def test_ray_tuned_pedal_keeps_overspeed_taper_and_cutoff(speed):
full = get_ray_pedal_target(3.5, speed, speed + 10.0)
assert get_ray_pedal_target(3.5, speed, max(speed, 1.0)) <= full
if speed >= 1.5:
assert get_ray_pedal_target(3.5, speed, speed) == pytest.approx(full * 0.65)
assert get_ray_pedal_target(3.5, speed, speed - 0.25) == pytest.approx(full * 0.325)
assert get_ray_pedal_target(3.5, speed, speed - 0.5) == 0.0
@pytest.mark.parametrize(("accel", "speed", "set_speed"), [
(float("nan"), 25.0, 40.0), (1.0, float("nan"), 40.0), (1.0, 25.0, float("nan")),
(float("inf"), 25.0, 40.0), (1.0, float("inf"), 40.0), (1.0, 25.0, float("inf")),
(1.0, 25.0, 0.0),
])
def test_ray_tuned_pedal_rejects_invalid_targets(accel, speed, set_speed):
assert get_ray_pedal_target(accel, speed, set_speed) == 0.0
@pytest.mark.parametrize("cutoff", ["brake", "driver", "override", "inactive", "invalid_sensor", "fault", "overspeed"])
def test_ray_highway_output_cuts_immediately_on_override_or_fault(cutoff):
def test_ray_controller_heartbeats_and_only_actuates_when_ready():
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
CS = SimpleNamespace(
lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
out=SimpleNamespace(vEgo=25.0, gasPressed=False, brakePressed=False, cruiseState=SimpleNamespace(enabled=False)),
ray_pedal_valid=True, ray_pedal_state=0, is_metric=True,
)
CC = SimpleNamespace(enabled=True, longActive=True, latActive=True,
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False))
hud = SimpleNamespace(visualAlert=CarControl.HUDControl.VisualAlert.none, setSpeed=40.0,
leftLaneVisible=True, rightLaneVisible=True, leftLaneDepart=False, rightLaneDepart=False)
actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.pid)
def pedal_msg(frame):
controller.frame = frame
messages = controller.create_can_msgs(True, 0, False, 0.0, 1.5, False, hud, actuators, CS, CC, 2, 0)
return next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)
for frame in range(0, 4 * 40, 4):
pedal_msg(frame)
assert controller._ray_pedal_gas_last == pytest.approx(RAY_PEDAL_COMMAND_CAP)
assert pedal_msg(160)[4] & 0x80
if cutoff == "brake":
CS.out.brakePressed = True
elif cutoff == "driver":
CS.out.gasPressed = True
elif cutoff == "override":
CC.cruiseControl.override = True
elif cutoff == "inactive":
CC.longActive = False
elif cutoff == "invalid_sensor":
CS.ray_pedal_valid = False
elif cutoff == "fault":
CS.ray_pedal_state = 5
else:
hud.setSpeed = CS.out.vEgo - 0.5
dat = pedal_msg(164)
assert dat[:4] == bytes(4)
assert not (dat[4] & 0x80)
assert controller._ray_pedal_gas_last == 0.0
@pytest.mark.parametrize("speed", [0.0, 0.1, 1.0, 4.9, 5.0, 12.0])
def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed):
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
CS = SimpleNamespace(
lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
out=SimpleNamespace(vEgo=speed, gasPressed=False, brakePressed=False,
out=SimpleNamespace(vEgo=12.0, gasPressed=False, brakePressed=False,
cruiseState=SimpleNamespace(enabled=False)),
ray_pedal_valid=True, ray_pedal_state=5, is_metric=True,
)
@@ -243,7 +144,6 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed):
)
hud = SimpleNamespace(
visualAlert=CarControl.HUDControl.VisualAlert.none,
setSpeed=20.0,
leftLaneVisible=True, rightLaneVisible=True,
leftLaneDepart=False, rightLaneDepart=False,
)
@@ -266,124 +166,5 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed):
controller.frame = 16
messages = controller.create_can_msgs(True, 0, False, 0.0, 2.0, False,
hud, actuators, CS, CC, 2, 0)
assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[4] & 0x80
assert any(addr == 0x4F1 and bus == 0 and dat[0] & 7 == 4 for addr, dat, bus in messages)
CS.out.cruiseState.enabled = False
CS.out.brakePressed = True
assert pedal_msg(2.0, 20)[:4] == bytes(4)
CS.out.brakePressed = False
assert pedal_msg(2.0, 24)[4] & 0x80
assert controller._ray_pedal_gas_last == pytest.approx(0.02)
assert pedal_msg(2.0, 28)[4] & 0x80
assert controller._ray_pedal_gas_last == pytest.approx(0.04)
CS.out.brakePressed = True
assert pedal_msg(2.0, 32)[:4] == bytes(4)
assert controller._ray_pedal_gas_last == 0.0
CS.out.brakePressed = False
assert pedal_msg(2.0, 36)[4] & 0x80
assert controller._ray_pedal_gas_last == pytest.approx(0.02)
CC.longActive = False
assert pedal_msg(2.0, 40)[:4] == bytes(4)
CC.longActive = True
CC.cruiseControl.override = True
assert pedal_msg(2.0, 44)[:4] == bytes(4)
CC.cruiseControl.override = False
CS.ray_pedal_valid = False
assert pedal_msg(2.0, 48)[:4] == bytes(4)
CS.ray_pedal_valid = True
for fault in range(1, 6):
CS.ray_pedal_state = fault
assert pedal_msg(2.0, 48 + 4 * fault)[:4] == bytes(4)
CS.ray_pedal_state = 0
CS.out.vEgo = 10.0
assert pedal_msg(0.0, 72)[4] & 0x80
assert pedal_msg(-1.0, 76)[:4] == bytes(4)
CS.out.vEgo = 12.0
for frame in range(80, 80 + 4 * 40, 4):
pedal_msg(1.5, frame)
assert controller._ray_pedal_gas_last == pytest.approx(0.55)
for frame in range(240, 240 + 4 * 12, 4):
dat = pedal_msg(-1.5, frame)
assert dat[:4] == bytes(4)
for frame in range(288, 288 + 4 * 40, 4):
pedal_msg(1.5, frame)
assert controller._ray_pedal_gas_last == pytest.approx(0.55)
hud.setSpeed = 12.0
pedal_msg(1.5, 448)
assert controller._ray_pedal_gas_last == pytest.approx(0.55 * 0.65)
hud.setSpeed = 11.8
dat = pedal_msg(1.5, 452)
assert controller._ray_pedal_gas_last == pytest.approx(0.55 * 0.65 * 0.6)
assert dat[4] & 0x80
hud.setSpeed = 11.4
assert pedal_msg(1.5, 456)[:4] == bytes(4)
hud.setSpeed = float('nan')
assert pedal_msg(1.5, 460)[:4] == bytes(4)
hud.setSpeed = 20.0
assert pedal_msg(-0.3, 464)[4] & 0x80 # partial pedal trims regen for mild slowing
CS.out.vEgo = 15.0
hud.setSpeed = 53.0 / 3.6
pedal_msg(-0.16, 468)
assert controller._ray_pedal_gas_last < get_ray_pedal_target(-0.16, 15.0, 20.0)
CS.out.vEgo = 12.0
hud.setSpeed = 8.0 / 3.6
assert pedal_msg(-0.3, 472)[:4] == bytes(4)
hud.setSpeed = 145.0 / 3.6
assert pedal_msg(1.5, 476)[4] & 0x80
assert controller._ray_pedal_gas_last == pytest.approx(0.02)
assert pedal_msg(1.5, 480)[4] & 0x80
assert controller._ray_pedal_gas_last == pytest.approx(0.04)
assert pedal_msg(-0.3, 484)[4] & 0x80
@pytest.mark.parametrize("candidate", [CAR.KIA_RAY_EV, CAR.HYUNDAI_KONA_EV_NON_SCC])
def test_ray_stock_cruise_cancellation_survives_accelerator_override(candidate):
CP = CarInterface.get_params(candidate, ray_fingerprint(), [], False, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
CS = SimpleNamespace(
lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
out=SimpleNamespace(vEgo=12.0, gasPressed=True, brakePressed=False,
cruiseState=SimpleNamespace(enabled=True)),
ray_pedal_valid=True, ray_pedal_state=0, is_metric=True,
)
CC = SimpleNamespace(
enabled=True, longActive=False, latActive=True,
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=True),
)
hud = SimpleNamespace(
visualAlert=CarControl.HUDControl.VisualAlert.none,
setSpeed=20.0,
leftLaneVisible=True, rightLaneVisible=True, leftLaneDepart=False, rightLaneDepart=False,
)
actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.off)
controller._create_can_redneck_button_messages = lambda _: []
def messages(frame):
controller.frame = frame
return controller.create_can_msgs(True, 0, False, 0.0, 2.0, False,
hud, actuators, CS, CC, 2, 0)
def cancel_frames(msgs):
return [dat for addr, dat, bus in msgs if addr == 0x4F1 and bus == 0 and dat[0] & 7 == 4]
msgs = messages(20)
assert bool(cancel_frames(msgs)) is (candidate == CAR.KIA_RAY_EV)
if candidate == CAR.KIA_RAY_EV:
pedal = next(dat for addr, dat, bus in msgs if addr == 0x200 and bus == 0)
assert pedal[:4] == bytes(4)
assert not (pedal[4] & 0x80)
assert not cancel_frames(messages(24)) # retain the existing cancellation rate limit
assert cancel_frames(messages(25))
assert cancel_frames(messages(32))
CS.out.cruiseState.enabled = False
assert not cancel_frames(messages(44))
CS.out.cruiseState.enabled = True
CC.enabled = False
assert not cancel_frames(messages(56)) # AOL alone must not cancel native cruise
assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4)
assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages) # cancel stock CC
@@ -117,7 +117,6 @@ class HyundaiSafetyFlags(IntFlag):
class HyundaiStarPilotSafetyFlags(IntFlag):
CANFD_NO_STOCK_LKA = 4096 # CAN-FD only; classic CAN uses this bit for NON_SCC.
AOL_MAIN_LKAS_ON_ENGAGE = 128
AOL_MAIN_LKAS_SYNC = 32
HAS_LDA_BUTTON = 1024
@@ -606,7 +605,6 @@ class CAR(Platforms):
KIA_K4_2025 = HyundaiCanFDPlatformConfig(
[
HyundaiCarDocs("Kia K4 (without HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarDocs("Kia K4 (without HDA II) 2026", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarDocs("Kia K4 (with HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_r])),
],
CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=13.4),
@@ -1229,7 +1227,6 @@ CANFD_RADAR_ECU_KEEPALIVE_CAR = {
CAR.GENESIS_GV60_EV_1ST_GEN,
}
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
CAR.HYUNDAI_PALISADE_2023,
CAR.HYUNDAI_IONIQ,
CAR.HYUNDAI_KONA_EV_2022,
CAR.HYUNDAI_SANTA_FE_2022,
+5 -21
View File
@@ -244,28 +244,15 @@ class CarInterfaceBase(ABC):
if 0x1FA in fingerprint[CAN.ECAN]:
fp_ret.flags |= HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE.value
if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2] and \
(candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6):
if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]:
fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value
sportage_stock_scc_buttons = (
candidate == HYUNDAI.KIA_SPORTAGE_HEV_2026 and
not CP.openpilotLongitudinalControl and
bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
fingerprint[CAN.ECAN].get(0x1CF) == 8 and
0x1AA not in fingerprint[CAN.ECAN]
)
fp_ret.redneckCruiseAvailable = (
(bool(CP.flags & HyundaiFlags.NON_SCC) and
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) or
sportage_stock_scc_buttons
)
fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED))
if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
fp_ret.pcmCruiseSpeed = False
if CP.flags & HyundaiFlags.NON_SCC:
CP.openpilotLongitudinalControl = True
CP.openpilotLongitudinalControl = True
if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl:
fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value
@@ -288,9 +275,6 @@ class CarInterfaceBase(ABC):
if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID:
fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
if candidate == HYUNDAI.KIA_STINGER_2022 and CP.openpilotLongitudinalControl:
fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
# The refresh Elantra's safety mapping comes from the resolved Galaxy
# toggle above, not from this legacy persisted-parameter fallback.
if candidate != HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and \
-95
View File
@@ -1,95 +0,0 @@
"""One bounded Legacy AVH ON request; 0x32B is status, never a TX command."""
AVH_REQUEST = 0x6BB
AVH_STATUS = 0x32B
INPUTS = (AVH_REQUEST, AVH_STATUS, 0x40, 0x48, 0x13A, 0x174)
def checksum(address, data):
return ((address & 0xFF) + (address >> 8) + sum(data[1:])) & 0xFF
def avh_request(template, step):
if len(template) != 8 or checksum(AVH_REQUEST, template) != template[0] or template[2] & 3 or step not in (1, 2):
raise ValueError("Invalid AVH template or counter step")
data = bytearray(template)
data[1] = (data[1] & 0xF0) | ((data[1] + step) & 0xF)
data[2] |= 2
data[0] = checksum(AVH_REQUEST, data)
return AVH_REQUEST, bytes(data), 1
class AvhStartup:
def __init__(self):
self.started = None
self.last_time = None
self.stable_since = None
self.frames = {}
self.done = False
self.followup = None
def update(self, now, frames, enabled, can_valid, controls_active):
if self.started is None:
self.started = now
if self.last_time is not None and now < self.last_time:
self.done = True
self.last_time = now
if self.done:
return []
if now - self.started > 30 or controls_active:
self.done = True
return []
for address, (timestamp, data) in frames.items():
if address not in INPUTS or timestamp <= 0:
continue
previous = self.frames.get(address)
if previous and timestamp == previous[0]:
continue
if len(data) != 8 or checksum(address, data) != data[0] or timestamp > now or (previous and timestamp < previous[0]):
self.done = True
return []
if (address == AVH_REQUEST and data[2] & 3) or (address == AVH_STATUS and data[5] & 0x20) or \
(address == 0x48 and data[3] != 4) or (address == 0x40 and data[4]) or \
(address == 0x13A and any((int.from_bytes(data, 'little') >> bit) & 0x1FFF for bit in (12, 25, 38, 51))):
self.done = True
return []
if previous and (data[1] & 15) == (previous[1][1] & 15):
continue # duplicate counters cannot refresh freshness
# Controller snapshots can skip 50/100 Hz samples between updates. Panda
# checks their full counter stream; require consecutive head-unit frames here.
sequential = bool(previous and (address not in (AVH_REQUEST, AVH_STATUS) or
(data[1] & 15) == ((previous[1][1] + 1) & 15)))
self.frames[address] = (timestamp, data, sequential)
fresh = all(a in self.frames and self.frames[a][2] and
0 <= now - self.frames[a][0] <= (1.5 if a == AVH_REQUEST else 0.3) for a in INPUTS)
if not enabled or not can_valid or not fresh:
self.stable_since = None
if self.followup is not None:
self.done = True
return []
throttle = self.frames[0x40][1]
rpm = int.from_bytes(throttle[2:4], 'little') & 0x1FFF
if rpm < 400 or not self.frames[0x174][1][2] & 8:
self.stable_since = None
if self.followup is not None:
self.done = True
return []
if self.stable_since is None:
self.stable_since = now
if self.followup is not None:
sent, timestamp, template = self.followup
if now - sent > 0.075 or self.frames[AVH_REQUEST][0] != timestamp:
self.done = True
elif now - sent >= 0.05:
self.done = True
return [avh_request(template, 2)]
return []
if now - self.started < 10 or now - self.stable_since < 3:
return []
timestamp, template, _ = self.frames[AVH_REQUEST]
if now - timestamp > 0.010:
return []
self.followup = (now, timestamp, template)
return [avh_request(template, 1)]
@@ -4,7 +4,6 @@ from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, apply_steer_angle_limits_vm, common_fault_avoidance
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.subaru import subarucan
from opendbc.car.subaru.avh import AvhStartup
from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, CanBus, CarControllerParams, SubaruFlags
from opendbc.car.vehicle_model import VehicleModel
@@ -46,10 +45,7 @@ class CarController(CarControllerBase):
self.angle_override_confirm_frames = 0
self.angle_lkas_active = False
self.angle_handoff_active = False
self.ascent_angle_initialized = False
self.ascent_aol_arm_frames = 0
self.ascent_es_distance_counter_last = None
self.ascent_lkas_alert_counter_last = None
self.cruise_button_prev = 0
self.steer_rate_counter = 0
@@ -73,7 +69,6 @@ class CarController(CarControllerBase):
self.stop_start_counter = 0
self.stop_start_acknowledged = False
self.last_redneck_button_frame = 0
self.avh_startup = AvhStartup()
def _stop_start_off_request(self, CC, CS, starpilot_toggles):
"""Send one bounded Subaru Stop/Start OFF request after ignition.
@@ -207,10 +202,6 @@ class CarController(CarControllerBase):
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and not self.ascent_angle_initialized:
self.apply_steer_last = CS.out.steeringAngleDeg
self.ascent_angle_initialized = True
mads_only = CC.latActive and not CC.enabled
mads_only_ok = CS.out.vEgoRaw > _ANGLE_MADS_MIN_SPEED and \
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
@@ -224,13 +215,12 @@ class CarController(CarControllerBase):
self.ascent_aol_arm_frames = _ASCENT_AOL_ARM_FRAMES if lkas_available else 0
if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
manual_handoff = not self.angle_lkas_active and \
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE
manual_handoff = False
else:
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
lkas_active = lkas_available and not manual_handoff
if lkas_active and not self.angle_lkas_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023:
if lkas_active and not self.angle_lkas_active:
self.apply_steer_last = CS.out.steeringAngleDeg
apply_steer = apply_std_steer_angle_limits(
@@ -302,16 +292,6 @@ class CarController(CarControllerBase):
return self.angle_lkas_active
return CC.latActive
def _ascent_lkas_alert(self, CC, CS):
if self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023 or not self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT:
return None
dat = getattr(CS, "es_lkas_alert_dat", b"")
msg = subarucan.create_ascent_lkas_alert(dat, self._lkas_status_active(CC), CC.hudControl.visualAlert)
if msg is None or (dat[1] & 0xf) == self.ascent_lkas_alert_counter_last:
return None
self.ascent_lkas_alert_counter_last = dat[1] & 0xf
return msg
def update(self, CC, CS, now_nanos, starpilot_toggles):
actuators = CC.actuators
hud_control = CC.hudControl
@@ -327,13 +307,6 @@ class CarController(CarControllerBase):
if stop_start_msg is not None:
can_sends.append(stop_start_msg)
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
can_sends.extend(self.avh_startup.update(
now_nanos / 1e9, getattr(CS, "avh_frames", {}),
getattr(starpilot_toggles, "subaru_avh_on", False), getattr(CS.out, "canValid", False),
CC.enabled or CC.latActive or CC.longActive,
))
# *** steering ***
if (self.frame % self.p.STEER_STEP) == 0:
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
@@ -363,10 +336,6 @@ class CarController(CarControllerBase):
cruise_brake = CarControllerParams.BRAKE_MIN
# *** alerts and pcm cancel ***
lkas_alert_msg = self._ascent_lkas_alert(CC, CS)
if lkas_alert_msg is not None:
can_sends.append(lkas_alert_msg)
if self.CP.flags & SubaruFlags.PREGLOBAL:
if self.frame % 5 == 0:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
@@ -427,12 +396,7 @@ class CarController(CarControllerBase):
can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
cancel_frame_ready = True
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
stock_counter = CS.es_distance_msg["COUNTER"]
cancel_frame_ready = stock_counter != self.ascent_es_distance_counter_last
self.ascent_es_distance_counter_last = stock_counter
if pcm_cancel_cmd and cancel_frame_ready:
if pcm_cancel_cmd:
if not (self.CP.flags & SubaruFlags.HYBRID):
bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
+3 -16
View File
@@ -4,9 +4,8 @@ from opendbc.can import CANDefine, CANParser
from opendbc.car import Bus, create_button_events, structs
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import CarStateBase
from opendbc.car.subaru.values import CAR, DBC, CanBus, SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags
from opendbc.car.subaru.values import DBC, CanBus, SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags
from opendbc.car import CanSignalRateCalculator
from opendbc.car.subaru.avh import INPUTS as AVH_INPUTS
ButtonType = structs.CarState.ButtonEvent.Type
@@ -27,9 +26,7 @@ class CarState(CarStateBase):
self.dashlights_msg = {}
self.dashlights_dat = b""
self.stop_start_state = 0
self.avh_frames = {}
self.cruise_buttons_msg = {}
self.es_lkas_alert_dat = b""
self.cruise_buttons = {button: 0 for button in SUBARU_CRUISE_BUTTONS}
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
@@ -40,9 +37,6 @@ class CarState(CarStateBase):
cp_angle = cp_main if self.CP.flags & SubaruFlags.D_PLATFORM else cp
ret = structs.CarState()
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
self.avh_frames = {a: (cp_alt.ts_nanos[a]["CHECKSUM"] / 1e9, cp_alt.vl_raw[a]) for a in AVH_INPUTS}
if self.CP.carFingerprint in SUBARU_STOP_START_CARS:
stop_start_cp = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
self.dashlights_msg = copy.copy(stop_start_cp.vl["Dashlights"])
@@ -167,8 +161,6 @@ class CarState(CarStateBase):
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
if self.CP.flags & SubaruFlags.ASCENT_LKAS_ALERT:
self.es_lkas_alert_dat = cp_cam.vl_raw[0x3C4]
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
@@ -185,16 +177,11 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parsers(CP):
avh_messages = [(a, 0) for a in (0x6BB, 0x32B, 0x40, 0x48)] if CP.carFingerprint == CAR.SUBARU_LEGACY_2025 else []
camera_messages = [(0x3C4, 0)] if CP.flags & SubaruFlags.ASCENT_LKAS_ALERT else []
parsers = {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main_for_cp(CP)),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], camera_messages, CanBus.camera),
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], avh_messages, CanBus.alt_for_cp(CP))
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt_for_cp(CP))
}
if CP.flags & SubaruFlags.D_PLATFORM:
parsers[Bus.main] = CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main)
if CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
for address in AVH_INPUTS:
parsers[Bus.alt].vl[address]
return parsers
@@ -25,9 +25,6 @@ class CarInterface(CarInterfaceBase):
if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]:
ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value
if candidate == CAR.SUBARU_ASCENT_2023 and fingerprint[CanBus.camera].get(0x3C4) == 8:
ret.flags |= SubaruFlags.ASCENT_LKAS_ALERT.value
if ret.flags & SubaruFlags.PREGLOBAL:
ret.enableBsm = 0x25c in fingerprint[0]
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaruPreglobal)]
@@ -43,12 +40,8 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
if ret.flags & SubaruFlags.D_PLATFORM_CAMERA:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
if ret.flags & SubaruFlags.ASCENT_LKAS_ALERT:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.ASCENT_LKAS_ALERT.value
if candidate in SUBARU_STOP_START_CARS:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
if candidate == CAR.SUBARU_LEGACY_2025:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.AVH_STARTUP.value
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
@@ -301,18 +301,6 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert, bus
return packer.make_can_msg("ES_Infotainment", bus, values)
def create_ascent_lkas_alert(raw_dat: bytes, lateral_active: bool, visual_alert):
address = 0x3C4
if len(raw_dat) != 8 or raw_dat[0] != (((address & 0xff) + (address >> 8) + sum(raw_dat[1:])) & 0xff):
return None
dat = bytearray(raw_dat)
if lateral_active and visual_alert != VisualAlert.steerRequired and dat[2:] == b"\x00\x01\x04\x00\x00\x00":
dat[3:5] = b"\x00\x00"
dat[0] = ((address & 0xff) + (address >> 8) + sum(dat[1:])) & 0xff
return address, bytes(dat), CanBus.main
def create_es_highbeamassist(packer):
values = {
"HBA_Available": False,
@@ -1,112 +0,0 @@
from types import SimpleNamespace
import pytest
from opendbc.car.subaru.avh import AVH_REQUEST, AVH_STATUS, INPUTS, AvhStartup, avh_request, checksum
from opendbc.car.subaru.carcontroller import CarController
from opendbc.car.subaru.interface import CarInterface
from opendbc.car.subaru.values import CAR, SubaruSafetyFlags
from opendbc.car import Bus
def sample(address, counter):
data = bytearray(8)
data[1] = counter & 15
if address == AVH_REQUEST:
data[3], data[5], data[6] = 1, 0x80, 0x0E # captured Legacy payload, not Outback constants
elif address == 0x40:
data[2:4] = (800).to_bytes(2, 'little')
elif address == 0x48:
data[3] = 4
elif address == 0x174:
data[2] = 8
data[0] = checksum(address, data)
return bytes(data)
def prepare(fast_counter_step=1):
policy = AvhStartup()
frames = {}
for tick in range(101):
now = 100 + tick / 10
for address in INPUTS:
if address != AVH_REQUEST or tick % 10 == 0:
counter = tick // 10 if address == AVH_REQUEST else tick * (1 if address == AVH_STATUS else fast_counter_step)
frames[address] = (now, sample(address, counter))
sent = policy.update(now, frames, True, True, False)
if tick < 100:
assert sent == []
assert sent == [avh_request(frames[AVH_REQUEST][1], 1)]
return policy, frames
def test_captured_legacy_press_bytes():
template = bytes.fromhex('5b0b000100800e00')
assert avh_request(template, 1) == (0x6BB, bytes.fromhex('5e0c020100800e00'), 1)
assert avh_request(template, 2) == (0x6BB, bytes.fromhex('5f0d020100800e00'), 1)
wrap = sample(AVH_REQUEST, 15)
assert avh_request(wrap, 1)[1][1] == 0
assert avh_request(wrap, 2)[1][1] == 1
def test_two_frames_only_and_no_retry():
policy, frames = prepare()
assert policy.update(110.04, frames, True, True, False) == []
assert policy.update(110.06, frames, True, True, False) == [avh_request(frames[AVH_REQUEST][1], 2)]
assert policy.update(110.07, frames, True, True, False) == []
assert policy.update(111, frames, True, True, False) == []
def test_controller_snapshots_may_skip_fast_can_samples():
policy, frames = prepare(fast_counter_step=2)
assert policy.update(110.06, frames, True, True, False) == [avh_request(frames[AVH_REQUEST][1], 2)]
@pytest.mark.parametrize('reason', ['late', 'new_template', 'manual', 'ack', 'moving', 'gas', 'gear', 'invalid', 'disabled', 'engaged', 'stale'])
def test_followup_aborts_permanently(reason):
policy, frames = prepare()
address, offset, value = {
'manual': (AVH_REQUEST, 2, 1), 'ack': (AVH_STATUS, 5, 32),
'moving': (0x13A, 2, 1), 'gas': (0x40, 4, 1), 'gear': (0x48, 3, 3),
'new_template': (AVH_REQUEST, 1, 11),
}.get(reason, (None, None, None))
if address is not None:
data = bytearray(frames[address][1])
data[1] = (data[1] + 1) & 15
data[offset] = value
data[0] = checksum(address, data)
frames[address] = (110.05, bytes(data))
if reason == 'stale':
frames[0x40] = (109, frames[0x40][1])
now = 110.08 if reason == 'late' else 110.06
assert policy.update(now, frames, reason != 'disabled', reason != 'invalid', reason == 'engaged') == []
assert policy.done
assert policy.update(111, frames, True, True, False) == []
def test_only_legacy_has_avh_safety_permission():
for car in CAR:
cp = CarInterface.get_non_essential_params(car)
assert bool(cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.AVH_STARTUP) == (car == CAR.SUBARU_LEGACY_2025)
def test_existing_required_messages_keep_alive_checks():
cp = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
parser = CarInterface.CarState.get_can_parsers(cp)[Bus.alt]
assert not parser.message_states[0x13A].ignore_alive
assert not parser.message_states[0x174].ignore_alive
assert parser.message_states[AVH_REQUEST].ignore_alive
assert parser.message_states[AVH_STATUS].ignore_alive
def test_controller_sends_only_when_opted_in():
cp = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
controller = CarController({}, cp)
cc = SimpleNamespace(enabled=False, latActive=False, longActive=False,
actuators=SimpleNamespace(as_builder=lambda: SimpleNamespace(steeringAngleDeg=0)),
hudControl=SimpleNamespace(leadVisible=False), cruiseControl=SimpleNamespace(cancel=False))
cs = SimpleNamespace(out=SimpleNamespace(canValid=True), avh_frames={})
toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=False, subaru_sng=False)
controller.frame = 1
_, sent = controller.update(cc, cs, 100_000_000_000, toggles)
assert not any(m[0] in (AVH_REQUEST, AVH_STATUS) for m in sent)
@@ -682,28 +682,6 @@ def test_ascent_angle_controller_reengages_immediately_after_manual_steering_sto
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CC.actuators.steeringAngleDeg
def test_ascent_reentry_rate_uses_last_transmitted_angle():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
controller.angle_handoff_active = True
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-1.45))
CS = SimpleNamespace(out=SimpleNamespace(
vEgoRaw=30.3, steeringAngleDeg=0.78, steeringRateDeg=-1.5, steeringTorque=56.0,
gearShifter=structs.CarState.GearShifter.drive, standstill=False,
))
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
parser.update([(1, [controller.lateral_angle(CC, CS)])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.78)
CS.out.steeringAngleDeg = 0.74
CS.out.steeringRateDeg = -1.99
parser.update([(2, [controller.lateral_angle(CC, CS)])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.53, abs=0.01)
def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
@@ -828,7 +806,7 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
CS = SimpleNamespace(out=SimpleNamespace(
vEgoRaw=0.9,
steeringAngleDeg=-57.0,
steeringRateDeg=0.0,
steeringRateDeg=-45.0,
steeringTorque=0.0,
steeringPressed=True,
gearShifter=structs.CarState.GearShifter.drive,
@@ -837,7 +815,6 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
for frame, steering_torque in enumerate((-79.0, -81.0, -170.0, -250.0, -250.0, 79.0, 81.0, 170.0, 250.0, 250.0), start=1):
CS.out.steeringRateDeg = 0.0 if frame == 1 else -45.0
CS.out.steeringTorque = steering_torque
CS.out.steeringPressed = abs(steering_torque) > 80.0
msg = controller.lateral_angle(CC, CS)
@@ -848,27 +825,6 @@ def test_outback_manual_steering_keeps_cooperative_angle_request():
assert controller._lkas_status_active(CC)
def test_outback_waits_for_manual_turn_to_settle_before_reentry():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-80.0))
CS = SimpleNamespace(out=SimpleNamespace(
vEgoRaw=7.3, steeringAngleDeg=-121.47, steeringRateDeg=126.5,
gearShifter=structs.CarState.GearShifter.drive, standstill=False,
))
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
for frame, (angle, rate, active) in enumerate([
(-121.47, 126.5, False), (-117.96, 122.5, False), (-88.65, 112.0, False),
(-0.24, 0.0, True),
], start=1):
CS.out.steeringAngleDeg = angle
CS.out.steeringRateDeg = rate
parser.update([(frame, [controller.lateral_angle(CC, CS)])])
assert bool(parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"]) == active
if not active:
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(angle, abs=0.01)
def test_ascent_hud_waits_for_angle_request():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
@@ -879,151 +835,6 @@ def test_ascent_hud_waits_for_angle_request():
assert controller._lkas_status_active(CC)
@pytest.mark.parametrize("platform", list(CAR))
def test_secondary_lkas_alert_is_detected_only_on_ascent(platform):
fingerprint = gen_empty_fingerprint()
fingerprint[CanBus.camera][0x3C4] = 8
CP = CarInterface.get_params(platform, fingerprint, [], False, False, False, None)
expected = platform == CAR.SUBARU_ASCENT_2023
assert bool(CP.flags & SubaruFlags.ASCENT_LKAS_ALERT) == expected
assert bool(CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.ASCENT_LKAS_ALERT) == expected
assert (0x3C4 in CarState.get_can_parsers(CP)[Bus.cam].addresses) == expected
@pytest.mark.parametrize("bus,length", [(0, 8), (1, 8), (2, 4), (2, 16)])
def test_ascent_secondary_alert_requires_native_camera_frame(bus, length):
fingerprint = gen_empty_fingerprint()
fingerprint[bus][0x3C4] = length
CP = CarInterface.get_params(CAR.SUBARU_ASCENT_2023, fingerprint, [], False, False, False, None)
assert not CP.flags & SubaruFlags.ASCENT_LKAS_ALERT
assert not CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.ASCENT_LKAS_ALERT
@pytest.mark.parametrize("active,visual_alert,masked", [
(True, structs.CarControl.HUDControl.VisualAlert.none, True),
(False, structs.CarControl.HUDControl.VisualAlert.none, False),
(True, structs.CarControl.HUDControl.VisualAlert.steerRequired, False),
(True, structs.CarControl.HUDControl.VisualAlert.fcw, True),
])
def test_ascent_secondary_hands_on_payload_is_filtered_only_during_active_lateral(active, visual_alert, masked):
raw = bytes.fromhex("ce02000104000000")
address, dat, bus = subarucan.create_ascent_lkas_alert(raw, active, visual_alert)
assert address == 0x3C4
assert bus == CanBus.main
assert dat[1] == raw[1]
assert dat[0] == ((address & 0xff) + (address >> 8) + sum(dat[1:])) & 0xff
assert dat[2:] == (bytes(6) if masked else raw[2:])
@pytest.mark.parametrize("payload", [
"000000000000", "000000110000", "000000100000", "1e0030000000",
"000704000000", "000000010000", "1b0002000000", "000002000000",
"0e0000000000", "190000100000", "1c0704000000", "190000110000",
"190000000000", "190000010000", "010104000000", "000604000000",
])
def test_ascent_secondary_alert_preserves_other_alerts_and_opaque_data(payload):
raw = bytearray(bytes.fromhex("0000" + payload))
raw[0] = (0xc4 + 3 + sum(raw[1:])) & 0xff
assert subarucan.create_ascent_lkas_alert(bytes(raw), True, structs.CarControl.HUDControl.VisualAlert.none)[1] == raw
@pytest.mark.parametrize("raw", [b"", bytes(7), bytes(9), bytes.fromhex("0002000104000000")])
def test_ascent_secondary_alert_does_not_transmit_invalid_data(raw):
assert subarucan.create_ascent_lkas_alert(raw, True, structs.CarControl.HUDControl.VisualAlert.none) is None
def test_ascent_secondary_alert_uses_fresh_camera_counter_and_actual_angle_request():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
CP.flags |= SubaruFlags.ASCENT_LKAS_ALERT.value
controller = CarController({}, CP)
CC = SimpleNamespace(latActive=True, hudControl=SimpleNamespace(visualAlert=structs.CarControl.HUDControl.VisualAlert.none))
CS = SimpleNamespace(es_lkas_alert_dat=bytes.fromhex("ce02000104000000"))
assert controller._ascent_lkas_alert(CC, CS)[1] == CS.es_lkas_alert_dat
assert controller._ascent_lkas_alert(CC, CS) is None
controller.angle_lkas_active = True
for counter in (3, 15, 0):
dat = bytearray(CS.es_lkas_alert_dat)
dat[1] = counter
dat[0] = (0xc4 + 3 + sum(dat[1:])) & 0xff
CS.es_lkas_alert_dat = bytes(dat)
msg = controller._ascent_lkas_alert(CC, CS)
assert msg[1][1] == counter
assert msg[1][2:] == bytes(6)
assert controller._ascent_lkas_alert(CC, CS) is None
@pytest.mark.parametrize("platform", [CAR.SUBARU_ASCENT, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_CROSSTREK_2025])
def test_secondary_alert_controller_does_not_touch_other_subarus(platform):
CP = CarInterface.get_non_essential_params(platform)
CP.flags |= SubaruFlags.ASCENT_LKAS_ALERT.value
controller = CarController({}, CP)
assert controller._ascent_lkas_alert(None, None) is None
@pytest.mark.parametrize("platform", [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_LEGACY_2025,
CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_ASCENT])
def test_stock_cruise_cancel_fresh_frame_gate_is_ascent_angle_only(platform):
CP = CarInterface.get_non_essential_params(platform)
controller = CarController({}, CP)
CC = structs.CarControl()
CC.cruiseControl.cancel = True
CS = SimpleNamespace(
out=structs.CarState(),
es_distance_msg=defaultdict(int, COUNTER=11),
es_dashstatus_msg=defaultdict(int),
es_lkas_state_msg=defaultdict(int),
es_infotainment_msg=defaultdict(int),
)
toggles = SimpleNamespace(subaru_sng=False)
cancel_messages = []
for frame in range(5):
_, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
cancel_messages.extend(msg for msg in sends if msg[0] == 0x221)
assert len(cancel_messages) == (1 if platform == CAR.SUBARU_ASCENT_2023 else 5)
assert all(msg[2] == (CanBus.alt if CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main) for msg in cancel_messages)
parser = CANParser(DBC[platform][Bus.pt], [("ES_Distance", 0)], cancel_messages[0][2])
parser.update([(1, [cancel_messages[0]])])
assert parser.vl["ES_Distance"]["COUNTER"] == 12
assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
def test_ascent_cancel_uses_fresh_stock_frames_and_handles_counter_rollover():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
CC = structs.CarControl()
CC.latActive = True
CS = SimpleNamespace(
out=structs.CarState(vEgoRaw=24.04, steeringAngleDeg=2.93, gearShifter="drive"),
es_distance_msg=defaultdict(int, COUNTER=14),
es_dashstatus_msg=defaultdict(int),
es_lkas_state_msg=defaultdict(int),
es_infotainment_msg=defaultdict(int),
)
CS.out.cruiseState.available = True
toggles = SimpleNamespace(subaru_sng=False)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_Distance", 0)], CanBus.alt)
cancel_counters = []
for frame, (stock_counter, cancel) in enumerate([
(14, False), (14, True), (15, True), (15, True), (0, True), (0, True),
(1, False), (1, True), (2, True),
]):
CS.es_distance_msg["COUNTER"] = stock_counter
CC.cruiseControl.cancel = cancel
_, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
cancel_messages = [msg for msg in sends if msg[0] == 0x221]
assert len(cancel_messages) <= 1
if cancel_messages:
parser.update([(frame + 1, cancel_messages)])
cancel_counters.append(parser.vl["ES_Distance"]["COUNTER"])
assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
assert cancel_counters == [0, 1, 3]
def test_other_angle_cars_keep_lateral_status_behavior():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
controller = CarController({}, CP)
@@ -90,8 +90,6 @@ class SubaruSafetyFlags(IntFlag):
FIXED_ANGLE_LIMITS = 128
STOP_START_BUTTON = 256
REDNECK_CRUISE = 512
AVH_STARTUP = 1024
ASCENT_LKAS_ALERT = 2048
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
@@ -111,7 +109,6 @@ class SubaruFlags(IntFlag):
LKAS_ANGLE = 64
D_PLATFORM = 128
D_PLATFORM_CAMERA = 256
ASCENT_LKAS_ALERT = 512
GLOBAL_ES_ADDR = 0x787
@@ -153,7 +153,7 @@ class CarController(CarControllerBase):
def _update_preap(self, CC, CS):
actuators = CC.actuators
can_sends = []
lat_active = CC.latActive and CS.hands_on_level < 3 and getattr(CS, "preap_lateral_authorized", False)
lat_active = CC.latActive and CS.hands_on_level < 3
if CC.cruiseControl.cancel and CS.cruiseEnabled:
CS.cruiseEnabled = False
@@ -169,10 +169,8 @@ class CarController(CarControllerBase):
CS.engagement.pedal_speed_kph = 0.0
if self.frame % 2 == 0:
requested_angle = float(np.clip(actuators.steeringAngleDeg,
CS.out.steeringAngleDeg - 20., CS.out.steeringAngleDeg + 20.))
self.apply_angle_last = apply_steer_angle_limits_vm(
requested_angle, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
lat_active, CarControllerParams, self.VM,
)
cntr = (self.frame // 2) % 16
+3 -39
View File
@@ -1,12 +1,12 @@
import copy
from cereal import custom
from opendbc.can import CANDefine, CANParser
from opendbc.car import Bus, create_button_events, structs
from opendbc.car import Bus, structs
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import CarStateBase
from opendbc.car.tesla.values import (
DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags,
CAR, LEGACY_CARS, TeslaFlags,
CAR, LEGACY_CARS,
)
from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap
from opendbc.car.tesla.preap.engagement import PreAPEngagement
@@ -19,20 +19,6 @@ TESLA_GAS_PRESS_ON = 0.8
TESLA_GAS_PRESS_OFF = 0.4
class TeslaScreenCANParser(CANParser):
def __init__(self):
super().__init__("tesla_model3_vehicle", [("UI_status2", 0), ("SCCM_rightStalk", 0)], CANBUS.vehicle)
def update(self, strings, sendcan=False):
if strings and not isinstance(strings[0], list | tuple):
strings = [strings]
# Match Panda's exact-length check before producing engagement button events.
return super().update([
(timestamp, [frame for frame in frames if (frame[0], len(frame[1])) in ((0x3DF, 8), (0x229, 3))])
for timestamp, frames in strings
], sendcan)
def update_tesla_gas_pressed(previous: bool, pedal_position: float) -> bool:
threshold = TESLA_GAS_PRESS_OFF if previous else TESLA_GAS_PRESS_ON
return float(pedal_position) > threshold
@@ -65,8 +51,6 @@ class CarState(CarStateBase):
self.cruise_buttons = 0
self.prev_cruise_buttons = 0
self.gas_pressed = False
self.active_touch_points = None
self.screen_cancel_pressed = False
self.msg_stw_actn_req = None
self.speed_units = "MPH"
self.cooperative_steering = any(
@@ -102,20 +86,6 @@ class CarState(CarStateBase):
return False
return super().update_button_enable(buttonEvents)
def update_screen_button(self, cp_vehicle):
events = []
for stalk_status in cp_vehicle.vl_all["SCCM_rightStalk"]["SCCM_rightStalkStatus"]:
cancel_pressed = int(stalk_status) in (1, 2) # UP_1, UP_2
events.extend(create_button_events(cancel_pressed, self.screen_cancel_pressed, {True: ButtonType.cancel}))
self.screen_cancel_pressed = cancel_pressed
for touch_points in cp_vehicle.vl_all["UI_status2"]["UI_activeTouchPoints"]:
touch_points = int(touch_points)
# Establish a baseline first; a touch already held during boot is not an engagement request.
if self.active_touch_points is not None:
events.extend(create_button_events(touch_points, self.active_touch_points, {3: ButtonType.lkas}))
self.active_touch_points = touch_points
return events
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
return update_preap(self, can_parsers)
@@ -211,15 +181,11 @@ class CarState(CarStateBase):
else:
pass
# Buttons # ToDo: add Gap adjust button
if self.CP.flags & TeslaFlags.AOL_SCREEN_BUTTON:
ret.buttonEvents = list(ret.buttonEvents) + self.update_screen_button(can_parsers[Bus.adas])
# Messages needed by carcontroller
self.das_control = copy.copy(cp_ap_party.vl["DAS_control"])
fp_ret = custom.StarPilotCarState.new_message()
if self.CP.flags & TeslaFlags.AOL_SCREEN_BUTTON:
fp_ret.cancelPressed = self.screen_cancel_pressed
return ret, fp_ret
@@ -313,7 +279,5 @@ class CarState(CarStateBase):
}
return {
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
**({Bus.adas: TeslaScreenCANParser()}
if CP.flags & TeslaFlags.AOL_SCREEN_BUTTON else {}),
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
}
+1 -10
View File
@@ -3,7 +3,7 @@ from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.tesla.carcontroller import CarController
from opendbc.car.tesla.carstate import CarState
from opendbc.car.tesla.radar_interface import RadarInterface
from opendbc.car.tesla.values import TeslaFlags, TeslaSafetyFlags, CANBUS, CAR, DBC, LEGACY_CARS
from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS
from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params
@@ -23,12 +23,6 @@ class CarInterface(CarInterfaceBase):
ret = super().get_params(candidate, fingerprint, car_fw, alpha_long, is_release, docs, starpilot_toggles)
if candidate == CAR.TESLA_MODEL_3 and getattr(starpilot_toggles, "tesla_cooperative_steering", False):
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.COOP_STEERING.value
if (ret.flags & TeslaFlags.HAS_VEHICLE_BUS and
getattr(starpilot_toggles, "tesla_aol_screen_tap_requested", False)):
ret.flags |= TeslaFlags.AOL_SCREEN_BUTTON.value
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_BUTTON.value
if getattr(starpilot_toggles, "tesla_aol_screen_brake_disengage_requested", False):
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE.value
return ret
@staticmethod
@@ -55,9 +49,6 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]
if candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y) and fingerprint[CANBUS.vehicle].get(0x3DF) == 8:
ret.flags |= TeslaFlags.HAS_VEHICLE_BUS.value
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
ret.steerAtStandstill = True
@@ -13,8 +13,6 @@ class PreAPEngagement:
self.enableDoublePull = double_pull_enabled
self.double_pull_window_ms = double_pull_window_ms
self.cruiseEnabled = False
self.lateralEnabled = False
self.lateralRearmRequired = False
self.enableLongControl = False
self.enableJustCC = False
self.pending_enable = False
@@ -30,8 +28,6 @@ class PreAPEngagement:
def handle_steering_disengage(self, steering_disengage: bool) -> None:
if steering_disengage and not self.prev_steering_disengage:
self.lateralEnabled = False
self.lateralRearmRequired = True
self.cruiseEnabled = False
self.enableLongControl = False
self.enableJustCC = False
@@ -49,8 +45,6 @@ class PreAPEngagement:
button_events: list[structs.CarState.ButtonEvent] = []
if cruise_buttons == CruiseButtons.MAIN and prev_cruise_buttons != CruiseButtons.MAIN:
self.lateralEnabled = True
self.lateralRearmRequired = False
if self.enableDoublePull:
self._handle_double_pull(curr_time_ms, v_ego, speed_units, use_pedal, pedal_long_allowed, long_control_allowed, di_cruise_state)
else:
@@ -81,8 +75,6 @@ class PreAPEngagement:
def check_can_engage(self, door_open: bool, gear_shifter, seatbelt_unlatched: bool) -> bool:
can_engage = not door_open and gear_shifter == structs.CarState.GearShifter.drive and not seatbelt_unlatched
if not can_engage:
self.lateralEnabled = False
self.lateralRearmRequired = True
self.cruiseEnabled = False
self.enableLongControl = False
self.enableJustCC = False
@@ -126,8 +118,6 @@ class PreAPEngagement:
((curr_time_ms - self.preap_last_cc_spoof_ms) < SPOOF_ECHO_WINDOW_MS)
be.type = ButtonType.unknown if is_echo else ButtonType.cancel
if not is_echo:
self.lateralEnabled = False
self.lateralRearmRequired = True
self.cruiseEnabled = False
self.enableLongControl = False
self.enableJustCC = False
@@ -155,3 +145,4 @@ class PreAPEngagement:
def _capture_target_speed(v_ego: float, speed_units: str) -> float:
speed_uom_kph = CV.MPH_TO_KPH if speed_units == "MPH" else 1.0
return max(int(v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph, 0.0)
@@ -1,21 +0,0 @@
from opendbc.car import structs
from opendbc.safety import ALTERNATIVE_EXPERIENCE
def preap_lateral_authorized(CP, CS, panda_states, panda_states_valid: bool) -> bool:
"""Match Pre-AP's existing safety authorization without treating software CC availability as ACC main."""
if not panda_states_valid or CS.out.gearShifter != structs.CarState.GearShifter.drive or CS.out.doorOpen or CS.out.steeringDisengage:
return False
if CS.engagement.lateralRearmRequired:
return False
config = CP.safetyConfigs[0]
matching = [p for p in panda_states if p.safetyModel == config.safetyModel and p.safetyParam == config.safetyParam]
if len(matching) != 1 or matching[0].safetyRxChecksInvalid:
return False
panda = matching[0]
# Physical cancel/override/gear changes clear this latch immediately, whereas
# Panda telemetry can lag. Longitudinal software cancellation leaves it intact.
stalk_authorized = CS.engagement.lateralEnabled and panda.controlsAllowed
stock_main = CS.di_cruise_state in ("STANDBY", "ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
aol_authorized = bool(panda.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) and stock_main
return bool(stalk_authorized or aol_authorized)
@@ -1,36 +0,0 @@
from types import SimpleNamespace
import pytest
from opendbc.car import structs
from opendbc.car.tesla.carcontroller import CarController
from opendbc.car.tesla.interface import CarInterface
from opendbc.car.tesla.values import CAR, DBC
@pytest.mark.parametrize('direction', [-1., 1.])
def test_preap_stalled_rack_request_stays_within_legacy_tracking_envelope(direction):
cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
controller = CarController(DBC[cp.carFingerprint], cp)
controller.stock_cc = None
cs = SimpleNamespace(out=SimpleNamespace(vEgoRaw=3., steeringAngleDeg=0.),
hands_on_level=0, preap_lateral_authorized=True, cruiseEnabled=False)
cc = structs.CarControl.new_message()
cc.latActive = True
cc.actuators.steeringAngleDeg = direction * 100.
previous = 0.
for frame in range(100):
output, _ = controller.update(cc.as_reader(), cs, frame * 10000000, None)
assert abs(output.steeringAngleDeg) <= 20.
assert abs(output.steeringAngleDeg - previous) <= 5.
previous = output.steeringAngleDeg
assert previous == direction * 20.
cs.out.steeringAngleDeg = -direction * 50.
output, _ = controller.update(cc.as_reader(), cs, 1000000000, None)
assert abs(output.steeringAngleDeg - previous) <= 5.
cc.latActive = False
controller.frame = 102
output, _ = controller.update(cc.as_reader(), cs, 1020000000, None)
assert output.steeringAngleDeg == cs.out.steeringAngleDeg
@@ -1,155 +0,0 @@
from types import SimpleNamespace
import pytest
from cereal import custom
from opendbc.can import CANPacker
from opendbc.car import Bus, gen_empty_fingerprint
from opendbc.car.tesla.carstate import ButtonType, CarState
from opendbc.car.tesla.interface import CarInterface
from opendbc.car.tesla.values import CANBUS, CAR, TeslaFlags, TeslaSafetyFlags
def screen_params(candidate=CAR.TESLA_MODEL_3, enabled=True, bus=CANBUS.vehicle, length=8, alpha_long=False):
fingerprint = gen_empty_fingerprint()
if bus is not None:
fingerprint[bus][0x3DF] = length
toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, trailer_load_kg=0.0)
return CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False, toggles)
@pytest.mark.parametrize("candidate", tuple(CAR))
@pytest.mark.parametrize("enabled", (False, True))
def test_detection_and_safety_flags_are_model_3_y_only(candidate, enabled):
cp = screen_params(candidate, enabled)
detected = candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y)
assert bool(cp.flags & TeslaFlags.HAS_VEHICLE_BUS) == detected
assert bool(cp.flags & TeslaFlags.AOL_SCREEN_BUTTON) == (detected and enabled)
assert any(config.safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON for config in cp.safetyConfigs) == (detected and enabled)
@pytest.mark.parametrize(("bus", "length"), ((None, 8), (0, 8), (2, 8), (1, 7), (1, 4)))
def test_harness_requires_correct_bus_and_length(bus, length):
cp = screen_params(bus=bus, length=length)
assert not cp.flags & (TeslaFlags.HAS_VEHICLE_BUS | TeslaFlags.AOL_SCREEN_BUTTON)
assert not cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON
assert Bus.adas not in CarState.get_can_parsers(cp)
@pytest.mark.parametrize("alpha_long", (False, True))
def test_gesture_does_not_select_longitudinal(alpha_long):
baseline = screen_params(enabled=False, alpha_long=alpha_long)
enabled = screen_params(alpha_long=alpha_long)
assert enabled.openpilotLongitudinalControl == baseline.openpilotLongitudinalControl
assert enabled.pcmCruise == baseline.pcmCruise
assert enabled.safetyConfigs[0].safetyParam == baseline.safetyConfigs[0].safetyParam | TeslaSafetyFlags.AOL_SCREEN_BUTTON
@pytest.mark.parametrize("enabled", (False, True))
def test_screen_brake_safety_flag_requires_the_gesture_feature(enabled):
fingerprint = gen_empty_fingerprint()
fingerprint[1][0x3DF] = 8
toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, tesla_aol_screen_brake_disengage_requested=True,
trailer_load_kg=0.0)
cp = CarInterface.get_params(CAR.TESLA_MODEL_3, fingerprint, [], False, False, False, toggles)
assert bool(cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE) == enabled
def test_parser_only_added_with_setting_and_detected_addon():
assert Bus.adas not in CarState.get_can_parsers(screen_params(enabled=False))
parser = CarState.get_can_parsers(screen_params())[Bus.adas]
assert parser.bus == CANBUS.vehicle
assert parser.message_states[0x3DF].ignore_alive
@pytest.mark.parametrize(("counts", "expected"), (
([0, 3, 3, 3, 0, 0, 3, 0], [True, False, True, False]),
([3, 3, 0, 3, 0], [False, True, False]),
([0, 1, 2, 4, 5, 0], []),
))
def test_touch_edges_and_startup_baseline(counts, expected):
cp = screen_params()
fp_cp = custom.StarPilotCarParams.new_message()
cs = CarState(cp, fp_cp)
parser = CarState.get_can_parsers(cp)[Bus.adas]
packer = CANPacker("tesla_model3_vehicle")
events = []
for i, count in enumerate(counts):
parser.update([[int((i + 1) * 0.5e9), [packer.make_can_msg("UI_status2", CANBUS.vehicle, {"UI_activeTouchPoints": count})]]])
events.extend(cs.update_screen_button(parser))
assert [event.pressed for event in events if event.type == ButtonType.lkas] == expected
parser.update([[60_000_000_000, []]])
assert cs.update_screen_button(parser) == []
assert parser.can_valid
assert not parser.bus_timeout
def test_multiple_touch_edges_in_one_update_are_not_lost():
cp = screen_params()
fp_cp = custom.StarPilotCarParams.new_message()
cs = CarState(cp, fp_cp)
parser = CarState.get_can_parsers(cp)[Bus.adas]
packer = CANPacker("tesla_model3_vehicle")
parser.update([[i * 10_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]]
for i, count in enumerate((0, 3, 0, 3, 0), 1)])
assert [event.pressed for event in cs.update_screen_button(parser) if event.type == ButtonType.lkas] == [True, False, True, False]
@pytest.mark.parametrize("length", (0, 3, 4, 5, 6, 7, 9, 12))
def test_malformed_touch_frames_do_not_produce_button_events(length):
cp = screen_params()
cs = CarState(cp, custom.StarPilotCarParams.new_message())
parser = CarState.get_can_parsers(cp)[Bus.adas]
packer = CANPacker("tesla_model3_vehicle")
parser.update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 0})]]])
cs.update_screen_button(parser)
address, data, bus = packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 3})
parser.update([[2_000_000_000, [(address, data[:length].ljust(length, b"\x00"), bus)]]])
assert cs.update_screen_button(parser) == []
assert cs.active_touch_points == 0
parser.update([[3_000_000_000, [(address, data, bus)]]])
assert any(event.type == ButtonType.lkas and event.pressed for event in cs.update_screen_button(parser))
def test_button_is_appended_without_changing_cruise_state():
cp = screen_params()
fp_cp = custom.StarPilotCarParams.new_message()
cs = CarState(cp, fp_cp)
parsers = CarState.get_can_parsers(cp)
parsers[Bus.party].vl["DI_state"]["DI_cruiseState"] = 0
packer = CANPacker("tesla_model3_vehicle")
for count in (0, 3):
parsers[Bus.adas].update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]]])
ret, _ = cs.update(parsers, SimpleNamespace())
assert any(event.type == ButtonType.lkas and event.pressed for event in ret.buttonEvents)
assert not ret.cruiseState.enabled
assert not ret.cruiseState.available
def test_addon_stalk_cancel_events_and_held_state():
cp = screen_params()
cs = CarState(cp, custom.StarPilotCarParams.new_message())
parsers = CarState.get_can_parsers(cp)
packer = CANPacker("tesla_model3_vehicle")
events = []
for i, status in enumerate((0, 1, 1, 2, 0, 3, 4, 0, 2, 0), 1):
parsers[Bus.adas].update([[i * 10_000_000, [packer.make_can_msg("SCCM_rightStalk", 1, {"SCCM_rightStalkStatus": status})]]])
ret, fp_ret = cs.update(parsers, SimpleNamespace())
assert fp_ret.cancelPressed == (status in (1, 2))
events.extend(ret.buttonEvents)
assert [event.pressed for event in events if event.type == ButtonType.cancel] == [True, False, True, False]
parsers[Bus.adas].update([[60_000_000_000, []]])
assert parsers[Bus.adas].can_valid
assert not parsers[Bus.adas].bus_timeout
@pytest.mark.parametrize(("bus", "length"), ((0, 3), (2, 3), (1, 2), (1, 4), (1, 8)))
def test_wrong_bus_or_length_stalk_frames_do_not_cancel(bus, length):
cp = screen_params()
cs = CarState(cp, custom.StarPilotCarParams.new_message())
parser = CarState.get_can_parsers(cp)[Bus.adas]
packer = CANPacker("tesla_model3_vehicle")
address, data, _ = packer.make_can_msg("SCCM_rightStalk", bus, {"SCCM_rightStalkStatus": 1})
parser.update([[1_000_000_000, [(address, data[:length].ljust(length, b"\x00"), bus)]]])
assert cs.update_screen_button(parser) == []
assert not cs.screen_cancel_pressed
-4
View File
@@ -144,14 +144,10 @@ class TeslaSafetyFlags(IntFlag):
FLAG_EXTERNAL_PANDA = 4
FLAG_HW1 = 8
COOP_STEERING = 256
AOL_SCREEN_BUTTON = 512
AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024
class TeslaFlags(IntFlag):
LONG_CONTROL = 1
HAS_VEHICLE_BUS = 2
AOL_SCREEN_BUTTON = 4
class CruiseButtons:
@@ -1,59 +0,0 @@
"""Continental ARS510 ACC-target radar, one point per radar cycle.
Adapted from ds-sebastian/ars510-radar at fb5b29c2b5999f34afeb6f7ac494a73949d66c67.
Copyright (c) 2026 ARS510 radar contributors. MIT license; see THIRD_PARTY_NOTICES.md.
"""
from math import nan
from opendbc.can import CANParser
from opendbc.car.structs import RadarData
from opendbc.car.interfaces import RadarInterfaceBase
RADAR_DBC = "toyota_tss2_ars510"
TARGET_ADDRS = {0x235, 0x237, 0x239}
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.rcp = CANParser(RADAR_DBC, [("ACC_TARGET_1", 50), ("ACC_TARGET_2", 50), ("ACC_TARGET_3", 50)], 1)
self.trigger_msg = 0x237
self.updated_messages = set()
self.cycle = None
self.last_record_t = None
def update(self, can_strings):
self.frame += 1
packets = [(nanos, [(addr, dat, src) for addr, dat, src in frames if addr not in TARGET_ADDRS or len(dat) == 8])
for nanos, frames in can_strings]
self.updated_messages.update(self.rcp.update(packets))
t = can_strings[-1][0] * 1e-9 if can_strings else None
cycle = self.rcp.vl["ACC_TARGET_3"]["TIMESTAMP"]
clock_seen = bool(self.rcp.ts_nanos["ACC_TARGET_3"]["TIMESTAMP"])
if self.trigger_msg not in self.updated_messages or not clock_seen or cycle == self.cycle:
# Keep card publishing through startup or a stopped cycle clock, without erasing healthy tracks between scans.
if t is None or self.frame % 5 != 0 or (self.last_record_t is not None and t - self.last_record_t <= 0.5):
return None
ret = RadarData()
ret.errors.radarUnavailableTemporary = self.last_record_t is not None
return ret
self.cycle = cycle
self.updated_messages.clear()
self.last_record_t = t
ret = RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
return ret
target, position = self.rcp.vl["ACC_TARGET_1"], self.rcp.vl["ACC_TARGET_2"]
if target["STATE"] == 5 and position["PRESENT"]:
pt = RadarData.RadarPoint()
pt.trackId = int(target["TARGET_ID"])
pt.dRel = position["LONG_DIST"]
pt.yRel = position["LAT_DIST"]
pt.vRel = target["REL_SPEED"]
pt.aRel = pt.yvRel = nan
pt.measured = True
ret.points = [pt]
return ret
@@ -1,230 +0,0 @@
"""Toyota / Continental ARS510 front radar (RAV4 2022+, fwdRadar 8821F0R03100): object list to RadarData.
The radar sends its object list on bus 1 as one 742-byte record split over 106 frames on 0x80 (consecutive-frame
numbers wrap every 16 frames, so it is reassembled here instead of read through a DBC). Each of its 20 slots holds one
object; the radar keeps an object in its slot while the slot's age counts up, which gives the track IDs.
The slot speed has slow, correlated errors at range (false closings of 1-10 s beyond ~40 m). Their size follows the
radar's 240|7 code. One Kalman filter per track fuses the slot speed, weighted by that code, with the radar's own ACC target
(0x235 / 0x237) for the track it describes. radard then filters as for any radar. radard's lead match weights
lateral position only through the camera's uncertainty, so beyond 15 m a track a lane away from the yaw-predicted path
is withheld.
Adapted from ds-sebastian/ars510-radar at fb5b29c2b5999f34afeb6f7ac494a73949d66c67.
Copyright (c) 2026 ARS510 radar contributors. MIT license; see THIRD_PARTY_NOTICES.md.
"""
import zlib
from math import isfinite, nan, pi
from opendbc.car import structs
from opendbc.car.interfaces import RadarInterfaceBase
RADAR_BUS, CAR_BUS = 1, 0
SPEED_ADDR, YAW_ADDR, OBJECTS_ADDR, ACC_SPEED_ADDR, ACC_POS_ADDR = 0xB4, 0x24, 0x80, 0x235, 0x237
RECORD_FRAMES, RECORD_LEN, SLOT_START, SLOT_LEN, SLOTS = 106, 742, 17, 36, 20
IDLE_SLOT = bytes.fromhex("FCE00000A0F07F00FFFDF71FFFA100F807000F0008000000000000000000000000000000")
EGO_MAX_AGE_S = 0.5
PUBLISH_AGE = 60 # ~3.6 s: young tracks have unconverged range and speed
RANGE_GAIN = 0.1 # velocity-aided range: cuts 1.5 s range walks by about a third
# Kalman speed filter (one state: the lead's over-ground speed)
SIGMA_PER_CODE = 0.045 # m/s per 240|7 count, against the radar's ACC target
YOUNG_SCALE, YOUNG_AGE = 1.8, 100 # young tracks err more than 240|7 says
ACC_SIGMA = 0.13 # m/s, the radar's ACC target speed (its measured noise; blended with the object list, which damps the ACC target's rare slides)
LEAD_ACCEL = 1.5 # m/s^2, process noise
GATE_SIGMA = 3.0 # innovations beyond 3 sigma count as 3 sigma
PUBLISH_STD = 0.75 # m/s: a new track is published once its speed is this certain
PATH_GATE, PATH_GATE_MIN_RANGE = 2.5, 15.0 # m: off the yaw-predicted path by more than this = another lane; closer in, cut-ins stay
FAR_ACC_ONLY = 60.0 # m: beyond this only the car the radar's ACC follows is published, as Toyota's own ACC uses it
STILL, STILL_GATE = 0.5, 1.5 # m/s, m: a stationary track (parked car, roadside object) is path-gated at every range, so it stays out of the lane
def bits(b: bytes, start: int, length: int) -> int:
return (int.from_bytes(b, "little") >> start) & ((1 << length) - 1)
def be_bits(b: bytes, start: int, length: int) -> int:
return (int.from_bytes(b, "big") >> start) & ((1 << length) - 1)
class Ars510Radar:
"""Raw CAN frames in, points (trackId, dRel, yRel, vRel) out once per 0x80 record."""
def __init__(self):
self.chunks, self.record_t, self.last_frame_t = [], 0.0, None
self.v_ego = self.yaw = None # (time, m/s), (time, rad/s left positive)
self.slots = {} # slot -> (radar track id, age, time)
self.next_tid = 1
self.acc_speed = self.acc_pos = self.acc_assoc = None
self.kf = {} # tid -> (time, speed, variance)
self.rng = {} # tid -> (time, dRel, vRel)
self.published = set()
def update(self, t: float, bus: int, addr: int, dat: bytes):
if bus == CAR_BUS and addr == SPEED_ADDR and len(dat) >= 7:
self.v_ego = (t, int.from_bytes(dat[5:7], "big") * 0.01 / 3.6)
elif bus == CAR_BUS and addr == YAW_ADDR and len(dat) >= 2:
self.yaw = (t, (((dat[0] & 0x03) << 8 | dat[1]) * 0.244 - 125.0) * pi / 180.0)
elif bus == RADAR_BUS and addr in (ACC_SPEED_ADDR, ACC_POS_ADDR):
if len(dat) != 8 or (not dat[1] & 4 if addr == ACC_SPEED_ADDR else dat[2:] == bytes.fromhex("003E80000000")):
self.acc_speed = self.acc_pos = None # idle
elif addr == ACC_SPEED_ADDR:
self.acc_speed = (t, (be_bits(dat, 29, 11) - 1024) * 0.125)
else:
self.acc_pos = (t, be_bits(dat, 39, 13) * 0.025, (be_bits(dat, 27, 12) - 2000) * 0.01)
elif bus == RADAR_BUS and addr == OBJECTS_ADDR:
record = self.reassemble(t, dat)
if record is not None and int.from_bytes(record[737:741], "little") == zlib.crc32(record[1:737]):
return self.points(self.record_t, record)
return None
def reassemble(self, t, dat):
"""The 106 frames of one record joined (bytes 1-7 of each); restarts on a first frame (0x12 0xE4)."""
if len(dat) != 8 or (self.chunks and t < self.last_frame_t):
self.chunks = []
if len(dat) != 8:
return None
if dat[0] == 0x12 and dat[1] == 0xE4:
self.chunks, self.record_t = [bytes(dat[1:8])], t
elif self.chunks:
self.chunks.append(bytes(dat[1:8]))
else:
return None
self.last_frame_t = t
if len(self.chunks) != RECORD_FRAMES:
return None
record, self.chunks = b"".join(self.chunks), []
return record if len(record) == RECORD_LEN else None
def track_id(self, t, slot, age):
prev = self.slots.get(slot)
if prev is not None and t == prev[2]:
return prev[0]
if prev is None or t - prev[2] > 0.3 or not (age > prev[1] or age == prev[1] == 126):
prev = (self.next_tid, age, t)
self.next_tid += 1
self.slots[slot] = (prev[0], age, t)
return prev[0]
def points(self, t, record):
v_ego = self.v_ego[1] if self.v_ego is not None and abs(t - self.v_ego[0]) <= EGO_MAX_AGE_S else None
objs = []
for slot in range(SLOTS):
s = record[SLOT_START + SLOT_LEN * slot:SLOT_START + SLOT_LEN * (slot + 1)]
if s == IDLE_SLOT:
continue
age, lat = bits(s, 24, 7), bits(s, 44, 12) - 2048
init_template = age == 1 and bits(s, 56, 7) == 0 and bits(s, 216, 6) == 0
# over-ground speed: 0.149 m/s per code against 0xB4 ego speed (DBC factor 0.15)
objs.append(dict(tid=self.track_id(t, slot, age), age=age, d=(bits(s, 32, 12) - 160) / 16, y=lat * 0.015,
vg=(bits(s, 64, 10) - 510.5) * 0.149, unc=bits(s, 240, 7),
valid=age >= 1 and not init_template and abs(lat) < 2000))
tracks = {o["tid"]: o for o in objs if o["valid"]}
acc_tid = self.acc_match(t, tracks)
yaw = self.yaw[1] if self.yaw is not None and abs(t - self.yaw[0]) <= EGO_MAX_AGE_S else None
out = []
for o in objs:
if not o["valid"]:
continue
tid, vrel, std = o["tid"], nan, 0.0
if v_ego is not None:
readings = [(self.acc_speed[1] + v_ego, ACC_SIGMA)] if tid == acc_tid else []
speed, std = self.speed_filter(tid, t, o, readings)
vrel = speed - v_ego
d_rel = self.fused_range(tid, t, self.acc_pos[1] if tid == acc_tid else o["d"], vrel) # ACC distance for its track
if o["age"] < PUBLISH_AGE or (tid not in self.published and std > PUBLISH_STD) or not isfinite(vrel):
continue # a point needs a fresh ego speed: one NaN would poison radard's filter
still = abs(speed) < STILL and d_rel <= PATH_GATE_MIN_RANGE
if tid != acc_tid and (d_rel > FAR_ACC_ONLY or (yaw is not None and (d_rel > PATH_GATE_MIN_RANGE or still) and abs(
o["y"] - yaw / max(v_ego, 2.0 if still else 5.0) * d_rel * d_rel / 2) > (STILL_GATE if still else PATH_GATE))):
continue # far: only the car the radar's ACC follows is published (the camera covers the rest); closer in: an object off the path is another lane's
self.published.add(tid)
out.append((tid, d_rel, o["y"], vrel))
self.prune(t)
return out
def speed_filter(self, tid, t, o, readings):
"""One-state Kalman filter on the over-ground speed; every reading weighted by its own sigma."""
sig = SIGMA_PER_CODE * max(o["unc"], 1)
if o["age"] < YOUNG_AGE:
sig *= 1.0 + (YOUNG_SCALE - 1.0) * min(max((YOUNG_AGE - o["age"]) / (YOUNG_AGE - PUBLISH_AGE), 0.0), 1.0)
prev = self.kf.get(tid)
if prev is None or not 0.0 < t - prev[0] <= 0.5:
self.kf[tid] = (t, o["vg"], sig * sig)
return o["vg"], sig
_, v, p = prev
p += (LEAD_ACCEL * (t - prev[0])) ** 2
for z, r in [(o["vg"], sig)] + readings:
s = p + r * r
innov = max(min(z - v, GATE_SIGMA * s ** 0.5), -GATE_SIGMA * s ** 0.5)
v, p = v + p / s * innov, p * (1.0 - p / s)
self.kf[tid] = (t, v, p)
return v, p ** 0.5
def fused_range(self, tid, t, d, vrel):
prev = self.rng.get(tid)
if prev is not None and isfinite(vrel) and isfinite(prev[2]) and 0.0 < t - prev[0] <= 0.5:
pred = prev[1] + 0.5 * (prev[2] + vrel) * (t - prev[0])
d = pred + RANGE_GAIN * (d - pred)
self.rng[tid] = (t, d, vrel)
return d
def acc_match(self, t, tracks):
"""The track the radar's ACC target describes: matched by position, kept while both persist."""
if self.acc_speed is None or self.acc_pos is None or max(abs(t - self.acc_speed[0]), abs(t - self.acc_pos[0])) > 0.1:
self.acc_assoc = None
return None
_, ax, ay = self.acc_pos
rs = max(12.0, 0.4 * ax) # the object list reads far cars short of the ACC distance; 10-20 % of new tracks by more than 10 m
costs = sorted((abs(o["d"] - ax) / rs + abs(o["y"] - ay) / 0.5, tid, o["age"]) for tid, o in tracks.items())
if self.acc_assoc is not None:
tid0, ax0, ay0 = self.acc_assoc
own = next((c for c, tid, _ in costs if tid == tid0), None)
if own is not None and own < 4.0 and abs(ax - ax0) <= 8.0 and abs(ay - ay0) <= 1.0:
self.acc_assoc = (tid0, ax, ay)
return tid0
self.acc_assoc = None
if not costs or costs[0][0] >= 1.0 or costs[0][2] < 20 or (len(costs) > 1 and costs[1][0] - costs[0][0] <= 1.0):
return None
self.acc_assoc = (costs[0][1], ax, ay)
return costs[0][1]
def prune(self, t):
for store in (self.kf, self.rng):
if len(store) > 400:
for k in [k for k, v in store.items() if v[0] < t - 5.0]:
store.pop(k)
if len(self.published) > 400:
# A live allocation can outlast its recent speed-filter entry after invalid geometry.
self.published &= set(self.kf) | {state[0] for state in self.slots.values()}
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.radar = Ars510Radar()
self.last_record_t = None
def update(self, can_packets):
self.frame += 1
points, t = None, None
for nanos, frames in can_packets:
t = nanos * 1e-9
for address, dat, src in frames:
out = self.radar.update(t, src, address, dat)
if out is not None:
points, self.last_record_t = out, t
if points is None: # between scans: report only at 20 Hz, and only before the first scan or once the radar is stale
if t is None or self.frame % 5 != 0 or (self.last_record_t is not None and t - self.last_record_t <= 0.5):
return None
ret = structs.RadarData()
ret.errors.radarUnavailableTemporary = self.last_record_t is not None
return ret
ret = structs.RadarData()
pts = []
for track_id, d_rel, y_rel, v_rel in points:
pt = structs.RadarData.RadarPoint()
pt.trackId, pt.dRel, pt.yRel, pt.vRel = track_id, d_rel, y_rel, v_rel
pt.aRel = pt.yvRel = nan
pt.measured = True
pts.append(pt)
ret.points = pts
return ret
@@ -10,8 +10,8 @@ from opendbc.car.secoc import add_mac, build_sync_mac
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.toyota import toyotacan
from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb
CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS
from opendbc.can import CANPacker
Ecu = structs.CarParams.Ecu
@@ -47,7 +47,6 @@ TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
MAX_STEER_RATE = 100 # deg/s
MAX_STEER_RATE_FRAMES = 17 # tx control frames needed before torque can be cut
COROLLA_MAX_STEER_RATE = 80
# EPS allows user torque above threshold for 50 frames before permanently faulting
MAX_USER_TORQUE = 500
@@ -68,11 +67,6 @@ def is_ths_hybrid(CP) -> bool:
return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP)
def uses_rav4_hybrid_sdsu_longitudinal(CP) -> bool:
return bool(CP.carFingerprint == CAR.TOYOTA_RAV4H and CP.openpilotLongitudinalControl and
CP.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LONG_FILTER.value)
def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
highlander_sdsu = (
CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and
@@ -83,12 +77,12 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
) or highlander_sdsu)
def get_toyota_lat_active(requested_active: bool, steering_torque: float) -> bool:
return requested_active and abs(steering_torque) < MAX_USER_TORQUE
def get_toyota_lat_active(car_fingerprint, requested_active: bool, steering_torque: float,
steering_pressed: bool) -> bool:
if not requested_active or abs(steering_torque) >= MAX_USER_TORQUE:
return False
def get_toyota_steer_rate_limit(car_fingerprint) -> int:
return COROLLA_MAX_STEER_RATE if car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE
return not (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and steering_pressed)
def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool:
@@ -112,7 +106,7 @@ def get_long_tune(CP, params):
kiV = [0.5, 0.25]
k_f = 1.0
if is_ths_hybrid(CP) or uses_rav4_hybrid_sdsu_longitudinal(CP):
if is_ths_hybrid(CP):
k_f = 0.8 if CP.carFingerprint in LEGACY_PRIUS_CAR else 1.0
elif CP.carFingerprint not in TSS2_CAR:
kiBP = [0., 5., 35.]
@@ -258,7 +252,6 @@ class CarController(CarControllerBase):
self.standstill_req = False
self.permit_braking = True
self.steer_rate_counter = 0
self.steer_rate_limit = get_toyota_steer_rate_limit(self.CP.carFingerprint)
self.distance_button = 0
# *** start long control state ***
@@ -283,7 +276,7 @@ class CarController(CarControllerBase):
self._brake_hold_counter = 0
def _compute_interceptor_gas_cmd(self, CC, CS):
if self.brake_hold_active or not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
return 0.0
if CS.out.standstill:
@@ -332,36 +325,16 @@ class CarController(CarControllerBase):
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
CS.out.gearShifter not in (PARK, REVERSE))
if not brake_hold_allowed:
self.reset_auto_hold_state()
elif not self.brake_hold_active:
if CS.out.brakePressed:
self._brake_hold_counter += 1
self.brake_hold_active = self._brake_hold_counter > activation_frames
else:
self._brake_hold_counter = 0
return self.brake_hold_active
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
CS.out.gearShifter not in (PARK, REVERSE))
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
self._brake_hold_counter += 1
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer
self.brake_hold_active = self._brake_hold_counter > activation_frames
elif not brake_hold_allowed:
self._brake_hold_counter = 0
self.brake_hold_active = False
if self.frame % 2 == 0:
return [toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active)]
return []
return self.brake_hold_active
def reset_auto_hold_state(self):
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
self.standstill_req = False
self._brake_hold_counter = 0
self.brake_hold_active = False
@@ -370,7 +343,8 @@ class CarController(CarControllerBase):
stopping = actuators.longControlState == LongCtrlState.stopping
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
lat_active = get_toyota_lat_active(CC.latActive, CS.out.steeringTorque)
lat_active = get_toyota_lat_active(self.CP.carFingerprint, CC.latActive,
CS.out.steeringTorque, CS.out.steeringPressed)
if len(CC.orientationNED) == 3:
self.pitch.update(CC.orientationNED[1])
@@ -397,7 +371,7 @@ class CarController(CarControllerBase):
# >100 degree/sec steering fault prevention
self.steer_rate_counter, apply_steer_req = common_fault_avoidance(
abs(CS.out.steeringRateDeg) >= self.steer_rate_limit, lat_active,
abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES,
)
@@ -461,10 +435,7 @@ class CarController(CarControllerBase):
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
if uses_toyota_auto_hold_aeb(self.CP):
can_sends.extend(self.create_auto_brake_hold_messages(CS))
else:
self.update_auto_hold_state(CS, pcm_cancel_cmd)
self.update_auto_hold_state(CS, pcm_cancel_cmd)
else:
self.reset_auto_hold_state()
@@ -574,7 +545,7 @@ class CarController(CarControllerBase):
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
if self.brake_hold_active:
pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL
self.permit_braking = True
self.standstill_req = True
@@ -9,7 +9,6 @@ from opendbc.car.interfaces import CarStateBase
from opendbc.car.toyota.values import ToyotaFlags, ToyotaStarPilotFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
SECOC_CAR, LEGACY_PRIUS_CAR
from opendbc.safety import ALTERNATIVE_EXPERIENCE
ButtonType = structs.CarState.ButtonEvent.Type
SteerControlType = structs.CarParams.SteerControlType
@@ -91,11 +90,6 @@ class CarState(CarStateBase):
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value
self.auto_brake_hold = bool(
self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value and
getattr(self.CP, "alternativeExperience", 0) & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
)
self.pre_collision_2 = {}
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
cp = can_parsers[Bus.pt]
@@ -231,9 +225,6 @@ class CarState(CarStateBase):
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
if self.auto_brake_hold:
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
@@ -318,10 +309,6 @@ class CarState(CarStateBase):
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
pt_messages.append(("PCM_CRUISE_4", 1))
if (CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value and
getattr(CP, "alternativeExperience", 0) & ALTERNATIVE_EXPERIENCE.ALLOW_AEB):
cam_messages.append(("PRE_COLLISION_2", 50))
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
+6 -21
View File
@@ -1,11 +1,10 @@
from opendbc.car import Bus, structs, get_safety_config, uds
from opendbc.car.toyota.carstate import CarState
from opendbc.car.toyota.carcontroller import CarController, uses_rav4_hybrid_sdsu_longitudinal
from opendbc.car.toyota.carcontroller import CarController
from opendbc.car.toyota.radar_interface import RadarInterface
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, \
uses_toyota_auto_hold_aeb, ARS510_RADAR_CAR
ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS
from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.safety import ALTERNATIVE_EXPERIENCE
@@ -19,17 +18,6 @@ class CarInterface(CarInterfaceBase):
CarController = CarController
RadarInterface = RadarInterface
@classmethod
def get_params(cls, candidate, fingerprint, car_fw, alpha_long, is_release, docs, starpilot_toggles):
ret = super().get_params(candidate, fingerprint, car_fw, alpha_long, is_release, docs, starpilot_toggles)
radar_mode = getattr(starpilot_toggles, "toyota_radar_mode_requested", 0)
if candidate in ARS510_RADAR_CAR and radar_mode in (1, 2):
ret.flags |= ToyotaFlags.ARS510_RADAR.value
if radar_mode == 2:
ret.flags |= ToyotaFlags.ARS510_FUSED.value
ret.radarUnavailable = False
return ret
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX
@@ -113,7 +101,7 @@ class CarInterface(CarInterfaceBase):
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
stop_and_go = candidate != CAR.TOYOTA_AVALON
elif candidate in ANGLE_CONTROL_CAR:
elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME):
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP = [0.0]
ret.lateralTuning.pid.kpBP = [0.0]
@@ -177,9 +165,7 @@ class CarInterface(CarInterfaceBase):
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
ret.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.ALLOW_AEB
if uses_toyota_auto_hold_aeb(ret)
else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
if not ret.openpilotLongitudinalControl:
@@ -187,8 +173,7 @@ class CarInterface(CarInterfaceBase):
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
rav4_hybrid_sdsu_long_defaults = uses_rav4_hybrid_sdsu_longitudinal(ret)
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED or rav4_hybrid_sdsu_long_defaults) else MIN_ACC_SPEED
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl
camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and
@@ -205,7 +190,7 @@ class CarInterface(CarInterfaceBase):
if ret.flags & ToyotaFlags.HYBRID.value:
ret.longitudinalActuatorDelay = 0.05
if camry_hybrid_long_defaults or rav4_hybrid_sdsu_long_defaults:
if camry_hybrid_long_defaults:
# The THS eCVT responds much faster than the legacy non-TSS2 ICE tune.
ret.longitudinalActuatorDelay = 0.05
ret.vEgoStopping = 0.25
@@ -2,9 +2,7 @@
from opendbc.can import CANParser
from opendbc.car import Bus
from opendbc.car.structs import RadarData
from opendbc.car.toyota.values import ARS510_RADAR_CAR, CAR, DBC, TSS2_CAR, ToyotaFlags
from opendbc.car.toyota.ars510_acc_target import RadarInterface as Ars510AccRadarInterface
from opendbc.car.toyota.ars510_radar import RadarInterface as Ars510FusedRadarInterface
from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR
from opendbc.car.interfaces import RadarInterfaceBase
RADAR_ACC_TSSP_CAR = {CAR.TOYOTA_CAMRY}
@@ -41,12 +39,6 @@ class RadarInterface(RadarInterfaceBase):
super().__init__(CP)
self.track_id = 0
self.radar_acc_tssp = CP.carFingerprint in RADAR_ACC_TSSP_CAR
self.ars510 = None
if CP.carFingerprint in ARS510_RADAR_CAR and CP.flags & ToyotaFlags.ARS510_RADAR and not CP.radarUnavailable:
parser = Ars510FusedRadarInterface if CP.flags & ToyotaFlags.ARS510_FUSED else Ars510AccRadarInterface
self.ars510 = parser(CP)
self.rcp = self.pt_cp = None
return
if self.radar_acc_tssp:
self.RADAR_MSGS = TSSP_CLUSTER_MSGS
@@ -68,8 +60,6 @@ class RadarInterface(RadarInterfaceBase):
self.updated_messages = set()
def update(self, can_strings):
if self.ars510 is not None:
return self.ars510.update(can_strings)
if self.rcp is None:
return super().update(None)
@@ -1,237 +0,0 @@
from math import isfinite, isnan
from types import SimpleNamespace
import zlib
import pytest
from opendbc.can import CANPacker
from opendbc.car import gen_empty_fingerprint, structs
from opendbc.car.toyota.ars510_acc_target import RadarInterface as AccRadarInterface, RADAR_DBC
from opendbc.car.toyota.ars510_radar import Ars510Radar, IDLE_SLOT, RadarInterface as FusedRadarInterface
from opendbc.car.toyota.interface import CarInterface
from opendbc.car.toyota.radar_interface import RadarInterface
from opendbc.car.toyota.values import ARS510_RADAR_CAR, CAR, ToyotaFlags
@pytest.mark.parametrize("candidate", list(CAR))
@pytest.mark.parametrize("mode", [0, 1, 2, 3])
@pytest.mark.parametrize("alpha_long,can_filter", [(False, False), (True, False), (True, True)])
def test_radar_selection_is_opt_in_and_changes_only_compatible_rav4_radar_fields(candidate, mode, alpha_long, can_filter):
fingerprint = gen_empty_fingerprint()
if can_filter:
fingerprint[0][0x2aa] = 8
baseline = CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False, None)
selected = CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False,
SimpleNamespace(toyota_radar_mode_requested=mode))
expected = baseline.to_dict()
if candidate in ARS510_RADAR_CAR and mode in (1, 2):
expected["flags"] |= ToyotaFlags.ARS510_RADAR
if mode == 2:
expected["flags"] |= ToyotaFlags.ARS510_FUSED
expected["radarUnavailable"] = False
radar = RadarInterface(selected)
assert isinstance(radar.ars510, FusedRadarInterface if mode == 2 else AccRadarInterface)
assert radar.rcp is radar.pt_cp is None
assert selected.to_dict() == expected
assert selected.safetyConfigs[0].to_dict() == baseline.safetyConfigs[0].to_dict()
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023])
def test_default_rav4_remains_vision_only(candidate):
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
radar = RadarInterface(CP)
assert CP.radarUnavailable
assert not CP.flags & (ToyotaFlags.ARS510_RADAR | ToyotaFlags.ARS510_FUSED)
assert radar.ars510 is radar.rcp is None
def set_bits(data, start, length, value):
raw = int.from_bytes(data, "little")
mask = ((1 << length) - 1) << start
data[:] = ((raw & ~mask) | (value << start)).to_bytes(len(data), "little")
def object_record(age=60, distance=30, lateral=0, speed_code=645, uncertainty=1, count=1):
record = bytearray(742)
record[0] = 0xe4
for slot in range(20):
data = bytearray(IDLE_SLOT)
if slot < count:
set_bits(data, 24, 7, age)
set_bits(data, 32, 12, round(160 + 16 * distance))
set_bits(data, 44, 12, round(2048 + lateral / 0.015))
set_bits(data, 64, 10, speed_code)
set_bits(data, 240, 7, uncertainty)
record[17 + slot * 36:17 + (slot + 1) * 36] = data
record[737:741] = zlib.crc32(record[1:737]).to_bytes(4, "little")
return bytes(record)
def record_frames(record, bus=1):
return [(0x80, bytes([0x12 if i == 0 else 0x20 | i % 16]) + record[i * 7:(i + 1) * 7], bus) for i in range(106)]
def ego_frames(speed=20, yaw_code=512):
speed_data = bytearray(8)
speed_data[5:7] = round(speed * 3.6 * 100).to_bytes(2, "big")
yaw_data = bytes([yaw_code >> 8, yaw_code & 0xff]) + bytes(6)
return [(0xb4, bytes(speed_data), 0), (0x24, yaw_data, 0)]
def fused_radar():
return FusedRadarInterface(structs.CarParams(carFingerprint=CAR.TOYOTA_RAV4_TSS2_2023))
def test_fused_decodes_20_slots_and_preserves_left_positive_geometry():
radar = fused_radar()
ret = radar.update([(1_000_000_000, ego_frames() + record_frames(object_record(count=20, lateral=0.3)))])
assert len(ret.points) == 20
assert len({pt.trackId for pt in ret.points}) == 20
for pt in ret.points:
assert (pt.dRel, pt.yRel, pt.vRel) == pytest.approx((30, 0.3, (645 - 510.5) * 0.149 - 20))
assert pt.measured
assert isnan(pt.aRel) and isnan(pt.yvRel)
assert not ret.errors.canError
@pytest.mark.parametrize("bad_input", ["crc", "short", "truncated", "reordered", "wrong_bus"])
def test_fused_rejects_incomplete_corrupt_or_non_native_records(bad_input):
radar = fused_radar()
record = bytearray(object_record())
if bad_input == "crc":
record[25] ^= 1
frames = record_frames(record, bus=2 if bad_input == "wrong_bus" else 1)
if bad_input == "short":
frames[50] = (0x80, frames[50][1][:-1], 1)
elif bad_input == "truncated":
frames.pop(50)
elif bad_input == "reordered":
frames[30], frames[31] = frames[31], frames[30]
assert radar.update([(1_000_000_000, ego_frames() + frames)]) is None
assert radar.last_record_t is None
def test_fused_requires_fresh_ego_speed_and_converged_tracks():
radar = Ars510Radar()
assert radar.points(1.0, object_record()) == []
radar.v_ego = (1.0, 20)
assert radar.points(1.0, object_record(age=59)) == []
assert radar.points(1.1, object_record(age=60))
assert radar.points(1.6, object_record(age=61)) == []
def test_fused_lifecycle_age_reset_allocates_a_new_track():
radar = Ars510Radar()
radar.v_ego = (1.0, 20)
first = radar.points(1.0, object_record(age=60))[0][0]
assert radar.points(1.06, object_record(age=61))[0][0] == first
radar.points(1.12, object_record(age=1))
radar.v_ego = (1.18, 20)
assert radar.points(1.18, object_record(age=60))[0][0] != first
saturated = radar.track_id(2.0, 0, 126)
assert radar.track_id(2.06, 0, 126) == saturated
@pytest.mark.parametrize("kwargs", [{"uncertainty": 127}, {"distance": 61}, {"lateral": 3}])
def test_fused_gates_uncertain_far_and_out_of_path_objects(kwargs):
radar = Ars510Radar()
radar.v_ego = (1.0, 20)
radar.yaw = (1.0, 0)
assert radar.points(1.0, object_record(**kwargs)) == []
def test_fused_damps_a_high_uncertainty_speed_excursion():
radar = Ars510Radar()
radar.v_ego = (1.0, 20)
first = radar.points(1.0, object_record(age=126))[0]
raw_closing = (600 - 510.5) * 0.149 - 20
second = radar.points(1.06, object_record(age=126, speed_code=600, uncertainty=127))[0]
assert raw_closing < -6
assert second[0] == first[0]
assert abs(second[3] - first[3]) < 0.2
def test_fused_keeps_healthy_tracks_between_scans_then_reports_actual_silence(monkeypatch):
radar = fused_radar()
monkeypatch.setattr(radar.radar, "update", lambda t, *_: [(1, 30, 0, -1)] if round(t * 100) % 6 == 0 else None)
for i in range(1, 101):
ret = radar.update([(i * 10_000_000, [(0x80, bytes(8), 1)])])
if i % 6 == 0:
assert len(ret.points) == 1
elif i > 6:
assert ret is None
monkeypatch.setattr(radar.radar, "update", lambda *_: None)
outputs = [radar.update([(2_000_000_000 + i * 10_000_000, [])]) for i in range(10)]
assert any(r is not None and r.errors.radarUnavailableTemporary and not r.points for r in outputs)
monkeypatch.setattr(radar.radar, "update", lambda *_: [])
ret = radar.update([(2_200_000_000, [(0x80, bytes(8), 1)])])
assert not ret.points and not ret.errors.radarUnavailableTemporary
def acc_frames(packer, counter, cycle, present=True, distance=35, relative_speed=-2, target_id=3):
return [packer.make_can_msg(name, 1, values | {"COUNTER": counter % 16}) for name, values in (
("ACC_TARGET_1", {"STATE": 5 if present else 1, "TARGET_ID": target_id, "REL_SPEED": relative_speed}),
("ACC_TARGET_2", {"PRESENT": int(present), "LONG_DIST": distance, "LAT_DIST": 0.25}),
("ACC_TARGET_3", {"TIMESTAMP": cycle}),
)]
def test_acc_target_decodes_one_lead_once_per_cycle_and_clears_no_target():
radar = AccRadarInterface(structs.CarParams())
packer = CANPacker(RADAR_DBC)
results = []
for i in range(30):
results.append(radar.update([(1_000_000_000 + i * 20_000_000, acc_frames(packer, i, 60000 * (1 + i // 3)))]))
reports = [result for result in results if result is not None]
assert len(reports) == 10
for result in reports:
assert not result.errors.canError
pt, = result.points
assert pt.trackId == 3
assert (pt.dRel, pt.yRel, pt.vRel) == pytest.approx((35, 0.25, -2))
assert pt.measured and isfinite(pt.vRel)
assert isnan(pt.aRel) and isnan(pt.yvRel)
ret = radar.update([(1_600_000_000, acc_frames(packer, 30, 660000, present=False))])
assert not ret.points and not ret.errors.canError
def test_acc_target_decodes_captured_rav4_hybrid_frame_bank():
# Stock RAV4 Hybrid 2022 route 2475fb3eb2ffcc2e/2022-04-29--12-46-23, segment 0.
frames = [(0x235, bytes.fromhex("af4563802b24040e"), 1),
(0x237, bytes.fromhex("ff404cc3c10a9550"), 1),
(0x239, bytes.fromhex("2a464008061677c8"), 1)]
ret = AccRadarInterface(structs.CarParams()).update([(1_000_000_000, frames)])
pt, = ret.points
assert not ret.errors.canError
assert pt.trackId == 14
assert (pt.dRel, pt.yRel, pt.vRel) == pytest.approx((3.825, 1.68, 0.125))
@pytest.mark.parametrize("bus,length", [(1, 7), (1, 9), (0, 8), (2, 8), (129, 8)])
def test_acc_target_ignores_malformed_frames_and_non_native_copies(bus, length):
radar = AccRadarInterface(structs.CarParams())
frames = [(addr, bytes(length), bus) for addr in (0x235, 0x237, 0x239)]
assert radar.update([(1_000_000_000, frames)]) is None
assert radar.last_record_t is None
@pytest.mark.parametrize("silent_clock", [False, True])
def test_acc_target_reports_silence_or_a_stuck_clock_without_stale_points(silent_clock):
radar = AccRadarInterface(structs.CarParams())
packer = CANPacker(RADAR_DBC)
assert radar.update([(1_000_000_000, acc_frames(packer, 0, 60000))]).points
outputs = []
for i in range(1, 51):
frames = acc_frames(packer, i, 60000) if silent_clock else []
outputs.append(radar.update([(1_000_000_000 + i * 20_000_000, frames)]))
assert any(r is not None and r.errors.radarUnavailableTemporary and not r.points for r in outputs)
@pytest.mark.parametrize("mode", [1, 2])
def test_rav4_startup_wait_does_not_stall_live_tracks(mode):
CP = CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2_2023, gen_empty_fingerprint(), [], False, False, False,
SimpleNamespace(toyota_radar_mode_requested=mode))
radar = RadarInterface(CP)
outputs = [radar.update([(1_000_000_000 + i * 10_000_000, [])]) for i in range(700)]
assert sum(r is not None for r in outputs) == 140
assert all(not r.points and not r.errors.radarUnavailableTemporary for r in outputs if r is not None)
@@ -6,14 +6,12 @@ from hypothesis import given, settings, strategies as st
from opendbc.car import Bus, structs
from opendbc.can import CANPacker, CANParser
from opendbc.car.structs import CarParams
from opendbc.car.lateral import common_fault_avoidance
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
from opendbc.car.toyota import toyotacan
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
get_prius_positive_feedforward_scale, \
get_rav4_interceptor_pedal_scale, \
get_toyota_lat_active, get_toyota_steer_rate_limit, \
MAX_STEER_RATE, MAX_STEER_RATE_FRAMES, MAX_USER_TORQUE, \
get_toyota_lat_active, \
limit_interceptor_pcm_accel, \
limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \
@@ -25,7 +23,6 @@ from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SP
from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \
uses_toyota_auto_hold_aeb, \
get_platform_codes
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
@@ -85,48 +82,11 @@ class TestToyotaInterfaces:
SimpleNamespace(force_torque_controller=True, nnff=False, nnff_lite=False),
)
assert default_params.lateralTuning.which() == "torque"
assert default_params.lateralTuning.which() == "pid"
assert forced_params.lateralTuning.which() == "torque"
assert default_params.lateralTuning.to_dict() == forced_params.lateralTuning.to_dict()
assert forced_params.lateralTuning.torque.latAccelFactor == pytest.approx(1.7)
assert forced_params.lateralTuning.torque.friction == pytest.approx(0.14)
@pytest.mark.parametrize("candidate", list(CAR))
def test_toyota_default_controller_preserves_native_angle_path(self, candidate):
fingerprint = {bus: {} for bus in range(8)}
default_params = CarInterface.get_params(
candidate, fingerprint, [], False, False, False,
SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
)
forced_params = CarInterface.get_params(
candidate, fingerprint, [], False, False, False,
SimpleNamespace(force_torque_controller=True, nnff=False, nnff_lite=False),
)
if candidate in ANGLE_CONTROL_CAR:
assert default_params.steerControlType == CarParams.SteerControlType.angle
assert default_params.lateralTuning.which() == "pid"
assert default_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LTA.value
else:
assert default_params.steerControlType == CarParams.SteerControlType.torque
assert default_params.lateralTuning.which() == "torque"
assert not default_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LTA.value
assert default_params.to_dict() == forced_params.to_dict()
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_PRIME])
@pytest.mark.parametrize("eps_version", [b'\x028965B0R01400', b'8965B42181\x00\x00\x00\x00\x00\x00', b'8965B0R01200'])
def test_rav4_rack_variants_default_to_torque(self, candidate, eps_version):
params = CarInterface.get_params(
candidate, {bus: {} for bus in range(8)}, [CarParams.CarFw(ecu=Ecu.eps, fwVersion=eps_version)],
False, False, False,
SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
)
assert params.lateralTuning.which() == "torque"
assert params.steerActuatorDelay == pytest.approx(0.12)
assert params.steerControlType == CarParams.SteerControlType.torque
def test_prius_force_torque_controller_preserves_vehicle_tune(self):
fingerprint = {bus: {} for bus in range(8)}
car_fw = [CarParams.CarFw(ecu=Ecu.eps, fwVersion=b'8965B47050\x00\x00\x00\x00\x00\x00')]
@@ -230,15 +190,8 @@ class TestToyotaInterfaces:
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
@pytest.mark.parametrize("candidate,hybrid", [
(CAR.TOYOTA_CAMRY_TSS2, False),
(CAR.TOYOTA_RAV4, False),
(CAR.TOYOTA_RAV4H, True),
(CAR.TOYOTA_RAV4_TSS2, False),
(CAR.TOYOTA_RAV4_TSS2, True),
(CAR.TOYOTA_COROLLA_TSS2, True),
])
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate, hybrid):
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate):
params = Params()
try:
params.put_bool("ToyotaAutoHold", True)
@@ -246,7 +199,7 @@ class TestToyotaInterfaces:
candidate,
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
for bus in range(8)},
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
[],
alpha_long=False,
is_release=False,
docs=False,
@@ -256,36 +209,22 @@ class TestToyotaInterfaces:
params.remove("ToyotaAutoHold")
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
legacy_hold = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
assert uses_toyota_auto_hold_aeb(car_params) == legacy_hold
if legacy_hold:
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
else:
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
can_parsers = CarState.get_can_parsers(car_params)
car_state = CarState(car_params, SimpleNamespace(flags=0))
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
assert (0x344 in can_parsers[Bus.cam].vl) == legacy_hold
assert car_state.auto_brake_hold == legacy_hold
assert "PRE_COLLISION_2" not in can_parsers[Bus.cam].vl
@pytest.mark.parametrize("candidate,hybrid", [
(CAR.TOYOTA_CAMRY_TSS2, False),
(CAR.TOYOTA_RAV4, False),
(CAR.TOYOTA_RAV4H, True),
(CAR.TOYOTA_RAV4_TSS2, False),
(CAR.TOYOTA_RAV4_TSS2, True),
(CAR.TOYOTA_RAV4_TSS2_2022, True),
])
def test_auto_hold_is_disabled_by_default(self, candidate, hybrid):
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
def test_auto_hold_is_disabled_by_default(self, candidate):
params = Params()
params.remove("ToyotaAutoHold")
car_params = CarInterface.get_params(
candidate,
{bus: {} for bus in range(8)},
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
[],
alpha_long=False,
is_release=False,
docs=False,
@@ -294,8 +233,6 @@ class TestToyotaInterfaces:
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
assert 0x344 not in CarState.get_can_parsers(car_params)[Bus.cam].vl
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
car_params = CarInterface.get_params(
@@ -798,43 +735,14 @@ class TestToyotaFingerprint:
class TestToyotaCarController:
@pytest.mark.parametrize("driver_torque", [-191, -117, -99, 99, 117, 191])
def test_toyota_assisting_driver_keeps_lateral_active(self, driver_torque):
assert get_toyota_lat_active(True, driver_torque)
def test_corolla_tss2_hands_off_immediately_when_driver_is_steering(self):
assert not get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 117, True)
@pytest.mark.parametrize("driver_torque", [-MAX_USER_TORQUE, MAX_USER_TORQUE, MAX_USER_TORQUE + 1])
def test_toyota_high_driver_torque_still_disables_lateral(self, driver_torque):
assert not get_toyota_lat_active(True, driver_torque)
def test_corolla_tss2_stays_active_without_driver_input(self):
assert get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 99, False)
def test_toyota_inactive_request_stays_inactive(self):
assert not get_toyota_lat_active(False, 0)
def test_toyota_assisting_driver_retains_rate_fault_protection(self):
counter = 0
requests = []
for _ in range(36):
counter, request = common_fault_avoidance(
150 >= MAX_STEER_RATE, get_toyota_lat_active(True, 117), counter, MAX_STEER_RATE_FRAMES,
)
requests.append(request)
assert requests == ([True] * 17 + [False]) * 2
@pytest.mark.parametrize("candidate", list(CAR))
def test_steer_rate_margin_is_corolla_only(self, candidate):
expected = 80 if candidate == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE
assert get_toyota_steer_rate_limit(candidate) == expected
@pytest.mark.parametrize("direction", [-1, 1])
def test_corolla_rate_margin_preserves_request_spacing(self, direction):
counter = 0
requests = []
for rate in [0] * 30 + [90 * direction] * 36 + [0] * 30:
counter, request = common_fault_avoidance(
abs(rate) >= get_toyota_steer_rate_limit(CAR.TOYOTA_COROLLA_TSS2),
get_toyota_lat_active(True, 117 * direction), counter, MAX_STEER_RATE_FRAMES,
)
requests.append(request)
assert requests == [True] * 30 + ([True] * 17 + [False]) * 2 + [True] * 30
def test_toyota_driver_handoff_behavior_is_corolla_only(self):
assert get_toyota_lat_active(CAR.TOYOTA_RAV4_TSS2, True, 117, True)
@staticmethod
def _make_controller(*, standstill_req=False, last_standstill=False):
@@ -950,31 +858,6 @@ class TestToyotaCarController:
controller.update_auto_hold_state(cs, activation_frames=0)
assert not controller.brake_hold_active
def test_camry_auto_hold_uses_legacy_aeb_brake_path(self):
controller = self._make_controller()
controller.CP.carFingerprint = CAR.TOYOTA_CAMRY_TSS2
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
controller.frame = 0
cs = SimpleNamespace(
out=SimpleNamespace(
standstill=True,
cruiseState=SimpleNamespace(available=True, enabled=False),
gasPressed=False,
brakePressed=True,
gearShifter=structs.CarState.GearShifter.drive,
),
pre_collision_2={},
)
can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0)
parser.update([(1, can_sends)])
assert controller.brake_hold_active
assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
def test_prius_resume_request_releases_standstill_latch(self):
controller = self._make_controller(standstill_req=True, last_standstill=True)
@@ -1149,17 +1032,6 @@ class TestToyotaCarController:
assert gas_cmd == 0.12
def test_interceptor_does_not_apply_gas_during_auto_hold(self):
controller = self._make_controller()
controller.CP.enableGasInterceptorDEPRECATED = True
controller.accel = 1.5
controller.brake_hold_active = True
assert controller._compute_interceptor_gas_cmd(
SimpleNamespace(longActive=True),
SimpleNamespace(out=SimpleNamespace(standstill=True, vEgo=0.0)),
) == 0.0
def test_interceptor_non_stop_and_go_scales_with_accel_request(self):
controller = self._make_controller()
controller.CP.enableGasInterceptorDEPRECATED = True
@@ -1292,271 +1164,6 @@ class TestToyotaCarController:
assert CP.stopAccel == -1.5
class TestToyotaAutoHoldCruise:
@staticmethod
def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, hybrid=False, enabled=True, capability=True):
cp = CarInterface.get_non_essential_params(candidate)
cp.openpilotLongitudinalControl = True
cp.flags &= ~(ToyotaFlags.AUTO_BRAKE_HOLD.value | ToyotaFlags.HYBRID.value)
if capability:
cp.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
if hybrid:
cp.flags |= ToyotaFlags.HYBRID.value
controller = CarController(DBC[candidate], cp)
cc = structs.CarControl(enabled=True, longActive=True)
cc.actuators.accel = -0.7
cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping
cc.hudControl.leadDistanceBars = 3
cs = SimpleNamespace(
out=structs.CarState(
standstill=True,
gearShifter=structs.CarState.GearShifter.drive,
cruiseState=structs.CarState.CruiseState(available=True, enabled=True, standstill=True),
),
gvc=0.0,
acc_type=1,
pcm_acc_status=7,
pcm_follow_distance=1,
lkas_hud={},
pre_collision_2={},
)
toggles = SimpleNamespace(toyota_auto_hold=enabled, sng_hack=False, lock_doors=False, unlock_doors=False)
parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0), ("PRE_COLLISION_2", 0)], 0)
return SimpleNamespace(controller=controller, cc=cc, cs=cs, toggles=toggles, parser=parser)
@staticmethod
def _tick(car):
# Run through a complete ACC_CONTROL send interval and decode the actual
# controller output, including the ordinary standstill and PID paths.
car.can_sends = []
for _ in range(3):
now_nanos = car.controller.frame * 10_000_000
_, messages = car.controller.update(car.cc.as_reader(), car.cs, now_nanos, car.toggles)
car.can_sends.extend(messages)
car.parser.update([(now_nanos, messages)])
return car.parser.vl["ACC_CONTROL"]
@pytest.mark.parametrize("candidate,hybrid", [
(CAR.TOYOTA_RAV4, False),
(CAR.TOYOTA_RAV4H, True),
(CAR.TOYOTA_RAV4_TSS2, False),
(CAR.TOYOTA_RAV4_TSS2, True),
(CAR.TOYOTA_CAMRY_TSS2, False),
])
@pytest.mark.parametrize("sng_hack", [False, True])
def test_auto_hold_does_not_change_cruise_stop_and_go(self, candidate, hybrid, sng_hack):
car = self._make_car(candidate=candidate, hybrid=hybrid)
baseline = self._make_car(candidate=candidate, hybrid=hybrid, enabled=False)
car.toggles.sng_hack = baseline.toggles.sng_hack = sng_hack
for _ in range(50):
command = self._tick(car)
expected = self._tick(baseline)
assert not car.controller.brake_hold_active
assert dict(command) == dict(expected)
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0
for vehicle in (car, baseline):
vehicle.cc.actuators.accel = 1.5
vehicle.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
vehicle.cc.cruiseControl.resume = True
for _ in range(30):
command = self._tick(car)
expected = self._tick(baseline)
assert not car.controller.brake_hold_active
assert dict(command) == dict(expected)
assert not car.cs.out.gasPressed
assert command["ACCEL_CMD"] > 0
assert command["RELEASE_STANDSTILL"] == 1
def test_manual_stop_still_holds_with_only_aol_active(self):
car = self._make_car()
car.cc.enabled = False
car.cc.longActive = False
car.cc.latActive = True
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.brakePressed = True
# Include the first ACC_CONTROL transmission after the one-second latch.
for _ in range(35):
command = self._tick(car)
assert car.controller.brake_hold_active
assert command["ACCEL_CMD"] == -1.0
assert command["RELEASE_STANDSTILL"] == 0
car.cs.out.brakePressed = False
command = self._tick(car)
assert car.controller.brake_hold_active
assert command["ACCEL_CMD"] == -1.0
car.cs.out.gasPressed = True
command = self._tick(car)
assert not car.controller.brake_hold_active
assert command["ACCEL_CMD"] == 0.0
assert command["RELEASE_STANDSTILL"] == 1
car.cs.out.gasPressed = False
for _ in range(50):
self._tick(car)
assert not car.controller.brake_hold_active
car.cs.out.brakePressed = True
for _ in range(35):
command = self._tick(car)
assert car.controller.brake_hold_active
assert command["ACCEL_CMD"] == -1.0
@pytest.mark.parametrize("long_state", ["off", "pid", "starting", "stopping"])
@pytest.mark.parametrize("brake_pressed", [False, True])
def test_engaged_cruise_never_activates_auto_hold(self, long_state, brake_pressed):
car = self._make_car()
car.cc.actuators.longControlState = getattr(structs.CarControl.Actuators.LongControlState, long_state)
car.cs.out.brakePressed = brake_pressed
for _ in range(50):
self._tick(car)
assert not car.controller.brake_hold_active
@pytest.mark.parametrize("enabled,capability,candidate", [
(False, True, CAR.TOYOTA_RAV4_TSS2),
(True, False, CAR.TOYOTA_RAV4_TSS2),
])
def test_manual_hold_requires_toggle_and_capability(self, enabled, capability, candidate):
car = self._make_car(candidate=candidate, enabled=enabled, capability=capability)
car.cc.longActive = False
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.brakePressed = True
for _ in range(50):
command = self._tick(car)
assert not car.controller.brake_hold_active
assert command["ACCEL_CMD"] == 0.0
@pytest.mark.parametrize("release", ["toggle", "cancel", "main", "park", "reverse", "moving", "cruise"])
def test_manual_hold_releases_when_conditions_change(self, release):
car = self._make_car()
car.cc.longActive = False
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.brakePressed = True
for _ in range(35):
self._tick(car)
assert car.controller.brake_hold_active
if release == "toggle":
car.toggles.toyota_auto_hold = False
elif release == "cancel":
car.cc.cruiseControl.cancel = True
elif release == "main":
car.cs.out.cruiseState.available = False
elif release in ("park", "reverse"):
car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release)
elif release == "moving":
car.cs.out.standstill = False
else:
car.cs.out.cruiseState.enabled = True
car.cc.longActive = True
self._tick(car)
assert not car.controller.brake_hold_active
if release == "cruise":
car.cs.out.brakePressed = False
car.cc.actuators.accel = 1.5
car.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
car.cc.cruiseControl.resume = True
for _ in range(20):
command = self._tick(car)
assert not car.controller.brake_hold_active
assert command["ACCEL_CMD"] > 0
assert command["RELEASE_STANDSTILL"] == 1
class TestToyotaAutoHoldAeb:
_make_car = staticmethod(TestToyotaAutoHoldCruise._make_car)
_tick = staticmethod(TestToyotaAutoHoldCruise._tick)
@pytest.mark.parametrize("candidate", list(CAR))
@pytest.mark.parametrize("hybrid", [False, True])
def test_legacy_path_is_scoped_to_camry_and_early_rav4_hybrid(self, candidate, hybrid):
cp = SimpleNamespace(carFingerprint=candidate, flags=ToyotaFlags.HYBRID.value if hybrid else 0)
expected = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
assert uses_toyota_auto_hold_aeb(cp) == expected
@pytest.mark.parametrize("candidate,hybrid", [(CAR.TOYOTA_CAMRY_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True)])
def test_manual_stop_uses_legacy_hold_and_releases_on_gas(self, candidate, hybrid):
car = self._make_car(candidate=candidate, hybrid=hybrid)
car.cc.enabled = False
car.cc.longActive = False
car.cc.latActive = True
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.cruiseState.standstill = False
car.cs.out.brakePressed = True
car.cs.pre_collision_2 = {"DSS1GDRV": -0.1, "PBRTRGR": 0}
# Pass through the camera message until the pedal has been held at a stop.
command = self._tick(car)
assert not car.controller.brake_hold_active
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
for _ in range(34):
command = self._tick(car)
assert car.controller.brake_hold_active
assert any(msg[0] == 0x344 for msg in car.can_sends)
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
# Do not simultaneously inject an ACC_CONTROL hold on legacy-path cars.
assert command["ACCEL_CMD"] == 0.0
assert command["RELEASE_STANDSTILL"] == 1
car.cs.out.brakePressed = False
for _ in range(50):
self._tick(car)
assert car.controller.brake_hold_active
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
car.cs.out.gasPressed = True
self._tick(car)
assert not car.controller.brake_hold_active
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0
@pytest.mark.parametrize("enabled,capability", [(False, True), (True, False)])
def test_rav4_hybrid_does_not_inject_legacy_messages_without_opt_in(self, enabled, capability):
car = self._make_car(hybrid=True, enabled=enabled, capability=capability)
car.cc.longActive = False
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.brakePressed = True
for _ in range(50):
self._tick(car)
assert not car.controller.brake_hold_active
assert all(msg[0] != 0x344 for msg in car.can_sends)
@pytest.mark.parametrize("release", ["toggle", "main", "park", "reverse", "moving", "cruise"])
def test_rav4_hybrid_legacy_hold_releases_when_conditions_change(self, release):
car = self._make_car(hybrid=True)
car.cc.longActive = False
car.cc.actuators.accel = 0.0
car.cs.out.cruiseState.enabled = False
car.cs.out.brakePressed = True
for _ in range(35):
self._tick(car)
assert car.controller.brake_hold_active
if release == "toggle":
car.toggles.toyota_auto_hold = False
elif release == "main":
car.cs.out.cruiseState.available = False
elif release in ("park", "reverse"):
car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release)
elif release == "moving":
car.cs.out.standstill = False
else:
car.cs.out.cruiseState.enabled = True
self._tick(car)
assert not car.controller.brake_hold_active
class TestToyotaCarState:
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT])
def test_legacy_prius_distance_button_generates_events(self, candidate):
@@ -89,38 +89,6 @@ def create_pcs_commands(packer, accel, active, mass):
return [msg1, msg2]
def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
values = {s: pre_collision_2[s] for s in [
"DSS1GDRV",
"DS1STAT2",
"DS1STBK2",
"PCSWAR",
"PCSALM",
"PCSOPR",
"PCSABK",
"PBATRGR",
"PPTRGR",
"IBTRGR",
"CLEXTRGR",
"IRLT_REQ",
"BRKHLD",
"AVSTRGR",
"VGRSTRGR",
"PREFILL",
"PBRTRGR",
"PCSDIS",
"PBPREPMP",
] if s in pre_collision_2}
if brake_hold_active:
values = {
"DSS1GDRV": 0x3FF,
"PBRTRGR": frame % 730 < 727,
}
return packer.make_can_msg("PRE_COLLISION_2", 0, values)
def create_acc_cancel_command(packer):
values = {
"GAS_RELEASED": 0,
-13
View File
@@ -67,8 +67,6 @@ class ToyotaFlags(IntFlag):
DISABLE_RADAR = 4
# The DSU's ACC messages are rerouted through the camera bus by an adapter.
DSU_BYPASS = 8192
ARS510_RADAR = 16384
ARS510_FUSED = 32768
# Static flags
TSS2 = 8
@@ -621,7 +619,6 @@ UNSUPPORTED_DSU_CAR = CAR.with_flags(ToyotaFlags.UNSUPPORTED_DSU)
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = CAR.with_flags(ToyotaFlags.RADAR_ACC)
ARS510_RADAR_CAR = {CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023}
ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL)
@@ -632,16 +629,6 @@ TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | {
CAR.TOYOTA_RAV4H,
}
# The Camry uses the legacy camera AEB replacement for Auto Hold. The
# 2019-2021 RAV4 uses it only with the detected hybrid powertrain.
TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2}
def uses_toyota_auto_hold_aeb(CP: CarParams) -> bool:
return (CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS or
(CP.carFingerprint == CAR.TOYOTA_RAV4_TSS2 and bool(CP.flags & ToyotaFlags.HYBRID.value)))
# no resume button press required
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
@@ -3,7 +3,6 @@ from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.interfaces import CarStateBase
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.gps import get_car_gps_config
from opendbc.car.volkswagen.values import DBC, CanBus, NetworkLocation, TransmissionType, GearShifter, \
CarControllerParams, VolkswagenFlags
@@ -25,32 +24,6 @@ class CarState(CarStateBase):
self.travel_assist_available = False
self.curvature_meas = 0.
self.klr_stock_values = {}
self.car_gps_config = get_car_gps_config(CP)
self.car_gps_supported = self.car_gps_config is not None
self.car_gps = None
self._car_gps_timestamp_nanos = 0
def _update_car_gps(self, cp) -> None:
if self.car_gps_config is None:
return
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in self.car_gps_config.messages]
if min(timestamps) > self._car_gps_timestamp_nanos and max(timestamps) - min(timestamps) <= 2_000_000_000:
gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages))
if gps is not None and (self.car_gps is None or not gps["hasFix"] or
gps["unixTimestampMillis"] > self.car_gps["unixTimestampMillis"]):
timestamp_nanos = max(timestamps)
gps["timestamp_nanos"] = timestamp_nanos
self.car_gps = gps
self._car_gps_timestamp_nanos = timestamp_nanos
if self.car_gps is not None and cp._last_update_nanos - self._car_gps_timestamp_nanos > 2_500_000_000:
if self.car_gps["hasFix"]:
self.car_gps = {**self.car_gps, "hasFix": False, "speed": 0.0, "vNED": [0.0, 0.0, 0.0],
"bearingAccuracyDeg": 180.0, "timestamp_nanos": cp._last_update_nanos}
def get_car_gps(self):
return self.car_gps
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
if not self.CP.pcmCruise:
@@ -78,7 +51,6 @@ class CarState(CarStateBase):
pt_cp = can_parsers[Bus.pt]
cam_cp = can_parsers[Bus.cam]
ext_cp = pt_cp if self.CP.networkLocation == NetworkLocation.fwdCamera else cam_cp
self._update_car_gps(pt_cp)
if self.CP.flags & VolkswagenFlags.PQ:
return self.update_pq(pt_cp, cam_cp, ext_cp)
@@ -455,9 +427,6 @@ class CarState(CarStateBase):
# manually configure some optional and variable-rate/edge-triggered messages
pt_messages, cam_messages = [], []
gps_config = get_car_gps_config(CP)
if gps_config is not None:
pt_messages += [(name, 0) for name in gps_config.messages]
if not CP.flags & VolkswagenFlags.MLB:
pt_messages += [
@@ -1,196 +0,0 @@
import pytest
from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus
from opendbc.car.gps import VOLKSWAGEN_TAOS_GPS_MESSAGES, get_car_gps_config, parse_volkswagen_taos_can_gps
from opendbc.car.volkswagen.carstate import CarState
from opendbc.car.volkswagen.interface import CarInterface
from opendbc.car.volkswagen.values import CAR, DBC
# Synthetic frames, not route data: 40 N / 110 W, 12.5 m/s westbound,
# altitude 300 m, UTC 1,800,000,000, ten satellites, packet ID 2.
GPS_FRAMES = (
(0x36F, bytes.fromhex("02688909e09da31d"), 0),
(0x374, bytes.fromhex("02c0a83200000000"), 0),
(0x378, bytes.fromhex("0221415290010000"), 0),
(0x37B, bytes.fromhex("00d2496b7f4c0900"), 0),
)
@pytest.fixture
def gps_state():
cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
state = CarState(cp, None)
parsers = state.get_can_parsers(cp)
return state, parsers
def decode_frames(frames=GPS_FRAMES):
parser = CANParser("vw_mqb", [(name, 0) for name in VOLKSWAGEN_TAOS_GPS_MESSAGES], 0)
parser.update([(1_000_000_000, list(frames))])
return parser, [parser.vl[name] for name in VOLKSWAGEN_TAOS_GPS_MESSAGES]
def test_synthetic_gps_frame_decode():
parser, values = decode_frames()
gps = parse_volkswagen_taos_can_gps(*values)
assert parser.can_valid
assert gps["hasFix"]
assert gps["latitude"] == pytest.approx(40.0)
assert gps["longitude"] == pytest.approx(-110.0)
assert gps["speed"] == 12.5
assert gps["bearingDeg"] == 270.0
assert gps["altitude"] == 300.0
assert gps["unixTimestampMillis"] == 1_800_000_000_000
assert gps["satelliteCount"] == 10
assert gps["vNED"] == pytest.approx([0.0, -12.5, 0.0])
def test_unvalidated_hemisphere_is_not_a_fix():
frames = ((0x36F, bytes.fromhex("02688929e09da319"), 0), *GPS_FRAMES[1:])
_, values = decode_frames(frames)
gps = parse_volkswagen_taos_can_gps(*values)
assert not gps["hasFix"]
assert gps["vNED"] == [0.0, 0.0, 0.0]
@pytest.mark.parametrize("message,signal,value", (
(0, "GNSS_LatitudeMagnitude", 91.0),
(0, "GNSS_LongitudeMagnitude", 181.0),
(0, "GNSS_LatitudeMagnitude", float("nan")),
(0, "GNSS_PositionStatus", 0),
(0, "GNSS_PositionStatus", 1),
(0, "GNSS_PositionStatus", 2),
(3, "GNSS_Empfaenger_Status", 0),
(3, "GNSS_Genutzte_Satelliten", 0),
(3, "GNSS_Genutzte_Satelliten", 3),
))
def test_invalid_solution_is_not_a_fix(message, signal, value):
_, values = decode_frames()
values[message][signal] = value
gps = parse_volkswagen_taos_can_gps(*values)
assert not gps["hasFix"]
assert gps["speed"] == 0.0
assert gps["vNED"] == [0.0, 0.0, 0.0]
def test_no_constellation_or_null_position_is_not_a_fix():
_, values = decode_frames()
values[3]["GNSS_GPS_in_Nutzung"] = values[3]["GNSS_GLONASS_in_Nutzung"] = 0
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
values[3]["GNSS_GPS_in_Nutzung"] = 1
values[0]["GNSS_LatitudeMagnitude"] = values[0]["GNSS_LongitudeMagnitude"] = 0
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
def test_initial_clock_and_mixed_epochs_are_rejected():
_, values = decode_frames()
values[3]["GNSS_UTC_Zeit"] = 0
assert parse_volkswagen_taos_can_gps(*values) is None
values[3]["GNSS_UTC_Zeit"] = 1_800_000_000
values[1]["GNSS_Nachrichtenpaket_ID2"] = 1
assert parse_volkswagen_taos_can_gps(*values) is None
def test_invalid_motion_and_altitude_have_unknown_accuracy():
_, values = decode_frames()
values[1]["GNSS_Speed"] = 127.75
values[1]["GNSS_Bearing"] = 409.5
values[2]["GNSS_Ortung_Hoehe"] = 7690.0
gps = parse_volkswagen_taos_can_gps(*values)
assert gps["hasFix"]
assert gps["speed"] == gps["bearingDeg"] == gps["altitude"] == 0.0
assert gps["bearingAccuracyDeg"] == 180.0
assert gps["speedAccuracy"] == 100.0
assert gps["verticalAccuracy"] == 500.0
assert gps["vNED"] == [0.0, 0.0, 0.0]
def test_unknown_course_does_not_report_accurate_zero_velocity():
_, values = decode_frames()
values[1]["GNSS_Bearing"] = 409.5
gps = parse_volkswagen_taos_can_gps(*values)
assert gps["hasFix"]
assert gps["speed"] == 12.5
assert gps["speedAccuracy"] == 100.0
assert gps["vNED"] == [0.0, 0.0, 0.0]
def test_gps_is_only_enabled_for_the_taos():
for model in (CAR.VOLKSWAGEN_TAOS_MK1, CAR.VOLKSWAGEN_GOLF_MK7, CAR.VOLKSWAGEN_ID4_MK1):
cp = CarInterface.get_non_essential_params(model)
state = CarState(cp, None)
config = get_car_gps_config(cp)
expected = model == CAR.VOLKSWAGEN_TAOS_MK1
assert state.car_gps_supported is expected
assert (config is not None) is expected
if expected:
assert config.messages == VOLKSWAGEN_TAOS_GPS_MESSAGES
cp.brand = "mock"
assert get_car_gps_config(cp) is None
def test_carstate_update_reads_gps(gps_state):
state, parsers = gps_state
parsers[Bus.pt].update([(1_000_000_000, list(GPS_FRAMES))])
state.update(parsers, None)
gps = state.get_car_gps()
assert gps["hasFix"]
assert gps["timestamp_nanos"] == 1_000_000_000
assert gps["longitude"] == pytest.approx(-110.0)
def test_partial_startup_and_missing_gps_are_optional(gps_state):
state, parsers = gps_state
parser = parsers[Bus.pt]
packer = CANPacker(DBC[state.CP.carFingerprint][Bus.pt])
blink = packer.make_can_msg("Blinkmodi_02", parser.bus, {})
for time in (1_000_000_000, 2_000_000_000):
parser.update([(time, [blink])])
state._update_car_gps(parser)
assert state.get_car_gps() is None
assert parser.can_valid
parser.update([(3_000_000_000, list(GPS_FRAMES[:3]))])
state._update_car_gps(parser)
assert state.get_car_gps() is None
assert all(parser.message_states[address].ignore_alive for address, _, _ in GPS_FRAMES)
@pytest.mark.parametrize("dropout", ("missing_position", "frozen_clock", "missing_all"))
def test_gps_dropout_and_recovery(gps_state, dropout):
state, parsers = gps_state
parser = parsers[Bus.pt]
parser.update([(1_000_000_000, list(GPS_FRAMES))])
state._update_car_gps(parser)
assert state.get_car_gps()["hasFix"]
frames = list(GPS_FRAMES)
if dropout == "missing_position":
frames = frames[1:]
elif dropout == "missing_all":
frames = []
parser.update([(4_000_000_000, frames)])
state._update_car_gps(parser)
assert not state.get_car_gps()["hasFix"]
assert state.get_car_gps()["timestamp_nanos"] == 4_000_000_000
assert state.get_car_gps()["speed"] == 0.0
packer = CANPacker("vw_mqb")
status = {**parser.vl["GNSS_05"], "GNSS_UTC_Zeit": 1_800_000_004}
parser.update([(5_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
state._update_car_gps(parser)
assert state.get_car_gps()["hasFix"]
assert state.get_car_gps()["unixTimestampMillis"] == 1_800_000_004_000
def test_invalid_receiver_status_clears_existing_fix(gps_state):
state, parsers = gps_state
parser = parsers[Bus.pt]
parser.update([(1_000_000_000, list(GPS_FRAMES))])
state._update_car_gps(parser)
status = {**parser.vl["GNSS_05"], "GNSS_Empfaenger_Status": 0}
packer = CANPacker("vw_mqb")
parser.update([(2_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
state._update_car_gps(parser)
assert not state.get_car_gps()["hasFix"]
@@ -170,6 +170,8 @@ class CarController(CarControllerBase):
# convention = driver pushing right → yields right authority
# (LOOSELY/+ arm), retains left (INV/- arm).
# Yield arm scales with |drv| above OVERRIDE_THRESH — strong presses
# (potholes, hard corrections) cross past zero so EPS hands the wheel
# to the driver in their direction.
excess = max(0.0, self.lca_auth_drv_mag_filt - float(P.LCA_AUTH_OVERRIDE_ENTER))
yield_signed = float(P.LCA_AUTH_YIELD_BASE) - P.LCA_AUTH_YIELD_SLOPE * excess
yield_signed = max(float(P.LCA_AUTH_YIELD_MIN), min(yield_signed, float(P.LCA_AUTH_YIELD_BASE)))
@@ -248,12 +250,12 @@ class CarController(CarControllerBase):
# LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz
# Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER)
if self.frame % 2 == 0: # 50 Hz
if not lat_active or self.lca_5_counter is None:
# Initialize counter from CarState on first run
if self.lca_5_counter is None:
self.lca_5_counter = CS.msg_lca_5['COUNTER']
# Increment counter by +4, wrap at 15 (0xF never used)
if lat_active:
self.lca_5_counter = (self.lca_5_counter + 4) % 15
self.lca_5_counter = (self.lca_5_counter + 4) % 15
can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle,
CS.msg_lca_5, self.lca_5_counter))
@@ -1,15 +1,11 @@
from collections import defaultdict
from types import SimpleNamespace
import pytest
from opendbc.can.parser import CANParser
from opendbc.car.volvo.carcontroller import CarController
from opendbc.car.volvo.helpers import checksum_lca_5_message
from opendbc.car.volvo.interface import CarInterface
from opendbc.car.volvo.values import CAR, DBC
from opendbc.car.volvo.volvocan import create_c1_checksum
from opendbc.safety.tests.libsafety import libsafety_py
def _zero_message():
@@ -57,65 +53,21 @@ def test_controller_emits_valid_eight_byte_messages_and_lca5_checksum():
assert data[2] == checksum_lca_5_message(data[0], data[1], data[3], data[4], data[5])
@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
def test_controller_relays_complete_stock_lca5_when_inactive(fingerprint):
cp = CarInterface.get_non_essential_params(fingerprint)
def test_controller_relays_stock_lca5_angle_when_inactive():
cp = CarInterface.get_non_essential_params("VOLVO_XC40_RECHARGE")
controller = CarController(DBC[cp.carFingerprint], cp)
parser = CANParser("volvo_mid_1", [("LCA_5", 50)], 0)
stock_data = bytes.fromhex("88c04cef1190ba00")
parser.update([0, [(0x67, stock_data, 0)]])
cs = _state()
cs.msg_lca_5 = parser.vl["LCA_5"]
cs.msg_lca_5["LCA_5_STEER"] = 12.0
cc = SimpleNamespace(latActive=False, actuators=_Actuators())
safety = libsafety_py.libsafety
config = cp.safetyConfigs[0]
assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0
safety.init_tests()
safety.set_controls_allowed(False)
safety.safety_rx_hook(libsafety_py.make_CANPacket(0x67, 0, stock_data))
_, can_sends = controller.update(cc, cs, 0, None)
lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
assert lca5[1] == stock_data
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(lca5[0], lca5[2], lca5[1]))
cs.msg_lca_5["COUNTER"] = 7
controller.update(cc, cs, 0, None)
controller.update(cc, cs, 0, None)
controller.update(cc, cs, 0, None)
cc.latActive = True
_, can_sends = controller.update(cc, cs, 0, None)
active_lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
assert active_lca5[1][3] >> 4 == 11
@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
@pytest.mark.parametrize("driver_torque", [20.0, -20.0, 128.0, -127.0])
def test_override_lca_stream_passes_safety_and_recovers(fingerprint, driver_torque):
cp = CarInterface.get_non_essential_params(fingerprint)
controller = CarController(DBC[cp.carFingerprint], cp)
cs = _state()
cc = SimpleNamespace(latActive=True, actuators=_Actuators())
safety = libsafety_py.libsafety
config = cp.safetyConfigs[0]
assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0
safety.init_tests()
safety.set_controls_allowed(True)
for active, torque in [(True, 0), (True, driver_torque), (True, -driver_torque),
(True, 0), (False, 0), (True, 0)]:
cc.latActive = active
cs.out.steeringTorque = torque
for _ in range(350):
_, messages = controller.update(cc, cs, 0, None)
address, data, bus = next(msg for msg in messages if msg[0] == 0x58)
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (
active, torque, controller.frame, controller.lca_auth_pos, controller.lca_auth_neg)
if active and torque == 0:
assert controller.lca_auth_pos == 614
assert controller.lca_auth_neg == -614
elif active:
assert min(abs(controller.lca_auth_pos), abs(controller.lca_auth_neg)) == 0
# The inactive path must not manufacture a new angle command.
raw = ((lca5[1][6] & 0x7F) << 8) | lca5[1][7]
if raw & (1 << 14):
raw -= 1 << 15
assert abs(raw * 0.05596 - 12.0) < 0.1
def _c1_state():
+3 -1
View File
@@ -71,12 +71,14 @@ class CarControllerParams:
# (potholes, lane corrections) get full yield while light sustained pressure
# only gets a soft yield. yield_signed = YIELD_BASE − YIELD_SLOPE *
# max(0, drv_mag_filt − OVERRIDE_ENTER), clamped to [YIELD_MIN, YIELD_BASE].
# At |drv|=7 (just over threshold): yield = +60 (light resistance).
# At |drv|=14: yield ≈ -4 (crosses past zero — EPS hands wheel to driver).
# drv_mag_filt is a low-pass of |drv| (alpha=0.04, ~250 ms time constant) —
# without it, 1-2 unit driver-torque jitter became ~10 unit yield-arm jitter
# which PSCM converted to felt ripple at sustained co-steering pressure.
LCA_AUTH_YIELD_BASE = 60 # yield-arm magnitude at the override threshold
LCA_AUTH_YIELD_SLOPE = 8 # counts of yield reduction per unit |drv torque| above threshold
LCA_AUTH_YIELD_MIN = 0 # yield authority without crossing the safety sign boundary
LCA_AUTH_YIELD_MIN = -30 # cap how far past zero the yield arm can go (full hand-over)
LCA_AUTH_YIELD_LP_ALPHA = 0.04 # LP-filter coefficient on |drv| for yield calc (~250 ms tau)
LCA_AUTH_SPLIT = 200 # symmetric → asymmetric handover
LCA_AUTH_REBUILD_RATE = 230 # counts/s (≈ 2.7 s rebuild from 0 to 614)
+1 -4
View File
@@ -250,9 +250,6 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
CAN message for LCA_5 on bus 2
"""
if not lat_active:
return packer.make_can_msg('LCA_5', 2, msg_lca_5)
# DBC defines LCA_5_STEER as 15-bit signed with scale 0.05596 deg/count
# Packer handles the encoding automatically - just pass the angle in degrees
@@ -264,7 +261,7 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
'WHEEL_SPEED_2': msg_lca_5['WHEEL_SPEED_2'],
'NEW_SIGNAL_5': msg_lca_5['NEW_SIGNAL_5'],
'NEW_SIGNAL_2': msg_lca_5['NEW_SIGNAL_2'],
'LCA_5_STEER': target_angle_deg,
'LCA_5_STEER': target_angle_deg if lat_active else msg_lca_5['LCA_5_STEER'],
'COUNTER': counter,
}
@@ -250,12 +250,6 @@ BO_ 803 ES_Infotainment: 8 XXX
SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
BO_ 964 ES_LKAS_Alert: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
SG_ Payload : 16|48@1+ (1,0) [0|281474976710655] "" XXX
BO_ 722 AC_State: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -1,15 +1,5 @@
CM_ "IMPORT _subaru_global.dbc";
BO_ 1723 AVH_Request: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ REQUEST : 16|2@1+ (1,0) [0|3] "" XXX
BO_ 811 AVH_Status: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ ENABLED : 45|1@1+ (1,0) [0|1] "" XXX
BO_ 72 Transmission: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -20,4 +20,4 @@ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
CM_ "Kia Ray EV comma pedal uses the standard 0x200/0x201 rolling counter and CRC8 protocol. Command scaling is Ray-only, estimated from the September 15 route: zero command raw 264/497, slopes about 3.79/7.68 raw counts per native E_EMS11 pedal unit, and 100 native units mapped to 255 comma pedal command units. October ray_1800_M100 firmware transforms physical ADC inputs by 1.8 minus 100, but receives command tracks directly as DAC counts: do not apply that input transform to GAS_COMMAND. Sensor offsets depend on firmware; driver override uses raw tracks, not these physical signal values. Confirm against installed Ray firmware before increasing the command cap.";
CM_ "Kia Ray EV comma pedal uses the standard 0x200/0x201 rolling counter and CRC8 protocol. Channel scaling is Ray-only, estimated from the September 15 route: sensor rest raw 264/497, slopes about 3.79/7.68 raw counts per native E_EMS11 pedal unit, and 100 native units mapped to 255 comma pedal command units. Confirm against installed Ray firmware before increasing the command cap.";
@@ -254,12 +254,6 @@ BO_ 803 ES_Infotainment: 8 XXX
SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
BO_ 964 ES_LKAS_Alert: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
SG_ Payload : 16|48@1+ (1,0) [0|281474976710655] "" XXX
BO_ 722 AC_State: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -313,16 +307,6 @@ VAL_ 544 AEB_Status 12 "AEB related" 8 "AEB actuation" 4 "AEB related" 0 "No AEB
CM_ "subaru_global_2017.dbc starts here";
BO_ 1723 AVH_Request: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ REQUEST : 16|2@1+ (1,0) [0|3] "" XXX
BO_ 811 AVH_Status: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ ENABLED : 45|1@1+ (1,0) [0|1] "" XXX
BO_ 72 Transmission: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -254,12 +254,6 @@ BO_ 803 ES_Infotainment: 8 XXX
SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
BO_ 964 ES_LKAS_Alert: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
SG_ Payload : 16|48@1+ (1,0) [0|281474976710655] "" XXX
BO_ 722 AC_State: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -236,9 +236,6 @@ BO_ 1013 ID3F5VCFRONT_lighting: 8 VEH
SG_ VCFRONT_indicatorRightRequest : 2|2@1+ (1,0) [0|2] "" Receiver
SG_ VCFRONT_indicatorLeftRequest : 0|2@1+ (1,0) [0|2] "" Receiver
BO_ 991 UI_status2: 8 VehicleBus
SG_ UI_activeTouchPoints : 24|8@1+ (1,0) [0|255] "" VehicleBus
VAL_ 568 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ;
VAL_ 568 DTR_Dist_Rq 255 "SNA" 200 "ACC_DIST_7" 166 "ACC_DIST_6" 133 "ACC_DIST_5" 100 "ACC_DIST_4" 66 "ACC_DIST_3" 33 "ACC_DIST_2" 0 "ACC_DIST_1" ;
VAL_ 568 TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ;
@@ -1,104 +0,0 @@
VERSION ""
NS_ :
NS_DESC_
CM_
BA_DEF_
BA_
VAL_
CAT_DEF_
CAT_
FILTER
BA_DEF_DEF_
EV_DATA_
ENVVAR_DATA_
SGTYPE_
SGTYPE_VAL_
BA_DEF_SGTYPE_
BA_SGTYPE_
SIG_TYPE_REF_
VAL_TABLE_
SIG_GROUP_
SIG_VALTYPE_
SIGTYPE_VALTYPE_
BO_TX_BU_
BA_DEF_REL_
BA_REL_
BA_DEF_DEF_REL_
BU_SG_REL_
BU_EV_REL_
BU_BO_REL_
SG_MUL_VAL_
BS_:
BU_: XXX
BO_ 565 ACC_TARGET_1: 8 XXX
SG_ CHECK_BYTE : 7|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
SG_ STATE : 11|4@0+ (1,0) [0|15] "" XXX
SG_ REL_ACCEL : 23|8@0+ (0.125,-12.5) [-12.5|19.375] "m/s^2" XXX
SG_ REL_SPEED : 31|11@0+ (0.125,-128) [-128|127.875] "m/s" XXX
SG_ LAT_ACCEL : 34|8@0+ (0.125,-12.5) [-12.5|19.375] "m/s^2" XXX
SG_ LAT_SPEED : 42|11@0+ (0.0125,-12.8) [-12.8|12.7875] "m/s" XXX
SG_ TARGET_ID : 60|5@0+ (1,0) [0|31] "" XXX
BO_ 567 ACC_TARGET_2: 8 XXX
SG_ CHECK_BYTE : 7|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
SG_ LONG_DIST : 11|13@0+ (0.025,0) [0|204.775] "m" XXX
SG_ LAT_DIST : 30|12@0+ (0.01,-20) [-20|20.95] "m" XXX
SG_ TRACKER_A : 34|8@0+ (1,0) [0|255] "" XXX
SG_ TRACKER_B : 42|8@0+ (1,0) [0|255] "" XXX
SG_ IN_PATH : 48|1@0+ (1,0) [0|1] "" XXX
SG_ PRESENT : 50|1@0+ (1,0) [0|1] "" XXX
SG_ IN_PATH_LEVEL : 62|3@0+ (1,0) [0|7] "" XXX
SG_ IN_PATH_CANDIDATE : 63|1@0+ (1,0) [0|1] "" XXX
BO_ 569 ACC_TARGET_3: 8 XXX
SG_ CHECK_BYTE : 7|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
SG_ CLASS : 11|4@0+ (1,0) [0|15] "" XXX
SG_ STATE : 23|2@0+ (1,0) [0|3] "" XXX
SG_ IN_OBJECT_LIST : 31|1@0+ (1,0) [0|1] "" XXX
SG_ PRESENT : 27|1@0+ (1,0) [0|1] "" XXX
SG_ TIMESTAMP : 26|32@0+ (1,0) [0|4294967295] "us" XXX
BO_ 571 ACC_TARGET_4: 3 XXX
SG_ CHECK_BYTE : 7|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 15|4@0+ (1,0) [0|15] "" XXX
SG_ WIDTH : 11|12@0+ (1,0) [0|4095] "cm" XXX
CM_ "Continental ARS510 front radar, radar bus: the target the radar's own ACC follows. Used on the radar-ACC platforms: RAV4 2022-25 (fwdRadar 8821F0R01100, 8821F0R03100), C-HR 2021-22 (8821FF4102xx-0500) and Yaris (8821F0D0xxxx). The 0x80 object list and 0x85 metadata record span many frames and are decoded by ars510-radar (ars510/). The radar interface uses STATE, REL_SPEED, TARGET_ID, LONG_DIST, LAT_DIST, PRESENT and TIMESTAMP; the other signals are documented for completeness.";
CM_ BO_ 565 "ACC target, part 1: 50 Hz, content updated once per 60 ms radar cycle.";
CM_ SG_ 565 CHECK_BYTE "Check byte over the rest of the frame (algorithm unknown).";
CM_ SG_ 565 STATE "5 = target present, 1 = idle (bytes 2-7 64 80 0B 24 00 FF), 15 = start-up.";
CM_ SG_ 565 REL_ACCEL "Target acceleration relative to ego, positive = opening.";
CM_ SG_ 565 REL_SPEED "Closing speed relative to ego, negative = closing.";
CM_ SG_ 565 LAT_ACCEL "Relative lateral acceleration, left positive (nominal unit). Mostly minus ego's own lateral acceleration.";
CM_ SG_ 565 LAT_SPEED "Lateral speed of the target relative to ego, left positive (about 0.0125 m/s per code).";
CM_ SG_ 565 TARGET_ID "Changes when the radar switches to a different target; 31 while idle.";
CM_ BO_ 567 "ACC target, part 2: 50 Hz.";
CM_ SG_ 567 CHECK_BYTE "Check byte: an affine parity function of bytes 1-7.";
CM_ SG_ 567 LONG_DIST "Distance from the radar.";
CM_ SG_ 567 LAT_DIST "Lateral position, left positive.";
CM_ SG_ 567 TRACKER_A "Raw tracker byte: about 35 on a steady target, higher while it accelerates or brakes.";
CM_ SG_ 567 TRACKER_B "Raw tracker byte: about 86 for a stopped target, 128-135 for a steady mover, rises (and wraps) while it accelerates or brakes.";
CM_ SG_ 567 IN_PATH "1 while the target is confirmed in the path (level 5).";
CM_ SG_ 567 PRESENT "1 = target present.";
CM_ SG_ 567 IN_PATH_LEVEL "In-path level 5, 4, 3, 2; steps to the neighboring level only; lower levels sit farther from the path.";
CM_ SG_ 567 IN_PATH_CANDIDATE "1 while the target is a candidate (levels 4-2).";
CM_ BO_ 569 "ACC target, part 3: 50 Hz.";
CM_ SG_ 569 CHECK_BYTE "Check byte: an affine parity function of bytes 1-7.";
CM_ SG_ 569 CLASS "Target class: 1 car, 2 truck, 0 / 4 / 5 / 6 other, 7 idle, 15 start-up.";
CM_ SG_ 569 STATE "1 with an ACC target, 2 idle.";
CM_ SG_ 569 IN_OBJECT_LIST "1 while the target also has an object in the 0x80 object list.";
CM_ SG_ 569 PRESENT "1 = target present.";
CM_ SG_ 569 TIMESTAMP "Radar cycle clock: changes once per 60 ms radar cycle, when the ACC target is recomputed.";
CM_ BO_ 571 "ACC target, part 4: 50 Hz, 3 bytes.";
CM_ SG_ 571 CHECK_BYTE "CRC-8 (polynomial 0x1D, init 0, xor 0x59) over the counter and width bits.";
CM_ SG_ 571 WIDTH "Target width: 0 while idle, 140-220 cm for the car class, 200-280 cm for the truck class.";
VAL_ 565 STATE 15 "STARTUP" 5 "TARGET" 1 "NO_TARGET" ;
VAL_ 569 CLASS 0 "OTHER_0" 1 "CAR" 2 "TRUCK" 4 "OTHER_4" 5 "OTHER_5" 6 "OTHER_6" 7 "NO_TARGET" 15 "STARTUP" ;
VAL_ 569 STATE 1 "TARGET" 2 "NO_TARGET" ;
-30
View File
@@ -1610,36 +1610,6 @@ BO_ 391 Motor_EV_01: 8 Motor_MQB_BEV
SG_ MO_HVEM_Eskalation : 54|1@1+ (1.0,0.0) [0.0|1] "" XXX
SG_ MO_HVEM_MaxLeistung : 55|9@1+ (50,0) [0|25450] "Unit_Watt" XXX
BO_ 879 GNSS_01: 8 XXX
SG_ GNSS_Nachrichtenpaket_ID1 : 0|2@1+ (1,0) [0|3] "" XXX
SG_ GNSS_LatitudeMagnitude : 2|27@1+ (0.000001,0) [0|90] "deg" XXX
SG_ GNSS_LatitudeSouth : 29|1@1+ (1,0) [0|1] "" XXX
SG_ GNSS_LongitudeMagnitude : 30|28@1+ (0.000001,0) [0|180] "deg" XXX
SG_ GNSS_LongitudeWest : 58|1@1+ (1,0) [0|1] "" XXX
SG_ GNSS_PositionStatus : 59|2@1+ (1,0) [0|3] "" XXX
BO_ 884 GNSS_02: 8 XXX
SG_ GNSS_Nachrichtenpaket_ID2 : 0|2@1+ (1,0) [0|3] "" XXX
SG_ GNSS_Bearing : 12|12@1+ (0.1,0) [0|359.9] "deg" XXX
SG_ GNSS_Speed : 24|9@1+ (0.25,0) [0|127.5] "m/s" XXX
BO_ 888 GNSS_04: 8 XXX
SG_ GNSS_Nachrichtenpaket_ID4 : 0|2@1+ (1,0) [0|3] "" XXX
SG_ GNSS_Ortung_Zeit_in_GPSWoche : 2|30@1+ (1,0) [0|604800001] "ms" XXX
SG_ GNSS_Ortung_Hoehe : 32|12@1+ (2,-500) [-500|7686] "m" XXX
BO_ 891 GNSS_05: 8 XXX
SG_ GNSS_UTC_Zeit : 0|32@1+ (1,0) [1|4294967295] "s" XXX
SG_ GNSS_Empfaenger_Status : 32|1@1+ (1,0) [0|1] "" XXX
SG_ GNSS_GPS_in_Nutzung : 33|1@1+ (1,0) [0|1] "" XXX
SG_ GNSS_GLONASS_in_Nutzung : 34|1@1+ (1,0) [0|1] "" XXX
SG_ GNSS_Empfangbare_Satelliten : 35|5@1+ (1,0) [0|31] "" XXX
SG_ GNSS_Sichtbare_Satelliten : 40|5@1+ (1,0) [0|31] "" XXX
SG_ GNSS_Genutzte_Satelliten : 45|5@1+ (1,0) [0|31] "" XXX
SG_ GNSS_Nachrichtenpaket_ID5 : 50|2@1+ (1,0) [0|3] "" XXX
CM_ BO_ 879 "Taos coordinate layout decoded from CAN/GPS comparison. Only north/west coordinates and position status 3 have been validated.";
CM_ BO_ 884 "Taos course and ground speed decoded from CAN/GPS comparison. Remaining fields are not decoded.";
CM_ SG_ 134 LWI_Lenkradwinkel "Steering angle WITH variable ratio effect included";
CM_ SG_ 159 EPS_HCA_Status "Status of Heading Control Assist feature";
CM_ SG_ 159 EPS_Lenkmoment "Steering input by driver, torque";
+6 -24
View File
@@ -96,8 +96,6 @@ static bool ford_lka_steering = false;
static bool ford_extended_lateral = false;
static bool ford_longitudinal = false;
static bool ford_cancel_resume_button = false;
static bool ford_mach_e_curvature = false;
static int ford_path_angle_last = 0;
// Curvature rate limits
#define FORD_LIMITS(limit_lateral_acceleration) { \
@@ -123,10 +121,10 @@ static int ford_path_angle_last = 0;
static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration, max_curvature_error) { \
#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration) { \
.max_angle = 1000, \
.angle_deg_to_can = 50000, \
.max_angle_error = (max_curvature_error), \
.max_angle_error = 100, \
.angle_rate_up_lookup = { \
{5., 16., 25.}, \
{0.0025, 0.0014, 0.00018} \
@@ -142,7 +140,7 @@ static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
.inactive_angle_is_zero = true, \
}
static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false, 100);
static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false);
static void ford_rx_hook(const CANPacket_t *msg) {
if (msg->bus == FORD_MAIN_BUS) {
@@ -320,8 +318,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
// Safety check for LateralMotionControl2 action
if (msg->addr == FORD_LateralMotionControl2) {
static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true);
static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true, 100);
static const AngleSteeringLimits FORD_MACH_E_CURVATURE_LIMITS = FORD_EXTENDED_LIMITS(true, 300);
static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true);
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
@@ -339,19 +336,9 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
if (ford_extended_lateral) {
violation |= desired_path_offset != 0;
violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023);
if (desired_path_angle != 0) {
const float speed = vehicle_speed.max / VEHICLE_SPEED_FACTOR;
const float curvature = (float)SAFETY_ABS(desired_curvature) / 50000.0f;
const float path_angle = (float)SAFETY_ABS(desired_path_angle) / 2000.0f;
const float combined_acceleration = (curvature + path_angle / SAFETY_MAX(speed, 1.0f)) * speed * speed;
violation |= !ford_mach_e_curvature || !steer_control_enabled || !controls_allowed;
violation |= (speed < 3.0f) || (speed >= 8.8f);
violation |= (SAFETY_ABS(desired_curvature) < 975) || (SAFETY_ABS(desired_path_angle) > 320);
violation |= (desired_curvature * desired_path_angle <= 0) || (combined_acceleration > 2.5f);
violation |= SAFETY_ABS(desired_path_angle - ford_path_angle_last) > 110;
}
violation |= desired_path_angle != 0;
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
ford_mach_e_curvature ? FORD_MACH_E_CURVATURE_LIMITS : FORD_CANFD_EXTENDED_STEERING_LIMITS);
FORD_CANFD_EXTENDED_STEERING_LIMITS);
if (!steer_control_enabled) {
violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
}
@@ -365,8 +352,6 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
if (violation) {
tx = false;
} else {
ford_path_angle_last = desired_path_angle;
}
}
@@ -421,11 +406,8 @@ static safety_config ford_init(uint16_t param) {
const uint16_t FORD_PARAM_CANFD = 2;
const uint16_t FORD_PARAM_LKA_STEERING = 4;
const uint16_t FORD_PARAM_MACH_E_CURVATURE = 8;
const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING);
ford_mach_e_curvature = ford_canfd && GET_FLAG(param, FORD_PARAM_MACH_E_CURVATURE);
ford_path_angle_last = 0;
ford_extended_lateral = false;
ford_cancel_resume_button = false;
+1 -1
View File
@@ -330,7 +330,7 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
const int expected_track2 = 497 + (2 * ((int)track1 - 264));
if ((msg->data[4] & 0x70U) != 0U ||
(msg->data[5] != hyundai_ray_pedal_checksum(msg)) ||
(enabled && (track1 < 264U || track1 > 530U || track2 < 497U || track2 > 1035U ||
(enabled && (track1 < 264U || track1 > 397U || track2 < 497U || track2 > 766U ||
SAFETY_ABS((int)track2 - expected_track2) > 40)) ||
(!enabled && ((track1 != 0U) || (track2 != 0U))) ||
longitudinal_interceptor_checks(msg) ||
@@ -65,7 +65,6 @@
static bool hyundai_canfd_alt_buttons = false;
static bool hyundai_canfd_lka_steering_alt = false;
static bool hyundai_canfd_angle_steering = false;
static bool hyundai_canfd_no_stock_lka = false;
static bool hyundai_ccnc = false;
static bool hyundai_canfd_ccnc_angle_long = false;
static bool hyundai_canfd_lka_alt_drive_gear = false;
@@ -101,8 +100,7 @@ static bool hyundai_canfd_lka_alt_openpilot_allowed(void) {
}
static bool hyundai_canfd_lka_alt_stock_forwarding(void) {
return hyundai_canfd_lka_steering_alt && hyundai_canfd_angle_steering &&
!hyundai_canfd_no_stock_lka && !hyundai_canfd_lka_alt_openpilot_allowed();
return hyundai_canfd_lka_steering_alt && hyundai_canfd_angle_steering && !hyundai_canfd_lka_alt_openpilot_allowed();
}
static void hyundai_canfd_rx_all_hook(const CANPacket_t *msg) {
@@ -272,10 +270,6 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
const int lkas_angle_active = (msg->data[9] >> 4U) & 0x3U;
const bool steer_angle_req = lkas_angle_active != 1;
if (hyundai_canfd_no_stock_lka && steer_angle_req && !hyundai_canfd_lka_alt_openpilot_allowed()) {
tx = false;
}
int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U);
desired_angle = to_signed(desired_angle, 14);
@@ -370,7 +364,6 @@ static safety_config hyundai_canfd_init(uint16_t param) {
const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT = 128;
const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
const uint16_t HYUNDAI_PARAM_CANFD_ANGLE_STEERING = 1024;
const uint16_t HYUNDAI_PARAM_CANFD_NO_STOCK_LKA = 4096U;
const uint16_t HYUNDAI_PARAM_CCNC = 32768U;
static const CanMsg HYUNDAI_CANFD_LKA_STEERING_TX_MSGS[] = {
@@ -486,15 +479,12 @@ static safety_config hyundai_canfd_init(uint16_t param) {
{0x7C4, 2, 8, .check_relay = true}, /* camera support frame */ \
{0xEA, 2, 24, .check_relay = true}, /* MDPS support frame */ \
// This CAN-FD-only bit is independent of classic CAN's NON_SCC mode.
hyundai_common_init(param & ~HYUNDAI_PARAM_CANFD_NO_STOCK_LKA);
hyundai_common_init(param);
gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS);
hyundai_canfd_lka_steering_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT);
hyundai_canfd_angle_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ANGLE_STEERING);
hyundai_canfd_no_stock_lka = hyundai_canfd_angle_steering && hyundai_canfd_lka_steering &&
hyundai_canfd_lka_steering_alt && GET_FLAG(param, HYUNDAI_PARAM_CANFD_NO_STOCK_LKA);
hyundai_ccnc = GET_FLAG(param, HYUNDAI_PARAM_CCNC);
hyundai_canfd_ccnc_angle_long = hyundai_longitudinal && hyundai_canfd_lka_steering &&
hyundai_canfd_lka_steering_alt && hyundai_canfd_angle_steering && hyundai_ccnc;
+1 -38
View File
@@ -35,7 +35,6 @@
#define MSG_SUBARU_ES_DashStatus 0x321U
#define MSG_SUBARU_ES_LKAS_State 0x322U
#define MSG_SUBARU_ES_Infotainment 0x323U
#define MSG_SUBARU_ES_LKAS_Alert 0x3C4U
#define MSG_SUBARU_Cruise_Buttons 0x146U
#define MSG_SUBARU_ES_UDS_Request 0x787U
@@ -137,8 +136,6 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
return checksum;
}
#include "opendbc/safety/modes/subaru_avh.h"
static void subaru_rx_hook(const CANPacket_t *msg) {
const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const unsigned int status_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS;
@@ -305,20 +302,12 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
}
if (msg->addr == MSG_SUBARU_ES_LKAS_Alert) {
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
}
if (msg->addr == MSG_SUBARU_Cruise_Buttons) {
violation |= !subaru_redneck_cruise;
violation |= msg->bus != SUBARU_MAIN_BUS;
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
}
if (msg->addr == 0x6BBU) {
violation |= !subaru_avh_tx(msg);
}
if (violation){
tx = false;
}
@@ -326,12 +315,6 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
}
static safety_config subaru_init(uint16_t param) {
static const CanMsg SUBARU_LEGACY_AVH_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
{0x6BBU, SUBARU_ALT_BUS, 8, .check_relay = false},
};
static const CanMsg SUBARU_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
@@ -382,12 +365,6 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static const CanMsg SUBARU_ASCENT_LKAS_ALERT_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
{MSG_SUBARU_ES_LKAS_Alert, SUBARU_MAIN_BUS, 8, .check_relay = true},
};
static const CanMsg SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
@@ -455,9 +432,6 @@ static safety_config subaru_init(uint16_t param) {
const uint16_t SUBARU_PARAM_REDNECK_CRUISE = 512;
subaru_redneck_cruise = GET_FLAG(param, SUBARU_PARAM_REDNECK_CRUISE);
const uint16_t SUBARU_PARAM_ASCENT_LKAS_ALERT = 2048;
const bool subaru_ascent_lkas_alert = GET_FLAG(param, SUBARU_PARAM_ASCENT_LKAS_ALERT);
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -468,8 +442,7 @@ static safety_config subaru_init(uint16_t param) {
ret = subaru_d_platform ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS) : \
(subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS))) : \
subaru_gen2 ? (subaru_ascent_lkas_alert ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_ASCENT_LKAS_ALERT_TX_MSGS) : \
subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS) : \
subaru_gen2 ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS)) : \
BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
} else if (subaru_gen2) {
@@ -482,22 +455,12 @@ static safety_config subaru_init(uint16_t param) {
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_STOP_AND_GO_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
}
bool avh_enabled = false;
#ifdef ALLOW_DEBUG
avh_enabled = GET_FLAG(param, 1024U) && subaru_gen2 && subaru_lkas_angle && subaru_fixed_angle_limits &&
subaru_stop_start_button && !subaru_d_platform && !GET_FLAG(param, 2U) && !subaru_redneck_cruise;
#endif
subaru_avh_init(avh_enabled);
if (avh_enabled) {
ret = BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_LEGACY_AVH_TX_MSGS);
}
return ret;
}
const safety_hooks subaru_hooks = {
.init = subaru_init,
.rx = subaru_rx_hook,
.rx_all = subaru_avh_rx,
.tx = subaru_tx_hook,
.get_counter = subaru_get_counter,
.get_checksum = subaru_get_checksum,
@@ -1,140 +0,0 @@
#pragma once
// Legacy startup AVH only. Never transmit the 0x32B status message.
static const unsigned int SUBARU_AVH_INPUTS[] = {0x6BBU, 0x32BU, 0x40U, 0x48U, 0x13AU, 0x174U};
static uint8_t subaru_avh_data[6][8];
static uint32_t subaru_avh_ts[6];
static bool subaru_avh_seen[6];
static bool subaru_avh_seq[6];
static bool subaru_avh_enabled;
static bool subaru_avh_done;
static unsigned int subaru_avh_count;
static uint32_t subaru_avh_start;
static uint32_t subaru_avh_sent;
static uint32_t subaru_avh_template_ts;
static uint32_t subaru_avh_stable_since;
static bool subaru_avh_stable;
static void subaru_avh_init(bool enabled) {
subaru_avh_enabled = enabled;
subaru_avh_done = false;
subaru_avh_count = 0U;
subaru_avh_start = microsecond_timer_get();
subaru_avh_sent = 0U;
subaru_avh_template_ts = 0U;
subaru_avh_stable_since = 0U;
subaru_avh_stable = false;
for (int i = 0; i < 6; i++) {
subaru_avh_seen[i] = false;
subaru_avh_seq[i] = false;
subaru_avh_ts[i] = 0U;
for (int j = 0; j < 8; j++) {
subaru_avh_data[i][j] = 0U;
}
}
}
static bool subaru_avh_ready(uint32_t now) {
bool ready = true;
for (int i = 0; i < 6; i++) {
ready &= subaru_avh_seen[i] && subaru_avh_seq[i] &&
(safety_get_ts_elapsed(now, subaru_avh_ts[i]) <= ((i == 0) ? 1500000U : 300000U));
}
const unsigned int rpm = ((unsigned int)subaru_avh_data[2][2] | ((unsigned int)subaru_avh_data[2][3] << 8U)) & 0x1FFFU;
ready &= (rpm >= 400U) && (subaru_avh_data[2][4] == 0U) && (subaru_avh_data[3][3] == 4U);
ready &= (subaru_avh_data[5][2] & 8U) != 0U;
ready &= !vehicle_moving && !controls_allowed;
return ready;
}
static void subaru_avh_rx(const CANPacket_t *msg) {
if (subaru_avh_enabled && !subaru_avh_done && (msg->bus == 1U)) {
const uint32_t now = microsecond_timer_get();
for (int i = 0; i < 6; i++) {
if (msg->addr == SUBARU_AVH_INPUTS[i]) {
if ((GET_LEN(msg) != 8U) || (subaru_get_checksum(msg) != subaru_compute_checksum(msg))) {
subaru_avh_done = true;
} else {
const uint8_t old_counter = subaru_avh_data[i][1] & 0xFU;
const uint8_t counter = msg->data[1] & 0xFU;
if (!subaru_avh_seen[i] || (counter != old_counter)) {
subaru_avh_seq[i] = subaru_avh_seen[i] && (counter == ((old_counter + 1U) & 0xFU));
subaru_avh_seen[i] = true;
subaru_avh_ts[i] = now;
for (int j = 0; j < 8; j++) {
subaru_avh_data[i][j] = msg->data[j];
}
}
if (((i == 0) && ((msg->data[2] & 3U) != 0U)) ||
((i == 1) && ((msg->data[5] & 0x20U) != 0U)) ||
((i == 2) && (msg->data[4] != 0U)) || ((i == 3) && (msg->data[3] != 4U)) ||
((i == 4) && (((GET_BYTES(msg, 1, 3) >> 4) & 0x1FFFU) != 0U ||
((GET_BYTES(msg, 3, 3) >> 1) & 0x1FFFU) != 0U ||
((GET_BYTES(msg, 4, 3) >> 6) & 0x1FFFU) != 0U ||
((GET_BYTES(msg, 6, 2) >> 3) & 0x1FFFU) != 0U))) {
subaru_avh_done = true;
}
}
}
}
if (controls_allowed || (safety_get_ts_elapsed(now, subaru_avh_start) > 30000000U)) {
subaru_avh_done = true;
}
if (!subaru_avh_ready(now)) {
subaru_avh_stable = false;
if (subaru_avh_count > 0U) {
subaru_avh_done = true;
}
} else if (!subaru_avh_stable) {
subaru_avh_stable = true;
subaru_avh_stable_since = now;
}
}
}
static bool subaru_avh_tx(const CANPacket_t *msg) {
const uint32_t now = microsecond_timer_get();
const uint32_t elapsed = safety_get_ts_elapsed(now, subaru_avh_start);
const bool second = subaru_avh_count == 1U;
bool allowed = subaru_avh_enabled && !subaru_avh_done && (subaru_avh_count < 2U) &&
(msg->bus == 1U) && (GET_LEN(msg) == 8U) && !safety_rx_checks_invalid &&
(elapsed >= 10000000U) && (elapsed <= 30000000U) && subaru_avh_ready(now) &&
subaru_avh_stable && (safety_get_ts_elapsed(now, subaru_avh_stable_since) >= 3000000U);
// Rejected generic RX frames may not reach our hook; invalidate their cached inputs too.
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
const RxCheck *check = &current_safety_config.rx_checks[i];
for (int j = 0; j < 6; j++) {
if (((unsigned int)check->msg[check->status.index].addr == SUBARU_AVH_INPUTS[j]) && (check->msg[check->status.index].bus == 1U)) {
allowed &= check->status.valid_checksum && (check->status.wrong_counters < MAX_WRONG_COUNTERS);
}
}
}
if (second) {
const uint32_t spacing = safety_get_ts_elapsed(now, subaru_avh_sent);
allowed &= (spacing >= 45000U) && (spacing <= 80000U) && (subaru_avh_ts[0] == subaru_avh_template_ts) &&
(safety_get_ts_elapsed(now, subaru_avh_ts[0]) <= 110000U);
} else {
allowed &= safety_get_ts_elapsed(now, subaru_avh_ts[0]) <= 30000U;
}
uint8_t sum = (uint8_t)(0xBBU + 6U);
for (int i = 1; i < 8; i++) {
uint8_t expected = subaru_avh_data[0][i];
if (i == 1) {
expected = (expected & 0xF0U) | ((expected + (second ? 2U : 1U)) & 0xFU);
} else if (i == 2) {
expected |= 2U;
} else {
// Preserve every unrelated payload bit.
}
allowed &= msg->data[i] == expected;
sum += expected;
}
allowed &= msg->data[0] == sum;
if (allowed) {
subaru_avh_count++;
subaru_avh_sent = now;
subaru_avh_template_ts = subaru_avh_ts[0];
subaru_avh_done = second;
}
return allowed;
}
+2 -54
View File
@@ -4,10 +4,6 @@
static bool tesla_longitudinal = false;
static bool tesla_coop_steering = false;
static bool tesla_aol_screen_button = false;
static bool tesla_screen_disengage_on_brake = false;
static bool tesla_touch_initialized = false;
static bool tesla_screen_cancel_pressed = false;
static bool tesla_stock_aeb = false;
#define TESLA_STEERING_DISENGAGE_TORQUE 500 // cNm
@@ -98,27 +94,6 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
return valid;
}
static void tesla_rx_all_hook(const CANPacket_t *msg) {
if (tesla_aol_screen_button && (msg->bus == 1U) && (msg->addr == 0x229U) && (GET_LEN(msg) == 3)) {
const int stalk_status = (msg->data[1] >> 4) & 0x7U; // SCCM_rightStalkStatus
tesla_screen_cancel_pressed = (stalk_status == 1) || (stalk_status == 2); // UP_1, UP_2
if (tesla_screen_cancel_pressed) {
lkas_on = false;
}
}
// The add-on's UI message is asynchronous, not a required periodic safety input.
if (tesla_aol_screen_button && (msg->bus == 1U) && (msg->addr == 0x3DFU) && (GET_LEN(msg) == 8)) {
const bool screen_button = msg->data[3] == 3U; // UI_activeTouchPoints
if (tesla_touch_initialized && screen_button && !lkas_button_prev && !steering_disengage && !tesla_screen_cancel_pressed &&
!(tesla_screen_disengage_on_brake && brake_pressed)) {
lkas_on = !lkas_on;
}
// A touch held during startup is not an engagement request.
tesla_touch_initialized = true;
lkas_button_prev = screen_button;
}
}
static void tesla_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
@@ -137,9 +112,6 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
steering_disengage = (hands_on_level >= 3) ||
(tesla_coop_steering && (SAFETY_ABS(torsion_bar_torque) > TESLA_STEERING_DISENGAGE_TORQUE)) ||
((eac_status == 0) && (eac_error_code == 9));
if (tesla_aol_screen_button && steering_disengage) {
lkas_on = false;
}
}
// Vehicle speed (DI_speed)
@@ -164,9 +136,6 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
// Brake pressed
if (msg->addr == 0x39dU) {
brake_pressed = (msg->data[2] & 0x03U) == 2U;
if (tesla_screen_disengage_on_brake && brake_pressed) {
lkas_on = false;
}
}
// Cruise and Autopark/Summon state
@@ -195,22 +164,9 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
(cruise_state == 7); // PRE_CANCEL
cruise_engaged = cruise_engaged && !tesla_autopark;
const bool cruise_engagement_started = cruise_engaged && !cruise_engaged_prev;
const bool cruise_cancelled = cruise_engaged_prev && !cruise_engaged && !brake_pressed;
pcm_cruise_check(cruise_engaged);
const bool acc_main_on_now = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark;
if (tesla_aol_screen_button) {
if (cruise_engagement_started && !tesla_screen_cancel_pressed && !steering_disengage &&
!(tesla_screen_disengage_on_brake && brake_pressed)) {
lkas_on = true;
}
if (cruise_cancelled) {
lkas_on = false;
}
}
// Availability alone is not an engagement request on the screen-toggle path.
acc_main_on = !tesla_aol_screen_button && acc_main_on_now;
acc_main_on = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark;
}
if (msg->addr == 0x155U) {
@@ -231,8 +187,7 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
bool tesla_stock_lkas_now = steering_control_type == 2; // "LANE_KEEP_ASSIST"
// Only consider rising edges while controls are not allowed
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev &&
!(controls_allowed || (tesla_aol_screen_button && aol_allowed))) {
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !controls_allowed) {
tesla_stock_lkas = true;
}
if (!tesla_stock_lkas_now) {
@@ -382,12 +337,6 @@ static safety_config tesla_init(uint16_t param) {
};
SAFETY_UNUSED(param);
const uint16_t TESLA_FLAG_AOL_SCREEN_BUTTON = 512;
const uint16_t TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024;
tesla_aol_screen_button = GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_BUTTON);
tesla_screen_disengage_on_brake = tesla_aol_screen_button && GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE);
tesla_touch_initialized = false;
tesla_screen_cancel_pressed = false;
#ifdef ALLOW_DEBUG
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
const uint16_t TESLA_FLAG_COOP_STEERING = 256;
@@ -428,7 +377,6 @@ static safety_config tesla_init(uint16_t param) {
const safety_hooks tesla_hooks = {
.init = tesla_init,
.rx_all = tesla_rx_all_hook,
.rx = tesla_rx_hook,
.tx = tesla_tx_hook,
.fwd = tesla_fwd_hook,
+1 -16
View File
@@ -404,12 +404,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
tx = false;
}
// Camry Auto Hold replaces the camera AEB message only while stopped.
if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) {
if (vehicle_moving || gas_pressed || !acc_main_on) {
tx = false;
}
} else if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
tx = false;
}
}
@@ -576,21 +571,11 @@ static safety_config toyota_init(uint16_t param) {
return ret;
}
static bool toyota_fwd_hook(int bus_num, int addr) {
bool block_msg = false;
if (bus_num == 2) {
block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) &&
!vehicle_moving && !gas_pressed && acc_main_on;
}
return block_msg;
}
const safety_hooks toyota_hooks = {
.init = toyota_init,
.rx = toyota_rx_hook,
.rx_all = toyota_rx_all_hook,
.tx = toyota_tx_hook,
.fwd = toyota_fwd_hook,
.get_checksum = toyota_get_checksum,
.compute_checksum = toyota_compute_checksum,
.get_quality_flag_valid = toyota_get_quality_flag_valid,
+3 -41
View File
@@ -71,27 +71,6 @@ static uint16_t volvo_ecm_1_addr;
static uint16_t volvo_bus1_cruise_control_addr;
static bool volvo_c1;
#define VOLVO_STOCK_LCA5_FRAMES 4U
#define VOLVO_STOCK_LCA5_MAX_AGE_US 100000U
static uint8_t volvo_stock_lca5_data[VOLVO_STOCK_LCA5_FRAMES][8];
static uint32_t volvo_stock_lca5_ts[VOLVO_STOCK_LCA5_FRAMES];
static bool volvo_stock_lca5_valid[VOLVO_STOCK_LCA5_FRAMES];
static uint8_t volvo_stock_lca5_index;
static bool volvo_lca5_stock_relay(const CANPacket_t *msg) {
const uint32_t now = microsecond_timer_get();
bool matches_stock = false;
for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
bool matches = volvo_stock_lca5_valid[i] &&
(safety_get_ts_elapsed(now, volvo_stock_lca5_ts[i]) <= VOLVO_STOCK_LCA5_MAX_AGE_US);
for (uint8_t byte = 0U; byte < 8U; byte++) {
matches &= msg->data[byte] == volvo_stock_lca5_data[i][byte];
}
matches_stock |= matches;
}
return matches_stock;
}
static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) {
return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]);
}
@@ -180,15 +159,6 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
// Main bus (bus 0) messages
if (msg->bus == VOLVO_MAIN_BUS) {
if (msg->addr == VOLVO_LCA_5) {
for (uint8_t byte = 0U; byte < 8U; byte++) {
volvo_stock_lca5_data[volvo_stock_lca5_index][byte] = msg->data[byte];
}
volvo_stock_lca5_ts[volvo_stock_lca5_index] = microsecond_timer_get();
volvo_stock_lca5_valid[volvo_stock_lca5_index] = true;
volvo_stock_lca5_index = (volvo_stock_lca5_index + 1U) % VOLVO_STOCK_LCA5_FRAMES;
}
// Update brake pedal and cruise state from BCM2
if (msg->addr == VOLVO_LCA_2) {
// DBC: SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) - inverted in DBC, so we invert raw bit
@@ -293,13 +263,9 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
// LCA frame also contains an angle-shaped field, but the imported controller
// deliberately leaves that field at the observed vehicle value.
if (msg->addr == VOLVO_LCA_5) {
if (!controls_allowed && volvo_lca5_stock_relay(msg)) {
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN);
} else {
const int desired_angle = volvo_lca_5_angle(msg);
tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
}
const int desired_angle = volvo_lca_5_angle(msg);
tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
}
// Keep the two torque-authority arms and the companion LCA angle bounded even
@@ -391,10 +357,6 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
static safety_config volvo_init(uint16_t param) {
bool spa = GET_FLAG(param, VOLVO_FLAG_SPA);
volvo_c1 = GET_FLAG(param, VOLVO_FLAG_C1);
volvo_stock_lca5_index = 0U;
for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
volvo_stock_lca5_valid[i] = false;
}
if (volvo_c1) {
static const CanMsg VOLVO_C1_TX_MSGS[] = {
@@ -20,7 +20,6 @@ def replay_drive(msgs, safety_mode, param, alternative_experience):
init_segment(safety, msgs, safety_mode, param)
rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0
tx_lateral, tx_lateral_blocked = 0, 0
safety_tick_rx_invalid = False
blocked_addrs = Counter()
invalid_addrs = set()
@@ -39,20 +38,14 @@ def replay_drive(msgs, safety_mode, param, alternative_experience):
if msg.which() == 'sendcan':
for canmsg in msg.sendcan:
_msg = package_can_msg(canmsg)
# TX hooks can revoke permission on a violation. Count the permission
# before checking the message, including lateral-only AOL operation.
controls_allowed = safety.get_controls_allowed()
lateral_allowed = controls_allowed or safety.get_aol_allowed()
sent = safety.safety_tx_hook(_msg)
if not sent:
tx_blocked += 1
tx_controls_blocked += controls_allowed
tx_lateral_blocked += lateral_allowed
tx_controls_blocked += safety.get_controls_allowed()
blocked_addrs[canmsg.address] += 1
carlog.debug("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9))
tx_controls += controls_allowed
tx_lateral += lateral_allowed
tx_controls += safety.get_controls_allowed()
tx_tot += 1
elif msg.which() == 'can':
# ignore msgs we sent
@@ -75,11 +68,9 @@ def replay_drive(msgs, safety_mode, param, alternative_experience):
print("total msgs with controls allowed:", tx_controls)
print("blocked msgs:", tx_blocked)
print("blocked with controls allowed:", tx_controls_blocked)
print("total msgs with lateral allowed:", tx_lateral)
print("blocked with lateral allowed:", tx_lateral_blocked)
print("blocked addrs:", blocked_addrs)
return tx_lateral_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid
return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid
if __name__ == "__main__":
@@ -1,63 +0,0 @@
import importlib.util
import sys
from pathlib import Path
from types import SimpleNamespace
from unittest.mock import Mock
import pytest
@pytest.fixture
def replay_module(monkeypatch):
# Load the sibling source explicitly, without native safety libraries or a
# host-runtime snapshot. These tests isolate replay accounting, not CAN rules.
monkeypatch.setitem(sys.modules, "opendbc.car.carlog", SimpleNamespace(carlog=Mock()))
monkeypatch.setitem(sys.modules, "opendbc.safety.tests.libsafety", SimpleNamespace(libsafety_py=SimpleNamespace()))
monkeypatch.setitem(sys.modules, "opendbc.safety.tests.safety_replay.helpers",
SimpleNamespace(package_can_msg=lambda msg: msg, init_segment=Mock()))
spec = importlib.util.spec_from_file_location("replay_drive_accounting", Path(__file__).with_name("replay_drive.py"))
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
module.tqdm = lambda msgs: msgs
return module
@pytest.mark.parametrize("controls,aol,accepted,post_controls,post_aol", [
(False, True, False, False, True), # AOL-only denial must fail replay.
(False, True, False, False, False), # A TX hook can revoke AOL permission.
(True, False, False, False, False), # A TX hook can revoke controls permission.
(False, False, False, False, False), # Expected inactive blocks remain allowed.
(False, False, False, True, True), # Post-hook permission must not misclassify a block.
(False, True, True, False, True),
(True, False, True, True, False),
(True, True, True, True, True), # Count overlapping permissions only once.
])
def test_tx_authorization_accounted_before_hook(replay_module, capsys, controls, aol, accepted, post_controls, post_aol):
state = SimpleNamespace(controls=controls, aol=aol)
def tx_hook(msg):
state.controls = post_controls
state.aol = post_aol
return accepted
safety = Mock()
safety.set_safety_hooks.return_value = 0
safety.get_controls_allowed.side_effect = lambda: state.controls
safety.get_aol_allowed.side_effect = lambda: state.aol
safety.safety_tx_hook.side_effect = tx_hook
replay_module.libsafety_py.libsafety = safety
packet = SimpleNamespace(address=0x488, src=0, dat=b"\x00" * 4)
msg = SimpleNamespace(logMonoTime=0, sendcan=[packet], which=lambda: "sendcan")
result = replay_module.replay_drive([msg], 10, 0, 0)
lateral_allowed = controls or aol
assert result == (accepted or not lateral_allowed)
safety.safety_tx_hook.assert_called_once_with(packet)
output = capsys.readouterr().out
assert "total openpilot msgs: 1\n" in output
assert f"total msgs with controls allowed: {int(controls)}\n" in output
assert f"blocked msgs: {int(not accepted)}\n" in output
assert f"blocked with controls allowed: {int(controls and not accepted)}\n" in output
assert f"total msgs with lateral allowed: {int(lateral_allowed)}\n" in output
assert f"blocked with lateral allowed: {int(lateral_allowed and not accepted)}\n" in output
@@ -467,92 +467,6 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
self.safety.init_tests()
class TestFordMachEExtendedCurvatureSafety(TestFordCANFDStockSafety):
def setUp(self):
self.packer = CANPackerSafety("ford_lincoln_base_pt")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.ford,
FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_CURVATURE)
self.safety.init_tests()
def test_mach_e_extended_curvature_error(self):
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(0.0, 12.0)
self.assertTrue(self._tx(self._extended_lka_msg()))
for curvature, allowed in ((0.0058, True), (0.0062, False), (-0.0058, True), (-0.0062, False)):
self._set_prev_desired_angle(curvature)
self.assertEqual(allowed, self._tx(self._lat_ctl_msg(True, 0.0, 0.0, curvature, 0.0)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.005, 0.0)))
def test_mach_e_bounded_path_angle_assist(self):
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(0.02, 7.5)
self._set_prev_desired_angle(0.02)
self.assertTrue(self._tx(self._extended_lka_msg()))
for path_angle in (0.055, 0.11, 0.15):
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, path_angle, 0.02, 0.0)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.161, 0.02, 0.0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.02, 0.0)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, -0.055, 0.02, 0.0)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.018, 0.0)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.12, 0.02, 0.0)))
self._reset_curvature_measurement(0.02, 9.0)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0)))
def test_mach_e_aol_keeps_curvature_without_path_angle_assist(self):
self.safety.set_alternative_experience(32)
self._rx(self._toggle_aol(True))
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._extended_lka_msg()))
for sign in (-1, 1):
self._reset_curvature_measurement(sign * 0.02, 7.5)
self._set_prev_desired_angle(sign * 0.02)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
def test_mach_e_accelerator_assist_requires_retained_engagement(self):
for sign in (-1, 1):
self.setUp()
self.safety.set_alternative_experience(32)
self._rx(self._toggle_aol(True))
self._reset_curvature_measurement(sign * 0.02, 7.5)
self._set_prev_desired_angle(sign * 0.02)
self._rx(self._pcm_status_msg(False))
self._rx(self._pcm_status_msg(True))
self.assertTrue(self._tx(self._extended_lka_msg()))
for gas in (0.0, 25.0, 0.0):
self._rx(self._user_gas_msg(gas))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
self._rx(self._toggle_aol(True))
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
self._rx(self._pcm_status_msg(True))
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
def test_other_canfd_fords_keep_original_error(self):
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
self.safety.init_tests()
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(0.0, 12.0)
self.assertTrue(self._tx(self._extended_lka_msg()))
self._set_prev_desired_angle(0.0058)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.0058, 0.0)))
self._reset_curvature_measurement(0.02, 7.5)
self._set_prev_desired_angle(0.02)
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0)))
class TestFordStockSafety(TestFordSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl
STOCK_LONGITUDINAL = True
@@ -635,23 +635,6 @@ class TestHyundaiLongitudinalAolLkasOnEngageSafety(HyundaiAolLkasOnEngageBase, T
HyundaiSafetyFlags.LONG | HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
self.safety.init_tests()
def test_main_off_after_brake_keeps_lateral_permission(self):
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
self._rx(self._button_msg(Buttons.NONE, main_button=1))
self._rx(self._button_msg(Buttons.NONE, main_button=0))
self._rx(self._button_msg(Buttons.SET))
self._rx(self._button_msg(Buttons.NONE))
self._rx(self._user_brake_msg(True))
self._rx(self._button_msg(Buttons.NONE, main_button=1))
self._rx(self._button_msg(Buttons.NONE, main_button=0))
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_acc_main_on())
self.assertTrue(self.safety.get_lkas_on())
self._set_prev_torque(0)
self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
class TestHyundaiLongitudinalAolMainLkasOnEngageSafety(TestHyundaiLongitudinalSafety):
def setUp(self):
@@ -959,85 +959,5 @@ class TestHyundaiCanfdLKASteeringAolLkasOnEngageEV(HyundaiAolLkasOnEngageStockBa
self.safety.init_tests()
class TestSportageNoStockLka(unittest.TestCase):
TX_MSGS = None # Supplemental transition tests, not a separate safety mode.
PARAM = (HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT |
HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.HYBRID_GAS)
def setUp(self):
self.safety = libsafety_py.libsafety
self.packer = CANPackerSafety("hyundai_canfd_generated")
self._init(True)
def _init(self, suppress):
param = self.PARAM | (HyundaiStarPilotSafetyFlags.CANFD_NO_STOCK_LKA if suppress else 0)
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param)
self.safety.init_tests()
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
def _speed(self, speed):
for _ in range(common.MAX_SAMPLE_VALS):
self.safety.safety_rx_hook(self.packer.make_can_msg_safety(
"WHEEL_SPEEDS", 1, {f"WHL_Spd{pos}Val": speed for pos in ("FL", "FR", "RL", "RR")}))
def _toggle(self):
for pressed in (1, 0):
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"LDA_BTN": pressed}))
def _steer(self, active, gain=None):
return self.packer.make_can_msg_safety("LKAS_ALT", 0, {
"LKAS_ANGLE_ACTIVE": 2 if active else 1,
"ADAS_StrAnglReqVal": 0,
"ADAS_ACIAnglTqRedcGainVal": (0.4 if active else 0.0) if gain is None else gain,
"Damping_Gain": 100,
})
def test_stock_scc_buttons_require_engagement(self):
resume = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.RESUME})
set_button = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.SET})
self.assertFalse(self.safety.safety_tx_hook(resume))
self.assertFalse(self.safety.safety_tx_hook(set_button))
self.safety.safety_rx_hook(set_button)
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 1}))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self.safety.safety_tx_hook(resume))
self.assertTrue(self.safety.safety_tx_hook(set_button))
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 0}))
self.assertFalse(self.safety.safety_tx_hook(resume))
self.assertFalse(self.safety.safety_tx_hook(set_button))
def test_aol_toggle_keeps_stock_blocked_and_inactive_status_allowed(self):
self._speed(30)
for expected_aol in (False, True, False, True, False):
if expected_aol != self.safety.get_aol_allowed():
self._toggle()
self.assertEqual(expected_aol, self.safety.get_aol_allowed())
for addr in (0x110, 0x362):
self.assertEqual(-1, self.safety.safety_fwd_hook(2, addr))
self.assertTrue(self.safety.safety_tx_hook(self._steer(False)))
self.assertTrue(self.safety.safety_tx_hook(common.make_msg(0, 0x362, 32)))
self.assertEqual(expected_aol, self.safety.safety_tx_hook(self._steer(True)))
self.assertFalse(self.safety.safety_tx_hook(self._steer(False, gain=0.4)))
def test_standstill_does_not_allow_active_steering(self):
self._speed(0)
self._toggle()
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self.safety.safety_tx_hook(self._steer(True)))
self.assertTrue(self.safety.safety_tx_hook(self._steer(False)))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x110))
def test_unflagged_handoff_and_reinitialization_unchanged(self):
self._init(False)
self._speed(30)
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x110))
self.assertFalse(self.safety.safety_tx_hook(self._steer(False)))
self._toggle()
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x110))
self.assertTrue(self.safety.safety_tx_hook(self._steer(True)))
if __name__ == "__main__":
unittest.main()
@@ -4,7 +4,6 @@ from opendbc.can import CANPacker
from opendbc.car import create_gas_interceptor_command
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests.test_hyundai import checksum
@pytest.mark.parametrize("param", [0x9405, 0x9C05, 0x9401, 0x1005, 0])
@@ -21,10 +20,8 @@ def test_ray_pedal_tx_isolation_and_limits(param):
has_ray_signature = param in (0x9405, 0x9C05)
assert tx(0) is has_ray_signature
assert tx(0.55) is has_ray_signature
assert tx(0.65) is has_ray_signature
assert tx(0.70) is has_ray_signature
assert not tx(0.71)
assert tx(0.35) is has_ray_signature
assert not tx(0.36) # above the Ray-only initial command cap
assert not tx(1.0)
if has_ray_signature:
@@ -52,24 +49,6 @@ def test_ray_pedal_rx_crc_is_checked_only_for_ray_signature():
assert not safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, bytes(bad_crc)))
@pytest.mark.parametrize(("track1", "track2", "allowed"), [
(530, 1035, True), (531, 1035, False), (530, 1036, False),
(263, 497, False), (264, 496, False), (530, 980, False),
])
def test_ray_tuned_command_validates_each_track(track1, track2, allowed):
safety = libsafety_py.libsafety
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
safety.init_tests()
safety.set_controls_allowed(True)
packer = CANPacker("hyundai_kia_ray_pedal")
addr, dat, bus = packer.make_can_msg("GAS_COMMAND", 0, {
"GAS_COMMAND": (track1 - 264) * 0.672,
"GAS_COMMAND2": (track2 - 497) * 0.332,
"ENABLE": 1, "COUNTER_PEDAL": 1,
})
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat)) is allowed
def test_ray_native_commanded_gas_does_not_cancel_driver_override_safety():
safety = libsafety_py.libsafety
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
@@ -103,49 +82,3 @@ def test_non_ray_hyundai_ev_keeps_native_driver_gas_detection():
native_gas = bytes.fromhex("004e008000ae0700")
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x371, 0, native_gas))
assert safety.get_gas_pressed_prev()
@pytest.mark.parametrize("controls_allowed", [False, True])
def test_ray_native_cruise_cancel_allowed_during_pedal_override(controls_allowed):
safety = libsafety_py.libsafety
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
safety.init_tests()
safety.set_controls_allowed(controls_allowed)
safety.set_gas_pressed_prev(True)
packer = CANPacker("hyundai_can_refresh_generated")
addr, dat, bus = packer.make_can_msg("CLU11", 0, {"CF_Clu_CruiseSwState": 4})
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
pedal_packer = CANPacker("hyundai_kia_ray_pedal")
addr, dat, bus = create_gas_interceptor_command(pedal_packer, 0.1, 3)
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
def test_ray_standstill_launch_obeys_hardware_brake_override():
safety = libsafety_py.libsafety
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
safety.init_tests()
packer = CANPacker("hyundai_can_refresh_generated")
pedal_packer = CANPacker("hyundai_kia_ray_pedal")
def rx(name, values):
addr, dat, bus = checksum(packer.make_can_msg(name, 0, values))
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
def tx(gas):
addr, dat, bus = create_gas_interceptor_command(pedal_packer, gas, 0)
return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
rx("WHL_SPD11", {"WHL_SPD_FL": 0, "WHL_SPD_RR": 0})
assert not safety.get_vehicle_moving()
rx("TCS13", {"DriverOverride": 2})
safety.set_controls_allowed(True)
assert safety.get_brake_pressed_prev()
assert tx(0)
assert not tx(0.012)
rx("TCS13", {"DriverOverride": 0})
assert not safety.get_brake_pressed_prev()
assert tx(0.012)
rx("TCS13", {"DriverOverride": 2})
assert not tx(0.012)
assert tx(0)
@@ -32,7 +32,6 @@ class SubaruMsg(enum.IntEnum):
ES_DashStatus = 0x321
ES_LKAS_State = 0x322
ES_Infotainment = 0x323
ES_LKAS_Alert = 0x3C4
ES_UDS_Request = 0x787
ES_HighBeamAssist = 0x121
ES_STATIC_1 = 0x22a
@@ -386,32 +385,6 @@ class TestSubaruGen2FixedAngleStopStartSafety(TestSubaruGen2FixedAngleSafety):
self.assertFalse(self._tx(self._stop_start_msg(False)))
class TestSubaruGen2AscentLkasAlertSafety(TestSubaruGen2FixedAngleSafety):
FLAGS = TestSubaruGen2FixedAngleSafety.FLAGS | SubaruSafetyFlags.ASCENT_LKAS_ALERT
TX_MSGS = TestSubaruGen2FixedAngleSafety.TX_MSGS + [[SubaruMsg.ES_LKAS_Alert, SUBARU_MAIN_BUS]]
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (*TestSubaruAngleSafetyBase.RELAY_MALFUNCTION_ADDRS[SUBARU_MAIN_BUS],
SubaruMsg.ES_LKAS_Alert)}
FWD_BLACKLISTED_ADDRS = {SUBARU_CAM_BUS: (*TestSubaruAngleSafetyBase.FWD_BLACKLISTED_ADDRS[SUBARU_CAM_BUS],
SubaruMsg.ES_LKAS_Alert)}
def test_secondary_alert_tx_requires_checksum(self):
from opendbc.safety.tests.libsafety.libsafety_py import make_CANPacket
self.assertTrue(self._tx(make_CANPacket(SubaruMsg.ES_LKAS_Alert, SUBARU_MAIN_BUS, bytes.fromhex("ce02000104000000"))))
self.assertFalse(self._tx(make_CANPacket(SubaruMsg.ES_LKAS_Alert, SUBARU_MAIN_BUS, bytes.fromhex("0002000104000000"))))
def test_secondary_alert_permission_and_forwarding_are_flag_gated(self):
from opendbc.safety.tests.libsafety.libsafety_py import make_CANPacket
msg = make_CANPacket(SubaruMsg.ES_LKAS_Alert, SUBARU_MAIN_BUS, bytes.fromhex("ce02000104000000"))
for flags in (0, SubaruSafetyFlags.GEN2, SubaruSafetyFlags.LKAS_ANGLE, TestSubaruGen2FixedAngleSafety.FLAGS,
TestSubaruDPlatformAngleSafety.FLAGS, TestSubaruDPlatformCameraAngleSafety.FLAGS,
SubaruSafetyFlags.ASCENT_LKAS_ALERT):
with self.subTest(flags=flags):
self.safety.set_safety_hooks(CarParams.SafetyModel.subaru, flags)
self.safety.init_tests()
self.assertFalse(self._tx(msg))
self.assertEqual(self.safety.safety_fwd_hook(SUBARU_CAM_BUS, SubaruMsg.ES_LKAS_Alert), SUBARU_MAIN_BUS)
class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM
ALT_MAIN_BUS = SUBARU_ALT_BUS
@@ -1,107 +0,0 @@
import pytest
from opendbc.car.structs import CarParams
from opendbc.car.subaru.avh import AVH_REQUEST, AVH_STATUS, INPUTS, avh_request, checksum
from opendbc.car.subaru.tests.test_avh import sample
from opendbc.car.subaru.values import SubaruSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
FLAGS = int(SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.FIXED_ANGLE_LIMITS |
SubaruSafetyFlags.STOP_START_BUTTON | SubaruSafetyFlags.AVH_STARTUP)
def packet(address, data, bus=1):
return libsafety_py.make_CANPacket(address, bus, data)
@pytest.fixture
def safety():
s = libsafety_py.libsafety
s.set_timer(0)
assert s.set_safety_hooks(CarParams.SafetyModel.subaru, FLAGS) == 0
s.set_controls_allowed(False)
for tick in range(101):
s.set_timer(tick * 100_000)
for address in INPUTS:
if address != AVH_REQUEST or tick % 10 == 0:
assert s.safety_rx_hook(packet(address, sample(address, tick // 10 if address == AVH_REQUEST else tick)))
return s
def request(step=1):
return avh_request(sample(AVH_REQUEST, 10), step)[1]
def test_pair_and_third_frame_blocked(safety):
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
safety.set_timer(10_050_000)
assert safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
safety.set_timer(10_100_000)
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
@pytest.mark.parametrize('byte', range(8))
def test_payload_mutation_blocked(safety, byte):
data = bytearray(request())
data[byte] ^= 4
if byte:
data[0] = checksum(AVH_REQUEST, data)
assert not safety.safety_tx_hook(packet(AVH_REQUEST, data))
@pytest.mark.parametrize('bus', [0, 2])
def test_wrong_bus_blocked(safety, bus):
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(), bus))
@pytest.mark.parametrize('delay', [44_999, 80_001])
def test_followup_timing(safety, delay):
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
safety.set_timer(10_000_000 + delay)
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
@pytest.mark.parametrize('address,offset,value', [(AVH_REQUEST, 2, 1), (AVH_STATUS, 5, 32),
(0x40, 4, 1), (0x48, 3, 3), (0x13A, 2, 1)])
def test_abort_on_manual_ack_or_movement(safety, address, offset, value):
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
safety.set_timer(10_050_000)
data = bytearray(sample(address, 11 if address == AVH_REQUEST else 101))
data[offset] = value
data[0] = checksum(address, data)
assert safety.safety_rx_hook(packet(address, data))
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
def test_stale_template_and_status_tx_blocked(safety):
assert not safety.safety_tx_hook(packet(AVH_STATUS, sample(AVH_STATUS, 1)))
safety.set_timer(10_030_001)
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
@pytest.mark.parametrize('flags', [FLAGS & ~1024, FLAGS | 32, FLAGS | 2, FLAGS & ~16, FLAGS | 512])
def test_permission_gates(safety, flags):
assert safety.set_safety_hooks(CarParams.SafetyModel.subaru, flags) == 0
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
@pytest.mark.parametrize('reason', ['new_template', 'corrupt', 'engaged', 'expired', 'duplicate'])
def test_extra_failure_gates(safety, reason):
if reason == 'engaged':
safety.set_controls_allowed(True)
elif reason == 'expired':
safety.set_timer(30_000_001)
elif reason == 'duplicate':
safety.set_timer(10_040_000)
assert safety.safety_rx_hook(packet(AVH_REQUEST, sample(AVH_REQUEST, 10)))
elif reason == 'corrupt':
data = bytearray(sample(AVH_REQUEST, 11))
data[0] ^= 1
safety.safety_rx_hook(packet(AVH_REQUEST, data))
else:
assert safety.safety_tx_hook(packet(AVH_REQUEST, request()))
safety.set_timer(10_050_000)
assert safety.safety_rx_hook(packet(AVH_REQUEST, sample(AVH_REQUEST, 11)))
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request(2)))
return
assert not safety.safety_tx_hook(packet(AVH_REQUEST, request()))
@@ -1,257 +0,0 @@
import unittest
from opendbc.car.structs import CarParams
from opendbc.car.tesla.values import TeslaSafetyFlags
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.safety.tests.common import CANPackerSafety, make_msg
from opendbc.safety.tests import test_tesla
class TestTeslaScreenButton(unittest.TestCase):
TX_MSGS = None
def setUp(self):
self.helper = test_tesla.TestTeslaStockSafety()
self.helper.setUp()
self.safety = self.helper.safety
self.vehicle_packer = CANPackerSafety("tesla_model3_vehicle")
self.init_safety()
def init_safety(self, flag=True, aol=True, long_control=False, disengage_on_brake=False, cooperative=False):
param = (TeslaSafetyFlags.AOL_SCREEN_BUTTON if flag else 0) | (TeslaSafetyFlags.LONG_CONTROL if long_control else 0)
if cooperative:
param |= TeslaSafetyFlags.COOP_STEERING
if disengage_on_brake:
param |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE
self.assertEqual(self.safety.set_safety_hooks(CarParams.SafetyModel.tesla, param), 0)
self.safety.init_tests()
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL if aol else 0)
self.helper._rx(self.helper._pcm_status_msg(False))
self.safety.set_angle_meas(0, 0)
self.safety.set_desired_angle_last(0)
def touch(self, fingers, bus=1):
self.helper._rx(self.vehicle_packer.make_can_msg_safety("UI_status2", bus, {"UI_activeTouchPoints": fingers}))
def gesture(self):
self.touch(0)
self.touch(3)
def stalk(self, status, bus=1):
self.helper._rx(self.vehicle_packer.make_can_msg_safety("SCCM_rightStalk", bus, {"SCCM_rightStalkStatus": status}))
def test_low_speed_acc_availability_does_not_clear_gesture(self):
for cooperative in (False, True):
for initial_available in (False, True):
with self.subTest(cooperative=cooperative, initial_available=initial_available):
self.init_safety(cooperative=cooperative)
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if initial_available else 0})
self.helper._rx(msg)
self.assertFalse(self.safety.get_aol_allowed())
self.gesture()
for available in (True, False) * 20:
self.helper._rx(self.helper._speed_msg(3.0))
self.helper._rx(self.helper._speed_msg_2(3.0))
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if available else 0})
self.helper._rx(msg)
self.assertTrue(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
self.gesture()
for available in (True, False) * 20:
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if available else 0})
self.helper._rx(msg)
self.assertFalse(self.safety.get_lkas_on())
self.assertFalse(self.safety.get_aol_allowed())
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_cruise_engagement_arms_lateral_and_brake_respects_option(self):
for disengage_on_brake in (False, True):
with self.subTest(disengage_on_brake=disengage_on_brake):
self.init_safety(disengage_on_brake=disengage_on_brake)
self.helper._rx(self.helper._pcm_status_msg(True))
self.assertTrue(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self.helper._rx(self.helper._user_brake_msg(True))
self.helper._rx(self.helper._pcm_status_msg(False))
self.helper._rx(self.helper._user_brake_msg(False))
self.assertEqual(self.safety.get_lkas_on(), not disengage_on_brake)
self.assertEqual(self.safety.get_aol_allowed(), not disengage_on_brake)
self.assertFalse(self.safety.get_controls_allowed())
def test_cruise_engagement_does_not_override_screen_disengagement_inputs(self):
for blocked_by in ("brake", "steering", "cancel"):
with self.subTest(blocked_by=blocked_by):
self.init_safety(disengage_on_brake=True)
if blocked_by == "brake":
self.helper._rx(self.helper._user_brake_msg(True))
elif blocked_by == "steering":
self.helper._rx(self.helper._angle_meas_msg(0, hands_on_level=3))
else:
self.stalk(1)
self.helper._rx(self.helper._pcm_status_msg(True))
self.assertFalse(self.safety.get_lkas_on())
self.assertFalse(self.safety.get_aol_allowed())
def test_stalk_cancel_clears_gesture_and_blocks_tap_while_held(self):
for status in (1, 2):
with self.subTest(status=status):
self.init_safety()
self.gesture()
self.stalk(status)
self.assertFalse(self.safety.get_lkas_on())
self.assertFalse(self.safety.get_aol_allowed())
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
self.gesture()
self.assertFalse(self.safety.get_lkas_on())
self.stalk(0)
self.assertFalse(self.safety.get_lkas_on())
self.gesture()
self.assertTrue(self.safety.get_lkas_on())
def test_non_cancel_stalk_positions_do_not_clear_gesture(self):
self.gesture()
for status in (0, 3, 4, 5, 6, 7):
self.stalk(status)
self.assertTrue(self.safety.get_lkas_on())
def test_stalk_cancel_is_addon_only(self):
self.gesture()
for bus in (0, 2, 3):
self.stalk(1, bus)
self.assertTrue(self.safety.get_lkas_on())
for length in (0, 1, 2, 4, 5, 8):
msg = make_msg(1, 0x229, length)
if length > 1:
msg[0].data[1] = 0x10
self.helper._rx(msg)
self.assertTrue(self.safety.get_lkas_on())
self.init_safety(flag=False)
self.helper._rx(self.helper._pcm_status_msg(True))
self.stalk(1)
self.assertTrue(self.safety.get_controls_allowed())
def test_gesture_enables_only_lateral(self):
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
self.gesture()
self.assertTrue(self.safety.get_aol_allowed())
self.assertTrue(self.safety.get_lkas_on())
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1))))
self.gesture()
self.assertFalse(self.safety.get_aol_allowed())
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_does_not_enable_alpha_long_actuation(self):
self.init_safety(long_control=True)
self.gesture()
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1))))
def test_flag_and_aol_are_required(self):
for flag, aol in ((False, False), (False, True), (True, False)):
with self.subTest(flag=flag, aol=aol):
self.init_safety(flag=flag, aol=aol)
self.gesture()
self.assertFalse(self.safety.get_aol_allowed())
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_wrong_bus_and_length_do_not_toggle(self):
for bus in (0, 2, 3):
self.gesture_on_wrong_bus(bus)
self.assertFalse(self.safety.get_lkas_on())
self.touch(0)
for length in (0, 3, 4, 5, 6, 7):
msg = make_msg(1, 0x3DF, length)
if length > 3:
msg[0].data[3] = 3
self.helper._rx(msg)
self.assertFalse(self.safety.get_lkas_on())
def gesture_on_wrong_bus(self, bus):
self.touch(0, bus)
self.touch(3, bus)
def test_held_touch_and_other_finger_counts(self):
for fingers in (3, 3, 0, 1, 2, 4, 5, 255, 0):
self.touch(fingers)
self.assertFalse(self.safety.get_lkas_on())
self.touch(3)
for _ in range(20):
self.touch(3)
self.assertTrue(self.safety.get_lkas_on())
self.touch(0)
self.touch(3)
self.assertFalse(self.safety.get_lkas_on())
def test_brake_disables_long_but_not_gesture_lateral(self):
self.gesture()
self.helper._rx(self.helper._user_brake_msg(True))
self.assertTrue(self.safety.get_aol_allowed())
self.assertFalse(self.safety.get_controls_allowed())
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_opt_in_brake_disengage_needs_brake_release_and_new_gesture(self):
self.init_safety(disengage_on_brake=True)
self.gesture()
self.helper._rx(self.helper._user_brake_msg(True))
self.assertFalse(self.safety.get_aol_allowed())
self.gesture()
self.assertFalse(self.safety.get_aol_allowed())
self.helper._rx(self.helper._user_brake_msg(False))
self.assertFalse(self.safety.get_aol_allowed())
self.gesture()
self.assertTrue(self.safety.get_aol_allowed())
def test_steering_override_requires_a_new_gesture(self):
self.gesture()
self.helper._rx(self.helper._angle_meas_msg(0, hands_on_level=3))
self.assertFalse(self.safety.get_aol_allowed())
self.gesture()
self.assertFalse(self.safety.get_aol_allowed())
self.helper._rx(self.helper._angle_meas_msg(0))
self.assertFalse(self.safety.get_aol_allowed())
self.gesture()
self.assertTrue(self.safety.get_aol_allowed())
def test_cancelled_cruise_clears_gesture_latch(self):
self.gesture()
self.helper._rx(self.helper._pcm_status_msg(True))
self.helper._rx(self.helper._pcm_status_msg(False))
self.assertFalse(self.safety.get_aol_allowed())
self.assertFalse(self.safety.get_lkas_on())
def test_safety_reinitialization_clears_touch_latch(self):
self.gesture()
self.init_safety()
self.touch(3)
self.assertFalse(self.safety.get_aol_allowed())
def test_stock_lkas_does_not_block_authorized_gesture_steering(self):
self.gesture()
self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2))
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_stock_lkas_still_blocks_when_not_authorized(self):
self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2))
self.gesture()
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_autopark_still_blocks_steering(self):
self.helper._rx(self.helper._pcm_status_msg(False, autopark_state=3))
self.gesture()
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
def test_aeb_still_blocks_longitudinal_and_is_forwarded(self):
self.init_safety(long_control=True)
self.gesture()
self.helper._rx(self.helper._long_control_msg(0, aeb_event=1, bus=2))
self.assertFalse(self.helper._tx(self.helper._long_control_msg(0, acc_state=13)))
self.assertEqual(self.safety.safety_fwd_hook(2, 0x2B9), 0)
if __name__ == "__main__":
unittest.main()
@@ -147,34 +147,6 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
self.safety.set_alternative_experience(0)
self.assertFalse(self._tx(hold_msg))
def test_auto_brake_hold_aeb_replacement_only_at_standstill(self):
if (not self.LONGITUDINAL or
self.safety.get_current_safety_param() & (ToyotaSafetyFlags.STOCK_LONGITUDINAL.value | ToyotaSafetyFlags.SECOC.value)):
raise unittest.SkipTest("Toyota AEB Auto Hold requires non-SecOC openpilot longitudinal control")
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB)
hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc")
self._rx(self._speed_msg(0))
self._rx(self._toggle_aol(True))
self._rx(self._user_gas_msg(False))
self.assertTrue(self._tx(hold_msg))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._speed_msg(1.0))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._speed_msg(0))
self._rx(self._user_gas_msg(True))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._user_gas_msg(False))
self._rx(self._toggle_aol(False))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
# Only allow LTA msgs with no actuation
def test_lta_steer_cmd(self):
for engaged, req, req2, torque_wind_down, angle in itertools.product([True, False],
@@ -177,68 +177,6 @@ class TestVolvoSafetyBase(common.CarSafetyTest):
self.assertTrue(self._tx(self._angle_cmd_msg(10)))
self.assertFalse(self._tx(self._angle_cmd_msg(20)))
STOCK_LCA5 = bytes.fromhex("88c04cef1190ba00")
def _stock_lca5(self, data=None, bus=VOLVO_PARTY_BUS):
return libsafety_py.make_CANPacket(VOLVO_LCA_5, bus, self.STOCK_LCA5 if data is None else data)
def test_inactive_lca5_stock_relay_requires_unchanged_received_frame(self):
self._reset_angle_measurement(10)
self.safety.set_timer(100000)
self.assertFalse(self._tx(self._stock_lca5()))
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
self.assertTrue(self._tx(self._stock_lca5()))
self.assertEqual(self.safety.get_desired_angle_last(), round(10 / 0.05596))
for byte in range(8):
altered = bytearray(self.STOCK_LCA5)
altered[byte] ^= 1
self.assertFalse(self._tx(self._stock_lca5(bytes(altered))), f"altered {byte=}")
self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_MAIN_BUS)))
self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_PT_BUS)))
def test_inactive_lca5_stock_relay_wrong_rx_bus_and_length(self):
self._rx(self._stock_lca5(bus=VOLVO_PT_BUS))
self._rx(self._stock_lca5(self.STOCK_LCA5[:7], bus=VOLVO_MAIN_BUS))
self.assertFalse(self._tx(self._stock_lca5()))
def test_inactive_lca5_stock_relay_expires_across_timer_wrap(self):
start = 0xFFFF0000
self.safety.set_timer(start)
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
self.safety.set_timer((start + 100000) & 0xFFFFFFFF)
self.assertTrue(self._tx(self._stock_lca5()))
self.safety.set_timer((start + 100001) & 0xFFFFFFFF)
self.assertFalse(self._tx(self._stock_lca5()))
def test_inactive_lca5_stock_relay_history_is_bounded(self):
frames = []
for i in range(5):
data = bytearray(self.STOCK_LCA5)
data[3] = (data[3] + i) & 0xFF
frames.append(bytes(data))
self.safety.set_timer(i * 20000)
self._rx(self._stock_lca5(frames[-1], bus=VOLVO_MAIN_BUS))
self.assertFalse(self._tx(self._stock_lca5(frames[0])))
for data in frames[1:]:
self.assertTrue(self._tx(self._stock_lca5(data)))
def test_inactive_lca5_stock_relay_cleared_on_safety_init(self):
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
self.assertTrue(self._tx(self._stock_lca5()))
self.safety.set_safety_hooks(SAFETY_VOLVO, self.SAFETY_PARAM)
self.assertFalse(self._tx(self._stock_lca5()))
def test_stock_lca5_placeholder_does_not_bypass_active_angle_limits(self):
self._reset_angle_measurement(10)
self._reset_speed_measurement(50)
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
self.assertTrue(self._tx(self._stock_lca5()))
self.safety.set_controls_allowed(True)
self.assertFalse(self._tx(self._stock_lca5()))
self.assertTrue(self._tx(self._angle_cmd_msg(10.4)))
self.assertFalse(self._tx(self._angle_cmd_msg(11)))
def test_angle_tx_rate_matches_controller_cadence(self):
"""LCA_5 is 50 Hz, so each frame may contain two 100 Hz controller steps."""
self._reset_speed_measurement(50)
-5
View File
@@ -1,8 +1,3 @@
#pragma once
#define SPI_CAN_WRITE_SEQUENCED_ENDPOINT 0x83U
#define SPI_CAN_REPLAY_CAPABILITY 0x5351U
typedef struct {
uint8_t request;
uint16_t param1;
-4
View File
@@ -2,7 +2,6 @@
#include "board/drivers/spi_declarations.h"
#include "board/crc.h"
#include "board/drivers/spi_can_replay.h"
uint8_t spi_buf_rx[SPI_BUF_SIZE];
uint8_t spi_buf_tx[SPI_BUF_SIZE];
@@ -131,9 +130,6 @@ void spi_rx_done(void) {
} else if (spi_endpoint == 2U) {
comms_endpoint2_write(&spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi);
response_ack = true;
} else if (spi_endpoint == SPI_CAN_WRITE_SEQUENCED_ENDPOINT) {
response_ack = spi_can_write_sequenced(spi_data_len_miso, &spi_buf_rx[SPI_HEADER_SIZE], spi_data_len_mosi,
&spi_can_tx_ready);
} else if (spi_endpoint == 3U) {
if (spi_data_len_mosi > 0U) {
if (spi_can_tx_ready) {
-34
View File
@@ -1,34 +0,0 @@
#pragma once
typedef struct {
uint16_t sequence;
uint16_t length;
bool valid;
uint8_t data[SPI_BUF_SIZE];
} spi_can_replay_t;
static spi_can_replay_t spi_can_replay;
static inline void spi_can_replay_reset(void) {
spi_can_replay.valid = false;
}
static inline bool spi_can_write_sequenced(uint16_t sequence, const uint8_t *data, uint16_t length, bool *tx_ready) {
bool accepted = false;
if ((sequence != 0U) && (length > 0U) && (length < SPI_BUF_SIZE)) {
if (spi_can_replay.valid && (sequence == spi_can_replay.sequence)) {
// A lost reply must not turn an accepted CAN write into a second transmission.
accepted = (length == spi_can_replay.length) && (memcmp(data, spi_can_replay.data, length) == 0);
} else if (*tx_ready) {
*tx_ready = false;
comms_can_write(data, length);
(void)memcpy(spi_can_replay.data, data, length);
spi_can_replay.sequence = sequence;
spi_can_replay.length = length;
spi_can_replay.valid = true;
accepted = true;
} else {
}
}
return accepted;
}
-8
View File
@@ -221,14 +221,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
break;
// **** 0xdd: get healthpacket and CANPacket versions
case 0xdd:
#ifdef STM32H7
if (req->param1 == SPI_CAN_REPLAY_CAPABILITY) {
if (req->param2 == 1U) spi_can_replay_reset();
resp[0] = 1U;
resp_len = 1;
break;
}
#endif
resp[0] = HEALTH_PACKET_VERSION;
resp[1] = CAN_PACKET_VERSION;
resp[2] = CAN_HEALTH_PACKET_VERSION;

Some files were not shown because too many files have changed in this diff Show More