mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-04 20:53:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dev-priv/master-priv' into custom-stock-long-planner-speed

Browse Source 
# Conflicts:
#	selfdrive/ui/qt/offroad/settings.cc
#	selfdrive/ui/qt/onroad_settings.cc
This commit is contained in:
Jason Wen
2023-10-31 19:07:01 -04:00
parent cf1e01cf45 d3118cabca
commit 2ac06e8c5c
166 changed files with 4934 additions and 5210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/docs.yaml
+1
View File
@@ -54,6 +54,7 @@ jobs:
cp -r ../build/docs/html/ docs/
touch docs/.nojekyll
echo -n docs.comma.ai > docs/CNAME
git add -f .
git commit -m "build docs"
.gitignore
+1
View File
@@ -79,6 +79,7 @@ comma*.sh
selfdrive/modeld/thneed/compile
selfdrive/modeld/models/*.thneed
selfdrive/modeld/models/*.pkl
*.bz2
.gitlab-ci.yml
+162
View File
@@ -0,0 +1,162 @@
variables:
BUILD_DIR: "/data/openpilot"
OUTPUT_DIR: "${CI_PROJECT_DIR}/output"
CI_DIR: "${CI_PROJECT_DIR}/release/ci"
VERSION: "unknown"
GIT_SUBMODULE_STRATEGY: recursive
GIT_DISCOVERY_ACROSS_FILESYSTEM: 1
DEV_C3_SOURCE_BRANCH: "master-dev-c3"
stages:
- build
- commit_changes
- publish
default:
tags:
- sunnypilot
- x86
.default_before_script: &default_before_script
- 'export VERSION=$(date "+%Y.%m.%d")-${CI_PIPELINE_IID}'
- 'mkdir -p "${BUILD_DIR}/"'
- 'git config --global user.email "gitlab@pipeline.com"'
- 'git config --global user.name "Gitlab Pipeline"'
workflow: # If running on any branch other than main, use the `aws-datacontracts-dev` account; otherwise use `aws-datacontracts`
rules:
# We are an MR, but it's a draft, we won't proceed with anything.
- if: '$CI_MERGE_REQUEST_TITLE =~ /^wip:/i || $CI_MERGE_REQUEST_TITLE =~ /^draft:/i'
when: never
# We are a merge request
- if: $CI_MERGE_REQUEST_IID #|| $CI_COMMIT_REF_NAME == "gitlab-pipelines" # TBD once merged
variables:
NEW_BRANCH: ${CI_COMMIT_REF_NAME}-prebuilt
when: always
# Below are the rules when a commit is done (code has been added to the branch)
# Commit to master-dev-c3
- if: $CI_COMMIT_REF_NAME == $DEV_C3_SOURCE_BRANCH
variables:
NEW_BRANCH: "dev-c3"
when: always
#commit made to main (master)
- if: $CI_COMMIT_REF_NAME == $CI_DEFAULT_BRANCH
variables:
NEW_BRANCH: "staging-c3"
when: always
# if tag
- if: $CI_COMMIT_TAG
variables:
NEW_BRANCH: "release-c3"
- when: always
build:
stage: build
cache:
key: "${CI_COMMIT_REF_SLUG}"
paths:
- "${CI_DIR}/scons_cache"
before_script:
- *default_before_script
- "echo Starting build stage..."
- "echo BUILD_DIR: ${BUILD_DIR}"
- "echo CI_DIR: ${CI_DIR}"
- "echo VERSION: ${VERSION}"
- "echo CI_COMMIT_REF_NAME: ${CI_COMMIT_REF_NAME}"
- git config --global --add safe.directory ${CI_PROJECT_DIR}
script:
- export PYTHONPATH="$BUILD_DIR"
- "echo Calling to build [${CI_DIR}/build.sh ${CI_PROJECT_DIR} ${BUILD_DIR} ${VERSION}]"
- "echo Building Panda..."
- scons -j$(nproc) cache_dir=${CI_DIR}/scons_cache ${CI_PROJECT_DIR}/panda
- mkdir -p ${BUILD_DIR}
- ls -la ${BUILD_DIR}
- "echo Building Rest..."
- cp -pR --parents $(cat release/files_common release/files_tici) $BUILD_DIR/
- cd $BUILD_DIR
- sed -i '/from .board.jungle import PandaJungle, PandaJungleDFU/s/^/#/' panda/__init__.py # comment panda jungle when prebuilt
- scons -j$(nproc) cache_dir=${CI_DIR}/scons_cache
# Cleanup
- echo "Doing cleanup for prebuilt\n\n"
- echo "Deleting specified files..."
- find ${BUILD_DIR} \( -name '*.a' -o -name '*.o' -o -name '*.os' -o -name '*.pyc' -o -name 'moc_*' -o -name '*.cc' -o -name "Jenkinsfile" -o -name "supercombo.onnx" -o -name ".sconsign.dblite" \) -print -delete
- echo "Deleting Headers from selfdrive/ui path..."
- find ${BUILD_DIR}/selfdrive/ui -type f -name '*.h' -print -delete
- echo "Deleting the following folders and subdirectories..."
- find "${BUILD_DIR}" -type d \( -path "*panda/board" -o -path "*panda/certs" -o -path "*panda/crypto" -o -path "*release" -o -path "*.github" -o -path "*selfdrive/ui/replay" -o -path "*__pycache__" \) -print -exec rm -rf {} +
# Move back signed panda fw
- mkdir -p ${BUILD_DIR}/panda/board/obj
- cp ${CI_PROJECT_DIR}/panda/board/obj/panda.bin.signed ${BUILD_DIR}/panda/board/obj/panda.bin.signed
- cp ${CI_PROJECT_DIR}/panda/board/obj/panda_h7.bin.signed ${BUILD_DIR}/panda/board/obj/panda_h7.bin.signed
- cp ${CI_PROJECT_DIR}/panda/board/obj/bootstub.panda.bin ${BUILD_DIR}/panda/board/obj/bootstub.panda.bin
- cp ${CI_PROJECT_DIR}/panda/board/obj/bootstub.panda_h7.bin ${BUILD_DIR}/panda/board/obj/bootstub.panda_h7.bin
- touch ${BUILD_DIR}/prebuilt
- mkdir -p ${OUTPUT_DIR}
- shopt -s dotglob && mv ${BUILD_DIR}/* ${OUTPUT_DIR}
artifacts:
paths:
- ${OUTPUT_DIR}/
tags: [ 'sunnypilot', 'tici' ]
rules:
- if: $CI_MERGE_REQUEST_IID
when: manual
- if: $NEW_BRANCH
when: always
commit changes for public branch:
variables:
GIT_SUBMODULE_STRATEGY: normal
GIT_ORIGIN: git@gitlab.com:sunnypilot/public/sunnypilot.git
stage: commit_changes
dependencies:
- build
before_script:
- 'apk update && apk upgrade'
- 'apk add git bash openssh'
- 'eval $(ssh-agent -s)'
- echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add -
- 'mkdir -p ~/.ssh/'
- 'chmod 700 ~/.ssh'
- 'ssh-keyscan -H gitlab.com >> ~/.ssh/known_hosts' # Adding gitlab to trusted
- 'chmod 644 ~/.ssh/known_hosts'
- *default_before_script
script:
#- GIT_ORIGIN=$(echo "${CI_REPOSITORY_URL}" | sed -e 's|https\?://gitlab-ci-token:.*@|ssh://git@|g')
- echo "${GIT_ORIGIN}"
- echo "Calling to publish [${CI_DIR}/publish.sh ${CI_PROJECT_DIR} ${OUTPUT_DIR} ${NEW_BRANCH} ${VERSION} ${GIT_ORIGIN}]"
- git config --global --add safe.directory ${OUTPUT_DIR}
- $CI_DIR/publish.sh "${CI_PROJECT_DIR}" "${OUTPUT_DIR}" "${NEW_BRANCH}" "${VERSION}" "${GIT_ORIGIN}"
rules:
- if: $NEW_BRANCH
when: on_success
- when: never
publish to github:
variables:
GIT_SUBMODULE_STRATEGY: normal
stage: publish
dependencies:
- build
before_script:
- 'apk update && apk upgrade'
- 'apk add git bash openssh'
- 'eval $(ssh-agent -s)'
- echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add -
- 'mkdir -p ~/.ssh/'
- 'chmod 700 ~/.ssh'
- 'ssh-keyscan -H gitlab.com >> ~/.ssh/known_hosts' # Adding gitlab to trusted
- 'chmod 644 ~/.ssh/known_hosts'
- *default_before_script
script:
- echo "This part is not yet implemented intentionally, just an example"
rules:
- if: $NEW_BRANCH
when: manual
.pre-commit-config.yaml
+1 -1
View File
@@ -41,7 +41,7 @@ repos:
args: ['--explicit-package-bases']
exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)'
- repo: https://github.com/astral-sh/ruff-pre-commit
rev: v0.0.292
rev: v0.1.1
hooks:
- id: ruff
exclude: '^(third_party/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)'
CHANGELOGS.md
+7 -5
View File
@@ -1,15 +1,17 @@
sunnypilot - 0.9.5.1 (2023-10-xx)
sunnypilot - 0.9.5.1 (2023-11-xx)
========================
* UPDATED: Synced with commaai's master commit 41a04b4
* UPDATED: Synced with commaai's master commit 9528383
* NEW❗: Night Strike driving model
* NEW❗: Onroad Settings Panel
* Onroad buttons (i.e., DLP, GAC) moved to its dedicated panel
* Driving Personality
* Dynamic Lane Profile (DLP)
* Dynamic Experimental Control (DEC)
* Speed Limit Control (SLC)
* NEW❗: Display main feature status on onroad view in real-time
* GAP - Driving Personality
* DLP - Dynamic Lane Profile
* DEC - Dynamic Experimental Control
* SLC - Speed Limit Control
* NEW❗: Dynamic Experimental Control (DEC) thanks to dragonpilot-community!
* Automatically determines and selects between openpilot ACC and openpilot End to End longitudinal based on conditions for a more natural drive
@@ -47,15 +49,15 @@ sunnypilot - 0.9.5.1 (2023-10-xx)
* NEW❗: Custom Tuning for setting offline and live values in real-time
* UPDATED: Auto-detect custom Mapbox token if a personal Mapbox token is provided
* REMOVED: "Enable Mapbox Navigation" toggle
* UPDATED: Update color from Green 🟢 to Blue 🔵 for settings toggles and buttons
* UPDATED: Button redesign for Driving Personality, Dynamic Lane Profile, and Sidebar Temperature
* UI updates
* New Settings menu redesign and improved interactions
* FIXED: Retain hotspot/tethering state was not consistently saved
* FIXED: Map stuck in "Map Loading" if comma Prime is active
* FIXED: UI elements adjustments
* FIXED: OpenStreetMap implementation on C3X devices
* M-TSC
* Altitude (ALT.) display on Developer UI
* Current street name on top of driving screen when "OSM Debug UI" is enabled
* Hyundai Kona Non-SCC 2019 support thanks to Quex!
* Kia Seltos Non-SCC 2023-24 support thanks to Moodkiller and jeroid_!
sunnypilot - 0.9.4.1 (2023-08-11)
Jenkinsfile
Vendored
+3 -1
View File
@@ -21,6 +21,8 @@ source ~/.bash_profile
if [ -f /TICI ]; then
source /etc/profile
rm -rf ~/.commacache
if ! systemctl is-active --quiet systemd-resolved; then
echo "restarting resolved"
sudo systemctl start systemd-resolved
@@ -154,7 +156,7 @@ node {
["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR ./build_devel.sh"],
["build openpilot", "cd selfdrive/manager && ./build.py"],
["check dirty", "release/check-dirty.sh"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
["onroad tests", "pytest selfdrive/test/test_onroad.py -s"],
["time to onroad", "cd selfdrive/test/ && pytest test_time_to_onroad.py"],
])
},
RELEASES.md
+1
View File
@@ -2,6 +2,7 @@ Version 0.9.5 (2023-XX-XX)
========================
* New driving model
* Improved navigate on openpilot performance using navigation instructions as an additional model input
* Cadillac Escalade ESV 2019 support thanks to twilsonco!
* Hyundai Azera 2022 support thanks to sunnyhaibin!
* Hyundai Azera Hybrid 2020 support thanks to chanhojung and haram-KONA!
* Hyundai Custin 2023 support thanks to sunnyhaibin and Saber422!
SConstruct
+2 -2
View File
@@ -230,7 +230,8 @@ if GetOption('compile_db'):
env.CompilationDatabase('compile_commands.json')
# Setup cache dir
cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache'
default_cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache'
cache_dir = ARGUMENTS.get('cache_dir', default_cache_dir)
CacheDir(cache_dir)
Clean(["."], cache_dir)
@@ -382,7 +383,6 @@ SConscript(['rednose/SConscript'])
# Build system services
SConscript([
'system/clocksd/SConscript',
'system/proclogd/SConscript',
'system/ubloxd/SConscript',
'system/loggerd/SConscript',
common/params.cc
+1 -3
View File
@@ -211,6 +211,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"AccMadsCombo", PERSISTENT},
{"AmapKey1", PERSISTENT},
{"AmapKey2", PERSISTENT},
{"ApiCache_DriveStats", PERSISTENT},
{"AutoLaneChangeTimer", PERSISTENT},
{"AutoLaneChangeBsmDelay", PERSISTENT},
{"BelowSpeedPause", PERSISTENT},
@@ -230,12 +231,10 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CustomStockLong", PERSISTENT},
{"CustomStockLongPlanner", PERSISTENT},
{"CustomTorqueLateral", PERSISTENT},
{"DevUI", PERSISTENT},
{"DevUIInfo", PERSISTENT},
{"DisableOnroadUploads", PERSISTENT},
{"DisengageLateralOnBrake", PERSISTENT},
{"DynamicExperimentalControl", PERSISTENT},
{"DynamicExperimentalControlToggle", PERSISTENT},
{"DynamicLaneProfile", PERSISTENT},
{"EnableAmap", PERSISTENT},
{"EnableDebugSnapshot", PERSISTENT},
@@ -285,7 +284,6 @@ std::unordered_map<std::string, uint32_t> keys = {
{"ShowDebugUI", PERSISTENT},
{"SidebarTemperatureOptions", PERSISTENT},
{"SpeedLimitControl", PERSISTENT},
{"SpeedLimitPercOffset", PERSISTENT},
{"SpeedLimitValueOffset", PERSISTENT},
{"SpeedLimitOffsetType", PERSISTENT},
{"StandStillTimer", PERSISTENT},
common/prefix.py
+3
View File
@@ -14,6 +14,7 @@ class OpenpilotPrefix:
self.clean_dirs_on_exit = clean_dirs_on_exit
def __enter__(self):
self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None)
os.environ['OPENPILOT_PREFIX'] = self.prefix
try:
os.mkdir(self.msgq_path)
@@ -28,6 +29,8 @@ class OpenpilotPrefix:
self.clean_dirs()
try:
del os.environ['OPENPILOT_PREFIX']
if self.original_prefix is not None:
os.environ['OPENPILOT_PREFIX'] = self.original_prefix
except KeyError:
pass
return False
conftest.py
+5
View File
@@ -10,8 +10,13 @@ def openpilot_function_fixture():
# setup a clean environment for each test
with OpenpilotPrefix():
prefix = os.environ["OPENPILOT_PREFIX"]
yield
# ensure the test doesn't change the prefix
assert "OPENPILOT_PREFIX" in os.environ and prefix == os.environ["OPENPILOT_PREFIX"]
os.environ.clear()
os.environ.update(starting_env)
docs/CARS.md
+5 -3
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
# 265 Supported Cars
# 267 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -20,6 +20,7 @@ A supported vehicle is one that just works when you install a comma device. All
|Buick|LaCrosse 2017-19[<sup>4</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Buick&model=LaCrosse 2017-19">Buy Here</a></sub></details>||
|Cadillac|Escalade 2017[<sup>4</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade 2017">Buy Here</a></sub></details>||
|Cadillac|Escalade ESV 2016[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade ESV 2016">Buy Here</a></sub></details>||
|Cadillac|Escalade ESV 2019[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade ESV 2019">Buy Here</a></sub></details>||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Bolt EUV 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Bolt EV 2022-23">Buy Here</a></sub></details>||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Silverado 1500 2020-21">Buy Here</a></sub></details>||
@@ -74,7 +75,8 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Azera 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Azera 2022">Buy Here</a></sub></details>||
|Hyundai|Azera Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Azera Hybrid 2020">Buy Here</a></sub></details>||
|Hyundai|Custin 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Custin 2023">Buy Here</a></sub></details>||
|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2017-19">Buy Here</a></sub></details>||
|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2017-18">Buy Here</a></sub></details>||
|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2019">Buy Here</a></sub></details>||
|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-19">Buy Here</a></sub></details>||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra Hybrid 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
@@ -139,7 +141,7 @@ A supported vehicle is one that just works when you install a comma device. All
|Kia|Sorento 2018|Advanced Smart Cruise Control & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Kia|Sorento 2021-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2021-23">Buy Here</a></sub></details>||
|Kia|Sorento Hybrid 2023[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento Hybrid 2023">Buy Here</a></sub></details>||
|Kia|Sorento Hybrid 2021-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento Hybrid 2021-23">Buy Here</a></sub></details>||
|Kia|Sorento Plug-in Hybrid 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Buy Here</a></sub></details>||
|Kia|Sportage 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sportage 2023">Buy Here</a></sub></details>||
|Kia|Sportage Hybrid 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sportage Hybrid 2023">Buy Here</a></sub></details>||
panda
+1 -1
Submodule panda updated: cebcc2e09b...343ad38a8b
poetry.lock
Generated
+477 -488
View File
File diff suppressed because it is too large Load Diff
pyproject.toml
+3 -1
View File
@@ -162,7 +162,9 @@ types-pycurl = "*"
types-PyYAML = "*"
types-requests = "*"
types-tabulate = "*"
pyqt5 = { version = "*", markers = "platform_machine == 'x86_64'" } # no aarch64 wheels for macOS/linux
# this is only pinned since 5.15.11 is broken
pyqt5 = { version = "==5.15.2", markers = "platform_machine == 'x86_64'" } # no aarch64 wheels for macOS/linux
[tool.poetry.group.carla]
optional = true
release/ci/build.sh
Executable
+45
View File
@@ -0,0 +1,45 @@
# UNUSED YET BUT I WANT TO KEEP IT BECAUSE I INTEND TO USE IT SOON
#!/usr/bin/env bash
# set -e
# DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
# cd $DIR
# # Take parameters as arguments
# SOURCE_DIR=$1
# OUTPUT_DIR=$2
# # Check parameters
# if [ -z "$SOURCE_DIR" ] || [ -z "$OUTPUT_DIR" ]; then
# echo "Error: No source or output directory provided."
# exit 1
# fi
# if [ -z "$TARGETPLATFORM" ]; then
# # Detect the platform (e.g., x86_64, aarch64, etc.)
# platform=$(uname -m)
# # Map to Docker's platform syntax
# if [ "$platform" = "x86_64" ]; then
# TARGETPLATFORM="linux/amd64"
# elif [ "$platform" = "aarch64" ]; then
# TARGETPLATFORM="linux/arm64"
# # Add here more elif statements for other architectures if needed.
# else
# echo "Unknown platform: $platform"
# exit 1
# fi
# export TARGETPLATFORM
# fi
# echo "TARGETPLATFORM IS: [${TARGETPLATFORM}]"
# # Let's bring the submodules!
# git submodule update --init --recursive
# # Build
# docker compose -f ${SOURCE_DIR}/docker-compose.yml build
# docker compose -f ${SOURCE_DIR}/docker-compose.yml up
release/ci/build_and_publish.sh
Executable
+27
View File
@@ -0,0 +1,27 @@
# UNUSED YET BUT I WANT TO KEEP IT BECAUSE I INTEND TO USE IT SOON
# #!/usr/bin/env bash
# DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
# cd $DIR
# DEV_BRANCH=$1
# GIT_ORIGIN=$2
# if [ -z "$DEV_BRANCH" ]; then
# DEV_BRANCH="dev-c3-test"
# fi
# if [ -z "$GIT_ORIGIN" ]; then
# GIT_ORIGIN="git@github.com:devtekve/openpilot-og.git" #This is a default that should be changed
# echo "No GIT_ORIGIN provided, we will use the default [${GIT_ORIGIN}]"
# fi
# VERSION=$(date '+%Y.%m.%d')
# SOURCE_DIR="$(git rev-parse --show-toplevel)"
# OUTPUT_DIR=${SOURCE_DIR}/output
# echo "Calling to build [${DIR}/build.sh ${SOURCE_DIR} ${OUTPUT_DIR} ${VERSION}]"
# $DIR/build.sh "${SOURCE_DIR}" "${OUTPUT_DIR}" "${VERSION}"
# echo "Calling to publish [${DIR}/publish.sh ${SOURCE_DIR} ${OUTPUT_DIR} ${DEV_BRANCH} ${VERSION} ${GIT_ORIGIN}]"
# $DIR/publish.sh "${SOURCE_DIR}" "${OUTPUT_DIR}" "${DEV_BRANCH}" "${VERSION}" "${GIT_ORIGIN}"
release/ci/install_gitlab_runner.sh
Executable
+80
View File
@@ -0,0 +1,80 @@
#!/bin/bash
# We need RW for the install process
sudo mount -o remount rw /
# Ensure filesystem is remounted as read-only on script exit
trap "sudo mount -o remount ro /" EXIT
# Define directories
BASE_DIR="/data/gitlab"
BIN_DIR="$BASE_DIR/bin"
CONFIG_DIR="$BASE_DIR"
BUILDS_DIR="$BASE_DIR/builds"
OPENPILOT_DIR="$BASE_DIR/openpilot"
LOGS_DIR="$BASE_DIR/logs"
CACHE_DIR="$BASE_DIR/cache"
GITLAB_RUNNER_USERNAME="gitlab-runner"
GROUPS_NEEDED="comma,gpu,gpio"
# Create necessary directories
sudo mkdir -p "$BIN_DIR" "$BUILDS_DIR" "$LOGS_DIR" "$CACHE_DIR" "$OPENPILOT_DIR"
# Download the GitLab Runner binary
sudo curl -L --output "$BIN_DIR/gitlab-runner" "https://gitlab-runner-downloads.s3.amazonaws.com/latest/binaries/gitlab-runner-linux-arm64"
# Give it permission to execute
sudo chmod +x "$BIN_DIR/gitlab-runner"
# Create a GitLab Runner user
sudo useradd --comment 'GitLab Runner' --create-home --home-dir ${BASE_DIR} ${GITLAB_RUNNER_USERNAME} --shell /bin/bash -G ${GROUPS_NEEDED} || sudo usermod -aG ${GROUPS_NEEDED} gitlab-runner
# Clean bash_logout as it break gitlab pipelines
sudo truncate -s 0 ${BASE_DIR}/.bash_logout
# Create a configuration file
cat <<EOL | sudo tee "$CONFIG_DIR/config.toml"
[[runners]]
name = "tici"
url = "https://gitlab.com/"
token = "$1"
executor = "shell"
builds_dir = "$BUILDS_DIR"
[runners.custom_build_dir]
[runners.docker]
volumes = ["$CACHE_DIR:/cache"]
[runners.cache]
MaxUploadedArchiveSize = 0
[runners.custom]
config_exec = "$LOGS_DIR"
EOL
# Set permissions
sudo chown -R ${GITLAB_RUNNER_USERNAME}:comma "$BASE_DIR"
sudo chmod g+rwx "$BASE_DIR"
sudo chmod g+s "$BASE_DIR"
# Create a systemd service file for gitlab-runner
cat <<EOL | sudo tee /etc/systemd/system/gitlab-runner.service
[Unit]
Description=GitLab Runner
After=syslog.target network.target
ConditionFileIsExecutable=$BIN_DIR/gitlab-runner
[Service]
StartLimitInterval=5
StartLimitBurst=10
ExecStart=/usr/bin/unshare -m -- sh -c 'mount --bind $OPENPILOT_DIR /data/openpilot && exec $BIN_DIR/gitlab-runner "run" "--working-directory" "$BUILDS_DIR" "--config" "$CONFIG_DIR/config.toml" "--service" "gitlab-runner" "--syslog" "--user" "${GITLAB_RUNNER_USERNAME}"'
Restart=always
RestartSec=120
[Install]
WantedBy=multi-user.target
EOL
# Reload systemd and start gitlab-runner
sudo systemctl daemon-reload
sudo systemctl disable gitlab-runner # Intentionally, making sure the service is NOT enabled on boot.
sudo systemctl start gitlab-runner
release/ci/publish.sh
Executable
+72
View File
@@ -0,0 +1,72 @@
#!/usr/bin/env bash
set -e
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
# Take parameters as arguments
SOURCE_DIR=$1
OUTPUT_DIR=$2
DEV_BRANCH=$3
VERSION=$4
GIT_ORIGIN=$5
# Check parameters
if [ -z "$SOURCE_DIR" ] || [ -z "$OUTPUT_DIR" ]; then
echo "Error: No source or output directory provided."
exit 1
fi
if [ -z "$DEV_BRANCH" ] || [ -z "$VERSION" ]; then
echo "Error: No dev branch or version provided."
exit 1
fi
if [ -z "$GIT_ORIGIN" ]; then
echo "Error: No GIT_ORIGIN provided"
exit 1
fi
# "Tagging"
echo "#define COMMA_VERSION \"$VERSION-dev\"" > ${OUTPUT_DIR}/common/version.h
## set git identity
#source $DIR/identity.sh
#export GIT_SSH_COMMAND="ssh -i /data/gitkey"
echo "[-] Setting up repo T=$SECONDS"
cd $OUTPUT_DIR
git init
# set git username/password
#source /data/identity.sh
git rm -rf $OUTPUT_DIR/.git || true # Doing cleanup, but it might fail if the .git doesn't exist or not allowed to delete
git remote remove origin || true # ensure cleanup
git remote add origin $GIT_ORIGIN
#git push origin -d $DEV_BRANCH || true # Ensuring we delete the remote branch if it exists as we are wiping it out
git fetch origin $DEV_BRANCH || (git checkout -b $DEV_BRANCH && git commit --allow-empty -m "sunnypilot v$VERSION release" && git push -u origin $DEV_BRANCH)
echo "[-] committing version $VERSION T=$SECONDS"
git add -f .
git commit -a -m "sunnypilot v$VERSION release"
git branch --set-upstream-to=origin/$DEV_BRANCH
# include source commit hash and build date in commit
GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse HEAD)
DATETIME=$(date '+%Y-%m-%dT%H:%M:%S')
SP_VERSION=$(cat $SOURCE_DIR/common/version.h | awk -F\" '{print $2}')
# Add built files to git
git add -f .
git commit --amend -m "sunnypilot v$VERSION
version: sunnypilot v$SP_VERSION release
date: $DATETIME
master commit: $GIT_HASH
"
git branch -m $DEV_BRANCH
# Push!
echo "[-] pushing T=$SECONDS"
git push -f origin $DEV_BRANCH
release/files_common
+7 -7
View File
@@ -140,10 +140,6 @@ selfdrive/car/tesla/*.py
selfdrive/car/toyota/*.py
selfdrive/car/volkswagen/*.py
system/clocksd/.gitignore
system/clocksd/SConscript
system/clocksd/clocksd.cc
selfdrive/debug/can_printer.py
selfdrive/debug/check_freq.py
selfdrive/debug/dump.py
@@ -257,7 +253,6 @@ system/ubloxd/*.cc
selfdrive/locationd/__init__.py
selfdrive/locationd/SConscript
selfdrive/locationd/.gitignore
selfdrive/locationd/laikad.py
selfdrive/locationd/locationd.h
selfdrive/locationd/locationd.cc
selfdrive/locationd/paramsd.py
@@ -348,8 +343,12 @@ selfdrive/ui/qt/network/*.h
selfdrive/ui/qt/offroad/*.cc
selfdrive/ui/qt/offroad/*.h
selfdrive/ui/qt/offroad/*.qml
selfdrive/ui/qt/offroad/sunnypilot/*.cc
selfdrive/ui/qt/offroad/sunnypilot/*.h
selfdrive/ui/qt/widgets/*.cc
selfdrive/ui/qt/widgets/*.h
selfdrive/ui/qt/widgets/sunnypilot/*.cc
selfdrive/ui/qt/widgets/sunnypilot/*.h
selfdrive/ui/qt/maps/*.cc
selfdrive/ui/qt/maps/*.h
selfdrive/ui/qt/screenrecorder/*
@@ -390,6 +389,9 @@ selfdrive/modeld/.gitignore
selfdrive/modeld/__init__.py
selfdrive/modeld/SConscript
selfdrive/modeld/modeld.py
selfdrive/modeld/parse_model_outputs.py
selfdrive/modeld/fill_model_msg.py
selfdrive/modeld/get_model_metadata.py
selfdrive/modeld/navmodeld.py
selfdrive/modeld/dmonitoringmodeld.py
selfdrive/modeld/constants.py
@@ -402,8 +404,6 @@ selfdrive/modeld/models/*.pyx
selfdrive/modeld/models/commonmodel.cc
selfdrive/modeld/models/commonmodel.h
selfdrive/modeld/models/driving.cc
selfdrive/modeld/models/driving.h
selfdrive/modeld/models/supercombo.onnx
selfdrive/modeld/models/dmonitoring_model_q.dlc
selfdrive/assets/offroad/icon_display.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.7 KiB

selfdrive/assets/offroad/icon_software.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.7 KiB

selfdrive/assets/offroad/icon_toggle.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.0 KiB

selfdrive/assets/offroad/icon_trips.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 5.2 KiB

selfdrive/assets/offroad/icon_vehicle.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 11 KiB

selfdrive/assets/offroad/icon_visuals.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

selfdrive/car/chrysler/values.py
+1
View File
@@ -284,6 +284,7 @@ FW_VERSIONS = {
],
(Ecu.eps, 0x75A, None): [
b'21590101AA',
b'21590101AB',
b'68273275AF',
b'68273275AG',
b'68273275AH',
selfdrive/car/gm/interface.py
+9 -4
View File
@@ -198,17 +198,22 @@ class CarInterface(CarInterfaceBase):
ret.centerToFront = ret.wheelbase * 0.5
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.ESCALADE_ESV:
elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019):
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 2739.
ret.wheelbase = 3.302
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
ret.tireStiffnessFactor = 1.0
if candidate == CAR.ESCALADE_ESV:
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
else:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BOLT_EUV:
ret.mass = 1669.
ret.wheelbase = 2.63779
selfdrive/car/gm/values.py
+6
View File
@@ -71,6 +71,7 @@ class CAR(StrEnum):
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
ESCALADE = "CADILLAC ESCALADE 2017"
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019"
BOLT_EUV = "CHEVROLET BOLT EUV 2022"
SILVERADO = "CHEVROLET SILVERADO 1500 2020"
EQUINOX = "CHEVROLET EQUINOX 2019"
@@ -106,6 +107,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"),
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"),
CAR.BOLT_EUV: [
GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"),
GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"),
@@ -198,6 +200,10 @@ FINGERPRINTS = {
{
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
}],
CAR.ESCALADE_ESV_2019: [
{
715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
}],
CAR.BOLT_EUV: [
{
189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
selfdrive/car/hyundai/interface.py
+7 -6
View File
@@ -4,8 +4,8 @@ from openpilot.common.conversions import Conversions as CV
from openpilot.common.params import Params
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, NON_SCC_CAR, \
Buttons
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, NON_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
UNSUPPORTED_LONGITUDINAL_CAR, Buttons
from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
@@ -95,7 +95,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.90
ret.steerRatio = 15.6 * 1.15
ret.tireStiffnessFactor = 0.63
elif candidate == CAR.ELANTRA:
elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30):
ret.mass = 1275.
ret.wheelbase = 2.7
ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
@@ -116,7 +116,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN):
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN, CAR.KONA_NON_SCC):
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.)
ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6)
ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec
@@ -270,7 +270,8 @@ class CarInterface(CarInterfaceBase):
if candidate in CANFD_CAR:
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate in (HYBRID_CAR | EV_CAR) and candidate not in (CANFD_RADAR_SCC_CAR | NON_SCC_CAR)
ret.experimentalLongitudinalAvailable = (candidate in (HYBRID_CAR | EV_CAR) and candidate not in
(CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR))
else:
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
@@ -339,7 +340,7 @@ class CarInterface(CarInterfaceBase):
elif candidate in EV_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS
if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_NON_SCC):
ret.flags |= HyundaiFlags.ALT_LIMITS.value
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS
selfdrive/car/hyundai/tests/test_hyundai.py
+2 -1
View File
@@ -23,6 +23,7 @@ NO_DATES_PLATFORMS = {
CAR.TUCSON_HYBRID_4TH_GEN,
# CAN
CAR.ELANTRA,
CAR.ELANTRA_GT_I30,
CAR.KIA_CEED,
CAR.KIA_FORTE,
CAR.KIA_OPTIMA_G4,
@@ -99,7 +100,7 @@ class TestHyundaiFingerprint(unittest.TestCase):
def test_platform_code_ecus_available(self):
# TODO: add queries for these non-CAN FD cars to get EPS
no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H,
CAR.KIA_OPTIMA_H_G4_FL, CAR.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80}
CAR.KIA_OPTIMA_H_G4_FL, CAR.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.ELANTRA}
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms
for car_model, ecus in FW_VERSIONS.items():
selfdrive/car/hyundai/values.py
+66 -10
View File
@@ -37,7 +37,7 @@ class CarControllerParams:
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ,
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ,
CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV,
CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO):
self.STEER_MAX = 255
@@ -79,6 +79,7 @@ class CAR(StrEnum):
AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN"
AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN"
ELANTRA = "HYUNDAI ELANTRA 2017"
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
ELANTRA_2022_NON_SCC = "HYUNDAI ELANTRA SE 2022 NON-SCC"
ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021"
@@ -90,6 +91,7 @@ class CAR(StrEnum):
IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_NON_SCC = "HYUNDAI KONA 2019 NON-SCC"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022"
KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN"
@@ -172,7 +174,11 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.AZERA_HEV_6TH_GEN: HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.ELANTRA: [
HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
# TODO: 2017-18 could be Hyundai G
HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])),
],
CAR.ELANTRA_GT_I30: [
HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
@@ -269,7 +275,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KIA_SORENTO_HEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Hybrid 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SORENTO_HEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0",
@@ -303,6 +309,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
# Non-SCC Cars
CAR.ELANTRA_2022_NON_SCC: HyundaiCarInfo("Hyundai Elantra Non-SCC 2022", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KONA_NON_SCC: HyundaiCarInfo("Hyundai Kona Non-SCC 2019", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_b])),
CAR.KIA_FORTE_2019_NON_SCC: HyundaiCarInfo("Kia Forte Non-SCC 2019", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_g])),
CAR.KIA_FORTE_2021_NON_SCC: HyundaiCarInfo("Kia Forte Non-SCC 2021", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_g])),
CAR.KIA_SELTOS_2023_NON_SCC: HyundaiCarInfo("Kia Seltos Non-SCC 2023-24", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_l])),
@@ -317,9 +324,6 @@ class Buttons:
CANCEL = 4 # on newer models, this is a pause/resume button
FINGERPRINTS = {
CAR.ELANTRA: [{
66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.HYUNDAI_GENESIS: [{
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
},
@@ -1391,6 +1395,23 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ],
},
CAR.KONA_NON_SCC: {
(Ecu.abs, 0x7d1, None): [
b'\xf1\x816V5RAJ00040.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8161699051\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9200 g30',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x006T6J0_C2\x00\x006T6K1051\x00\x00TOS4N20NS2\x00\x00\x00\x00',
],
},
CAR.KIA_CEED: {
(Ecu.fwdRadar, 0x7D0, None): [b'\xf1\000CD__ SCC F-CUP 1.00 1.02 99110-J7000 ', ],
(Ecu.eps, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ],
@@ -1791,6 +1812,33 @@ FW_VERSIONS = {
],
},
CAR.ELANTRA: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00ADP LKAS AT USA LHD 1.00 1.03 99211-F2000 X31',
b'\xf1\x00AD LKAS AT USA LHD 1.01 1.01 95895-F2000 251',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD(\xfcA\x9d',
b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD\x00\x00\x00\x00',
b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\x00\x00\x00\x00',
b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\xc5\x92\x9e\x8a',
b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20SS2.~\x90\x87',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8161698051\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x8161657051\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x816165D051\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x816165E051\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7d1, None): [
b'\xf1\x00AD ESC \x11 11 \x18\x05\x06 58910-F2840',
b'\xf1\x00AD ESC \x11 12 \x15\t\t 58920-F2810',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00AD__ SCC H-CUP 1.00 1.00 99110-F2100 ',
b'\xf1\x00AD__ SCC H-CUP 1.00 1.01 96400-F2100 ',
],
},
CAR.ELANTRA_GT_I30: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54',
b'\xf1\x00PD LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51',
@@ -2145,9 +2193,11 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00MQ4HMFC AT KOR LHD 1.00 1.12 99210-P2000 230331',
b'\xf1\x00MQ4HMFC AT USA LHD 1.00 1.11 99210-P2000 211217',
b'\xf1\x00MQ4HMFC AT KOR LHD 1.00 1.04 99210-P2000 200330',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00MQhe SCC FHCUP 1.00 1.07 99110-P4000 ',
b'\xf1\x00MQhe SCC FHCUP 1.00 1.04 99110-P4000 ',
],
},
CAR.KIA_K8_HEV_1ST_GEN: {
@@ -2169,7 +2219,7 @@ CHECKSUM = {
CAN_GEARS = {
# which message has the gear. hybrid and EV use ELECT_GEAR
"use_cluster_gears": {CAR.ELANTRA, CAR.KONA},
"use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
}
@@ -2180,7 +2230,11 @@ CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCS
# The radar does SCC on these cars when HDA I, rather than the camera
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80,
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.IONIQ_6}
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN}
# These CAN FD cars do not accept communication control to disable the ADAS ECU,
# responds with 0x7F2822 - 'conditions not correct'
CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN}
# The camera does SCC on these cars, rather than the radar
CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
@@ -2198,14 +2252,14 @@ EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR
# these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4,
CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022,
CAR.KIA_OPTIMA_H}
CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc.
UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL,
CAR.KIA_OPTIMA_H_G4_FL}
NON_SCC_CAR = {CAR.KIA_FORTE_2021_NON_SCC, CAR.ELANTRA_2022_NON_SCC, CAR.KIA_FORTE_2019_NON_SCC, CAR.GENESIS_G70_2021_NON_SCC,
CAR.KIA_SELTOS_2023_NON_SCC}
CAR.KIA_SELTOS_2023_NON_SCC, CAR.KONA_NON_SCC}
NON_SCC_NO_FCA_CAR = {CAR.KIA_FORTE_2019_NON_SCC, }
NON_SCC_RADAR_FCA_CAR = {CAR.GENESIS_G70_2021_NON_SCC, }
@@ -2215,6 +2269,7 @@ DBC = {
CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_2022_NON_SCC: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
@@ -2248,6 +2303,7 @@ DBC = {
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_NON_SCC: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
selfdrive/car/sunnypilot_carname.json
+35 -29
View File
@@ -12,6 +12,7 @@
"Buick LaCrosse 2017-19": "BUICK LACROSSE 2017",
"Cadillac Escalade 2017": "CADILLAC ESCALADE 2017",
"Cadillac Escalade ESV 2016": "CADILLAC ESCALADE ESV 2016",
"Cadillac Escalade ESV 2019": "CADILLAC ESCALADE ESV 2019",
"Chevrolet Bolt EUV 2022-23": "CHEVROLET BOLT EUV 2022",
"Chevrolet Bolt EV 2022-23": "CHEVROLET BOLT EUV 2022",
"Chevrolet Silverado 1500 2020-21": "CHEVROLET SILVERADO 1500 2020",
@@ -66,13 +67,16 @@
"Honda Pilot 2016-22": "HONDA PILOT 2017",
"Honda Ridgeline 2017-23": "HONDA RIDGELINE 2017",
"Hyundai Azera 2022": "HYUNDAI AZERA 6TH GEN",
"Hyundai Elantra 2017-19": "HYUNDAI ELANTRA 2017",
"Hyundai Azera Hybrid 2020": "HYUNDAI AZERA HYBRID 6TH GEN",
"Hyundai Custin 2023": "HYUNDAI CUSTIN 1ST GEN",
"Hyundai Elantra 2017-18": "HYUNDAI ELANTRA 2017",
"Hyundai Elantra 2019": "HYUNDAI ELANTRA 2017",
"Hyundai Elantra 2021-23": "HYUNDAI ELANTRA 2021",
"Hyundai Elantra GT 2017-19": "HYUNDAI ELANTRA 2017",
"Hyundai Elantra GT 2017-19": "HYUNDAI I30 N LINE 2019 & GT 2018 DCT",
"Hyundai Elantra Hybrid 2021-23": "HYUNDAI ELANTRA HYBRID 2021",
"Hyundai Elantra Non-SCC 2022": "HYUNDAI ELANTRA SE 2022 NON-SCC",
"Hyundai Genesis 2015-16": "HYUNDAI GENESIS 2015-2016",
"Hyundai i30 2017-19": "HYUNDAI ELANTRA 2017",
"Hyundai i30 2017-19": "HYUNDAI I30 N LINE 2019 & GT 2018 DCT",
"Hyundai Ioniq 5 (Southeast Asia only) 2022-23": "HYUNDAI IONIQ 5 2022",
"Hyundai Ioniq 5 (with HDA II) 2022-23": "HYUNDAI IONIQ 5 2022",
"Hyundai Ioniq 5 (without HDA II) 2022-23": "HYUNDAI IONIQ 5 2022",
@@ -88,12 +92,13 @@
"Hyundai Kona Electric 2022-23": "HYUNDAI KONA ELECTRIC 2022",
"Hyundai Kona Electric (with HDA II, Korea only) 2023": "HYUNDAI KONA ELECTRIC 2ND GEN",
"Hyundai Kona Hybrid 2020": "HYUNDAI KONA HYBRID 2020",
"Hyundai Kona Non-SCC 2019": "HYUNDAI KONA 2019 NON-SCC",
"Hyundai Palisade 2020-22": "HYUNDAI PALISADE 2020",
"Hyundai Santa Cruz 2022-23": "HYUNDAI SANTA CRUZ 1ST GEN",
"Hyundai Santa Fe 2019-20": "HYUNDAI SANTA FE 2019",
"Hyundai Santa Fe 2021-23": "HYUNDAI SANTA FE 2022",
"Hyundai Santa Fe Hybrid 2022-23": "HYUNDAI SANTA FE HYBRID 2022",
"Hyundai Santa Fe Plug-in Hybrid 2022": "HYUNDAI SANTA FE PlUG-IN HYBRID 2022",
"Hyundai Santa Fe Plug-in Hybrid 2022-23": "HYUNDAI SANTA FE PlUG-IN HYBRID 2022",
"Hyundai Sonata 2018-19": "HYUNDAI SONATA 2019",
"Hyundai Sonata 2020-23": "HYUNDAI SONATA 2020",
"Hyundai Sonata Hybrid 2020-23": "HYUNDAI SONATA HYBRID 2021",
@@ -115,8 +120,8 @@
"Kia Forte 2023": "KIA FORTE E 2018 & GT 2021",
"Kia Forte Non-SCC 2019": "KIA FORTE 2019 NON-SCC",
"Kia Forte Non-SCC 2021": "KIA FORTE LXS 2021 NON-SCC",
"Kia K5 2021-22": "KIA K5 2021",
"Kia K5 Hybrid 2020": "KIA K5 HYBRID 2020",
"Kia K5 2021-24": "KIA K5 2021",
"Kia K5 Hybrid 2020-22": "KIA K5 HYBRID 2020",
"Kia K8 Hybrid (with HDA II) 2023": "KIA K8 HYBRID 1ST GEN",
"Kia Niro EV 2019": "KIA NIRO EV 2020",
"Kia Niro EV 2020": "KIA NIRO EV 2020",
@@ -129,12 +134,13 @@
"Kia Niro Plug-in Hybrid 2020": "KIA NIRO HYBRID 2019",
"Kia Optima 2017": "KIA OPTIMA 4TH GEN",
"Kia Optima 2019-20": "KIA OPTIMA 4TH GEN FACELIFT",
"Kia Optima Hybrid 2019": "KIA OPTIMA HYBRID 4TH GEN FACELIFT",
"Kia Seltos 2021": "KIA SELTOS 2021",
"Kia Seltos Non-SCC 2023-24": "KIA SELTOS 2023 NON-SCC",
"Kia Sorento 2018": "KIA SORENTO GT LINE 2018",
"Kia Sorento 2019": "KIA SORENTO GT LINE 2018",
"Kia Sorento 2021-23": "KIA SORENTO 4TH GEN",
"Kia Sorento Hybrid 2023": "KIA SORENTO HYBRID 4TH GEN",
"Kia Sorento Hybrid 2021-23": "KIA SORENTO HYBRID 4TH GEN",
"Kia Sorento Plug-in Hybrid 2022-23": "KIA SORENTO PLUG-IN HYBRID 4TH GEN",
"Kia Sportage 2023": "KIA SPORTAGE 5TH GEN",
"Kia Sportage Hybrid 2023": "KIA SPORTAGE HYBRID 5TH GEN",
@@ -143,27 +149,27 @@
"Kia Telluride 2020-22": "HYUNDAI PALISADE 2020",
"Lexus CT Hybrid 2017-18": "LEXUS CT HYBRID 2018",
"Lexus ES 2017-18": "LEXUS ES 2018",
"Lexus ES 2019-22": "LEXUS ES 2019",
"Lexus ES 2019-24": "LEXUS ES 2019",
"Lexus ES Hybrid 2017-18": "LEXUS ES HYBRID 2018",
"Lexus ES Hybrid 2019-23": "LEXUS ES HYBRID 2019",
"Lexus ES Hybrid 2019-23": "LEXUS ES 2019",
"Lexus IS 2017-19": "LEXUS IS 2018",
"Lexus IS 2023": "LEXUS IS 2023",
"Lexus IS 2022-23": "LEXUS IS 2023",
"Lexus NX 2018-19": "LEXUS NX 2018",
"Lexus NX 2020-21": "LEXUS NX 2020",
"Lexus NX Hybrid 2018-19": "LEXUS NX HYBRID 2018",
"Lexus NX Hybrid 2020-21": "LEXUS NX HYBRID 2020",
"Lexus NX Hybrid 2018-19": "LEXUS NX 2018",
"Lexus NX Hybrid 2020-21": "LEXUS NX 2020",
"Lexus RC 2018-20": "LEXUS RC 2020",
"Lexus RX 2016": "LEXUS RX 2016",
"Lexus RX 2017-19": "LEXUS RX 2016",
"Lexus RX 2020-22": "LEXUS RX 2020",
"Lexus RX Hybrid 2016": "LEXUS RX HYBRID 2017",
"Lexus RX Hybrid 2017-19": "LEXUS RX HYBRID 2017",
"Lexus RX Hybrid 2020-22": "LEXUS RX HYBRID 2020",
"Lexus UX Hybrid 2019-23": "TOYOTA COROLLA HYBRID TSS2 2019",
"Lexus RX Hybrid 2020-22": "LEXUS RX 2020",
"Lexus UX Hybrid 2019-23": "TOYOTA COROLLA TSS2 2019",
"Lincoln Aviator 2020-21": "FORD EXPLORER 6TH GEN",
"MAN eTGE 2020-23": "VOLKSWAGEN CRAFTER 2ND GEN",
"MAN TGE 2017-23": "VOLKSWAGEN CRAFTER 2ND GEN",
"Mazda CX-5 2022-23": "MAZDA CX-5 2022",
"Mazda CX-5 2022-24": "MAZDA CX-5 2022",
"Mazda CX-9 2021-23": "MAZDA CX-9 2021",
"Nissan Altima 2019-20": "NISSAN ALTIMA 2020",
"Nissan Leaf 2018-23": "NISSAN LEAF 2018",
@@ -197,32 +203,32 @@
"Škoda Scala 2020-23": "SKODA SCALA 1ST GEN",
"Škoda Superb 2015-22": "SKODA SUPERB 3RD GEN",
"Toyota Alphard 2019-20": "TOYOTA ALPHARD 2020",
"Toyota Alphard Hybrid 2021": "TOYOTA ALPHARD HYBRID 2021",
"Toyota Alphard Hybrid 2021": "TOYOTA ALPHARD 2020",
"Toyota Avalon 2016": "TOYOTA AVALON 2016",
"Toyota Avalon 2017-18": "TOYOTA AVALON 2016",
"Toyota Avalon 2019-21": "TOYOTA AVALON 2019",
"Toyota Avalon 2022": "TOYOTA AVALON 2022",
"Toyota Avalon Hybrid 2019-21": "TOYOTA AVALON HYBRID 2019",
"Toyota Avalon Hybrid 2022": "TOYOTA AVALON HYBRID 2022",
"Toyota Avalon Hybrid 2019-21": "TOYOTA AVALON 2019",
"Toyota Avalon Hybrid 2022": "TOYOTA AVALON 2022",
"Toyota C-HR 2017-20": "TOYOTA C-HR 2018",
"Toyota C-HR 2021": "TOYOTA C-HR 2021",
"Toyota C-HR Hybrid 2017-20": "TOYOTA C-HR HYBRID 2018",
"Toyota C-HR Hybrid 2021-22": "TOYOTA C-HR HYBRID 2022",
"Toyota C-HR Hybrid 2017-20": "TOYOTA C-HR 2018",
"Toyota C-HR Hybrid 2021-22": "TOYOTA C-HR 2021",
"Toyota Camry 2018-20": "TOYOTA CAMRY 2018",
"Toyota Camry 2021-23": "TOYOTA CAMRY 2021",
"Toyota Camry Hybrid 2018-20": "TOYOTA CAMRY HYBRID 2018",
"Toyota Camry Hybrid 2021-23": "TOYOTA CAMRY HYBRID 2021",
"Toyota Camry Hybrid 2018-20": "TOYOTA CAMRY 2018",
"Toyota Camry Hybrid 2021-24": "TOYOTA CAMRY 2021",
"Toyota Corolla 2017-19": "TOYOTA COROLLA 2017",
"Toyota Corolla 2020-22": "TOYOTA COROLLA TSS2 2019",
"Toyota Corolla Cross (Non-US only) 2020-23": "TOYOTA COROLLA TSS2 2019",
"Toyota Corolla Cross Hybrid (Non-US only) 2020-22": "TOYOTA COROLLA HYBRID TSS2 2019",
"Toyota Corolla Cross Hybrid (Non-US only) 2020-22": "TOYOTA COROLLA TSS2 2019",
"Toyota Corolla Hatchback 2019-22": "TOYOTA COROLLA TSS2 2019",
"Toyota Corolla Hybrid 2020-22": "TOYOTA COROLLA HYBRID TSS2 2019",
"Toyota Corolla Hybrid (Non-US only) 2020-23": "TOYOTA COROLLA HYBRID TSS2 2019",
"Toyota Corolla Hybrid 2020-22": "TOYOTA COROLLA TSS2 2019",
"Toyota Corolla Hybrid (Non-US only) 2020-23": "TOYOTA COROLLA TSS2 2019",
"Toyota Highlander 2017-19": "TOYOTA HIGHLANDER 2017",
"Toyota Highlander 2020-23": "TOYOTA HIGHLANDER 2020",
"Toyota Highlander Hybrid 2017-19": "TOYOTA HIGHLANDER HYBRID 2018",
"Toyota Highlander Hybrid 2020-23": "TOYOTA HIGHLANDER HYBRID 2020",
"Toyota Highlander Hybrid 2020-23": "TOYOTA HIGHLANDER 2020",
"Toyota Mirai 2021": "TOYOTA MIRAI 2021",
"Toyota Prius 2016": "TOYOTA PRIUS 2017",
"Toyota Prius 2017-20": "TOYOTA PRIUS 2017",
@@ -236,15 +242,15 @@
"Toyota RAV4 2022": "TOYOTA RAV4 2022",
"Toyota RAV4 Hybrid 2016": "TOYOTA RAV4 HYBRID 2017",
"Toyota RAV4 Hybrid 2017-18": "TOYOTA RAV4 HYBRID 2017",
"Toyota RAV4 Hybrid 2019-21": "TOYOTA RAV4 HYBRID 2019",
"Toyota RAV4 Hybrid 2022": "TOYOTA RAV4 HYBRID 2022",
"Toyota RAV4 Hybrid 2019-21": "TOYOTA RAV4 2019",
"Toyota RAV4 Hybrid 2022": "TOYOTA RAV4 2022",
"Toyota Sienna 2018-20": "TOYOTA SIENNA 2018",
"Volkswagen Arteon 2018-23": "VOLKSWAGEN ARTEON 1ST GEN",
"Volkswagen Arteon eHybrid 2020-23": "VOLKSWAGEN ARTEON 1ST GEN",
"Volkswagen Arteon R 2020-23": "VOLKSWAGEN ARTEON 1ST GEN",
"Volkswagen Atlas 2018-23": "VOLKSWAGEN ATLAS 1ST GEN",
"Volkswagen Atlas Cross Sport 2021-22": "VOLKSWAGEN ATLAS 1ST GEN",
"Volkswagen California 2021": "VOLKSWAGEN TRANSPORTER T6.1",
"Volkswagen California 2021-23": "VOLKSWAGEN TRANSPORTER T6.1",
"Volkswagen Caravelle 2020": "VOLKSWAGEN TRANSPORTER T6.1",
"Volkswagen CC 2018-22": "VOLKSWAGEN ARTEON 1ST GEN",
"Volkswagen Crafter 2017-23": "VOLKSWAGEN CRAFTER 2ND GEN",
selfdrive/car/tests/routes.py
+11 -11
View File
@@ -25,7 +25,6 @@ non_tested_cars = [
HONDA.ODYSSEY_CHN,
VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter
TOYOTA.RAV4_TSS2_2023,
TOYOTA.RAV4H_TSS2_2023,
SUBARU.FORESTER_HYBRID,
]
@@ -61,6 +60,7 @@ routes = [
CarTestRoute("75a6bcb9b8b40373|2023-03-11--22-47-33", GM.BUICK_LACROSSE),
CarTestRoute("ef8f2185104d862e|2023-02-09--18-37-13", GM.ESCALADE),
CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.ESCALADE_ESV),
CarTestRoute("168f8b3be57f66ae|2023-09-12--21-44-42", GM.ESCALADE_ESV_2019),
CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.VOLT),
CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.BOLT_EUV, segment=1),
CarTestRoute("555d4087cf86aa91|2022-12-02--12-15-07", GM.BOLT_EUV, segment=14), # Bolt EV
@@ -159,7 +159,7 @@ routes = [
CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE),
CarTestRoute("192283cdbb7a58c2|2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN),
CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.ELANTRA),
CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.ELANTRA), # 2019 Elantra GT
CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.ELANTRA_GT_I30),
CarTestRoute("82e9cdd3f43bf83e|2021-05-15--02-42-51", HYUNDAI.ELANTRA_2021),
CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.ELANTRA_HEV_2021),
CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.SONATA_HYBRID),
@@ -167,17 +167,17 @@ routes = [
CarTestRoute("6b0d44d22df18134|2023-05-06--10-36-55", HYUNDAI.GENESIS_GV80),
CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.ALPHARD_TSS2),
CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARDH_TSS2),
CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARD_TSS2), # hybrid
CarTestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.AVALON),
CarTestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.AVALON_2019),
CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALONH_2019),
CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALON_2019), # hybrid
CarTestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.AVALON_TSS2),
CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALONH_TSS2),
CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALON_TSS2), # hybrid
CarTestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.CAMRY),
CarTestRoute("2f37c007683e85ba|2023-09-02--14-39-44", TOYOTA.CAMRY), # openpilot longitudinal, with radar CAN filter
CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.CAMRY), # hybrid
CarTestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.CAMRY_TSS2),
CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRYH_TSS2),
CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.CAMRYH),
CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRY_TSS2), # hybrid
CarTestRoute("4e45c89c38e8ec4d|2021-05-02--02-49-28", TOYOTA.COROLLA),
CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2),
CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLA_TSS2), # hybrid
@@ -186,7 +186,7 @@ routes = [
CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2),
CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022),
CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4_TSS2), # hybrid
CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022),
CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4_TSS2_2022), # hybrid
CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES),
CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2),
CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ES_TSS2), # hybrid
@@ -202,7 +202,7 @@ routes = [
CarTestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2),
CarTestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NX_TSS2), # hybrid
CarTestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.HIGHLANDER_TSS2),
CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDERH_TSS2),
CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDER_TSS2), # hybrid
CarTestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.HIGHLANDER),
CarTestRoute("80d16a262e33d57f|2021-05-23--20-01-43", TOYOTA.HIGHLANDERH),
CarTestRoute("eb6acd681135480d|2019-06-20--20-00-00", TOYOTA.SIENNA),
@@ -214,8 +214,8 @@ routes = [
CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHR), # hybrid
CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.CHR_TSS2),
CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.CHR_TSS2), # openpilot longitudinal, with smartDSU
CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHRH_TSS2),
# CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.CHRH_TSS2), # openpilot longitudinal, radar disabled
CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHR_TSS2), # hybrid
# CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled
CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V),
CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1),
selfdrive/car/tests/test_car_interfaces.py
+1 -1
View File
@@ -133,7 +133,7 @@ class TestCarInterfaces(unittest.TestCase):
# Make sure we can combine dicts
ret = get_interface_attr('DBC', combine_brands=True)
self.assertGreaterEqual(len(ret), 170)
self.assertGreaterEqual(len(ret), 160)
# We don't support combining non-dicts
ret = get_interface_attr('CAR', combine_brands=True)
selfdrive/car/tests/test_models_segs.txt
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2e57357c95937475fe92c13709ff2ee067f036643d5d455383bd9c3335525614
size 126281
oid sha256:8674d9a8304d501aee2bfaea2d668d746fd424b5b0872b83ab265d15d3a3d887
size 125301
selfdrive/car/torque_data/override.yaml
+1 -1
View File
@@ -14,7 +14,6 @@ SUBARU ASCENT 2023: [.nan, 3.0, .nan]
# Toyota LTA also has torque
TOYOTA RAV4 2023: [.nan, 3.0, .nan]
TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan]
# Tesla has high torque
TESLA AP1 MODEL S: [.nan, 2.5, .nan]
@@ -37,6 +36,7 @@ RAM 1500 5TH GEN: [2.0, 2.0, 0.05]
RAM HD 5TH GEN: [1.4, 1.4, 0.05]
SUBARU OUTBACK 6TH GEN: [2.0, 2.0, 0.2]
CADILLAC ESCALADE 2017: [1.899999976158142, 1.842270016670227, 0.1120000034570694]
CADILLAC ESCALADE ESV 2019: [1.15, 1.3, 0.2]
CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05]
CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112]
CHEVROLET TRAILBLAZER 2021: [1.33, 1.9, 0.16]
selfdrive/car/torque_data/params.yaml
+5 -8
View File
@@ -62,16 +62,13 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506]
TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615]
TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565]
TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257]
TOYOTA CAMRY 2018: [2.0568162685952505, 1.7576185169559122, 0.108878753]
TOYOTA CAMRY 2021: [2.3548324999999997, 2.368900128946771, 0.118436]
TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
TOYOTA COROLLA TSS2 2019: [1.991132339206426, 1.868866242720403, 0.19570063298031432]
TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796]
TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457]
TOYOTA HIGHLANDER 2020: [1.9617570834136164, 1.8611643317268927, 0.14519673256119725]
TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515]
TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
@@ -80,8 +77,8 @@ TOYOTA RAV4 2019: [2.279239424615458, 2.087101966779332, 0.13682208413446817]
TOYOTA RAV4 2019 8965: [2.3080951748210854, 2.1189367835820603, 0.12942102328134028]
TOYOTA RAV4 2019 x02: [2.762293266024922, 2.243615865975329, 0.11113568178327986]
TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
TOYOTA RAV4 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
TOYOTA RAV4 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456]
VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
selfdrive/car/torque_data/substitute.yaml
+4 -7
View File
@@ -4,18 +4,13 @@ MAZDA CX-5: MAZDA CX-9 2021
MAZDA CX-5 2022: MAZDA CX-9 2021
MAZDA CX-9: MAZDA CX-9 2021
TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018
TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018
TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017
TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022
TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021
LEXUS IS 2018: LEXUS NX 2018
LEXUS CT HYBRID 2018 : LEXUS NX 2018
LEXUS ES 2018: TOYOTA CAMRY HYBRID 2018
LEXUS ES HYBRID 2018: TOYOTA CAMRY HYBRID 2018
LEXUS ES 2018: TOYOTA CAMRY 2018
LEXUS ES HYBRID 2018: TOYOTA CAMRY 2018
LEXUS RC 2020: LEXUS NX 2020
TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019
TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022
KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020
KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020
@@ -30,6 +25,7 @@ KIA SELTOS 2023 NON-SCC: HYUNDAI SONATA 2020
KIA NIRO HYBRID 2019: KIA NIRO EV 2020
KIA NIRO HYBRID 2021: KIA NIRO EV 2020
HYUNDAI VELOSTER 2019: HYUNDAI SONATA 2019
HYUNDAI KONA 2019 NON-SCC: HYUNDAI KONA ELECTRIC 2019
HYUNDAI KONA 2020: HYUNDAI KONA ELECTRIC 2019
HYUNDAI KONA HYBRID 2020: HYUNDAI KONA ELECTRIC 2019
HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019
@@ -37,6 +33,7 @@ HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019
HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019
HYUNDAI I30 N LINE 2019 & GT 2018 DCT: HYUNDAI SONATA 2019
HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020
HYUNDAI ELANTRA SE 2022 NON-SCC: HYUNDAI ELANTRA 2021
HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019
selfdrive/car/toyota/carcontroller.py
+3
View File
@@ -119,6 +119,9 @@ class CarController:
if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:
pcm_cancel_cmd = 1
if self.CP.pcmCruise and not CS.pcm_acc_status and CS.out.cruiseState.enabled and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor) and \
not self._sng_hack:
selfdrive/car/toyota/carstate.py
+6 -6
View File
@@ -116,6 +116,7 @@ class CarState(CarStateBase):
self.prev_lkas_enabled = self.lkas_enabled
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# On some cars, the angle measurement is non-zero while initializing
@@ -123,8 +124,8 @@ class CarState(CarStateBase):
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
# Offset seems to be invalid for large steering angles
if abs(ret.steeringAngleDeg) < 90 and cp.can_valid:
# Offset seems to be invalid for large steering angles and high angle rates
if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid:
self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
if self.angle_offset.initialized:
@@ -146,8 +147,6 @@ class CarState(CarStateBase):
# Apply offset
ret.steeringAngleDeg = zorro_steer - self.zss_angle_offset
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
@@ -195,7 +194,8 @@ class CarState(CarStateBase):
if self.CP.flags & ToyotaFlags.SMART_DSU:
self.gap_dist_button = cp.vl["SDSU"]["FD_BUTTON"]
self.follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
fd_src = "PCM_CRUISE_ALT" if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR else "PCM_CRUISE_2"
self.follow_distance = cp.vl[fd_src]["PCM_FOLLOW_DISTANCE"]
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
# these cars are identified by an ACC_TYPE value of 2.
@@ -357,7 +357,7 @@ class CarState(CarStateBase):
messages.append(("SDSU", 33))
if CP.spFlags & ToyotaFlagsSP.SP_ZSS:
messages.apend(("SECONDARY_STEER_ANGLE", 0))
messages.append(("SECONDARY_STEER_ANGLE", 0))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
selfdrive/car/toyota/interface.py
+9 -7
View File
@@ -96,28 +96,28 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFactor = 0.5533
ret.mass = 4481. * CV.LB_TO_KG # mean between min and max
elif candidate in (CAR.CHR, CAR.CHR_TSS2, CAR.CHRH_TSS2):
elif candidate in (CAR.CHR, CAR.CHR_TSS2):
stop_and_go = True
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
ret.tireStiffnessFactor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG
elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2):
stop_and_go = True
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
ret.tireStiffnessFactor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2):
stop_and_go = True
ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in
ret.steerRatio = 16.0
ret.tireStiffnessFactor = 0.8
ret.mass = 4516. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2):
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALON_TSS2):
# starting from 2019, all Avalon variants have stop and go
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
stop_and_go = candidate != CAR.AVALON
@@ -126,8 +126,7 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFactor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2022,
CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023):
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
ret.tireStiffnessFactor = 0.7933
@@ -202,7 +201,7 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFactor = 0.8
ret.mass = 4300. * CV.LB_TO_KG
elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2):
elif candidate == CAR.ALPHARD_TSS2:
ret.wheelbase = 3.00
ret.steerRatio = 14.2
ret.tireStiffnessFactor = 0.444
@@ -257,6 +256,9 @@ class CarInterface(CarInterfaceBase):
if not ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
if candidate in UNSUPPORTED_DSU_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_UNSUPPORTED_DSU_CAR
# 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.
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
selfdrive/car/toyota/toyotacan.py
+1 -1
View File
@@ -30,7 +30,7 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, reverse_acc, gac_send):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
"ACCEL_CMD": 0 if pcm_cancel else accel,
"ACC_TYPE": acc_type,
"DISTANCE": gac_send,
"MINI_CAR": lead,
selfdrive/car/toyota/values.py
+136 -319
View File
@@ -53,26 +53,19 @@ class ToyotaFlagsSP(IntFlag):
class CAR(StrEnum):
# Toyota
ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
ALPHARDH_TSS2 = "TOYOTA ALPHARD HYBRID 2021"
AVALON = "TOYOTA AVALON 2016"
AVALON_2019 = "TOYOTA AVALON 2019"
AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5
AVALONH_TSS2 = "TOYOTA AVALON HYBRID 2022"
CAMRY = "TOYOTA CAMRY 2018"
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5
CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021"
CHR = "TOYOTA C-HR 2018"
CHR_TSS2 = "TOYOTA C-HR 2021"
CHRH_TSS2 = "TOYOTA C-HR HYBRID 2022"
COROLLA = "TOYOTA COROLLA 2017"
# LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020"
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020"
PRIUS = "TOYOTA PRIUS 2017"
PRIUS_V = "TOYOTA PRIUS v 2017"
PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
@@ -81,8 +74,6 @@ class CAR(StrEnum):
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
SIENNA = "TOYOTA SIENNA 2018"
@@ -115,26 +106,38 @@ class ToyotaCarInfo(CarInfo):
CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
# Toyota
CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"),
CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
CAR.ALPHARD_TSS2: [
ToyotaCarInfo("Toyota Alphard 2019-20"),
ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
],
CAR.AVALON: [
ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"),
ToyotaCarInfo("Toyota Avalon 2017-18"),
],
CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21"),
CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"),
CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]),
CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-24"),
CAR.AVALON_2019: [
ToyotaCarInfo("Toyota Avalon 2019-21"),
ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
],
CAR.AVALON_TSS2: [
ToyotaCarInfo("Toyota Avalon 2022"),
ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
],
CAR.CAMRY: [
ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
],
CAR.CAMRY_TSS2: [
ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]),
ToyotaCarInfo("Toyota Camry Hybrid 2021-24"),
],
CAR.CHR: [
ToyotaCarInfo("Toyota C-HR 2017-20"),
ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"),
],
CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"),
CAR.CHRH_TSS2: ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"),
CAR.CHR_TSS2: [
ToyotaCarInfo("Toyota C-HR 2021"),
ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"),
],
CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"),
CAR.COROLLA_TSS2: [
ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
@@ -147,9 +150,11 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
ToyotaCarInfo("Lexus UX Hybrid 2019-23"),
],
CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-23"),
CAR.HIGHLANDER_TSS2: [
ToyotaCarInfo("Toyota Highlander 2020-23"),
ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"),
],
CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"),
CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"),
CAR.PRIUS: [
ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
@@ -172,10 +177,14 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
],
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
CAR.RAV4_TSS2_2022: [
ToyotaCarInfo("Toyota RAV4 2022"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
],
CAR.RAV4_TSS2_2023: [
ToyotaCarInfo("Toyota RAV4 2023"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
],
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
@@ -350,7 +359,7 @@ FW_CHUNK_LEN = 16
PLATFORM_CODE_ECUS = [Ecu.fwdCamera, Ecu.abs, Ecu.eps]
# These platforms have at least one platform code for all ECUs shared with another platform.
FUZZY_EXCLUDED_PLATFORMS = {CAR.LEXUS_ES_TSS2, CAR.LEXUS_RX_TSS2}
FUZZY_EXCLUDED_PLATFORMS: set[CAR] = set()
# Some ECUs that use KWP2000 have their FW versions on non-standard data identifiers.
# Toyota diagnostic software first gets the supported data ids, then queries them one by one.
@@ -385,7 +394,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
],
non_essential_ecus={
# FIXME: On some models, abs can sometimes be missing
Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS],
Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2],
# On some models, the engine can show on two different addresses
Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC,
CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX_TSS2],
@@ -457,44 +466,26 @@ FW_VERSIONS = {
b'F152607171\x00\x00\x00\x00\x00\x00',
b'F152607110\x00\x00\x00\x00\x00\x00',
b'F152607180\x00\x00\x00\x00\x00\x00',
b'F152641040\x00\x00\x00\x00\x00\x00',
b'F152641061\x00\x00\x00\x00\x00\x00',
b'F152641050\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881510703200\x00\x00\x00\x00',
b'881510704200\x00\x00\x00\x00',
b'881514107100\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B41080\x00\x00\x00\x00\x00\x00',
b'8965B07010\x00\x00\x00\x00\x00\x00',
b'8965B41090\x00\x00\x00\x00\x00\x00',
b'8965B41070\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630725200\x00\x00\x00\x00',
b'\x01896630725300\x00\x00\x00\x00',
b'\x01896630735100\x00\x00\x00\x00',
b'\x01896630738000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4702300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F0702100\x00\x00\x00\x00',
],
},
CAR.AVALONH_2019: {
(Ecu.abs, 0x7b0, None): [
b'F152641040\x00\x00\x00\x00\x00\x00',
b'F152641061\x00\x00\x00\x00\x00\x00',
b'F152641050\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881510704200\x00\x00\x00\x00',
b'881514107100\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B07010\x00\x00\x00\x00\x00\x00',
b'8965B41090\x00\x00\x00\x00\x00\x00',
b'8965B41070\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
@@ -511,6 +502,7 @@ FW_VERSIONS = {
b'\x01F152607240\x00\x00\x00\x00\x00\x00',
b'\x01F152607250\x00\x00\x00\x00\x00\x00',
b'\x01F152607280\x00\x00\x00\x00\x00\x00',
b'F152641080\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B41110\x00\x00\x00\x00\x00\x00',
@@ -518,24 +510,6 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x01896630742000\x00\x00\x00\x00',
b'\x01896630743000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.AVALONH_TSS2: {
(Ecu.abs, 0x7b0, None): [
b'F152641080\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B41110\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x018966306Q6000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -571,65 +545,6 @@ FW_VERSIONS = {
b'\x018966333Q6300\x00\x00\x00\x00',
b'\x018966333Q6500\x00\x00\x00\x00',
b'\x018966333W6000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'8821F0601200 ',
b'8821F0601300 ',
b'8821F0602000 ',
b'8821F0603300 ',
b'8821F0604100 ',
b'8821F0605200 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609100 ',
],
(Ecu.abs, 0x7b0, None): [
b'F152606210\x00\x00\x00\x00\x00\x00',
b'F152606230\x00\x00\x00\x00\x00\x00',
b'F152606270\x00\x00\x00\x00\x00\x00',
b'F152606290\x00\x00\x00\x00\x00\x00',
b'F152606410\x00\x00\x00\x00\x00\x00',
b'F152633540\x00\x00\x00\x00\x00\x00',
b'F152633A10\x00\x00\x00\x00\x00\x00',
b'F152633A20\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B33540\x00\x00\x00\x00\x00\x00',
b'8965B33542\x00\x00\x00\x00\x00\x00',
b'8965B33580\x00\x00\x00\x00\x00\x00',
b'8965B33581\x00\x00\x00\x00\x00\x00',
b'8965B33621\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791
b'8821F0601200 ',
b'8821F0601300 ',
b'8821F0602000 ',
b'8821F0603300 ',
b'8821F0604100 ',
b'8821F0605200 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F0601200 ',
b'8646F0601300 ',
b'8646F0601400 ',
b'8646F0603400 ',
b'8646F0604100 ',
b'8646F0605000 ',
b'8646F0606000 ',
b'8646F0606100 ',
b'8646F0607100 ',
],
},
CAR.CAMRYH: {
(Ecu.engine, 0x700, None): [
b'\x018966306Q6000\x00\x00\x00\x00',
b'\x018966333N1100\x00\x00\x00\x00',
b'\x018966333N4300\x00\x00\x00\x00',
@@ -652,7 +567,35 @@ FW_VERSIONS = {
b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'8821F0601200 ',
b'8821F0601300 ',
b'8821F0602000 ',
b'8821F0603300 ',
b'8821F0604100 ',
b'8821F0605200 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609100 ',
b'8821F0603400 ',
b'8821F0604000 ',
b'8821F0604200 ',
b'8821F0606200 ',
b'8821F0609000 ',
],
(Ecu.abs, 0x7b0, None): [
b'F152606210\x00\x00\x00\x00\x00\x00',
b'F152606230\x00\x00\x00\x00\x00\x00',
b'F152606270\x00\x00\x00\x00\x00\x00',
b'F152606290\x00\x00\x00\x00\x00\x00',
b'F152606410\x00\x00\x00\x00\x00\x00',
b'F152633540\x00\x00\x00\x00\x00\x00',
b'F152633A10\x00\x00\x00\x00\x00\x00',
b'F152633A20\x00\x00\x00\x00\x00\x00',
b'F152633214\x00\x00\x00\x00\x00\x00',
b'F152633660\x00\x00\x00\x00\x00\x00',
b'F152633712\x00\x00\x00\x00\x00\x00',
@@ -660,70 +603,61 @@ FW_VERSIONS = {
b'F152633B51\x00\x00\x00\x00\x00\x00',
b'F152633B60\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'8821F0601200 ',
b'8821F0601300 ',
b'8821F0603400 ',
b'8821F0604000 ',
b'8821F0604100 ',
b'8821F0604200 ',
b'8821F0605200 ',
b'8821F0606200 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609000 ',
b'8821F0609100 ',
],
(Ecu.eps, 0x7a1, None): [
b'8965B33540\x00\x00\x00\x00\x00\x00',
b'8965B33542\x00\x00\x00\x00\x00\x00',
b'8965B33550\x00\x00\x00\x00\x00\x00',
b'8965B33551\x00\x00\x00\x00\x00\x00',
b'8965B33580\x00\x00\x00\x00\x00\x00',
b'8965B33581\x00\x00\x00\x00\x00\x00',
b'8965B33611\x00\x00\x00\x00\x00\x00',
b'8965B33621\x00\x00\x00\x00\x00\x00',
b'8965B33550\x00\x00\x00\x00\x00\x00',
b'8965B33551\x00\x00\x00\x00\x00\x00',
b'8965B33611\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791
b'8821F0601200 ',
b'8821F0601300 ',
b'8821F0603400 ',
b'8821F0604000 ',
b'8821F0602000 ',
b'8821F0603300 ',
b'8821F0604100 ',
b'8821F0604200 ',
b'8821F0605200 ',
b'8821F0606200 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
b'8821F0609000 ',
b'8821F0609100 ',
b'8821F0603400 ',
b'8821F0604000 ',
b'8821F0604200 ',
b'8821F0606200 ',
b'8821F0609000 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F0601200 ',
b'8646F0601300 ',
b'8646F0601400 ',
b'8646F0603400 ',
b'8646F0603500 ',
b'8646F0604100 ',
b'8646F0605000 ',
b'8646F0606000 ',
b'8646F0606100 ',
b'8646F0607000 ',
b'8646F0607100 ',
b'8646F0603500 ',
b'8646F0607000 ',
],
},
CAR.CAMRY_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B33630\x00\x00\x00\x00\x00\x00',
b'8965B33640\x00\x00\x00\x00\x00\x00',
b'8965B33650\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F152606370\x00\x00\x00\x00\x00\x00',
b'\x01F152606390\x00\x00\x00\x00\x00\x00',
b'\x01F152606400\x00\x00\x00\x00\x00\x00',
b'\x01F152606431\x00\x00\x00\x00\x00\x00',
b'F152633D00\x00\x00\x00\x00\x00\x00',
b'F152633D60\x00\x00\x00\x00\x00\x00',
b'F152633310\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x018966306Q5000\x00\x00\x00\x00',
@@ -734,10 +668,16 @@ FW_VERSIONS = {
b'\x018966306T3200\x00\x00\x00\x00',
b'\x018966306T4000\x00\x00\x00\x00',
b'\x018966306T4100\x00\x00\x00\x00',
b'\x018966306Q6000\x00\x00\x00\x00',
b'\x018966306Q7000\x00\x00\x00\x00',
b'\x018966306T0000\x00\x00\x00\x00',
b'\x018966306V1000\x00\x00\x00\x00',
b'\x01896633T20000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
b'\x018821F6201400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
@@ -747,35 +687,7 @@ FW_VERSIONS = {
b'\x028646F3305200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.CAMRYH_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B33630\x00\x00\x00\x00\x00\x00',
b'8965B33650\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152633D00\x00\x00\x00\x00\x00\x00',
b'F152633D60\x00\x00\x00\x00\x00\x00',
b'F152633310\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x018966306Q6000\x00\x00\x00\x00',
b'\x018966306Q7000\x00\x00\x00\x00',
b'\x018966306T0000\x00\x00\x00\x00',
b'\x018966306V1000\x00\x00\x00\x00',
b'\x01896633T20000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 15): [
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
b'\x018821F6201400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 109): [
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.CHR: {
@@ -867,13 +779,19 @@ FW_VERSIONS = {
(Ecu.abs, 0x7b0, None): [
b'F152610260\x00\x00\x00\x00\x00\x00',
b'F1526F4270\x00\x00\x00\x00\x00\x00',
b'F152610041\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B10091\x00\x00\x00\x00\x00\x00',
b'8965B10110\x00\x00\x00\x00\x00\x00',
b'8965B10092\x00\x00\x00\x00\x00\x00',
b'8965B10111\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x0189663F459000\x00\x00\x00\x00',
b'\x0189663F438000\x00\x00\x00\x00',
b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x0331014000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
@@ -881,33 +799,12 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821FF410200\x00\x00\x00\x00',
b'\x018821FF410300\x00\x00\x00\x00',
b'\x018821FF410500\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00',
b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00',
],
},
CAR.CHRH_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B10092\x00\x00\x00\x00\x00\x00',
b'8965B10091\x00\x00\x00\x00\x00\x00',
b'8965B10111\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152610041\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x0189663F438000\x00\x00\x00\x00',
b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 15): [
b'\x018821FF410500\x00\x00\x00\x00',
b'\x018821FF410300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 109): [
b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00',
b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00',
],
},
CAR.COROLLA: {
@@ -1193,6 +1090,18 @@ FW_VERSIONS = {
b'\x01F15260E110\x00\x00\x00\x00\x00\x00',
b'\x01F15260E170\x00\x00\x00\x00\x00\x00',
b'\x01F15260E05300\x00\x00\x00\x00',
b'\x01F15264872300\x00\x00\x00\x00',
b'\x01F15264872400\x00\x00\x00\x00',
b'\x01F15264872500\x00\x00\x00\x00',
b'\x01F15264872600\x00\x00\x00\x00',
b'\x01F15264872700\x00\x00\x00\x00',
b'\x01F15264873500\x00\x00\x00\x00',
b'\x01F152648C6300\x00\x00\x00\x00',
b'\x01F152648J4000\x00\x00\x00\x00',
b'\x01F152648J5000\x00\x00\x00\x00',
b'\x01F152648J6000\x00\x00\x00\x00',
b'\x01F152648J7000\x00\x00\x00\x00',
b'\x01F152648L5000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630E62100\x00\x00\x00\x00',
@@ -1213,44 +1122,13 @@ FW_VERSIONS = {
b'\x01896630EE1100\x00\x00\x00\x00',
b'\x01896630EG3000\x00\x00\x00\x00',
b'\x01896630EG5000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F4803200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.HIGHLANDERH_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B48241\x00\x00\x00\x00\x00\x00',
b'8965B48310\x00\x00\x00\x00\x00\x00',
b'8965B48400\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F15264872300\x00\x00\x00\x00',
b'\x01F15264872400\x00\x00\x00\x00',
b'\x01F15264872500\x00\x00\x00\x00',
b'\x01F15264872600\x00\x00\x00\x00',
b'\x01F15264873500\x00\x00\x00\x00',
b'\x01F152648C6300\x00\x00\x00\x00',
b'\x01F152648J4000\x00\x00\x00\x00',
b'\x01F152648J5000\x00\x00\x00\x00',
b'\x01F152648J6000\x00\x00\x00\x00',
b'\x01F15264872700\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630E67000\x00\x00\x00\x00',
b'\x01896630EA1000\x00\x00\x00\x00',
b'\x01896630EE4000\x00\x00\x00\x00',
b'\x01896630EE4100\x00\x00\x00\x00',
b'\x01896630EE5000\x00\x00\x00\x00',
b'\x01896630EE6000\x00\x00\x00\x00',
b'\x01896630EE7000\x00\x00\x00\x00',
b'\x01896630EF8000\x00\x00\x00\x00',
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -1638,46 +1516,6 @@ FW_VERSIONS = {
(Ecu.abs, 0x7b0, None): [
b'\x01F15260R350\x00\x00\x00\x00\x00\x00',
b'\x01F15260R361\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AA0000\x00\x00\x00\x00',
b'\x01896634AA0100\x00\x00\x00\x00',
b'\x01896634AA1000\x00\x00\x00\x00',
b'\x01896634A88000\x00\x00\x00\x00',
b'\x01896634A89000\x00\x00\x00\x00',
b'\x01896634A89100\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R01100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634A88100\x00\x00\x00\x00',
b'\x01896634AJ2000\x00\x00\x00\x00',
b'\x01896634A89100\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2_2022: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15264283100\x00\x00\x00\x00',
b'\x01F15264286200\x00\x00\x00\x00',
b'\x01F15264286100\x00\x00\x00\x00',
@@ -1689,6 +1527,12 @@ FW_VERSIONS = {
b'8965B42172\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AA0000\x00\x00\x00\x00',
b'\x01896634AA0100\x00\x00\x00\x00',
b'\x01896634AA1000\x00\x00\x00\x00',
b'\x01896634A88000\x00\x00\x00\x00',
b'\x01896634A89000\x00\x00\x00\x00',
b'\x01896634A89100\x00\x00\x00\x00',
b'\x01896634A02001\x00\x00\x00\x00',
b'\x01896634A03000\x00\x00\x00\x00',
b'\x01896634A08000\x00\x00\x00\x00',
@@ -1704,8 +1548,9 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2_2023: {
CAR.RAV4_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
b'\x01F15264283200\x00\x00\x00\x00',
b'\x01F15264283300\x00\x00\x00\x00',
b'\x01F152642F1000\x00\x00\x00\x00',
@@ -1715,15 +1560,12 @@ FW_VERSIONS = {
b'8965B42371\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634A88100\x00\x00\x00\x00',
b'\x01896634AJ2000\x00\x00\x00\x00',
b'\x01896634A89100\x00\x00\x00\x00',
b'\x01896634AE1001\x00\x00\x00\x00',
b'\x01896634AF0000\x00\x00\x00\x00',
],
(Ecu.hybrid, 0x7d2, None): [
b'\x02899830R39000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
b'\x02899830R41000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
b'\x028998342C0000\x00\x00\x00\x00899854224000\x00\x00\x00\x00',
b'\x028998342C6000\x00\x00\x00\x00899854224000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
@@ -1788,7 +1630,6 @@ FW_VERSIONS = {
CAR.LEXUS_ES_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x018966306U6000\x00\x00\x00\x00',
b'\x01896630EC9100\x00\x00\x00\x00',
b'\x018966333T5000\x00\x00\x00\x00',
b'\x018966333T5100\x00\x00\x00\x00',
b'\x018966333X6000\x00\x00\x00\x00',
@@ -1806,7 +1647,6 @@ FW_VERSIONS = {
b'\x01F152606281\x00\x00\x00\x00\x00\x00',
b'\x01F152606340\x00\x00\x00\x00\x00\x00',
b'\x01F152606461\x00\x00\x00\x00\x00\x00',
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
b'\x01F15260646200\x00\x00\x00\x00',
b'F152633423\x00\x00\x00\x00\x00\x00',
b'F152633680\x00\x00\x00\x00\x00\x00',
@@ -1819,7 +1659,6 @@ FW_VERSIONS = {
b'8965B33590\x00\x00\x00\x00\x00\x00',
b'8965B33690\x00\x00\x00\x00\x00\x00',
b'8965B33721\x00\x00\x00\x00\x00\x00',
b'8965B48271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -1835,7 +1674,6 @@ FW_VERSIONS = {
b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
@@ -2090,6 +1928,7 @@ FW_VERSIONS = {
b'\x01896630EA9000\x00\x00\x00\x00',
b'\x01896630EB0000\x00\x00\x00\x00',
b'\x01896630EC9000\x00\x00\x00\x00',
b'\x01896630EC9100\x00\x00\x00\x00',
b'\x01896630ED0000\x00\x00\x00\x00',
b'\x01896630ED0100\x00\x00\x00\x00',
b'\x01896630ED6000\x00\x00\x00\x00',
@@ -2185,11 +2024,15 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B58040\x00\x00\x00\x00\x00\x00',
b'8965B58052\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152658341\x00\x00\x00\x00\x00\x00'
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301200\x00\x00\x00\x00',
b'\x018821F3301400\x00\x00\x00\x00',
@@ -2197,22 +2040,6 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.ALPHARDH_TSS2: {
(Ecu.engine, 0x7e0, None): [
b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B58040\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152658341\x00\x00\x00\x00\x00\x00'
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646FV201000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
@@ -2232,20 +2059,14 @@ DBC = {
CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.CHRH_TSS2: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
@@ -2257,14 +2078,11 @@ DBC = {
CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4H_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.ALPHARDH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
}
# These cars have non-standard EPS torque scale factors. All others are 73
@@ -2272,20 +2090,19 @@ EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_I
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2,
CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2,
CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.LEXUS_IS_TSS2, CAR.MIRAI, CAR.LEXUS_NX_TSS2,
CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.CHR_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY, CAR.CAMRYH}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY}
# the DSU uses the AEB message for longitudinal on these cars
UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2}
RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2}
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023}
# no resume button press required
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}
selfdrive/car/volkswagen/carcontroller.py
+4 -8
View File
@@ -170,12 +170,9 @@ class CarController:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values,
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled:
if gra_send_ready and not self.CP.pcmCruiseSpeed:
if not self.CP.pcmCruiseSpeed:
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
if self.cruise_button is not None:
send_freq = (self.frame % 10 == 0)
if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2:
send_freq = (self.frame - self.last_button_frame) * DT_CTRL > 0.01
if self.acc_type == -1:
if self.button_count >= 2 and self.v_set_dis_prev != self.v_set_dis:
self.acc_type = 1 if abs(self.v_set_dis - self.v_set_dis_prev) >= 10 and self.last_cruise_button in (1, 2) else \
@@ -186,10 +183,9 @@ class CarController:
self.cruise_button = 1 if self.cruise_button == 1 else 2 # accel, decel
elif self.acc_type == 1:
self.cruise_button = 3 if self.cruise_button == 1 else 4 # resume, set
if send_freq:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values,
buttons=self.cruise_button))
self.last_button_frame = self.frame
if self.frame % self.CCP.BTN_STEP == 0:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, frame=(self.frame // self.CCP.BTN_STEP),
buttons=self.cruise_button, custom_stock_long=True))
self.send_count += 1
else:
self.send_count = 0
selfdrive/car/volkswagen/mqbcan.py
+2 -2
View File
@@ -31,7 +31,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, s
return packer.make_can_msg("LDW_02", bus, values)
def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0, cancel=False, resume=False):
def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalter", # ACC button, on/off
"GRA_Typ_Hauptschalter", # ACC main button type
@@ -46,7 +46,7 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0, cancel=
set_cruise = 1 if buttons == 4 else 0
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Tip_Wiederaufnahme": resume or resume_cruise,
"GRA_Tip_Setzen": set_cruise,
selfdrive/car/volkswagen/pqcan.py
+2 -2
View File
@@ -31,7 +31,7 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, s
return packer.make_can_msg("LDW_Status", bus, values)
def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0, cancel=False, resume=False):
def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0, cancel=False, resume=False, custom_stock_long=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalt", # ACC button, on/off
"GRA_Typ_Hauptschalt", # ACC button, momentary vs latching
@@ -45,7 +45,7 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, buttons=0, cancel=
set_cruise = 1 if buttons == 4 else 0
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"COUNTER": (frame + 1) % 0x10 if custom_stock_long else (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Recall": resume or resume_cruise,
"GRA_Neu_Setzen": set_cruise,
selfdrive/car/volkswagen/values.py
+8
View File
@@ -46,6 +46,7 @@ class CarControllerParams:
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00))
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
self.BTN_STEP = 2
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"]
@@ -75,6 +76,7 @@ class CarControllerParams:
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
self.BTN_STEP = 3
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
@@ -842,6 +844,7 @@ FW_VERSIONS = {
b'\xf1\x8783A907115F \xf1\x890002',
b'\xf1\x8783A907115G \xf1\x890001',
b'\xf1\x8783A907115K \xf1\x890001',
b'\xf1\x8783A907115K \xf1\x890002',
b'\xf1\x8704E906024AP\xf1\x891461',
b'\xf1\x8783A907115 \xf1\x890007',
],
@@ -851,6 +854,7 @@ FW_VERSIONS = {
b'\xf1\x8709G927158GC\xf1\x893821',
b'\xf1\x8709G927158GD\xf1\x893820',
b'\xf1\x8709G927158GM\xf1\x893936',
b'\xf1\x8709G927158GN\xf1\x893938',
b'\xf1\x870D9300043 \xf1\x895202',
b'\xf1\x870DL300011N \xf1\x892001',
b'\xf1\x870DL300011N \xf1\x892012',
@@ -889,6 +893,7 @@ FW_VERSIONS = {
b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1',
b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002RA60A2ROM',
b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002SA6092SOM',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6017A00',
b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60804A1',
@@ -1081,11 +1086,13 @@ FW_VERSIONS = {
b'\xf1\x8783A906259 \xf1\x890001',
b'\xf1\x8783A906259 \xf1\x890005',
b'\xf1\x8783A906259C \xf1\x890002',
b'\xf1\x8783A906259D \xf1\x890001',
b'\xf1\x8783A906259F \xf1\x890001',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158CN\xf1\x893608',
b'\xf1\x8709G927158FL\xf1\x893758',
b'\xf1\x8709G927158GG\xf1\x893825',
b'\xf1\x8709G927158GP\xf1\x893937',
b'\xf1\x870GC300045D \xf1\x892802',
b'\xf1\x870GC300046F \xf1\x892701',
@@ -1095,6 +1102,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218C219321532111',
b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111',
b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111',
b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218A219321532111',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300',
selfdrive/controls/lib/drive_helpers.py
+2 -2
View File
@@ -4,7 +4,7 @@ from cereal import car, log
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
# WARNING: this value was determined based on the model's training distribution,
# model predictions above this speed can be unpredictable
@@ -243,7 +243,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
current_curvature_desired = curvatures[0]
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
average_curvature_desired = psi / (v_ego * delay)
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+3 -3
View File
@@ -5,7 +5,7 @@ import numpy as np
from casadi import SX, vertcat, sin, cos
# WARNING: imports outside of constants will not trigger a rebuild
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -66,7 +66,7 @@ def gen_lat_ocp():
ocp = AcadosOcp()
ocp.model = gen_lat_model()
Tf = np.array(T_IDXS)[N]
Tf = np.array(ModelConstants.T_IDXS)[N]
# set dimensions
ocp.dims.N = N
@@ -122,7 +122,7 @@ def gen_lat_ocp():
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
ocp.code_export_directory = EXPORT_DIR
return ocp
selfdrive/controls/lib/longcontrol.py
+6 -6
View File
@@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -70,19 +70,19 @@ class LongControl:
# Interp control trajectory
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now
v_target = min(v_target_lower, v_target_upper)
a_target = min(a_target_lower, a_target_upper)
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds)
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds)
else:
v_target = 0.0
v_target_now = 0.0
selfdrive/controls/lib/longitudinal_planner.py
+11 -10
View File
@@ -11,7 +11,9 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import SpeedLimitController
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import SpeedLimitController
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
@@ -85,8 +87,7 @@ class LongitudinalPlanner:
def read_param(self):
try:
self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl") and
self.params.get_bool("DynamicExperimentalControlToggle"))
self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
except AttributeError:
self.dynamic_experimental_controller = DynamicExperimentalController()
try:
@@ -99,9 +100,9 @@ class LongitudinalPlanner:
if (len(model_msg.position.x) == 33 and
len(model_msg.velocity.x) == 33 and
len(model_msg.acceleration.x) == 33):
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
j = np.zeros(len(T_IDXS_MPC))
else:
x = np.zeros(len(T_IDXS_MPC))
@@ -167,11 +168,11 @@ class LongitudinalPlanner:
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality)
self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
@@ -180,7 +181,7 @@ class LongitudinalPlanner:
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.a_desired = float(interp(DT_MDL, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
self.e2e_events(sm)
selfdrive/controls/lib/sunnypilot/speed_limit_controller.py
+10 -13
View File
@@ -12,7 +12,7 @@ from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source, Policy
from openpilot.selfdrive.controls.lib.sunnypilot.helpers import description_for_state, debug
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
class SpeedLimitController:
@@ -23,7 +23,6 @@ class SpeedLimitController:
self._last_op_enabled_time = 0.0
self._is_metric = self._params.get_bool("IsMetric")
self._is_enabled = self._params.get_bool("SpeedLimitControl")
self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset")
self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator")
self._op_enabled = False
self._op_enabled_prev = False
@@ -96,13 +95,12 @@ class SpeedLimitController:
@property
def speed_limit_offset(self):
if self._offset_enabled:
if self._offset_type == 0:
return interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit
elif self._offset_type == 1:
return self._offset_value * 0.01 * self._speed_limit
elif self._offset_type == 2:
return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
if self._offset_type == 0:
return interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit
elif self._offset_type == 1:
return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
elif self._offset_type == 2:
return self._offset_value * 0.01 * self._speed_limit
return 0.
@property
@@ -121,10 +119,9 @@ class SpeedLimitController:
t = time.monotonic()
if t > self._last_params_update + PARAMS_UPDATE_PERIOD:
self._is_enabled = self._params.get_bool("SpeedLimitControl")
self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset")
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_enabled}')
debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}')
self._last_params_update = t
def _update_calculations(self):
@@ -196,11 +193,11 @@ class SpeedLimitController:
if self.distance > 0:
return (self.speed_limit_offseted**2 - self._v_ego**2) / (2. * self.distance)
return self._v_offset / T_IDXS[CONTROL_N]
return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
def get_active_state_target_acceleration(self):
""" In active state, aim to keep speed constant around control time horizon """
return self._v_offset / T_IDXS[CONTROL_N]
return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
def _update_solution(self):
a_target = self.acceleration_solutions[self.state]()
selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py
+24 -53
View File
@@ -15,8 +15,8 @@ class SpeedLimitResolver:
self._policy = policy
self._policy_to_sources_map = {
Policy.car_state_only: [Source.car_state],
Policy.car_state_priority: [Source.car_state, Source.map_data],
Policy.map_data_priority: [Source.map_data, Source.car_state],
Policy.car_state_priority: [Source.car_state, Source.nav, Source.map_data],
Policy.map_data_priority: [Source.map_data, Source.nav, Source.car_state],
Policy.nav_priority: [Source.nav, Source.map_data, Source.car_state],
Policy.map_data_only: [Source.map_data],
Policy.combined: [Source.car_state, Source.nav, Source.map_data],
@@ -86,7 +86,7 @@ class SpeedLimitResolver:
self._limit_solutions[Source.map_data] = speed_limit
self._distance_solutions[Source.map_data] = 0.
if next_speed_limit > 0. and next_speed_limit < self._v_ego:
if 0. < next_speed_limit < self._v_ego:
adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time**2
@@ -95,57 +95,28 @@ class SpeedLimitResolver:
self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead
def _consolidate(self):
solutions = self._get_solutions_according_to_policy()
limits = solutions['limits']
distances = solutions['distances']
sources = solutions['sources']
extra_sources = []
# Get all non-zero values and set the minimum if any, otherwise 0.
mask = limits > 0.
limits = limits[mask]
distances = distances[mask]
sources = sources[mask]
if len(limits) > 0:
min_idx = np.argmin(limits)
self.speed_limit = limits[min_idx]
self.distance = distances[min_idx]
self.source = Source(sources[min_idx])
elif self._policy in [Policy.car_state_priority, Policy.nav_priority, Policy.map_data_priority]:
# If policy is car_state_priority -> append map data
if self._policy == Policy.car_state_priority:
extra_sources = [Source.map_data]
# If policy is map_data_priority --> append car state
elif self._policy == Policy.map_data_priority:
extra_sources = [Source.car_state]
# If policy is nav_priority -> append map data then car state
elif self._policy == Policy.nav_priority:
extra_sources = [Source.map_data, Source.car_state]
for src in extra_sources:
if self._limit_solutions[src] > 0:
# If the speed limit from the source is above 0, then choose this source
self.speed_limit = self._limit_solutions[src]
self.distance = self._distance_solutions[src]
self.source = src
break
else:
self.speed_limit = 0.
self.distance = 0.
self.source = Source.none
else:
self.speed_limit = 0.
self.distance = 0.
self.source = Source.none
source = self._get_source_solution_according_to_policy()
self.speed_limit = self._limit_solutions[source] if source else 0.
self.distance = self._distance_solutions[source] if source else 0.
self.source = source or Source.none
debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
return self.speed_limit, self.distance, self.source
def _get_solutions_according_to_policy(self):
source_codes = self._policy_to_sources_map[self._policy]
limits = np.array([self._limit_solutions[source] for source in source_codes], dtype=float)
distances = np.array([self._distance_solutions[source] for source in source_codes], dtype=float)
sources = np.array([source.value for source in source_codes], dtype=int)
return {'limits': limits, 'distances': distances, 'sources': sources}
def _get_source_solution_according_to_policy(self) -> Source | None:
sources_for_policy = self._policy_to_sources_map[self._policy]
if self._policy != Policy.combined:
# They are ordered in the order of preference, so we pick the first that's non zero
for source in sources_for_policy:
if self._limit_solutions[source] > 0.:
return source
limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
sources = np.array([source.value for source in sources_for_policy], dtype=int)
if len(limits) > 0:
min_idx = np.argmin(limits)
return sources[min_idx]
return None
selfdrive/controls/lib/sunnypilot/tests/test_speed_limit_resolver.py
+132
View File
@@ -0,0 +1,132 @@
import random
import time
import pytest
from unittest.mock import MagicMock
from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_MAX_MAP_DATA_AGE
# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver
from selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
from openpilot.selfdrive.controls.lib.sunnypilot.common import Policy
def create_mock(properties):
mock = MagicMock()
for property, value in properties.items():
setattr(mock, property, value)
return mock
def setup_sm_mock():
cruise_speed_limit = random.uniform(0, 120)
nav_instruction_limit = random.uniform(0, 120)
live_map_data_limit = random.uniform(0, 120)
cruise_state = create_mock({'speedLimit': cruise_speed_limit})
car_state = create_mock({
'cruiseState': cruise_state,
'gasPressed': False,
'brakePressed': False,
'standstill': False,
})
nav_instruction = create_mock({'speedLimit': nav_instruction_limit})
live_map_data = create_mock({
'speedLimit': live_map_data_limit,
'speedLimitValid': True,
'speedLimitAhead': 0.,
'speedLimitAheadValid': 0.,
'speedLimitAheadDistance': 0.,
'lastGpsTimestamp': time.time() * 1e3,
})
sm_mock = MagicMock()
sm_mock.__getitem__.side_effect = lambda key: {
'carState': car_state,
'navInstruction': nav_instruction,
'liveMapDataSP': live_map_data,
}[key]
return sm_mock
parametrized_policies = pytest.mark.parametrize(
"policy, sm_key, function_key", [
(Policy.car_state_only, 'carState', 'car_state'),
(Policy.car_state_priority, 'carState', 'car_state'),
(Policy.map_data_only, 'liveMapDataSP', 'map_data'),
(Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
(Policy.nav_only, 'navInstruction', 'nav'),
(Policy.nav_priority, 'navInstruction', 'nav'),
],
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
)
@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"])
class TestSpeedLimitResolverValidation:
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_initial_state(self, resolver_class, policy):
resolver = resolver_class(policy)
for source in Source:
if source in resolver._limit_solutions:
assert resolver._limit_solutions[source] == 0.
assert resolver._distance_solutions[source] == 0.
@parametrized_policies
def test_resolver(self, resolver_class, policy, sm_key, function_key):
resolver = resolver_class(policy)
sm_mock = setup_sm_mock()
source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
# Assert the resolver
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
assert speed_limit == source_speed_limit
assert source == Source[function_key]
def test_resolver_combined(self, resolver_class):
resolver = resolver_class(Policy.combined)
sm_mock = setup_sm_mock()
socket_to_source = {'carState': Source.car_state, 'liveMapDataSP': Source.map_data, 'navInstruction': Source.nav}
minimum_key, minimum_speed_limit = min(
((key, sm_mock[key].cruiseState.speedLimit) if key == 'carState' else (key, sm_mock[key].speedLimit) for key in
socket_to_source.keys()), key=lambda x: x[1])
# Assert the resolver
speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock)
assert speed_limit == minimum_speed_limit
assert source == socket_to_source[minimum_key]
@parametrized_policies
def test_parser(self, resolver_class, policy, sm_key, function_key):
resolver = resolver_class(policy)
sm_mock = setup_sm_mock()
source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
# Assert the parsing
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
assert resolver._limit_solutions[Source[function_key]] == source_speed_limit
assert resolver._distance_solutions[Source[function_key]] == 0.
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_resolve_interaction_in_update(self, resolver_class, policy):
v_ego = 50
resolver = resolver_class(policy)
sm_mock = setup_sm_mock()
_speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock)
# After resolution
assert _speed_limit is not None
assert _distance is not None
assert _source is not None
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_old_map_data_ignored(self, resolver_class, policy):
resolver = resolver_class(policy)
sm_mock = MagicMock()
sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
resolver._sm = sm_mock
resolver._get_from_map_data()
assert resolver._limit_solutions[Source.map_data] == 0.
assert resolver._distance_solutions[Source.map_data] == 0.
selfdrive/controls/lib/turn_speed_controller.py
+2 -2
View File
@@ -4,7 +4,7 @@ from common.params import Params
from cereal import custom
from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_SPEED, LIMIT_MAX_MAP_DATA_AGE, \
LIMIT_SPEED_OFFSET_TH, CONTROL_N, LIMIT_MIN_ACC, LIMIT_MAX_ACC
from selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
_ACTIVE_LIMIT_MIN_ACC = -0.5 # m/s^2 Maximum deceleration allowed while active.
@@ -223,7 +223,7 @@ class TurnSpeedController():
elif self.state == TurnSpeedControlState.active:
# When active we are trying to keep the speed constant around the control time horizon.
# but under constrained acceleration limits since we are in a turn.
a_target = self._v_offset / T_IDXS[CONTROL_N]
a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
a_target = np.clip(a_target, _ACTIVE_LIMIT_MIN_ACC, _ACTIVE_LIMIT_MAX_ACC)
# update solution values.
selfdrive/controls/plannerd.py
+3 -3
View File
@@ -5,7 +5,7 @@ from cereal import car
from openpilot.common.params import Params
from openpilot.common.realtime import Priority, config_realtime_process
from openpilot.system.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
import cereal.messaging as messaging
@@ -14,8 +14,8 @@ def cumtrapz(x, t):
return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS)
model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS)
plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
ui_send = messaging.new_message('uiPlan')
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
selfdrive/locationd/laikad.py
-468
View File
@@ -1,468 +0,0 @@
#!/usr/bin/env python3
import math
import os
import time
import shutil
from collections import defaultdict
from concurrent.futures import Future, ProcessPoolExecutor
from enum import IntEnum
from typing import List, Optional, Dict, Any
import numpy as np
from cereal import log, messaging
from openpilot.common.params import Params, put_nonblocking
from laika import AstroDog
from laika.constants import SECS_IN_HR, SECS_IN_MIN
from laika.downloader import DownloadFailed
from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId, get_sv_id
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports
from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman
from openpilot.selfdrive.locationd.models.gnss_kf import States as GStates
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import cloudlog
MAX_TIME_GAP = 10
EPHEMERIS_CACHE = 'LaikadEphemerisV3'
CACHE_VERSION = 0.2
POS_FIX_RESIDUAL_THRESHOLD = 100.0
class LogEphemerisType(IntEnum):
nav = 0
nasaUltraRapid = 1
glonassIacUltraRapid = 2
qcom = 3
class EphemerisSource(IntEnum):
gnssChip = 0
internet = 1
cache = 2
unknown = 3
def get_log_eph_type(ephem):
if ephem.eph_type == EphemerisType.NAV:
source_type = LogEphemerisType.nav
elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = LogEphemerisType.qcom
else:
assert ephem.file_epoch is not None
file_src = ephem.file_source
if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z'
source_type = LogEphemerisType.nasaUltraRapid
elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
source_type = LogEphemerisType.glonassIacUltraRapid
else:
raise Exception(f"Didn't expect file source {file_src}")
return source_type
def get_log_eph_source(ephem):
if ephem.file_name == 'qcom' or ephem.file_name == 'ublox':
source = EphemerisSource.gnssChip
elif ephem.file_name == EPHEMERIS_CACHE:
source = EphemerisSource.cache
else:
source = EphemerisSource.internet
return source
class Laikad:
def __init__(self, valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), auto_fetch_navs=True, auto_update=False,
valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY),
save_ephemeris=False, use_qcom=False):
"""
valid_const: GNSS constellation which can be used
auto_fetch_navs: If true fetch navs from internet when needed
auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere.
valid_ephem_types: Valid ephemeris types to be used by AstroDog
save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
"""
self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types,
clear_old_ephemeris=True, cache_dir=Paths.download_cache_root())
self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
self.auto_fetch_navs = auto_fetch_navs
self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
self.orbit_fetch_future: Optional[Future] = None
self.last_report_time = GPSTime(0, 0)
self.last_fetch_navs_t = GPSTime(0, 0)
self.last_cached_t = GPSTime(0, 0)
self.save_ephemeris = save_ephemeris
self.load_cache()
self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)}
self.velfix_function = get_velfix_sympy_func()
self.last_fix_pos = None
self.last_fix_t = None
self.use_qcom = use_qcom
self.first_log_time = None
self.ttff = -1
self.measurement_lag = 0.630 if self.use_qcom else 0.095
# qcom specific stuff
self.qcom_reports_received = 4
self.qcom_reports = []
def load_cache(self):
if not self.save_ephemeris:
return
cache_bytes = Params().get(EPHEMERIS_CACHE)
if not cache_bytes:
return
nav_dict = {}
try:
with ephemeris_structs.EphemerisCache.from_bytes(cache_bytes) as ephem_cache:
glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides]
gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides]
for e in sum([glonass_navs, gps_navs], []):
if e.prn not in nav_dict:
nav_dict[e.prn] = []
nav_dict[e.prn].append(e)
self.astro_dog.add_navs(nav_dict)
except Exception:
cloudlog.exception("Error parsing cache")
cloudlog.debug(
f"Loaded navs ({sum([len(nav_dict[prn]) for prn in nav_dict.keys()])}). Unique orbit and nav sats: {list(nav_dict.keys())} ")
def cache_ephemeris(self):
if self.save_ephemeris and (self.last_report_time - self.last_cached_t > SECS_IN_MIN):
nav_list: List = sum([v for k,v in self.astro_dog.navs.items()], [])
#TODO this only saves currently valid ephems, when we download future ephems we should save them too
valid_navs = [e for e in nav_list if e.valid(self.last_report_time)]
if len(valid_navs) > 0:
ephem_cache = ephemeris_structs.EphemerisCache(glonassEphemerides=[e.data for e in valid_navs if e.prn[0]=='R'],
gpsEphemerides=[e.data for e in valid_navs if e.prn[0]=='G'])
put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes())
cloudlog.debug("Cache saved")
self.last_cached_t = self.last_report_time
def create_ephem_statuses(self):
ephemeris_statuses = []
eph_list: List = sum([v for k,v in self.astro_dog.navs.items()], []) + sum([v for k,v in self.astro_dog.qcom_polys.items()], [])
for eph in eph_list:
status = log.GnssMeasurements.EphemerisStatus.new_message()
status.constellationId = ConstellationId.from_rinex_char(eph.prn[0]).value
status.svId = get_sv_id(eph.prn)
status.type = get_log_eph_type(eph).value
status.source = get_log_eph_source(eph).value
status.tow = eph.epoch.tow
status.gpsWeek = eph.epoch.week
ephemeris_statuses.append(status)
return ephemeris_statuses
def get_lsq_fix(self, t, measurements):
if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4
position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
if len(position_solution) < 3:
return None
position_estimate = position_solution[:3]
position_std = pos_std[:3]
velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements)
if len(velocity_solution) < 3:
return None
velocity_estimate = velocity_solution[:3]
velocity_std = vel_std[:3]
return position_estimate, position_std, velocity_estimate, velocity_std
def is_good_report(self, gnss_msg):
if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom:
# TODO: Understand and use remaining unknown constellations
try:
if gnss_msg.which() == 'drMeasurementReport':
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
else:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source)
good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS]
report_time = gps_time_from_qcom_report(gnss_msg)
except NotImplementedError:
return False
# Garbage timestamps with week > 32767 are sometimes sent by module.
# This is an issue with gpsTime and GLONASS time.
good_week = report_time.week < np.iinfo(np.int16).max
return good_constellation and good_week
elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True
else:
return False
def read_report(self, gnss_msg):
if self.use_qcom:
# QCOM reports are per constellation, so we need to aggregate them
# Additionally, the pseudoranges are broken in the measurementReports
# and the doppler filteredSpeed is broken in the drMeasurementReports
report_time = gps_time_from_qcom_report(gnss_msg)
if report_time - self.last_report_time == 0:
self.qcom_reports.append(gnss_msg)
self.last_report_time = report_time
elif report_time - self.last_report_time > 0:
self.qcom_reports_received = max(1, len(self.qcom_reports))
self.qcom_reports = [gnss_msg]
self.last_report_time = report_time
else:
# Sometimes DR reports get sent one iteration late (1second), they need to be ignored
cloudlog.warning(f"Received report with time {report_time} before last report time {self.last_report_time}")
if len(self.qcom_reports) == self.qcom_reports_received:
new_meas = get_measurements_from_qcom_reports(self.qcom_reports)
else:
new_meas = []
else:
report = gnss_msg.measurementReport
self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow)
new_meas = read_raw_ublox(report)
return self.last_report_time, new_meas
def is_ephemeris(self, gnss_msg):
if self.use_qcom:
return gnss_msg.which() == 'drSvPoly'
else:
return gnss_msg.which() in ('ephemeris', 'glonassEphemeris')
def read_ephemeris(self, gnss_msg):
if self.use_qcom:
try:
ephem = parse_qcom_ephem(gnss_msg.drSvPoly)
self.astro_dog.add_qcom_polys({ephem.prn: [ephem]})
except Exception:
cloudlog.exception("Error parsing qcom svPoly ephemeris from qcom module")
return
else:
if gnss_msg.which() == 'ephemeris':
data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
try:
ephem = GPSEphemeris(data_struct, file_name='ublox')
except Exception:
cloudlog.exception("Error parsing GPS ephemeris from ublox")
return
elif gnss_msg.which() == 'glonassEphemeris':
data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
try:
ephem = GLONASSEphemeris(data_struct, file_name='ublox')
except Exception:
cloudlog.exception("Error parsing GLONASS ephemeris from ublox")
return
else:
cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
return
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris()
def process_report(self, new_meas, t):
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
processed_measurements = process_measurements(new_meas, self.astro_dog)
if self.last_fix_pos is not None:
est_pos = self.last_fix_pos
correct_delay = True
else:
est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
correct_delay = False
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog, correct_delay=correct_delay)
# If many measurements weren't corrected, position may be garbage, so reset
if len(processed_measurements) >= 8 and len(corrected_measurements) < 5:
cloudlog.error("Didn't correct enough measurements, resetting estimate position")
self.last_fix_pos = None
self.last_fix_t = None
return corrected_measurements
def calc_fix(self, t, measurements):
instant_fix = self.get_lsq_fix(t, measurements)
if instant_fix is None:
return None
else:
position_estimate, position_std, velocity_estimate, velocity_std = instant_fix
self.last_fix_t = t
self.last_fix_pos = position_estimate
self.lat_fix_pos_std = position_std
return position_estimate, position_std, velocity_estimate, velocity_std
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
out_msg = messaging.new_message("gnssMeasurements")
t = gnss_mono_time * 1e-9
msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)}
if self.first_log_time is None:
self.first_log_time = 1e-9 * gnss_mono_time
if self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
elif self.is_good_report(gnss_msg):
report_t, new_meas = self.read_report(gnss_msg)
if report_t.week > 0:
if self.auto_fetch_navs:
self.fetch_navs(report_t, block)
corrected_measurements = self.process_report(new_meas, t)
msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
fix = self.calc_fix(t, corrected_measurements)
measurement_msg = log.LiveLocationKalman.Measurement.new_message
if fix is not None:
position_estimate, position_std, velocity_estimate, velocity_std = fix
if self.ttff <= 0:
self.ttff = max(1e-3, t - self.first_log_time)
msg_dict["positionECEF"] = measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t))
msg_dict["velocityECEF"] = measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t))
self.update_localizer(self.last_fix_pos, t, corrected_measurements)
P_diag = self.gnss_kf.P.diagonal()
kf_valid = all(self.kf_valid(t))
msg_dict["kalmanPositionECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(),
valid=kf_valid)
msg_dict["kalmanVelocityECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(),
valid=kf_valid)
msg_dict['gpsWeek'] = self.last_report_time.week
msg_dict['gpsTimeOfWeek'] = self.last_report_time.tow
msg_dict['timeToFirstFix'] = self.ttff
msg_dict['ephemerisStatuses'] = self.create_ephem_statuses()
out_msg.gnssMeasurements = msg_dict
return out_msg
def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
valid = self.kf_valid(t)
if not all(valid):
if not valid[0]: # Filter not initialized
pass
elif not valid[1]:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
if est_pos is not None and len(est_pos) > 0:
cloudlog.info(f"Reset kalman filter with {est_pos}")
self.init_gnss_localizer(est_pos)
else:
return
if len(measurements) > 0:
kf_add_observations(self.gnss_kf, t, measurements)
else:
# Ensure gnss filter is updated even with no new measurements
self.gnss_kf.predict(t)
def kf_valid(self, t: float) -> List[bool]:
filter_time = self.gnss_kf.filter.get_filter_time()
return [not math.isnan(filter_time),
abs(t - filter_time) < MAX_TIME_GAP,
all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))]
def init_gnss_localizer(self, est_pos):
x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))
x_initial[GStates.ECEF_POS] = est_pos
p_initial_diag[GStates.ECEF_POS] = 1000 ** 2
self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)
def fetch_navs(self, t: GPSTime, block):
# Download new navs if 1 hour of navs data left
if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
ret = None
if block: # Used for testing purposes
ret = get_orbit_data(t, *astro_dog_vars)
elif self.orbit_fetch_future is None:
self.orbit_fetch_executor = ProcessPoolExecutor(max_workers=1)
self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars)
elif self.orbit_fetch_future.done():
ret = self.orbit_fetch_future.result()
self.orbit_fetch_executor = self.orbit_fetch_future = None
if ret is not None:
if ret[0] is None:
self.last_fetch_navs_t = ret[2]
else:
self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret
self.cache_ephemeris()
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir)
cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}")
start_time = time.monotonic()
try:
astro_dog.get_navs(t)
cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s")
cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" +
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}")
return astro_dog.navs, astro_dog.navs_fetched_times, t
except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
cloudlog.warning(f"No orbit data found or parsing failure: {e}")
return None, None, t
def create_measurement_msg(meas: GNSSMeasurement):
c = log.GnssMeasurements.CorrectedMeasurement.new_message()
c.constellationId = meas.constellation_id.value
c.svId = meas.sv_id
c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0
c.pseudorange = float(meas.observables_final['C1C'])
c.pseudorangeStd = float(meas.observables_std['C1C'])
c.pseudorangeRate = float(meas.observables_final['D1C'])
c.pseudorangeRateStd = float(meas.observables_std['D1C'])
c.satPos = meas.sat_pos_final.tolist()
c.satVel = meas.sat_vel.tolist()
c.satVel = meas.sat_vel.tolist()
return c
def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]):
ekf_data = defaultdict(list)
for m in measurements:
m_arr = m.as_array()
if m.constellation_id == ConstellationId.GPS:
ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr)
elif m.constellation_id == ConstellationId.GLONASS:
ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr)
ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS]
ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS]
for kind, data in ekf_data.items():
if len(data) > 0:
gnss_kf.predict_and_observe(t, kind, data)
def clear_tmp_cache():
if os.path.exists(Paths.download_cache_root()):
shutil.rmtree(Paths.download_cache_root())
os.mkdir(Paths.download_cache_root())
def main():
#clear_tmp_cache()
use_qcom = not Params().get_bool("UbloxAvailable")
if use_qcom:
raw_name = "qcomGnss"
else:
raw_name = "ubloxGnss"
raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False)
pm = messaging.PubMaster(['gnssMeasurements'])
# disable until set as main gps source, to better analyze startup time
# TODO ensure low CPU usage before enabling
use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ
replay = "REPLAY" in os.environ
laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
while True:
for in_msg in messaging.drain_sock(raw_gnss_sock, wait_for_one=True):
out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay)
pm.send('gnssMeasurements', out_msg)
if __name__ == "__main__":
main()
selfdrive/locationd/test/test_laikad.py
-319
View File
@@ -1,319 +0,0 @@
#!/usr/bin/env python3
import pytest
import time
import unittest
from cereal import log
from openpilot.common.params import Params
from datetime import datetime
from unittest import mock
from laika.constants import SECS_IN_DAY
from laika.downloader import DownloadFailed
from laika.ephemeris import EphemerisType
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, read_raw_ublox, read_raw_qcom
from openpilot.selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad
from openpilot.selfdrive.test.openpilotci import get_url
from openpilot.tools.lib.logreader import LogReader
from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process
GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC = GPSTime.from_datetime(datetime(2022, month=1, day=29, hour=12))
UBLOX_TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
QCOM_TEST_ROUTE = "616dc83ca1f7f11e|2023-07-11--10-52-31"
def get_log_ublox():
logs = LogReader(get_url(UBLOX_TEST_ROUTE, 0))
ublox_cfg = get_process_config("ubloxd")
all_logs = replay_process(ublox_cfg, logs)
low_gnss = []
for m in all_logs:
if m.which() != "ubloxGnss" or m.ubloxGnss.which() != 'measurementReport':
continue
MAX_MEAS = 7
if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS:
mb = log.Event.new_message(ubloxGnss=m.ubloxGnss.to_dict())
mb.logMonoTime = m.logMonoTime
mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS
mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS]
mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000
low_gnss.append(mb.as_reader())
else:
low_gnss.append(m)
return all_logs, low_gnss
def get_log_qcom():
logs = LogReader(get_url(QCOM_TEST_ROUTE, 0))
all_logs = [m for m in logs if m.which() == 'qcomGnss']
return all_logs
def verify_messages(lr, laikad, return_one_success=False):
good_msgs = []
for m in lr:
if m.which() == 'ubloxGnss':
gnss_msg = m.ubloxGnss
elif m.which() == 'qcomGnss':
gnss_msg = m.qcomGnss
else:
continue
msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=True)
if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
good_msgs.append(msg)
if return_one_success:
return msg
return good_msgs
def get_first_gps_time(logs):
for m in logs:
if m.which() == 'ubloxGnss':
if m.ubloxGnss.which == 'measurementReport':
new_meas = read_raw_ublox(m.ubloxGnss.measurementReport)
if len(new_meas) > 0:
return new_meas[0].recv_time
elif m.which() == "qcomGnss":
if m.qcomGnss.which == 'measurementReport':
new_meas = read_raw_qcom(m.qcomGnss.measurementReport)
if len(new_meas) > 0:
return new_meas[0].recv_time
def get_measurement_mock(gpstime, sat_ephemeris):
meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.})
# Fake measurement being processed
meas.observables_final = meas.observables
meas.sat_ephemeris = sat_ephemeris
return meas
@pytest.mark.slow
class TestLaikad(unittest.TestCase):
@classmethod
def setUpClass(cls):
logs, low_gnss = get_log_ublox()
cls.logs = logs
cls.low_gnss = low_gnss
cls.logs_qcom = get_log_qcom()
first_gps_time = get_first_gps_time(logs)
cls.first_gps_time = first_gps_time
def setUp(self):
Params().remove(EPHEMERIS_CACHE)
def test_fetch_navs_non_blocking(self):
gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad()
laikad.fetch_navs(gpstime, block=False)
laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad:
laikad.fetch_navs(gpstime, block=False)
ephem = laikad.astro_dog.navs['G01'][0]
self.assertIsNotNone(ephem)
laikad.fetch_navs(gpstime+2*SECS_IN_DAY, block=False)
laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad:
laikad.fetch_navs(gpstime + 2 * SECS_IN_DAY, block=False)
ephem2 = laikad.astro_dog.navs['G01'][0]
self.assertIsNotNone(ephem)
self.assertNotEqual(ephem, ephem2)
def test_fetch_navs_with_wrong_clocks(self):
laikad = Laikad()
def check_has_navs():
self.assertGreater(len(laikad.astro_dog.navs), 0)
ephem = laikad.astro_dog.navs['G01'][0]
self.assertIsNotNone(ephem)
real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1))
wrong_future_clock_time = real_current_time + SECS_IN_DAY
laikad.fetch_navs(wrong_future_clock_time, block=True)
check_has_navs()
self.assertEqual(laikad.last_fetch_navs_t, wrong_future_clock_time)
# Test fetching orbits with earlier time
assert real_current_time < laikad.last_fetch_navs_t
laikad.astro_dog.orbits = {}
laikad.fetch_navs(real_current_time, block=True)
check_has_navs()
self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
def test_laika_online(self):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
correct_msgs = verify_messages(self.logs, laikad)
correct_msgs_expected = 560
self.assertEqual(correct_msgs_expected, len(correct_msgs))
self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_kf_becomes_valid(self):
laikad = Laikad(auto_update=False)
m = self.logs[0]
self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9)))
kf_valid = False
for m in self.logs:
if m.which() != 'ubloxGnss':
continue
laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9))
if kf_valid:
break
self.assertTrue(kf_valid)
def test_laika_online_nav_only(self):
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom)
# Disable fetch_orbits to test NAV only
correct_msgs = verify_messages(logs, laikad)
correct_msgs_expected = 55 if use_qcom else 560
valid_fix_expected = 55 if use_qcom else 560
self.assertEqual(correct_msgs_expected, len(correct_msgs))
self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
@mock.patch('laika.downloader.download_and_cache_file')
def test_laika_offline(self, downloader_mock):
downloader_mock.side_effect = DownloadFailed("Mock download failed")
laikad = Laikad(auto_update=False)
laikad.fetch_navs(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True)
@mock.patch('laika.downloader.download_and_cache_file')
def test_download_failed_russian_source(self, downloader_mock):
downloader_mock.side_effect = DownloadFailed
laikad = Laikad(auto_update=False)
correct_msgs = verify_messages(self.logs, laikad)
expected_msgs = 376
self.assertEqual(expected_msgs, len(correct_msgs))
self.assertEqual(expected_msgs, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_laika_get_orbits(self):
laikad = Laikad(auto_update=False)
# Pretend process has loaded the orbits on startup by using the time of the first gps message.
laikad.fetch_navs(self.first_gps_time, block=True)
self.dict_has_values(laikad.astro_dog.navs)
@unittest.skip("Use to debug live data")
def test_laika_get_navs_now(self):
laikad = Laikad(auto_update=False)
laikad.fetch_navs(GPSTime.from_datetime(datetime.utcnow()), block=True)
prn = "G01"
self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
prn = "R01"
self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
def test_get_navs_in_process(self):
for auto_fetch_navs in [True, False]:
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=auto_fetch_navs)
has_navs = False
has_fix = False
seen_chip_eph = False
seen_internet_eph = False
for m in logs:
if m.which() != 'ubloxGnss' and m.which() != 'qcomGnss':
continue
gnss_msg = m.qcomGnss if use_qcom else m.ubloxGnss
out_msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=False)
if laikad.orbit_fetch_future is not None:
laikad.orbit_fetch_future.result()
vals = laikad.astro_dog.navs.values()
has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0
vals = laikad.astro_dog.qcom_polys.values()
has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0
has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid
if len(out_msg.gnssMeasurements.ephemerisStatuses):
seen_chip_eph = seen_chip_eph or any(x.source == 'gnssChip' for x in out_msg.gnssMeasurements.ephemerisStatuses)
seen_internet_eph = seen_internet_eph or any(x.source == 'internet' for x in out_msg.gnssMeasurements.ephemerisStatuses)
self.assertTrue(has_navs or has_polys)
self.assertTrue(has_fix)
self.assertTrue(seen_chip_eph or auto_fetch_navs)
self.assertTrue(seen_internet_eph or not auto_fetch_navs)
self.assertEqual(len(laikad.astro_dog.navs_fetched_times._ranges), 0)
self.assertEqual(None, laikad.orbit_fetch_future)
def test_cache(self):
use_qcom = True
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs], strict=True):
Params().remove(EPHEMERIS_CACHE)
laikad = Laikad(auto_update=True, save_ephemeris=True, use_qcom=use_qcom)
def wait_for_cache():
max_time = 2
while Params().get(EPHEMERIS_CACHE) is None:
time.sleep(0.1)
max_time -= 0.1
if max_time < 0:
self.fail("Cache has not been written after 2 seconds")
# Test cache with no ephemeris
laikad.last_report_time = GPSTime(1,0)
laikad.cache_ephemeris()
if Params().get(EPHEMERIS_CACHE) is not None:
self.fail("Cache should not have been written without valid ephem")
#laikad.astro_dog.get_navs(self.first_gps_time)
msg = verify_messages(logs, laikad, return_one_success=True)
laikad.cache_ephemeris()
wait_for_cache()
# Check both nav and orbits separate
laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV,
save_ephemeris=True, use_qcom=use_qcom, auto_fetch_navs=False)
# Verify navs are loaded from cache
self.dict_has_values(laikad.astro_dog.navs)
# Verify cache is working for only nav by running a segment
msg = verify_messages(logs, laikad, return_one_success=True)
self.assertTrue(len(msg.gnssMeasurements.ephemerisStatuses))
self.assertTrue(any(x.source=='cache' for x in msg.gnssMeasurements.ephemerisStatuses))
self.assertIsNotNone(msg)
#TODO test cache with only orbits
#with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method:
# # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently
# laikad.astro_dog.orbit_fetched_times = TimeRangeHolder()
# laikad.fetch_navs(self.first_gps_time, block=False)
# mock_method.assert_not_called()
# # Verify cache is working for only orbits by running a segment
# laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT, save_ephemeris=True)
# msg = verify_messages(self.logs, laikad, return_one_success=True)
# self.assertIsNotNone(msg)
# # Verify orbit data is not downloaded
# mock_method.assert_not_called()
#break
def test_low_gnss_meas(self):
cnt = 0
laikad = Laikad()
for m in self.low_gnss:
msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
if msg is None:
continue
gm = msg.gnssMeasurements
if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid:
cnt += 1
self.assertEqual(cnt, 560)
def dict_has_values(self, dct):
self.assertGreater(len(dct), 0)
self.assertGreater(min([len(v) for v in dct.values()]), 0)
if __name__ == "__main__":
unittest.main()
selfdrive/manager/gitlab_runner.sh
Executable
+40
View File
@@ -0,0 +1,40 @@
#!/bin/bash
# Define the service name
SERVICE_NAME="gitlab-runner"
# Function to control the service
control_service() {
local action=$1 # Store the function argument in a local variable
sudo systemctl $action ${SERVICE_NAME}
}
service_exists_and_is_loaded() {
sudo systemctl status ${SERVICE_NAME} &>/dev/null
if [[ $? -ne 4 ]]; then
return 0 # Service is known to systemd (i.e., loaded)
else
return 1 # Service is unknown to systemd (i.e., not loaded)
fi
}
# Check for required argument
if [[ -z $1 ]] || { [[ $1 != "start" ]] && [[ $1 != "stop" ]]; }; then
echo "Usage: $0 {start|stop}"
exit 1
fi
# Store the script argument in a descriptive variable
ACTION=$1
# Trap EXIT signal (Ctrl+C) and stop the service
trap 'control_service stop ; exit' SIGINT SIGKILL EXIT
# Enter the main loop
while true; do
# Check if the service is actually present on the system
if service_exists_and_is_loaded; then
control_service $ACTION # Call the function with the specified action
fi
sleep 1 # Pause before the next iteration
done
selfdrive/manager/manager.py
+1 -3
View File
@@ -65,8 +65,7 @@ def manager_init() -> None:
("MadsCruiseMain", "1"),
("CustomBootScreen", "0"),
("CustomOffsets", "0"),
("DevUI", "1"),
("DevUIInfo", "1"),
("DevUIInfo", "0"),
("DisableOnroadUploads", "0"),
("DisengageLateralOnBrake", "0"),
("DynamicLaneProfile", "1"),
@@ -88,7 +87,6 @@ def manager_init() -> None:
("ScreenRecorder", "1"),
("ShowDebugUI", "1"),
("SpeedLimitControl", "1"),
("SpeedLimitPercOffset", "1"),
("SpeedLimitValueOffset", "0"),
("SpeedLimitOffsetType", "0"),
("StandStillTimer", "0"),
selfdrive/manager/process.py
+3 -3
View File
@@ -89,7 +89,7 @@ class ManagerProcess(ABC):
self.stop(sig=signal.SIGKILL)
self.start()
def check_watchdog(self, started: bool, params: Params) -> None:
def check_watchdog(self, started: bool) -> None:
if self.watchdog_max_dt is None or self.proc is None:
return
@@ -103,7 +103,7 @@ class ManagerProcess(ABC):
dt = time.monotonic() - self.last_watchdog_time / 1e9
always_watchdog = self.always_watchdog and params.get_bool("IsOffroad") and self.proc.exitcode is not None
always_watchdog = self.always_watchdog and not started and self.proc.exitcode is not None
if dt > self.watchdog_max_dt:
if (self.watchdog_seen or always_watchdog) and ENABLE_WATCHDOG:
@@ -291,6 +291,6 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None
else:
p.stop(block=False)
p.check_watchdog(started, params)
p.check_watchdog(started)
return running
selfdrive/manager/process_config.py
+8 -5
View File
@@ -45,7 +45,6 @@ procs = [
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
NativeProcess("camerad", "system/camerad", ["./camerad"], driverview),
NativeProcess("clocksd", "system/clocksd", ["./clocksd"], only_onroad),
NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad),
NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad),
PythonProcess("logmessaged", "system.logmessaged", always_run),
@@ -68,7 +67,6 @@ procs = [
PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
PythonProcess("deleter", "system.loggerd.deleter", always_run),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
PythonProcess("laikad", "selfdrive.locationd.laikad", only_onroad),
PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", qcomgps, enabled=TICI),
PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),
@@ -84,15 +82,20 @@ procs = [
PythonProcess("statsd", "selfdrive.statsd", always_run),
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None), always_watchdog=True),
PythonProcess("gpxd", "selfdrive.gpxd.gpxd", only_onroad),
PythonProcess("gpxd_uploader", "selfdrive.gpxd.gpx_uploader", only_offroad),
PythonProcess("mapd", "selfdrive.mapd.mapd", only_onroad),
PythonProcess("otisserv", "selfdrive.navd.otisserv", always_run),
PythonProcess("fleet_manager", "system.fleetmanager.fleet_manager", only_offroad),
PythonProcess("fleet_manager", "system.fleetmanager.fleet_manager", always_run),
# debug procs
NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
]
if os.path.exists("./gitlab_runner.sh") and True: # Of course and True is always true. Placeholder for a param :D
# Only devs!
procs += [
NativeProcess("gitlab_runner_start", "selfdrive/manager", ["./gitlab_runner.sh", "start"], only_offroad, sigkill=False),
NativeProcess("gitlab_runner_stop", "selfdrive/manager", ["./gitlab_runner.sh", "stop"], only_onroad, sigkill=False)
]
managed_processes = {p.name: p for p in procs}
selfdrive/mapd/mapd.py
+16 -36
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
import json
import math
import threading
from traceback import print_exception
import numpy as np
@@ -37,16 +36,6 @@ def excepthook(args):
threading.excepthook = excepthook
class GPSData:
def __init__(self, unixTimestampMillis, latitude, longitude, speed, bearingDeg, accuracy, bearingAccuracyDeg):
self.unixTimestampMillis = unixTimestampMillis
self.latitude = latitude
self.longitude = longitude
self.speed = speed
self.bearingDeg = bearingDeg
self.accuracy = accuracy
self.bearingAccuracyDeg = bearingAccuracyDeg
class MapD():
def __init__(self):
@@ -68,6 +57,7 @@ class MapD():
self._disengaging = False
self._query_thread = None
self._lock = threading.RLock()
self.gps_sock = 'gpsLocationExternal'
# dp - use LastGPSPosition as init position (if we are in a undercover car park?)
# this way we can prefetch osm data before we get a fix.
@@ -91,37 +81,27 @@ class MapD():
self._op_enabled = hud_control.speedVisible
def update_gps(self, sm):
sock = 'liveLocationKalman'
if not sm.updated[sock] or not sm.valid[sock]:
self.gps_sock = 'gpsLocationExternal' if sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
if not sm.updated[self.gps_sock] or not sm.valid[self.gps_sock]:
return
log = sm[sock]
log = sm[self.gps_sock]
self.last_gps = log
# ignore the message if the fix is invalid
if not log.positionGeodetic.valid:
if log.flags % 2 == 0:
return
self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
self.location_rad = np.radians(np.array([log.positionGeodetic.value[0], log.positionGeodetic.value[1]], dtype=float))
self.location_deg = (log.positionGeodetic.value[0], log.positionGeodetic.value[1])
self.bearing_rad = math.degrees(log.calibratedOrientationNED.value[2])
self.gps_speed = log.velocityCalibrated.value[2]
self.location_stdev = 1 # hardcoding, this is not available in liveLocationKalman
self.last_gps = GPSData(
unixTimestampMillis=self.last_gps_fix_timestamp,
latitude=self.location_deg[0],
longitude=self.location_deg[1],
speed=self.gps_speed,
bearingDeg=self.bearing_rad,
accuracy=self.location_stdev, # assuming this is equivalent information
bearingAccuracyDeg=1. # you'll need to assign this if available
)
self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
self.location_deg = (log.latitude, log.longitude)
self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
self.gps_speed = log.speed
self.location_stdev = log.accuracy if self.gps_sock == 'gpsLocationExternal' else 1 # gpsLocation doesn't report accuracy
_debug('Mapd: ********* Got GPS fix'
+ f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n'
+ f'Bearing: {log.calibratedOrientationNED.value[2]} +/- {1. * 2.} deg.\n'
+ f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n'
+ f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}'
+ '*******', log_to_cloud=False)
@@ -242,8 +222,8 @@ class MapD():
current_road_name = self.route.current_road_name
map_data_msg = messaging.new_message('liveMapDataSP')
map_data_msg.valid = sm.all_alive(service_list=['liveLocationKalman']) and \
sm.all_valid(service_list=['liveLocationKalman'])
map_data_msg.valid = sm.all_alive(service_list=[self.gps_sock]) and \
sm.all_valid(service_list=[self.gps_sock])
liveMapDataSP = map_data_msg.liveMapDataSP
liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis
@@ -251,7 +231,7 @@ class MapD():
liveMapDataSP.lastGpsLongitude = float(self.last_gps.longitude)
liveMapDataSP.lastGpsSpeed = float(self.last_gps.speed)
liveMapDataSP.lastGpsBearingDeg = float(self.last_gps.bearingDeg)
liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy)
liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy if self.gps_sock == 'gpsLocationExternal' else 1) # gpsLocation doesnt report accuracy
liveMapDataSP.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg)
liveMapDataSP.speedLimitValid = bool(speed_limit is not None)
@@ -292,7 +272,7 @@ def mapd_thread(sm=None, pm=None):
# *** setup messaging
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carControl'])
sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'carControl'])
if pm is None:
pm = messaging.PubMaster(['liveMapDataSP'])
selfdrive/mapd/sp_osm_db.json
+5
View File
@@ -44,6 +44,11 @@
"waypoint": "2729449",
"url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQzbZeus-yBNsgP1KQyLn9wBYZVMMknVoHdB7_ewulLbdA?e=DrnKkb&download=1"
},
"Spain": {
"name": "spain",
"waypoint": "4263034",
"url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/Ee21EfLqZxxLjksDCB_2dscBXKN9Kch8ZOYz3cIWnVpBTg?e=LWBJ1n&download=1"
},
"Taiwan": {
"name": "taiwan",
"waypoint": "198637969",
selfdrive/modeld/SConscript
+6 -4
View File
@@ -45,17 +45,19 @@ snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else sn
cython_libs = envCython["LIBS"] + libs
snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc'])
commonmodel_lib = lenv.Library('commonmodel', common_src)
driving_lib = lenv.Library('driving', ['models/driving.cc'])
lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath)
lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
lenvCython.Program('models/driving_pyx.so', 'models/driving_pyx.pyx', LIBS=[driving_lib, commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
# Get model metadata
fn = File("models/supercombo").abspath
cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.endswith("get_model_metadata.py")], [])
lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"]+files, cmd)
# Build thneed model
if arch == "larch64" or GetOption('pc_thneed'):
fn = File("models/supercombo").abspath
tinygrad_opts = ["NOLOCALS=1", "IMAGE=2", "GPU=1"]
if not GetOption('pc_thneed'):
# use FLOAT16 on device for speed + don't cache the CL kernels for space
selfdrive/modeld/constants.py
+75 -2
View File
@@ -1,7 +1,80 @@
IDX_N = 33
import numpy as np
def index_function(idx, max_val=192, max_idx=32):
return (max_val) * ((idx/max_idx)**2)
class ModelConstants:
# time and distance indices
IDX_N = 33
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
LEAD_T_OFFSETS = [0., 2., 4.]
META_T_IDXS = [2., 4., 6., 8., 10.]
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
# model inputs constants
MODEL_FREQ = 20
FEATURE_LEN = 512
HISTORY_BUFFER_LEN = 99
DESIRE_LEN = 8
TRAFFIC_CONVENTION_LEN = 2
NAV_FEATURE_LEN = 256
NAV_INSTRUCTION_LEN = 150
DRIVING_STYLE_LEN = 12
# model outputs constants
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
FCW_5MS2_PROBS_WIDTH = 5
FCW_3MS2_PROBS_WIDTH = 2
DISENGAGE_WIDTH = 5
POSE_WIDTH = 6
WIDE_FROM_DEVICE_WIDTH = 3
SIM_POSE_WIDTH = 6
LEAD_WIDTH = 4
LANE_LINES_WIDTH = 2
ROAD_EDGES_WIDTH = 2
PLAN_WIDTH = 15
DESIRE_PRED_WIDTH = 8
NUM_LANE_LINES = 4
NUM_ROAD_EDGES = 2
LEAD_TRAJ_LEN = 6
DESIRE_PRED_LEN = 4
PLAN_MHP_N = 5
LEAD_MHP_N = 2
PLAN_MHP_SELECTION = 1
LEAD_MHP_SELECTION = 3
FCW_THRESHOLD_5MS2_HIGH = 0.15
FCW_THRESHOLD_5MS2_LOW = 0.05
FCW_THRESHOLD_3MS2 = 0.7
CONFIDENCE_BUFFER_LEN = 5
RYG_GREEN = 0.01165
RYG_YELLOW = 0.06157
# model outputs slices
class Plan:
POSITION = slice(0, 3)
VELOCITY = slice(3, 6)
ACCELERATION = slice(6, 9)
T_FROM_CURRENT_EULER = slice(9, 12)
ORIENTATION_RATE = slice(12, 15)
class Meta:
ENGAGED = slice(0, 1)
# next 2, 4, 6, 8, 10 seconds
GAS_DISENGAGE = slice(1, 36, 7)
BRAKE_DISENGAGE = slice(2, 36, 7)
STEER_OVERRIDE = slice(3, 36, 7)
HARD_BRAKE_3 = slice(4, 36, 7)
HARD_BRAKE_4 = slice(5, 36, 7)
HARD_BRAKE_5 = slice(6, 36, 7)
GAS_PRESS = slice(7, 36, 7)
# next 0, 2, 4, 6, 8, 10 seconds
LEFT_BLINKER = slice(36, 48, 2)
RIGHT_BLINKER = slice(37, 48, 2)
selfdrive/modeld/fill_model_msg.py
+188
View File
@@ -0,0 +1,188 @@
import os
import capnp
import numpy as np
from typing import Dict
from cereal import log
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
ConfidenceClass = log.ModelDataV2.ConfidenceClass
class PublishState:
def __init__(self):
self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
builder.t = t
builder.x = x.tolist()
builder.y = y.tolist()
builder.z = z.tolist()
if x_std is not None:
builder.xStd = x_std.tolist()
if y_std is not None:
builder.yStd = y_std.tolist()
if z_std is not None:
builder.zStd = z_std.tolist()
def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
builder.t = t
builder.x = x.tolist()
builder.y = y.tolist()
builder.v = v.tolist()
builder.a = a.tolist()
if x_std is not None:
builder.xStd = x_std.tolist()
if y_std is not None:
builder.yStd = y_std.tolist()
if v_std is not None:
builder.vStd = v_std.tolist()
if a_std is not None:
builder.aStd = a_std.tolist()
def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState,
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
nav_enabled: bool, valid: bool) -> None:
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
msg.valid = valid
modelV2 = msg.modelV2
modelV2.frameId = vipc_frame_id
modelV2.frameIdExtra = vipc_frame_id_extra
modelV2.frameAge = frame_age
modelV2.frameDropPerc = frame_drop * 100
modelV2.timestampEof = timestamp_eof
modelV2.locationMonoTime = timestamp_llk
modelV2.modelExecutionTime = model_execution_time
modelV2.navEnabled = nav_enabled
# plan
position = modelV2.position
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
velocity = modelV2.velocity
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
acceleration = modelV2.acceleration
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
orientation = modelV2.orientation
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
orientation_rate = modelV2.orientationRate
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# times at X_IDXS according to model plan
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
PLAN_T_IDXS[0] = 0.0
plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
for xidx in range(1, ModelConstants.IDX_N):
tidx = 0
# increment tidx until we find an element that's further away than the current xidx
while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]:
tidx += 1
if tidx == ModelConstants.IDX_N - 1:
# if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
break
# interpolate to find `t` for the current xidx
current_x_val = plan_x[tidx]
next_x_val = plan_x[tidx+1]
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val)
PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
# lane lines
modelV2.init('laneLines', 4)
for i in range(4):
lane_line = modelV2.laneLines[i]
fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
# road edges
modelV2.init('roadEdges', 2)
for i in range(2):
road_edge = modelV2.roadEdges[i]
fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
# leads
modelV2.init('leadsV3', 3)
for i in range(3):
lead = modelV2.leadsV3[i]
fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
lead.prob = net_output_data['lead_prob'][0,i].tolist()
lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
# meta
meta = modelV2.meta
meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
meta.init('disengagePredictions')
disengage_predictions = meta.disengagePredictions
disengage_predictions.t = ModelConstants.META_T_IDXS
disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
meta.hardBrakePredicted = hard_brake_predicted.item()
# temporal pose
temporal_pose = modelV2.temporalPose
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
# confidence
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
# any disengage prob
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
# independent disengage prob for each 2s slice
ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
# rolling buf for 2, 4, 6, 8, 10s
publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
score = 0.
for i in range(ModelConstants.DISENGAGE_WIDTH):
score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
if score < ModelConstants.RYG_GREEN:
modelV2.confidence = ConfidenceClass.green
elif score < ModelConstants.RYG_YELLOW:
modelV2.confidence = ConfidenceClass.yellow
else:
modelV2.confidence = ConfidenceClass.red
# raw prediction if enabled
if SEND_RAW_PRED:
modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray],
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
cameraOdometry = msg.cameraOdometry
cameraOdometry.frameId = vipc_frame_id
cameraOdometry.timestampEof = timestamp_eof
cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()
selfdrive/modeld/get_model_metadata.py
Executable
+29
View File
@@ -0,0 +1,29 @@
#!/usr/bin/env python3
import sys
import pathlib
import onnx
import codecs
import pickle
from typing import Tuple
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]:
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
name = value_info.name
return name, shape
if __name__ == "__main__":
model_path = pathlib.Path(sys.argv[1])
model = onnx.load(str(model_path))
i = [x.key for x in model.metadata_props].index('output_slices')
output_slices = model.metadata_props[i].value
metadata = {}
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
with open(metadata_path, 'wb') as f:
pickle.dump(metadata, f)
print(f'saved metadata to {metadata_path}')
selfdrive/modeld/modeld.py
+52 -30
View File
@@ -1,7 +1,10 @@
#!/usr/bin/env python3
import sys
import os
import time
import pickle
import numpy as np
import cereal.messaging as messaging
from pathlib import Path
from typing import Dict, Optional
from setproctitle import setproctitle
@@ -13,16 +16,19 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import config_realtime_process
from openpilot.common.transformations.model import get_warp_matrix
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
from openpilot.selfdrive.modeld.models.driving_pyx import (
PublishState, create_model_msg, create_pose_msg,
FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN,
OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
MODEL_PATHS = {
ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
class FrameMeta:
frame_id: int = 0
timestamp_sof: int = 0
@@ -43,28 +49,41 @@ class ModelState:
def __init__(self, context: CLContext):
self.frame = ModelFrame(context)
self.wide_frame = ModelFrame(context)
self.prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
self.output = np.zeros(NET_OUTPUT_SIZE, dtype=np.float32)
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.inputs = {
'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32),
'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32),
'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32),
'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32),
'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32),
'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
}
with open(METADATA_PATH, 'rb') as f:
model_metadata = pickle.load(f)
self.output_slices = model_metadata['output_slices']
net_output_size = model_metadata['output_shapes']['outputs'][1]
self.output = np.zeros(net_output_size, dtype=np.float32)
self.parser = Parser()
self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context)
self.model.addInput("input_imgs", None)
self.model.addInput("big_input_imgs", None)
for k,v in self.inputs.items():
self.model.addInput(k, v)
def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]:
parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
if SEND_RAW_PRED:
parsed_model_outputs['raw_pred'] = model_outputs.copy()
return parsed_model_outputs
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[np.ndarray]:
inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]:
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
inputs['desire'][0] = 0
self.inputs['desire'][:-DESIRE_LEN] = self.inputs['desire'][DESIRE_LEN:]
self.inputs['desire'][-DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
self.prev_desire[:] = inputs['desire']
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
@@ -81,9 +100,11 @@ class ModelState:
return None
self.model.execute()
self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:]
self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN]
return self.output
outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
return outputs
def main():
@@ -122,22 +143,21 @@ def main():
pm = PubMaster(["modelV2", "cameraOdometry"])
sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
state = PublishState()
publish_state = PublishState()
params = Params()
# setup filter to track dropped frames
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / MODEL_FREQ)
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
frame_id = 0
last_vipc_frame_id = 0
run_count = 0
# last = 0.0
model_transform_main = np.zeros((3, 3), dtype=np.float32)
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
live_calib_seen = False
driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
nav_features = np.zeros(NAV_FEATURE_LEN, dtype=np.float32)
nav_instructions = np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32)
nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
buf_main, buf_extra = None, None
meta_main = FrameMeta()
meta_extra = FrameMeta()
@@ -190,8 +210,8 @@ def main():
traffic_convention = np.zeros(2)
traffic_convention[int(is_rhd)] = 1
vec_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
if desire >= 0 and desire < DESIRE_LEN:
vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
vec_desire[desire] = 1
# Enable/disable nav features
@@ -244,13 +264,15 @@ def main():
model_execution_time = mt2 - mt1
if model_output is not None:
pm.send("modelV2", create_model_msg(model_output, state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen))
pm.send("cameraOdometry", create_pose_msg(model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen))
modelv2_send = messaging.new_message('modelV2')
posenet_send = messaging.new_message('cameraOdometry')
fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
pm.send('modelV2', modelv2_send)
pm.send('cameraOdometry', posenet_send)
# print("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f" %
# ((mt2 - mt1)*1000, (mt1 - last)*1000, meta_extra.frame_id, frame_id, frame_drop_ratio))
# last = mt1
last_vipc_frame_id = meta_main.frame_id
selfdrive/modeld/models/README.md
+5 -1
View File
@@ -20,7 +20,11 @@ To view the architecture of the ONNX networks, you can use [netron](https://netr
* **traffic convention**
* one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
* **feature buffer**
* A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 128
* A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
* **nav features**
* 1 * 150
* **nav instructions**
* 1 * 256
### Supercombo output format (Full size: XXX x float32)
selfdrive/modeld/models/driving.cc
-330
View File
@@ -1,330 +0,0 @@
#include "selfdrive/modeld/models/driving.h"
#include <cstring>
void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) {
std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
const auto &best_prediction = leads.get_best_prediction(t_idx);
lead.setProb(sigmoid(leads.prob[t_idx]));
lead.setProbTime(prob_t);
std::array<float, LEAD_TRAJ_LEN> lead_x, lead_y, lead_v, lead_a;
std::array<float, LEAD_TRAJ_LEN> lead_x_std, lead_y_std, lead_v_std, lead_a_std;
for (int i=0; i<LEAD_TRAJ_LEN; i++) {
lead_x[i] = best_prediction.mean[i].x;
lead_y[i] = best_prediction.mean[i].y;
lead_v[i] = best_prediction.mean[i].velocity;
lead_a[i] = best_prediction.mean[i].acceleration;
lead_x_std[i] = exp(best_prediction.std[i].x);
lead_y_std[i] = exp(best_prediction.std[i].y);
lead_v_std[i] = exp(best_prediction.std[i].velocity);
lead_a_std[i] = exp(best_prediction.std[i].acceleration);
}
lead.setT(to_kj_array_ptr(lead_t));
lead.setX(to_kj_array_ptr(lead_x));
lead.setY(to_kj_array_ptr(lead_y));
lead.setV(to_kj_array_ptr(lead_v));
lead.setA(to_kj_array_ptr(lead_a));
lead.setXStd(to_kj_array_ptr(lead_x_std));
lead.setYStd(to_kj_array_ptr(lead_y_std));
lead.setVStd(to_kj_array_ptr(lead_v_std));
lead.setAStd(to_kj_array_ptr(lead_a_std));
}
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data, PublishState &ps) {
std::array<float, DESIRE_LEN> desire_state_softmax;
softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
std::array<float, DESIRE_PRED_LEN * DESIRE_LEN> desire_pred_softmax;
for (int i=0; i<DESIRE_PRED_LEN; i++) {
softmax(meta_data.desire_pred_prob[i].array.data(), desire_pred_softmax.data() + (i * DESIRE_LEN), DESIRE_LEN);
}
std::array<float, DISENGAGE_LEN> lat_long_t = {2, 4, 6, 8, 10};
std::array<float, DISENGAGE_LEN> gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid,
brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid;
for (int i=0; i<DISENGAGE_LEN; i++) {
gas_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_disengage);
brake_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_disengage);
steer_override_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].steer_override);
brake_3ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_3ms2);
brake_4ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_4ms2);
brake_5ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_5ms2);
//gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
}
std::memmove(ps.prev_brake_5ms2_probs.data(), &ps.prev_brake_5ms2_probs[1], 4*sizeof(float));
std::memmove(ps.prev_brake_3ms2_probs.data(), &ps.prev_brake_3ms2_probs[1], 2*sizeof(float));
ps.prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
ps.prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
bool above_fcw_threshold = true;
for (int i=0; i<ps.prev_brake_5ms2_probs.size(); i++) {
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_5ms2_probs[i] > threshold;
}
for (int i=0; i<ps.prev_brake_3ms2_probs.size(); i++) {
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
}
auto disengage = meta.initDisengagePredictions();
disengage.setT(to_kj_array_ptr(lat_long_t));
disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid));
disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid));
disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid));
disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid));
disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid));
disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid));
meta.setEngagedProb(sigmoid(meta_data.engaged_prob));
meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax));
meta.setDesireState(to_kj_array_ptr(desire_state_softmax));
meta.setHardBrakePredicted(above_fcw_threshold);
}
void fill_confidence(cereal::ModelDataV2::Builder &framed, PublishState &ps) {
if (framed.getFrameId() % (2*MODEL_FREQ) == 0) {
// update every 2s to match predictions interval
auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs();
auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs();
auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs();
float any_dp[DISENGAGE_LEN];
float dp_ind[DISENGAGE_LEN];
for (int i = 0; i < DISENGAGE_LEN; i++) {
any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob
}
dp_ind[0] = any_dp[0];
for (int i = 0; i < DISENGAGE_LEN-1; i++) {
dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice
}
// rolling buf for 2, 4, 6, 8, 10s
std::memmove(&ps.disengage_buffer[0], &ps.disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1));
std::memcpy(&ps.disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN);
}
float score = 0;
for (int i = 0; i < DISENGAGE_LEN; i++) {
score += ps.disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN;
}
if (score < RYG_GREEN) {
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN);
} else if (score < RYG_YELLOW) {
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW);
} else {
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED);
}
}
template<size_t size>
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
xyzt.setT(to_kj_array_ptr(t));
xyzt.setX(to_kj_array_ptr(x));
xyzt.setY(to_kj_array_ptr(y));
xyzt.setZ(to_kj_array_ptr(z));
}
template<size_t size>
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
fill_xyzt(xyzt, t, x, y, z);
xyzt.setXStd(to_kj_array_ptr(x_std));
xyzt.setYStd(to_kj_array_ptr(y_std));
xyzt.setZStd(to_kj_array_ptr(z_std));
}
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) {
std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
std::array<float, TRAJECTORY_SIZE> acc_x, acc_y, acc_z;
std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
for (int i=0; i<TRAJECTORY_SIZE; i++) {
pos_x[i] = plan.mean[i].position.x;
pos_y[i] = plan.mean[i].position.y;
pos_z[i] = plan.mean[i].position.z;
pos_x_std[i] = exp(plan.std[i].position.x);
pos_y_std[i] = exp(plan.std[i].position.y);
pos_z_std[i] = exp(plan.std[i].position.z);
vel_x[i] = plan.mean[i].velocity.x;
vel_y[i] = plan.mean[i].velocity.y;
vel_z[i] = plan.mean[i].velocity.z;
acc_x[i] = plan.mean[i].acceleration.x;
acc_y[i] = plan.mean[i].acceleration.y;
acc_z[i] = plan.mean[i].acceleration.z;
rot_x[i] = plan.mean[i].rotation.x;
rot_y[i] = plan.mean[i].rotation.y;
rot_z[i] = plan.mean[i].rotation.z;
rot_rate_x[i] = plan.mean[i].rotation_rate.x;
rot_rate_y[i] = plan.mean[i].rotation_rate.y;
rot_rate_z[i] = plan.mean[i].rotation_rate.z;
}
fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, acc_x, acc_y, acc_z);
fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
}
void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
const ModelOutputLaneLines &lanes) {
std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
for (int j=0; j<TRAJECTORY_SIZE; j++) {
left_far_y[j] = lanes.mean.left_far[j].y;
left_far_z[j] = lanes.mean.left_far[j].z;
left_near_y[j] = lanes.mean.left_near[j].y;
left_near_z[j] = lanes.mean.left_near[j].z;
right_near_y[j] = lanes.mean.right_near[j].y;
right_near_z[j] = lanes.mean.right_near[j].z;
right_far_y[j] = lanes.mean.right_far[j].y;
right_far_z[j] = lanes.mean.right_far[j].z;
}
auto lane_lines = framed.initLaneLines(4);
fill_xyzt(lane_lines[0], plan_t, X_IDXS_FLOAT, left_far_y, left_far_z);
fill_xyzt(lane_lines[1], plan_t, X_IDXS_FLOAT, left_near_y, left_near_z);
fill_xyzt(lane_lines[2], plan_t, X_IDXS_FLOAT, right_near_y, right_near_z);
fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z);
framed.setLaneLineStds({
exp(lanes.std.left_far[0].y),
exp(lanes.std.left_near[0].y),
exp(lanes.std.right_near[0].y),
exp(lanes.std.right_far[0].y),
});
framed.setLaneLineProbs({
sigmoid(lanes.prob.left_far.val),
sigmoid(lanes.prob.left_near.val),
sigmoid(lanes.prob.right_near.val),
sigmoid(lanes.prob.right_far.val),
});
}
void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
const ModelOutputRoadEdges &edges) {
std::array<float, TRAJECTORY_SIZE> left_y, left_z;
std::array<float, TRAJECTORY_SIZE> right_y, right_z;
for (int j=0; j<TRAJECTORY_SIZE; j++) {
left_y[j] = edges.mean.left[j].y;
left_z[j] = edges.mean.left[j].z;
right_y[j] = edges.mean.right[j].y;
right_z[j] = edges.mean.right[j].z;
}
auto road_edges = framed.initRoadEdges(2);
fill_xyzt(road_edges[0], plan_t, X_IDXS_FLOAT, left_y, left_z);
fill_xyzt(road_edges[1], plan_t, X_IDXS_FLOAT, right_y, right_z);
framed.setRoadEdgeStds({
exp(edges.std.left[0].y),
exp(edges.std.right[0].y),
});
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs, PublishState &ps) {
const auto &best_plan = net_outputs.plans.get_best_prediction();
std::array<float, TRAJECTORY_SIZE> plan_t;
std::fill_n(plan_t.data(), plan_t.size(), NAN);
plan_t[0] = 0.0;
for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
// increment tidx until we find an element that's further away than the current xidx
for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
tidx++;
}
if (tidx == TRAJECTORY_SIZE - 1) {
// if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
plan_t[xidx] = T_IDXS[TRAJECTORY_SIZE - 1];
break;
}
// interpolate to find `t` for the current xidx
float current_x_val = best_plan.mean[tidx].position.x;
float next_x_val = best_plan.mean[tidx+1].position.x;
float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
plan_t[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
}
fill_plan(framed, best_plan);
fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
fill_road_edges(framed, plan_t, net_outputs.road_edges);
// meta
fill_meta(framed.initMeta(), net_outputs.meta, ps);
// confidence
fill_confidence(framed, ps);
// leads
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
for (int i=0; i<LEAD_MHP_SELECTION; i++) {
fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]);
}
// temporal pose
const auto &v_mean = net_outputs.temporal_pose.velocity_mean;
const auto &r_mean = net_outputs.temporal_pose.rotation_mean;
const auto &v_std = net_outputs.temporal_pose.velocity_std;
const auto &r_std = net_outputs.temporal_pose.rotation_std;
auto temporal_pose = framed.initTemporalPose();
temporal_pose.setTrans({v_mean.x, v_mean.y, v_mean.z});
temporal_pose.setRot({r_mean.x, r_mean.y, r_mean.z});
temporal_pose.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
}
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
auto framed = msg.initEvent(valid).initModelV2();
framed.setFrameId(vipc_frame_id);
framed.setFrameIdExtra(vipc_frame_id_extra);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);
framed.setLocationMonoTime(timestamp_llk);
framed.setModelExecutionTime(model_execution_time);
framed.setNavEnabled(nav_enabled);
if (send_raw_pred) {
framed.setRawPredictions(kj::ArrayPtr<const float>(net_output_data, NET_OUTPUT_SIZE).asBytes());
}
fill_model(framed, *((ModelOutput*) net_output_data), ps);
}
void fill_pose_msg(MessageBuilder &msg, float *net_output_data, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid) {
const ModelOutput &net_outputs = *((ModelOutput*) net_output_data);
const auto &v_mean = net_outputs.pose.velocity_mean;
const auto &r_mean = net_outputs.pose.rotation_mean;
const auto &t_mean = net_outputs.wide_from_device_euler.mean;
const auto &v_std = net_outputs.pose.velocity_std;
const auto &r_std = net_outputs.pose.rotation_std;
const auto &t_std = net_outputs.wide_from_device_euler.std;
const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean;
const auto &road_transform_trans_std = net_outputs.road_transform.position_std;
auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z});
posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z});
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)});
posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)});
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(vipc_frame_id);
}
selfdrive/modeld/models/driving.h
-257
View File
@@ -1,257 +0,0 @@
#pragma once
#include <array>
#include <memory>
#include "cereal/messaging/messaging.h"
#include "common/modeldata.h"
#include "common/util.h"
#include "selfdrive/modeld/models/commonmodel.h"
#include "selfdrive/modeld/runners/run.h"
constexpr int FEATURE_LEN = 512;
constexpr int HISTORY_BUFFER_LEN = 99;
constexpr int DESIRE_LEN = 8;
constexpr int DESIRE_PRED_LEN = 4;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int NAV_FEATURE_LEN = 256;
constexpr int NAV_INSTRUCTION_LEN = 150;
constexpr int DRIVING_STYLE_LEN = 12;
constexpr int MODEL_FREQ = 20;
constexpr int DISENGAGE_LEN = 5;
constexpr int BLINKER_LEN = 6;
constexpr int META_STRIDE = 7;
constexpr int PLAN_MHP_N = 5;
constexpr int LEAD_MHP_N = 2;
constexpr int LEAD_TRAJ_LEN = 6;
constexpr int LEAD_MHP_SELECTION = 3;
// Padding to get output shape as multiple of 4
constexpr int PAD_SIZE = 2;
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
struct ModelOutputXYZ {
float x;
float y;
float z;
};
static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3);
struct ModelOutputYZ {
float y;
float z;
};
static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
struct ModelOutputPlanElement {
ModelOutputXYZ position;
ModelOutputXYZ velocity;
ModelOutputXYZ acceleration;
ModelOutputXYZ rotation;
ModelOutputXYZ rotation_rate;
};
static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5);
struct ModelOutputPlanPrediction {
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> mean;
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> std;
float prob;
};
static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
struct ModelOutputPlans {
std::array<ModelOutputPlanPrediction, PLAN_MHP_N> prediction;
constexpr const ModelOutputPlanPrediction &get_best_prediction() const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob > prediction[max_idx].prob) {
max_idx = i;
}
}
return prediction[max_idx];
}
};
static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
struct ModelOutputLinesXY {
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far;
};
static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
struct ModelOutputLineProbVal {
float val_deprecated;
float val;
};
static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2);
struct ModelOutputLinesProb {
ModelOutputLineProbVal left_far;
ModelOutputLineProbVal left_near;
ModelOutputLineProbVal right_near;
ModelOutputLineProbVal right_far;
};
static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
struct ModelOutputLaneLines {
ModelOutputLinesXY mean;
ModelOutputLinesXY std;
ModelOutputLinesProb prob;
};
static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
struct ModelOutputEdgessXY {
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right;
};
static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2);
struct ModelOutputRoadEdges {
ModelOutputEdgessXY mean;
ModelOutputEdgessXY std;
};
static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2));
struct ModelOutputLeadElement {
float x;
float y;
float velocity;
float acceleration;
};
static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4);
struct ModelOutputLeadPrediction {
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> mean;
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> std;
std::array<float, LEAD_MHP_SELECTION> prob;
};
static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
struct ModelOutputLeads {
std::array<ModelOutputLeadPrediction, LEAD_MHP_N> prediction;
std::array<float, LEAD_MHP_SELECTION> prob;
constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
max_idx = i;
}
}
return prediction[max_idx];
}
};
static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
struct ModelOutputPose {
ModelOutputXYZ velocity_mean;
ModelOutputXYZ rotation_mean;
ModelOutputXYZ velocity_std;
ModelOutputXYZ rotation_std;
};
static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputWideFromDeviceEuler {
ModelOutputXYZ mean;
ModelOutputXYZ std;
};
static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2);
struct ModelOutputTemporalPose {
ModelOutputXYZ velocity_mean;
ModelOutputXYZ rotation_mean;
ModelOutputXYZ velocity_std;
ModelOutputXYZ rotation_std;
};
static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputRoadTransform {
ModelOutputXYZ position_mean;
ModelOutputXYZ rotation_mean;
ModelOutputXYZ position_std;
ModelOutputXYZ rotation_std;
};
static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputDisengageProb {
float gas_disengage;
float brake_disengage;
float steer_override;
float brake_3ms2;
float brake_4ms2;
float brake_5ms2;
float gas_pressed;
};
static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7);
struct ModelOutputBlinkerProb {
float left;
float right;
};
static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2);
struct ModelOutputDesireProb {
union {
struct {
float none;
float turn_left;
float turn_right;
float lane_change_left;
float lane_change_right;
float keep_left;
float keep_right;
float null;
};
struct {
std::array<float, DESIRE_LEN> array;
};
};
};
static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN);
struct ModelOutputMeta {
ModelOutputDesireProb desire_state_prob;
float engaged_prob;
std::array<ModelOutputDisengageProb, DISENGAGE_LEN> disengage_prob;
std::array<ModelOutputBlinkerProb, BLINKER_LEN> blinker_prob;
std::array<ModelOutputDesireProb, DESIRE_PRED_LEN> desire_pred_prob;
};
static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
struct ModelOutputFeatures {
std::array<float, FEATURE_LEN> feature;
};
static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN));
struct ModelOutput {
const ModelOutputPlans plans;
const ModelOutputLaneLines lane_lines;
const ModelOutputRoadEdges road_edges;
const ModelOutputLeads leads;
const ModelOutputMeta meta;
const ModelOutputPose pose;
const ModelOutputWideFromDeviceEuler wide_from_device_euler;
const ModelOutputTemporalPose temporal_pose;
const ModelOutputRoadTransform road_transform;
};
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE;
struct PublishState {
std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
std::array<float, 5> prev_brake_5ms2_probs = {};
std::array<float, 3> prev_brake_3ms2_probs = {};
};
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid);
void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid);
selfdrive/modeld/models/driving.pxd
-25
View File
@@ -1,25 +0,0 @@
# distutils: language = c++
from libcpp cimport bool
from libc.stdint cimport uint32_t, uint64_t
cdef extern from "cereal/messaging/messaging.h":
cdef cppclass MessageBuilder:
size_t getSerializedSize()
int serializeToBuffer(unsigned char *, size_t)
cdef extern from "selfdrive/modeld/models/driving.h":
cdef int FEATURE_LEN
cdef int HISTORY_BUFFER_LEN
cdef int DESIRE_LEN
cdef int TRAFFIC_CONVENTION_LEN
cdef int DRIVING_STYLE_LEN
cdef int NAV_FEATURE_LEN
cdef int NAV_INSTRUCTION_LEN
cdef int OUTPUT_SIZE
cdef int NET_OUTPUT_SIZE
cdef int MODEL_FREQ
cdef struct PublishState: pass
void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool)
void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool)
selfdrive/modeld/models/driving_pyx.pyx
-52
View File
@@ -1,52 +0,0 @@
# distutils: language = c++
# cython: c_string_encoding=ascii
import numpy as np
cimport numpy as cnp
from libcpp cimport bool
from libc.string cimport memcpy
from libc.stdint cimport uint32_t, uint64_t
from .commonmodel cimport mat3
from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \
TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \
NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
from .driving cimport MessageBuilder, PublishState as cppPublishState
from .driving cimport fill_model_msg, fill_pose_msg
FEATURE_LEN = CPP_FEATURE_LEN
HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
DESIRE_LEN = CPP_DESIRE_LEN
TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN
DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN
NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN
NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN
OUTPUT_SIZE = CPP_OUTPUT_SIZE
NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE
MODEL_FREQ = CPP_MODEL_FREQ
cdef class PublishState:
cdef cppPublishState state
def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid):
cdef MessageBuilder msg
fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop,
timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid)
output_size = msg.getSerializedSize()
output_data = bytearray(output_size)
cdef unsigned char * output_ptr = output_data
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
return bytes(output_data)
def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid):
cdef MessageBuilder msg
fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid)
output_size = msg.getSerializedSize()
output_data = bytearray(output_size)
cdef unsigned char * output_ptr = output_data
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
return bytes(output_data)
selfdrive/modeld/navmodeld.py
+4 -4
View File
@@ -13,13 +13,13 @@ from cereal.visionipc import VisionIpcClient, VisionStreamType
from openpilot.system.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.common.realtime import set_realtime_priority
from openpilot.selfdrive.modeld.constants import IDX_N
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
NAV_INPUT_SIZE = 256*256
NAV_FEATURE_LEN = 256
NAV_DESIRE_LEN = 32
NAV_OUTPUT_SIZE = 2*2*IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
MODEL_PATHS = {
ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc',
ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'}
@@ -31,8 +31,8 @@ class NavModelOutputXY(ctypes.Structure):
class NavModelOutputPlan(ctypes.Structure):
_fields_ = [
("mean", NavModelOutputXY*IDX_N),
("std", NavModelOutputXY*IDX_N)]
("mean", NavModelOutputXY*ModelConstants.IDX_N),
("std", NavModelOutputXY*ModelConstants.IDX_N)]
class NavModelResult(ctypes.Structure):
_fields_ = [
selfdrive/modeld/parse_model_outputs.py
+100
View File
@@ -0,0 +1,100 @@
import numpy as np
from typing import Dict
from openpilot.selfdrive.modeld.constants import ModelConstants
def sigmoid(x):
return 1. / (1. + np.exp(-x))
def softmax(x, axis=-1):
x -= np.max(x, axis=axis, keepdims=True)
if x.dtype == np.float32 or x.dtype == np.float64:
np.exp(x, out=x)
else:
x = np.exp(x)
x /= np.sum(x, axis=axis, keepdims=True)
return x
class Parser:
def __init__(self, ignore_missing=False):
self.ignore_missing = ignore_missing
def check_missing(self, outs, name):
if name not in outs and not self.ignore_missing:
raise ValueError(f"Missing output {name}")
return name not in outs
def parse_categorical_crossentropy(self, name, outs, out_shape=None):
if self.check_missing(outs, name):
return
raw = outs[name]
if out_shape is not None:
raw = raw.reshape((raw.shape[0],) + out_shape)
outs[name] = softmax(raw, axis=-1)
def parse_binary_crossentropy(self, name, outs):
if self.check_missing(outs, name):
return
raw = outs[name]
outs[name] = sigmoid(raw)
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
if self.check_missing(outs, name):
return
raw = outs[name]
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2]
n_values = (raw.shape[2] - out_N)//2
pred_mu = raw[:,:,:n_values]
pred_std = np.exp(raw[:,:,n_values: 2*n_values])
if in_N > 1:
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
for i in range(out_N):
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
if out_N == 1:
for fidx in range(weights.shape[0]):
idxs = np.argsort(weights[fidx][:,0])[::-1]
weights[fidx] = weights[fidx][idxs]
pred_mu[fidx] = pred_mu[fidx][idxs]
pred_std[fidx] = pred_std[fidx][idxs]
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
outs[name + '_weights'] = weights
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
for fidx in range(weights.shape[0]):
for hidx in range(out_N):
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
else:
pred_mu_final = pred_mu
pred_std_final = pred_std
if out_N > 1:
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
else:
final_shape = tuple([raw.shape[0],] + list(out_shape))
outs[name] = pred_mu_final.reshape(final_shape)
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]:
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
for k in ['lead_prob', 'lane_lines_prob', 'meta']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
return outs
selfdrive/modeld/thneed/lib.py
-32
View File
@@ -1,32 +0,0 @@
import struct
import json
def load_thneed(fn):
with open(fn, "rb") as f:
json_len = struct.unpack("I", f.read(4))[0]
jdat = json.loads(f.read(json_len).decode('latin_1'))
weights = f.read()
ptr = 0
for o in jdat['objects']:
if o['needs_load']:
nptr = ptr + o['size']
o['data'] = weights[ptr:nptr]
ptr = nptr
for o in jdat['binaries']:
nptr = ptr + o['length']
o['data'] = weights[ptr:nptr]
ptr = nptr
return jdat
def save_thneed(jdat, fn):
new_weights = []
for o in jdat['objects'] + jdat['binaries']:
if 'data' in o:
new_weights.append(o['data'])
del o['data']
new_weights_bytes = b''.join(new_weights)
with open(fn, "wb") as f:
j = json.dumps(jdat, ensure_ascii=False).encode('latin_1')
f.write(struct.pack("I", len(j)))
f.write(j)
f.write(new_weights_bytes)
selfdrive/test/longitudinal_maneuvers/plant.py
+4 -4
View File
@@ -6,7 +6,7 @@ from cereal import log
import cereal.messaging as messaging
from openpilot.common.realtime import Ratekeeper, DT_MDL
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.modeld.constants import T_IDXS
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
@@ -100,13 +100,13 @@ class Plant:
# this is to ensure lead policy is effective when model
# does not predict slowdown in e2e mode
position = log.XYZTData.new_message()
position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)]
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
model.modelV2.position = position
velocity = log.XYZTData.new_message()
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)]
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
model.modelV2.velocity = velocity
acceleration = log.XYZTData.new_message()
acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)]
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
model.modelV2.acceleration = acceleration
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
selfdrive/test/process_replay/README.md
+1 -3
View File
@@ -17,7 +17,6 @@ Currently the following processes are tested:
* locationd
* paramsd
* ubloxd
* laikad
* torqued
### Usage
@@ -71,7 +70,7 @@ lr = LogReader(...)
output_logs = replay_process_with_name('locationd', lr)
# or list of names
output_logs = replay_process_with_name(['ubloxd', 'locationd', 'laikad'], lr)
output_logs = replay_process_with_name(['ubloxd', 'locationd'], lr)
```
Supported processes:
@@ -83,7 +82,6 @@ Supported processes:
* locationd
* paramsd
* ubloxd
* laikad
* torqued
* modeld
* dmonitoringmodeld
selfdrive/test/process_replay/compare_logs.py
+25 -13
View File
@@ -90,6 +90,26 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
return diff
def format_process_diff(diff):
diff_short, diff_long = "", ""
if isinstance(diff, str):
diff_short += f" {diff}\n"
diff_long += f"\t{diff}\n"
else:
cnt: Dict[str, int] = {}
for d in diff:
diff_long += f"\t{str(d)}\n"
k = str(d[1])
cnt[k] = 1 if k not in cnt else cnt[k] + 1
for k, v in sorted(cnt.items()):
diff_short += f" {k}: {v}\n"
return diff_short, diff_long
def format_diff(results, log_paths, ref_commit):
diff_short, diff_long = "", ""
diff_long += f"***** tested against commit {ref_commit} *****\n"
@@ -105,25 +125,17 @@ def format_diff(results, log_paths, ref_commit):
diff_long += f"\tnew: {log_paths[segment][proc]['new']}\n\n"
diff_short += f" {proc}\n"
if isinstance(diff, str):
if isinstance(diff, str) or len(diff):
diff_short += f" ref: {log_paths[segment][proc]['ref']}\n"
diff_short += f" new: {log_paths[segment][proc]['new']}\n\n"
diff_short += f" {diff}\n"
failed = True
elif len(diff):
diff_short += f" ref: {log_paths[segment][proc]['ref']}\n"
diff_short += f" new: {log_paths[segment][proc]['new']}\n\n"
cnt: Dict[str, int] = {}
for d in diff:
diff_long += f"\t{str(d)}\n"
proc_diff_short, proc_diff_long = format_process_diff(diff)
k = str(d[1])
cnt[k] = 1 if k not in cnt else cnt[k] + 1
diff_long += proc_diff_long
diff_short += proc_diff_short
for k, v in sorted(cnt.items()):
diff_short += f" {k}: {v}\n"
failed = True
return diff_short, diff_long, failed
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
f851c7e7f90eff828a59444d20fac5df8cd7ae0c
0e0f55cf3bb2cf79b44adf190e6387a83deb6646
selfdrive/test/process_replay/process_replay.py
-21
View File
@@ -448,16 +448,6 @@ def controlsd_config_callback(params, cfg, lr):
params.put("ReplayControlsState", controlsState.as_builder().to_bytes())
def laikad_config_pubsub_callback(params, cfg, lr):
ublox = params.get_bool("UbloxAvailable")
main_key = "ubloxGnss" if ublox else "qcomGnss"
sub_keys = ({"qcomGnss", } if ublox else {"ubloxGnss", })
cfg.pubs = set(cfg.pubs) - sub_keys
cfg.main_pub = main_key
cfg.main_pub_drained = True
def locationd_config_pubsub_callback(params, cfg, lr):
ublox = params.get_bool("UbloxAvailable")
sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
@@ -543,17 +533,6 @@ CONFIGS = [
subs=["ubloxGnss", "gpsLocationExternal"],
ignore=["logMonoTime"],
),
ProcessConfig(
proc_name="laikad",
pubs=["ubloxGnss", "qcomGnss"],
subs=["gnssMeasurements"],
ignore=["logMonoTime"],
config_callback=laikad_config_pubsub_callback,
tolerance=NUMPY_TOLERANCE,
processing_time=0.002,
timeout=60*10, # first messages are blocked on internet assistance
main_pub="ubloxGnss", # config_callback will switch this to qcom if needed
),
ProcessConfig(
proc_name="torqued",
pubs=["liveLocationKalman", "carState", "carControl"],
selfdrive/test/process_replay/test_fuzzy.py
+1 -1
View File
@@ -13,7 +13,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr
# These processes currently fail because of unrealistic data breaking assumptions
# that openpilot makes causing error with NaN, inf, int size, array indexing ...
# TODO: Make each one testable
NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'laikad', 'dmonitoringmodeld', 'modeld']
NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED]
class TestFuzzProcesses(unittest.TestCase):
selfdrive/test/profiling/profiler.py
-2
View File
@@ -83,14 +83,12 @@ if __name__ == '__main__':
from openpilot.selfdrive.controls.radard import radard_thread
from openpilot.selfdrive.locationd.paramsd import main as paramsd_thread
from openpilot.selfdrive.controls.plannerd import main as plannerd_thread
from openpilot.selfdrive.locationd.laikad import main as laikad_thread
procs = {
'radard': radard_thread,
'controlsd': controlsd_thread,
'paramsd': paramsd_thread,
'plannerd': plannerd_thread,
'laikad': laikad_thread,
}
proc = sys.argv[1]
selfdrive/test/test_onroad.py
+23 -3
View File
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
import bz2
import math
import json
import os
@@ -36,7 +37,7 @@ PROCS = {
"selfdrive.locationd.paramsd": 9.0,
"./sensord": 7.0,
"selfdrive.controls.radard": 4.5,
"selfdrive.modeld.modeld": 8.0,
"selfdrive.modeld.modeld": 13.0,
"selfdrive.modeld.dmonitoringmodeld": 8.0,
"selfdrive.modeld.navmodeld": 1.0,
"selfdrive.thermald.thermald": 3.87,
@@ -46,7 +47,6 @@ PROCS = {
"selfdrive.monitoring.dmonitoringd": 4.0,
"./proclogd": 1.54,
"system.logmessaged": 0.2,
"./clocksd": 0.02,
"selfdrive.tombstoned": 0,
"./logcatd": 0,
"system.micd": 10.0,
@@ -56,7 +56,6 @@ PROCS = {
"selfdrive.navd.navd": 0.4,
"system.loggerd.uploader": 3.0,
"system.loggerd.deleter": 0.1,
"selfdrive.locationd.laikad": (1.0, 80.0), # TODO: better GPS setup in testing closet
}
PROCS.update({
@@ -161,6 +160,7 @@ class TestOnroad(unittest.TestCase):
# use the second segment by default as it's the first full segment
cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog")))
cls.log_path = cls.segments[1]
@cached_property
def service_msgs(self):
@@ -191,6 +191,26 @@ class TestOnroad(unittest.TestCase):
big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.]
self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}")
def test_log_sizes(self):
for f in self.log_path.iterdir():
assert f.is_file()
sz = f.stat().st_size / 1e6
if f.name in ("qlog", "rlog"):
with open(f, 'rb') as ff:
sz = len(bz2.compress(ff.read())) / 1e6
if f.name == "qcamera.ts":
assert 2.15 < sz < 2.35
elif f.name == "qlog":
assert 0.7 < sz < 1.0
elif f.name == "rlog":
assert 5 < sz < 50
elif f.name.endswith('.hevc'):
assert 70 < sz < 77
else:
raise NotImplementedError
def test_ui_timings(self):
result = "\n"
result += "------------------------------------------------\n"
selfdrive/test/update_ci_routes.py
-2
View File
@@ -8,7 +8,6 @@ from azure.storage.blob import ContainerClient
from tqdm import tqdm
from openpilot.selfdrive.car.tests.routes import routes as test_car_models_routes
from openpilot.selfdrive.locationd.test.test_laikad import UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE
from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments
from openpilot.selfdrive.test.openpilotci import (DATA_CI_ACCOUNT, DATA_CI_ACCOUNT_URL, DATA_CI_CONTAINER,
get_azure_credential, get_container_sas)
@@ -90,7 +89,6 @@ if __name__ == "__main__":
if not len(to_sync):
# sync routes from the car tests routes and process replay
to_sync.extend([UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE])
to_sync.extend([rt.route for rt in test_car_models_routes])
to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments])
selfdrive/ui/SConscript
+8 -2
View File
@@ -25,8 +25,14 @@ widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc",
"qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc",
"qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc",
"qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc",
"qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc",
"qt/offroad/sunnypilot_settings.cc"]
"qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"]
widgets_src += ["qt/offroad/sunnypilot/display_settings.cc", "qt/offroad/sunnypilot/sunnypilot_settings.cc",
"qt/offroad/sunnypilot/vehicle_settings.cc", "qt/offroad/sunnypilot/visuals_settings.cc",
"qt/offroad/sunnypilot/trips_settings.cc", "qt/offroad/sunnypilot/mads_settings.cc",
"qt/offroad/sunnypilot/lane_change_settings.cc", "qt/offroad/sunnypilot/monitoring_settings.cc",
"qt/offroad/sunnypilot/osm_settings.cc", "qt/offroad/sunnypilot/custom_offsets_settings.cc",
"qt/widgets/sunnypilot/drive_stats.cc"]
qt_env['CPPDEFINES'] = []
if maps:
selfdrive/ui/qt/network/networking.cc
+17
View File
@@ -130,6 +130,18 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
list->addItem(tetheringToggle);
QObject::connect(tetheringToggle, &ToggleControl::toggleFlipped, this, &AdvancedNetworking::toggleTethering);
hotspotOnBootToggle = new ToggleControl(
tr("Retain hotspot/tethering state"),
tr("Enabling this toggle will retain the hotspot/tethering toggle state across reboots."),
"",
params.getBool("HotspotOnBoot")
);
hotspotOnBootToggle->setEnabled(wifi->isTetheringEnabled() || set_hotspot_on_boot);
QObject::connect(hotspotOnBootToggle, &ToggleControl::toggleFlipped, [=](bool state) {
params.putBool("HotspotOnBoot", state);
});
list->addItem(hotspotOnBootToggle);
// Change tethering password
ButtonControl *editPasswordButton = new ButtonControl(tr("Tethering Password"), tr("EDIT"));
connect(editPasswordButton, &ButtonControl::clicked, [=]() {
@@ -205,6 +217,11 @@ void AdvancedNetworking::toggleTethering(bool enabled) {
params.putBool("HotspotOnBootConfirmed", enabled);
wifi->setTetheringEnabled(enabled);
tetheringToggle->setEnabled(false);
hotspotOnBootToggle->setEnabled(enabled);
if (!enabled) {
params.remove("HotspotOnBoot");
}
}
// WifiUI functions
selfdrive/ui/qt/network/networking.h
+2
View File
@@ -66,6 +66,8 @@ private:
WifiManager* wifi = nullptr;
Params params;
ToggleControl* hotspotOnBootToggle;
signals:
void backPress();
selfdrive/ui/qt/offroad/settings.cc
+80 -49
View File
@@ -17,7 +17,7 @@
#include "common/watchdog.h"
#include "common/util.h"
#include "system/hardware/hw.h"
#include "selfdrive/ui/qt/offroad/sunnypilot_settings.h"
#include "selfdrive/ui/qt/offroad/sunnypilot_main.h"
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/widgets/input.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
@@ -34,7 +34,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"OpenpilotEnabledToggle",
tr("Enable sunnypilot"),
tr("Use the sunnypilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off."),
"../assets/offroad/icon_openpilot.png",
"../assets/offroad/icon_blank.png",
},
{
"ExperimentalLongitudinalEnabled",
@@ -43,13 +43,13 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
.arg(tr("WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB)."))
.arg(tr("On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. "
"Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.")),
"../assets/offroad/icon_speed_limit.png",
"../assets/offroad/icon_blank.png",
},
{
"CustomStockLong",
tr("Custom Stock Longitudinal Control"),
tr("When enabled, sunnypilot will attempt to control stock longitudinal control with ACC button presses.\nThis feature must be used along with SLC, and/or V-TSC, and/or M-TSC."),
"../assets/offroad/icon_speed_limit.png",
"../assets/offroad/icon_blank.png",
},
{
"CustomStockLongPlanner",
@@ -61,55 +61,74 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"ExperimentalMode",
tr("Experimental Mode"),
"",
"../assets/img_experimental_white.svg",
"../assets/offroad/icon_blank.png",
},
{
"DynamicExperimentalControl",
tr("Enable Dynamic Experimental Control"),
tr("Enable toggle to allow the model to determine when to use openpilot ACC or openpilot End to End Longitudinal."),
"../assets/offroad/icon_blank.png",
},
{
"DisengageOnAccelerator",
tr("Disengage on Accelerator Pedal"),
tr("When enabled, pressing the accelerator pedal will disengage openpilot."),
"../assets/offroad/icon_disengage_on_accelerator.svg",
"../assets/offroad/icon_blank.png",
},
{
"IsLdwEnabled",
tr("Enable Lane Departure Warnings"),
tr("Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h)."),
"../assets/offroad/icon_warning.png",
"../assets/offroad/icon_blank.png",
},
{
"RecordFront",
tr("Record and Upload Driver Camera"),
tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
"../assets/offroad/icon_monitoring.png",
"../assets/offroad/icon_blank.png",
},
{
"DisableOnroadUploads",
tr("Disable Onroad Uploads"),
tr("Disable uploads completely when onroad. Necessary to avoid high data usage when connected to Wi-Fi hotspot. Turn on this feature if you are looking to utilize map-based features, such as Speed Limit Control (SLC) and Map-based Turn Speed Control (MTSC)."),
"../assets/offroad/icon_blank.png",
},
{
"EnableDebugSnapshot",
tr("Debug snapshot on screen center tap"),
tr("Stores snapshot file with current state of some modules."),
"../assets/offroad/icon_blank.png"
},
{
"IsMetric",
tr("Use Metric System"),
tr("Display speed in km/h instead of mph."),
"../assets/offroad/icon_metric.png",
"../assets/offroad/icon_blank.png",
},
#ifdef ENABLE_MAPS
{
"NavSettingTime24h",
tr("Show ETA in 24h Format"),
tr("Use 24h format instead of am/pm"),
"../assets/offroad/icon_metric.png",
"../assets/offroad/icon_blank.png",
},
{
"NavSettingLeftSide",
tr("Show Map on Left Side of UI"),
tr("Show map on left side when in split screen view."),
"../assets/offroad/icon_road.png",
"../assets/offroad/icon_blank.png",
},
#endif
};
std::vector<QString> longi_button_texts{tr("Maniac"), tr("Aggro"), tr("Stock"), tr("Relaxed")};
std::vector<QString> longi_button_texts{tr("Maniac"), tr("Aggressive"), tr("Stock"), tr("Relaxed")};
long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"),
tr("Stock is recommended. In aggressive/maniac mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. "
"In relaxed mode openpilot will stay further away from lead cars."),
"../assets/offroad/icon_speed_limit.png",
longi_button_texts);
"../assets/offroad/icon_blank.png",
longi_button_texts,
380);
long_personality_setting->showDescription();
for (auto &[param, title, desc, icon] : toggle_defs) {
auto toggle = new ParamControl(param, title, desc, icon, this);
@@ -127,7 +146,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
}
// Toggles with confirmation dialogs
toggles["ExperimentalMode"]->setActiveIcon("../assets/img_experimental.svg");
//toggles["ExperimentalMode"]->setActiveIcon("../assets/img_experimental.svg");
toggles["ExperimentalMode"]->setConfirmation(true, true);
toggles["ExperimentalLongitudinalEnabled"]->setConfirmation(true, false);
toggles["CustomStockLong"]->setConfirmation(true, false);
@@ -142,7 +161,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
param_watcher = new ParamWatcher(this);
QObject::connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString &param_name, const QString &param_value) {
updateButtons();
updateToggles();
});
}
@@ -152,13 +171,19 @@ void TogglesPanel::expandToggleDescription(const QString &param) {
void TogglesPanel::showEvent(QShowEvent *event) {
updateToggles();
updateButtons();
}
void TogglesPanel::updateToggles() {
param_watcher->addParam("LongitudinalPersonality");
if (!isVisible()) return;
long_personality_setting->setButton("LongitudinalPersonality");
auto experimental_mode_toggle = toggles["ExperimentalMode"];
auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"];
auto custom_stock_long_toggle = toggles["CustomStockLong"];
auto dec_toggle = toggles["DynamicExperimentalControl"];
auto custom_stock_long_planner = toggles["CustomStockLongPlanner"];
const QString e2e_description = QString("%1<br>"
"<h4>%2</h4><br>"
@@ -193,7 +218,7 @@ void TogglesPanel::updateToggles() {
op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
if (!CP.getCustomStockLongAvailable()) {
params.remove("CustomStockLongControl");
params.remove("CustomStockLong");
}
custom_stock_long_toggle->setVisible(CP.getCustomStockLongAvailable());
@@ -202,14 +227,18 @@ void TogglesPanel::updateToggles() {
experimental_mode_toggle->setEnabled(true);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(true);
op_long_toggle->setEnabled(true);
custom_stock_long_toggle->setEnabled(false);
params.remove("CustomStockLong");
dec_toggle->setEnabled(true);
} else if (custom_stock_long_toggle->isToggled()) {
op_long_toggle->setEnabled(false);
experimental_mode_toggle->setEnabled(true);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(false);
op_long_toggle->setEnabled(false);
custom_stock_long_planner->setEnabled(true);
params.remove("ExperimentalLongitudinalEnabled");
params.remove("ExperimentalMode");
} else {
// no long for now
experimental_mode_toggle->setEnabled(false);
@@ -229,31 +258,26 @@ void TogglesPanel::updateToggles() {
}
experimental_mode_toggle->setDescription("<b>" + long_desc + "</b><br><br>" + e2e_description);
custom_stock_long_toggle->setEnabled(CP.getCustomStockLongAvailable());
custom_stock_long_planner->setEnabled(false);
op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
op_long_toggle->setEnabled(CP.getExperimentalLongitudinalAvailable() && !is_release);
custom_stock_long_toggle->setEnabled(CP.getCustomStockLongAvailable());
dec_toggle->setEnabled(false);
custom_stock_long_planner->setEnabled(false);
params.remove("DynamicExperimentalControl");
}
experimental_mode_toggle->refresh();
custom_stock_long_toggle->refresh();
op_long_toggle->refresh();
custom_stock_long_toggle->refresh();
dec_toggle->refresh();
} else {
experimental_mode_toggle->setDescription(e2e_description);
op_long_toggle->setVisible(false);
custom_stock_long_toggle->setVisible(false);
dec_toggle->setVisible(false);
custom_stock_long_planner->setVisible(false);
}
}
void TogglesPanel::updateButtons() {
param_watcher->addParam("LongitudinalPersonality");
if (!isVisible()) return;
long_personality_setting->setButton("LongitudinalPersonality");
}
DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
setSpacing(50);
addItem(new LabelControl(tr("Dongle ID"), getDongleId().value_or(tr("N/A"))));
@@ -528,34 +552,42 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
TogglesPanel *toggles = new TogglesPanel(this);
QObject::connect(this, &SettingsWindow::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription);
QList<QPair<QString, QWidget *>> panels = {
{tr(" Device"), device},
{tr(" Network"), new Networking(this)},
{tr(" Toggles"), toggles},
{tr(" Software"), new SoftwarePanel(this)},
QList<PanelInfo> panels = {
PanelInfo(" " + tr("Device"), device, "../assets/navigation/icon_home.svg"),
PanelInfo(" " + tr("Network"), new Networking(this), "../assets/offroad/icon_network.png"),
PanelInfo(" " + tr("Toggles"), toggles, "../assets/offroad/icon_toggle.png"),
PanelInfo(" " + tr("Software"), new SoftwarePanel(this), "../assets/offroad/icon_software.png"),
PanelInfo(" " + tr("sunnypilot"), new SunnypilotPanel(this), "../assets/offroad/icon_openpilot.png"),
PanelInfo(" " + tr("OSM"), new OsmPanel(this), "../assets/offroad/icon_map.png"),
PanelInfo(" " + tr("Monitoring"), new MonitoringPanel(this), "../assets/offroad/icon_monitoring.png"),
PanelInfo(" " + tr("Visuals"), new VisualsPanel(this), "../assets/offroad/icon_visuals.png"),
PanelInfo(" " + tr("Display"), new DisplayPanel(this), "../assets/offroad/icon_display.png"),
PanelInfo(" " + tr("Trips"), new TripsPanel(this), "../assets/offroad/icon_trips.png"),
PanelInfo(" " + tr("Vehicle"), new VehiclePanel(this), "../assets/offroad/icon_vehicle.png"),
};
panels.push_back({tr(" SP - General"), new SPGeneralPanel(this)});
panels.push_back({tr(" SP - Controls"), new SPControlsPanel(this)});
panels.push_back({tr(" SP - Vehicles"), new SPVehiclesPanel(this)});
panels.push_back({tr(" SP - Visuals"), new SPVisualsPanel(this)});
nav_btns = new QButtonGroup(this);
for (auto &[name, panel] : panels) {
for (auto &[name, panel, icon] : panels) {
QPushButton *btn = new QPushButton(name);
btn->setCheckable(true);
btn->setChecked(nav_btns->buttons().size() == 0);
btn->setIcon(QIcon(QPixmap("../assets/offroad/icon_blank.png")));
btn->setIconSize(QSize(45, 45));
btn->setIcon(QIcon(QPixmap(icon)));
btn->setIconSize(QSize(70, 70));
btn->setStyleSheet(R"(
QPushButton {
color: grey;
border-radius: 20px;
width: 400px;
height: 98px;
color: #bdbdbd;
border: none;
background: none;
font-size: 50px;
font-weight: 500;
text-align: left;
padding-left: 22px;
}
QPushButton:checked {
background-color: #696868;
color: white;
}
QPushButton:pressed {
@@ -565,9 +597,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
btn->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
nav_btns->addButton(btn);
buttons_layout->addWidget(btn, 0, Qt::AlignLeft | Qt::AlignBottom);
buttons_layout->addSpacing(25);
const int lr_margin = name != tr("Network") ? 50 : 0; // Network panel handles its own margins
const int lr_margin = (name != (" " + tr("Network")) || (name != (" " + tr("sunnypilot")))) ? 50 : 0; // Network and sunnypilot panel handles its own margins
panel->setContentsMargins(lr_margin, 25, lr_margin, 25);
ScrollView *panel_frame = new ScrollView(panel, this);
@@ -578,7 +609,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
panel_widget->setCurrentWidget(w);
});
}
sidebar_layout->setContentsMargins(50, 50, 50, 50);
sidebar_layout->setContentsMargins(50, 50, 25, 50);
// main settings layout, sidebar + main panel
QHBoxLayout *main_layout = new QHBoxLayout(this);
@@ -603,7 +634,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
background-color: black;
}
QStackedWidget, ScrollView {
background-color: #292929;
background-color: black;
border-radius: 30px;
}
)");
selfdrive/ui/qt/offroad/settings.h
+8 -113
View File
@@ -37,6 +37,14 @@ private:
QWidget *sidebar_widget;
QButtonGroup *nav_btns;
QStackedWidget *panel_widget;
struct PanelInfo {
QString name;
QWidget *widget;
QString icon;
PanelInfo(const QString &name, QWidget *widget, const QString &icon) : name(name), widget(widget), icon(icon) {}
};
};
class DevicePanel : public ListWidget {
@@ -79,7 +87,6 @@ private:
ButtonParamControl *long_personality_setting;
ParamWatcher *param_watcher;
void updateButtons();
};
class SoftwarePanel : public ListWidget {
@@ -95,124 +102,12 @@ private:
bool is_onroad = false;
QLabel *onroadLbl;
QLabel *osmUpdateLbl;
LabelControl *currentModelLbl;
LabelControl *versionLbl;
ButtonControl *installBtn;
ButtonControl *downloadBtn;
ButtonControl *targetBranchBtn;
ButtonControl *osmDownloadBtn;
ButtonControl *osmUpdateBtn;
Params params;
ParamWatcher *fs_watch;
inline std::vector<std::tuple<QString, QString, QString, QString>> getOsmLocations() {
// location_title, location_name, waypoint, url
std::vector<std::tuple<QString, QString, QString, QString>> locations{
{
"== None ==",
"",
"",
"",
},
{
"Australia",
"australia",
"514911884",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EathcAhPP8dHiTdBiFPIGYoB7zwlUR79OkSTH5dnS-9Shg?e=9cS6er&download=1",
},
{
"Brazil",
"brazil",
"124199196",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXWcrd8ukahDtoFDXVOxvkYBjGGa9b29fVvRdJlADv3YdA?e=jwabo2&download=1",
},
{
"Canada",
"canada",
"68588664",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETpwTMZftJpCkep2goPmzUQBl5SW_YfHBKLTd7w5CUagkA?e=dshC10&download=1",
},
{
"GCC States",
"gcc-states",
"69021390",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWtFOVR9mj5CoRpATYR0_tUBWNEcy5jeasp8zO0ZYdduRg?e=Cg0l2Z&download=1",
},
{
"Germany",
"germany",
"461526153",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EbNzQuA8jHBCuGch1WY4cuoBJfK1S6m5odj5qMnOu3HA3Q?e=cNDn30&download=1",
},
{
"Malaysia, Singapore, Brunei",
"malaysia-singapore-brunei",
"1112741782",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQb9bq_YkE1NuFx0jp-52zoBnAbdo4nyLXMnEY1pUbCB8g?e=CoFiUJ&download=1",
},
{
"New Zealand",
"new-zealand",
"154430132",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EVI6eMuNa4dJpXg-r92Vw1EBq5RxR83_7Z5C56AYftg8xQ?e=xouRBl&download=1",
},
{
"South Africa",
"south-africa",
"2729449",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQzbZeus-yBNsgP1KQyLn9wBYZVMMknVoHdB7_ewulLbdA?e=DrnKkb&download=1",
},
{
"Taiwan",
"taiwan",
"198637969",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXgP3Z5_lsBPiQe-aXOOSY4Bu-r54Z_3kKKmT8-IqLO42A?e=tOhUYw&download=1",
},
{
"Turkey",
"turkey",
"698359658",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWJRrYpV7I1IvOW0LUWrDsUBjEjuKRTla0hA4yRbsVX2xw?e=RGcu8n&download=1",
},
{
"US - Florida",
"florida",
"147221754",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ET9rAvCaretAnO4TjocAddkBFxAziblxblAn9YWzGk5hTw?e=tts5Oz&download=1",
},
{
"US - Midwest",
"us-midwest",
"1059596607",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EaDYnugxokRHhOukMndJuZMBKz-kzPfM6C_-iQcki-r94Q?e=oUX6hz&download=1",
},
{
"US - Northeast",
"us-northeast",
"575213527",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ERrlIKNuT4ZIuUOfr4a3BwABLfd7X1wSq9nUrk9buKrIgA?e=HHIT8R&download=1",
},
{
"US - Pacific",
"us-pacific",
"112909709",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQOzoMksk9tAluahQ2qm3rQBHOZD03qFU8Aw4o-TFUTuJA?e=CcBOxL&download=1",
},
{
"US - South",
"us-south",
"243729876",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETizacxh8lFNk8NMVRyKCEsBMMYZl1iq0r-D4IWoOAd_DQ?e=EVIElm&download=1",
},
{
"US - West",
"us-west",
"30023440",
"https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETTRiSKwlEVKoRHtylruy7wBKgITKXdaCUAkJMuGAN2_mA?e=7jlglL&download=1",
},
};
return locations;
}
};
selfdrive/ui/qt/offroad/software_settings.cc
-59
View File
@@ -76,54 +76,6 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
});
addItem(targetBranchBtn);
osmUpdateLbl = new QLabel(tr("Database updates can be downloaded while the car is off."));
osmUpdateLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;");
addItem(osmUpdateLbl);
osmUpdateBtn = new ButtonControl(tr("OpenStreetMap Database Update"), tr("CHECK"));
connect(osmUpdateBtn, &ButtonControl::clicked, [=]() {
if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot and perform a database update?\nEstimated time: 30-90 minutes"), tr("Reboot"), parent)) {
params.putBool("OsmDbUpdatesCheck", true);
Hardware::reboot();
}
});
addItem(osmUpdateBtn);
osmDownloadBtn = new ButtonControl(tr("OpenStreetMap Database"), tr("SELECT"));
connect(osmDownloadBtn, &ButtonControl::clicked, [=]() {
std::vector<std::tuple<QString, QString, QString, QString>> locations = getOsmLocations();
QString initTitle = QString::fromStdString(params.get("OsmLocationTitle"));
QString currentTitle = ((initTitle == "== None ==") || (initTitle.length() == 0)) ? "== None ==" : initTitle;
QStringList locationTitles;
for (auto& loc : locations) {
locationTitles.push_back(std::get<0>(loc));
}
QString selection = MultiOptionDialog::getSelection(tr("Select your location"), locationTitles, currentTitle, this);
if (!selection.isEmpty()) {
params.put("OsmLocal", "1");
params.put("OsmLocationTitle", selection.toStdString());
for (auto& loc : locations) {
if (std::get<0>(loc) == selection) {
params.put("OsmLocationName", std::get<1>(loc).toStdString());
params.put("OsmWayTest", std::get<2>(loc).toStdString());
params.put("OsmLocationUrl", std::get<3>(loc).toStdString());
break;
}
}
osmDownloadBtn->setValue(selection);
if (selection != "== None ==") {
if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes"), tr("Reboot"), parent)) {
params.putBool("OsmDbUpdatesCheck", true);
Hardware::reboot();
}
}
}
updateLabels();
});
addItem(osmDownloadBtn);
// uninstall button
auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL"));
connect(uninstallBtn, &ButtonControl::clicked, [&]() {
@@ -168,16 +120,6 @@ void SoftwarePanel::updateLabels() {
onroadLbl->setVisible(is_onroad);
downloadBtn->setVisible(!is_onroad);
QString name = QString::fromStdString(params.get("OsmLocationName"));
if (!name.isEmpty()) {
osmUpdateBtn->setVisible(!is_onroad);
} else {
params.remove("OsmLocal");
osmUpdateBtn->setVisible(false);
}
osmUpdateLbl->setVisible(is_onroad);
osmDownloadBtn->setEnabled(!is_onroad);
// download update
QString updater_state = QString::fromStdString(params.get("UpdaterState"));
bool failed = std::atoi(params.get("UpdateFailedCount").c_str()) > 0;
@@ -203,7 +145,6 @@ void SoftwarePanel::updateLabels() {
downloadBtn->setEnabled(true);
}
targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch")));
osmDownloadBtn->setValue(QString::fromStdString(params.get("OsmLocationTitle")));
// current + new versions
versionLbl->setText(QString::fromStdString(params.get("UpdaterCurrentDescription")));
selfdrive/ui/qt/offroad/sunnypilot/custom_offsets_settings.cc
+68
View File
@@ -0,0 +1,68 @@
#include "selfdrive/ui/qt/offroad/sunnypilot/custom_offsets_settings.h"
CustomOffsetsSettings::CustomOffsetsSettings(QWidget* parent) : QWidget(parent) {
QVBoxLayout* main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(50, 20, 50, 20);
main_layout->setSpacing(20);
// Back button
PanelBackButton* back = new PanelBackButton(tr("Back"));
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
main_layout->addWidget(back, 0, Qt::AlignLeft);
ListWidget *list = new ListWidget(this, false);
// Controls: Camera Offset (cm)
camera_offset = new CameraOffset();
connect(camera_offset, &SPOptionControl::updateLabels, camera_offset, &CameraOffset::refresh);
camera_offset->showDescription();
list->addItem(camera_offset);
// Controls: Path Offset (cm)
path_offset = new PathOffset();
connect(path_offset, &SPOptionControl::updateLabels, path_offset, &PathOffset::refresh);
path_offset->showDescription();
list->addItem(path_offset);
main_layout->addWidget(new ScrollView(list, this));
}
// Camera Offset Value
CameraOffset::CameraOffset() : SPOptionControl (
"CameraOffset",
tr("Camera Offset - Laneful Only"),
tr("Hack to trick vehicle to be left or right biased in its lane. Decreasing the value will make the car keep more left, increasing will make it keep more right. Changes take effect immediately. Default: +4 cm"),
"../assets/offroad/icon_blank.png",
{-20, 20}) {
refresh();
}
void CameraOffset::refresh() {
QString option = QString::fromStdString(params.get("CameraOffset"));
QString unit = tr(" cm");
if (option.toInt() >= 0) {
option = "+" + option;
}
setLabel(option + unit);
}
// Path Offset Value
PathOffset::PathOffset() : SPOptionControl (
"PathOffset",
tr("Path Offset"),
tr("Hack to trick the model path to be left or right biased of the lane. Decreasing the value will shift the model more left, increasing will shift the model more right. Changes take effect immediately."),
"../assets/offroad/icon_blank.png",
{-10, 10}) {
refresh();
}
void PathOffset::refresh() {
QString option = QString::fromStdString(params.get("PathOffset"));
QString unit = tr(" cm");
if (option.toInt() >= 0) {
option = "+" + option;
}
setLabel(option + unit);
}
selfdrive/ui/qt/offroad/sunnypilot/custom_offsets_settings.h
+45
View File
@@ -0,0 +1,45 @@
#pragma once
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
class CameraOffset : public SPOptionControl {
Q_OBJECT
public:
CameraOffset();
void refresh();
private:
Params params;
};
class PathOffset : public SPOptionControl {
Q_OBJECT
public:
PathOffset();
void refresh();
private:
Params params;
};
class CustomOffsetsSettings : public QWidget {
Q_OBJECT
public:
explicit CustomOffsetsSettings(QWidget* parent = nullptr);
signals:
void backPress();
private:
Params params;
std::map<std::string, ParamControl*> toggles;
CameraOffset *camera_offset;
PathOffset *path_offset;
};
selfdrive/ui/qt/offroad/sunnypilot/display_settings.cc
+185
View File
@@ -0,0 +1,185 @@
#include "selfdrive/ui/qt/offroad/sunnypilot/display_settings.h"
DisplayPanel::DisplayPanel(QWidget *parent) : ListWidget(parent, false) {
// param, title, desc, icon
std::vector<std::tuple<QString, QString, QString, QString>> toggle_defs{
{
"OnroadScreenOffEvent",
tr("Driving Screen Off: Non-Critical Events"),
QString("%1<br>"
"<h4>%2</h4><br>"
"<h4>%3</h4><br>")
.arg(tr("When <b>Driving Screen Off Timer</b> is not set to <b>\"Always On\"</b>:"))
.arg(tr("Enabled: Wake the brightness of the screen to display all events."))
.arg(tr("Disabled: Wake the brightness of the screen to display critical events.")),
"../assets/offroad/icon_blank.png",
},
{
"ScreenRecorder",
tr("Enable Screen Recorder"),
tr("Enable this will display a button on the onroad screen to toggle on or off real-time screen recording with UI elements."),
"../assets/offroad/icon_blank.png"
}
};
// General: Max Time Offroad (Shutdown timer)
auto max_time_offroad = new MaxTimeOffroad();
connect(max_time_offroad, &SPOptionControl::updateLabels, max_time_offroad, &MaxTimeOffroad::refresh);
addItem(max_time_offroad);
// General: Onroad Screen Off (Auto Onroad Screen Timer)
onroad_screen_off = new OnroadScreenOff();
onroad_screen_off->setUpdateOtherToggles(true);
connect(onroad_screen_off, &SPOptionControl::updateLabels, onroad_screen_off, &OnroadScreenOff::refresh);
connect(onroad_screen_off, &SPOptionControl::updateOtherToggles, this, &DisplayPanel::updateToggles);
addItem(onroad_screen_off);
// General: Onroad Screen Off Brightness
onroad_screen_off_brightness = new OnroadScreenOffBrightness();
connect(onroad_screen_off_brightness, &SPOptionControl::updateLabels, onroad_screen_off_brightness, &OnroadScreenOffBrightness::refresh);
addItem(onroad_screen_off_brightness);
// General: Brightness Control (Global)
auto brightness_control = new BrightnessControl();
connect(brightness_control, &SPOptionControl::updateLabels, brightness_control, &BrightnessControl::refresh);
for (auto &[param, title, desc, icon] : toggle_defs) {
auto toggle = new ParamControl(param, title, desc, icon, this);
addItem(toggle);
toggles[param.toStdString()] = toggle;
if (param == "OnroadScreenOffEvent") {
// General: Brightness Control (Global)
addItem(brightness_control);
}
}
}
void DisplayPanel::showEvent(QShowEvent *event) {
updateToggles();
}
void DisplayPanel::updateToggles() {
if (!isVisible()) {
return;
}
// toggle names to update when OnroadScreenOff is toggled
onroad_screen_off_brightness->setVisible(QString::fromStdString(params.get("OnroadScreenOff")) != "-2");
toggles["OnroadScreenOffEvent"]->setVisible(QString::fromStdString(params.get("OnroadScreenOff")) != "-2");
}
// Max Time Offroad (Shutdown timer)
MaxTimeOffroad::MaxTimeOffroad() : SPOptionControl (
"MaxTimeOffroad",
tr("Max Time Offroad"),
tr("Device is automatically turned off after a set time when the engine is turned off (off-road) after driving (on-road)."),
"../assets/offroad/icon_blank.png",
{0, 12}) {
refresh();
}
void MaxTimeOffroad::refresh() {
QString option = QString::fromStdString(params.get("MaxTimeOffroad"));
QString second = tr("s");
QString minute = tr("m");
QString hour = tr("hr");
if (option == "0") {
setLabel(tr("Always On"));
} else if (option == "1") {
setLabel(tr("Immediate"));
} else if (option == "2") {
setLabel("30 " + second);
} else if (option == "3") {
setLabel("1 " + minute);
} else if (option == "4") {
setLabel("3 " + minute);
} else if (option == "5") {
setLabel("5 " + minute);
} else if (option == "6") {
setLabel("10 " + minute);
} else if (option == "7") {
setLabel("30 " + minute);
} else if (option == "8") {
setLabel("1 " + hour);
} else if (option == "9") {
setLabel("3 " + hour);
} else if (option == "10") {
setLabel("5 " + hour);
} else if (option == "11") {
setLabel("10 " + hour);
} else if (option == "12") {
setLabel("30 " + hour);
}
}
// Onroad Screen Off (Auto Onroad Screen Timer)
OnroadScreenOff::OnroadScreenOff() : SPOptionControl (
"OnroadScreenOff",
tr("Driving Screen Off Timer"),
tr("Turn off the device screen or reduce brightness to protect the screen after driving starts. It automatically brightens or turns on when a touch or event occurs."),
"../assets/offroad/icon_blank.png",
{-2, 10}) {
refresh();
}
void OnroadScreenOff::refresh() {
QString option = QString::fromStdString(params.get("OnroadScreenOff"));
QString second = tr("s");
QString minute = tr("min");
if (option == "-2") {
setLabel(tr("Always On"));
} else if (option == "-1") {
setLabel("15 " + second);
} else if (option == "0") {
setLabel("30 " + second);
} else {
setLabel(option + " " + minute);
}
}
// Onroad Screen Off Brightness
OnroadScreenOffBrightness::OnroadScreenOffBrightness() : SPOptionControl (
"OnroadScreenOffBrightness",
tr("Driving Screen Off Brightness (%)"),
tr("When using the Driving Screen Off feature, the brightness is reduced according to the automatic brightness ratio."),
"../assets/offroad/icon_blank.png",
{0, 100},
10) {
refresh();
}
void OnroadScreenOffBrightness::refresh() {
QString option = QString::fromStdString(params.get("OnroadScreenOffBrightness"));
if (option == "0") {
setLabel(tr("Dark"));
} else {
setLabel(option);
}
}
// Brightness Control (Global)
BrightnessControl::BrightnessControl() : SPOptionControl (
"BrightnessControl",
tr("Brightness"),
tr("Manually adjusts the global brightness of the screen."),
"../assets/offroad/icon_blank.png",
{0, 100},
5) {
refresh();
}
void BrightnessControl::refresh() {
QString option = QString::fromStdString(params.get("BrightnessControl"));
QString percentage = " %";
if (option == "0") {
setLabel(tr("Auto"));
} else {
setLabel(option + percentage);
}
}
selfdrive/ui/qt/offroad/sunnypilot/display_settings.h
+69
View File
@@ -0,0 +1,69 @@
#pragma once
#include "selfdrive/ui/qt/widgets/controls.h"
class OnroadScreenOff : public SPOptionControl {
Q_OBJECT
public:
OnroadScreenOff();
void refresh();
private:
Params params;
};
class OnroadScreenOffBrightness : public SPOptionControl {
Q_OBJECT
public:
OnroadScreenOffBrightness();
void refresh();
private:
Params params;
};
class MaxTimeOffroad : public SPOptionControl {
Q_OBJECT
public:
MaxTimeOffroad();
void refresh();
private:
Params params;
};
class BrightnessControl : public SPOptionControl {
Q_OBJECT
public:
BrightnessControl();
void refresh();
private:
Params params;
};
class DisplayPanel : public ListWidget {
Q_OBJECT
public:
explicit DisplayPanel(QWidget *parent = nullptr);
void showEvent(QShowEvent *event) override;
public slots:
void updateToggles();
private:
Params params;
std::map<std::string, ParamControl*> toggles;
OnroadScreenOff *onroad_screen_off;
OnroadScreenOffBrightness *onroad_screen_off_brightness;
};
selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc
+107
View File
@@ -0,0 +1,107 @@
#include "selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h"
LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) {
QVBoxLayout* main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(50, 20, 50, 20);
main_layout->setSpacing(20);
// Back button
PanelBackButton* back = new PanelBackButton(tr("Back"));
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
main_layout->addWidget(back, 0, Qt::AlignLeft);
ListWidget *list = new ListWidget(this, false);
// param, title, desc, icon
std::vector<std::tuple<QString, QString, QString, QString>> toggle_defs{
{
"AutoLaneChangeBsmDelay",
tr("Auto Lane Change: Delay with Blind Spot"),
tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
"../assets/offroad/icon_blank.png",
},
{
"RoadEdge",
tr("Block Lane Change: Road Edge Detection"),
tr("Enable this toggle to block lane change when road edge is detected on the stalk actuated side."),
"../assets/offroad/icon_blank.png",
}
};
// Controls: Auto Lane Change Timer
auto_lane_change_timer = new AutoLaneChangeTimer();
auto_lane_change_timer->setUpdateOtherToggles(true);
auto_lane_change_timer->showDescription();
connect(auto_lane_change_timer, &SPOptionControl::updateLabels, auto_lane_change_timer, &AutoLaneChangeTimer::refresh);
connect(auto_lane_change_timer, &AutoLaneChangeTimer::updateOtherToggles, this, &LaneChangeSettings::updateToggles);
list->addItem(auto_lane_change_timer);
for (auto &[param, title, desc, icon] : toggle_defs) {
auto toggle = new ParamControl(param, title, desc, icon, this);
list->addItem(toggle);
toggles[param.toStdString()] = toggle;
// trigger offroadTransition when going onroad/offroad
connect(uiState(), &UIState::offroadTransition, toggle, &ParamControl::setEnabled);
}
main_layout->addWidget(new ScrollView(list, this));
}
void LaneChangeSettings::showEvent(QShowEvent *event) {
updateToggles();
}
void LaneChangeSettings::updateToggles() {
if (!isVisible()) {
return;
}
auto auto_lane_change_bsm_delay_toggle = toggles["AutoLaneChangeBsmDelay"];
auto auto_lane_change_timer_param = QString::fromStdString(params.get("AutoLaneChangeTimer"));
auto cp_bytes = params.get("CarParamsPersistent");
if (!cp_bytes.empty()) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
if (!CP.getEnableBsm()) {
params.remove("AutoLaneChangeBsmDelay");
}
auto_lane_change_bsm_delay_toggle->setEnabled(CP.getEnableBsm() && auto_lane_change_timer_param != "0");
auto_lane_change_bsm_delay_toggle->refresh();
} else {
auto_lane_change_bsm_delay_toggle->setEnabled(false);
}
}
// Auto Lane Change Timer (ALCT)
AutoLaneChangeTimer::AutoLaneChangeTimer() : SPOptionControl (
"AutoLaneChangeTimer",
tr("Auto Lane Change Timer"),
tr("Set a timer to delay the auto lane change operation when the blinker is used. No nudge on the steering wheel is required to auto lane change if a timer is set.\nPlease use caution when using this feature. Only use the blinker when traffic and road conditions permit."),
"../assets/offroad/icon_blank.png",
{0, 5}) {
refresh();
}
void AutoLaneChangeTimer::refresh() {
QString option = QString::fromStdString(params.get("AutoLaneChangeTimer"));
QString second = tr("s");
if (option == "0") {
setLabel(tr("Nudge"));
} else if (option == "1") {
setLabel(tr("Nudgeless"));
} else if (option == "2") {
setLabel("0.5 " + second);
} else if (option == "3") {
setLabel("1 " + second);
} else if (option == "4") {
setLabel("1.5 " + second);
} else {
setLabel("2 " + second);
}
}
selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h
+40
View File
@@ -0,0 +1,40 @@
#pragma once
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
class AutoLaneChangeTimer : public SPOptionControl {
Q_OBJECT
public:
AutoLaneChangeTimer();
void refresh();
signals:
void toggleUpdated();
private:
Params params;
};
class LaneChangeSettings : public QWidget {
Q_OBJECT
public:
explicit LaneChangeSettings(QWidget* parent = nullptr);
void showEvent(QShowEvent *event) override;
signals:
void backPress();
public slots:
void updateToggles();
private:
Params params;
std::map<std::string, ParamControl*> toggles;
AutoLaneChangeTimer *auto_lane_change_timer;
};
selfdrive/ui/qt/offroad/sunnypilot/mads_settings.cc
+76
View File
@@ -0,0 +1,76 @@
#include "selfdrive/ui/qt/offroad/sunnypilot/mads_settings.h"
MadsSettings::MadsSettings(QWidget* parent) : QWidget(parent) {
QVBoxLayout* main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(50, 20, 50, 20);
main_layout->setSpacing(20);
// Back button
PanelBackButton* back = new PanelBackButton();
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
main_layout->addWidget(back, 0, Qt::AlignLeft);
ListWidget *list = new ListWidget(this, false);
// param, title, desc, icon
std::vector<std::tuple<QString, QString, QString, QString>> toggle_defs{
{
"AccMadsCombo",
tr("Enable ACC+MADS with RES+/SET-"),
QString("%1<br>"
"<h4>%2</h4>")
.arg(tr("Engage both M.A.D.S. and ACC with a single press of RES+ or SET- button."))
.arg(tr("Note: Once M.A.D.S. is engaged via this mode, it will remain engaged until it is manually disabled via the M.A.D.S. button or car shut off.")),
"../assets/offroad/icon_blank.png",
},
{
"MadsCruiseMain",
tr("Toggle M.A.D.S. with Cruise Main"),
tr("Allows M.A.D.S. engagement/disengagement with \"Cruise Main\" cruise control button from the steering wheel."),
"../assets/offroad/icon_blank.png",
}
};
// Disengage Lateral on Brake (DLOB)
std::vector<QString> dlob_settings_texts{tr("Remain Active"), tr("Pause Steering")};
dlob_settings = new ButtonParamControl(
"DisengageLateralOnBrake", "Steering Mode After Braking", "Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.\n\nRemain Active: ALC will remain active even after the brake pedal is pressed.\nPause Steering: ALC will be paused after the brake pedal is manually pressed.",
"../assets/offroad/icon_blank.png",
dlob_settings_texts,
500
);
dlob_settings->showDescription();
list->addItem(dlob_settings);
for (auto &[param, title, desc, icon] : toggle_defs) {
auto toggle = new ParamControl(param, title, desc, icon, this);
list->addItem(toggle);
toggles[param.toStdString()] = toggle;
// trigger offroadTransition when going onroad/offroad
connect(uiState(), &UIState::offroadTransition, toggle, &ParamControl::setEnabled);
}
// trigger offroadTransition when going onroad/offroad
connect(uiState(), &UIState::offroadTransition, dlob_settings, &ButtonParamControl::setEnabled);
main_layout->addWidget(new ScrollView(list, this));
}
void MadsSettings::showEvent(QShowEvent *event) {
updateToggles();
}
void MadsSettings::updateToggles() {
if (!isVisible()) {
return;
}
const bool is_offroad = !uiState()->scene.started;
const bool enable_mads = params.getBool("EnableMads");
const bool enabled = is_offroad && enable_mads;
toggles["AccMadsCombo"]->setEnabled(enabled);
toggles["MadsCruiseMain"]->setEnabled(enabled);
dlob_settings->setEnabled(enabled);
}

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