mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-02 03:54:03 +08:00
Compare commits
27 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| f20731225f | |||
| 244dccfae2 | |||
| 48c4abb898 | |||
| 48c4ab68df | |||
| ee223ec1f2 | |||
| eed130bf06 | |||
| 25df1b9071 | |||
| 2f1ea6d60e | |||
| 2300a51edf | |||
| 6d3d2eb847 | |||
| f1db0546f4 | |||
| 0294c53e67 | |||
| a934fa6696 | |||
| d5ace15455 | |||
| eff2a7408d | |||
| ee87261cd5 | |||
| 18a81d11e6 | |||
| 7b96d6cbd1 | |||
| 13eb3754c0 | |||
| 41f8e9288d | |||
| 0098beaf25 | |||
| 7cbbac8f60 | |||
| 03456fa4b2 | |||
| e9b3180737 | |||
| 947c6f6b10 | |||
| e84eafd9d1 | |||
| b06d8baa56 |
@@ -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
|
||||
@@ -1,19 +1,26 @@
|
||||
# StarPilot
|
||||
|
||||
[](https://deepwiki.com/StarPilotHQ/StarPilot)
|
||||
[](https://deepwiki.com/firestar5683/StarPilot)
|
||||
[](https://firestar.link/discord)
|
||||
[](https://github.com/StarPilotHQ/StarPilot)
|
||||
[](https://github.com/firestar5683/StarPilot)
|
||||
[](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,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`.
|
||||
|
||||
@@ -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.
@@ -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
@@ -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
@@ -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> |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|[](##)|[](##)|<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|[](##)|[](##)|<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|[](##)|[](##)|<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|[](##)|[](##)|<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|[](##)|[](##)|<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|[](##)|[](##)|<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|[](##)|[](##)|<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>|||
|
||||
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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
@@ -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,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)|
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -50,7 +50,6 @@ class FordSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
CANFD = 2
|
||||
LKA_STEERING = 4
|
||||
MACH_E_CURVATURE = 8
|
||||
|
||||
|
||||
class FordFlags(IntFlag):
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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: {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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 \
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -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),
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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" ;
|
||||
@@ -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";
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 = ¤t_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;
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
Reference in New Issue
Block a user