mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-07 06:35:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into dev-priv/master

Browse Source 
# Conflicts:
#	panda
#	selfdrive/car/car_helpers.py
#	selfdrive/car/toyota/interface.py
#	selfdrive/ui/ui.cc
This commit is contained in:
Jason Wen
2023-03-24 15:24:05 -04:00
parent c726a82eaf 18a5cf0277
commit ee0dfacb93
102 changed files with 1710 additions and 1010 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+4 -3
View File
@@ -108,7 +108,10 @@ Directory Structure
├── clocksd # Broadcasts current time
├── hardware # Hardware abstraction classes
├── logcatd # systemd journal as a service
└── proclogd # Logs information from /proc
├── loggerd # Logger and uploader of car data
├── proclogd # Logs information from /proc
├── sensord # IMU interface code
└── ubloxd # u-blox GNSS module interface code
└── selfdrive # Code needed to drive the car
├── assets # Fonts, images, and sounds for UI
├── athena # Allows communication with the app
@@ -117,12 +120,10 @@ Directory Structure
├── controls # Planning and controls
├── debug # Tools to help you debug and do car ports
├── locationd # Precise localization and vehicle parameter estimation
├── loggerd # Logger and uploader of car data
├── manager # Daemon that starts/stops all other daemons as needed
├── modeld # Driving and monitoring model runners
├── monitoring # Daemon to determine driver attention
├── navd # Turn-by-turn navigation
├── sensord # IMU interface code
├── test # Unit tests, system tests, and a car simulator
└── ui # The UI
RELEASES.md
+1
View File
@@ -1,6 +1,7 @@
Version 0.9.2 (2023-03-XX)
========================
* Draw MPC path instead of model predicted path, this is a more accurate representation of what the car will do.
* Buick LaCrosse 2017-19 support thanks to koch-cf!
* Škoda Fabia 2022-23 support thanks to jyoung8607!
Version 0.9.1 (2023-02-28)
cereal
+1 -1
Submodule cereal updated: 7968cfe989...b53ed5bd84
common/params.cc
+5 -3
View File
@@ -98,6 +98,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CurrentBootlog", PERSISTENT},
{"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"DisablePowerDown", PERSISTENT},
@@ -110,7 +111,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"DoReboot", CLEAR_ON_MANAGER_START},
{"DoShutdown", CLEAR_ON_MANAGER_START},
{"DoUninstall", CLEAR_ON_MANAGER_START},
{"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"ForcePowerDown", CLEAR_ON_MANAGER_START},
{"GitBranch", PERSISTENT},
{"GitCommit", PERSISTENT},
@@ -137,7 +138,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"IsReleaseBranch", CLEAR_ON_MANAGER_START},
{"IsUpdateAvailable", CLEAR_ON_MANAGER_START},
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"LaikadEphemerisV2", PERSISTENT | DONT_LOG},
{"LaikadEphemerisV3", PERSISTENT | DONT_LOG},
{"LanguageSetting", PERSISTENT},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
@@ -154,7 +155,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"NavSettingTime24h", PERSISTENT},
{"NavSettingLeftSide", PERSISTENT},
{"NavdRender", PERSISTENT},
{"ObdMultiplexingDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"PandaSignatures", CLEAR_ON_MANAGER_START},
docs/CARS.md
+4 -3
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system.
# 238 Supported Cars
# 239 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -17,6 +17,7 @@ A supported vehicle is one that just works when you install a comma three. All s
|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q3 2019-23">J533</a>||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=RS3 2018">J533</a>||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=S3 2015-17">J533</a>||
|Buick|LaCrosse 2017-19[<sup>3</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Buick&model=LaCrosse 2017-19">OBD-II</a>||
|Cadillac|Escalade 2017[<sup>3</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade 2017">OBD-II</a>||
|Cadillac|Escalade ESV 2016[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade ESV 2016">OBD-II</a>||
|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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EUV 2022-23">GM</a>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
@@ -116,7 +117,7 @@ A supported vehicle is one that just works when you install a comma three. All s
|Kia|Seltos 2021|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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Seltos 2021">Hyundai A</a>||
|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2018">Hyundai C</a>|<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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2019">Hyundai E</a>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Kia|Sorento 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2022-23">Hyundai K</a>||
|Kia|Sorento 2021-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2021-23">Hyundai K</a>||
|Kia|Sorento Plug-in Hybrid 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Hyundai A</a>||
|Kia|Sportage 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage 2023">Hyundai N</a>||
|Kia|Sportage Hybrid 2023[<sup>5</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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage Hybrid 2023">Hyundai N</a>||
@@ -148,7 +149,7 @@ A supported vehicle is one that just works when you install a comma three. All s
|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Leaf 2018-22">Nissan A</a>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Rogue 2018-20">Nissan A</a>||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=X-Trail 2017">Nissan A</a>||
|Ram|1500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-22">Ram</a>||
|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-23">Ram</a>||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Ateca 2018">J533</a>||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Leon 2014-20">J533</a>||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Ascent 2019-21">Subaru A</a>||
laika_repo
+1 -1
Submodule laika_repo updated: b740b71c82...6fadabd860
opendbc
+1 -1
Submodule opendbc updated: b8895f7c16...51bd657bb9
panda
+1 -1
Submodule panda updated: ad2a0fe0cb...4b814c7381
poetry.lock
Generated
+101 -39
View File
@@ -113,10 +113,12 @@ category = "dev"
optional = false
python-versions = "*"
[package.dependencies]
packaging = ">20.6"
[package.source]
type = "url"
url = "https://github.com/commaai/apex/releases/download/pytorch1.10.0%2Bcu11.1/apex-0.1-cp38-cp38-linux_x86_64.whl"
url = "https://github.com/commaai/apex/releases/download/pytorch2.0.0%2Bcu11.8/apex-0.1-cp38-cp38-linux_x86_64.whl"
[[package]]
name = "appdirs"
version = "1.4.4"
@@ -600,6 +602,17 @@ category = "dev"
optional = false
python-versions = ">=3.6"
[[package]]
name = "cmake"
version = "3.26.0"
description = "CMake is an open-source, cross-platform family of tools designed to build, test and package software"
category = "dev"
optional = false
python-versions = "*"
[package.extras]
test = ["codecov (>=2.0.5)", "coverage (>=4.2)", "flake8 (>=3.0.4)", "path.py (>=11.5.0)", "pytest (>=3.0.3)", "pytest-cov (>=2.4.0)", "pytest-runner (>=2.9)", "pytest-virtualenv (>=1.7.0)", "scikit-build (>=0.10.0)", "setuptools (>=28.0.0)", "virtualenv (>=15.0.3)", "wheel"]
[[package]]
name = "colorama"
version = "0.4.5"
@@ -716,8 +729,8 @@ ssh = ["bcrypt (>=3.1.5)"]
test = ["hypothesis (>=1.11.4,!=3.79.2)", "iso8601", "pretend", "pytest (>=6.2.0)", "pytest-benchmark", "pytest-cov", "pytest-subtests", "pytest-xdist", "pytz"]
[[package]]
name = "cupy-cuda113"
version = "10.6.0"
name = "cupy-cuda11x"
version = "11.6.0"
description = "CuPy: NumPy & SciPy for GPU"
category = "dev"
optional = false
@@ -725,12 +738,12 @@ python-versions = ">=3.7"
[package.dependencies]
fastrlock = ">=0.5"
numpy = ">=1.18,<1.25"
numpy = ">=1.20,<1.27"
[package.extras]
all = ["Cython (>=0.29.22,<3)", "optuna (>=2.0)", "scipy (>=1.4,<1.11)"]
all = ["Cython (>=0.29.22,<3)", "optuna (>=2.0)", "scipy (>=1.6,<1.12)"]
stylecheck = ["autopep8 (==1.5.5)", "flake8 (==3.8.4)", "mypy (==0.950)", "pbr (==5.5.1)", "pycodestyle (==2.6.0)", "types-setuptools (==57.4.14)"]
test = ["hypothesis (>=6.37.2)", "pytest (>=6.2)"]
test = ["hypothesis (>=6.37.2,<6.55.0)", "pytest (>=7.2)"]
[[package]]
name = "cycler"
@@ -1898,6 +1911,14 @@ category = "main"
optional = false
python-versions = "*"
[[package]]
name = "lit"
version = "15.0.7"
description = "A Software Testing Tool"
category = "dev"
optional = false
python-versions = "*"
[[package]]
name = "lockfile"
version = "0.12.2"
@@ -2586,8 +2607,7 @@ numpy = [
[package.source]
type = "url"
url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu113/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl"
url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu118/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl"
[[package]]
name = "osmium"
version = "3.4.1"
@@ -4220,19 +4240,26 @@ python-versions = ">=3.6,<4.0"
[[package]]
name = "torch"
version = "1.11.0+cu113"
version = "2.0.0+cu118"
description = "Tensors and Dynamic neural networks in Python with strong GPU acceleration"
category = "dev"
optional = false
python-versions = ">=3.7.0"
python-versions = ">=3.8.0"
[package.dependencies]
filelock = "*"
jinja2 = "*"
networkx = "*"
sympy = "*"
triton = {version = "2.0.0", markers = "platform_system == \"Linux\" and platform_machine == \"x86_64\""}
typing-extensions = "*"
[package.extras]
opt-einsum = ["opt-einsum (>=3.3)"]
[package.source]
type = "url"
url = "https://download.pytorch.org/whl/cu113/torch-1.11.0%2Bcu113-cp38-cp38-linux_x86_64.whl"
url = "https://download.pytorch.org/whl/cu118/torch-2.0.0%2Bcu118-cp38-cp38-linux_x86_64.whl"
[[package]]
name = "torchsummary"
version = "1.5.1"
@@ -4243,26 +4270,24 @@ python-versions = "*"
[[package]]
name = "torchvision"
version = "0.12.0+cu113"
version = "0.15.1+cu118"
description = "image and video datasets and models for torch deep learning"
category = "dev"
optional = false
python-versions = ">=3.7"
python-versions = ">=3.8"
[package.dependencies]
numpy = "*"
pillow = ">=5.3.0,<8.3.0 || >=8.4.0"
requests = "*"
torch = "1.11.0"
typing-extensions = "*"
torch = "2.0.0"
[package.extras]
scipy = ["scipy"]
[package.source]
type = "url"
url = "https://download.pytorch.org/whl/cu113/torchvision-0.12.0%2Bcu113-cp38-cp38-linux_x86_64.whl"
url = "https://download.pytorch.org/whl/cu118/torchvision-0.15.1%2Bcu118-cp38-cp38-linux_x86_64.whl"
[[package]]
name = "tornado"
version = "6.2"
@@ -4302,16 +4327,22 @@ test = ["pre-commit", "pytest"]
[[package]]
name = "triton"
version = "1.1.1"
version = "2.0.0"
description = "A language and compiler for custom Deep Learning operations"
category = "dev"
optional = false
python-versions = "*"
[package.dependencies]
cmake = "*"
filelock = "*"
lit = "*"
torch = "*"
[package.extras]
tests = ["autopep8", "flake8", "isort", "numpy", "pytest", "scipy (>=1.7.1)"]
tutorials = ["matplotlib", "pandas", "tabulate"]
[[package]]
name = "types-atomicwrites"
version = "1.4.5.1"
@@ -4357,9 +4388,9 @@ types-urllib3 = "<1.27"
[[package]]
name = "types-tabulate"
version = "0.9.0.1"
version = "0.8.11"
description = "Typing stubs for tabulate"
category = "main"
category = "dev"
optional = false
python-versions = "*"
@@ -4575,7 +4606,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"]
[metadata]
lock-version = "1.1"
python-versions = "~3.8"
content-hash = "669485055bf8d77336509cb7a3878e06aa32431c520825948914d76b57347fde"
content-hash = "a16ec604c5251dc860a44f7133289d0b7b16120757f8c903f3917329753c52d1"
[metadata.files]
adal = [
@@ -5040,6 +5071,25 @@ cloudpickle = [
{file = "cloudpickle-2.2.0-py3-none-any.whl", hash = "sha256:7428798d5926d8fcbfd092d18d01a2a03daf8237d8fcdc8095d256b8490796f0"},
{file = "cloudpickle-2.2.0.tar.gz", hash = "sha256:3f4219469c55453cfe4737e564b67c2a149109dabf7f242478948b895f61106f"},
]
cmake = [
{file = "cmake-3.26.0-py2.py3-none-macosx_10_10_universal2.macosx_10_10_x86_64.macosx_11_0_arm64.macosx_11_0_universal2.whl", hash = "sha256:4881727389325af84e39f1ec646b7249d8910b4ed637205bee6d589cb2b2ebd2"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2010_i686.manylinux_2_12_i686.whl", hash = "sha256:babd1e38c85d38a4bf4164c3126ec8cf4cd8d374072e2a4a181e52e953007f8c"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2010_x86_64.manylinux_2_12_x86_64.whl", hash = "sha256:1851be29d79bb39505954165e934d31994268d49f566ead6fff840a5092e444d"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:483aaaeb9535deaa2657c928af5d9f0da9329f89bc249f494923495745a03677"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2014_i686.manylinux_2_17_i686.whl", hash = "sha256:2f78c6194f224d462333e08d3acd571c553b58d04935971b87efbe76f241353c"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2014_ppc64le.manylinux_2_17_ppc64le.whl", hash = "sha256:cf129c8b71f1344975f179f30287baa8804c4c61ff1b13003244b4157b676e13"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2014_s390x.manylinux_2_17_s390x.whl", hash = "sha256:fabb82c69223eda27e673c5c6bb02985d60cf0baa631a2e4932eed87e8229928"},
{file = "cmake-3.26.0-py2.py3-none-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:502cbed2335557920b88687c6f3ff4ce433bd416204c928ad489e399e76149f5"},
{file = "cmake-3.26.0-py2.py3-none-musllinux_1_1_aarch64.whl", hash = "sha256:b81c7775b125786c1696232078e9ceb0c9c66d26fb0a2259f40e33983170f924"},
{file = "cmake-3.26.0-py2.py3-none-musllinux_1_1_i686.whl", hash = "sha256:f4cc1dde7613cc813d15fd840a33e18cd07a443aa67205d74b9b05f55f08a459"},
{file = "cmake-3.26.0-py2.py3-none-musllinux_1_1_ppc64le.whl", hash = "sha256:ac6ca9d9ff58900138bbbebc55857647fb99f1c40b84d8a232f23b0d27d7f48a"},
{file = "cmake-3.26.0-py2.py3-none-musllinux_1_1_s390x.whl", hash = "sha256:85e0bad5aeb3a82919ed7d78b76a0462eafe2f918076a1823a09c6f37910f3e0"},
{file = "cmake-3.26.0-py2.py3-none-musllinux_1_1_x86_64.whl", hash = "sha256:5523b9701be367572155e77294bf8aa3ac9aa0d73c50dcd6c57903d12bcb308e"},
{file = "cmake-3.26.0-py2.py3-none-win32.whl", hash = "sha256:96f0e887260255eeb83bcf3465d51bc6c94078251c9312fa142dce6d3e80acac"},
{file = "cmake-3.26.0-py2.py3-none-win_amd64.whl", hash = "sha256:a0719a6f79cdc4d7b16caf757b8ae13eae1c6ce5a08d594cc09774afe129515d"},
{file = "cmake-3.26.0-py2.py3-none-win_arm64.whl", hash = "sha256:11159c9b64c6473d84361ab44ef2c85e1bd2db7a2b1b798ce8bf5988f32adf43"},
{file = "cmake-3.26.0.tar.gz", hash = "sha256:c18185c9cc147d0fa1e9228962aa37901b37866bd5d617e9efa23dfe706f7321"},
]
colorama = [
{file = "colorama-0.4.5-py2.py3-none-any.whl", hash = "sha256:854bf444933e37f5824ae7bfc1e98d5bce2ebe4160d46b5edf346a89358e99da"},
{file = "colorama-0.4.5.tar.gz", hash = "sha256:e6c6b4334fc50988a639d9b98aa429a0b57da6e17b9a44f0451f930b6967b7a4"},
@@ -5209,15 +5259,17 @@ cryptography = [
{file = "cryptography-37.0.4-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:4c590ec31550a724ef893c50f9a97a0c14e9c851c85621c5650d699a7b88f7ab"},
{file = "cryptography-37.0.4.tar.gz", hash = "sha256:63f9c17c0e2474ccbebc9302ce2f07b55b3b3fcb211ded18a42d5764f5c10a82"},
]
cupy-cuda113 = [
{file = "cupy_cuda113-10.6.0-cp310-cp310-manylinux1_x86_64.whl", hash = "sha256:27e5efe2c3afa80ff48654cb27f9e0eddb36f8b26ef0d32d3ba0a233e1359b51"},
{file = "cupy_cuda113-10.6.0-cp310-cp310-win_amd64.whl", hash = "sha256:8b96076d1ddd33fdb2c908ed0f8109caf69d37d36f839a8a8cdae1312508336f"},
{file = "cupy_cuda113-10.6.0-cp37-cp37m-manylinux1_x86_64.whl", hash = "sha256:22363c2863727cae5154aa4bab9e8a648d7fe66c9e2195d81dd4e8693c2e61ce"},
{file = "cupy_cuda113-10.6.0-cp37-cp37m-win_amd64.whl", hash = "sha256:8cc69b9d5735372477a7af3822c8f8e996ffe6de05cfc917500af9dc0117ca3e"},
{file = "cupy_cuda113-10.6.0-cp38-cp38-manylinux1_x86_64.whl", hash = "sha256:10dc6899577e445426d81f0960ba9059d9aaa750426997c61fad882d6345264c"},
{file = "cupy_cuda113-10.6.0-cp38-cp38-win_amd64.whl", hash = "sha256:c6893ac9040a11610e63973063dfd715dbda8bd07ef99951bab7a09c7f335e1e"},
{file = "cupy_cuda113-10.6.0-cp39-cp39-manylinux1_x86_64.whl", hash = "sha256:4bf4bc06d991c06b95f6fe558d117cafd93bd4eeaf80606f18dd31d20d2eff25"},
{file = "cupy_cuda113-10.6.0-cp39-cp39-win_amd64.whl", hash = "sha256:3745fc42dca86ba8a1109ddc7964aed8e1efc0ce8085cb2f140dcd6429f26354"},
cupy-cuda11x = [
{file = "cupy_cuda11x-11.6.0-cp310-cp310-manylinux1_x86_64.whl", hash = "sha256:1b9914f57868a1559e9bfabfbae8c724585914e8e1f277acb9cdb6aa0756eaa4"},
{file = "cupy_cuda11x-11.6.0-cp310-cp310-win_amd64.whl", hash = "sha256:ac8dd082ddb00996bc4d37cc5765907048f467aadb61bcbff25f3c2a88c50583"},
{file = "cupy_cuda11x-11.6.0-cp311-cp311-manylinux1_x86_64.whl", hash = "sha256:bf7496340dfbc2eaa3e0ebfd03c4ab8b1fc36d7d14f68718c33eeb395aaa6eed"},
{file = "cupy_cuda11x-11.6.0-cp311-cp311-win_amd64.whl", hash = "sha256:e3bbd55c26d60069d7e5af2200fb47e7c42ca6437acac297637a2d3c00f6fe46"},
{file = "cupy_cuda11x-11.6.0-cp37-cp37m-manylinux1_x86_64.whl", hash = "sha256:0d7ebcfd7234946719ad28890593d9fc78fad5753ef4b073e0d7bdb7da2f5640"},
{file = "cupy_cuda11x-11.6.0-cp37-cp37m-win_amd64.whl", hash = "sha256:3bc3f81fbc5a86c7155c6036809f2ada4023fc0870dce158d3d9f6d0b575727f"},
{file = "cupy_cuda11x-11.6.0-cp38-cp38-manylinux1_x86_64.whl", hash = "sha256:1d6810568e683cb153972b3019ddb5efc369036511122117d9eda09cf84d1042"},
{file = "cupy_cuda11x-11.6.0-cp38-cp38-win_amd64.whl", hash = "sha256:31c1ec72ffe9ad6fda9fb0a2aff1fcca38da66a8e521e333bae0d67ebc80ead0"},
{file = "cupy_cuda11x-11.6.0-cp39-cp39-manylinux1_x86_64.whl", hash = "sha256:6a4822daadfce0464cb619099eb82c7ba30ae7755d6869ba5eba1c675a9eed67"},
{file = "cupy_cuda11x-11.6.0-cp39-cp39-win_amd64.whl", hash = "sha256:3d0f44d747b38ce2b213d64270265f953b68cf29860ec5fa65848b9faaea3fc1"},
]
cycler = [
{file = "cycler-0.11.0-py3-none-any.whl", hash = "sha256:3a27e95f763a428a739d2add979fa7494c912a32c17c4c38c4d5f082cad165a3"},
@@ -6045,6 +6097,9 @@ libusb1 = [
{file = "libusb1-3.0.0-py3-none-win_amd64.whl", hash = "sha256:6f6bb010632ada35c661d17a65e135077beef0fbb2434d5ffdb3a4a911fd9490"},
{file = "libusb1-3.0.0.tar.gz", hash = "sha256:5792a9defee40f15d330a40d9b1800545c32e47ba7fc66b6f28f133c9fcc8538"},
]
lit = [
{file = "lit-15.0.7.tar.gz", hash = "sha256:ed08ac55afe714a193653df293ae8a6ee6c45d6fb11eeca72ce347d99b88ecc8"},
]
lockfile = [
{file = "lockfile-0.12.2-py2.py3-none-any.whl", hash = "sha256:6c3cb24f344923d30b2785d5ad75182c8ea7ac1b6171b08657258ec7429d50fa"},
{file = "lockfile-0.12.2.tar.gz", hash = "sha256:6aed02de03cba24efabcd600b30540140634fc06cfa603822d508d5361e9f799"},
@@ -7009,6 +7064,7 @@ pycryptodome = [
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2010_i686.whl", hash = "sha256:7c9ed8aa31c146bef65d89a1b655f5f4eab5e1120f55fc297713c89c9e56ff0b"},
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2010_x86_64.whl", hash = "sha256:5099c9ca345b2f252f0c28e96904643153bae9258647585e5e6f649bb7a1844a"},
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2014_aarch64.whl", hash = "sha256:2ec709b0a58b539a4f9d33fb8508264c3678d7edb33a68b8906ba914f71e8c13"},
{file = "pycryptodome-3.15.0-cp27-cp27m-musllinux_1_1_aarch64.whl", hash = "sha256:2ae53125de5b0d2c95194d957db9bb2681da8c24d0fb0fe3b056de2bcaf5d837"},
{file = "pycryptodome-3.15.0-cp27-cp27m-win32.whl", hash = "sha256:fd2184aae6ee2a944aaa49113e6f5787cdc5e4db1eb8edb1aea914bd75f33a0c"},
{file = "pycryptodome-3.15.0-cp27-cp27m-win_amd64.whl", hash = "sha256:7e3a8f6ee405b3bd1c4da371b93c31f7027944b2bcce0697022801db93120d83"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux1_i686.whl", hash = "sha256:b9c5b1a1977491533dfd31e01550ee36ae0249d78aae7f632590db833a5012b8"},
@@ -7016,12 +7072,14 @@ pycryptodome = [
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2010_i686.whl", hash = "sha256:2aa55aae81f935a08d5a3c2042eb81741a43e044bd8a81ea7239448ad751f763"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2010_x86_64.whl", hash = "sha256:c3640deff4197fa064295aaac10ab49a0d55ef3d6a54ae1499c40d646655c89f"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2014_aarch64.whl", hash = "sha256:045d75527241d17e6ef13636d845a12e54660aa82e823b3b3341bcf5af03fa79"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-musllinux_1_1_aarch64.whl", hash = "sha256:eb6fce570869e70cc8ebe68eaa1c26bed56d40ad0f93431ee61d400525433c54"},
{file = "pycryptodome-3.15.0-cp35-abi3-macosx_10_9_x86_64.whl", hash = "sha256:9ee40e2168f1348ae476676a2e938ca80a2f57b14a249d8fe0d3cdf803e5a676"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux1_i686.whl", hash = "sha256:4c3ccad74eeb7b001f3538643c4225eac398c77d617ebb3e57571a897943c667"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux1_x86_64.whl", hash = "sha256:1b22bcd9ec55e9c74927f6b1f69843cb256fb5a465088ce62837f793d9ffea88"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2010_i686.whl", hash = "sha256:57f565acd2f0cf6fb3e1ba553d0cb1f33405ec1f9c5ded9b9a0a5320f2c0bd3d"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2010_x86_64.whl", hash = "sha256:4b52cb18b0ad46087caeb37a15e08040f3b4c2d444d58371b6f5d786d95534c2"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2014_aarch64.whl", hash = "sha256:092a26e78b73f2530b8bd6b3898e7453ab2f36e42fd85097d705d6aba2ec3e5e"},
{file = "pycryptodome-3.15.0-cp35-abi3-musllinux_1_1_aarch64.whl", hash = "sha256:50ca7e587b8e541eb6c192acf92449d95377d1f88908c0a32ac5ac2703ebe28b"},
{file = "pycryptodome-3.15.0-cp35-abi3-win32.whl", hash = "sha256:e244ab85c422260de91cda6379e8e986405b4f13dc97d2876497178707f87fc1"},
{file = "pycryptodome-3.15.0-cp35-abi3-win_amd64.whl", hash = "sha256:c77126899c4b9c9827ddf50565e93955cb3996813c18900c16b2ea0474e130e9"},
{file = "pycryptodome-3.15.0-pp27-pypy_73-macosx_10_9_x86_64.whl", hash = "sha256:9eaadc058106344a566dc51d3d3a758ab07f8edde013712bc8d22032a86b264f"},
@@ -8032,11 +8090,15 @@ traitlets = [
{file = "traitlets-5.5.0.tar.gz", hash = "sha256:b122f9ff2f2f6c1709dab289a05555be011c87828e911c0cf4074b85cb780a79"},
]
triton = [
{file = "triton-1.1.1-cp36-cp36m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8441e6f44517aef8f6345f621c003926cbe970892802411a949ccda516cbd5ba"},
{file = "triton-1.1.1-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:840776bc1f4757fb2d6af974694c5e5313220ceec238ee6118b9728bc2aa9ade"},
{file = "triton-1.1.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:97d42cdaa7d56de463d762c18cc876bfd0828a2b6a706263393fe7e10d1c83ca"},
{file = "triton-1.1.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ebc19c0e902bbf7d29de4d444455608065a2c56e3524f4bc94e724511ca518f3"},
{file = "triton-1.1.1-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f02f798cd2dd922228082ce1a4e9d81badb9a6217a9aac6d783e95bf7055974d"},
{file = "triton-2.0.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8f05a7e64e4ca0565535e3d5d3405d7e49f9d308505bb7773d21fb26a4c008c2"},
{file = "triton-2.0.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:bb4b99ca3c6844066e516658541d876c28a5f6e3a852286bbc97ad57134827fd"},
{file = "triton-2.0.0-cp36-cp36m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:47b4d70dc92fb40af553b4460492c31dc7d3a114a979ffb7a5cdedb7eb546c08"},
{file = "triton-2.0.0-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fedce6a381901b1547e0e7e1f2546e4f65dca6d91e2d8a7305a2d1f5551895be"},
{file = "triton-2.0.0-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:75834f27926eab6c7f00ce73aaf1ab5bfb9bec6eb57ab7c0bfc0a23fac803b4c"},
{file = "triton-2.0.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0117722f8c2b579cd429e0bee80f7731ae05f63fe8e9414acd9a679885fcbf42"},
{file = "triton-2.0.0-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:bcd9be5d0c2e45d2b7e6ddc6da20112b6862d69741576f9c3dbaf941d745ecae"},
{file = "triton-2.0.0-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:42a0d2c3fc2eab4ba71384f2e785fbfd47aa41ae05fa58bf12cb31dcbd0aeceb"},
{file = "triton-2.0.0-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:52c47b72c72693198163ece9d90a721299e4fb3b8e24fd13141e384ad952724f"},
]
types-atomicwrites = [
{file = "types-atomicwrites-1.4.5.1.tar.gz", hash = "sha256:9e9f0923ebf93524b28bcece5a23ac8c3820f39b060df29f671936d2e4bc04bc"},
@@ -8059,8 +8121,8 @@ types-requests = [
{file = "types_requests-2.28.11.2-py3-none-any.whl", hash = "sha256:14941f8023a80b16441b3b46caffcbfce5265fd14555844d6029697824b5a2ef"},
]
types-tabulate = [
{file = "types-tabulate-0.9.0.1.tar.gz", hash = "sha256:e486292c279f19247865bdabe802419740a0e74b53444e7f7a8009e08129da5d"},
{file = "types_tabulate-0.9.0.1-py3-none-any.whl", hash = "sha256:be2ea0de05f615ccfcbadf6206aa720e265955eb1de23e343aec9d8bf3fa9aaa"},
{file = "types-tabulate-0.8.11.tar.gz", hash = "sha256:17a5fa3b5ca453815778fc9865e8ecd0118b07b2b9faff3e2b06fe448174dd5e"},
{file = "types_tabulate-0.8.11-py3-none-any.whl", hash = "sha256:af811268241e8fb87b63c052c87d1e329898a93191309d5d42111372232b2e0e"},
]
types-urllib3 = [
{file = "types-urllib3-1.26.25.1.tar.gz", hash = "sha256:a948584944b2412c9a74b9cf64f6c48caf8652cb88b38361316f6d15d8a184cd"},
pyproject.toml
+7 -7
View File
@@ -59,7 +59,6 @@ urllib3 = "^1.26.10"
utm = "^0.7.0"
websocket_client = "^1.3.3"
polyline = "^1.4.0"
types-tabulate = "^0.9.0.1"
[tool.poetry.group.dev.dependencies]
@@ -84,7 +83,7 @@ mypy = "^0.961"
myst-parser = "^0.18.0"
natsort = "^8.1.0"
numpy = "^1.23.0"
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu113/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl", platform = "linux" }
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu118/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl", platform = "linux" }
pandas = "^1.4.3"
parameterized = "^0.8.1"
paramiko = "^2.11.0"
@@ -108,6 +107,7 @@ types-certifi = "^2021.10.8"
types-pycurl = "^7.45.1"
types-PyYAML = "^6.0"
types-requests = "^2.28.11"
types-tabulate = "^0.8.10"
[tool.poetry.group.xx]
@@ -117,7 +117,7 @@ optional = true
aenum = "^3.1.11"
aiohttp = "^3.8.1"
albumentations = "^1.2.1"
apex = { url = "https://github.com/commaai/apex/releases/download/pytorch1.10.0%2Bcu11.1/apex-0.1-cp38-cp38-linux_x86_64.whl" }
apex = { url = "https://github.com/commaai/apex/releases/download/pytorch2.0.0%2Bcu11.8/apex-0.1-cp38-cp38-linux_x86_64.whl" }
azure-cli-core = "^2.38.0"
azure-common = "^1.1.28"
azure-core = "^1.24.2"
@@ -127,7 +127,7 @@ azure-storage-nspkg = "~3.1"
blosc = "==1.9.2"
cloudpickle = "^2.1.0"
configargparse = "^1.5.3"
cupy-cuda113 = "^10.6.0"
cupy-cuda11x = "^11.6.0"
datadog = "^0.44.0"
dotmap = "^1.3.30"
einops = "^0.5.0"
@@ -166,10 +166,10 @@ scikit-learn = "^1.1.1"
segmentation-models-pytorch = "==0.2.1"
simplejson = "^3.17.6"
SQLAlchemy = "^1.4.39"
torch = { url = "https://download.pytorch.org/whl/cu113/torch-1.11.0%2Bcu113-cp38-cp38-linux_x86_64.whl" }
torch = { url = "https://download.pytorch.org/whl/cu118/torch-2.0.0%2Bcu118-cp38-cp38-linux_x86_64.whl" }
torchsummary = "^1.5.1"
torchvision = { url = "https://download.pytorch.org/whl/cu113/torchvision-0.12.0%2Bcu113-cp38-cp38-linux_x86_64.whl" }
triton = "^1.1.1"
torchvision = { url = "https://download.pytorch.org/whl/cu118/torchvision-0.15.1%2Bcu118-cp38-cp38-linux_x86_64.whl" }
triton = "^2.0.0"
Werkzeug = "^2.1.2"
zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" }
omegaconf = "^2.3.0"
selfdrive/boardd/boardd.cc
+16 -13
View File
@@ -113,32 +113,35 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
return false;
}
// set to ELM327 for fingerprinting
// initialize to ELM327 without OBD multiplexing for fingerprinting
bool obd_multiplexing_enabled = false;
for (int i = 0; i < pandas.size(); i++) {
const uint16_t safety_param = (i > 0) ? 1U : 0U;
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
}
// wait for FW query at OBD port to finish
// openpilot can switch between multiplexing modes for different FW queries
while (true) {
if (do_exit || !check_all_connected(pandas) || !ignition) {
return false;
}
if (p.getBool("FirmwareObdQueryDone")) {
LOGW("finished FW query at OBD port");
bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled");
if (obd_multiplexing_requested != obd_multiplexing_enabled) {
for (int i = 0; i < pandas.size(); i++) {
const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
}
obd_multiplexing_enabled = obd_multiplexing_requested;
p.putBool("ObdMultiplexingChanged", true);
}
if (p.getBool("FirmwareQueryDone")) {
LOGW("finished FW query");
break;
}
util::sleep_for(20);
}
// set to ELM327 to finish fingerprinting and for potential ECU knockouts
for (Panda *panda : pandas) {
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
}
p.putBool("ObdMultiplexingDisabled", true);
std::string params;
LOGW("waiting for params to set safety model");
while (true) {
selfdrive/boardd/tests/test_boardd_loopback.py
+1 -1
View File
@@ -51,7 +51,7 @@ class TestBoardd(unittest.TestCase):
cp.safetyConfigs = [safety_config]*num_pandas
params = Params()
params.put_bool("FirmwareObdQueryDone", True)
params.put_bool("FirmwareQueryDone", True)
params.put_bool("ControlsReady", True)
params.put("CarParams", cp.to_bytes())
selfdrive/car/car_helpers.py
+9 -6
View File
@@ -11,7 +11,7 @@ from system.version import is_comma_remote, is_tested_branch
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import disable_obd_multiplexing, get_fw_versions_ordered, match_fw_to_car, get_present_ecus
from selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
import selfdrive.sentry as sentry
from system.swaglog import cloudlog
import cereal.messaging as messaging
@@ -84,12 +84,13 @@ def fingerprint(logcan, sendcan, num_pandas):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
ecu_rx_addrs = set()
params = Params()
if not skip_fw_query:
# Vin query only reliably works through OBDII
bus = 1
cached_params = Params().get("CarParamsCache")
cached_params = params.get("CarParamsCache")
if cached_params is not None:
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carName == "mock":
@@ -102,6 +103,7 @@ def fingerprint(logcan, sendcan, num_pandas):
cached = True
else:
cloudlog.warning("Getting VIN & FW versions")
set_obd_multiplexing(params, True)
vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas)
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas)
@@ -117,10 +119,11 @@ def fingerprint(logcan, sendcan, num_pandas):
cloudlog.event("Malformed VIN", vin=vin, error=True)
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
params = Params()
params.put("CarVin", vin)
disable_obd_multiplexing(params)
# disable OBD multiplexing for potential ECU knockouts
set_obd_multiplexing(params, False)
params.put_bool("FirmwareQueryDone", True)
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
@@ -224,7 +227,7 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
candidate = car_model.decode("utf-8")
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True)
candidate = "mock"
y = threading.Thread(target=crash_log2, args=(fingerprints, car_fw,))
y.start()
selfdrive/car/chrysler/values.py
+119 -37
View File
@@ -50,6 +50,7 @@ class CarControllerParams:
self.STEER_DELTA_DOWN = 3
self.STEER_MAX = 261 # higher than this faults the EPS
STEER_THRESHOLD = 120
RAM_DT = {CAR.RAM_1500, }
@@ -62,6 +63,7 @@ class ChryslerCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC)"
harness: Enum = Harness.fca
CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"),
CAR.PACIFICA_2018_HYBRID: None, # same platforms
@@ -73,10 +75,10 @@ CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
],
CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"),
CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"),
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram),
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness=Harness.ram),
CAR.RAM_HD: [
ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram),
ChryslerCarInfo("Ram 3500 2020-22", harness=Harness.ram),
ChryslerCarInfo("Ram 3500 2019-22", harness=Harness.ram),
],
}
@@ -99,7 +101,7 @@ FINGERPRINTS = {
55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
},
{
55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
55: 8, 58: 6, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 956: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
}],
CAR.PACIFICA_2020: [{
55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8
@@ -180,79 +182,153 @@ FW_QUERY_CONFIG = FwQueryConfig(
)
FW_VERSIONS = {
CAR.RAM_1500: {
CAR.JEEP_CHEROKEE_2019: {
(Ecu.combinationMeter, 0x742, None): [
b'68294063AH',
b'68294063AG',
b'68434860AC',
b'68527375AD',
b'68453503AC',
b'68402971AD',
],
(Ecu.srs, 0x744, None): [
b'68441329AB',
b'68490898AA',
b'68428609AB',
b'68500728AA',
b'68355363AB',
],
(Ecu.abs, 0x747, None): [
b'68432418AD',
b'68432418AB',
b'68436004AE',
b'68438454AD',
b'68436004AD',
b'68535469AB',
b'68438454AC',
b'68408639AD',
],
(Ecu.fwdRadar, 0x753, None): [
b'68320950AL',
b'68320950AJ',
b'68454268AB',
b'68475160AG',
b'04672892AB',
b'68475160AE',
b'68456722AC',
],
(Ecu.eps, 0x75A, None): [
b'68273275AG',
b'68469901AA',
b'68552788AA',
b'68453431AA',
],
(Ecu.engine, 0x7e0, None): [
b'68448163AJ',
b'68500630AD',
b'68539650AD',
b'05035674AB ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035707AA',
],
},
CAR.RAM_1500: {
(Ecu.combinationMeter, 0x742, None): [
b'68294051AG',
b'68294051AI',
b'68294052AG',
b'68294063AG',
b'68294063AH',
b'68294063AI',
b'68434846AC',
b'68434858AC',
b'68434860AC',
b'68453503AC',
b'68453505AC',
b'68453511AC',
b'68453513AD',
b'68453514AD',
b'68510283AG',
b'68527375AD',
],
(Ecu.srs, 0x744, None): [
b'68428609AB',
b'68441329AB',
b'68473844AB',
b'68490898AA',
b'68500728AA',
b'68615033AA',
],
(Ecu.abs, 0x747, None): [
b'68292406AH',
b'68432418AB',
b'68432418AD',
b'68436004AD',
b'68436004AE',
b'68438454AC',
b'68438454AD',
b'68438456AE',
b'68438456AF',
b'68535469AB',
b'68535470AC',
b'68586307AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672892AB',
b'04672932AB',
b'68320950AH',
b'68320950AI',
b'68320950AJ',
b'68320950AL',
b'68320950AM',
b'68454268AB',
b'68475160AE',
b'68475160AF',
b'68475160AG',
],
(Ecu.eps, 0x75A, None): [
b'68273275AF',
b'68273275AG',
b'68312176AE',
b'68312176AG',
b'68440789AC',
b'68466110AB',
b'68469901AA',
b'68522583AB',
b'68522585AB',
b'68552788AA',
b'68552790AA',
b'68585112AB',
],
(Ecu.engine, 0x7e0, None): [
b'05036065AE ',
b'05036066AE ',
b'68378701AI ',
b'68378758AM ',
b'68448163AJ',
b'68448165AK',
b'68500630AD',
b'68500630AE',
b'68539650AD',
],
(Ecu.transmission, 0x7e1, None): [
b'68360078AL',
b'68384328AD',
b'68360085AL',
b'68360080AM',
b'68360081AM',
b'68502994AD',
b'68360085AL',
b'68384328AD',
b'68384332AD',
b'68445533AB',
b'68540431AB',
b'68484467AC',
b'68502994AD',
b'68540431AB',
],
},
CAR.RAM_HD: {
(Ecu.combinationMeter, 0x742, None): [
b'68361606AH',
b'68437735AC',
b'68492693AD',
b'68525485AB',
b'68525487AB',
b'68525498AB',
],
(Ecu.srs, 0x744, None): [
b'68399794AC',
b'68428503AA',
b'68428505AA',
b'68428507AA',
],
(Ecu.abs, 0x747, None): [
b'68334977AH',
b'68455481AC',
b'68504022AA',
b'68504022AB',
b'68530686AB',
b'68504022AC',
b'68530686AB',
b'68530686AC',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672895AB',
b'56029827AG',
b'56029827AH',
b'68462657AE',
b'68484694AD',
b'68484694AE',
],
(Ecu.eps, 0x761, None): [
@@ -260,7 +336,13 @@ FW_VERSIONS = {
b'68507906AB',
],
(Ecu.engine, 0x7e0, None): [
b'52370131AF',
b'52370231AF',
b'52370231AG',
b'52370931CT',
b'52401032AE',
b'52421132AF',
b'68527616AD ',
b'M2370131MB',
b'M2421132MB',
],
selfdrive/car/ecu_addrs.py
+7 -5
View File
@@ -9,6 +9,8 @@ from selfdrive.car import make_can_msg
from selfdrive.boardd.boardd import can_list_to_can_capnp
from system.swaglog import cloudlog
EcuAddrBusType = Tuple[int, Optional[int], int]
def make_tester_present_msg(addr, bus, subaddr=None):
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
@@ -33,16 +35,16 @@ def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subadd
return False
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]:
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]:
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list}
queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
responses = queries
return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug)
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]],
responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]:
ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),)
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType],
responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]:
ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
try:
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
selfdrive/car/fw_query_definitions.py
+12 -2
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import capnp
import copy
from dataclasses import dataclass, field
import struct
from typing import Dict, List, Optional, Tuple
@@ -57,10 +58,12 @@ class Request:
whitelist_ecus: List[int] = field(default_factory=list)
rx_offset: int = 0x8
bus: int = 1
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
auxiliary: bool = False
# FW responses from these queries will not be used for fingerprinting
logging: bool = False
# These requests are done once OBD multiplexing is disabled, after all others
non_obd: bool = False
# boardd toggles OBD multiplexing on/off as needed
obd_multiplexing: bool = True
@dataclass
@@ -71,3 +74,10 @@ class FwQueryConfig:
non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict)
# Ecus added for data collection, not to be fingerprinted on
extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)
def __post_init__(self):
for i in range(len(self.requests)):
if self.requests[i].auxiliary:
new_request = copy.deepcopy(self.requests[i])
new_request.bus += 4
self.requests.append(new_request)
selfdrive/car/fw_versions.py
+31 -32
View File
@@ -1,12 +1,12 @@
#!/usr/bin/env python3
from collections import defaultdict
from typing import Any, Optional, Set, Tuple
from typing import Any, Dict, List, Set
from tqdm import tqdm
import panda.python.uds as uds
from cereal import car
from common.params import Params
from selfdrive.car.ecu_addrs import get_ecu_addrs
from selfdrive.car.ecu_addrs import EcuAddrBusType, get_ecu_addrs
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import FW_VERSIONS
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
@@ -146,9 +146,11 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True):
return True, set()
def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]:
queries = list()
parallel_queries = list()
def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
params = Params()
# queries are split by OBD multiplexing mode
queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []}
parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []}
responses = set()
for brand, r in REQUESTS:
@@ -163,21 +165,24 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[i
a = (addr, sub_addr, r.bus)
# Build set of queries
if sub_addr is None:
if a not in parallel_queries:
parallel_queries.append(a)
if a not in parallel_queries[r.obd_multiplexing]:
parallel_queries[r.obd_multiplexing].append(a)
else: # subaddresses must be queried one by one
if [a] not in queries:
queries.append([a])
if [a] not in queries[r.obd_multiplexing]:
queries[r.obd_multiplexing].append([a])
# Build set of expected responses to filter
response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
responses.add((response_addr, sub_addr, r.bus))
queries.insert(0, parallel_queries)
for obd_multiplexing in queries:
queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
ecu_responses = set()
for query in queries:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
for obd_multiplexing in queries:
set_obd_multiplexing(params, obd_multiplexing)
for query in queries[obd_multiplexing]:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
return ecu_responses
@@ -198,13 +203,13 @@ def get_brand_ecu_matches(ecu_rx_addrs):
return brand_matches
def disable_obd_multiplexing(params):
if not params.get_bool("ObdMultiplexingDisabled"):
params.put_bool("FirmwareObdQueryDone", True)
cloudlog.warning("Waiting for OBD multiplexing to be disabled")
params.get_bool("ObdMultiplexingDisabled", block=True)
cloudlog.warning("OBD multiplexing disabled")
def set_obd_multiplexing(params: Params, obd_multiplexing: bool):
if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
params.remove("ObdMultiplexingChanged")
params.put_bool("ObdMultiplexingEnabled", obd_multiplexing)
params.get_bool("ObdMultiplexingChanged", block=True)
cloudlog.warning("OBD multiplexing set successfully")
def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False):
@@ -212,7 +217,6 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
all_car_fw = []
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
matched_brand: Optional[str] = None
for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True):
car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
@@ -220,21 +224,14 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
# Try to match using FW returned from this brand only
matches = match_fw_to_car_exact(build_fw_dict(car_fw))
if len(matches) == 1:
matched_brand = brand
break
disable_obd_multiplexing(Params())
# Do non-OBD queries for matched brand, or all if no match is found
for brand in FW_QUERY_CONFIGS.keys():
if brand == matched_brand or matched_brand is None:
all_car_fw.extend(get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, obd_multiplexed=False, debug=debug, progress=progress))
return all_car_fw
def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, obd_multiplexed=True, debug=False, progress=False):
def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False):
versions = VERSIONS.copy()
params = Params()
# Each brand can define extra ECUs to query for data collection
for brand, config in FW_QUERY_CONFIGS.items():
@@ -281,9 +278,10 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
# Or if request is not designated for current multiplexing mode
elif r.non_obd == obd_multiplexed:
continue
# Toggle OBD multiplexing for each request
if r.bus % 4 == 1:
set_obd_multiplexing(params, r.obd_multiplexing)
try:
addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
@@ -303,6 +301,7 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
f.brand = brand
f.bus = r.bus
f.logging = r.logging or ecu_key in logging_addrs
f.obdMultiplexing = r.obd_multiplexing
if sub_addr is not None:
f.subAddress = sub_addr
selfdrive/car/gm/interface.py
+8 -3
View File
@@ -78,7 +78,6 @@ class CarInterface(CarInterfaceBase):
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.longitudinalActuatorDelayUpperBound = 0.5
if experimental_long:
ret.pcmCruise = False
@@ -113,6 +112,7 @@ class CarInterface(CarInterfaceBase):
ret.steerLimitTimer = 0.4
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
if candidate == CAR.VOLT:
ret.mass = 1607. + STD_CARGO_KG
@@ -148,7 +148,13 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia()
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
elif candidate == CAR.BUICK_LACROSSE:
ret.mass = 1712. + STD_CARGO_KG
ret.wheelbase = 2.91
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_REGAL:
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
@@ -168,7 +174,6 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.95 # 116 inches in meters
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.ESCALADE_ESV:
selfdrive/car/gm/values.py
+7
View File
@@ -66,6 +66,7 @@ class CAR:
CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018"
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
ACADIA = "GMC ACADIA DENALI 2018"
BUICK_LACROSSE = "BUICK LACROSSE 2017"
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
ESCALADE = "CADILLAC ESCALADE 2017"
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
@@ -99,6 +100,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"),
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"),
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"),
CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"),
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"),
@@ -156,6 +158,11 @@ FINGERPRINTS = {
{
170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7
}],
CAR.BUICK_LACROSSE: [
# LaCrosse Premium AWD 2017
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.BUICK_REGAL : [
# Regal TourX Essence w/ ACC 2018
{
selfdrive/car/honda/tests/__init__.py
View File
selfdrive/car/honda/values.py
+1 -1
View File
@@ -186,7 +186,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
logging=True,
non_obd=True,
obd_multiplexing=False,
),
],
extra_ecus=[
selfdrive/car/hyundai/tests/__init__.py
View File
selfdrive/car/hyundai/values.py
+42 -6
View File
@@ -227,7 +227,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", "https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e),
],
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2022-23", harness=Harness.hyundai_k),
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness=Harness.hyundai_k),
CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness=Harness.hyundai_a),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness=Harness.hyundai_n),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c),
@@ -338,10 +338,15 @@ FINGERPRINTS = {
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf100) # Long description
HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf110) # Alt long description
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
p16(0xf100)
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
FW_QUERY_CONFIG = FwQueryConfig(
@@ -358,22 +363,46 @@ FW_QUERY_CONFIG = FwQueryConfig(
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar],
),
# CAN-FD queries (camera)
# CAN-FD queries (from camera)
# TODO: combine shared whitelists with CAN requests
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar],
bus=4,
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar],
bus=5,
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
# CAN-FD debugging queries
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parking],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parking],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
],
extra_ecus=[
(Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms
(Ecu.parking, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms)
(Ecu.hvac, 0x7b3, None), # HVAC Control Assembly
(Ecu.cornerRadar, 0x7b7, None),
],
)
@@ -1555,6 +1584,8 @@ FW_VERSIONS = {
b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328',
b'\xf1\x00CV1 MFC AT EUR RHD 1.00 1.00 99210-CV100 220630',
b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630',
b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.04 99210-CV000 210823',
b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.06 99210-CV000 220328',
],
},
CAR.IONIQ_5: {
@@ -1565,6 +1596,7 @@ FW_VERSIONS = {
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.02 99211-GI010 211206',
b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614',
b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.05 99211-GI010 220614',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206',
@@ -1631,6 +1663,7 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215',
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU000 211215',
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.03 99211-CU000 221118',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ',
@@ -1639,14 +1672,17 @@ FW_VERSIONS = {
CAR.KIA_SORENTO_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.05 99210-R5000 210623',
b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.03 99210-R5000 200903',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00MQ4_ SCC FHCUP 1.00 1.06 99110-P2000 ',
b'\xf1\x00MQ4_ SCC F-CUP 1.00 1.06 99110-P2000 ',
],
},
CAR.KIA_NIRO_HEV_2ND_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.08 99211-AT000 220531',
b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.09 99211-AT000 220801',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ',
selfdrive/car/subaru/values.py
+4
View File
@@ -273,9 +273,11 @@ FW_VERSIONS = {
b'\xa3 \031\024\000',
b'\xa3 \x14\x01',
b'\xf1\x00\xbb\r\x05',
b'\xa3 \x18&\x00',
],
(Ecu.eps, 0x746, None): [
b'\x8d\xc0\x04\x00',
b'\x8d\xc0\x00\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00e!\x1f@ \x11',
@@ -292,6 +294,7 @@ FW_VERSIONS = {
b'\xcb\"`p\a',
b'\xf1\x00\xa2\x10\n',
b'\xcf"`p\x07',
b'\xb6\xa2`A\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\032\xf6B0\000',
@@ -299,6 +302,7 @@ FW_VERSIONS = {
b'\032\xf6b`\000',
b'\x1a\xf6B`\x00',
b'\x1a\xf6b0\x00',
b'\x1a\xe6B1\x00',
],
},
CAR.FORESTER_PREGLOBAL: {
selfdrive/car/tests/routes.py
+3
View File
@@ -27,6 +27,8 @@ non_tested_cars = [
HYUNDAI.KIA_OPTIMA_H,
HONDA.ODYSSEY_CHN,
VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter
TOYOTA.RAV4_TSS2_2023,
TOYOTA.RAV4H_TSS2_2023,
]
CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,))
@@ -50,6 +52,7 @@ routes = [
CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA),
CarTestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL),
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("c950e28c26b5b168|2018-05-30--22-03-41", GM.VOLT),
selfdrive/car/tests/test_lateral_limits.py
-12
View File
@@ -10,7 +10,6 @@ from common.realtime import DT_CTRL
from selfdrive.car.car_helpers import interfaces
from selfdrive.car.fingerprints import all_known_cars
from selfdrive.car.interfaces import get_torque_params
from selfdrive.car.hyundai.values import CAR as HYUNDAI
CAR_MODELS = all_known_cars()
@@ -22,14 +21,6 @@ MAX_LAT_ACCEL = 3.0 # m/s^2
# jerk is measured over half a second
JERK_MEAS_FRAMES = 0.5 / DT_CTRL
# TODO: update the max measured lateral accel for these cars
ABOVE_LIMITS_CARS = [
HYUNDAI.KONA_EV,
HYUNDAI.KONA_HEV,
HYUNDAI.KONA,
HYUNDAI.KONA_EV_2022,
]
car_model_jerks: DefaultDict[str, Dict[str, float]] = defaultdict(dict)
@@ -52,9 +43,6 @@ class TestLateralLimits(unittest.TestCase):
if CP.notCar:
raise unittest.SkipTest
if CP.carFingerprint in ABOVE_LIMITS_CARS:
raise unittest.SkipTest
CarControllerParams = importlib.import_module(f'selfdrive.car.{CP.carName}.values').CarControllerParams
cls.control_params = CarControllerParams(CP)
cls.torque_params = get_torque_params(cls.car_model)
selfdrive/car/torque_data/override.yaml
+4
View File
@@ -7,6 +7,10 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
NISSAN LEAF 2018: [.nan, 1.5, .nan]
NISSAN ROGUE 2019: [.nan, 1.5, .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]
TESLA AP2 MODEL S: [.nan, 2.5, .nan]
selfdrive/car/torque_data/params.yaml
+1 -1
View File
@@ -30,7 +30,7 @@ HYUNDAI IONIQ 5 2022: [3.172929, 2.713050, 0.096019]
HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276]
HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778]
HYUNDAI IONIQ PLUG-IN HYBRID 2019: [2.970807902012267, 1.6312321830002083, 0.1088964990357482]
HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 3.2961956260770484, 0.12359762054065548]
HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 2.307336938253934, 0.12359762054065548]
HYUNDAI PALISADE 2020: [2.544642494803999, 1.8721703683337008, 0.1301424599248651]
HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.1207019341823945]
HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963]
selfdrive/car/torque_data/substitute.yaml
+1
View File
@@ -50,6 +50,7 @@ HONDA CIVIC SEDAN 1.6 DIESEL 2019: HONDA CIVIC (BOSCH) 2019
HONDA E 2020: HONDA CIVIC (BOSCH) 2019
HONDA ODYSSEY CHN 2019: HONDA ODYSSEY 2018
BUICK LACROSSE 2017: CHEVROLET VOLT PREMIER 2017
BUICK REGAL ESSENCE 2018: CHEVROLET VOLT PREMIER 2017
CADILLAC ESCALADE ESV 2016: CHEVROLET VOLT PREMIER 2017
CADILLAC ATS Premium Performance 2018: CHEVROLET VOLT PREMIER 2017
selfdrive/car/toyota/carcontroller.py
+5
View File
@@ -80,6 +80,11 @@ class CarController:
apply_steer_req = 0
self.steer_rate_counter = 0
# Never actuate with LKA on cars that only support LTA
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
apply_steer = 0
apply_steer_req = 0
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:
selfdrive/car/toyota/interface.py
+12 -6
View File
@@ -3,7 +3,8 @@ from cereal import car
from common.conversions import Conversions as CV
from common.params import Params, put_nonblocking
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config, create_mads_event
from selfdrive.car.interfaces import CarInterfaceBase
@@ -28,13 +29,18 @@ class CarInterface(CarInterfaceBase):
if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
if candidate in ANGLE_CONTROL_CAR:
ret.dashcamOnly = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = False
steering_angle_deadzone_deg = 0.0
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
if candidate == CAR.PRIUS:
stop_and_go = True
@@ -45,9 +51,8 @@ class CarInterface(CarInterfaceBase):
# Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
steering_angle_deadzone_deg = 0.2
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate == CAR.PRIUS_V:
stop_and_go = True
@@ -107,7 +112,8 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
stop_and_go = True
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
selfdrive/car/toyota/tests/__init__.py
View File
selfdrive/car/toyota/tests/test_toyota.py
Executable
+13
View File
@@ -0,0 +1,13 @@
#!/usr/bin/env python3
import unittest
from selfdrive.car.toyota.values import TSS2_CAR, ANGLE_CONTROL_CAR
class TestToyotaInterfaces(unittest.TestCase):
def test_angle_car_set(self):
self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0)
if __name__ == "__main__":
unittest.main()
selfdrive/car/toyota/toyotacan.py
+1 -1
View File
@@ -17,7 +17,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
"SETME_X1": 1,
"SETME_X3": 3,
"PERCENTAGE": 100,
"SETME_X64": 0x64,
"SETME_X64": 0,
"ANGLE": 0,
"STEER_ANGLE_CMD": steer,
"STEER_REQUEST": steer_req,
selfdrive/car/toyota/values.py
+57 -4
View File
@@ -69,8 +69,10 @@ class CAR:
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
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"
@@ -159,8 +161,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
],
CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
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.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),
@@ -529,14 +533,18 @@ FW_VERSIONS = {
CAR.CAMRY_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B33630\x00\x00\x00\x00\x00\x00',
b'8965B33640\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',
],
(Ecu.engine, 0x700, None): [
b'\x018966306Q5000\x00\x00\x00\x00',
b'\x018966306Q9000\x00\x00\x00\x00',
b'\x018966306R3000\x00\x00\x00\x00',
b'\x018966306T3100\x00\x00\x00\x00',
b'\x018966306T3200\x00\x00\x00\x00',
b'\x018966306T4000\x00\x00\x00\x00',
@@ -544,12 +552,15 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
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: {
@@ -1456,6 +1467,23 @@ FW_VERSIONS = {
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'\x01896634AJ2000\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: {
(Ecu.engine, 0x700, None): [
b'\x01896634A15000\x00\x00\x00\x00',
@@ -1543,6 +1571,23 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15264283200\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AE1001\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.SIENNA: {
(Ecu.engine, 0x700, None): [
b'\x01896630832100\x00\x00\x00\x00',
@@ -1731,6 +1776,7 @@ FW_VERSIONS = {
b'\x018966378B2100\x00\x00\x00\x00',
b'\x018966378B3000\x00\x00\x00\x00',
b'\x018966378B4100\x00\x00\x00\x00',
b'\x018966378G2000\x00\x00\x00\x00',
b'\x018966378G3000\x00\x00\x00\x00',
b'\x018966378B2000\x00\x00\x00\x00',
],
@@ -1753,6 +1799,7 @@ FW_VERSIONS = {
CAR.LEXUS_NXH_TSS2: {
(Ecu.engine, 0x7e0, None): [
b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x02378A0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152678210\x00\x00\x00\x00\x00\x00',
@@ -1762,6 +1809,7 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
b'\x018821F3301300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
@@ -2098,6 +2146,7 @@ DBC = {
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),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2108,6 +2157,7 @@ DBC = {
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_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_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
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'),
@@ -2122,8 +2172,8 @@ DBC = {
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
@@ -2132,10 +2182,13 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
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.CHR_TSS2, CAR.CHRH_TSS2}
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}
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
# no resume button press required
selfdrive/car/volkswagen/carstate.py
+5 -5
View File
@@ -249,13 +249,13 @@ class CarState(CarStateBase):
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["GRA_Status"] in (1, 2)
if self.CP.pcmCruise:
ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7)
ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7)
else:
ret.accFaulted = pt_cp.vl["Motor_2"]["GRA_Status"] == 3
# Update ACC setpoint. When the setpoint reads as 255, the driver has not
# yet established an ACC setpoint, so treat it as zero.
ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anziege"]["ACA_V_Wunsch"] * CV.KPH_TO_MS
ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] * CV.KPH_TO_MS
if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint
ret.cruiseState.speed = 0
@@ -547,12 +547,12 @@ class PqExtraSignals:
# Additional signal and message lists for optional or bus-portable controllers
fwd_radar_signals = [
("ACS_Typ_ACC", "ACC_System"), # Basic vs FtS (no SnG support on PQ)
("ACA_StaACC", "ACC_GRA_Anziege"), # ACC drivetrain coordinator status
("ACA_V_Wunsch", "ACC_GRA_Anziege"), # ACC set speed
("ACA_StaACC", "ACC_GRA_Anzeige"), # ACC drivetrain coordinator status
("ACA_V_Wunsch", "ACC_GRA_Anzeige"), # ACC set speed
]
fwd_radar_checks = [
("ACC_System", 50), # From J428 ACC radar control module
("ACC_GRA_Anziege", 25), # From J428 ACC radar control module
("ACC_GRA_Anzeige", 25), # From J428 ACC radar control module
]
bsm_radar_signals = [
("SWA_Infostufe_SWA_li", "SWA_1"), # Blind spot object info, left
selfdrive/car/volkswagen/pqcan.py
+1 -1
View File
@@ -88,4 +88,4 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance
# kmh_mph handling probably needed to resolve rounding errors in displayed setpoint
}
return packer.make_can_msg("ACC_GRA_Anziege", bus, values)
return packer.make_can_msg("ACC_GRA_Anzeige", bus, values)
selfdrive/car/volkswagen/values.py
+10 -1
View File
@@ -36,7 +36,7 @@ class CarControllerParams:
if CP.carFingerprint in PQ_CARS:
self.LDW_STEP = 5 # LDW_1 message frequency 20Hz
self.ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz
self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz
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))
@@ -404,6 +404,7 @@ FW_VERSIONS = {
b'\xf1\x8704L906021DT\xf1\x895520',
b'\xf1\x8704L906021DT\xf1\x898127',
b'\xf1\x8704L906021N \xf1\x895518',
b'\xf1\x8704L906026BN\xf1\x891197',
b'\xf1\x8704L906026BP\xf1\x897608',
b'\xf1\x8704L906026NF\xf1\x899528',
b'\xf1\x8704L906056CL\xf1\x893823',
@@ -524,6 +525,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0511A00403A0',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00404A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00504A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A07A02A1',
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A00507A1',
@@ -1083,6 +1085,8 @@ FW_VERSIONS = {
b'\xf1\x8704L906026DE\xf1\x895418',
b'\xf1\x8704L906026EJ\xf1\x893661',
b'\xf1\x8704L906026HT\xf1\x893617',
b'\xf1\x8783A907115E \xf1\x890001',
b'\xf1\x8705E906018DJ\xf1\x890915',
b'\xf1\x875NA907115E \xf1\x890003',
b'\xf1\x875NA907115E \xf1\x890005',
],
@@ -1092,6 +1096,8 @@ FW_VERSIONS = {
b'\xf1\x870DL300012M \xf1\x892107',
b'\xf1\x870DL300012N \xf1\x892110',
b'\xf1\x870DL300013G \xf1\x892119',
b'\xf1\x870GC300014N \xf1\x892801',
b'\xf1\x870GC300046Q \xf1\x892802',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655AP\xf1\x890306\xf1\x82\r11110011110011421111314211',
@@ -1099,11 +1105,13 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1213001211001244212111442100',
b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e1213001211001205212112052100',
b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111',
b'\xf1\x873Q0959655DJ\xf1\x890731\xf1\x82\x0e1513001511001205232113052J00',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405',
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405',
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6070405',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G500',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600',
],
(Ecu.fwdRadar, 0x757, None): [
@@ -1111,6 +1119,7 @@ FW_VERSIONS = {
b'\xf1\x872Q0907572R \xf1\x890372',
b'\xf1\x872Q0907572T \xf1\x890383',
b'\xf1\x872Q0907572AA\xf1\x890396',
b'\xf1\x872Q0907572AB\xf1\x890397',
],
},
CAR.SKODA_OCTAVIA_MK3: {
selfdrive/controls/plannerd.py
+1
View File
@@ -19,6 +19,7 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
ui_send = messaging.new_message('uiPlan')
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
uiPlan = ui_send.uiPlan
uiPlan.frameId = sm['modelV2'].frameId
uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
selfdrive/controls/tests/test_startup.py
+4 -1
View File
@@ -72,7 +72,6 @@ class TestStartup(unittest.TestCase):
params.clear_all()
params.put_bool("Passive", False)
params.put_bool("OpenpilotEnabledToggle", True)
params.put_bool("ObdMultiplexingDisabled", True)
# Build capnn version of FW array
if fw_versions is not None:
@@ -109,6 +108,10 @@ class TestStartup(unittest.TestCase):
finger = _FINGERPRINTS[car_model][0]
for _ in range(1000):
# controlsd waits for boardd to echo back that it has changed the multiplexing mode
if not params.get_bool("ObdMultiplexingChanged"):
params.put_bool("ObdMultiplexingChanged", True)
msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()]
pm.send('can', can_list_to_can_capnp(msgs))
selfdrive/debug/hyundai_enable_radar_points.py
+3
View File
@@ -32,6 +32,9 @@ SUPPORTED_FW_VERSIONS = {
b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
b"DN8_ SCC F-CUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
# 2021 SONATA HYBRID
b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
selfdrive/debug/internal/check_frame_frequencies.py
Executable
+43
View File
@@ -0,0 +1,43 @@
#!/usr/bin/env python3
import time
import statistics
import cereal.messaging as messaging
from typing import Dict
camera_states = [
'roadCameraState',
'wideRoadCameraState',
'driverCameraState'
]
def fmt(val):
ref = 0.05
return f"{val:.6f} ({100 * val / ref:.2f}%)"
if __name__ == "__main__":
sm = messaging.SubMaster(camera_states)
prev_sof = {state: None for state in camera_states}
diffs: Dict[str, list] = {state: [] for state in camera_states}
st = time.monotonic()
while True:
sm.update()
for state in camera_states:
if sm.updated[state]:
if prev_sof[state] is not None:
diffs[state].append((sm[state].timestampSof - prev_sof[state]) / 1e9)
prev_sof[state] = sm[state].timestampSof
if time.monotonic() - st > 10:
for state in camera_states:
values = diffs[state]
ref = 0.05
print(f"{state} \tMean: {fmt(statistics.mean(values))} \t Min: {fmt(min(values))} \t Max: {fmt(max(values))} \t Std: {statistics.stdev(values):.6f} \t Num frames: {len(values)}")
diffs[state] = []
print()
st = time.monotonic()
selfdrive/locationd/laikad.py
+154 -155
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
import json
import math
import os
import time
@@ -8,7 +7,7 @@ from collections import defaultdict
from concurrent.futures import Future, ProcessPoolExecutor
from datetime import datetime
from enum import IntEnum
from typing import List, Optional
from typing import List, Optional, Dict, Any
import numpy as np
@@ -17,9 +16,9 @@ from 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 Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem
from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.helpers import ConstellationId, get_sv_id
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@@ -28,15 +27,53 @@ from selfdrive.locationd.models.gnss_kf import States as GStates
from system.swaglog import cloudlog
MAX_TIME_GAP = 10
EPHEMERIS_CACHE = 'LaikadEphemerisV2'
EPHEMERIS_CACHE = 'LaikadEphemerisV3'
DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/"
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=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False,
valid_ephem_types=(EphemerisType.NAV,),
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
@@ -51,10 +88,11 @@ class Laikad:
self.auto_fetch_navs = auto_fetch_navs
self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
self.orbit_fetch_future: Optional[Future] = None
self.last_fetch_navs_t = None
self.got_first_gnss_msg = False
self.last_cached_t = 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()
@@ -64,36 +102,58 @@ class Laikad:
self.last_fix_t = None
self.gps_week = None
self.use_qcom = use_qcom
self.first_log_time = None
self.ttff = -1
def load_cache(self):
if not self.save_ephemeris:
return
cache = Params().get(EPHEMERIS_CACHE)
if not cache:
cache_bytes = Params().get(EPHEMERIS_CACHE)
if not cache_bytes:
return
nav_dict = {}
try:
cache = json.loads(cache, object_hook=deserialize_hook)
if cache['version'] == CACHE_VERSION:
self.astro_dog.add_navs(cache['navs'])
self.last_fetch_navs_t = cache['last_fetch_navs_t']
else:
cache['navs'] = {}
except json.decoder.JSONDecodeError:
ephem_cache = ephemeris_structs.EphemerisCache.from_bytes(cache_bytes)
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")
timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan'
cloudlog.debug(
f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " +
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}")
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, t: GPSTime):
if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
put_nonblocking(EPHEMERIS_CACHE, json.dumps(
{'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs},
cls=CacheSerializer))
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()], [])
ephem_cache = ephemeris_structs.EphemerisCache(**{'glonassEphemerides': [e.data for e in nav_list if e.prn[0]=='R'],
'gpsEphemerides': [e.data for e in nav_list if e.prn[0]=='G']})
put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes())
cloudlog.debug("Cache saved")
self.last_cached_t = t
self.last_cached_t = self.last_report_time
def create_ephem_statuses(self):
ephemeris_statuses = []
prns_to_check = list(self.astro_dog.get_all_ephem_prns())
prns_to_check.sort()
for prn in prns_to_check:
eph = self.astro_dog.get_eph(prn, self.last_report_time)
if eph is not None:
status = log.GnssMeasurements.EphemerisStatus.new_message()
status.constellationId = ConstellationId.from_rinex_char(prn[0]).value
status.svId = get_sv_id(prn)
status.type = get_log_eph_type(eph).value
status.source = get_log_eph_source(eph).value
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:
@@ -121,8 +181,15 @@ class Laikad:
def is_good_report(self, gnss_msg):
if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
# TODO support GLONASS
return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
# TODO: Understand and use remaining unknown constellations
try:
good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
except NotImplementedError:
good_constellation = False
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
good_week = not getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max
return good_constellation and good_week
elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True
else:
@@ -139,6 +206,7 @@ class Laikad:
week = report.gpsWeek
tow = report.rcvTow
new_meas = read_raw_ublox(report)
self.last_report_time = GPSTime(week, tow)
return week, tow, new_meas
def is_ephemeris(self, gnss_msg):
@@ -153,28 +221,34 @@ class Laikad:
if self.gps_week is None:
return
ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
self.astro_dog.add_qcom_polys({ephem.prn: [ephem]})
else:
if gnss_msg.which() == 'ephemeris':
ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris)
data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
ephem = GPSEphemeris(data_struct, file_name='ublox')
elif gnss_msg.which() == 'glonassEphemeris':
ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris)
data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
ephem = GLONASSEphemeris(data_struct, file_name='ublox')
else:
cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
return
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
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:
corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog)
instant_fix = self.get_lsq_fix(t, corrected_measurements)
#instant_fix = self.get_lsq_fix(t, processed_measurements)
est_pos = self.last_fix_pos
else:
corrected_measurements = []
instant_fix = self.get_lsq_fix(t, processed_measurements)
est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
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:
@@ -182,60 +256,53 @@ class Laikad:
self.last_fix_t = t
self.last_fix_pos = position_estimate
self.lat_fix_pos_std = position_std
if (t*1e9) % 10 == 0:
cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
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}
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)
return None
elif self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
self.gps_week = week
if len(new_meas) == 0:
return None
t = gnss_mono_time * 1e-9
if week > 0:
self.got_first_gnss_msg = True
latest_msg_t = GPSTime(week, tow)
if self.auto_fetch_navs:
self.fetch_navs(latest_msg_t, block)
output = self.process_report(new_meas, t)
if output is None:
return None
position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
corrected_measurements = self.process_report(new_meas, t)
msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
self.update_localizer(position_estimate, t, corrected_measurements)
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
msg = messaging.new_message("gnssMeasurements")
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.gnssMeasurements = {
"gpsWeek": week,
"gpsTimeOfWeek": tow,
"kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
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),
"kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].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),
"positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)),
"velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)),
"measTime": gnss_mono_time,
"correctedMeasurements": meas_msgs
}
return msg
#elif gnss_msg.which() == 'ionoData':
# TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
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
@@ -247,7 +314,7 @@ class Laikad:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
if len(est_pos) > 0:
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:
@@ -272,7 +339,7 @@ class Laikad:
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 (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
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
@@ -290,7 +357,7 @@ class Laikad:
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(t=t)
self.cache_ephemeris()
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
@@ -320,31 +387,8 @@ def create_measurement_msg(meas: GNSSMeasurement):
c.satPos = meas.sat_pos_final.tolist()
c.satVel = meas.sat_vel.tolist()
c.satVel = meas.sat_vel.tolist()
ephem = meas.sat_ephemeris
assert ephem is not None
week, time_of_week = -1, -1
if ephem.eph_type == EphemerisType.NAV:
source_type = EphemerisSourceType.nav
elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = EphemerisSourceType.qcom
else:
assert ephem.file_epoch is not None
week = ephem.file_epoch.week
time_of_week = ephem.file_epoch.tow
file_src = ephem.file_source
if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z'
source_type = EphemerisSourceType.nasaUltraRapid
elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
source_type = EphemerisSourceType.glonassIacUltraRapid
else:
raise Exception(f"Didn't expect file source {file_src}")
c.ephemerisSource.type = source_type.value
c.ephemerisSource.gpsWeek = week
c.ephemerisSource.gpsTimeOfWeek = int(time_of_week)
return c
def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]):
ekf_data = defaultdict(list)
for m in measurements:
@@ -360,69 +404,29 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe
gnss_kf.predict_and_observe(t, kind, data)
class CacheSerializer(json.JSONEncoder):
def default(self, o):
if isinstance(o, Ephemeris):
return o.to_json()
if isinstance(o, GPSTime):
return o.__dict__
if isinstance(o, np.ndarray):
return o.tolist()
return json.JSONEncoder.default(self, o)
def deserialize_hook(dct):
if 'ephemeris' in dct:
return Ephemeris.from_json(dct)
if 'week' in dct:
return GPSTime(dct['week'], dct['tow'])
return dct
class EphemerisSourceType(IntEnum):
nav = 0
nasaUltraRapid = 1
glonassIacUltraRapid = 2
qcom = 3
def process_msg(laikad, gnss_msg, mono_time, block=False):
# TODO: Understand and use remaining unknown constellations
if gnss_msg.which() == "drMeasurementReport":
if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
return None
if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
return None
return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
def clear_tmp_cache():
if os.path.exists(DOWNLOADS_CACHE_FOLDER):
shutil.rmtree(DOWNLOADS_CACHE_FOLDER)
os.mkdir(DOWNLOADS_CACHE_FOLDER)
def main(sm=None, pm=None, qc=None):
def main(sm=None, pm=None):
#clear_tmp_cache()
use_qcom = not Params().get_bool("UbloxAvailable", block=True)
if use_qcom or (qc is not None and qc):
raw_gnss_socket = "qcomGnss"
if use_qcom:
raw_name = "qcomGnss"
else:
raw_gnss_socket = "ubloxGnss"
raw_name = "ubloxGnss"
raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False, timeout=1000)
if sm is None:
sm = messaging.SubMaster([raw_gnss_socket, 'clocks'])
sm = messaging.SubMaster(['clocks',])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])
# disable until set as main gps source, to better analyze startup time
use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ
use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ
replay = "REPLAY" in os.environ
if replay or "CI" in os.environ:
@@ -431,22 +435,17 @@ def main(sm=None, pm=None, qc=None):
laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
while True:
sm.update()
if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket]
msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
if msg is None:
# TODO: beautify this, locationd needs a valid message
msg = messaging.new_message("gnssMeasurements")
pm.send('gnssMeasurements', msg)
for in_msg in messaging.drain_sock(raw_gnss_sock):
out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay)
pm.send('gnssMeasurements', out_msg)
sm.update(0)
if not laikad.got_first_gnss_msg and sm.updated['clocks']:
clocks_msg = sm['clocks']
t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
if laikad.auto_fetch_navs:
laikad.fetch_navs(t, block=replay)
if __name__ == "__main__":
main()
selfdrive/locationd/test/test_laikad.py
+152 -108
View File
@@ -1,21 +1,51 @@
#!/usr/bin/env python3
import time
import unittest
from collections import defaultdict
from cereal import log
import cereal.messaging as messaging
from common.params import Params
from datetime import datetime
from unittest import mock
from unittest.mock import patch
#from unittest.mock import patch
from tqdm import tqdm
from common.params import Params
from laika.constants import SECS_IN_DAY
from laika.downloader import DownloadFailed
from laika.ephemeris import EphemerisType, GPSEphemeris
from laika.ephemeris import EphemerisType
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId, TimeRangeHolder
from laika.raw_gnss import GNSSMeasurement, read_raw_ublox
from selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, read_raw_ublox, read_raw_qcom
from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad
from selfdrive.test.openpilotci import get_url
from tools.lib.logreader import LogReader
from selfdrive.manager.process_config import managed_processes
from selfdrive.test.process_replay.helpers import OpenpilotPrefix
def get_ublox_gnss(ubloxraw):
with OpenpilotPrefix():
managed_processes['ubloxd'].start()
timeout_ms = 30
pm = messaging.PubMaster(['ubloxRaw'])
sock = messaging.sub_sock('ubloxGnss', timeout=timeout_ms)
log_msgs = []
log_t = []
for x in tqdm(ubloxraw):
pm.send(x.which(), x.as_builder())
ret = messaging.recv_one(sock)
if ret is not None:
msg = log.Event.new_message(ubloxGnss=ret.ubloxGnss.to_dict())
msg.logMonoTime = x.logMonoTime
log_msgs.append(msg)
log_t.append(1e-9 * x.logMonoTime)
assert managed_processes['ubloxd'].get_process_state_msg().running
assert len(log_msgs) > 1 or len(ubloxraw) == 0
managed_processes['ubloxd'].stop()
return log_t, log_msgs
def get_log(segs=range(0)):
@@ -23,7 +53,8 @@ def get_log(segs=range(0)):
for i in segs:
logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i)))
all_logs = [m for m in logs if m.which() == 'ubloxGnss']
raw_logs = [m for m in logs if m.which() == 'ubloxRaw']
all_logs = get_ublox_gnss(raw_logs)[1]
low_gnss = []
for m in all_logs:
if m.ubloxGnss.which() != 'measurementReport':
@@ -31,21 +62,31 @@ def get_log(segs=range(0)):
MAX_MEAS = 7
if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS:
mb = m.as_builder()
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(segs=range(0)):
logs = []
for i in segs:
logs.extend(LogReader(get_url("b0b3cba7abf862d1|2023-03-11--09-40-33", i)))
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:
msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
if m.which() == 'ubloxGnss':
gnss_msg = m.ubloxGnss
else:
gnss_msg = m.qcomGnss
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:
@@ -55,10 +96,16 @@ def verify_messages(lr, laikad, return_one_success=False):
def get_first_gps_time(logs):
for m in logs:
if m.ubloxGnss.which == 'measurementReport':
new_meas = read_raw_ublox(m.ubloxGnss.measurementReport)
if len(new_meas) > 0:
return new_meas[0].recv_time
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
else:
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):
@@ -79,6 +126,7 @@ class TestLaikad(unittest.TestCase):
logs, low_gnss = get_log(range(1))
cls.logs = logs
cls.low_gnss = low_gnss
cls.logs_qcom = get_log_qcom(range(1))
first_gps_time = get_first_gps_time(logs)
cls.first_gps_time = first_gps_time
@@ -90,9 +138,9 @@ class TestLaikad(unittest.TestCase):
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)
@@ -105,6 +153,7 @@ class TestLaikad(unittest.TestCase):
self.assertIsNotNone(ephem)
self.assertNotEqual(ephem, ephem2)
def test_fetch_navs_with_wrong_clocks(self):
laikad = Laikad()
@@ -127,40 +176,11 @@ class TestLaikad(unittest.TestCase):
check_has_navs()
self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
def test_ephemeris_source_in_msg(self):
data_mock = defaultdict(str)
data_mock['sv_id'] = 1
gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC
laikad = Laikad()
laikad.fetch_navs(gpstime, block=True)
meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
# Verify gps satellite returns same source
meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
# Test nasa source by using older date
gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad()
laikad.fetch_navs(gpstime, block=True)
meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
# Test nav source type
ephem = GPSEphemeris(data_mock, gpstime)
meas = get_measurement_mock(gpstime, ephem)
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
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 = 559
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]))
@@ -177,12 +197,15 @@ class TestLaikad(unittest.TestCase):
self.assertTrue(kf_valid)
def test_laika_online_nav_only(self):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV)
# Disable fetch_orbits to test NAV only
correct_msgs = verify_messages(self.logs, laikad)
correct_msgs_expected = 559
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]))
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
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 = 44 if use_qcom else 560
valid_fix_expected = 43 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):
@@ -195,8 +218,9 @@ class TestLaikad(unittest.TestCase):
downloader_mock.side_effect = DownloadFailed
laikad = Laikad(auto_update=False)
correct_msgs = verify_messages(self.logs, laikad)
self.assertEqual(255, len(correct_msgs))
self.assertEqual(255, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
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)
@@ -212,65 +236,87 @@ class TestLaikad(unittest.TestCase):
self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
prn = "R01"
self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
print(min(laikad.astro_dog.navs[prn], key=lambda e: e.epoch).epoch.as_datetime())
def test_get_navs_in_process(self):
laikad = Laikad(auto_update=False)
has_navs = False
for m in self.logs:
laikad.process_gnss_msg(m.ubloxGnss, 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
if has_navs:
break
self.assertTrue(has_navs)
self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0)
self.assertEqual(None, laikad.orbit_fetch_future)
for auto_fetch_navs in [True, False]:
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
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:
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):
laikad = Laikad(auto_update=True, save_ephemeris=True)
use_qcom = True
for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
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")
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()
wait_for_cache()
Params().remove(EPHEMERIS_CACHE)
# Test cache with no ephemeris
laikad.cache_ephemeris(t=GPSTime(0, 0))
wait_for_cache()
Params().remove(EPHEMERIS_CACHE)
#laikad.astro_dog.get_navs(self.first_gps_time)
msg = verify_messages(logs, laikad, return_one_success=True)
laikad.cache_ephemeris()
wait_for_cache()
#laikad.astro_dog.get_navs(self.first_gps_time)
laikad.fetch_navs(self.first_gps_time, block=True)
# Wait for cache to save
wait_for_cache()
# Check both nav and orbits separate
laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True)
# 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(self.logs, laikad, return_one_success=True)
self.assertIsNotNone(msg)
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)
# 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)
# Verify orbit data is not downloaded
mock_method.assert_not_called()
#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
@@ -282,12 +328,10 @@ class TestLaikad(unittest.TestCase):
gm = msg.gnssMeasurements
if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid:
cnt += 1
self.assertEqual(cnt, 559)
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/manager.py
+1 -1
View File
@@ -220,7 +220,7 @@ def manager_thread() -> None:
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}")
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
selfdrive/manager/process_config.py
+8 -2
View File
@@ -17,11 +17,17 @@ def logging(started, params, CP: car.CarParams) -> bool:
run = (not CP.notCar) or not params.get_bool("DisableLogging")
return started and run
def ublox_available() -> bool:
return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')
def ublox(started, params, CP: car.CarParams) -> bool:
use_ublox = os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')
use_ublox = ublox_available()
params.put_bool("UbloxAvailable", use_ublox)
return started and use_ublox
def qcomgps(started, params, CP: car.CarParams) -> bool:
return started and not ublox_available()
procs = [
# due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview),
@@ -50,7 +56,7 @@ procs = [
PythonProcess("deleter", "system.loggerd.deleter", offroad=True),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
PythonProcess("laikad", "selfdrive.locationd.laikad"),
PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI),
PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps),
PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
selfdrive/modeld/models/supercombo.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:736ddc08497d7596bae4d9515a8efb996676be80e67a6d34d632bb8af2ed3fa9
size 45962515
oid sha256:5121deb0d5c683b0fbee4c1cad7bc625953bf127b1383fb7599a6b644efd0aea
size 46011200
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
ab64afd1abd1059c14f50c67b51e5ef89029f216
82db08d52b155336e9a1dadd11485d5acdf2eba0
selfdrive/test/process_replay/process_replay.py
+51 -56
View File
@@ -238,21 +238,12 @@ def torqued_rcv_callback(msg, CP, cfg, fsm):
return recv_socks, fsm.frame == 0 or msg.which() == 'liveLocationKalman'
def ublox_rcv_callback(msg):
def ublox_rcv_callback(msg, CP, cfg, fsm):
msg_class, msg_id = msg.ubloxRaw[2:4]
if (msg_class, msg_id) in {(1, 7 * 16)}:
return ["gpsLocationExternal"]
return ["gpsLocationExternal"], True
elif (msg_class, msg_id) in {(2, 1 * 16 + 5), (10, 9)}:
return ["ubloxGnss"]
else:
return []
def laika_rcv_callback(msg, CP, cfg, fsm):
if msg.which() == 'ubloxGnss' and msg.ubloxGnss.which() == "measurementReport":
return ["gnssMeasurements"], True
elif msg.which() == 'qcomGnss' and msg.qcomGnss.which() == "drMeasurementReport":
return ["gnssMeasurements"], True
return ["ubloxGnss"], True
else:
return [], False
@@ -331,7 +322,7 @@ CONFIGS = [
pub_sub={
"cameraOdometry": ["liveLocationKalman"],
"accelerometer": [], "gyroscope": [],
"gpsLocationExternal": [], "liveCalibration": [], "carState": [],
"gpsLocationExternal": [], "liveCalibration": [], "carState": [], "gpsLocation": [],
},
ignore=["logMonoTime", "valid"],
init_callback=get_car_params,
@@ -371,9 +362,9 @@ CONFIGS = [
},
ignore=["logMonoTime"],
init_callback=get_car_params,
should_recv_callback=laika_rcv_callback,
should_recv_callback=None,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
fake_pubsubmaster=False,
),
ProcessConfig(
proc_name="torqued",
@@ -395,10 +386,10 @@ def replay_process(cfg, lr, fingerprint=None):
if cfg.fake_pubsubmaster:
return python_replay_process(cfg, lr, fingerprint)
else:
return cpp_replay_process(cfg, lr, fingerprint)
return replay_process_with_sockets(cfg, lr, fingerprint)
def setup_env(simulation=False, CP=None, cfg=None, controlsState=None):
def setup_env(simulation=False, CP=None, cfg=None, controlsState=None, lr=None):
params = Params()
params.clear_all()
params.put_bool("OpenpilotEnabledToggle", True)
@@ -406,13 +397,19 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None):
params.put_bool("DisengageOnAccelerator", True)
params.put_bool("WideCameraOnly", False)
params.put_bool("DisableLogging", False)
params.put_bool("UbloxAvailable", True)
params.put_bool("ObdMultiplexingDisabled", True)
os.environ["NO_RADAR_SLEEP"] = "1"
os.environ["REPLAY"] = "1"
os.environ['SKIP_FW_QUERY'] = ""
os.environ['FINGERPRINT'] = ""
os.environ["SKIP_FW_QUERY"] = ""
os.environ["FINGERPRINT"] = ""
if lr is not None:
services = {m.which() for m in lr}
params.put_bool("UbloxAvailable", "ubloxGnss" in services)
if lr is not None:
services = {m.which() for m in lr}
params.put_bool("UbloxAvailable", "ubloxGnss" in services)
if cfg is not None:
# Clear all custom processConfig environment variables
@@ -464,12 +461,6 @@ def python_replay_process(cfg, lr, fingerprint=None):
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
# laikad needs decision between submaster ubloxGnss and qcomGnss, prio given to ubloxGnss
if cfg.proc_name == "laikad":
args = (*args, not any(m.which() == "ubloxGnss" for m in pub_msgs))
service = "qcomGnss" if args[2] else "ubloxGnss"
pub_msgs = [m for m in pub_msgs if m.which() == service or m.which() == 'clocks']
controlsState = None
initialized = False
for msg in lr:
@@ -485,10 +476,10 @@ def python_replay_process(cfg, lr, fingerprint=None):
if fingerprint is not None:
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['FINGERPRINT'] = fingerprint
setup_env(cfg=cfg, controlsState=controlsState)
setup_env(cfg=cfg, controlsState=controlsState, lr=lr)
else:
CP = [m for m in lr if m.which() == 'carParams'][0].carParams
setup_env(CP=CP, cfg=cfg, controlsState=controlsState)
setup_env(CP=CP, cfg=cfg, controlsState=controlsState, lr=lr)
assert(type(managed_processes[cfg.proc_name]) is PythonProcess)
managed_processes[cfg.proc_name].prepare()
@@ -517,7 +508,7 @@ def python_replay_process(cfg, lr, fingerprint=None):
recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm)
else:
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
(fsm.frame + 1) % max(1, int(service_list[msg.which()].frequency / service_list[s].frequency)) == 0]
should_recv = bool(len(recv_socks))
if msg.which() == 'can':
@@ -540,49 +531,53 @@ def python_replay_process(cfg, lr, fingerprint=None):
return log_msgs
def cpp_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] # We get responses here
def replay_process_with_sockets(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
pm = messaging.PubMaster(cfg.pub_sub.keys())
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
log_msgs = []
# We need to fake SubMaster alive since we can't inject a fake clock
setup_env(simulation=True, cfg=cfg)
setup_env(simulation=True, cfg=cfg, lr=lr)
if cfg.proc_name == "laikad":
ublox = Params().get_bool("UbloxAvailable")
keys = set(cfg.pub_sub.keys()) - ({"qcomGnss", } if ublox else {"ubloxGnss", })
pub_msgs = [msg for msg in pub_msgs if msg.which() in keys]
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
log_msgs = []
try:
with Timeout(TIMEOUT, error_msg=f"timed out testing process {repr(cfg.proc_name)}"):
while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
# Wait for process to startup
with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(cfg.proc_name)}"):
while not any(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
time.sleep(0)
# Make sure all subscribers are connected
sockets = {s: messaging.sub_sock(s, timeout=2000) for s in sub_sockets}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
# Make sure all subscribers are connected
sockets = {s: messaging.sub_sock(s, timeout=2000) for s in sub_sockets}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
for i, msg in enumerate(pub_msgs):
# Do the replay
cnt = 0
for msg in pub_msgs:
with Timeout(TIMEOUT, error_msg=f"timed out testing process {repr(cfg.proc_name)}, {cnt}/{len(pub_msgs)} msgs done"):
pm.send(msg.which(), msg.as_builder())
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
resp_sockets = cfg.pub_sub[msg.which()] if cfg.should_recv_callback is None else cfg.should_recv_callback(msg)
resp_sockets = cfg.pub_sub[msg.which()]
if cfg.should_recv_callback is not None:
resp_sockets, _ = cfg.should_recv_callback(msg, None, None, None)
for s in resp_sockets:
response = messaging.recv_one_retry(sockets[s])
if response is None:
print(f"Warning, no response received {i}")
else:
response = response.as_builder()
response.logMonoTime = msg.logMonoTime
response = response.as_reader()
log_msgs.append(response)
if not len(resp_sockets): # We only need to wait if we didn't already wait for a response
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
m = messaging.recv_one_retry(sockets[s])
m = m.as_builder()
m.logMonoTime = msg.logMonoTime
log_msgs.append(m.as_reader())
cnt += 1
finally:
managed_processes[cfg.proc_name].signal(signal.SIGKILL)
managed_processes[cfg.proc_name].stop()
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
f9c7e05b836c4bff364978752e82d64b90f9d6e6
50f1e873095fe2462d2aadb9c401bda76759c01c
selfdrive/test/process_replay/test_processes.py
+7 -4
View File
@@ -18,7 +18,7 @@ from tools.lib.logreader import LogReader
source_segments = [
("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY
("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA
("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.KIA_EV6
("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.KIA_EV6 (+ QCOM GPS)
("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI)
("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR)
("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2
@@ -70,7 +70,7 @@ def run_test_process(data):
res = None
if not args.upload_only:
lr = LogReader.from_bytes(lr_dat)
res, log_msgs = test_process(cfg, lr, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs)
res, log_msgs = test_process(cfg, lr, segment, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs)
# save logs so we can upload when updating refs
save_log(cur_log_fn, log_msgs)
@@ -88,7 +88,7 @@ def get_log_data(segment):
return (segment, f.read())
def test_process(cfg, lr, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None):
def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None):
if ignore_fields is None:
ignore_fields = []
if ignore_msgs is None:
@@ -96,7 +96,10 @@ def test_process(cfg, lr, ref_log_path, new_log_path, ignore_fields=None, ignore
ref_log_msgs = list(LogReader(ref_log_path))
log_msgs = replay_process(cfg, lr)
try:
log_msgs = replay_process(cfg, lr)
except Exception as e:
raise Exception("failed on segment: " + segment) from e
# check to make sure openpilot is engaged in the route
if cfg.proc_name == "controlsd":
selfdrive/ui/qt/maps/map.cc
-38
View File
@@ -24,8 +24,6 @@ const float MAX_PITCH = 50;
const float MIN_PITCH = 0;
const float MAP_SCALE = 2;
const float VALID_POS_STD = 50.0; // m
const QString ICON_SUFFIX = ".png";
MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) {
@@ -125,42 +123,6 @@ void MapWindow::updateState(const UIState &s) {
}
}
// TODO should check a valid/status flag
if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){
auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements();
auto laikad_pos = laikad_location.getPositionECEF();
auto laikad_pos_ecef = laikad_pos.getValue();
auto laikad_pos_std = laikad_pos.getStd();
auto laikad_velocity_ecef = laikad_location.getVelocityECEF().getValue();
laikad_valid = laikad_pos.getValid() && Eigen::Vector3d(laikad_pos_std[0], laikad_pos_std[1], laikad_pos_std[2]).norm() < VALID_POS_STD;
if (laikad_valid && !locationd_valid) {
ECEF ecef = {.x = laikad_pos_ecef[0], .y = laikad_pos_ecef[1], .z = laikad_pos_ecef[2]};
Geodetic laikad_pos_geodetic = ecef2geodetic(ecef);
last_position = QMapbox::Coordinate(laikad_pos_geodetic.lat, laikad_pos_geodetic.lon);
// Compute NED velocity
LocalCoord converter(ecef);
ECEF next_ecef = {.x = ecef.x + laikad_velocity_ecef[0], .y = ecef.y + laikad_velocity_ecef[1], .z = ecef.z + laikad_velocity_ecef[2]};
Eigen::VectorXd ned_vel = converter.ecef2ned(next_ecef).to_vector() - converter.ecef2ned(ecef).to_vector();
float velocity = ned_vel.norm();
velocity_filter.update(velocity);
// Convert NED velocity to angle
if (velocity > 1.0) {
float new_bearing = fmod(RAD2DEG(atan2(ned_vel[1], ned_vel[0])) + 360.0, 360.0);
if (last_bearing) {
float delta = 0.1 * angle_difference(*last_bearing, new_bearing); // Smooth heading
last_bearing = fmod(*last_bearing + delta + 360.0, 360.0);
} else {
last_bearing = new_bearing;
}
}
}
}
if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) {
qWarning() << "Got new navRoute from navd. Opening map:" << allow_open;
selfdrive/ui/qt/onroad.cc
+11 -9
View File
@@ -1606,16 +1606,18 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) {
}
// Wide or narrow cam dependent on speed
float v_ego = sm["carState"].getCarState().getVEgo();
if ((v_ego < 10) || s->wide_cam_only) {
wide_cam_requested = true;
} else if (v_ego > 15) {
wide_cam_requested = false;
bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD);
if (has_wide_cam) {
float v_ego = sm["carState"].getCarState().getVEgo();
if ((v_ego < 10) || available_streams.size() == 1) {
wide_cam_requested = true;
} else if (v_ego > 15) {
wide_cam_requested = false;
}
wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode();
// for replay of old routes, never go to widecam
wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
}
wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode();
// TODO: also detect when ecam vision stream isn't available
// for replay of old routes, never go to widecam
wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
selfdrive/ui/qt/widgets/cameraview.cc
+14
View File
@@ -96,8 +96,10 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio)
CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) :
stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) {
setAttribute(Qt::WA_OpaquePaintEvent);
qRegisterMetaType<std::set<VisionStreamType>>("availableStreams");
QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection);
QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection);
QObject::connect(this, &CameraWidget::vipcAvailableStreamsUpdated, this, &CameraWidget::availableStreamsUpdated, Qt::QueuedConnection);
}
CameraWidget::~CameraWidget() {
@@ -181,6 +183,10 @@ void CameraWidget::stopVipcThread() {
}
}
void CameraWidget::availableStreamsUpdated(std::set<VisionStreamType> streams) {
available_streams = streams;
}
void CameraWidget::updateFrameMat() {
int w = width(), h = height();
@@ -366,6 +372,13 @@ void CameraWidget::vipcThread() {
if (!vipc_client->connected) {
clearFrames();
auto streams = VisionIpcClient::getAvailableStreams(stream_name, false);
if (streams.empty()) {
QThread::msleep(100);
continue;
}
emit vipcAvailableStreamsUpdated(streams);
if (!vipc_client->connect(false)) {
QThread::msleep(100);
continue;
@@ -400,4 +413,5 @@ void CameraWidget::vipcThread() {
void CameraWidget::clearFrames() {
std::lock_guard lk(frame_lock);
frames.clear();
available_streams.clear();
}
selfdrive/ui/qt/widgets/cameraview.h
+5
View File
@@ -41,6 +41,7 @@ signals:
void clicked();
void vipcThreadConnected(VisionIpcClient *);
void vipcThreadFrameReceived();
void vipcAvailableStreamsUpdated(std::set<VisionStreamType>);
protected:
void paintGL() override;
@@ -71,6 +72,7 @@ protected:
int stream_stride = 0;
std::atomic<VisionStreamType> active_stream_type;
std::atomic<VisionStreamType> requested_stream_type;
std::set<VisionStreamType> available_streams;
QThread *vipc_thread = nullptr;
// Calibration
@@ -88,4 +90,7 @@ protected:
protected slots:
void vipcConnected(VisionIpcClient *vipc_client);
void vipcFrameReceived();
void availableStreamsUpdated(std::set<VisionStreamType> streams);
};
Q_DECLARE_METATYPE(std::set<VisionStreamType>);
selfdrive/ui/ui.cc
+2 -5
View File
@@ -280,7 +280,6 @@ void UIState::updateStatus() {
if (scene.started) {
status = STATUS_DISENGAGED;
scene.started_frame = sm->frame;
wide_cam_only = Params().getBool("WideCameraOnly");
scene.live_torque_toggle = Params().getBool("LiveTorque");
scene.custom_torque_toggle = Params().getBool("CustomTorqueLateral");
}
@@ -300,13 +299,11 @@ UIState::UIState(QObject *parent) : QObject(parent) {
sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements",
"uiPlan", "carControl", "lateralPlan", "longitudinalPlan", "liveMapData", "gpsLocationExternal", "liveParameters",
"liveTorqueParameters",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "longitudinalPlan", "liveMapData",
"carControl", "lateralPlan", "gpsLocationExternal", "liveParameters", "liveTorqueParameters",
});
Params params;
wide_cam_only = params.getBool("WideCameraOnly");
prime_type = std::atoi(params.get("PrimeType").c_str());
language = QString::fromStdString(params.get("LanguageSetting"));
selfdrive/ui/ui.h
-1
View File
@@ -203,7 +203,6 @@ public:
QString language;
QTransform car_space_transform;
bool wide_cam_only;
signals:
void uiUpdate(const UIState &s);
selfdrive/ui/update_translations.py
+1 -1
View File
@@ -19,7 +19,7 @@ def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLA
for file in translation_files.values():
tr_file = os.path.join(translations_dir, f"{file}.ts")
args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file}"
args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file} -I {BASEDIR}"
if vanish:
args += " -no-obsolete"
if file in plural_only:
system/camerad/cameras/camera_common.cc
-3
View File
@@ -157,9 +157,6 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
framed.setHighConversionGain(frame_data.high_conversion_gain);
framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
framed.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setLensPos(frame_data.lens_pos);
framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos);
framed.setProcessingTime(frame_data.processing_time);
const float ev = c->cur_ev[frame_data.frame_id % 3];
system/camerad/cameras/camera_common.h
-5
View File
@@ -66,11 +66,6 @@ typedef struct FrameMetadata {
float measured_grey_fraction;
float target_grey_fraction;
// Focus
unsigned int lens_pos;
float lens_err;
float lens_true_pos;
float processing_time;
} FrameMetadata;
system/camerad/cameras/camera_qcom2.cc
+1 -1
View File
@@ -1175,7 +1175,7 @@ void CameraState::set_camera_exposure(float grey_frac) {
// t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD
uint32_t hcg_time = exposure_time;
uint32_t lcg_time = hcg_time;
uint32_t spd_time = exposure_time_max + VS_TIME_MAX_OX03C10;
uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10);
uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10);
uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g];
system/camerad/cameras/sensor2_i2c.h
+6 -3
View File
@@ -58,7 +58,7 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
// SC ctrl
{0x3001, 0x03}, // io_pad_oen
{0x3002, 0xf8}, // io_pad_oen
{0x3002, 0xfc}, // io_pad_oen
{0x3005, 0x80}, // io_pad_out
{0x3007, 0x01}, // io_pad_sel
{0x3008, 0x80}, // io_pad_sel
@@ -85,6 +85,9 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
{0x3892, 0x44},
{0x3823, 0x48},
{0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
{0x3020, 0x05}, // SC_CTRL_20
@@ -179,8 +182,8 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3820, 0x04},
{0x3821, 0x19},
{0x3832, 0x00},
{0x3834, 0x00},
{0x3832, 0xF0},
{0x3834, 0xF0},
{0x384c, 0x02},
{0x384d, 0x0d},
{0x3850, 0x00},
system/hardware/tici/amplifier.py
Regular → Executable
+9
View File
@@ -129,3 +129,12 @@ class Amplifier:
self.set_config(config)
self.set_global_shutdown(amp_disabled=False)
if __name__ == "__main__":
with open("/sys/firmware/devicetree/base/model") as f:
model = f.read().strip('\x00')
model = model.split('comma ')[-1]
amp = Amplifier()
amp.initialize_configuration(model)
system/loggerd/bootlog.cc
+6 -1
View File
@@ -2,6 +2,7 @@
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "system/loggerd/logger.h"
@@ -48,7 +49,8 @@ static kj::Array<capnp::word> build_boot_log() {
}
int main(int argc, char** argv) {
const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name();
const std::string timestr = logger_get_route_name();
const std::string path = LOG_ROOT + "/boot/" + timestr;
LOGW("bootlog to %s", path.c_str());
// Open bootlog
@@ -61,5 +63,8 @@ int main(int argc, char** argv) {
// Write bootlog
file.write(build_boot_log().asBytes());
// Write out bootlog param to match routes with bootlog
Params().put("CurrentBootlog", timestr.c_str());
return 0;
}
system/loggerd/tests/test_loggerd.py
+4 -4
View File
@@ -86,7 +86,7 @@ class TestLoggerd(unittest.TestCase):
params.clear_all()
for k, _, v in fake_params:
params.put(k, v)
params.put("LaikadEphemerisV2", "abc")
params.put("LaikadEphemerisV3", "abc")
lr = list(LogReader(str(self._gen_bootlog())))
initData = lr[0].initData
@@ -103,14 +103,14 @@ class TestLoggerd(unittest.TestCase):
# check params
logged_params = {entry.key: entry.value for entry in initData.params.entries}
expected_params = set(k for k, _, __ in fake_params) | {'LaikadEphemerisV2'}
expected_params = set(k for k, _, __ in fake_params) | {'LaikadEphemerisV3'}
assert set(logged_params.keys()) == expected_params, set(logged_params.keys()) ^ expected_params
assert logged_params['LaikadEphemerisV2'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['LaikadEphemerisV2'])}"
assert logged_params['LaikadEphemerisV3'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['LaikadEphemerisV3'])}"
for param_key, initData_key, v in fake_params:
self.assertEqual(getattr(initData, initData_key), v)
self.assertEqual(logged_params[param_key].decode(), v)
params.put("LaikadEphemerisV2", "")
params.put("LaikadEphemerisV3", "")
def test_rotation(self):
os.environ["LOGGERD_TEST"] = "1"
system/ubloxd/generated/ubx.cpp
+112 -24
View File
@@ -40,6 +40,11 @@ void ubx_t::_read() {
m_body = new rxm_sfrbx_t(m__io, this, m__root);
break;
}
case 309: {
n_body = false;
m_body = new nav_sat_t(m__io, this, m__root);
break;
}
case 2571: {
n_body = false;
m_body = new mon_hw2_t(m__io, this, m__root);
@@ -70,9 +75,9 @@ void ubx_t::_clean_up() {
ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
m_measurements = 0;
m__raw_measurements = 0;
m__io__raw_measurements = 0;
m_meas = 0;
m__raw_meas = 0;
m__io__raw_meas = 0;
try {
_read();
@@ -89,15 +94,15 @@ void ubx_t::rxm_rawx_t::_read() {
m_num_meas = m__io->read_u1();
m_rec_stat = m__io->read_u1();
m_reserved1 = m__io->read_bytes(3);
m__raw_measurements = new std::vector<std::string>();
m__io__raw_measurements = new std::vector<kaitai::kstream*>();
m_measurements = new std::vector<meas_t*>();
const int l_measurements = num_meas();
for (int i = 0; i < l_measurements; i++) {
m__raw_measurements->push_back(m__io->read_bytes(32));
kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1));
m__io__raw_measurements->push_back(io__raw_measurements);
m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root));
m__raw_meas = new std::vector<std::string>();
m__io__raw_meas = new std::vector<kaitai::kstream*>();
m_meas = new std::vector<measurement_t*>();
const int l_meas = num_meas();
for (int i = 0; i < l_meas; i++) {
m__raw_meas->push_back(m__io->read_bytes(32));
kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1));
m__io__raw_meas->push_back(io__raw_meas);
m_meas->push_back(new measurement_t(io__raw_meas, this, m__root));
}
}
@@ -106,24 +111,24 @@ ubx_t::rxm_rawx_t::~rxm_rawx_t() {
}
void ubx_t::rxm_rawx_t::_clean_up() {
if (m__raw_measurements) {
delete m__raw_measurements; m__raw_measurements = 0;
if (m__raw_meas) {
delete m__raw_meas; m__raw_meas = 0;
}
if (m__io__raw_measurements) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) {
if (m__io__raw_meas) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) {
delete *it;
}
delete m__io__raw_measurements; m__io__raw_measurements = 0;
delete m__io__raw_meas; m__io__raw_meas = 0;
}
if (m_measurements) {
for (std::vector<meas_t*>::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) {
if (m_meas) {
for (std::vector<measurement_t*>::iterator it = m_meas->begin(); it != m_meas->end(); ++it) {
delete *it;
}
delete m_measurements; m_measurements = 0;
delete m_meas; m_meas = 0;
}
}
ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
ubx_t::rxm_rawx_t::measurement_t::measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
@@ -135,7 +140,7 @@ ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__
}
}
void ubx_t::rxm_rawx_t::meas_t::_read() {
void ubx_t::rxm_rawx_t::measurement_t::_read() {
m_pr_mes = m__io->read_f8le();
m_cp_mes = m__io->read_f8le();
m_do_mes = m__io->read_f4le();
@@ -152,11 +157,11 @@ void ubx_t::rxm_rawx_t::meas_t::_read() {
m_reserved3 = m__io->read_bytes(1);
}
ubx_t::rxm_rawx_t::meas_t::~meas_t() {
ubx_t::rxm_rawx_t::measurement_t::~measurement_t() {
_clean_up();
}
void ubx_t::rxm_rawx_t::meas_t::_clean_up() {
void ubx_t::rxm_rawx_t::measurement_t::_clean_up() {
}
ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
@@ -198,6 +203,89 @@ void ubx_t::rxm_sfrbx_t::_clean_up() {
}
}
ubx_t::nav_sat_t::nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
m_svs = 0;
m__raw_svs = 0;
m__io__raw_svs = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_sat_t::_read() {
m_itow = m__io->read_u4le();
m_version = m__io->read_u1();
m_num_svs = m__io->read_u1();
m_reserved = m__io->read_bytes(2);
m__raw_svs = new std::vector<std::string>();
m__io__raw_svs = new std::vector<kaitai::kstream*>();
m_svs = new std::vector<nav_t*>();
const int l_svs = num_svs();
for (int i = 0; i < l_svs; i++) {
m__raw_svs->push_back(m__io->read_bytes(12));
kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1));
m__io__raw_svs->push_back(io__raw_svs);
m_svs->push_back(new nav_t(io__raw_svs, this, m__root));
}
}
ubx_t::nav_sat_t::~nav_sat_t() {
_clean_up();
}
void ubx_t::nav_sat_t::_clean_up() {
if (m__raw_svs) {
delete m__raw_svs; m__raw_svs = 0;
}
if (m__io__raw_svs) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) {
delete *it;
}
delete m__io__raw_svs; m__io__raw_svs = 0;
}
if (m_svs) {
for (std::vector<nav_t*>::iterator it = m_svs->begin(); it != m_svs->end(); ++it) {
delete *it;
}
delete m_svs; m_svs = 0;
}
}
ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_sat_t::nav_t::_read() {
m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
m_sv_id = m__io->read_u1();
m_cno = m__io->read_u1();
m_elev = m__io->read_s1();
m_azim = m__io->read_s2le();
m_pr_res = m__io->read_s2le();
m_flags = m__io->read_u4le();
}
ubx_t::nav_sat_t::nav_t::~nav_t() {
_clean_up();
}
void ubx_t::nav_sat_t::nav_t::_clean_up() {
}
ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
system/ubloxd/generated/ubx.h
+84 -10
View File
@@ -16,6 +16,7 @@ class ubx_t : public kaitai::kstruct {
public:
class rxm_rawx_t;
class rxm_sfrbx_t;
class nav_sat_t;
class nav_pvt_t;
class mon_hw2_t;
class mon_hw_t;
@@ -42,7 +43,7 @@ public:
class rxm_rawx_t : public kaitai::kstruct {
public:
class meas_t;
class measurement_t;
rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
@@ -53,18 +54,18 @@ public:
public:
~rxm_rawx_t();
class meas_t : public kaitai::kstruct {
class measurement_t : public kaitai::kstruct {
public:
meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0);
measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~meas_t();
~measurement_t();
private:
double m_pr_mes;
@@ -110,11 +111,11 @@ public:
uint8_t m_num_meas;
uint8_t m_rec_stat;
std::string m_reserved1;
std::vector<meas_t*>* m_measurements;
std::vector<measurement_t*>* m_meas;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_measurements;
std::vector<kaitai::kstream*>* m__io__raw_measurements;
std::vector<std::string>* m__raw_meas;
std::vector<kaitai::kstream*>* m__io__raw_meas;
public:
double rcv_tow() const { return m_rcv_tow; }
@@ -123,11 +124,11 @@ public:
uint8_t num_meas() const { return m_num_meas; }
uint8_t rec_stat() const { return m_rec_stat; }
std::string reserved1() const { return m_reserved1; }
std::vector<meas_t*>* measurements() const { return m_measurements; }
std::vector<measurement_t*>* meas() const { return m_meas; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_measurements() const { return m__raw_measurements; }
std::vector<kaitai::kstream*>* _io__raw_measurements() const { return m__io__raw_measurements; }
std::vector<std::string>* _raw_meas() const { return m__raw_meas; }
std::vector<kaitai::kstream*>* _io__raw_meas() const { return m__io__raw_meas; }
};
class rxm_sfrbx_t : public kaitai::kstruct {
@@ -170,6 +171,79 @@ public:
ubx_t* _parent() const { return m__parent; }
};
class nav_sat_t : public kaitai::kstruct {
public:
class nav_t;
nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_sat_t();
class nav_t : public kaitai::kstruct {
public:
nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
uint8_t m_cno;
int8_t m_elev;
int16_t m_azim;
int16_t m_pr_res;
uint32_t m_flags;
ubx_t* m__root;
ubx_t::nav_sat_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
uint8_t cno() const { return m_cno; }
int8_t elev() const { return m_elev; }
int16_t azim() const { return m_azim; }
int16_t pr_res() const { return m_pr_res; }
uint32_t flags() const { return m_flags; }
ubx_t* _root() const { return m__root; }
ubx_t::nav_sat_t* _parent() const { return m__parent; }
};
private:
uint32_t m_itow;
uint8_t m_version;
uint8_t m_num_svs;
std::string m_reserved;
std::vector<nav_t*>* m_svs;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_svs;
std::vector<kaitai::kstream*>* m__io__raw_svs;
public:
uint32_t itow() const { return m_itow; }
uint8_t version() const { return m_version; }
uint8_t num_svs() const { return m_num_svs; }
std::string reserved() const { return m_reserved; }
std::vector<nav_t*>* svs() const { return m_svs; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_svs() const { return m__raw_svs; }
std::vector<kaitai::kstream*>* _io__raw_svs() const { return m__io__raw_svs; }
};
class nav_pvt_t : public kaitai::kstruct {
public:
system/ubloxd/tests/ublox.py
+1
View File
@@ -48,6 +48,7 @@ MSG_NAV_TIMEGPS = 0x20
MSG_NAV_TIMEUTC = 0x21
MSG_NAV_CLOCK = 0x22
MSG_NAV_SVINFO = 0x30
MSG_NAV_SAT = 0x35
MSG_NAV_AOPSTATUS = 0x60
MSG_NAV_DGPS = 0x31
MSG_NAV_DOP = 0x04
system/ubloxd/ublox_msg.cc
+44 -40
View File
@@ -65,22 +65,6 @@ inline bool UbloxMsgParser::valid_so_far() {
return true;
}
inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) {
// convert time to year (conversion from A3.1.3)
int J = 0;
if (1 <= Nt && Nt <= 366) {
J = 1;
} else if (367 <= Nt && Nt <= 731) {
J = 2;
} else if (732 <= Nt && Nt <= 1096) {
J = 3;
} else if (1097 <= Nt && Nt <= 1461) {
J = 4;
}
uint16_t year = 1996 + 4*(N4 -1) + (J - 1);
return year;
}
bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
last_log_time = log_time;
int needed = needed_bytes();
@@ -203,6 +187,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
int iode_s2 = 0;
int iode_s3 = 0;
int iodc_lsb = 0;
int week;
// Subframe 1
{
@@ -210,7 +195,14 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
gps_t subframe(&stream);
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
eph.setGpsWeek(subframe_1->week_no());
// Each message is incremented to be greater or equal than week 1877 (2015-12-27).
// To skip this use the current_time argument
week = subframe_1->week_no();
week += 1024;
if (week < 1877) {
week += 1024;
}
//eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
eph.setToc(subframe_1->t_oc() * pow(2, 4));
eph.setAf2(subframe_1->af_2() * pow(2, -55));
@@ -227,6 +219,12 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
// GPS week refers to current week, the ephemeris can be valid for the next
// if toe equals 0, this can be verified by the TOW count if it is within the
// last 2 hours of the week (gps ephemeris valid for 4hours)
if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
week += 1;
}
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
@@ -256,6 +254,9 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
iode_s3 = subframe_3->iode();
}
eph.setToeWeek(week);
eph.setTocWeek(week);
gps_subframes[msg->sv_id()].clear();
if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
// data set cutover, reject ephemeris
@@ -329,7 +330,10 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
eph.setSvId(msg->sv_id());
eph.setFreqNum(msg->freq_id() - 7);
uint16_t current_day = 0;
uint16_t tk = 0;
// string number 1
{
@@ -338,7 +342,8 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
eph.setP1(data->p1());
eph.setTk(data->t_k());
tk = data->t_k();
eph.setTkDEPRECATED(tk);
eph.setXVel(data->x_vel() * pow(2, -20));
eph.setXAccel(data->x_accel() * pow(2, -30));
eph.setX(data->x() * pow(2, -11));
@@ -379,6 +384,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
current_day = data->n_t();
eph.setNt(current_day);
eph.setTauN(data->tau_n() * pow(2, -30));
eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
eph.setAge(data->e_n());
@@ -398,27 +404,9 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string5 parsing is only needed to get the year, this can be removed and
// the year can be fetched later in laika (note rollovers and leap year)
uint8_t n_4 = data->n_4();
uint16_t year = get_glonass_year(n_4, current_day);
if (current_day > 1461) {
// impossible day within last 4 year, reject ephemeris
// TODO: check if this can be detected via hamming code
LOGE("INVALID DATA: current day out of range: %d, %d", current_day, n_4);
glonass_strings[msg->sv_id()].clear();
return kj::Array<capnp::word>();
}
uint16_t last_leap_year = 1996 + 4*(n_4-1);
uint16_t days_till_this_year = (year - last_leap_year)*365;
if (days_till_this_year != 0) {
days_till_this_year++;
}
eph.setYear(year);
eph.setDayInYear(current_day - days_till_this_year);
eph.setHour((eph.getTk()>>7) & 0x1F);
eph.setMinute((eph.getTk()>>1) & 0x3F);
eph.setSecond((eph.getTk() & 0x1) * 30);
eph.setN4(data->n_4());
int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
eph.setTkSeconds(tk_seconds);
}
glonass_strings[msg->freq_id()].clear();
@@ -446,7 +434,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
mr.setGpsWeek(msg->week());
auto mb = mr.initMeasurements(msg->num_meas());
auto measurements = *msg->measurements();
auto measurements = *msg->meas();
for(int8_t i = 0; i < msg->num_meas(); i++) {
mb[i].setSvId(measurements[i]->sv_id());
mb[i].setPseudorange(measurements[i]->pr_mes());
@@ -475,6 +463,22 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) {
MessageBuilder msg_builder;
auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport();
sr.setITow(msg->itow());
auto svs = sr.initSvs(msg->num_svs());
auto svs_data = *msg->svs();
for(int8_t i = 0; i < msg->num_svs(); i++) {
svs[i].setSvId(svs_data[i]->sv_id());
svs[i].setGnssId(svs_data[i]->gnss_id());
svs[i].setFlagsBitfield(svs_data[i]->flags());
}
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
MessageBuilder msg_builder;
auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
system/ubloxd/ublox_msg.h
+6 -1
View File
@@ -15,6 +15,11 @@
using namespace std::string_literals;
const int SECS_IN_MIN = 60;
const int SECS_IN_HR = 60 * SECS_IN_MIN;
const int SECS_IN_DAY = 24 * SECS_IN_HR;
const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
// protocol constants
namespace ublox {
const uint8_t PREAMBLE1 = 0xb5;
@@ -97,12 +102,12 @@ class UbloxMsgParser {
kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
kj::Array<capnp::word> gen_mon_hw2(ubx_t::mon_hw2_t *msg);
kj::Array<capnp::word> gen_nav_sat(ubx_t::nav_sat_t *msg);
private:
inline bool valid_cheksum();
inline bool valid();
inline bool valid_so_far();
inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt);
kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
system/ubloxd/ubx.ksy
+37 -3
View File
@@ -17,6 +17,7 @@ seq:
0x0215: rxm_rawx
0x0a09: mon_hw
0x0a0b: mon_hw2
0x0135: nav_sat
instances:
checksum:
pos: length + 6
@@ -142,13 +143,13 @@ types:
type: u1
- id: reserved1
size: 3
- id: measurements
type: meas
- id: meas
type: measurement
size: 32
repeat: expr
repeat-expr: num_meas
types:
meas:
measurement:
seq:
- id: pr_mes
type: f8
@@ -179,6 +180,39 @@ types:
type: u1
- id: reserved3
size: 1
nav_sat:
seq:
- id: itow
type: u4
- id: version
type: u1
- id: num_svs
type: u1
- id: reserved
size: 2
- id: svs
type: nav
size: 12
repeat: expr
repeat-expr: num_svs
types:
nav:
seq:
- id: gnss_id
type: u1
enum: gnss_type
- id: sv_id
type: u1
- id: cno
type: u1
- id: elev
type: s1
- id: azim
type: s2
- id: pr_res
type: s2
- id: flags
type: u4
nav_pvt:
seq:
tools/cabana/binaryview.cc
+1 -1
View File
@@ -422,7 +422,7 @@ void BinaryItemDelegate::drawBorder(QPainter* painter, const QStyleOptionViewIte
painter->setClipRegion(QRegion(rc).subtracted(subtract));
if (!subtract.isEmpty()) {
// fill gaps inside corners.
painter->setPen(QPen(border_color, 2));
painter->setPen(QPen(border_color, 2, Qt::SolidLine, Qt::SquareCap, Qt::MiterJoin));
for (auto &r : subtract) {
painter->drawRect(r);
}
tools/cabana/chartswidget.cc
+60 -77
View File
@@ -16,6 +16,8 @@
#include <QtConcurrent>
const int MAX_COLUMN_COUNT = 4;
static inline bool xLessThan(const QPointF &p, float x) { return p.x() < x; }
// ChartsWidget
ChartsWidget::ChartsWidget(QWidget *parent) : QFrame(parent) {
@@ -108,14 +110,14 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QFrame(parent) {
void ChartsWidget::eventsMerged() {
{
assert(!can->liveStreaming());
QFutureSynchronizer<void> future_synchronizer;
const auto events = can->events();
for (auto c : charts) {
future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr, events, true));
future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr));
}
}
updateState();
if (can->isPaused()) {
updateState();
}
}
void ChartsWidget::zoomIn(double min, double max) {
@@ -133,22 +135,13 @@ void ChartsWidget::zoomReset() {
void ChartsWidget::updateState() {
if (charts.isEmpty()) return;
const auto events = can->events();
if (can->liveStreaming()) {
// appends incoming events to the end of series
for (auto c : charts) {
c->updateSeries(nullptr, events, false);
}
}
const double cur_sec = can->currentSec();
if (!is_zoomed) {
double pos = (cur_sec - display_range.first) / std::max(1.0, (display_range.second - display_range.first));
double pos = (cur_sec - display_range.first) / std::max<float>(1.0, max_chart_range);
if (pos < 0 || pos > 0.8) {
display_range.first = std::max(0.0, cur_sec - max_chart_range * 0.1);
}
double max_event_sec = events->empty() ? 0 : (events->back()->mono_time / 1e9 - can->routeStartTime());
double max_sec = std::min(std::floor(display_range.first + max_chart_range), max_event_sec);
double max_sec = std::min(std::floor(display_range.first + max_chart_range), can->lastEventSecond());
display_range.first = std::max(0.0, max_sec - max_chart_range);
display_range.second = display_range.first + max_chart_range;
} else if (cur_sec < zoomed_range.first || cur_sec >= zoomed_range.second) {
@@ -333,6 +326,7 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) {
setRenderHint(QPainter::Antialiasing);
// TODO: enable zoomIn/seekTo in live streaming mode.
setRubberBand(can->liveStreaming() ? QChartView::NoRubberBand : QChartView::HorizontalRubberBand);
setMouseTracking(true);
QObject::connect(dbc(), &DBCManager::signalRemoved, this, &ChartView::signalRemoved);
QObject::connect(dbc(), &DBCManager::signalUpdated, this, &ChartView::signalUpdated);
@@ -421,8 +415,8 @@ void ChartView::signalUpdated(const cabana::Signal *sig) {
}
}
void ChartView::msgUpdated(uint32_t address) {
if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.msg_id.address == address; }))
void ChartView::msgUpdated(MessageId id) {
if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.msg_id == id; }))
updateTitle();
}
@@ -488,8 +482,8 @@ void ChartView::updatePlot(double cur, double min, double max) {
void ChartView::updateSeriesPoints() {
// Show points when zoomed in enough
for (auto &s : sigs) {
auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; });
auto end = std::lower_bound(begin, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; });
auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan);
auto end = std::lower_bound(begin, s.vals.end(), axis_x->max(), xLessThan);
int num_points = std::max<int>(end - begin, 1);
int pixels_per_point = width() / num_points;
@@ -502,65 +496,34 @@ void ChartView::updateSeriesPoints() {
}
}
void ChartView::updateSeries(const cabana::Signal *sig, const std::vector<Event *> *events, bool clear) {
events = events ? events : can->events();
void ChartView::updateSeries(const cabana::Signal *sig) {
for (auto &s : sigs) {
if (!sig || s.sig == sig) {
if (clear) {
if (!can->liveStreaming()) {
s.vals.clear();
s.step_vals.clear();
s.vals.reserve(settings.max_cached_minutes * 60 * 100); // [n]seconds * 100hz
s.step_vals.reserve(settings.max_cached_minutes * 60 * 100 * 2);
s.last_value_mono_time = 0;
}
s.series->setColor(getColor(s.sig));
struct Chunk {
std::vector<Event *>::const_iterator first, second;
QVector<QPointF> vals;
QVector<QPointF> step_vals;
};
// split into one minitue chunks
QVector<Chunk> chunks;
Event begin_event(cereal::Event::Which::INIT_DATA, s.last_value_mono_time);
auto begin = std::upper_bound(events->begin(), events->end(), &begin_event, Event::lessThan());
for (auto it = begin, second = begin; it != events->end(); it = second) {
second = std::lower_bound(it, events->end(), (*it)->mono_time + 1e9 * 60, [](auto &e, uint64_t ts) { return e->mono_time < ts; });
chunks.push_back({it, second});
auto msgs = can->events().at(s.msg_id);
auto first = std::upper_bound(msgs.cbegin(), msgs.cend(), CanEvent{.mono_time=s.last_value_mono_time});
int new_size = std::max<int>(s.vals.size() + std::distance(first, msgs.cend()), settings.max_cached_minutes * 60 * 100);
if (s.vals.capacity() <= new_size) {
s.vals.reserve(new_size * 2);
s.step_vals.reserve(new_size * 4);
}
QtConcurrent::blockingMap(chunks, [&](Chunk &chunk) {
chunk.vals.reserve(60 * 100); // 100 hz
chunk.step_vals.reserve(60 * 100 * 2); // 100 hz
double route_start_time = can->routeStartTime();
for (auto it = chunk.first; it != chunk.second; ++it) {
if ((*it)->which == cereal::Event::Which::CAN) {
for (const auto &c : (*it)->event.getCan()) {
if (s.msg_id.address == c.getAddress() && s.msg_id.source == c.getSrc()) {
auto dat = c.getDat();
double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *s.sig);
double ts = ((*it)->mono_time / (double)1e9) - route_start_time; // seconds
chunk.vals.push_back({ts, value});
if (!chunk.step_vals.empty()) {
chunk.step_vals.push_back({ts, chunk.step_vals.back().y()});
}
chunk.step_vals.push_back({ts,value});
}
}
}
const double route_start_time = can->routeStartTime();
for (auto end = msgs.cend(); first != end; ++first) {
double value = get_raw_value(first->dat, first->size, *s.sig);
double ts = first->mono_time / 1e9 - route_start_time; // seconds
s.vals.append({ts, value});
if (!s.step_vals.empty()) {
s.step_vals.append({ts, s.step_vals.back().y()});
}
});
for (auto &c : chunks) {
s.vals.append(c.vals);
if (!c.step_vals.empty()) {
if (!s.step_vals.empty()) {
s.step_vals.append({c.step_vals.first().x(), s.step_vals.back().y()});
}
s.step_vals.append(c.step_vals);
}
}
if (events->size()) {
s.last_value_mono_time = events->back()->mono_time;
s.step_vals.append({ts, value});
s.last_value_mono_time = first->mono_time;
}
if (!can->liveStreaming()) {
s.segment_tree.build(s.vals);
@@ -587,8 +550,8 @@ void ChartView::updateAxisY() {
unit.clear();
}
auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; });
auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; });
auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan);
auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), xLessThan);
if (can->liveStreaming()) {
for (auto it = first; it != last; ++it) {
if (it->y() < min) min = it->y();
@@ -703,6 +666,8 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) {
} else if ((max_rounded - min_rounded) >= 0.5) {
// zoom in if selected range is greater than 0.5s
emit zoomIn(min_rounded, max_rounded);
} else {
scene()->invalidate({}, QGraphicsScene::ForegroundLayer);
}
event->accept();
} else if (!can->liveStreaming() && event->button() == Qt::RightButton) {
@@ -723,10 +688,12 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) {
void ChartView::mouseMoveEvent(QMouseEvent *ev) {
// Scrubbing
if (is_scrubbing && QApplication::keyboardModifiers().testFlag(Qt::ShiftModifier)) {
double t = chart()->mapToValue({(double)ev->x(), (double)ev->y()}).x();
// Prevent seeking past the end of the route
t = std::clamp(t, 0., can->totalSeconds());
can->seekTo(t);
if (chart()->plotArea().contains(ev->pos())) {
double t = chart()->mapToValue(ev->pos()).x();
// Prevent seeking past the end of the route
t = std::clamp(t, 0., can->totalSeconds());
can->seekTo(t);
}
return;
}
@@ -773,6 +740,7 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) {
if (rubber_rect != rubber->geometry()) {
rubber->setGeometry(rubber_rect);
}
scene()->invalidate({}, QGraphicsScene::ForegroundLayer);
}
}
@@ -831,14 +799,29 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) {
painter->setPen(Qt::NoPen);
for (auto &s : sigs) {
if (s.series->useOpenGL() && s.series->isVisible() && s.series->pointsVisible()) {
auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; });
auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; });
auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan);
auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), xLessThan);
for (auto it = first; it != last; ++it) {
painter->setBrush(s.series->color());
painter->drawEllipse(chart()->mapToPosition(*it), 4, 4);
}
}
}
// paint zoom range
auto rubber = findChild<QRubberBand *>();
if (rubber && rubber->isVisible() && rubber->width() > 1) {
painter->setPen(Qt::white);
auto rubber_rect = rubber->geometry().normalized();
for (const auto &pt : {rubber_rect.bottomLeft(), rubber_rect.bottomRight()}) {
QString sec = QString::number(chart()->mapToValue(pt).x(), 'f', 1);
// ChartAxisElement's padding is 4 (https://codebrowser.dev/qt5/qtcharts/src/charts/axis/chartaxiselement_p.h.html)
auto r = painter->fontMetrics().boundingRect(sec).adjusted(-6, -4, 6, 4);
pt == rubber_rect.bottomLeft() ? r.moveTopRight(pt + QPoint{0, 2}) : r.moveTopLeft(pt + QPoint{0, 2});
painter->fillRect(r, Qt::gray);
painter->drawText(r, Qt::AlignCenter, sec);
}
}
}
QXYSeries *ChartView::createSeries(SeriesType type, QColor color) {
@@ -860,7 +843,7 @@ QXYSeries *ChartView::createSeries(SeriesType type, QColor color) {
series->setUseOpenGL(true);
// Qt doesn't properly apply device pixel ratio in OpenGL mode
QPen pen = series->pen();
pen.setWidth(2.0 * qApp->devicePixelRatio());
pen.setWidthF(2.0 * devicePixelRatioF());
series->setPen(pen);
#endif
chart()->addSeries(series);
@@ -934,7 +917,7 @@ SeriesSelector::SeriesSelector(QString title, QWidget *parent) : QDialog(parent)
auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel);
main_layout->addWidget(buttonBox, 3, 2);
for (auto it = can->can_msgs.cbegin(); it != can->can_msgs.cend(); ++it) {
for (auto it = can->last_msgs.cbegin(); it != can->last_msgs.cend(); ++it) {
if (auto m = dbc()->msg(it.key())) {
msgs_combo->addItem(QString("%1 (%2)").arg(m->name).arg(it.key().toString()), QVariant::fromValue(it.key()));
}
tools/cabana/chartswidget.h
+3 -3
View File
@@ -32,7 +32,7 @@ public:
ChartView(QWidget *parent = nullptr);
void addSeries(const MessageId &msg_id, const cabana::Signal *sig);
bool hasSeries(const MessageId &msg_id, const cabana::Signal *sig) const;
void updateSeries(const cabana::Signal *sig = nullptr, const std::vector<Event*> *events = nullptr, bool clear = true);
void updateSeries(const cabana::Signal *sig = nullptr);
void updatePlot(double cur, double min, double max);
void setSeriesType(SeriesType type);
void updatePlotArea(int left);
@@ -57,11 +57,11 @@ signals:
void axisYLabelWidthChanged(int w);
private slots:
void msgUpdated(uint32_t address);
void signalUpdated(const cabana::Signal *sig);
void manageSeries();
void handleMarkerClicked();
void msgRemoved(uint32_t address) { removeIf([=](auto &s) { return s.msg_id.address == address; }); }
void msgUpdated(MessageId id);
void msgRemoved(MessageId id) { removeIf([=](auto &s) { return s.msg_id == id; }); }
void signalRemoved(const cabana::Signal *sig) { removeIf([=](auto &s) { return s.sig == sig; }); }
private:
tools/cabana/dbc.cc
+6 -1
View File
@@ -1,4 +1,5 @@
#include "tools/cabana/dbc.h"
#include "tools/cabana/util.h"
uint qHash(const MessageId &item) {
return qHash(item.source) ^ qHash(item.address);
@@ -12,6 +13,10 @@ std::vector<const cabana::Signal*> cabana::Msg::getSignals() const {
return ret;
}
void cabana::Signal::updatePrecision() {
precision = std::max(num_decimals(factor), num_decimals(offset));
}
// helper functions
static QVector<int> BIG_ENDIAN_START_BITS = []() {
@@ -22,7 +27,7 @@ static QVector<int> BIG_ENDIAN_START_BITS = []() {
return ret;
}();
double get_raw_value(uint8_t *data, size_t data_size, const cabana::Signal &sig) {
double get_raw_value(const uint8_t *data, size_t data_size, const cabana::Signal &sig) {
int64_t val = 0;
int i = sig.msb / 8;
tools/cabana/dbc.h
+7 -1
View File
@@ -38,6 +38,10 @@ struct MessageId {
uint qHash(const MessageId &item);
Q_DECLARE_METATYPE(MessageId);
template <>
struct std::hash<MessageId> {
std::size_t operator()(const MessageId &k) const noexcept { return qHash(k); }
};
typedef QList<std::pair<QString, QString>> ValueDescription;
@@ -51,6 +55,8 @@ namespace cabana {
QString min, max, unit;
QString comment;
ValueDescription val_desc;
int precision = 0;
void updatePrecision();
};
struct Msg {
@@ -70,7 +76,7 @@ namespace cabana {
}
// Helper functions
double get_raw_value(uint8_t *data, size_t data_size, const cabana::Signal &sig);
double get_raw_value(const uint8_t *data, size_t data_size, const cabana::Signal &sig);
int bigEndianStartBitsIndex(int start_bit);
int bigEndianBitIndex(int index);
void updateSigSizeParamsFromRange(cabana::Signal &s, int start_bit, int size);
tools/cabana/dbcmanager.cc
+20 -4
View File
@@ -38,6 +38,7 @@ bool DBCManager::open(const QString &name, const QString &content, QString *erro
sig.factor = s.factor;
sig.offset = s.offset;
sig.is_little_endian = s.is_little_endian;
sig.updatePrecision();
}
}
parseExtraInfo(content);
@@ -143,19 +144,31 @@ void DBCManager::updateMsg(const MessageId &id, const QString &name, uint32_t si
auto &m = msgs[id.address];
m.name = name;
m.size = size;
emit msgUpdated(id.address);
// This DBC applies to all active sources, emit for every source
for (uint8_t source : sources) {
emit msgUpdated({.source = source, .address = id.address});
}
}
void DBCManager::removeMsg(const MessageId &id) {
msgs.erase(id.address);
emit msgRemoved(id.address);
// This DBC applies to all active sources, emit for every source
for (uint8_t source : sources) {
emit msgRemoved({.source = source, .address = id.address});
}
}
void DBCManager::addSignal(const MessageId &id, const cabana::Signal &sig) {
if (auto m = const_cast<cabana::Msg *>(msg(id.address))) {
m->sigs.push_back(sig);
auto s = &m->sigs.last();
emit signalAdded(id.address, s);
// This DBC applies to all active sources, emit for every source
for (uint8_t source : sources) {
emit signalAdded({.source = source, .address = id.address}, s);
}
}
}
@@ -191,8 +204,11 @@ QStringList DBCManager::signalNames() {
return ret;
}
void DBCManager::updateSources(const QSet<uint8_t> &s) {
sources = s;
}
DBCManager *dbc() {
static DBCManager dbc_manager(nullptr);
return &dbc_manager;
}
tools/cabana/dbcmanager.h
+31 -7
View File
@@ -5,6 +5,8 @@
#include <QMetaType>
#include <QObject>
#include <QString>
#include <QSet>
#include <QDebug>
#include "tools/cabana/dbc.h"
@@ -20,30 +22,52 @@ public:
void addSignal(const MessageId &id, const cabana::Signal &sig);
void updateSignal(const MessageId &id, const QString &sig_name, const cabana::Signal &sig);
void removeSignal(const MessageId &id, const QString &sig_name);
inline int msgCount() const { return msgs.size(); }
inline QString name() const { return name_; }
void updateMsg(const MessageId &id, const QString &name, uint32_t size);
void removeMsg(const MessageId &id);
inline const std::map<uint32_t, cabana::Msg> &messages() const { return msgs; }
inline std::map<MessageId, cabana::Msg> getMessages(uint8_t source) {
std::map<MessageId, cabana::Msg> ret;
for (auto &[address, msg] : msgs) {
MessageId id = {.source = source, .address = address};
ret[id] = msg;
}
return ret;
}
inline const cabana::Msg *msg(const MessageId &id) const { return msg(id.address); }
inline const cabana::Msg *msg(uint32_t address) const {
auto it = msgs.find(address);
return it != msgs.end() ? &it->second : nullptr;
inline const cabana::Msg* msg(uint8_t source, const QString &name) {
for (auto &[_, msg] : msgs) {
if (msg.name == name) {
return &msg;
}
}
return nullptr;
}
QStringList signalNames();
public slots:
void updateSources(const QSet<uint8_t> &s);
signals:
void signalAdded(uint32_t address, const cabana::Signal *sig);
void signalAdded(MessageId id, const cabana::Signal *sig);
void signalRemoved(const cabana::Signal *sig);
void signalUpdated(const cabana::Signal *sig);
void msgUpdated(uint32_t address);
void msgRemoved(uint32_t address);
void msgUpdated(MessageId id);
void msgRemoved(MessageId id);
void DBCFileChanged();
private:
void parseExtraInfo(const QString &content);
std::map<uint32_t, cabana::Msg> msgs;
QString name_;
QSet<uint8_t> sources;
inline const cabana::Msg *msg(uint32_t address) const {
auto it = msgs.find(address);
return it != msgs.end() ? &it->second : nullptr;
}
};
DBCManager *dbc();
tools/cabana/detailwidget.cc
+2 -3
View File
@@ -177,7 +177,7 @@ void DetailWidget::removeMsg() {
// EditMessageDialog
EditMessageDialog::EditMessageDialog(const MessageId &msg_id, const QString &title, int size, QWidget *parent)
: original_name(title), QDialog(parent) {
: original_name(title), msg_id(msg_id), QDialog(parent) {
setWindowTitle(tr("Edit message: %1").arg(msg_id.toString()));
QFormLayout *form_layout = new QFormLayout(this);
@@ -207,8 +207,7 @@ void EditMessageDialog::validateName(const QString &text) {
bool valid = text.compare(UNTITLED, Qt::CaseInsensitive) != 0;
error_label->setVisible(false);
if (!text.isEmpty() && valid && text != original_name) {
valid = std::none_of(dbc()->messages().begin(), dbc()->messages().end(),
[&text](auto &m) { return m.second.name == text; });
valid = dbc()->msg(msg_id.source, text) == nullptr;
if (!valid) {
error_label->setText(tr("Name already exists"));
error_label->setVisible(true);
tools/cabana/detailwidget.h
+1
View File
@@ -15,6 +15,7 @@ public:
EditMessageDialog(const MessageId &msg_id, const QString &title, int size, QWidget *parent);
void validateName(const QString &text);
MessageId msg_id;
QString original_name;
QDialogButtonBox *btn_box;
QLineEdit *name_edit;
tools/cabana/historylog.cc
+22 -28
View File
@@ -14,11 +14,14 @@ QVariant HistoryLogModel::data(const QModelIndex &index, int role) const {
if (index.column() == 0) {
return QString::number((m.mono_time / (double)1e9) - can->routeStartTime(), 'f', 2);
}
return show_signals ? QString::number(m.sig_values[index.column() - 1]) : toHex(m.data);
int i = index.column() - 1;
return show_signals ? QString::number(m.sig_values[i], 'f', sigs[i]->precision) : toHex(m.data);
} else if (role == ColorsRole) {
return QVariant::fromValue(m.colors);
} else if (role == BytesRole) {
return m.data;
} else if (role == Qt::TextAlignmentRole) {
return (uint32_t)(Qt::AlignRight | Qt::AlignVCenter);
}
return {};
}
@@ -116,40 +119,31 @@ template <class InputIt>
std::deque<HistoryLogModel::Message> HistoryLogModel::fetchData(InputIt first, InputIt last, uint64_t min_time) {
std::deque<HistoryLogModel::Message> msgs;
QVector<double> values(sigs.size());
for (auto it = first; it != last && (*it)->mono_time > min_time; ++it) {
if ((*it)->which == cereal::Event::Which::CAN) {
for (const auto &c : (*it)->event.getCan()) {
if (msg_id.address == c.getAddress() && msg_id.source == c.getSrc()) {
const auto dat = c.getDat();
for (int i = 0; i < sigs.size(); ++i) {
values[i] = get_raw_value((uint8_t *)dat.begin(), dat.size(), *sigs[i]);
}
if (!filter_cmp || filter_cmp(values[filter_sig_idx], filter_value)) {
auto &m = msgs.emplace_back();
m.mono_time = (*it)->mono_time;
m.data = QByteArray((char *)dat.begin(), dat.size());
m.sig_values = values;
if (msgs.size() >= batch_size && min_time == 0)
return msgs;
}
}
for (; first != last && first->mono_time > min_time; ++first) {
for (int i = 0; i < sigs.size(); ++i) {
values[i] = get_raw_value(first->dat, first->size, *sigs[i]);
}
if (!filter_cmp || filter_cmp(values[filter_sig_idx], filter_value)) {
auto &m = msgs.emplace_back();
m.mono_time = first->mono_time;
m.data = QByteArray((const char *)first->dat, first->size);
m.sig_values = values;
if (msgs.size() >= batch_size && min_time == 0) {
return msgs;
}
}
}
return msgs;
}
template std::deque<HistoryLogModel::Message> HistoryLogModel::fetchData<>(std::vector<const Event*>::iterator first, std::vector<const Event*>::iterator last, uint64_t min_time);
template std::deque<HistoryLogModel::Message> HistoryLogModel::fetchData<>(std::vector<const Event*>::reverse_iterator first, std::vector<const Event*>::reverse_iterator last, uint64_t min_time);
std::deque<HistoryLogModel::Message> HistoryLogModel::fetchData(uint64_t from_time, uint64_t min_time) {
auto events = can->events();
const auto &events = can->events().at(msg_id);
const auto freq = can->lastMessage(msg_id).freq;
const bool update_colors = !display_signals_mode || sigs.empty();
if (dynamic_mode) {
auto first = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { return e->mono_time < ts; });
auto msgs = fetchData(first, events->rend(), min_time);
auto first = std::upper_bound(events.rbegin(), events.rend(), CanEvent{.mono_time=from_time}, std::greater<CanEvent>());
auto msgs = fetchData(first, events.rend(), min_time);
if (update_colors && (min_time > 0 || messages.empty())) {
for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) {
hex_colors.compute(it->data, it->mono_time / (double)1e9, freq);
@@ -159,10 +153,10 @@ std::deque<HistoryLogModel::Message> HistoryLogModel::fetchData(uint64_t from_ti
return msgs;
} else {
assert(min_time == 0);
auto first = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { return ts < e->mono_time; });
auto msgs = fetchData(first, events->end(), 0);
auto first = std::upper_bound(events.begin(), events.end(), CanEvent{.mono_time=from_time});
auto msgs = fetchData(first, events.end(), 0);
if (update_colors) {
for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) {
for (auto it = msgs.begin(); it != msgs.end(); ++it) {
hex_colors.compute(it->data, it->mono_time / (double)1e9, freq);
it->colors = hex_colors.colors;
}
@@ -235,7 +229,7 @@ LogsWidget::LogsWidget(QWidget *parent) : QFrame(parent) {
delegate = new MessageBytesDelegate(this);
logs->setItemDelegateForColumn(1, new MessageBytesDelegate(this));
logs->setHorizontalHeader(new HeaderView(Qt::Horizontal, this));
logs->horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap);
logs->horizontalHeader()->setDefaultAlignment(Qt::AlignRight | (Qt::Alignment)Qt::TextWordWrap);
logs->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents);
logs->verticalHeader()->setVisible(false);
logs->setFrameShape(QFrame::NoFrame);
tools/cabana/mainwin.cc
+8 -12
View File
@@ -42,6 +42,7 @@ MainWindow::MainWindow() : QMainWindow() {
messages_widget->restoreHeaderState(settings.message_header_state);
qRegisterMetaType<uint64_t>("uint64_t");
qRegisterMetaType<QSet<uint8_t>>("QSet<uint8_t>");
qRegisterMetaType<ReplyMsgType>("ReplyMsgType");
installMessageHandler([this](ReplyMsgType type, const std::string msg) {
// use queued connection to recv the log messages from replay.
@@ -70,6 +71,7 @@ MainWindow::MainWindow() : QMainWindow() {
QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::loadDBCFromFingerprint);
QObject::connect(can, &AbstractStream::eventsMerged, this, &MainWindow::updateStatus);
QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged);
QObject::connect(can, &AbstractStream::sourcesUpdated, dbc(), &DBCManager::updateSources);
QObject::connect(UndoStack::instance(), &QUndoStack::cleanChanged, this, &MainWindow::undoStackCleanChanged);
QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &MainWindow::undoStackIndexChanged);
QObject::connect(&settings, &Settings::changed, this, &MainWindow::updateStatus);
@@ -131,8 +133,10 @@ void MainWindow::createActions() {
commands_act->setDefaultWidget(undo_view);
commands_menu->addAction(commands_act);
QMenu *tools_menu = menuBar()->addMenu(tr("&Tools"));
tools_menu->addAction(tr("Find &Similar Bits"), this, &MainWindow::findSimilarBits);
if (!can->liveStreaming()) {
QMenu *tools_menu = menuBar()->addMenu(tr("&Tools"));
tools_menu->addAction(tr("Find &Similar Bits"), this, &MainWindow::findSimilarBits);
}
QMenu *help_menu = menuBar()->addMenu(tr("&Help"));
help_menu->addAction(tr("Help"), this, &MainWindow::onlineHelp)->setShortcuts(QKeySequence::HelpContents);
@@ -310,7 +314,7 @@ void MainWindow::loadDBCFromClipboard() {
QString dbc_str = QGuiApplication::clipboard()->text();
QString error;
bool ret = dbc()->open("clipboard", dbc_str, &error);
if (ret && dbc()->messages().size() > 0) {
if (ret && dbc()->msgCount() > 0) {
QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!"));
} else {
QMessageBox msg_box(QMessageBox::Warning, tr("Failed to load DBC from clipboard"), tr("Make sure that you paste the text with correct format."));
@@ -444,15 +448,7 @@ void MainWindow::updateDownloadProgress(uint64_t cur, uint64_t total, bool succe
}
void MainWindow::updateStatus() {
float cached_minutes = 0;
if (!can->liveStreaming()) {
if (auto events = can->events(); !events->empty()) {
cached_minutes = (events->back()->mono_time - events->front()->mono_time) / (1e9 * 60);
}
} else {
settings.max_cached_minutes = settings.max_cached_minutes;
}
status_label->setText(tr("Cached Minutes:%1 FPS:%2").arg(cached_minutes, 0, 'f', 1).arg(settings.fps));
status_label->setText(tr("Cached Minutes:%1 FPS:%2").arg(settings.max_cached_minutes).arg(settings.fps));
}
void MainWindow::dockCharts(bool dock) {
tools/cabana/messageswidget.cc
+3 -3
View File
@@ -160,7 +160,7 @@ void MessageListModel::setFilterString(const QString &string) {
filter_str = string;
msgs.clear();
for (auto it = can->can_msgs.begin(); it != can->can_msgs.end(); ++it) {
for (auto it = can->last_msgs.begin(); it != can->last_msgs.end(); ++it) {
if (filter_str.isEmpty() || contains(it.key(), filter_str)) {
msgs.push_back(it.key());
}
@@ -206,8 +206,8 @@ void MessageListModel::sortMessages() {
void MessageListModel::msgsReceived(const QHash<MessageId, CanData> *new_msgs) {
int prev_row_count = msgs.size();
if (filter_str.isEmpty() && msgs.size() != can->can_msgs.size()) {
msgs = can->can_msgs.keys();
if (filter_str.isEmpty() && msgs.size() != can->last_msgs.size()) {
msgs = can->last_msgs.keys();
}
if (msgs.size() != prev_row_count) {
sortMessages();
tools/cabana/signaledit.cc
+10 -11
View File
@@ -62,7 +62,7 @@ void SignalModel::updateState(const QHash<MessageId, CanData> *msgs) {
auto &dat = can->lastMessage(msg_id).dat;
int row = 0;
for (auto item : root->children) {
QString value = QString::number(get_raw_value((uint8_t *)dat.begin(), dat.size(), *item->sig));
QString value = QString::number(get_raw_value((uint8_t *)dat.begin(), dat.size(), *item->sig), 'f', item->sig->precision);
if (!item->sig->unit.isEmpty()){
value += " " + item->sig->unit;
}
@@ -184,6 +184,7 @@ bool SignalModel::setData(const QModelIndex &index, const QVariant &value, int r
case Item::Desc: s.val_desc = value.value<ValueDescription>(); break;
default: return false;
}
s.updatePrecision();
bool ret = saveSignal(item->sig, s);
emit dataChanged(index, index, {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole});
return ret;
@@ -238,14 +239,13 @@ void SignalModel::addSignal(int start_bit, int size, bool little_endian) {
auto msg = dbc()->msg(msg_id);
for (int i = 0; !msg; ++i) {
QString name = QString("NEW_MSG_") + QString::number(msg_id.address, 16).toUpper();
if (i > 0) name += QString("_%1").arg(i);
if (std::none_of(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; })) {
if (!dbc()->msg(msg_id.source, name)) {
UndoStack::push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size()));
msg = dbc()->msg(msg_id);
}
}
cabana::Signal sig = {.is_little_endian = little_endian, .factor = 1};
cabana::Signal sig = {.is_little_endian = little_endian, .factor = 1, .min = "0", .max = QString::number(std::pow(2, size) - 1)};
for (int i = 1; /**/; ++i) {
sig.name = QString("NEW_SIGNAL_%1").arg(i);
if (msg->sig(sig.name) == nullptr) break;
@@ -264,14 +264,14 @@ void SignalModel::removeSignal(const cabana::Signal *sig) {
UndoStack::push(new RemoveSigCommand(msg_id, sig));
}
void SignalModel::handleMsgChanged(uint32_t address) {
if (address == msg_id.address) {
void SignalModel::handleMsgChanged(MessageId id) {
if (id == msg_id) {
refresh();
}
}
void SignalModel::handleSignalAdded(uint32_t address, const cabana::Signal *sig) {
if (address == msg_id.address) {
void SignalModel::handleSignalAdded(MessageId id, const cabana::Signal *sig) {
if (id == msg_id) {
int i = 0;
for (; i < root->children.size(); ++i) {
if (sig->start_bit < root->children[i]->sig->start_bit) break;
@@ -415,7 +415,7 @@ SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts),
hl->addWidget(collapse_btn);
// tree view
tree = new QTreeView(this);
tree = new TreeView(this);
tree->setModel(model = new SignalModel(this));
tree->setItemDelegate(new SignalItemDelegate(this));
tree->setFrameShape(QFrame::NoFrame);
@@ -439,9 +439,8 @@ SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts),
QObject::connect(tree, &QTreeView::viewportEntered, [this]() { emit highlight(nullptr); });
QObject::connect(tree, &QTreeView::entered, [this](const QModelIndex &index) { emit highlight(model->getItem(index)->sig); });
QObject::connect(model, &QAbstractItemModel::modelReset, this, &SignalView::rowsChanged);
QObject::connect(model, &QAbstractItemModel::rowsInserted, this, &SignalView::rowsChanged);
QObject::connect(model, &QAbstractItemModel::rowsRemoved, this, &SignalView::rowsChanged);
QObject::connect(dbc(), &DBCManager::signalAdded, [this](uint32_t address, const cabana::Signal *sig) { selectSignal(sig); });
QObject::connect(dbc(), &DBCManager::signalAdded, [this](MessageId id, const cabana::Signal *sig) { selectSignal(sig); });
setWhatsThis(tr(R"(
<b>Signal view</b><br />
tools/cabana/signaledit.h
+12 -3
View File
@@ -48,10 +48,10 @@ public:
private:
void insertItem(SignalModel::Item *parent_item, int pos, const cabana::Signal *sig);
void handleSignalAdded(uint32_t address, const cabana::Signal *sig);
void handleSignalAdded(MessageId id, const cabana::Signal *sig);
void handleSignalUpdated(const cabana::Signal *sig);
void handleSignalRemoved(const cabana::Signal *sig);
void handleMsgChanged(uint32_t address);
void handleMsgChanged(MessageId id);
void refresh();
void updateState(const QHash<MessageId, CanData> *msgs);
@@ -108,8 +108,17 @@ private:
void rowsChanged();
void leaveEvent(QEvent *event);
struct TreeView : public QTreeView {
TreeView(QWidget *parent) : QTreeView(parent) {}
void rowsInserted(const QModelIndex &parent, int start, int end) override {
((SignalView *)parentWidget())->rowsChanged();
// update widget geometries in QTreeView::rowsInserted
QTreeView::rowsInserted(parent, start, end);
}
};
MessageId msg_id;
QTreeView *tree;
TreeView *tree;
QLineEdit *filter_edit;
ChartsWidget *charts;
QLabel *signal_count_lb;
tools/cabana/streams/abstractstream.cc
+61 -53
View File
@@ -1,6 +1,5 @@
#include "tools/cabana/streams/abstractstream.h"
#include <QtConcurrent>
#include <QTimer>
AbstractStream *can = nullptr;
@@ -13,7 +12,7 @@ AbstractStream::AbstractStream(QObject *parent, bool is_live_streaming) : is_liv
void AbstractStream::process(QHash<MessageId, CanData> *messages) {
for (auto it = messages->begin(); it != messages->end(); ++it) {
can_msgs[it.key()] = it.value();
last_msgs[it.key()] = it.value();
}
emit updated();
emit msgsReceived(messages);
@@ -39,6 +38,11 @@ bool AbstractStream::updateEvent(const Event *event) {
data.colors = tracker.colors;
data.last_change_t = tracker.last_change_t;
data.bit_change_counts = tracker.bit_change_counts;
if (!sources.contains(id.source)) {
sources.insert(id.source);
emit sourcesUpdated(sources);
}
}
double ts = millis_since_boot();
@@ -57,64 +61,68 @@ bool AbstractStream::updateEvent(const Event *event) {
const CanData &AbstractStream::lastMessage(const MessageId &id) {
static CanData empty_data;
auto it = can_msgs.find(id);
return it != can_msgs.end() ? it.value() : empty_data;
auto it = last_msgs.find(id);
return it != last_msgs.end() ? it.value() : empty_data;
}
static QHash<MessageId, CanData> parseEvents(std::vector<Event *>::const_reverse_iterator first,
std::vector<Event *>::const_reverse_iterator last, double route_start_time) {
QHash<MessageId, CanData> msgs;
msgs.reserve(500);
for (auto it = first; it != last; ++it) {
if ((*it)->which == cereal::Event::Which::CAN) {
for (const auto &c : (*it)->event.getCan()) {
auto &m = msgs[{.source = c.getSrc(), .address = c.getAddress()}];
if (++m.count == 1) {
m.ts = ((*it)->mono_time / 1e9) - route_start_time;
m.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size());
m.colors = QVector<QColor>(m.dat.size(), QColor(0, 0, 0, 0));
m.last_change_t = QVector<double>(m.dat.size(), m.ts);
m.bit_change_counts.resize(m.dat.size());
}
}
}
}
return msgs;
};
// it is thread safe to update data in updateLastMsgsTo.
// updateEvent will not be called before replayStream::seekedTo return.
void AbstractStream::updateLastMsgsTo(double sec) {
uint64_t ts = (sec + routeStartTime()) * 1e9;
const uint64_t delta = std::max(std::ceil(sec / std::thread::hardware_concurrency()), 30.0) * 1e9;
const auto evs = events();
auto first = std::upper_bound(evs->crbegin(), evs->crend(), ts, [](uint64_t ts, auto &e) { return ts > e->mono_time; });
QFutureSynchronizer<QHash<MessageId, CanData>> synchronizer;
while(first != evs->crend()) {
ts = (*first)->mono_time > delta ? (*first)->mono_time - delta : 0;
auto last = std::lower_bound(first, evs->crend(), ts, [](auto &e, uint64_t ts) { return e->mono_time > ts; });
synchronizer.addFuture(QtConcurrent::run(parseEvents, first, last, routeStartTime()));
first = last;
}
synchronizer.waitForFinished();
new_msgs->clear();
change_trackers.clear();
can_msgs.clear();
last_msgs.clear();
counters.clear();
for (const auto &f : synchronizer.futures()) {
auto msgs = f.result();
for (auto it = msgs.cbegin(); it != msgs.cend(); ++it) {
counters[it.key()] += it.value().count;
auto m = can_msgs.find(it.key());
if (m == can_msgs.end()) {
m = can_msgs.insert(it.key(), it.value());
} else {
m.value().count += it.value().count;
}
m.value().freq = m.value().count / std::max(1.0, m.value().ts);
CanEvent last_event = {.mono_time = uint64_t((sec + routeStartTime()) * 1e9)};
for (auto &[id, e] : events_) {
auto it = std::lower_bound(e.crbegin(), e.crend(), last_event, std::greater<CanEvent>());
if (it != e.crend()) {
auto &m = last_msgs[id];
m.dat = QByteArray((const char *)it->dat, it->size);
m.ts = it->mono_time / 1e9 - routeStartTime();
m.count = std::distance(it, e.crend());
m.freq = m.count / std::max(1.0, m.ts);
m.last_change_t = QVector<double>(m.dat.size(), m.ts);
m.colors = QVector<QColor>(m.dat.size(), QColor(0, 0, 0, 0));
m.bit_change_counts = QVector<std::array<uint32_t, 8>>(m.dat.size());
counters[id] = m.count;
}
}
emit updated();
emit msgsReceived(&can_msgs);
QTimer::singleShot(0, [this]() {
emit updated();
emit msgsReceived(&last_msgs);
});
}
void AbstractStream::parseEvents(std::unordered_map<MessageId, std::deque<CanEvent>> &msgs,
std::vector<Event *>::const_iterator first, std::vector<Event *>::const_iterator last) {
for (; first != last; ++first) {
if ((*first)->which == cereal::Event::Which::CAN) {
for (const auto &c : (*first)->event.getCan()) {
auto dat = c.getDat();
auto &m = msgs[{.source = c.getSrc(), .address = c.getAddress()}].emplace_back();
m.size = std::min(dat.size(), std::size(m.dat));
memcpy(m.dat, (uint8_t *)dat.begin(), m.size);
m.mono_time = (*first)->mono_time;
}
last_event_ts = std::max(last_event_ts, (*first)->mono_time);
}
}
}
void AbstractStream::mergeEvents(std::vector<Event *>::const_iterator first, std::vector<Event *>::const_iterator last, bool append) {
if (first == last) return;
if (append) {
parseEvents(events_, first, last);
} else {
std::unordered_map<MessageId, std::deque<CanEvent>> new_events;
parseEvents(new_events, first, last);
for (auto &[id, new_e] : new_events) {
auto &e = events_[id];
auto it = std::upper_bound(e.cbegin(), e.cend(), new_e.front());
e.insert(it, new_e.cbegin(), new_e.cend());
}
}
emit eventsMerged();
}
tools/cabana/streams/abstractstream.h
+21 -4
View File
@@ -1,7 +1,8 @@
#pragma once
#include <atomic>
#include <deque>
#include <unordered_map>
#include <QColor>
#include <QHash>
@@ -20,6 +21,14 @@ struct CanData {
QVector<std::array<uint32_t, 8>> bit_change_counts;
};
struct CanEvent {
uint64_t mono_time;
uint8_t size;
uint8_t dat[64];
inline bool operator<(const CanEvent &r) const { return mono_time < r.mono_time; }
inline bool operator>(const CanEvent &r) const { return mono_time > r.mono_time; }
};
class AbstractStream : public QObject {
Q_OBJECT
@@ -27,6 +36,7 @@ public:
AbstractStream(QObject *parent, bool is_live_streaming);
virtual ~AbstractStream() {};
inline bool liveStreaming() const { return is_live_streaming; }
inline double lastEventSecond() const { return last_event_ts / 1e9 - routeStartTime(); }
virtual void seekTo(double ts) {}
virtual QString routeName() const = 0;
virtual QString carFingerprint() const { return ""; }
@@ -37,11 +47,13 @@ public:
virtual const CanData &lastMessage(const MessageId &id);
virtual VisionStreamType visionStreamType() const { return VISION_STREAM_ROAD; }
virtual const Route *route() const { return nullptr; }
virtual const std::vector<Event *> *events() const = 0;
virtual void setSpeed(float speed) {}
virtual bool isPaused() const { return false; }
virtual void pause(bool pause) {}
virtual const std::vector<Event*> *rawEvents() const { return nullptr; }
const std::unordered_map<MessageId, std::deque<CanEvent>> &events() const { return events_; }
virtual const std::vector<std::tuple<int, int, TimelineType>> getTimeline() { return {}; }
void mergeEvents(std::vector<Event *>::const_iterator first, std::vector<Event *>::const_iterator last, bool append);
signals:
void paused();
@@ -52,20 +64,25 @@ signals:
void updated();
void msgsReceived(const QHash<MessageId, CanData> *);
void received(QHash<MessageId, CanData> *);
void sourcesUpdated(const QSet<uint8_t> &s);
public:
QHash<MessageId, CanData> can_msgs;
QHash<MessageId, CanData> last_msgs;
QSet<uint8_t> sources;
protected:
void process(QHash<MessageId, CanData> *);
virtual void process(QHash<MessageId, CanData> *);
bool updateEvent(const Event *event);
void updateLastMsgsTo(double sec);
void parseEvents(std::unordered_map<MessageId, std::deque<CanEvent>> &msgs, std::vector<Event *>::const_iterator first, std::vector<Event *>::const_iterator last);
bool is_live_streaming = false;
std::atomic<bool> processing = false;
QHash<MessageId, uint32_t> counters;
std::unique_ptr<QHash<MessageId, CanData>> new_msgs;
QHash<MessageId, ChangeTracker> change_trackers;
std::unordered_map<MessageId, std::deque<CanEvent>> events_;
uint64_t last_event_ts = 0;
};
// A global pointer referring to the unique AbstractStream object
tools/cabana/streams/livestream.cc
+26 -43
View File
@@ -1,10 +1,8 @@
#include "tools/cabana/streams/livestream.h"
LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) {
timer = new QTimer(this);
timer->callOnTimeout(this, &LiveStream::removeExpiredEvents);
timer->start(3 * 1000);
#include <QTimer>
LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) {
stream_thread = new QThread(this);
QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); });
QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater);
@@ -15,8 +13,6 @@ LiveStream::~LiveStream() {
stream_thread->requestInterruption();
stream_thread->quit();
stream_thread->wait();
for (Event *e : can_events) ::delete e;
for (auto m : messages) delete m;
}
void LiveStream::streamThread() {
@@ -35,66 +31,53 @@ void LiveStream::streamThread() {
QThread::msleep(50);
continue;
}
AlignedBuffer *buf = messages.emplace_back(new AlignedBuffer());
Event *evt = ::new Event(buf->align(msg));
delete msg;
handleEvent(evt);
std::lock_guard lk(lock);
handleEvent(messages.emplace_back(msg).event);
// TODO: write stream to log file to replay it with cabana --data_dir flag.
}
}
void LiveStream::handleEvent(Event *evt) {
current_ts = evt->mono_time;
if (start_ts == 0 || current_ts < start_ts) {
if (current_ts < start_ts) {
if (start_ts == 0 || evt->mono_time < start_ts) {
if (evt->mono_time < start_ts) {
qDebug() << "stream is looping back to old time stamp";
}
start_ts = current_ts.load();
start_ts = current_ts = evt->mono_time;
emit streamStarted();
}
std::lock_guard lk(lock);
can_events.push_back(evt);
received.push_back(evt);
if (!pause_) {
if (speed_ < 1 && last_update_ts > 0) {
auto it = std::upper_bound(can_events.begin(), can_events.end(), last_update_event_ts, [](uint64_t ts, auto &e) {
auto it = std::upper_bound(received.cbegin(), received.cend(), current_ts, [](uint64_t ts, auto &e) {
return ts < e->mono_time;
});
if (it != received.cend()) {
bool skip = (nanos_since_boot() - last_update_ts) < ((*it)->mono_time - current_ts) / speed_;
if (skip) return;
if (it != can_events.end() &&
(nanos_since_boot() - last_update_ts) < ((*it)->mono_time - last_update_event_ts) / speed_) {
return;
evt = *it;
}
evt = (*it);
}
updateEvent(evt);
last_update_event_ts = evt->mono_time;
current_ts = evt->mono_time;
last_update_ts = nanos_since_boot();
updateEvent(evt);
}
}
void LiveStream::removeExpiredEvents() {
std::lock_guard lk(lock);
if (can_events.size() > 0) {
const uint64_t max_ns = settings.max_cached_minutes * 60 * 1e9;
const uint64_t last_ns = can_events.back()->mono_time;
while (!can_events.empty() && (last_ns - can_events.front()->mono_time) > max_ns) {
::delete can_events.front();
delete messages.front();
can_events.pop_front();
messages.pop_front();
void LiveStream::process(QHash<MessageId, CanData> *last_messages) {
{
std::lock_guard lk(lock);
auto first = std::upper_bound(received.cbegin(), received.cend(), last_event_ts, [](uint64_t ts, auto &e) {
return ts < e->mono_time;
});
mergeEvents(first, received.cend(), true);
if (speed_ == 1) {
received.clear();
messages.clear();
}
}
}
const std::vector<Event *> *LiveStream::events() const {
std::lock_guard lk(lock);
if (events_vector.capacity() <= can_events.size()) {
events_vector.reserve(can_events.size() * 2);
}
events_vector.assign(can_events.begin(), can_events.end());
return &events_vector;
AbstractStream::process(last_messages);
}
void LiveStream::pause(bool pause) {
tools/cabana/streams/livestream.h
+13 -8
View File
@@ -1,6 +1,5 @@
#pragma once
#include <QTimer>
#include "tools/cabana/streams/abstractstream.h"
class LiveStream : public AbstractStream {
@@ -17,25 +16,31 @@ public:
void setSpeed(float speed) override { speed_ = std::min<float>(1.0, speed); }
bool isPaused() const override { return pause_; }
void pause(bool pause) override;
const std::vector<Event *> *events() const override;
protected:
virtual void handleEvent(Event *evt);
virtual void streamThread();
virtual void removeExpiredEvents();
void process(QHash<MessageId, CanData> *) override;
struct Msg {
Msg(Message *m) {
event = ::new Event(aligned_buf.align(m));
delete m;
}
~Msg() { ::delete event; }
Event *event;
AlignedBuffer aligned_buf;
};
mutable std::mutex lock;
mutable std::vector<Event *> events_vector;
std::deque<Event *> can_events;
std::deque<AlignedBuffer *> messages;
std::vector<Event *> received;
std::deque<Msg> messages;
std::atomic<uint64_t> start_ts = 0;
std::atomic<uint64_t> current_ts = 0;
std::atomic<float> speed_ = 1;
std::atomic<bool> pause_ = false;
uint64_t last_update_event_ts = 0;
uint64_t last_update_ts = 0;
const QString zmq_address;
QThread *stream_thread;
QTimer *timer;
};
tools/cabana/streams/replaystream.cc
+12 -1
View File
@@ -14,13 +14,24 @@ static bool event_filter(const Event *e, void *opaque) {
return ((ReplayStream *)opaque)->eventFilter(e);
}
void ReplayStream::mergeSegments() {
for (auto &[n, seg] : replay->segments()) {
if (seg && seg->isLoaded() && !processed_segments.count(n)) {
const auto &events = seg->log->events;
bool append = processed_segments.empty() || *processed_segments.rbegin() < n;
processed_segments.insert(n);
mergeEvents(events.cbegin(), events.cend(), append);
}
}
}
bool ReplayStream::loadRoute(const QString &route, const QString &data_dir) {
replay.reset(new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this));
replay->setSegmentCacheLimit(settings.max_cached_minutes);
replay->installEventFilter(event_filter, this);
QObject::connect(replay.get(), &Replay::seekedTo, this, &AbstractStream::seekedTo);
QObject::connect(replay.get(), &Replay::segmentsMerged, this, &AbstractStream::eventsMerged);
QObject::connect(replay.get(), &Replay::streamStarted, this, &AbstractStream::streamStarted);
QObject::connect(replay.get(), &Replay::segmentsMerged, this, &ReplayStream::mergeSegments);
if (replay->load()) {
const auto &segments = replay->route()->segments();
if (std::none_of(segments.begin(), segments.end(), [](auto &s) { return s.second.rlog.length() > 0; })) {
tools/cabana/streams/replaystream.h
+3 -1
View File
@@ -20,13 +20,15 @@ public:
inline double currentSec() const override { return replay->currentSeconds(); }
inline QDateTime currentDateTime() const override { return replay->currentDateTime(); }
inline const Route *route() const override { return replay->route(); }
inline const std::vector<Event *> *events() const override { return replay->events(); }
inline void setSpeed(float speed) override { replay->setSpeed(speed); }
inline bool isPaused() const override { return replay->isPaused(); }
void pause(bool pause) override;
const std::vector<Event*> *rawEvents() const override { return replay->events(); }
inline const std::vector<std::tuple<int, int, TimelineType>> getTimeline() override { return replay->getTimeline(); }
private:
void mergeSegments();
std::unique_ptr<Replay> replay = nullptr;
uint32_t replay_flags = REPLAY_FLAG_NONE;
std::set<int> processed_segments;
};
tools/cabana/tests/test_cabana.cc
+6 -6
View File
@@ -15,11 +15,11 @@ TEST_CASE("DBCManager::generateDBC") {
DBCManager dbc_from_generated(nullptr);
dbc_from_generated.open("", dbc_origin.generateDBC());
auto &msgs = dbc_origin.messages();
auto &new_msgs = dbc_from_generated.messages();
REQUIRE(msgs.size() == new_msgs.size());
for (auto &[address, m] : msgs) {
auto &new_m = new_msgs.at(address);
REQUIRE(dbc_origin.msgCount() == dbc_from_generated.msgCount());
auto msgs = dbc_origin.getMessages(0);
auto new_msgs = dbc_from_generated.getMessages(0);
for (auto &[id, m] : msgs) {
auto &new_m = new_msgs.at(id);
REQUIRE(m.name == new_m.name);
REQUIRE(m.size == new_m.size);
REQUIRE(m.getSignals().size() == new_m.getSignals().size());
@@ -43,7 +43,7 @@ TEST_CASE("Parse can messages") {
if (e->which == cereal::Event::Which::CAN) {
std::map<std::pair<uint32_t, QString>, std::vector<double>> values_1;
for (const auto &c : e->event.getCan()) {
const auto msg = dbc.msg(c.getAddress());
const auto msg = dbc.msg({.source = c.getSrc(), .address = c.getAddress()});
if (c.getSrc() == 0 && msg) {
for (auto sig : msg->getSignals()) {
double val = get_raw_value((uint8_t *)c.getDat().begin(), c.getDat().size(), *sig);
tools/cabana/tools/findsimilarbits.cc
+56 -32
View File
@@ -1,5 +1,6 @@
#include "tools/cabana/tools/findsimilarbits.h"
#include <QGridLayout>
#include <QHeaderView>
#include <QHBoxLayout>
#include <QIntValidator>
@@ -16,49 +17,68 @@ FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent, Qt::Wi
QVBoxLayout *main_layout = new QVBoxLayout(this);
QHBoxLayout *form_layout = new QHBoxLayout();
bus_combo = new QComboBox(this);
QHBoxLayout *src_layout = new QHBoxLayout();
src_bus_combo = new QComboBox(this);
find_bus_combo = new QComboBox(this);
QSet<uint8_t> bus_set;
for (auto it = can->can_msgs.begin(); it != can->can_msgs.end(); ++it) {
for (auto it = can->last_msgs.begin(); it != can->last_msgs.end(); ++it) {
bus_set << it.key().source;
}
for (uint8_t bus : bus_set) {
bus_combo->addItem(QString::number(bus), bus);
for (auto cb : {src_bus_combo, find_bus_combo}) {
for (uint8_t bus : bus_set) {
cb->addItem(QString::number(bus), bus);
}
cb->model()->sort(0);
cb->setCurrentIndex(0);
}
bus_combo->model()->sort(0);
bus_combo->setCurrentIndex(0);
msg_cb = new QComboBox(this);
for (auto &[address, msg] : dbc()->messages()) {
msg_cb->addItem(msg.name, address);
// TODO: update when src_bus_combo changes
for (auto &[id, msg] : dbc()->getMessages(0)) {
msg_cb->addItem(msg.name, id.address);
}
msg_cb->model()->sort(0);
msg_cb->setCurrentIndex(0);
byte_idx_sb = new QSpinBox(this);
byte_idx_sb->setFixedWidth(50);
byte_idx_sb->setRange(0, 63);
bit_idx_sb = new QSpinBox(this);
bit_idx_sb->setFixedWidth(50);
bit_idx_sb->setRange(0, 7);
form_layout->addWidget(new QLabel("Bus"));
form_layout->addWidget(bus_combo);
form_layout->addWidget(msg_cb);
form_layout->addWidget(new QLabel("Byte Index"));
form_layout->addWidget(byte_idx_sb);
form_layout->addWidget(new QLabel("Bit Index"));
form_layout->addWidget(bit_idx_sb);
src_layout->addWidget(new QLabel(tr("Bus")));
src_layout->addWidget(src_bus_combo);
src_layout->addWidget(msg_cb);
src_layout->addWidget(new QLabel(tr("Byte Index")));
src_layout->addWidget(byte_idx_sb);
src_layout->addWidget(new QLabel(tr("Bit Index")));
src_layout->addWidget(bit_idx_sb);
src_layout->addStretch(0);
QHBoxLayout *find_layout = new QHBoxLayout();
find_layout->addWidget(new QLabel(tr("Bus")));
find_layout->addWidget(find_bus_combo);
find_layout->addWidget(new QLabel(tr("Equal")));
equal_combo = new QComboBox(this);
equal_combo->addItems({"Yes", "No"});
find_layout->addWidget(equal_combo);
min_msgs = new QLineEdit(this);
min_msgs->setValidator(new QIntValidator(this));
min_msgs->setText("100");
form_layout->addWidget(new QLabel("Min msg count"));
form_layout->addWidget(min_msgs);
find_layout->addWidget(new QLabel(tr("Min msg count")));
find_layout->addWidget(min_msgs);
search_btn = new QPushButton(tr("&Find"), this);
form_layout->addWidget(search_btn);
form_layout->addStretch(1);
main_layout->addLayout(form_layout);
find_layout->addWidget(search_btn);
find_layout->addStretch(0);
QGridLayout *grid_layout = new QGridLayout();
grid_layout->addWidget(new QLabel("Find From:"), 0, 0);
grid_layout->addLayout(src_layout, 0, 1);
grid_layout->addWidget(new QLabel("Find In:"), 1, 0);
grid_layout->addLayout(find_layout, 1, 1);
main_layout->addLayout(grid_layout);
table = new QTableWidget(this);
table->setSelectionBehavior(QAbstractItemView::SelectRows);
@@ -69,10 +89,9 @@ FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent, Qt::Wi
setMinimumSize({700, 500});
QObject::connect(search_btn, &QPushButton::clicked, this, &FindSimilarBitsDlg::find);
QObject::connect(table, &QTableWidget::doubleClicked, [this](const QModelIndex &index) {
if (index.isValid()) {
MessageId msg_id = {.source = (uint8_t)bus_combo->currentData().toUInt(), .address = table->item(index.row(), 0)->text().toUInt(0, 16)};
MessageId msg_id = {.source = (uint8_t)find_bus_combo->currentData().toUInt(), .address = table->item(index.row(), 0)->text().toUInt(0, 16)};
emit openMessage(msg_id);
}
});
@@ -82,7 +101,8 @@ void FindSimilarBitsDlg::find() {
search_btn->setEnabled(false);
table->clear();
uint32_t selected_address = msg_cb->currentData().toUInt();
auto msg_mismatched = calcBits(bus_combo->currentText().toUInt(), selected_address, byte_idx_sb->value(), bit_idx_sb->value(), min_msgs->text().toInt());
auto msg_mismatched = calcBits(src_bus_combo->currentText().toUInt(), selected_address, byte_idx_sb->value(), bit_idx_sb->value(),
find_bus_combo->currentText().toUInt(), equal_combo->currentIndex() == 0, min_msgs->text().toInt());
table->setRowCount(msg_mismatched.size());
table->setColumnCount(6);
table->setHorizontalHeaderLabels({"address", "byte idx", "bit idx", "mismatches", "total msgs", "% mismatched"});
@@ -93,25 +113,29 @@ void FindSimilarBitsDlg::find() {
table->setItem(i, 2, new QTableWidgetItem(QString::number(m.bit_idx)));
table->setItem(i, 3, new QTableWidgetItem(QString::number(m.mismatches)));
table->setItem(i, 4, new QTableWidgetItem(QString::number(m.total)));
table->setItem(i, 5, new QTableWidgetItem(QString::number(m.perc)));
table->setItem(i, 5, new QTableWidgetItem(QString::number(m.perc, 'f', 2)));
}
search_btn->setEnabled(true);
}
QList<FindSimilarBitsDlg::mismatched_struct> FindSimilarBitsDlg::calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt) {
QList<FindSimilarBitsDlg::mismatched_struct> FindSimilarBitsDlg::calcBits(uint8_t bus, uint32_t selected_address, int byte_idx,
int bit_idx, uint8_t find_bus, bool equal, int min_msgs_cnt) {
QHash<uint32_t, QVector<uint32_t>> mismatches;
QHash<uint32_t, uint32_t> msg_count;
auto events = can->events();
auto events = can->rawEvents();
int bit_to_find = -1;
for (auto e : *events) {
if (e->which == cereal::Event::Which::CAN) {
for (const auto &c : e->event.getCan()) {
if (c.getSrc() == bus) {
const auto dat = c.getDat();
uint32_t address = c.getAddress();
uint8_t src = c.getSrc();
uint32_t address = c.getAddress();
const auto dat = c.getDat();
if (src == bus) {
if (address == selected_address && dat.size() > byte_idx) {
bit_to_find = ((dat[byte_idx] >> (7 - bit_idx)) & 1) != 0;
}
}
if (src == find_bus) {
++msg_count[address];
if (bit_to_find == -1) continue;
@@ -122,7 +146,7 @@ QList<FindSimilarBitsDlg::mismatched_struct> FindSimilarBitsDlg::calcBits(uint8_
for (int i = 0; i < dat.size(); ++i) {
for (int j = 0; j < 8; ++j) {
int bit = ((dat[i] >> (7 - j)) & 1) != 0;
mismatched[i * 8 + j] += (bit != bit_to_find);
mismatched[i * 8 + j] += equal ? (bit != bit_to_find) : (bit == bit_to_find);
}
}
}
tools/cabana/tools/findsimilarbits.h
+3 -2
View File
@@ -22,11 +22,12 @@ private:
uint32_t address, byte_idx, bit_idx, mismatches, total;
float perc;
};
QList<mismatched_struct> calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt);
QList<mismatched_struct> calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, uint8_t find_bus,
bool equal, int min_msgs_cnt);
void find();
QTableWidget *table;
QComboBox *bus_combo, *msg_cb;
QComboBox *src_bus_combo, *find_bus_combo, *msg_cb, *equal_combo;
QSpinBox *byte_idx_sb, *bit_idx_sb;
QPushButton *search_btn;
QLineEdit *min_msgs;
tools/cabana/util.cc
+10
View File
@@ -184,3 +184,13 @@ QString toHex(uint8_t byte) {
}();
return hex[byte];
}
int num_decimals(double num) {
const QString string = QString::number(num);
const QStringList split = string.split('.');
if (split.size() == 1) {
return 0;
} else {
return split[1].size();
}
}
tools/cabana/util.h
+1
View File
@@ -91,3 +91,4 @@ QPixmap icon(const QString &id);
}
QToolButton *toolButton(const QString &icon, const QString &tooltip);
int num_decimals(double num);
tools/cabana/videowidget.cc
+14 -10
View File
@@ -8,11 +8,12 @@
#include <QPixmap>
#include <QStyleOptionSlider>
#include <QTimeEdit>
#include <QTimer>
#include <QToolTip>
#include <QVBoxLayout>
#include <QtConcurrent>
const int MIN_VIDEO_HEIGHT = 100;
static const QColor timeline_colors[] = {
[(int)TimelineType::None] = QColor(111, 143, 175),
[(int)TimelineType::Engaged] = QColor(0, 163, 108),
@@ -22,7 +23,7 @@ static const QColor timeline_colors[] = {
[(int)TimelineType::AlertCritical] = QColor(199, 0, 57),
};
inline QString formatTime(int seconds) {
static inline QString formatTime(int seconds) {
return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss");
}
@@ -106,7 +107,7 @@ QWidget *VideoWidget::createCameraWidget() {
slider_layout->addWidget(end_time_label);
l->addLayout(slider_layout);
cam_widget->setMinimumHeight(100);
cam_widget->setMinimumHeight(MIN_VIDEO_HEIGHT);
cam_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding);
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum);
@@ -164,16 +165,12 @@ void VideoWidget::updatePlayBtnState() {
}
// Slider
Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) {
QTimer *timer = new QTimer(this);
timer->setInterval(2000);
timer->callOnTimeout([this]() {
Slider::Slider(QWidget *parent) : timer(this), QSlider(Qt::Horizontal, parent) {
timer.callOnTimeout([this]() {
timeline = can->getTimeline();
update();
});
setMouseTracking(true);
QObject::connect(can, SIGNAL(streamStarted()), timer, SLOT(start()));
QObject::connect(can, &AbstractStream::streamStarted, this, &Slider::streamStarted);
}
@@ -187,11 +184,13 @@ void Slider::streamStarted() {
thumnail_future.waitForFinished();
abort_load_thumbnail = false;
thumbnails.clear();
timeline.clear();
timer.start(2000);
thumnail_future = QtConcurrent::run(this, &Slider::loadThumbnails);
}
void Slider::loadThumbnails() {
const auto segments = can->route()->segments();
const auto &segments = can->route()->segments();
for (auto it = segments.rbegin(); it != segments.rend() && !abort_load_thumbnail; ++it) {
std::string qlog = it->second.qlog.toStdString();
if (!qlog.empty()) {
@@ -280,3 +279,8 @@ void Slider::mouseMoveEvent(QMouseEvent *e) {
QToolTip::showText(pt, thumb, this, rect());
QSlider::mouseMoveEvent(e);
}
void Slider::leaveEvent(QEvent *event) {
QToolTip::hideText();
QSlider::leaveEvent(event);
}
tools/cabana/videowidget.h
+3
View File
@@ -8,6 +8,7 @@
#include <QLabel>
#include <QPushButton>
#include <QSlider>
#include <QTimer>
#include "selfdrive/ui/qt/widgets/cameraview.h"
#include "selfdrive/ui/qt/widgets/controls.h"
@@ -24,6 +25,7 @@ public:
private:
void mousePressEvent(QMouseEvent *e) override;
void mouseMoveEvent(QMouseEvent *e) override;
void leaveEvent(QEvent *event) override;
void sliderChange(QAbstractSlider::SliderChange change) override;
void paintEvent(QPaintEvent *ev) override;
void streamStarted();
@@ -37,6 +39,7 @@ private:
QMap<uint64_t, QString> thumbnails;
QFuture<void> thumnail_future;
QSize thumbnail_size = {};
QTimer timer;
};
class VideoWidget : public QWidget {
tools/gpstest/rpc_server.py
+1 -1
View File
@@ -17,7 +17,7 @@ ALT_DELTA = 30
MATCH_NUM = 10
REPORT_STATS = 10
EPHEM_CACHE = "/data/params/d/LaikadEphemerisV2"
EPHEM_CACHE = "/data/params/d/LaikadEphemerisV3"
DOWNLOAD_CACHE = "/tmp/comma_download_cache"
SERVER_LOG_FILE = "/tmp/fuzzy_server.log"
tools/gpstest/test_laikad.py
+1 -1
View File
@@ -39,7 +39,7 @@ class TestLaikad(unittest.TestCase):
def setUp(self):
# ensure laikad cold start
Params().remove("LaikadEphemerisV2")
Params().remove("LaikadEphemerisV3")
os.environ["LAIKAD_NO_INTERNET"] = "1"
managed_processes['laikad'].start()

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