diff --git a/README.md b/README.md index 4896d9ff72..a71a9e671e 100755 --- a/README.md +++ b/README.md @@ -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 diff --git a/RELEASES.md b/RELEASES.md index 9cfcbafea1..55b0dbad7e 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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) diff --git a/cereal b/cereal index 7968cfe989..b53ed5bd84 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7968cfe98920181e844adae6422f932e84a6da71 +Subproject commit b53ed5bd84cb6da6d21b04088a905dc631f031fe diff --git a/common/params.cc b/common/params.cc index 18d1e426b1..a54aee61bf 100644 --- a/common/params.cc +++ b/common/params.cc @@ -98,6 +98,7 @@ std::unordered_map 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 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 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 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}, diff --git a/docs/CARS.md b/docs/CARS.md index 070d068cab..1f668dbe8b 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| |Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| @@ -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[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai 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)](##)|Hyundai C|| |Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|Sorento 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| +|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| |Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| |Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| @@ -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)](##)|Nissan A|| |Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| |Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan 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)](##)|Ram|| +|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)](##)|Ram|| |SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| diff --git a/laika_repo b/laika_repo index b740b71c82..6fadabd860 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit b740b71c82a748e3520b1599487d9a7aaf728670 +Subproject commit 6fadabd86043ee19e06c6ed59aa4e688c14fa8e4 diff --git a/opendbc b/opendbc index b8895f7c16..51bd657bb9 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b8895f7c16f84c1880089a864e2224b6f518bb11 +Subproject commit 51bd657bb9fd48e6ac361ace6b7530d93328c4c4 diff --git a/panda b/panda index ad2a0fe0cb..4b814c7381 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ad2a0fe0cb0d82db63492bb1b8ffff9d1f0184ee +Subproject commit 4b814c738120182812ebb45f6e8e3748a9f14373 diff --git a/poetry.lock b/poetry.lock index ec4022c81f..4abd6777ba 100644 --- a/poetry.lock +++ b/poetry.lock @@ -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"}, diff --git a/pyproject.toml b/pyproject.toml index b1ad273cd7..7506ee552d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -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" diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 5d87ff3903..70a02bfc84 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -113,32 +113,35 @@ bool safety_setter_thread(std::vector 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) { diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index b8ebbd88a3..d614f0b126 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -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()) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 971a7c35eb..9e32582c86 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -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() diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 2af5246b88..8799f073c9 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -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', ], diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index e5d550fac8..86bae91b06 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -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] diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index dd3b19f6de..7ae9bee404 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -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) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 8092ac0b76..9bb04ee821 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -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 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d67f22f8fa..85d4c01f64 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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: diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 2ac418b629..0d13cb9827 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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 { diff --git a/selfdrive/car/honda/tests/__init__.py b/selfdrive/car/honda/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 0737bea147..009a549afd 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -186,7 +186,7 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.UDS_VERSION_RESPONSE], bus=1, logging=True, - non_obd=True, + obd_multiplexing=False, ), ], extra_ecus=[ diff --git a/selfdrive/car/hyundai/tests/__init__.py b/selfdrive/car/hyundai/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 9f471076ae..c8cc9a4e52 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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 ', diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 5c7d3f5e4d..9c4447631d 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -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: { diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 4ca7c0a16c..4dd19d24e3 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -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), diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py index 3efdd7404e..d3e4de1798 100755 --- a/selfdrive/car/tests/test_lateral_limits.py +++ b/selfdrive/car/tests/test_lateral_limits.py @@ -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) diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index cc1681bce1..75051316fa 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -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] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 6f8cfe0ce6..cb423e1d87 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -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] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 696d9829d1..5feef12206 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 75cc48d6d8..85c0dc1a31 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 26fc898109..4cbfd53535 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/toyota/tests/__init__.py b/selfdrive/car/toyota/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/toyota/tests/test_toyota.py b/selfdrive/car/toyota/tests/test_toyota.py new file mode 100755 index 0000000000..5648f75fe7 --- /dev/null +++ b/selfdrive/car/toyota/tests/test_toyota.py @@ -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() diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 44a91d7edf..2fbb3c9908 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 36c707fb8b..24e479a66e 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a9877d7667..5c9bcfe4f6 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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 diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 4607635bcb..1d92123367 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -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) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index a522f917ff..ef85c368e4 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -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: { diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 0532087d39..76dbb5e11e 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -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() diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 92fc2468bb..18c8e79026 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -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)) diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 07ce5ebddb..3a0ff33cb6 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -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", diff --git a/selfdrive/debug/internal/check_frame_frequencies.py b/selfdrive/debug/internal/check_frame_frequencies.py new file mode 100755 index 0000000000..9ac8bfc80e --- /dev/null +++ b/selfdrive/debug/internal/check_frame_frequencies.py @@ -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() diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2b18dcf152..59176c586b 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -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() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 10ac7790b6..e184dc3ec4 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -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() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 7fa790aa98..ec5818a71a 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -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: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index d33852a83f..35b247dd5c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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"), diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index a483fa4db4..13759c62f8 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:736ddc08497d7596bae4d9515a8efb996676be80e67a6d34d632bb8af2ed3fa9 -size 45962515 +oid sha256:5121deb0d5c683b0fbee4c1cad7bc625953bf127b1383fb7599a6b644efd0aea +size 46011200 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index ad09668a46..bd026f5710 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -ab64afd1abd1059c14f50c67b51e5ef89029f216 +82db08d52b155336e9a1dadd11485d5acdf2eba0 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 971ddb3fe2..2b9096b423 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -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() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 79769e14b6..556781738d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f9c7e05b836c4bff364978752e82d64b90f9d6e6 +50f1e873095fe2462d2aadb9c401bda76759c01c diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 07117c2e7f..a2dd938e2f 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -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": diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index dd30b5feff..abbf434d69 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -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; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 9f7babca70..9779620371 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -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; diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 8c7a7072e2..de3b64cffd 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -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>("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 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(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index f8b97be03e..9135620d35 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -41,6 +41,7 @@ signals: void clicked(); void vipcThreadConnected(VisionIpcClient *); void vipcThreadFrameReceived(); + void vipcAvailableStreamsUpdated(std::set); protected: void paintGL() override; @@ -71,6 +72,7 @@ protected: int stream_stride = 0; std::atomic active_stream_type; std::atomic requested_stream_type; + std::set 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 streams); }; + +Q_DECLARE_METATYPE(std::set); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 12247c8b93..d062f36b22 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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>({ "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")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 70d1439646..63dc30de30 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -203,7 +203,6 @@ public: QString language; QTransform car_space_transform; - bool wide_cam_only; signals: void uiUpdate(const UIState &s); diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index e15d4c3433..5964eb49dc 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -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: diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 7ee3738057..9ef5d89a4e 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -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]; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 088b9f7939..07d1291a2a 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -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; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 92b3bde413..94116b11cd 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -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]; diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 83fcb8f7a9..4db4bfd4d8 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -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}, diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py old mode 100644 new mode 100755 index 840d575fdf..9725cd689c --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -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) diff --git a/system/loggerd/bootlog.cc b/system/loggerd/bootlog.cc index 4deb5b41f3..becd293c02 100644 --- a/system/loggerd/bootlog.cc +++ b/system/loggerd/bootlog.cc @@ -2,6 +2,7 @@ #include #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 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; } diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py index 54098d13d0..f39b8bb9d6 100755 --- a/system/loggerd/tests/test_loggerd.py +++ b/system/loggerd/tests/test_loggerd.py @@ -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" diff --git a/system/ubloxd/generated/ubx.cpp b/system/ubloxd/generated/ubx.cpp index 34fe1e52ca..81b82ccafc 100644 --- a/system/ubloxd/generated/ubx.cpp +++ b/system/ubloxd/generated/ubx.cpp @@ -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(); - m__io__raw_measurements = new std::vector(); - m_measurements = new std::vector(); - 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(); + m__io__raw_meas = new std::vector(); + m_meas = new std::vector(); + 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::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + if (m__io__raw_meas) { + for (std::vector::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::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + if (m_meas) { + for (std::vector::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(); + m__io__raw_svs = new std::vector(); + m_svs = new std::vector(); + 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::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::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(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; diff --git a/system/ubloxd/generated/ubx.h b/system/ubloxd/generated/ubx.h index 6be4ce8c4b..022108489f 100644 --- a/system/ubloxd/generated/ubx.h +++ b/system/ubloxd/generated/ubx.h @@ -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* m_measurements; + std::vector* m_meas; ubx_t* m__root; ubx_t* m__parent; - std::vector* m__raw_measurements; - std::vector* m__io__raw_measurements; + std::vector* m__raw_meas; + std::vector* 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* measurements() const { return m_measurements; } + std::vector* meas() const { return m_meas; } ubx_t* _root() const { return m__root; } ubx_t* _parent() const { return m__parent; } - std::vector* _raw_measurements() const { return m__raw_measurements; } - std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + std::vector* _raw_meas() const { return m__raw_meas; } + std::vector* _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* m_svs; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_svs; + std::vector* 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* svs() const { return m_svs; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_svs() const { return m__raw_svs; } + std::vector* _io__raw_svs() const { return m__io__raw_svs; } + }; + class nav_pvt_t : public kaitai::kstruct { public: diff --git a/system/ubloxd/tests/ublox.py b/system/ubloxd/tests/ublox.py index 80abc64e2c..0d271ad785 100644 --- a/system/ubloxd/tests/ublox.py +++ b/system/ubloxd/tests/ublox.py @@ -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 diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc index 83e64b7ddc..a01187d560 100644 --- a/system/ubloxd/ublox_msg.cc +++ b/system/ubloxd/ublox_msg.cc @@ -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 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 UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_1_t* subframe_1 = static_cast(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 UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_2_t* subframe_2 = static_cast(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 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 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 UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ glonass_t::string_1_t* data = static_cast(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 UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ glonass_t::string_4_t* data = static_cast(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 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(); - } - - 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 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 UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { return capnp::messageToFlatArray(msg_builder); } +kj::Array 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 UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) { MessageBuilder msg_builder; auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h index 06877cc50d..a52a7db3e5 100644 --- a/system/ubloxd/ublox_msg.h +++ b/system/ubloxd/ublox_msg.h @@ -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 gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); + kj::Array 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 parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); diff --git a/system/ubloxd/ubx.ksy b/system/ubloxd/ubx.ksy index 6945680d32..02c757fe71 100644 --- a/system/ubloxd/ubx.ksy +++ b/system/ubloxd/ubx.ksy @@ -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: diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index b81f7d5e79..0a4f6fc999 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -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); } diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index c0432d0c09..c98ca3bc69 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -16,6 +16,8 @@ #include 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 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(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(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 *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::const_iterator first, second; - QVector vals; - QVector step_vals; - }; - // split into one minitue chunks - QVector 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(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(); + 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())); } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index adce1e0f4d..c7470a6f12 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -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 *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: diff --git a/tools/cabana/dbc.cc b/tools/cabana/dbc.cc index 8ffb660286..38002af06c 100644 --- a/tools/cabana/dbc.cc +++ b/tools/cabana/dbc.cc @@ -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 cabana::Msg::getSignals() const { return ret; } +void cabana::Signal::updatePrecision() { + precision = std::max(num_decimals(factor), num_decimals(offset)); +} + // helper functions static QVector BIG_ENDIAN_START_BITS = []() { @@ -22,7 +27,7 @@ static QVector 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; diff --git a/tools/cabana/dbc.h b/tools/cabana/dbc.h index 3c58e38055..701908112f 100644 --- a/tools/cabana/dbc.h +++ b/tools/cabana/dbc.h @@ -38,6 +38,10 @@ struct MessageId { uint qHash(const MessageId &item); Q_DECLARE_METATYPE(MessageId); +template <> +struct std::hash { + std::size_t operator()(const MessageId &k) const noexcept { return qHash(k); } +}; typedef QList> 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); diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index b3a90e0b70..49aac5c18b 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -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(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 &s) { + sources = s; +} + DBCManager *dbc() { static DBCManager dbc_manager(nullptr); return &dbc_manager; } - diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index 4be7593808..dd3d9811c7 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #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 &messages() const { return msgs; } + inline std::map getMessages(uint8_t source) { + std::map 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 &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 msgs; QString name_; + QSet sources; + + inline const cabana::Msg *msg(uint32_t address) const { + auto it = msgs.find(address); + return it != msgs.end() ? &it->second : nullptr; + } }; DBCManager *dbc(); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 05a0528d0e..ab723fa0e2 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -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); diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 0bfd8a74af..cc5bffd534 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -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; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 57f65b97d8..4ebe8e0473 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -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 std::deque HistoryLogModel::fetchData(InputIt first, InputIt last, uint64_t min_time) { std::deque msgs; QVector 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::fetchData<>(std::vector::iterator first, std::vector::iterator last, uint64_t min_time); -template std::deque HistoryLogModel::fetchData<>(std::vector::reverse_iterator first, std::vector::reverse_iterator last, uint64_t min_time); - std::deque 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()); + 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::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); diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 1559700951..b7cfd9dc2a 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -42,6 +42,7 @@ MainWindow::MainWindow() : QMainWindow() { messages_widget->restoreHeaderState(settings.message_header_state); qRegisterMetaType("uint64_t"); + qRegisterMetaType>("QSet"); qRegisterMetaType("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) { diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index b4a7ed7b34..8dd0bac820 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -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 *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(); diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index ced7cd7d24..4cd06e8a7d 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -62,7 +62,7 @@ void SignalModel::updateState(const QHash *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(); 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"( Signal view
diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index 39fe5f270f..d6bad6d88c 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -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 *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; diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc index 6af831a1d0..d60beed0c8 100644 --- a/tools/cabana/streams/abstractstream.cc +++ b/tools/cabana/streams/abstractstream.cc @@ -1,6 +1,5 @@ #include "tools/cabana/streams/abstractstream.h" - -#include +#include AbstractStream *can = nullptr; @@ -13,7 +12,7 @@ AbstractStream::AbstractStream(QObject *parent, bool is_live_streaming) : is_liv void AbstractStream::process(QHash *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 parseEvents(std::vector::const_reverse_iterator first, - std::vector::const_reverse_iterator last, double route_start_time) { - QHash 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(m.dat.size(), QColor(0, 0, 0, 0)); - m.last_change_t = QVector(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> 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()); + 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(m.dat.size(), m.ts); + m.colors = QVector(m.dat.size(), QColor(0, 0, 0, 0)); + m.bit_change_counts = QVector>(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> &msgs, + std::vector::const_iterator first, std::vector::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::const_iterator first, std::vector::const_iterator last, bool append) { + if (first == last) return; + + if (append) { + parseEvents(events_, first, last); + } else { + std::unordered_map> 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(); } diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h index e582682971..8e19f69d2a 100644 --- a/tools/cabana/streams/abstractstream.h +++ b/tools/cabana/streams/abstractstream.h @@ -1,7 +1,8 @@ #pragma once #include - +#include +#include #include #include @@ -20,6 +21,14 @@ struct CanData { QVector> 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 *events() const = 0; virtual void setSpeed(float speed) {} virtual bool isPaused() const { return false; } virtual void pause(bool pause) {} + virtual const std::vector *rawEvents() const { return nullptr; } + const std::unordered_map> &events() const { return events_; } virtual const std::vector> getTimeline() { return {}; } + void mergeEvents(std::vector::const_iterator first, std::vector::const_iterator last, bool append); signals: void paused(); @@ -52,20 +64,25 @@ signals: void updated(); void msgsReceived(const QHash *); void received(QHash *); + void sourcesUpdated(const QSet &s); public: - QHash can_msgs; + QHash last_msgs; + QSet sources; protected: - void process(QHash *); + virtual void process(QHash *); bool updateEvent(const Event *event); void updateLastMsgsTo(double sec); + void parseEvents(std::unordered_map> &msgs, std::vector::const_iterator first, std::vector::const_iterator last); bool is_live_streaming = false; std::atomic processing = false; QHash counters; std::unique_ptr> new_msgs; QHash change_trackers; + std::unordered_map> events_; + uint64_t last_event_ts = 0; }; // A global pointer referring to the unique AbstractStream object diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc index 8f63d3baec..62ad635152 100644 --- a/tools/cabana/streams/livestream.cc +++ b/tools/cabana/streams/livestream.cc @@ -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 +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 *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 *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) { diff --git a/tools/cabana/streams/livestream.h b/tools/cabana/streams/livestream.h index ba20032b67..598c0b9365 100644 --- a/tools/cabana/streams/livestream.h +++ b/tools/cabana/streams/livestream.h @@ -1,6 +1,5 @@ #pragma once -#include #include "tools/cabana/streams/abstractstream.h" class LiveStream : public AbstractStream { @@ -17,25 +16,31 @@ public: void setSpeed(float speed) override { speed_ = std::min(1.0, speed); } bool isPaused() const override { return pause_; } void pause(bool pause) override; - const std::vector *events() const override; protected: virtual void handleEvent(Event *evt); virtual void streamThread(); - virtual void removeExpiredEvents(); + void process(QHash *) 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 events_vector; - std::deque can_events; - std::deque messages; + std::vector received; + std::deque messages; std::atomic start_ts = 0; std::atomic current_ts = 0; std::atomic speed_ = 1; std::atomic pause_ = false; - uint64_t last_update_event_ts = 0; uint64_t last_update_ts = 0; const QString zmq_address; QThread *stream_thread; - QTimer *timer; }; diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc index ebd969be61..140a795d3a 100644 --- a/tools/cabana/streams/replaystream.cc +++ b/tools/cabana/streams/replaystream.cc @@ -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; })) { diff --git a/tools/cabana/streams/replaystream.h b/tools/cabana/streams/replaystream.h index 3505f6abf4..10dc804716 100644 --- a/tools/cabana/streams/replaystream.h +++ b/tools/cabana/streams/replaystream.h @@ -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 *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 *rawEvents() const override { return replay->events(); } inline const std::vector> getTimeline() override { return replay->getTimeline(); } private: + void mergeSegments(); std::unique_ptr replay = nullptr; uint32_t replay_flags = REPLAY_FLAG_NONE; + std::set processed_segments; }; diff --git a/tools/cabana/tests/test_cabana.cc b/tools/cabana/tests/test_cabana.cc index 124eb7428a..8445867acb 100644 --- a/tools/cabana/tests/test_cabana.cc +++ b/tools/cabana/tests/test_cabana.cc @@ -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::vector> 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); diff --git a/tools/cabana/tools/findsimilarbits.cc b/tools/cabana/tools/findsimilarbits.cc index ffb0e54b0e..a44ac76059 100644 --- a/tools/cabana/tools/findsimilarbits.cc +++ b/tools/cabana/tools/findsimilarbits.cc @@ -1,5 +1,6 @@ #include "tools/cabana/tools/findsimilarbits.h" +#include #include #include #include @@ -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 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::calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt) { +QList 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> mismatches; QHash 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::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); } } } diff --git a/tools/cabana/tools/findsimilarbits.h b/tools/cabana/tools/findsimilarbits.h index 53d7806a8f..a40e2cd8df 100644 --- a/tools/cabana/tools/findsimilarbits.h +++ b/tools/cabana/tools/findsimilarbits.h @@ -22,11 +22,12 @@ private: uint32_t address, byte_idx, bit_idx, mismatches, total; float perc; }; - QList calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt); + QList 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; diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc index 656c97489c..b37b7911ed 100644 --- a/tools/cabana/util.cc +++ b/tools/cabana/util.cc @@ -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(); + } + } diff --git a/tools/cabana/util.h b/tools/cabana/util.h index b1acdd8c6e..3cf14982a9 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -91,3 +91,4 @@ QPixmap icon(const QString &id); } QToolButton *toolButton(const QString &icon, const QString &tooltip); +int num_decimals(double num); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 536ae7ba97..e36e62131f 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -8,11 +8,12 @@ #include #include #include -#include #include #include #include +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); +} diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index d10b09fec5..a5ce8c345f 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -8,6 +8,7 @@ #include #include #include +#include #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 thumbnails; QFuture thumnail_future; QSize thumbnail_size = {}; + QTimer timer; }; class VideoWidget : public QWidget { diff --git a/tools/gpstest/rpc_server.py b/tools/gpstest/rpc_server.py index a368566982..178e8b2c3c 100644 --- a/tools/gpstest/rpc_server.py +++ b/tools/gpstest/rpc_server.py @@ -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" diff --git a/tools/gpstest/test_laikad.py b/tools/gpstest/test_laikad.py index 24ffa6c424..689b0f0b9f 100644 --- a/tools/gpstest/test_laikad.py +++ b/tools/gpstest/test_laikad.py @@ -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() diff --git a/tools/plotjuggler/layouts/controls_mismatch_debug.xml b/tools/plotjuggler/layouts/controls_mismatch_debug.xml index adca3eed29..54586d6e7b 100644 --- a/tools/plotjuggler/layouts/controls_mismatch_debug.xml +++ b/tools/plotjuggler/layouts/controls_mismatch_debug.xml @@ -1,37 +1,44 @@ - + - + - - + + - - + + - - + + - + - - + + - + - - + + - - + + + + + + + + + @@ -46,8 +53,8 @@ - - + + diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 6788a97d03..2bb426361b 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -71,6 +71,7 @@ public: inline void setSpeed(float speed) { speed_ = speed; } inline float getSpeed() const { return speed_; } inline const std::vector *events() const { return events_.get(); } + inline const std::map> &segments() const { return segments_; }; inline const std::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock);