diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index f2cc51285d..7cc45d23b9 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -294,7 +294,7 @@ jobs: - name: Run model replay with ONNX timeout-minutes: 2 run: | - ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 coverage run selfdrive/test/process_replay/model_replay.py && \ + ${{ env.RUN_CL }} "ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \ coverage xml" - name: Run unit tests timeout-minutes: 5 diff --git a/.gitignore b/.gitignore index 31cef94222..2b283d3b11 100644 --- a/.gitignore +++ b/.gitignore @@ -52,6 +52,7 @@ selfdrive/sensord/_sensord system/camerad/camerad system/camerad/test/ae_gray_test selfdrive/modeld/_modeld +selfdrive/modeld/_navmodeld selfdrive/modeld/_dmonitoringmodeld /src/ diff --git a/Dockerfile.openpilot_base_cl b/Dockerfile.openpilot_base_cl index 7652b7e4e6..4c8ecfc78d 100644 --- a/Dockerfile.openpilot_base_cl +++ b/Dockerfile.openpilot_base_cl @@ -17,7 +17,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends\ # Intel OpenCL driver ARG INTEL_DRIVER=l_opencl_p_18.1.0.015.tgz -ARG INTEL_DRIVER_URL=http://registrationcenter-download.intel.com/akdlm/irc_nas/vcp/15532 +ARG INTEL_DRIVER_URL=https://registrationcenter-download.intel.com/akdlm/irc_nas/vcp/15532 RUN mkdir -p /tmp/opencl-driver-intel WORKDIR /tmp/opencl-driver-intel RUN echo INTEL_DRIVER is $INTEL_DRIVER && \ diff --git a/Jenkinsfile b/Jenkinsfile index c34d253585..37621dde5c 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -11,6 +11,7 @@ export SOURCE_DIR=${env.SOURCE_DIR} export GIT_BRANCH=${env.GIT_BRANCH} export GIT_COMMIT=${env.GIT_COMMIT} export AZURE_TOKEN='${env.AZURE_TOKEN}' +export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}' source ~/.bash_profile if [ -f /TICI ]; then @@ -47,9 +48,11 @@ pipeline { TEST_DIR = "/data/openpilot" SOURCE_DIR = "/data/openpilot_source/" AZURE_TOKEN = credentials('azure_token') + MAPBOX_TOKEN = credentials('mapbox_token') } options { - timeout(time: 4, unit: 'HOURS') + timeout(time: 3, unit: 'HOURS') + disableConcurrentBuilds(abortPrevious: env.BRANCH_NAME != 'master') } stages { @@ -115,7 +118,6 @@ pipeline { ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"], ["build openpilot", "cd selfdrive/manager && ./build.py"], ["check dirty", "release/check-dirty.sh"], - ["test manager", "python selfdrive/manager/test/test_manager.py"], ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], ]) @@ -141,6 +143,7 @@ pipeline { ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], ["test pigeond", "python selfdrive/sensord/tests/test_pigeond.py"], + ["test manager", "python selfdrive/manager/test/test_manager.py"], ]) } } diff --git a/RELEASES.md b/RELEASES.md index e15286c5f0..7d55887ec4 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Version 0.9.1 (2022-12-XX) +======================== +* Adjust alert volume using ambient noise level +* Removed driver monitoring timer resetting on interaction if face detected and distracted +* Chevrolet Bolt EV 2022-23 support thanks to JasonJShuler! +* Genesis GV60 2023 support thanks to sunnyhaibin! +* Hyundai Tucson 2022-23 support +* Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! + Version 0.9.0 (2022-11-21) ======================== * New driving model diff --git a/SConstruct b/SConstruct index 033e10a1f0..54b008004e 100644 --- a/SConstruct +++ b/SConstruct @@ -300,7 +300,7 @@ if arch == "Darwin": else: qt_install_prefix = subprocess.check_output(['qmake', '-query', 'QT_INSTALL_PREFIX'], encoding='utf8').strip() qt_install_headers = subprocess.check_output(['qmake', '-query', 'QT_INSTALL_HEADERS'], encoding='utf8').strip() - + qt_env['QTDIR'] = qt_install_prefix qt_dirs = [ f"{qt_install_headers}", @@ -409,10 +409,10 @@ if arch != "Darwin": # build submodules SConscript([ - 'cereal/SConscript', 'body/board/SConscript', - 'panda/board/SConscript', + 'cereal/SConscript', 'opendbc/can/SConscript', + 'panda/SConscript', ]) SConscript(['third_party/SConscript']) @@ -442,9 +442,6 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): Export('opendbc') SConscript(['tools/cabana/SConscript']) -if GetOption('test'): - SConscript('panda/tests/safety/SConscript') - external_sconscript = GetOption('external_sconscript') if external_sconscript: SConscript([external_sconscript]) diff --git a/body b/body index 04aeb30ce0..dc780f858c 160000 --- a/body +++ b/body @@ -1 +1 @@ -Subproject commit 04aeb30ce0bb14759989cd374158233877e1e151 +Subproject commit dc780f858c1ef641471d09b72569e199e3e10acb diff --git a/cereal b/cereal index 3bae09cf65..22b1431132 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 3bae09cf6527674d7eda3a9956242aad94a8f3d2 +Subproject commit 22b1431132b038253a24ab3fbbe3af36ef93b95b diff --git a/common/prefix.h b/common/prefix.h new file mode 100644 index 0000000000..f5abe14b2b --- /dev/null +++ b/common/prefix.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "common/params.h" +#include "common/util.h" + +class OpenpilotPrefix { +public: + OpenpilotPrefix(std::string prefix = {}) { + if (prefix.empty()) { + prefix = util::random_string(15); + } + msgq_path = "/dev/shm/" + prefix; + bool ret = util::create_directories(msgq_path, 0777); + assert(ret); + setenv("OPENPILOT_PREFIX", prefix.c_str(), 1); + } + + ~OpenpilotPrefix() { + auto param_path = Params().getParamPath(); + if (util::file_exists(param_path)) { + std::string real_path = util::readlink(param_path); + system(util::string_format("rm %s -rf", real_path.c_str()).c_str()); + unlink(param_path.c_str()); + } + system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str()); + unsetenv("OPENPILOT_PREFIX"); + } + +private: + std::string msgq_path; +}; diff --git a/common/util.cc b/common/util.cc index 010fe8a11a..10dff6a9ea 100644 --- a/common/util.cc +++ b/common/util.cc @@ -10,6 +10,7 @@ #include #include #include +#include #include #ifdef __linux__ @@ -228,6 +229,18 @@ std::string hexdump(const uint8_t* in, const size_t size) { return ss.str(); } +std::string random_string(std::string::size_type length) { + const char* chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"; + std::mt19937 rg{std::random_device{}()}; + std::uniform_int_distribution pick(0, sizeof(chrs) - 2); + std::string s; + s.reserve(length); + while (length--) { + s += chrs[pick(rg)]; + } + return s; +} + std::string dir_name(std::string const &path) { size_t pos = path.find_last_of("/"); if (pos == std::string::npos) return ""; diff --git a/common/util.h b/common/util.h index b46f7bde4a..028074384e 100644 --- a/common/util.h +++ b/common/util.h @@ -75,6 +75,7 @@ int getenv(const char* key, int default_val); float getenv(const char* key, float default_val); std::string hexdump(const uint8_t* in, const size_t size); +std::string random_string(std::string::size_type length); std::string dir_name(std::string const& path); // **** file fhelpers ***** diff --git a/common/version.h b/common/version.h index 74a56a7c1b..7b5764785a 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.9.0" +#define COMMA_VERSION "0.9.1" diff --git a/docs/CARS.md b/docs/CARS.md index f88bcb20e7..e989947bd6 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,21 +4,22 @@ 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. -# 215 Supported Cars +# 220 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| |Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|A3 2014-19|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|A3 Sportback e-tron 2017-18|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|Q2 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|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| |Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 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| +|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| @@ -31,7 +32,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Genesis|GV70 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Genesis|GV60 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| @@ -59,9 +61,9 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Hyundai|Elantra Hybrid 2021-23|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 K| |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J| -|Hyundai|i30 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Hyundai|Ioniq 5 (with HDA II) 2022-23|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| -|Hyundai|Ioniq 5 (without HDA II) 2022-23|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| +|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| @@ -73,7 +75,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O| |Hyundai|Kona Hybrid 2020|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 I| |Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Santa Cruz 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Santa Cruz 2021-22[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| |Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D| |Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| @@ -82,14 +84,16 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Sonata 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson 2022[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| +|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Tucson Diesel 2019|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 L| -|Hyundai|Tucson Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Tucson Hybrid 2022[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Kia|Ceed 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|EV6 (with HDA II) 2022|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| -|Kia|EV6 (without HDA II) 2022|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Kia|EV6 (with HDA II) 2022[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| +|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Kia|Forte 2019-21|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 G| |Kia|K5 2021-22|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|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| @@ -104,8 +108,9 @@ 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|Sportage 2023|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|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| +|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| |Kia|Stinger 2018-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| @@ -127,17 +132,17 @@ A supported vehicle is one that just works when you install a comma three. All s |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| -|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| +|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B| |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| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#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,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|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| |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| -|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| @@ -145,13 +150,13 @@ A supported vehicle is one that just works when you install a comma three. All s |Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B| |Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| -|Škoda|Kamiq 2021[6](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Kamiq 2021[7](#footnotes)|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[10](#footnotes)| +|Škoda|Karoq 2019-21|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| +|Škoda|Kodiaq 2018-19|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| +|Škoda|Octavia 2015, 2018-19|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| +|Škoda|Octavia RS 2016|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| +|Škoda|Scala 2020|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[10](#footnotes)| +|Škoda|Superb 2015-22|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| |Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| @@ -162,10 +167,10 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2018-20|All|Stock|0 mph[5](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2021-22|All|openpilot|0 mph[5](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| @@ -192,48 +197,49 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat 2015-22[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon 2018-22|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| +|Volkswagen|Arteon eHybrid 2020-22|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| +|Volkswagen|Arteon R 2020-22|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| +|Volkswagen|Atlas 2018-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| +|Volkswagen|Atlas Cross Sport 2021-22|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| +|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|CC 2018-22|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| +|Volkswagen|e-Golf 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| +|Volkswagen|Golf 2015-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| +|Volkswagen|Golf Alltrack 2015-19|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| +|Volkswagen|Golf GTD 2015-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| +|Volkswagen|Golf GTE 2015-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| +|Volkswagen|Golf GTI 2015-21|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| +|Volkswagen|Golf R 2015-19|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| +|Volkswagen|Golf SportsVan 2015-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| +|Volkswagen|Jetta 2018-22|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| +|Volkswagen|Jetta GLI 2021-22|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| +|Volkswagen|Passat 2015-22[8](#footnotes)|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| +|Volkswagen|Passat Alltrack 2015-22|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| +|Volkswagen|Passat GTE 2015-22|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| +|Volkswagen|Polo 2020-22|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[10](#footnotes)| +|Volkswagen|Polo GTI 2020-22|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[10](#footnotes)| +|Volkswagen|T-Cross 2021|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[10](#footnotes)| +|Volkswagen|T-Roc 2021|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[10](#footnotes)| +|Volkswagen|Taos 2022|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| +|Volkswagen|Teramont 2018-22|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| +|Volkswagen|Teramont Cross Sport 2021-22|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| +|Volkswagen|Teramont X 2021-22|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| +|Volkswagen|Tiguan 2019-22|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| +|Volkswagen|Touran 2017|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| -1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `master-ci`. Using openpilot longitudinal may disable Automatic Emergency Braking (AEB).
-2When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
+1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
+2By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace stock ACC. NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
3Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
-5openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-6Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
-7Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
-8Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
-9Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
+5Requires a red panda, additional harness box, additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.
+6openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
+7Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
+8Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
+9Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
+10Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
## Community Maintained Cars Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). diff --git a/docs/INTEGRATION.md b/docs/INTEGRATION.md index 97b72e39d3..ba6291c1e3 100644 --- a/docs/INTEGRATION.md +++ b/docs/INTEGRATION.md @@ -8,4 +8,4 @@ Additionally, on specific supported cars (see ACC column in [supported cars](CAR * Stock ACC is replaced by openpilot ACC. * openpilot FCW operates in addition to stock FCW. -openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. +openpilot should preserve all other vehicle's stock features, including, but not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. diff --git a/opendbc b/opendbc index 296f190000..4a7ad636ff 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 296f190000a2e71408e207ba21a2257cc853ec15 +Subproject commit 4a7ad636ff806146a93f7ae541e463a7dfa5696d diff --git a/poetry.lock b/poetry.lock index d79a3f035f..6360f6f3ab 100644 --- a/poetry.lock +++ b/poetry.lock @@ -2439,6 +2439,20 @@ typing-extensions = ">=3.6.2.1" [package.extras] lint = ["clang-format (==13.0.0)", "flake8", "mypy (==0.782)", "types-protobuf (==3.18.4)"] +[[package]] +name = "onnx2torch" +version = "1.5.4" +description = "Nice Onnx to Pytorch converter" +category = "dev" +optional = false +python-versions = "*" + +[package.dependencies] +numpy = ">=1.16.4" +onnx = ">=1.9.0" +torch = ">=1.8.0" +torchvision = ">=0.9.0" + [[package]] name = "onnxoptimizer" version = "0.3.1" @@ -3696,6 +3710,20 @@ category = "dev" optional = false python-versions = "*" +[[package]] +name = "sounddevice" +version = "0.4.5" +description = "Play and Record Sound with Python" +category = "main" +optional = false +python-versions = ">=3.7" + +[package.dependencies] +CFFI = ">=1.0" + +[package.extras] +numpy = ["NumPy"] + [[package]] name = "soupsieve" version = "2.3.2.post1" @@ -4386,7 +4414,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "d2854112975a9d83a9540175b2d430487e40e0292d48a1ba6c591db60a08c136" +content-hash = "82e450801a9a1de9fd98615d7deb3d8f0aa5bd3ccf0cf8b258cd8f6e6e4c5309" [metadata.files] adal = [ @@ -5392,7 +5420,6 @@ gevent = [ {file = "gevent-22.10.1-cp310-cp310-win_amd64.whl", hash = "sha256:d2ea4ce36c09355379bc038be2bd50118f97d2eb6381b7096de4d05aa4c3e241"}, {file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3e73c9f71aa2a6795ecbec9b57282b002375e863e283558feb87b62840c8c1ac"}, {file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:5bc3758f0dc95007c1780d28a9fd2150416a79c50f308f62a674d78a845ea1b9"}, - {file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:03c10ca0beeab0c6be516030471ea630447ddd1f649d3335e5b162097cd4130a"}, {file = "gevent-22.10.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:fe2c0ff095171c49f78f1d4e6dc89fa58253783c7b6dccab9f1d76e2ee391f10"}, {file = "gevent-22.10.1-cp36-cp36m-manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:d18fcc324f39a3b21795022eb47c7752d6e4f4ed89d8cca41f1cc604553265b3"}, {file = "gevent-22.10.1-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:06ea39c70ce166c4a1d4386c7fae96cb8d84ad799527b3378406051104d15443"}, @@ -6388,6 +6415,9 @@ onnx = [ {file = "onnx-1.12.0-cp39-cp39-win_amd64.whl", hash = "sha256:af90427ca04c6b7b8107c2021e1273227a3ef1a7a01f3073039cae7855a59833"}, {file = "onnx-1.12.0.tar.gz", hash = "sha256:13b3e77d27523b9dbf4f30dfc9c959455859d5e34e921c44f712d69b8369eff9"}, ] +onnx2torch = [ + {file = "onnx2torch-1.5.4-py3-none-any.whl", hash = "sha256:fd1a0fe05072bfb9f3d86d9330299b130b41f11bd4ae634db17078974e711725"}, +] onnxoptimizer = [ {file = "onnxoptimizer-0.3.1-cp37-cp37m-macosx_10_15_x86_64.whl", hash = "sha256:e73a5e2e3ca4db9bff54f7131768749c861677b97ee811a136fcf1a52783cf6e"}, {file = "onnxoptimizer-0.3.1-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:c8bf2bfe0dc43f0776867688e1759122dec049ff4f45f7221931b687fe7e139e"}, @@ -7515,6 +7545,13 @@ sortedcontainers = [ {file = "sortedcontainers-2.4.0-py2.py3-none-any.whl", hash = "sha256:a163dcaede0f1c021485e957a39245190e74249897e2ae4b2aa38595db237ee0"}, {file = "sortedcontainers-2.4.0.tar.gz", hash = "sha256:25caa5a06cc30b6b83d11423433f65d1f9d76c4c6a0c90e3379eaa43b9bfdb88"}, ] +sounddevice = [ + {file = "sounddevice-0.4.5-py3-none-any.whl", hash = "sha256:5cea4afd9412e731f50ae09a54d68b10628a604cfd56b42a976c54d424c6c39d"}, + {file = "sounddevice-0.4.5-py3-none-macosx_10_6_x86_64.macosx_10_6_universal2.whl", hash = "sha256:0875173595a8bd5a66b5a03a3d958e7b89c3b956b8befbe4491a24a3ce7784c0"}, + {file = "sounddevice-0.4.5-py3-none-win32.whl", hash = "sha256:442adf53850916374a58f902200aaf9412227378181264e60c966da64be47d41"}, + {file = "sounddevice-0.4.5-py3-none-win_amd64.whl", hash = "sha256:d3216c5d3d678c3301058e9aac7000879e255140c524c9ef98730091b67ea676"}, + {file = "sounddevice-0.4.5.tar.gz", hash = "sha256:2fe0d41299e4f3037dad2acede4eff0666b34a1fa3da5335e47120373964bef5"}, +] soupsieve = [ {file = "soupsieve-2.3.2.post1-py3-none-any.whl", hash = "sha256:3b2503d3c7084a42b1ebd08116e5f81aadfaea95863628c80a3b774a11b7c759"}, {file = "soupsieve-2.3.2.post1.tar.gz", hash = "sha256:fc53893b3da2c33de295667a0e19f078c14bf86544af307354de5fcf12a3f30d"}, diff --git a/pyproject.toml b/pyproject.toml index 7e76b9cdfc..a20868d877 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -12,7 +12,7 @@ documentation = "https://docs.comma.ai" [tool.poetry.dependencies] python = "~3.8" atomicwrites = "^1.4.0" -casadi = { version = "==3.5.5", markers = "platform_system != 'Darwin'" } +casadi = { version = "==3.5.5", platform = "linux" } cffi = "^1.15.1" crcmod = "^1.7" cryptography = "^37.0.4" @@ -29,7 +29,7 @@ libusb1 = "^3.0.0" nose = "^1.3.7" numpy = "^1.23.0" onnx = "^1.12.0" -onnxruntime-gpu = { version = "^1.11.1", markers = "platform_system != 'Darwin'" } +onnxruntime-gpu = { version = "^1.11.1", platform = "linux" } pillow = "^9.2.0" poetry = "==1.2.2" protobuf = "==3.20.1" @@ -49,20 +49,21 @@ sentry-sdk = "^1.6.0" setproctitle = "^1.2.3" six = "^1.16.0" smbus2 = "^0.4.2" +sounddevice = "^0.4.5" +spidev = { version = "^3.6", platform = "linux" } sympy = "^1.10.1" timezonefinder = "^6.0.1" tqdm = "^4.64.0" urllib3 = "^1.26.10" utm = "^0.7.0" websocket_client = "^1.3.3" -spidev = "^3.6" [tool.poetry.group.dev.dependencies] av = "^9.2.0" azure-storage-blob = "~2.1" breathe = "^4.34.0" -carla = "==0.9.13" +carla = { version = "==0.9.13", platform = "linux" } control = "^0.9.2" coverage = "^6.4.1" dictdiffer = "^0.9.0" @@ -80,7 +81,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" } +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" } pandas = "^1.4.3" parameterized = "^0.8.1" paramiko = "^2.11.0" @@ -118,7 +119,6 @@ azure-cli-core = "^2.38.0" azure-common = "^1.1.28" azure-core = "^1.24.2" azure-nspkg = "~3.0" -azure-storage-blob = "~2.1" azure-storage-common = "~2.1" azure-storage-nspkg = "~3.1" blosc = "==1.9.2" @@ -142,14 +142,12 @@ jupyter = "^1.0.0" jupyterlab = "^3.4.4" jupyterlab-vim = "^0.15.1" Markdown = "^3.4.1" -mpld3 = "^0.5.8" msgpack-python = "^0.5.6" networkx = "~2.3" nvidia-ml-py3 = "^7.352.0" +onnx2torch = "^1.5.4" onnxoptimizer = "^0.3.1" -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" } osmium = "^3.3.0" -pandas = "^1.4.3" pillow-avif-plugin = "^1.2.2" pipenv = "==2022.10.12" plotly = "^5.9.0" diff --git a/release/files_common b/release/files_common index a294e1e5b5..f438904118 100644 --- a/release/files_common +++ b/release/files_common @@ -71,6 +71,7 @@ selfdrive/rtshield.py selfdrive/statsd.py system/logmessaged.py +system/micd.py system/swaglog.py system/version.py @@ -145,6 +146,7 @@ selfdrive/debug/vw_mqb_config.py common/SConscript common/version.h +common/prefix.h common/swaglog.h common/swaglog.cc common/statlog.h @@ -216,6 +218,7 @@ system/hardware/tici/amplifier.py system/hardware/tici/updater system/hardware/tici/iwlist.py system/hardware/pc/__init__.py +system/hardware/pc/hardware.h system/hardware/pc/hardware.py selfdrive/locationd/__init__.py @@ -348,20 +351,28 @@ selfdrive/manager/test/test_manager.py selfdrive/modeld/__init__.py selfdrive/modeld/SConscript selfdrive/modeld/modeld.cc +selfdrive/modeld/navmodeld.cc selfdrive/modeld/dmonitoringmodeld.cc selfdrive/modeld/constants.py selfdrive/modeld/modeld +selfdrive/modeld/navmodeld selfdrive/modeld/dmonitoringmodeld selfdrive/modeld/models/commonmodel.cc selfdrive/modeld/models/commonmodel.h + selfdrive/modeld/models/driving.cc selfdrive/modeld/models/driving.h +selfdrive/modeld/models/supercombo.onnx + selfdrive/modeld/models/dmonitoring.cc selfdrive/modeld/models/dmonitoring.h -selfdrive/modeld/models/supercombo.onnx selfdrive/modeld/models/dmonitoring_model_q.dlc +selfdrive/modeld/models/nav.cc +selfdrive/modeld/models/nav.h +selfdrive/modeld/models/navmodel_q.dlc + selfdrive/modeld/transforms/loadyuv.cc selfdrive/modeld/transforms/loadyuv.h selfdrive/modeld/transforms/loadyuv.cl @@ -484,6 +495,7 @@ cereal/visionipc/*.pxd panda/.gitignore panda/__init__.py +panda/SConscript panda/board/** panda/certs/** panda/crypto/** diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 5b351ca0f5..d1cc4cea83 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +from __future__ import annotations + import base64 import bz2 import hashlib @@ -14,14 +16,15 @@ import sys import tempfile import threading import time -from collections import namedtuple +from dataclasses import asdict, dataclass, replace from datetime import datetime from functools import partial -from typing import Any, Dict +from queue import Queue +from typing import BinaryIO, Callable, Dict, List, Optional, Set, Union, cast import requests from jsonrpc import JSONRPCResponseManager, dispatcher -from websocket import (ABNF, WebSocketException, WebSocketTimeoutException, +from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException, create_connection) import cereal.messaging as messaging @@ -54,19 +57,54 @@ WS_FRAME_SIZE = 4096 NetworkType = log.DeviceState.NetworkType +UploadFileDict = Dict[str, Union[str, int, float, bool]] +UploadItemDict = Dict[str, Union[str, bool, int, float, Dict[str, str]]] + +UploadFilesToUrlResponse = Dict[str, Union[int, List[UploadItemDict], List[str]]] + + +@dataclass +class UploadFile: + fn: str + url: str + headers: Dict[str, str] + allow_cellular: bool + + @classmethod + def from_dict(cls, d: Dict) -> UploadFile: + return cls(d.get("fn", ""), d.get("url", ""), d.get("headers", {}), d.get("allow_cellular", False)) + + +@dataclass +class UploadItem: + path: str + url: str + headers: Dict[str, str] + created_at: int + id: Optional[str] + retry_count: int = 0 + current: bool = False + progress: float = 0 + allow_cellular: bool = False + + @classmethod + def from_dict(cls, d: Dict) -> UploadItem: + return cls(d["path"], d["url"], d["headers"], d["created_at"], d["id"], d["retry_count"], d["current"], + d["progress"], d["allow_cellular"]) + + dispatcher["echo"] = lambda s: s -recv_queue: Any = queue.Queue() -send_queue: Any = queue.Queue() -upload_queue: Any = queue.Queue() -low_priority_send_queue: Any = queue.Queue() -log_recv_queue: Any = queue.Queue() -cancelled_uploads: Any = set() -UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress', 'allow_cellular'], defaults=(0, False, 0, False)) +recv_queue: Queue[str] = queue.Queue() +send_queue: Queue[str] = queue.Queue() +upload_queue: Queue[UploadItem] = queue.Queue() +low_priority_send_queue: Queue[str] = queue.Queue() +log_recv_queue: Queue[str] = queue.Queue() +cancelled_uploads: Set[str] = set() -cur_upload_items: Dict[int, Any] = {} +cur_upload_items: Dict[int, Optional[UploadItem]] = {} -def strip_bz2_extension(fn): +def strip_bz2_extension(fn: str) -> str: if fn.endswith('.bz2'): return fn[:-4] return fn @@ -76,29 +114,30 @@ class AbortTransferException(Exception): pass -class UploadQueueCache(): +class UploadQueueCache: params = Params() @staticmethod - def initialize(upload_queue): + def initialize(upload_queue: Queue[UploadItem]) -> None: try: upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue") if upload_queue_json is not None: for item in json.loads(upload_queue_json): - upload_queue.put(UploadItem(**item)) + upload_queue.put(UploadItem.from_dict(item)) except Exception: cloudlog.exception("athena.UploadQueueCache.initialize.exception") @staticmethod - def cache(upload_queue): + def cache(upload_queue: Queue[UploadItem]) -> None: try: - items = [i._asdict() for i in upload_queue.queue if i.id not in cancelled_uploads] + queue: List[Optional[UploadItem]] = list(upload_queue.queue) + items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items)) except Exception: cloudlog.exception("athena.UploadQueueCache.cache.exception") -def handle_long_poll(ws): +def handle_long_poll(ws: WebSocket) -> None: end_event = threading.Event() threads = [ @@ -126,7 +165,7 @@ def handle_long_poll(ws): thread.join() -def jsonrpc_handler(end_event): +def jsonrpc_handler(end_event: threading.Event) -> None: dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event) while not end_event.is_set(): try: @@ -147,11 +186,12 @@ def jsonrpc_handler(end_event): def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None: - if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: - item = cur_upload_items[tid] + item = cur_upload_items[tid] + if item is not None and item.retry_count < MAX_RETRY_COUNT: new_retry_count = item.retry_count + 1 if increase_count else item.retry_count - item = item._replace( + item = replace( + item, retry_count=new_retry_count, progress=0, current=False @@ -175,44 +215,44 @@ def upload_handler(end_event: threading.Event) -> None: cur_upload_items[tid] = None try: - cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True) + cur_upload_items[tid] = item = replace(upload_queue.get(timeout=1), current=True) - if cur_upload_items[tid].id in cancelled_uploads: - cancelled_uploads.remove(cur_upload_items[tid].id) + if item.id in cancelled_uploads: + cancelled_uploads.remove(item.id) continue # Remove item if too old - age = datetime.now() - datetime.fromtimestamp(cur_upload_items[tid].created_at / 1000) + age = datetime.now() - datetime.fromtimestamp(item.created_at / 1000) if age.total_seconds() > MAX_AGE: - cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True) + cloudlog.event("athena.upload_handler.expired", item=item, error=True) continue # Check if uploading over metered connection is allowed sm.update(0) metered = sm['deviceState'].networkMetered network_type = sm['deviceState'].networkType.raw - if metered and (not cur_upload_items[tid].allow_cellular): + if metered and (not item.allow_cellular): retry_upload(tid, end_event, False) continue try: - def cb(sz, cur): + def cb(sz: int, cur: int) -> None: # Abort transfer if connection changed to metered after starting upload sm.update(0) metered = sm['deviceState'].networkMetered - if metered and (not cur_upload_items[tid].allow_cellular): + if metered and (not item.allow_cellular): raise AbortTransferException - cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) + cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1) - fn = cur_upload_items[tid].path + fn = item.path try: sz = os.path.getsize(fn) except OSError: sz = -1 - cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=cur_upload_items[tid].retry_count) - response = _do_upload(cur_upload_items[tid], cb) + cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count) + response = _do_upload(item, cb) if response.status_code not in (200, 201, 401, 403, 412): cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered) @@ -234,7 +274,7 @@ def upload_handler(end_event: threading.Event) -> None: cloudlog.exception("athena.upload_handler.exception") -def _do_upload(upload_item, callback=None): +def _do_upload(upload_item: UploadItem, callback: Optional[Callable] = None) -> requests.Response: path = upload_item.path compress = False @@ -244,27 +284,25 @@ def _do_upload(upload_item, callback=None): compress = True with open(path, "rb") as f: + data: BinaryIO if compress: cloudlog.event("athena.upload_handler.compress", fn=path, fn_orig=upload_item.path) - data = bz2.compress(f.read()) - size = len(data) - data = io.BytesIO(data) + compressed = bz2.compress(f.read()) + size = len(compressed) + data = io.BytesIO(compressed) else: size = os.fstat(f.fileno()).st_size data = f - if callback: - data = CallbackReader(data, callback, size) - return requests.put(upload_item.url, - data=data, + data=CallbackReader(data, callback, size) if callback else data, headers={**upload_item.headers, 'Content-Length': str(size)}, timeout=30) # security: user should be able to request any message from their car @dispatcher.add_method -def getMessage(service=None, timeout=1000): +def getMessage(service: str, timeout: int = 1000) -> Dict: if service is None or service not in service_list: raise Exception("invalid service") @@ -274,7 +312,8 @@ def getMessage(service=None, timeout=1000): if ret is None: raise TimeoutError - return ret.to_dict() + # this is because capnp._DynamicStructReader doesn't have typing information + return cast(Dict, ret.to_dict()) @dispatcher.add_method @@ -288,7 +327,7 @@ def getVersion() -> Dict[str, str]: @dispatcher.add_method -def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=None): +def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optional[str] = None, place_details: Optional[str] = None) -> Dict[str, int]: destination = { "latitude": latitude, "longitude": longitude, @@ -300,8 +339,8 @@ def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=No return {"success": 1} -def scan_dir(path, prefix): - files = list() +def scan_dir(path: str, prefix: str) -> List[str]: + files = [] # only walk directories that match the prefix # (glob and friends traverse entire dir tree) with os.scandir(path) as i: @@ -320,18 +359,18 @@ def scan_dir(path, prefix): return files @dispatcher.add_method -def listDataDirectory(prefix=''): +def listDataDirectory(prefix='') -> List[str]: return scan_dir(ROOT, prefix) @dispatcher.add_method -def reboot(): +def reboot() -> Dict[str, int]: sock = messaging.sub_sock("deviceState", timeout=1000) ret = messaging.recv_one(sock) if ret is None or ret.deviceState.started: raise Exception("Reboot unavailable") - def do_reboot(): + def do_reboot() -> None: time.sleep(2) HARDWARE.reboot() @@ -341,50 +380,53 @@ def reboot(): @dispatcher.add_method -def uploadFileToUrl(fn, url, headers): - return uploadFilesToUrls([{ +def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesToUrlResponse: + # this is because mypy doesn't understand that the decorator doesn't change the return type + response: UploadFilesToUrlResponse = uploadFilesToUrls([{ "fn": fn, "url": url, "headers": headers, }]) + return response @dispatcher.add_method -def uploadFilesToUrls(files_data): - items = [] - failed = [] - for file in files_data: - fn = file.get('fn', '') - if len(fn) == 0 or fn[0] == '/' or '..' in fn or 'url' not in file: - failed.append(fn) +def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlResponse: + files = map(UploadFile.from_dict, files_data) + + items: List[UploadItemDict] = [] + failed: List[str] = [] + for file in files: + if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0: + failed.append(file.fn) continue - path = os.path.join(ROOT, fn) + path = os.path.join(ROOT, file.fn) if not os.path.exists(path) and not os.path.exists(strip_bz2_extension(path)): - failed.append(fn) + failed.append(file.fn) continue # Skip item if already in queue - url = file['url'].split('?')[0] + url = file.url.split('?')[0] if any(url == item['url'].split('?')[0] for item in listUploadQueue()): continue item = UploadItem( path=path, - url=file['url'], - headers=file.get('headers', {}), + url=file.url, + headers=file.headers, created_at=int(time.time() * 1000), id=None, - allow_cellular=file.get('allow_cellular', False), + allow_cellular=file.allow_cellular, ) upload_id = hashlib.sha1(str(item).encode()).hexdigest() - item = item._replace(id=upload_id) + item = replace(item, id=upload_id) upload_queue.put_nowait(item) - items.append(item._asdict()) + items.append(asdict(item)) UploadQueueCache.cache(upload_queue) - resp = {"enqueued": len(items), "items": items} + resp: UploadFilesToUrlResponse = {"enqueued": len(items), "items": items} if failed: resp["failed"] = failed @@ -392,32 +434,32 @@ def uploadFilesToUrls(files_data): @dispatcher.add_method -def listUploadQueue(): +def listUploadQueue() -> List[UploadItemDict]: items = list(upload_queue.queue) + list(cur_upload_items.values()) - return [i._asdict() for i in items if (i is not None) and (i.id not in cancelled_uploads)] + return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)] @dispatcher.add_method -def cancelUpload(upload_id): +def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]]: if not isinstance(upload_id, list): upload_id = [upload_id] uploading_ids = {item.id for item in list(upload_queue.queue)} cancelled_ids = uploading_ids.intersection(upload_id) if len(cancelled_ids) == 0: - return 404 + return {"success": 0, "error": "not found"} cancelled_uploads.update(cancelled_ids) return {"success": 1} @dispatcher.add_method -def primeActivated(activated): +def primeActivated(activated: bool) -> Dict[str, int]: return {"success": 1} @dispatcher.add_method -def setBandwithLimit(upload_speed_kbps, download_speed_kbps): +def setBandwithLimit(upload_speed_kbps: int, download_speed_kbps: int) -> Dict[str, Union[int, str]]: if not AGNOS: return {"success": 0, "error": "only supported on AGNOS"} @@ -428,7 +470,7 @@ def setBandwithLimit(upload_speed_kbps, download_speed_kbps): return {"success": 0, "error": "failed to set limit", "stdout": e.stdout, "stderr": e.stderr} -def startLocalProxy(global_end_event, remote_ws_uri, local_port): +def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> Dict[str, int]: try: if local_port not in LOCAL_PORT_WHITELIST: raise Exception("Requested local port not whitelisted") @@ -462,7 +504,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port): @dispatcher.add_method -def getPublicKey(): +def getPublicKey() -> Optional[str]: if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'): return None @@ -471,7 +513,7 @@ def getPublicKey(): @dispatcher.add_method -def getSshAuthorizedKeys(): +def getSshAuthorizedKeys() -> str: return Params().get("GithubSshKeys", encoding='utf8') or '' @@ -486,7 +528,7 @@ def getNetworkType(): @dispatcher.add_method -def getNetworkMetered(): +def getNetworkMetered() -> bool: network_type = HARDWARE.get_network_type() return HARDWARE.get_network_metered(network_type) @@ -497,7 +539,7 @@ def getNetworks(): @dispatcher.add_method -def takeSnapshot(): +def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: from system.camerad.snapshot.snapshot import jpeg_write, snapshot ret = snapshot() if ret is not None: @@ -514,16 +556,19 @@ def takeSnapshot(): raise Exception("not available while camerad is started") -def get_logs_to_send_sorted(): +def get_logs_to_send_sorted() -> List[str]: # TODO: scan once then use inotify to detect file creation/deletion curr_time = int(time.time()) logs = [] for log_entry in os.listdir(SWAGLOG_DIR): log_path = os.path.join(SWAGLOG_DIR, log_entry) + time_sent = 0 try: - time_sent = int.from_bytes(getxattr(log_path, LOG_ATTR_NAME), sys.byteorder) + value = getxattr(log_path, LOG_ATTR_NAME) + if value is not None: + time_sent = int.from_bytes(value, sys.byteorder) except (ValueError, TypeError): - time_sent = 0 + pass # assume send failed and we lost the response if sent more than one hour ago if not time_sent or curr_time - time_sent > 3600: logs.append(log_entry) @@ -531,7 +576,7 @@ def get_logs_to_send_sorted(): return sorted(logs)[:-1] -def log_handler(end_event): +def log_handler(end_event: threading.Event) -> None: if PC: return @@ -593,7 +638,7 @@ def log_handler(end_event): cloudlog.exception("athena.log_handler.exception") -def stat_handler(end_event): +def stat_handler(end_event: threading.Event) -> None: while not end_event.is_set(): last_scan = 0 curr_scan = sec_since_boot() @@ -619,7 +664,7 @@ def stat_handler(end_event): time.sleep(0.1) -def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): +def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket, end_event: threading.Event, global_end_event: threading.Event) -> None: while not (end_event.is_set() or global_end_event.is_set()): try: data = ws.recv() @@ -638,7 +683,7 @@ def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): end_event.set() -def ws_proxy_send(ws, local_sock, signal_sock, end_event): +def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket.socket, end_event: threading.Event) -> None: while not end_event.is_set(): try: r, _, _ = select.select((local_sock, signal_sock), (), ()) @@ -663,7 +708,7 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event): cloudlog.debug("athena.ws_proxy_send done closing sockets") -def ws_recv(ws, end_event): +def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: last_ping = int(sec_since_boot() * 1e9) while not end_event.is_set(): try: @@ -685,7 +730,7 @@ def ws_recv(ws, end_event): end_event.set() -def ws_send(ws, end_event): +def ws_send(ws: WebSocket, end_event: threading.Event) -> None: while not end_event.is_set(): try: try: @@ -704,7 +749,7 @@ def ws_send(ws, end_event): end_event.set() -def backoff(retries): +def backoff(retries: int) -> int: return random.randrange(0, min(128, int(2 ** retries))) diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 5e86a2e821..128fde0319 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -8,6 +8,7 @@ import time import threading import queue import unittest +from dataclasses import asdict, replace from datetime import datetime, timedelta from typing import Optional @@ -226,7 +227,7 @@ class TestAthenadMethods(unittest.TestCase): """When an upload times out or fails to connect it should be placed back in the queue""" fn = self._create_file('qlog.bz2') item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) - item_no_retry = item._replace(retry_count=MAX_RETRY_COUNT) + item_no_retry = replace(item, retry_count=MAX_RETRY_COUNT) end_event = threading.Event() thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) @@ -296,7 +297,7 @@ class TestAthenadMethods(unittest.TestCase): self.assertEqual(len(items), 0) @with_http_server - def test_listUploadQueueCurrent(self, host): + def test_listUploadQueueCurrent(self, host: str): fn = self._create_file('qlog.bz2') item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) @@ -321,7 +322,7 @@ class TestAthenadMethods(unittest.TestCase): items = dispatcher["listUploadQueue"]() self.assertEqual(len(items), 1) - self.assertDictEqual(items[0], item._asdict()) + self.assertDictEqual(items[0], asdict(item)) self.assertFalse(items[0]['current']) athenad.cancelled_uploads.add(item.id) @@ -346,7 +347,7 @@ class TestAthenadMethods(unittest.TestCase): athenad.UploadQueueCache.initialize(athenad.upload_queue) self.assertEqual(athenad.upload_queue.qsize(), 1) - self.assertDictEqual(athenad.upload_queue.queue[-1]._asdict(), item1._asdict()) + self.assertDictEqual(asdict(athenad.upload_queue.queue[-1]), asdict(item1)) @mock.patch('selfdrive.athena.athenad.create_connection') def test_startLocalProxy(self, mock_create_connection): @@ -417,5 +418,6 @@ class TestAthenadMethods(unittest.TestCase): sl = athenad.get_logs_to_send_sorted() self.assertListEqual(sl, fl[:-1]) + if __name__ == '__main__': unittest.main() diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 5496902252..1f1249194d 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -528,9 +528,6 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? - // Other pandas don't have fan/IR to control - if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue; - if (sm.updated("deviceState") && !no_fan_control) { // Fan speed uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index deccee3e76..3e447c830e 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -6,7 +6,6 @@ #include #include "cereal/messaging/messaging.h" -#include "panda/board/dlc_to_len.h" #include "common/swaglog.h" #include "common/util.h" @@ -24,7 +23,8 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { (hw_type != cereal::PandaState::PandaType::GREY_PANDA)); has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || - (hw_type == cereal::PandaState::PandaType::DOS); + (hw_type == cereal::PandaState::PandaType::DOS) || + (hw_type == cereal::PandaState::PandaType::TRES); return; } @@ -169,22 +169,15 @@ static uint8_t len_to_dlc(uint8_t len) { } } -static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) { - for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) { - // Insert counter every 64 bytes (first byte of 64 bytes USB packet) - if (pos % USBPACKET_MAX_SIZE == 0) { - dest[pos] = pos / USBPACKET_MAX_SIZE; - pos++; - } - dest[pos] = src[i]; - } -} - void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func) { int32_t pos = 0; uint8_t send_buf[2 * USB_TX_SOFT_LIMIT]; + uint32_t magic = CAN_TRANSACTION_MAGIC; + memcpy(&send_buf[0], &magic, sizeof(uint32_t)); + pos += sizeof(uint32_t); + for (auto cmsg : can_data_list) { // check if the message is intended for this panda uint8_t bus = cmsg.getSrc(); @@ -202,16 +195,19 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data header.data_len_code = data_len_code; header.bus = bus - bus_offset; - write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header)); - write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.size()); + memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header)); + pos += sizeof(can_header); + memcpy(&send_buf[pos], (uint8_t *)can_data.begin(), can_data.size()); + pos += can_data.size(); + if (pos >= USB_TX_SOFT_LIMIT) { write_func(send_buf, pos); - pos = 0; + pos = sizeof(uint32_t); } } // send remaining packets - if (pos > 0) write_func(send_buf, pos); + if (pos > sizeof(uint32_t)) write_func(send_buf, pos); } void Panda::can_send(capnp::List::Reader can_data_list) { @@ -234,33 +230,38 @@ bool Panda::can_receive(std::vector& out_vec) { } bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) { - recv_buf.clear(); - for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { - if (data[i] != i / USBPACKET_MAX_SIZE) { - LOGE("CAN: MALFORMED USB RECV PACKET"); - handle->comms_healthy = false; - return false; - } - int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); - recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]); + if (size < sizeof(uint32_t)) { + return true; + } + + uint32_t magic; + memcpy(&magic, &data[0], sizeof(uint32_t)); + if (magic != CAN_TRANSACTION_MAGIC) { + LOGE("CAN recv: buffer didn't start with magic"); + handle->comms_healthy = false; + return false; } - int pos = 0; - while (pos < recv_buf.size()) { + int pos = sizeof(uint32_t); + while (pos < size) { can_header header; - memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE); + memcpy(&header, &data[pos], sizeof(can_header)); can_frame &canData = out_vec.emplace_back(); canData.busTime = 0; canData.address = header.addr; canData.src = header.bus + bus_offset; - if (header.rejected) { canData.src += CANPACKET_REJECTED; } - if (header.returned) { canData.src += CANPACKET_RETURNED; } + if (header.rejected) { + canData.src += CAN_REJECTED_BUS_OFFSET; + } + if (header.returned) { + canData.src += CAN_RETURNED_BUS_OFFSET; + } const uint8_t data_len = dlc_to_len[header.data_len_code]; - canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len); + canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); - pos += CANPACKET_HEAD_SIZE + data_len; + pos += sizeof(can_header) + data_len; } return true; } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 5b3cbb9a3e..75fec57a3e 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -11,20 +11,16 @@ #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" #include "panda/board/health.h" +#include "panda/board/can_definitions.h" #include "selfdrive/boardd/panda_comms.h" - -#define PANDA_CAN_CNT 3 -#define PANDA_BUS_CNT 4 - #define USB_TX_SOFT_LIMIT (0x100U) #define USBPACKET_MAX_SIZE (0x40) #define RECV_SIZE (0x4000U) -#define CANPACKET_HEAD_SIZE 5U -#define CANPACKET_MAX_SIZE 72U -#define CANPACKET_REJECTED (0xC0U) -#define CANPACKET_RETURNED (0x80U) + +#define CAN_REJECTED_BUS_OFFSET 0xC0U +#define CAN_RETURNED_BUS_OFFSET 0x80U struct __attribute__((packed)) can_header { uint8_t reserved : 1; @@ -47,7 +43,6 @@ struct can_frame { class Panda { private: std::unique_ptr handle; - std::vector recv_buf; public: Panda(std::string serial="", uint32_t bus_offset=0); diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index f42eadc5b2..bd262dfa0e 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -13,8 +13,6 @@ #define TIMEOUT 0 #define SPI_BUF_SIZE 1024 -const bool PANDA_NO_RETRY = getenv("PANDA_NO_RETRY"); - // comms base class class PandaCommsHandle { @@ -52,7 +50,6 @@ public: private: libusb_context *ctx = NULL; libusb_device_handle *dev_handle = NULL; - std::vector recv_buf; void handle_usb_issue(int err, const char func[]); }; diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 2803f58db0..717b6ce820 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -6,6 +6,7 @@ #include #include "common/util.h" +#include "common/timing.h" #include "common/swaglog.h" #include "panda/board/comms_definitions.h" #include "selfdrive/boardd/panda_comms.h" @@ -24,6 +25,9 @@ struct __attribute__((packed)) spi_header { uint16_t max_rx_len; }; +const int SPI_MAX_RETRIES = 5; +const int SPI_ACK_TIMEOUT = 50; // milliseconds + PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { LOGD("opening SPI panda: %s", serial.c_str()); @@ -109,7 +113,7 @@ int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int l int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) { std::lock_guard lk(hw_lock); - const int xfer_size = 0x40; + const int xfer_size = 0x40 * 15; int ret = 0; uint16_t length = (tx_data != NULL) ? tx_len : rx_len; @@ -119,7 +123,8 @@ int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t t int len = std::min(xfer_size, tx_len - (xfer_size * i)); d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0); } else { - d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), xfer_size); + uint16_t to_read = std::min(xfer_size, rx_len - ret); + d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read); } if (d < 0) { @@ -137,15 +142,11 @@ int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t t return ret; } - - std::vector PandaSpiHandle::list() { // TODO: list all pandas available over SPI return {}; } - - void add_checksum(uint8_t *data, int data_len) { data[data_len] = SPI_CHECKSUM_START; for (int i=0; i < data_len; i++) { @@ -165,17 +166,19 @@ bool check_checksum(uint8_t *data, int data_len) { int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) { int ret; + int count = SPI_MAX_RETRIES; std::lock_guard lk(hw_lock); do { // TODO: handle error ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); - } while (ret < 0 && connected && !PANDA_NO_RETRY); + count--; + } while (ret < 0 && connected && count > 0); return ret; } int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { - // TODO: add timeout? + double start_millis = millis_since_boot(); while (true) { int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); if (ret < 0) { @@ -189,6 +192,12 @@ int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { LOGW("SPI: got NACK"); return -1; } + + // handle timeout + if (millis_since_boot() - start_millis > SPI_ACK_TIMEOUT) { + LOGE("SPI: timed out waiting for ACK"); + return -1; + } } return 0; diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index 6a13cbd71f..cc3b4bce9a 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -6,8 +6,6 @@ #include "cereal/messaging/messaging.h" #include "selfdrive/boardd/panda.h" -const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U}; - int random_int(int min, int max) { std::random_device dev; std::mt19937 rng(dev()); @@ -50,7 +48,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState can.setAddress(i); can.setSrc(random_int(0, 3) + bus_offset); can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); - total_pakets_size += CANPACKET_HEAD_SIZE + dat.size(); + total_pakets_size += sizeof(can_header) + dat.size(); } can_data_list = can_list.asReader(); @@ -60,14 +58,11 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState void PandaTest::test_can_send() { std::vector unpacked_data; this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { - int size_left = size; - for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) { - REQUIRE(chunk[i] == counter); - - const int len = std::min(USBPACKET_MAX_SIZE, size_left); - unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]); - size_left -= len; - } + uint32_t magic; + memcpy(&magic, &chunk[0], sizeof(uint32_t)); + + REQUIRE(magic == CAN_TRANSACTION_MAGIC); + unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]); }); REQUIRE(unpacked_data.size() == total_pakets_size); @@ -75,9 +70,9 @@ void PandaTest::test_can_send() { INFO("test can message integrity"); for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) { can_header header; - memcpy(&header, &unpacked_data[pos], CANPACKET_HEAD_SIZE); + memcpy(&header, &unpacked_data[pos], sizeof(can_header)); const uint8_t data_len = dlc_to_len[header.data_len_code]; - pckt_len = CANPACKET_HEAD_SIZE + data_len; + pckt_len = sizeof(can_header) + data_len; REQUIRE(header.addr == cnt); REQUIRE(test_data.find(data_len) != test_data.end()); diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 491c551b1b..c056f7ca3d 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,8 +1,9 @@ # functions common among cars import capnp +from collections import namedtuple from cereal import car -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from typing import Dict # kg of standard extra cargo to count for drive, gas, etc... @@ -10,6 +11,7 @@ STD_CARGO_KG = 136. ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v']) def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: @@ -111,6 +113,15 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq return int(round(float(apply_torque))) +def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS): + # pick angle rate limits based on wind up/down + steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN + + angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v) + return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) + + def crc8_pedal(data): crc = 0xFF # standard init value poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 0d5d780bd3..00673a7e28 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -85,6 +85,7 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.accel = torque_l new_actuators.steer = torque_r + new_actuators.steerOutputCan = torque_r self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index ae7ab89aab..bc5b36e2ee 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -2,16 +2,13 @@ import math from cereal import car from common.realtime import DT_CTRL -from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car import get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=None, car_fw=None, experimental_long=False): - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] @@ -31,10 +28,6 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = True ret.steerControlType = car.CarParams.SteerControlType.angle - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret def _update(self, c): diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 66f1b947a8..548039bc70 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -26,6 +26,12 @@ CAR_INFO: Dict[str, CarInfo] = { CAR.BODY: CarInfo("comma body", package="All"), } +FINGERPRINTS = { + CAR.BODY: [{ + 513: 8, 516: 8, 514: 3, 515: 4, + }], +} + FW_QUERY_CONFIG = FwQueryConfig( requests=[ Request( @@ -40,10 +46,13 @@ FW_VERSIONS = { CAR.BODY: { (Ecu.engine, 0x720, None): [ b'0.0.01', - b'02/27/2022' + b'02/27/2022', + b'0.3.00a', ], + # git hash of the firmware used (Ecu.debug, 0x721, None): [ - b'166bd860' # git hash of the firmware used + b'166bd860', + b'dc780f85', ], }, } diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 5a2d90c64c..31cd01b654 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -2,7 +2,7 @@ from opendbc.can.packer import CANPacker from common.realtime import DT_CTRL from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons -from selfdrive.car.chrysler.values import CAR, RAM_CARS, CarControllerParams +from selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags class CarController: @@ -51,7 +51,7 @@ class CarController: lkas_control_bit = self.lkas_control_bit_prev if CS.out.vEgo > self.CP.minSteerSpeed: lkas_control_bit = True - elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED: if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0): lkas_control_bit = False elif self.CP.carFingerprint in RAM_CARS: @@ -78,5 +78,6 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last return new_actuators, can_sends diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 245e10650c..2f058165ac 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,21 +1,18 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config -from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT +from selfdrive.car import STD_CARGO_KG, get_safety_config +from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "chrysler" - ret.dashcamOnly = candidate in RAM_HD ret.radarOffCan = DBC[candidate]['radar'] is None - ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 @@ -27,9 +24,12 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT ret.minSteerSpeed = 3.8 # m/s - if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): - # TODO: allow 2019 cars to steer down to 13 m/s if already engaged. - ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + if candidate not in RAM_CARS: + # Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed. + new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019) + new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw) + if new_eps_platform or new_eps_firmware: + ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value # Chrysler if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020): @@ -58,10 +58,9 @@ class CarInterface(CarInterfaceBase): ret.mass = 2493. + STD_CARGO_KG CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.minSteerSpeed = 14.5 - if car_fw is not None: - for fw in car_fw: - if fw.ecu == 'eps' and fw.fwVersion[:8] in (b"68312176", b"68273275"): - ret.minSteerSpeed = 0. + for fw in car_fw: + if fw.ecu == 'eps' and fw.fwVersion.startswith((b"68312176", b"68273275")): + ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: ret.steerActuatorDelay = 0.2 @@ -74,15 +73,11 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"Unsupported car: {candidate}") - ret.centerToFront = ret.wheelbase * 0.44 - - # starting with reasonable value for civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED: + # TODO: allow these cars to steer down to 13 m/s if already engaged. + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + ret.centerToFront = ret.wheelbase * 0.44 ret.enableBsm = 720 in fingerprint[0] return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 16530ed989..02261a0b63 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,3 +1,4 @@ +from enum import IntFlag from dataclasses import dataclass from enum import Enum from typing import Dict, List, Optional, Union @@ -11,6 +12,10 @@ from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu +class ChryslerFlags(IntFlag): + HIGHER_MIN_STEERING_SPEED = 1 + + class CAR: # Chrysler PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py index 58afed27eb..03313e2ff6 100755 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -40,6 +40,7 @@ def get_all_car_info() -> List[CarInfo]: for _car_info in car_info: if not hasattr(_car_info, "row"): + _car_info.init_make(CP) _car_info.init(CP, footnotes) all_car_info.append(_car_info) diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 8e057a1563..03e9e721b5 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -68,17 +68,16 @@ class Harness(Enum): none = "None" -CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only"], defaults=(None, False)) +CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False)) class CommonFootnote(Enum): EXP_LONG_AVAIL = CarFootnote( - "Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `master-ci`. " + - "Using openpilot longitudinal may disable Automatic Emergency Braking (AEB).", + "Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. ", Column.LONGITUDINAL, docs_only=True) EXP_LONG_DSU = CarFootnote( - "When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace " + - "stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).", + "By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace " + + "stock ACC. NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).", Column.LONGITUDINAL) @@ -173,6 +172,9 @@ class CarInfo: return self + def init_make(self, CP: car.CarParams): + """CarInfo subclasses can add make-specific logic for harness selection, footnotes, etc.""" + def get_detail_sentence(self, CP): if not CP.notCar: sentence_builder = "openpilot upgrades your {car_model} with automated lane centering{alc} and adaptive cruise control{acc}." diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 4943db076f..f3d77bc05a 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter from selfdrive.car.interfaces import CarInterfaceBase @@ -10,12 +10,7 @@ CarParams = car.CarParams class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - if car_fw is None: - car_fw = [] - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "ford" ret.dashcamOnly = True ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] @@ -24,7 +19,6 @@ class CarInterface(CarInterfaceBase): ret.steerControlType = CarParams.SteerControlType.angle ret.steerActuatorDelay = 0.4 ret.steerLimitTimer = 1.0 - tire_stiffness_factor = 1.0 if candidate == CAR.ESCAPE_MK4: ret.wheelbase = 2.71 @@ -60,10 +54,7 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 0. ret.autoResumeSng = ret.minEnableSpeed == -1. - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.centerToFront = ret.wheelbase * 0.44 - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, - tire_stiffness_factor=tire_stiffness_factor) return ret def _update(self, c): diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a6cd2f19b9..d380abf8e3 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -158,6 +158,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index df1b4b2866..de0fd2eed6 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -100,6 +100,9 @@ class CarState(CarStateBase): if self.CP.networkLocation == NetworkLocation.fwdCamera: ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0 + # openpilot controls nonAdaptive when not pcmCruise + if self.CP.pcmCruise: + ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3) return ret @@ -112,6 +115,7 @@ class CarState(CarStateBase): ("AEBCmdActive", "AEBCmd"), ("RollingCounter", "ASCMLKASteeringCmd"), ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"), + ("ACCCruiseState", "ASCMActiveCruiseControlStatus"), ] checks += [ ("AEBCmd", 10), diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 56c981326f..63189bcd89 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -6,7 +6,16 @@ def create_buttons(packer, bus, idx, button): values = { "ACCButtons": button, "RollingCounter": idx, + "ACCAlwaysOne": 1, + "DistanceButton": 0, } + + checksum = 240 + int(values["ACCAlwaysOne"] * 0xf) + checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0) + checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0 + checksum -= 2 * values["DistanceButton"] + + values["SteeringButtonChecksum"] = checksum return packer.make_can_msg("ASCMSteeringButton", bus, values) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index eb5ab7329a..195df36a7f 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -4,7 +4,7 @@ from math import fabs from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR from selfdrive.car.interfaces import CarInterfaceBase @@ -44,8 +44,7 @@ class CarInterface(CarInterfaceBase): return CarInterfaceBase.get_steer_feedforward_default @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -98,7 +97,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX, CAR.BOLT_EV} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. # Some GMs need some tolerance above 10 kph to avoid a fault @@ -171,7 +170,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.000045 tire_stiffness_factor = 1.0 - elif candidate in (CAR.BOLT_EV, CAR.BOLT_EUV): + elif candidate == CAR.BOLT_EUV: ret.mass = 1669. + STD_CARGO_KG ret.wheelbase = 2.63779 ret.steerRatio = 16.8 @@ -195,10 +194,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 35f87307d6..84fa36a994 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -1,5 +1,5 @@ from collections import defaultdict -from dataclasses import dataclass, field +from dataclasses import dataclass from enum import Enum from typing import Dict, List, Union @@ -68,7 +68,6 @@ class CAR: ACADIA = "GMC ACADIA DENALI 2018" BUICK_REGAL = "BUICK REGAL ESSENCE 2018" ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" - BOLT_EV = "CHEVROLET BOLT EV 2022" BOLT_EUV = "CHEVROLET BOLT EUV 2022" SILVERADO = "CHEVROLET SILVERADO 1500 2020" EQUINOX = "CHEVROLET EQUINOX 2019" @@ -84,8 +83,13 @@ class Footnote(Enum): @dataclass class GMCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC)" - harness: Enum = Harness.obd_ii - footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II]) + + def init_make(self, CP: car.CarParams): + if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera: + self.harness = Harness.gm + else: + self.harness = Harness.obd_ii + self.footnotes.append(Footnote.OBD_II) CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { @@ -96,13 +100,15 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), - CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23", footnotes=[], harness=Harness.gm), - CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm), + CAR.BOLT_EUV: [ + GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210"), + GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), + ], CAR.SILVERADO: [ - GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II", footnotes=[], harness=Harness.gm), - GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", "https://youtu.be/5HbNoBLzRwE", footnotes=[], harness=Harness.gm), + GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"), + GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", "https://youtu.be/5HbNoBLzRwE"), ], - CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22", footnotes=[], harness=Harness.gm), + CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"), } @@ -188,9 +194,9 @@ FINGERPRINTS = { DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) -EV_CAR = {CAR.VOLT, CAR.BOLT_EV, CAR.BOLT_EUV} +EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} # We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) -CAMERA_ACC_CAR = {CAR.BOLT_EV, CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} +CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0dbb4126d8..122b69463c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -210,14 +210,14 @@ class CarController: clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) - pcm_accel = int(1.0 * 0xc6) + pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX) else: pcm_speed_V = [0.0, clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) - pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6) + pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX) if not self.CP.openpilotLongitudinalControl: if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message @@ -275,7 +275,7 @@ class CarController: self.speed = pcm_speed if not self.CP.enableGasInterceptor: - self.gas = pcm_accel / 0xc6 + self.gas = pcm_accel / self.params.NIDEC_GAS_MAX new_actuators = actuators.copy() new_actuators.speed = self.speed @@ -283,6 +283,7 @@ class CarController: new_actuators.gas = self.gas new_actuators.brake = self.brake new_actuators.steer = self.last_steer + new_actuators.steerOutputCan = apply_steer self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 7aeed2445d..dae4f91091 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -13,7 +13,8 @@ def get_pt_bus(car_fingerprint): def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): - if radar_disabled: + no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS + if radar_disabled or no_radar: # when radar is disabled, steering commands are sent directly to powertrain bus return get_pt_bus(car_fingerprint) # normally steering commands are sent to radar, which forwards them to powertrain bus @@ -112,7 +113,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint): def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) - radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl + radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) if CP.openpilotLongitudinalControl: @@ -165,7 +166,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, else: commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values)) - if radar_disabled and CP.carFingerprint in HONDA_BOSCH: + if radar_disabled: radar_hud_values = { 'CMBS_OFF': 0x01, 'SET_TO_1': 0x01, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c11828f311..816524f6e9 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from common.numpy_fast import interp from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS -from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu @@ -29,8 +29,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "honda" if candidate in HONDA_BOSCH: @@ -291,10 +290,6 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor ret.minEnableSpeed = -1. if stop_and_go else 25.5 * CV.MPH_TO_MS - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 2510f2e1ff..905c9f4b4f 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -27,6 +27,7 @@ class CarControllerParams: NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6] NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.] + NIDEC_GAS_MAX = 198 # 0xc6 NIDEC_BRAKE_MAX = 1024 // 4 BOSCH_ACCEL_MIN = -3.5 # m/s^2 @@ -106,40 +107,46 @@ class Footnote(Enum): class HondaCarInfo(CarInfo): package: str = "Honda Sensing" + def init_make(self, CP: car.CarParams): + if CP.carFingerprint in HONDA_BOSCH: + self.harness = Harness.bosch_b if CP.carFingerprint in HONDA_BOSCH_RADARLESS else Harness.bosch_a + else: + self.harness = Harness.nidec + CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.ACCORD: [ - HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), ], - CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"), + CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), + CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"), CAR.CIVIC_BOSCH: [ - HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS, harness=Harness.bosch_a), - HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS), + HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), ], CAR.CIVIC_BOSCH_DIESEL: None, # same platform CAR.CIVIC_2022: [ - HondaCarInfo("Honda Civic 2022", "All", harness=Harness.bosch_b), - HondaCarInfo("Honda Civic Hatchback 2022", "All", harness=Harness.bosch_b), + HondaCarInfo("Honda Civic 2022", "All"), + HondaCarInfo("Honda Civic Hatchback 2022", "All"), ], - CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS), + CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring - CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", harness=Harness.nidec), + CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"), CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey - CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec), - CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), - CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), + CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), + CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), } FW_QUERY_CONFIG = FwQueryConfig( @@ -236,7 +243,7 @@ FW_VERSIONS = { b'39990-TVA-A340\x00\x00', b'39990-TVA-X030\x00\x00', b'39990-TVA-X040\x00\x00', - b'39990-TVA,A150\x00\x00', + b'39990-TVA,A150\x00\x00', # modified firmware b'39990-TVE-H130\x00\x00', ], (Ecu.unknown, 0x18da3af1, None): [ diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index e5fdbfd57a..5582499f25 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -60,9 +60,6 @@ class CarController: # steering torque steer = actuators.steer - if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): - # these cars have significantly more torque than most HKG; limit to 70% of max - steer = clip(steer, -0.7, 0.7) new_steer = int(round(steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -86,11 +83,16 @@ class CarController: # tester present - w/ no response (keeps relevant ECU disabled) if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl: + # for longitudinal control, either radar or ADAS driving ECU addr, bus = 0x7d0, 0 if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: addr, bus = 0x730, 5 can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) + # for blinkers + if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: + can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5]) + # >90 degree steering fault prevention # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE: @@ -121,6 +123,10 @@ class CarController: if self.frame % 5 == 0 and (not hda2 or hda2_long): can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + # blinkers + if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: + can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, CC.leftBlinker, CC.rightBlinker)) + if self.CP.openpilotLongitudinalControl: if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) @@ -189,6 +195,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX + new_actuators.steerOutputCan = apply_steer new_actuators.accel = accel self.frame += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 2c309fa0df..fc196528d7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams +from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase PREV_BUTTON_SAMPLES = 8 @@ -32,7 +32,6 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] self.is_metric = False - self.brake_error = False self.buttons_counter = 0 self.cruise_info = {} @@ -72,8 +71,10 @@ class CarState(CarStateBase): self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] self.cluster_speed_counter = 0 - # mimic how dash converts to imperial - if not self.is_metric: + # Mimic how dash converts to imperial. + # Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric + # TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this + if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,): self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH) ret.vEgoCluster = self.cluster_speed * speed_conv @@ -103,8 +104,9 @@ class CarState(CarStateBase): # TODO: Find brake pressure ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0 - ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY + ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 + ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR): if self.CP.carFingerprint in HYBRID_CAR: @@ -145,7 +147,6 @@ class CarState(CarStateBase): self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE - self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) @@ -187,7 +188,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1 ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] - ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], @@ -197,7 +198,7 @@ class CarState(CarStateBase): ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0 ret.cruiseState.available = True - self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1 + self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 if not self.CP.openpilotLongitudinalControl: speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp @@ -211,6 +212,7 @@ class CarState(CarStateBase): self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] + ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED if self.CP.flags & HyundaiFlags.CANFD_HDA2: self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"]) @@ -432,12 +434,12 @@ class CarState(CarStateBase): ("LKA_FAULT", "MDPS"), ("DriverBraking", "TCS"), + ("ACCEnable", "TCS"), ("COUNTER", cruise_btn_msg), ("CRUISE_BUTTONS", cruise_btn_msg), ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), - - ("DISTANCE_UNIT", "CLUSTER_INFO"), + ("DISTANCE_UNIT", "CRUISE_BUTTONS_ALT"), ("LEFT_LAMP", "BLINKERS"), ("RIGHT_LAMP", "BLINKERS"), @@ -452,12 +454,14 @@ class CarState(CarStateBase): ("STEERING_SENSORS", 100), ("MDPS", 100), ("TCS", 50), - (cruise_btn_msg, 50), - ("CLUSTER_INFO", 4), + ("CRUISE_BUTTONS_ALT", 50), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), ] + if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): + checks.append(("CRUISE_BUTTONS", 50)) + if CP.enableBsm: signals += [ ("FL_INDICATOR", "BLINDSPOTS_REAR_CORNERS"), diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 8b53e7c378..c492e6a5ff 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,7 +1,6 @@ from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags - def get_e_can_bus(CP): # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars # have a different harness than the HDA1 and non-HDA variants in order to split @@ -98,6 +97,25 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) +def create_spas_messages(packer, frame, left_blink, right_blink): + ret = [] + + values = { + } + ret.append(packer.make_can_msg("SPAS1", 5, values)) + + blink = 0 + if left_blink: + blink = 3 + elif right_blink: + blink = 4 + values = { + "BLINKER_CONTROL": blink, + } + ret.append(packer.make_can_msg("SPAS2", 5, values)) + + return ret + def create_adrv_messages(packer, frame): # messages needed to car happy after disabling diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 8738aabd17..811d629f70 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,9 +2,9 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams +from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR -from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu @@ -18,13 +18,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): - return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX - - @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "hyundai" ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -120,8 +114,8 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.67 ret.steerRatio = 14.00 * 1.15 tire_stiffness_factor = 0.385 - elif candidate == CAR.TUCSON_HYBRID_4TH_GEN: - ret.mass = 1680. + STD_CARGO_KG # average of all 3 trims + elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN): + ret.mass = 1630. + STD_CARGO_KG # average ret.wheelbase = 2.756 ret.steerRatio = 16. tire_stiffness_factor = 0.385 @@ -191,8 +185,16 @@ class CarInterface(CarInterfaceBase): ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only ret.wheelbase = 2.756 ret.steerRatio = 13.6 + elif candidate == CAR.KIA_SORENTO_PHEV_4TH_GEN: + ret.mass = 4095.8 * CV.LB_TO_KG + STD_CARGO_KG # weight from EX and above trims, average of FWD and AWD versions (EX, X-Line EX AWD, SX, SX Pestige, X-Line SX Prestige AWD) + ret.wheelbase = 2.81 + ret.steerRatio = 13.27 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sorento-phev/2022/specifications # Genesis + elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN: + ret.mass = 2205 + STD_CARGO_KG + ret.wheelbase = 2.9 + ret.steerRatio = 12.6 # https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. elif candidate == CAR.GENESIS_G70: ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG @@ -269,11 +271,11 @@ class CarInterface(CarInterfaceBase): elif candidate in EV_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS - ret.centerToFront = ret.wheelbase * 0.4 + if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + ret.flags |= HyundaiFlags.ALT_LIMITS.value + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.4 # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors @@ -289,6 +291,10 @@ class CarInterface(CarInterfaceBase): addr, bus = 0x730, 5 disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') + # for blinkers + if CP.flags & HyundaiFlags.ENABLE_BLINKERS: + disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) @@ -306,9 +312,6 @@ class CarInterface(CarInterfaceBase): allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons) events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) - if self.CS.brake_error: - events.add(EventName.brakeUnavailable) - # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: self.low_speed_alert = True diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index ecba7b7494..cdcaf629af 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,12 +1,12 @@ -from enum import IntFlag from dataclasses import dataclass +from enum import Enum, IntFlag from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -39,6 +39,12 @@ class CarControllerParams: CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 + # these cars have significantly more torque than most HKG; limit to 70% of max + elif CP.flags & HyundaiFlags.ALT_LIMITS: + self.STEER_MAX = 270 + self.STEER_DELTA_UP = 2 + self.STEER_DELTA_DOWN = 3 + # Default for most HKG else: self.STEER_MAX = 384 @@ -50,6 +56,10 @@ class HyundaiFlags(IntFlag): CANFD_ALT_GEARS = 4 CANFD_CAMERA_SCC = 8 + ALT_LIMITS = 16 + + ENABLE_BLINKERS = 32 + class CAR: # Hyundai @@ -78,6 +88,7 @@ class CAR: VELOSTER = "HYUNDAI VELOSTER 2019" SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" IONIQ_5 = "HYUNDAI IONIQ 5 2022" + TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN" TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN" SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN" @@ -93,6 +104,7 @@ class CAR: KIA_SELTOS = "KIA SELTOS 2021" KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN" KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN" KIA_STINGER = "KIA STINGER GT2 2018" KIA_STINGER_2022 = "KIA STINGER 2022" @@ -100,6 +112,7 @@ class CAR: KIA_EV6 = "KIA EV6 2022" # Genesis + GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN" GENESIS_G70 = "GENESIS G70 2018" GENESIS_G70_2020 = "GENESIS G70 2020" GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" @@ -107,16 +120,27 @@ class CAR: GENESIS_G90 = "GENESIS G90 2017" +class Footnote(Enum): + CANFD = CarFootnote( + "Requires a red panda, additional harness box, " + + "additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.", + Column.MODEL, shop_footnote=True) + + @dataclass class HyundaiCarInfo(CarInfo): package: str = "Smart Cruise Control (SCC)" + def init_make(self, CP: car.CarParams): + if CP.carFingerprint in CANFD_CAR: + self.footnotes.insert(0, Footnote.CANFD) + CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.ELANTRA: [ HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b), HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e), - HyundaiCarInfo("Hyundai i30 2019", harness=Harness.hyundai_e), + HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e), ], CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), @@ -148,11 +172,15 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e), CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a), CAR.IONIQ_5: [ - HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23" , "Highway Driving Assist", harness=Harness.hyundai_k), + HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_k), HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_q), ], + CAR.TUCSON_4TH_GEN: [ + HyundaiCarInfo("Hyundai Tucson 2022", harness=Harness.hyundai_n), + HyundaiCarInfo("Hyundai Tucson 2023", "All", harness=Harness.hyundai_n), + ], CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022", "All", harness=Harness.hyundai_n), - CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2021-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n), + CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2021-22", harness=Harness.hyundai_n), # Kia CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), @@ -175,11 +203,12 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { HyundaiCarInfo("Kia Optima Hybrid 2019"), ], CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a), - CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n), + CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness=Harness.hyundai_n), CAR.KIA_SORENTO: [ 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_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "Smart Cruise Control (SCC)", 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), CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness=Harness.hyundai_k), @@ -190,6 +219,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { ], # Genesis + CAR.GENESIS_GV60_EV_1ST_GEN: HyundaiCarInfo("Genesis GV60 2023", "All", harness=Harness.hyundai_k), CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f), CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f), CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l), @@ -685,12 +715,14 @@ FW_VERSIONS = { b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', b'\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', + b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x82TACVN5GMI3XXXH0A', b'\xf1\x82TMBZN5TMD3XXXG2E', b'\xf1\x82TACVN5GSI3XXXH0A', b'\xf1\x82TMCFD5MMCXXXXG0A', + b'\xf1\x81HM6M1_0a0_G20', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A', b'\xf1\x81HM6M2_0a0_G00', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', @@ -715,6 +747,7 @@ FW_VERSIONS = { b'\xf1\x00HT6TA290BLHT6TAF00A1STM0M25GS1\x00\x00\x00\x00\x00\x006\xd8\x97\x15', b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02900A1 \xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', + b'\xf1\x00T02601BL T02800A1 VTMPT25XXX800NS4\xed\xaf\xed\xf5', ], }, CAR.SANTA_FE_HEV_2022: { @@ -819,6 +852,7 @@ FW_VERSIONS = { b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ', b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ', + b'\xf1\x00ON__ FCA FHCUP 1.00 1.01 99110-S9110 ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', @@ -832,6 +866,7 @@ FW_VERSIONS = { b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360', b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360', b'\xf1\x00ON ESC \x0b 101\x19\t\x05 58910-S9320', + b'\xf1\x00ON ESC \x01 101\x19\t\x08 58910-S9360', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', @@ -854,11 +889,13 @@ FW_VERSIONS = { b'\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 181105', b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.04 99211-S9100 211227', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00TON2G38NB5j\x94.\xde', b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', @@ -1021,11 +1058,13 @@ FW_VERSIONS = { b'\xf1\x8799110L2000\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ', + b'\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101', b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102', + b'\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915', @@ -1036,6 +1075,7 @@ FW_VERSIONS = { b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200', b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600', + b'\xf1\x00DL ESC \t 100 \x06\x02 58910-L3800', ], (Ecu.engine, 0x7E0, None): [ b'\xf1\x87391212MKT0', @@ -1048,6 +1088,7 @@ FW_VERSIONS = { b'\xf1\x87SALFEA6046104GK2wvwgeTeFg\x88\x96xwwwwffvfe?\xfd\xff\x86fo\xff\x97A\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 WDL3T25XXX730NS2b\x1f\xb8%', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', ], }, CAR.KONA_EV: { @@ -1240,18 +1281,23 @@ FW_VERSIONS = { CAR.ELANTRA: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54', + b'\xf1\x00PD LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DPD0H16NS0e\x0e\xcd\x8e', + b'\xf1\x006U2U0_C2\x00\x006U2T0051\x00\x00DPD0D16KS0u\xce\x1fk', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00PD MDPS C 1.00 1.04 56310/G3300 4PDDC104', + b'\xf1\x00PD MDPS C 1.00 1.00 56310G3300\x00 4PDDC100', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00PD ESC \x0b 104\x18\t\x03 58920-G3350', + b'\xf1\x00PD ESC \t 104\x18\t\x03 58920-G3350', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00PD__ SCC F-CUP 1.00 1.00 96400-G3300 ', + b'\xf1\x00PD__ SCC FNCUP 1.01 1.00 96400-G3000 ', ], }, CAR.ELANTRA_2021: { @@ -1390,6 +1436,14 @@ FW_VERSIONS = { b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00' ], }, + CAR.KIA_SORENTO_PHEV_4TH_GEN: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00MQhe SCC FHCUP 1.00 1.06 99110-P4000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00MQ4HMFC AT USA LHD 1.00 1.11 99210-P2000 211217', + ] + }, CAR.KIA_EV6: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', @@ -1413,10 +1467,19 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', ], }, + CAR.TUCSON_4TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9240 14T', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', + ], + }, CAR.TUCSON_HYBRID_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9240 14Q', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9220 14K', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9100 14A', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', @@ -1456,6 +1519,14 @@ FW_VERSIONS = { b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ', ], }, + CAR.GENESIS_GV60_EV_1ST_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ', + ], + }, } CHECKSUM = { @@ -1473,16 +1544,16 @@ FEATURES = { "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022}, } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN} +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN} # The radar does SCC on these cars when HDA I, rather than the camera -CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, } +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN} # these cars use a different gas signal -EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5} +HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # these cars use a different gas signal +EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} # these cars require a special panda safety mode due to missing counters and checksums in the messages LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} @@ -1532,10 +1603,13 @@ DBC = { CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), + CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.IONIQ_5: dbc_dict('hyundai_canfd', None), CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None), CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8e8872a539..ad1fd22b90 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -8,10 +8,10 @@ from cereal import car from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D -from common.numpy_fast import interp +from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.car import apply_hysteresis, gen_empty_fingerprint -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone +from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -87,10 +87,34 @@ class CarInterfaceBase(ABC): def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX + @classmethod + def get_params(cls, candidate: str, fingerprint: Optional[Dict[int, Dict[int, int]]] = None, car_fw: Optional[List[car.CarParams.CarFw]] = None, experimental_long: bool = False): + if fingerprint is None: + fingerprint = gen_empty_fingerprint() + + if car_fw is None: + car_fw = list() + + ret = CarInterfaceBase.get_std_params(candidate) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long) + + # Set common params using fields set by the car interface + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: some car interfaces set stiffness factor + if ret.tireStiffnessFront == 0 or ret.tireStiffnessRear == 0: + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + @staticmethod @abstractmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - pass + def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool): + raise NotImplementedError @staticmethod def init(CP, logcan, sendcan): @@ -109,7 +133,7 @@ class CarInterfaceBase(ABC): def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation): # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction_interp = interp( - apply_deadzone(lateral_accel_error, lateral_accel_deadzone), + apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-torque_params.friction, torque_params.friction] ) @@ -121,7 +145,7 @@ class CarInterfaceBase(ABC): # returns a set of default params to avoid repetition in car specific params @staticmethod - def get_std_params(candidate, fingerprint): + def get_std_params(candidate): ret = car.CarParams.new_message() ret.carFingerprint = candidate @@ -303,6 +327,7 @@ class CarStateBase(ABC): self.cruise_buttons = 0 self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 + self.steering_pressed_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False self.cluster_speed_hyst_gap = 0.0 @@ -340,6 +365,12 @@ class CarStateBase(ABC): self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0) return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0 + def update_steering_pressed(self, steering_pressed, steering_pressed_min_count): + """Applies filtering on steering pressed for noisy driver torque signals.""" + self.steering_pressed_cnt += 1 if steering_pressed else -1 + self.steering_pressed_cnt = clip(self.steering_pressed_cnt, 0, steering_pressed_min_count * 2) + return self.steering_pressed_cnt > steering_pressed_min_count + def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool): """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time, or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 4b4bdcc0ca..d9c658a14c 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -137,17 +137,17 @@ class IsoTpParallelQuery: else: response_timeouts[tx_addr] = 0 request_done[tx_addr] = True - cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") + cloudlog.error(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") cur_time = time.monotonic() if cur_time - max(response_timeouts.values()) > 0: for tx_addr in msgs: if request_counter[tx_addr] > 0 and not request_done[tx_addr]: - cloudlog.warning(f"iso-tp query timeout after receiving response: {tx_addr}") + cloudlog.error(f"iso-tp query timeout after receiving response: {tx_addr}") break if cur_time - start_time > total_timeout: - cloudlog.warning("iso-tp query timeout while receiving data") + cloudlog.error("iso-tp query timeout while receiving data") break return results diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 2add59ccb0..027822cc3f 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -59,6 +59,7 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steerOutputCan = apply_steer self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 7c42431e33..fdd2439ff9 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -2,7 +2,7 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car.mazda.values import CAR, LKAS_LIMITS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -11,9 +11,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarOffCan = True @@ -48,10 +46,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.41 - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 129273efee..e6e9b3aee8 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -42,7 +42,7 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"), CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), - CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-22", video_link="https://youtu.be/dA3duO4a0O4"), + CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"), } @@ -283,6 +283,7 @@ FW_VERSIONS = { b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ + b'PXGW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -299,6 +300,7 @@ FW_VERSIONS = { b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a3194cd79e..3ac487dbb7 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,39 +1,28 @@ #!/usr/bin/env python3 -import math from cereal import car -from common.conversions import Conversions as CV from system.swaglog import cloudlog import cereal.messaging as messaging -from selfdrive.car import gen_empty_fingerprint, get_safety_config +from selfdrive.car import get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -# mocked car interface to work with chffrplus -TS = 0.01 # 100Hz -YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter -# low pass gain -LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS) - +# mocked car interface to work with chffrplus class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) cloudlog.debug("Using Mock Car Interface") - self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) + self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) self.speed = 0. self.prev_speed = 0. - self.yaw_rate = 0. - self.yaw_rate_meas = 0. @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mock" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] ret.mass = 1700. - ret.rotationalInertia = 2500. ret.wheelbase = 2.70 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 13. # reasonable @@ -45,11 +34,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): self.sm.update(0) - - # get basic data from phone and gps since CAN isn't connected - if self.sm.updated['gyroscope']: - self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0] - gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' if self.sm.updated[gps_sock]: self.prev_speed = self.speed @@ -61,10 +45,9 @@ class CarInterface(CarInterfaceBase): # speeds ret.vEgo = self.speed ret.vEgoRaw = self.speed - a = self.speed - self.prev_speed - ret.aEgo = a - ret.brakePressed = a < -0.5 + ret.aEgo = self.speed - self.prev_speed + ret.brakePressed = ret.aEgo < -0.5 ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed @@ -72,10 +55,6 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed - self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate - curvature = self.yaw_rate / max(self.speed, 1.) - ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG - return ret def apply(self, c): diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index dbc2b33c6b..ff13812398 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car -from common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.nissan import nissancan from selfdrive.car.nissan.values import CAR, CarControllerParams @@ -14,7 +14,7 @@ class CarController: self.frame = 0 self.lkas_max_torque = 0 - self.last_angle = 0 + self.apply_angle_last = 0 self.packer = CANPacker(dbc_name) @@ -28,18 +28,11 @@ class CarController: ### STEER ### lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg - apply_angle = actuators.steeringAngleDeg - steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 if CC.latActive: # windup slower - if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): - angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) - else: - angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) - - apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): @@ -57,7 +50,7 @@ class CarController: apply_angle = CS.out.steeringAngleDeg self.lkas_max_torque = 0 - self.last_angle = apply_angle + self.apply_angle_last = apply_angle if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index e095ceb461..386e859089 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.nissan.values import CAR @@ -8,9 +8,7 @@ from selfdrive.car.nissan.values import CAR class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False @@ -20,6 +18,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 ret.steerRatio = 17 + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.radarOffCan = True + if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 @@ -35,17 +36,6 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.824 ret.centerToFront = ret.wheelbase * 0.44 - ret.steerControlType = car.CarParams.SteerControlType.angle - ret.radarOffCan = True - - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret # returns a car.CarState diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 09bd7ca838..a6ee27a4a3 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -4,7 +4,7 @@ from enum import Enum from cereal import car from panda.python import uds -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries @@ -12,9 +12,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: - ANGLE_DELTA_BP = [0., 5., 15.] - ANGLE_DELTA_V = [5., .8, .15] # windup limit - ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index b5429daef2..a56e63408e 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -87,6 +87,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index a920c02534..22468801ec 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS @@ -9,9 +9,7 @@ from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "subaru" ret.radarOffCan = True ret.dashcamOnly = candidate in PREGLOBAL_CARS @@ -103,14 +101,6 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"unknown car: {candidate}") - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret # returns a car.CarState diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 7a1e9a8a3d..6ac2637fa2 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -63,7 +63,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { ], CAR.IMPREZA_2020: [ SubaruCarInfo("Subaru Impreza 2020-22"), - SubaruCarInfo("Subaru Crosstrek 2020-21"), + SubaruCarInfo("Subaru Crosstrek 2020-23"), SubaruCarInfo("Subaru XV 2020-21"), ], CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), @@ -235,12 +235,14 @@ FW_VERSIONS = { b'\x9a\xc0\000\000', b'\n\xc0\004\000', b'\x9a\xc0\x04\x00', + b'\n\xc0\x04\x01', ], (Ecu.fwdCamera, 0x787, None): [ b'\000\000eb\037@ \"', b'\000\000e\x8f\037@ )', b'\x00\x00eq\x1f@ "', b'\x00\x00eq\x00\x00\x00\x00', + b'\x00\x00e\x8f\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xca!ap\a', @@ -250,6 +252,7 @@ FW_VERSIONS = { b'\xcc!fp\a', b'\xca!f@\x07', b'\xca!fp\x07', + b'\xf3"f@\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe6\xf5\004\000\000', @@ -258,6 +261,7 @@ FW_VERSIONS = { b'\xe7\xf5D0\000', b'\xf1\x00\xd7\x10@', b'\xe6\xf5D0\x00', + b'\xe9\xf6F0\x00', ], }, CAR.FORESTER: { @@ -461,6 +465,7 @@ FW_VERSIONS = { b'\xa1 \x08\x02', b'\xa1 \x06\x02', b'\xa1 \x08\x00', + b'\xa1 "\t\x00', ], (Ecu.eps, 0x746, None): [ b'\x9b\xc0\x10\x00', @@ -482,6 +487,7 @@ FW_VERSIONS = { b'\xe2"`p\x07', b'\xf1\x82\xe2,\xa0@\x07', b'\xbc"`q\x07', + b'\xe3,\xa0@\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xa5\xfe\xf7@\x00', diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index cf43b8ef00..6e2869d1c2 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,5 +1,6 @@ -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.tesla.teslacan import TeslaCAN from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams @@ -8,7 +9,7 @@ class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.frame = 0 - self.last_angle = 0 + self.apply_angle_last = 0 self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) @@ -24,20 +25,15 @@ class CarController: lkas_enabled = CC.latActive and not hands_on_fault if lkas_enabled: - apply_angle = actuators.steeringAngleDeg - # Angular rate limit based on speed - steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) - rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN - max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) # To not fault the EPS apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) else: apply_angle = CS.out.steeringAngleDeg - self.last_angle = apply_angle + self.apply_angle_last = apply_angle can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) # Longitudinal control (in sync with stock message, about 40Hz) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 2eb29efb41..49e06d8923 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -2,14 +2,13 @@ from cereal import car from panda import Panda from selfdrive.car.tesla.values import CANBUS, CAR -from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, @@ -51,9 +50,6 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"Unsupported car: {candidate}") - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret def _update(self, c): diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index e28666c625..52f2aedf98 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -2,14 +2,13 @@ from collections import namedtuple from typing import Dict, List, Union from cereal import car -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) -AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) class CAR: @@ -99,13 +98,13 @@ BUTTONS = [ Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), - Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), - Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), ] class CarControllerParams: - RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) JERK_LIMIT_MAX = 8 JERK_LIMIT_MIN = -8 ACCEL_TO_SPEED_MULTIPLIER = 3 diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index cd61528439..3edc406e19 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -22,7 +22,6 @@ non_tested_cars = [ GM.HOLDEN_ASTRA, GM.MALIBU, GM.EQUINOX, - GM.BOLT_EV, HYUNDAI.GENESIS_G90, HYUNDAI.KIA_OPTIMA_H, HONDA.ODYSSEY_CHN, @@ -51,6 +50,7 @@ routes = [ CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.ESCALADE_ESV), CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.VOLT), CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.BOLT_EUV, segment=1), + CarTestRoute("555d4087cf86aa91|2022-12-02--12-15-07", GM.BOLT_EUV, segment=14), # Bolt EV CarTestRoute("38aa7da107d5d252|2022-08-15--16-01-12", GM.SILVERADO), CarTestRoute("0e7a2ba168465df5|2020-10-18--14-14-22", HONDA.ACURA_RDX_3G), @@ -80,6 +80,7 @@ routes = [ CarTestRoute("f44aa96ace22f34a|2021-12-22--06-22-31", HONDA.CIVIC_2022), CarTestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS), + CarTestRoute("b5d6dc830ad63071|2022-12-12--21-28-25", HYUNDAI.GENESIS_GV60_EV_1ST_GEN, segment=12), CarTestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70), CarTestRoute("ca4de5b12321bd98|2022-10-18--21-15-59", HYUNDAI.GENESIS_GV70_1ST_GEN), CarTestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80), @@ -96,8 +97,10 @@ routes = [ CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA), CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF), CarTestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.TUCSON), + CarTestRoute("db68bbe12250812c|2022-12-05--00-54-12", HYUNDAI.TUCSON_4TH_GEN), # 2023 CarTestRoute("36e10531feea61a4|2022-07-25--13-37-42", HYUNDAI.TUCSON_HYBRID_4TH_GEN), CarTestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), + CarTestRoute("628935d7d3e5f4f7|2022-11-30--01-12-46", HYUNDAI.KIA_SORENTO_PHEV_4TH_GEN), CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.PALISADE), CarTestRoute("05a8f0197fdac372|2022-10-19--14-14-09", HYUNDAI.IONIQ_5), # HDA2 CarTestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.IONIQ_PHEV_2019), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 8d22173671..48d85584b3 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -31,6 +31,8 @@ class TestCarInterfaces(unittest.TestCase): assert car_interface self.assertGreater(car_params.mass, 1) + self.assertGreater(car_params.wheelbase, 0) + self.assertGreater(car_params.centerToFront, 0) self.assertGreater(car_params.maxLateralAccel, 0) if car_params.steerControlType != car.CarParams.SteerControlType.angle: diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 56530dd738..641c109316 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -9,7 +9,6 @@ from parameterized import parameterized_class from cereal import log, car from common.realtime import DT_CTRL -from selfdrive.boardd.boardd import can_capnp_to_can_list from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces from selfdrive.car.gm.values import CAR as GM @@ -20,8 +19,7 @@ from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader from tools.lib.route import Route -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import package_can_msg +from panda.tests.libpanda import libpanda_py PandaType = log.PandaState.PandaType @@ -118,7 +116,7 @@ class TestCarModelBase(unittest.TestCase): assert self.CI # TODO: check safetyModel is in release panda build - self.safety = libpandasafety_py.libpandasafety + self.safety = libpanda_py.libpanda cfg = self.CP.safetyConfigs[-1] set_status = self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam) @@ -192,7 +190,7 @@ class TestCarModelBase(unittest.TestCase): if msg.src >= 64: continue - to_send = package_can_msg([msg.address, 0, msg.dat, msg.src % 4]) + to_send = libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) if self.safety.safety_rx_hook(to_send) != 1: failed_addrs[hex(msg.address)] += 1 @@ -215,8 +213,8 @@ class TestCarModelBase(unittest.TestCase): # warm up pass, as initial states may be different for can in self.can_msgs[:300]: self.CI.update(CC, (can.as_builder().to_bytes(), )) - for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): - to_send = package_can_msg(msg) + for msg in filter(lambda m: m.src in range(64), can.can): + to_send = libpanda_py.make_CANPacket(msg.address, msg.src, msg.dat) self.safety.safety_rx_hook(to_send) controls_allowed_prev = False @@ -224,10 +222,8 @@ class TestCarModelBase(unittest.TestCase): checks = defaultdict(lambda: 0) for idx, can in enumerate(self.can_msgs): CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) - for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): - msg = list(msg) - msg[3] %= 4 - to_send = package_can_msg(msg) + for msg in filter(lambda m: m.src in range(64), can.can): + to_send = libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) ret = self.safety.safety_rx_hook(to_send) self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 6ec782444c..d7b2ec4079 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -34,6 +34,8 @@ HYUNDAI SANTA CRUZ 1ST GEN: [2.7, 2.7, 0.1] KIA SPORTAGE 5TH GEN: [2.7, 2.7, 0.1] KIA SPORTAGE HYBRID 5TH GEN: [2.5, 2.5, 0.1] GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1] +KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1] +GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index a9023b4edc..eb1a04cee6 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -27,10 +27,11 @@ HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819] HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171] +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: [4.398306735170212, 3.2961956260770484, 0.08651833437845884] +HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 3.2961956260770484, 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] @@ -41,7 +42,7 @@ HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488 HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272] JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185] JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003] -KIA EV6 2022: [3.2, 3.0, 0.05] +KIA EV6 2022: [3.2, 2.093457, 0.05] KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716] KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558] @@ -77,7 +78,7 @@ TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054 TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600] TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993] TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565] -TOYOTA PRIUS 2017: [1.746445, 1.5023147650693636, 0.151515] +TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515] TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968] TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975] TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 77236e393e..aeb2e6f280 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -29,13 +29,13 @@ HYUNDAI VELOSTER 2019: HYUNDAI SONATA 2019 HYUNDAI KONA 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA HYBRID 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019 -HYUNDAI IONIQ 5 2022: KIA EV6 2022 HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019 HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019 HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 +HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 KIA STINGER 2022: KIA STINGER GT2 2018 GENESIS G90 2017: GENESIS G70 2018 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 4ec9500171..6dbdd4b5d9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -146,7 +146,7 @@ class CarController: if self.frame % 100 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled)) + hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) @@ -158,6 +158,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steerOutputCan = apply_steer new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8efb2c79e3..050f8747a2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,3 +1,5 @@ +import copy + from cereal import car from common.conversions import Conversions as CV from common.numpy_fast import mean @@ -6,7 +8,7 @@ from common.realtime import DT_CTRL from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.toyota.values import ToyotaFlags, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR class CarState(CarStateBase): @@ -26,6 +28,7 @@ class CarState(CarStateBase): self.low_speed_lockout = False self.acc_type = 1 + self.lkas_hud = {} def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -85,16 +88,20 @@ class CarState(CarStateBase): ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - # steer rate fault, goes to 21 or 25 for 1 frame, then 9 for ~2 seconds - ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 21, 25) + # steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds + # lka msg drop out: goes to 9 then 11 for a combined total of 2 seconds + ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 11, 21, 25) # 17 is a fault from a prolonged high torque delta between cmd and user - ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17 + # 3 is a fault from the lka command message not being received by the EPS + ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in (3, 17) if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: + # TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"] else: + ret.accFaulted = cp.vl["PCM_CRUISE_2"]["ACC_FAULTED"] != 0 ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"] @@ -120,11 +127,8 @@ class CarState(CarStateBase): self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] - if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: - # ignore standstill in hybrid vehicles, since pcm allows to restart without - # receiving any special command. Also if interceptor is detected - ret.cruiseState.standstill = False - else: + if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR): + # ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request ret.cruiseState.standstill = self.pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) @@ -139,6 +143,9 @@ class CarState(CarStateBase): ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) + if self.CP.carFingerprint != CAR.PRIUS_V: + self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + return ret @staticmethod @@ -208,6 +215,7 @@ class CarState(CarStateBase): else: signals.append(("MAIN_ON", "PCM_CRUISE_2")) signals.append(("SET_SPEED", "PCM_CRUISE_2")) + signals.append(("ACC_FAULTED", "PCM_CRUISE_2")) signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) checks.append(("PCM_CRUISE_2", 33)) @@ -252,6 +260,18 @@ class CarState(CarStateBase): signals = [] checks = [] + if CP.carFingerprint != CAR.PRIUS_V: + signals += [ + ("LANE_SWAY_FLD", "LKAS_HUD"), + ("LANE_SWAY_BUZZER", "LKAS_HUD"), + ("LANE_SWAY_WARNING", "LKAS_HUD"), + ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"), + ("LANE_SWAY_TOGGLE", "LKAS_HUD"), + ] + checks += [ + ("LKAS_HUD", 1), + ] + if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3f4edb36d2..8e180e2301 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,7 @@ from cereal import car from common.conversions import Conversions as CV 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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase EventName = car.CarEvent.EventName @@ -15,9 +15,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -42,7 +40,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 = 1.0 + steering_angle_deadzone_deg = 0.2 + ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate == CAR.PRIUS_V: @@ -51,7 +50,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -190,10 +188,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, @@ -231,6 +225,8 @@ class CarInterface(CarInterfaceBase): tune.kiBP = [0., 5., 12., 20., 27.] tune.kiV = [.35, .23, .20, .17, .1] if candidate in TSS2_CAR: + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 # reach stopping target smoothly else: tune.kpBP = [0., 5., 35.] @@ -247,16 +243,19 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: - events.add(EventName.lowSpeedLockout) - if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: - events.add(EventName.belowEngageSpeed) - if c.actuators.accel > 0.3: - # some margin on the actuator to not false trigger cancellation while stopping - events.add(EventName.speedTooLow) - if ret.vEgo < 0.001: - # while in standstill, send a user alert - events.add(EventName.manualRestart) + if self.CP.openpilotLongitudinalControl: + if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor: + events.add(EventName.resumeRequired) + if self.CS.low_speed_lockout: + events.add(EventName.lowSpeedLockout) + if ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + if c.actuators.accel > 0.3: + # some margin on the actuator to not false trigger cancellation while stopping + events.add(EventName.speedTooLow) + if ret.vEgo < 0.001: + # while in standstill, send a user alert + events.add(EventName.manualRestart) ret.events = events.to_msg() diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7ab3ab3e78..7e360cc4e1 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -66,13 +66,13 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("ACC_HUD", 0, values) -def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled): +def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud): values = { "TWO_BEEPS": chime, "LDA_ALERT": steer, "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, - "BARRIERS" : 1 if enabled else 0, + "BARRIERS": 1 if enabled else 0, # static signals "SET_ME_X02": 2, @@ -96,4 +96,9 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep "ADJUSTING_CAMERA": 0, "LDW_EXIST": 1, } + + # lane sway functionality + # not all cars have LKAS_HUD — update with camera values if available + values.update(stock_lkas_hud) + return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ba26c7e03b..6a92540f00 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -113,7 +113,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-22", footnotes=[Footnote.CAMRY]), - CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-22"), + CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"), CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"), CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), @@ -209,16 +209,19 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST], [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps], bus=0, ), Request( [StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST], [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine], bus=0, ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.abs, Ecu.eps], bus=0, ), ], @@ -548,10 +551,12 @@ FW_VERSIONS = { (Ecu.abs, 0x7b0, None): [ b'F152633D00\x00\x00\x00\x00\x00\x00', b'F152633D60\x00\x00\x00\x00\x00\x00', + b'F152633310\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966306Q7000\x00\x00\x00\x00', + b'\x018966306V1000\x00\x00\x00\x00', b'\x01896633T20000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 15): [ @@ -562,6 +567,7 @@ FW_VERSIONS = { b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.CHR: { @@ -735,6 +741,7 @@ FW_VERSIONS = { b'\x018966312Q8000\x00\x00\x00\x00', b'\x018966312R0000\x00\x00\x00\x00', b'\x018966312R0100\x00\x00\x00\x00', + b'\x018966312R0200\x00\x00\x00\x00', b'\x018966312R1000\x00\x00\x00\x00', b'\x018966312R1100\x00\x00\x00\x00', b'\x018966312R3100\x00\x00\x00\x00', @@ -742,6 +749,7 @@ FW_VERSIONS = { b'\x018966312S7000\x00\x00\x00\x00', b'\x018966312W3000\x00\x00\x00\x00', b'\x018966312W9000\x00\x00\x00\x00', + b'\x01896637644000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0230A10000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -769,6 +777,7 @@ FW_VERSIONS = { b'\x018965B1255000\x00\x00\x00\x00', b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'8965B76012\x00\x00\x00\x00\x00\x00', b'\x018965B12510\x00\x00\x00\x00\x00\x00', b'\x018965B1256000\x00\x00\x00\x00', ], @@ -786,6 +795,7 @@ FW_VERSIONS = { b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', b'\x01F152612B62\x00\x00\x00\x00\x00\x00', + b'\x01F152612B70\x00\x00\x00\x00\x00\x00', b'\x01F152612B71\x00\x00\x00\x00\x00\x00', b'\x01F152612B81\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', @@ -794,6 +804,7 @@ FW_VERSIONS = { b'\x01F152612862\x00\x00\x00\x00\x00\x00', b'\x01F152612B91\x00\x00\x00\x00\x00\x00', b'\x01F15260A070\x00\x00\x00\x00\x00\x00', + b'\x01F152676250\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -821,6 +832,7 @@ FW_VERSIONS = { b'\x01896637621000\x00\x00\x00\x00', b'\x01896637624000\x00\x00\x00\x00', b'\x01896637626000\x00\x00\x00\x00', + b'\x01896637639000\x00\x00\x00\x00', b'\x01896637648000\x00\x00\x00\x00', b'\x01896637643000\x00\x00\x00\x00', b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -1014,6 +1026,7 @@ FW_VERSIONS = { b'\x01F15264873500\x00\x00\x00\x00', b'\x01F152648C6300\x00\x00\x00\x00', b'\x01F152648J4000\x00\x00\x00\x00', + b'\x01F152648J5000\x00\x00\x00\x00', b'\x01F152648J6000\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ @@ -1021,6 +1034,7 @@ FW_VERSIONS = { b'\x01896630EA1000\x00\x00\x00\x00', b'\x01896630EE4000\x00\x00\x00\x00', b'\x01896630EE4100\x00\x00\x00\x00', + b'\x01896630EE5000\x00\x00\x00\x00', b'\x01896630EE6000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -1320,6 +1334,7 @@ FW_VERSIONS = { b'\x01F15260R302\x00\x00\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x01F152642561\x00\x00\x00\x00\x00\x00', + b'\x01F152642601\x00\x00\x00\x00\x00\x00', b'\x01F152642700\x00\x00\x00\x00\x00\x00', b'\x01F152642701\x00\x00\x00\x00\x00\x00', b'\x01F152642710\x00\x00\x00\x00\x00\x00', @@ -1646,6 +1661,7 @@ FW_VERSIONS = { b'\x018966378B2100\x00\x00\x00\x00', b'\x018966378B3000\x00\x00\x00\x00', b'\x018966378G3000\x00\x00\x00\x00', + b'\x018966378B2000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F152678221\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index fff5548671..628962de75 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,6 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from common.numpy_fast import clip from common.conversions import Conversions as CV +from common.realtime import DT_CTRL from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams @@ -88,10 +89,13 @@ class CarController: CS.out.steeringPressed, hud_alert, hud_control)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + lead_distance = 0 + if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor + lead_distance = 512 if CS.upscale_lead_car_signal else 8 acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - hud_control.leadVisible)) + lead_distance)) # **** Stock ACC Button Controls **************************************** # @@ -103,6 +107,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index def14ab019..7ea9aa871b 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -13,6 +13,7 @@ class CarState(CarStateBase): self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False + self.upscale_lead_car_signal = False def create_button_events(self, pt_cp, buttons): button_events = [] @@ -62,7 +63,9 @@ class CarState(CarStateBase): ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0 ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + brake_pedal_pressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"]) + brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + ret.brakePressed = brake_pedal_pressed or brake_pressure_detected ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well # Update gear and/or clutch position data. @@ -139,6 +142,9 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently + self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): @@ -218,12 +224,13 @@ class CarState(CarStateBase): ret.stockAeb = False # Update ACC radar status. - self.acc_type = 0 # TODO: this is ACC "basic" with nonzero min speed, support FtS (1) later + self.acc_type = ext_cp.vl["ACC_System"]["ACS_Typ_ACC"] ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) - ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) + 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) - # TODO: update opendbc with PQ TSK state for OP long accFaulted + 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. @@ -268,8 +275,9 @@ class CarState(CarStateBase): ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval ("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger - ("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed - ("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied + ("ESP_Fahrer_bremst", "ESP_05"), # Driver applied brake pressure over threshold + ("MO_Fahrer_bremst", "Motor_14"), # Brake pedal switch + ("ESP_Bremsdruck", "ESP_05"), # Brake pressure ("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value ("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input ("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign @@ -277,6 +285,7 @@ class CarState(CarStateBase): ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("KBI_Variante", "Kombi_03"), # Digital/full-screen instrument cluster installed ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel @@ -304,9 +313,11 @@ class CarState(CarStateBase): ("ESP_02", 50), # From J104 ABS/ESP controller ("GRA_ACC_01", 33), # From J533 CAN gateway (via LIN from steering wheel controls) ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) + ("Motor_14", 10), # From J623 Engine control module ("Airbag_02", 5), # From J234 Airbag control module ("Kombi_01", 2), # From J285 Instrument cluster ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) + ("Kombi_03", 0), # From J285 instrument cluster (not present on older cars, 1Hz when present) ] if CP.transmissionType == TransmissionType.automatic: @@ -318,7 +329,6 @@ class CarState(CarStateBase): elif CP.transmissionType == TransmissionType.manual: signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM - checks.append(("Motor_14", 10)) # From J623 Engine control module if CP.networkLocation == NetworkLocation.fwdCamera: # Radars are here on CANBUS.pt @@ -482,15 +492,15 @@ class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ ("ACC_Wunschgeschw_02", "ACC_02"), # ACC set speed - ("ACC_Typ", "ACC_06"), # Basic vs F2S vs SNG + ("ACC_Typ", "ACC_06"), # Basic vs FtS vs SnG ("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release ("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release ("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release ] fwd_radar_checks = [ - ("ACC_06", 50), # From J428 ACC radar control module - ("ACC_10", 50), # From J428 ACC radar control module - ("ACC_02", 17), # From J428 ACC radar control module + ("ACC_06", 50), # From J428 ACC radar control module + ("ACC_10", 50), # From J428 ACC radar control module + ("ACC_02", 17), # From J428 ACC radar control module ] bsm_radar_signals = [ ("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left @@ -499,24 +509,26 @@ class MqbExtraSignals: ("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right ] bsm_radar_checks = [ - ("SWA_01", 20), # From J1086 Lane Change Assist + ("SWA_01", 20), # From J1086 Lane Change Assist ] class PqExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACA_StaACC", "ACC_GRA_Anziege", 0), # ACC drivetrain coordinator status - ("ACA_V_Wunsch", "ACC_GRA_Anziege", 0), # ACC set speed + ("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 ] fwd_radar_checks = [ - ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module + ("ACC_System", 50), # From J428 ACC radar control module + ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module ] bsm_radar_signals = [ - ("SWA_Infostufe_SWA_li", "SWA_1", 0), # Blind spot object info, left - ("SWA_Warnung_SWA_li", "SWA_1", 0), # Blind spot object warning, left - ("SWA_Infostufe_SWA_re", "SWA_1", 0), # Blind spot object info, right - ("SWA_Warnung_SWA_re", "SWA_1", 0), # Blind spot object warning, right + ("SWA_Infostufe_SWA_li", "SWA_1"), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_1"), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_1"), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_1"), # Blind spot object warning, right ] bsm_radar_checks = [ - ("SWA_1", 20), # From J1086 Lane Change Assist + ("SWA_1", 20), # From J1086 Lane Change Assist ] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 816e7fcf34..da0ce25afa 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,8 +1,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, \ - gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter @@ -22,8 +21,7 @@ class CarInterface(CarInterfaceBase): self.cp_ext = self.cp_cam @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" ret.radarOffCan = True @@ -74,7 +72,6 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out - tire_stiffness_factor = 1.0 # Let the params learner figure this out ret.lateralTuning.pid.kpBP = [0.] ret.lateralTuning.pid.kiBP = [0.] ret.lateralTuning.pid.kf = 0.00006 @@ -137,7 +134,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.SHARAN_MK2: ret.mass = 1639 + STD_CARGO_KG ret.wheelbase = 2.92 - ret.minEnableSpeed = 30 * CV.KPH_TO_MS ret.minSteerSpeed = 50 * CV.KPH_TO_MS ret.steerActuatorDelay = 0.2 @@ -214,10 +210,7 @@ class CarInterface(CarInterfaceBase): raise ValueError(f"unsupported car {candidate}") ret.autoResumeSng = ret.minEnableSpeed == -1 - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.centerToFront = ret.wheelbase * 0.45 - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, - tire_stiffness_factor=tire_stiffness_factor) return ret # returns a car.CarState @@ -237,7 +230,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.belowSteerSpeed) if self.CS.CP.openpilotLongitudinalControl: - if ret.vEgo < self.CP.minEnableSpeed + 2.: + if ret.vEgo < self.CP.minEnableSpeed + 0.5: events.add(EventName.belowEngageSpeed) if c.enabled and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.speedTooLow) diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 25a710dbb8..30a51f6fe6 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -96,13 +96,13 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, "ACC_Gesetzte_Zeitluecke": 3, "ACC_Display_Prio": 3, - # TODO: ACC_Abstandsindex for lead car distance, must determine analog vs digital cluster for scaling + "ACC_Abstandsindex": lead_distance, } return packer.make_can_msg("ACC_02", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 0bcbf6abb3..130f107950 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -66,6 +66,7 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, "ACS_Sta_ADR": acc_control, "ACS_StSt_Info": acc_control != 1, "ACS_Typ_ACC": acc_type, + "ACS_Anhaltewunsch": acc_type == 1 and stopping, "ACS_Sollbeschl": accel if acc_control == 1 else 3.01, "ACS_zul_Regelabw": 0.2 if acc_control == 1 else 1.27, "ACS_max_AendGrad": 3.0 if acc_control == 1 else 5.08, @@ -76,12 +77,12 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): values = { "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, - "ACA_gemZeitl": 8 if lead_visible else 0, + "ACA_gemZeitl": lead_distance, # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke # display/display-prio handling probably needed to stop confusing the instrument cluster # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index cb2343e08f..208d146e2f 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,5 +1,5 @@ from collections import defaultdict, namedtuple -from dataclasses import dataclass, field +from dataclasses import dataclass from enum import Enum from typing import Dict, List, Union @@ -165,7 +165,9 @@ class Footnote(Enum): class VWCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC) & Lane Assist" harness: Enum = Harness.j533 - footnotes: List[Enum] = field(default_factory=lambda: [Footnote.VW_EXP_LONG]) + + def init_make(self, CP: car.CarParams): + self.footnotes.insert(0, Footnote.VW_EXP_LONG) CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { @@ -197,28 +199,28 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Jetta GLI 2021-22"), ], CAR.PASSAT_MK8: [ - VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.PASSAT]), + VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]), VWCarInfo("Volkswagen Passat Alltrack 2015-22"), VWCarInfo("Volkswagen Passat GTE 2015-22"), ], CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"), CAR.POLO_MK6: [ - VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), - VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_MQB_A0]), + VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_MQB_A0]), ], CAR.SHARAN_MK2: [ VWCarInfo("Volkswagen Sharan 2018-22"), VWCarInfo("SEAT Alhambra 2018-20"), ], CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"), - CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22"), CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ VWCarInfo("Volkswagen Caravelle 2020"), VWCarInfo("Volkswagen California 2021"), ], - CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_MQB_A0]), CAR.AUDI_A3_MK3: [ VWCarInfo("Audi A3 2014-19"), VWCarInfo("Audi A3 Sportback e-tron 2017-18"), @@ -229,10 +231,10 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2019-23"), CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"), CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"), - CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0, Footnote.KAMIQ]), + CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]), CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21"), CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2018-19"), - CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_EXP_LONG, Footnote.VW_MQB_A0]), + CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_MQB_A0]), CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"), CAR.SKODA_OCTAVIA_MK3: [ VWCarInfo("Škoda Octavia 2015, 2018-19"), @@ -278,12 +280,14 @@ FW_VERSIONS = { CAR.ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x873G0906259F \xf1\x890004', + b'\xf1\x873G0906259N \xf1\x890004', b'\xf1\x873G0906259P \xf1\x890001', b'\xf1\x875NA907115H \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158L \xf1\x893611', b'\xf1\x870GC300011L \xf1\x891401', + b'\xf1\x870GC300014M \xf1\x892802', b'\xf1\x870GC300040P \xf1\x891401', ], (Ecu.srs, 0x715, None): [ @@ -295,8 +299,10 @@ FW_VERSIONS = { b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800', + b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002MB4092M7N', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x875Q0907572J \xf1\x890654', ], @@ -314,11 +320,13 @@ FW_VERSIONS = { b'\xf1\x8703H906026J \xf1\x899971', b'\xf1\x8703H906026S \xf1\x896693', b'\xf1\x8703H906026S \xf1\x899970', + b'\xf1\x873CN906259 \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158A \xf1\x893387', b'\xf1\x8709G927158DR\xf1\x893536', b'\xf1\x8709G927158DR\xf1\x893742', + b'\xf1\x8709G927158F \xf1\x893489', b'\xf1\x8709G927158FT\xf1\x893835', b'\xf1\x8709G927158GL\xf1\x893939', ], @@ -332,6 +340,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6080105', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], (Ecu.fwdRadar, 0x757, None): [ @@ -371,6 +380,7 @@ FW_VERSIONS = { b'\xf1\x870EA906016Q \xf1\x895993', b'\xf1\x870EA906016S \xf1\x897207', b'\xf1\x875G0906259 \xf1\x890007', + b'\xf1\x875G0906259D \xf1\x890002', b'\xf1\x875G0906259J \xf1\x890002', b'\xf1\x875G0906259L \xf1\x890002', b'\xf1\x875G0906259N \xf1\x890003', @@ -406,6 +416,7 @@ FW_VERSIONS = { b'\xf1\x870D9300012 \xf1\x895045', b'\xf1\x870D9300014M \xf1\x895004', b'\xf1\x870D9300014Q \xf1\x895006', + b'\xf1\x870D9300020J \xf1\x894902', b'\xf1\x870D9300020Q \xf1\x895201', b'\xf1\x870D9300020S \xf1\x895201', b'\xf1\x870D9300040A \xf1\x893613', @@ -453,6 +464,7 @@ FW_VERSIONS = { b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\x0561A01612A0', b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\x0566A0J612A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A00514A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A01613A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A0J712A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571A0J714A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1', @@ -795,6 +807,7 @@ FW_VERSIONS = { b'\xf1\x875G0906259L \xf1\x890002', b'\xf1\x875G0906259Q \xf1\x890002', b'\xf1\x878V0906259F \xf1\x890002', + b'\xf1\x878V0906259H \xf1\x890002', b'\xf1\x878V0906259J \xf1\x890002', b'\xf1\x878V0906259K \xf1\x890001', b'\xf1\x878V0906264B \xf1\x890003', @@ -805,6 +818,7 @@ FW_VERSIONS = { b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300048 \xf1\x895201', b'\xf1\x870D9300012 \xf1\x894912', + b'\xf1\x870D9300012 \xf1\x894931', b'\xf1\x870D9300012K \xf1\x894513', b'\xf1\x870D9300013B \xf1\x894931', b'\xf1\x870D9300041N \xf1\x894512', @@ -827,12 +841,14 @@ FW_VERSIONS = { b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13111112111111--241115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13111112111111--241115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221', b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112110004110411111421149114', b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566G0HA14A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G01A16A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571G0JA14A1', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a18bec83a8..36f0b559c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -12,7 +12,7 @@ import cereal.messaging as messaging from common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE from system.swaglog import cloudlog -from system.version import is_tested_branch, get_short_branch +from system.version import is_release_branch, get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET @@ -21,7 +21,7 @@ from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI -from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert @@ -132,7 +132,7 @@ class Controls: safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] - if is_tested_branch(): + if is_release_branch(): self.CP.experimentalLongitudinalAvailable = False # Write CarParams for radard @@ -249,6 +249,9 @@ class Controls: (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): self.events.add(EventName.pedalPressed) + if CS.brakePressed and CS.standstill: + self.events.add(EventName.preEnableStandstill) + if CS.gasPressed: self.events.add(EventName.gasPressedOverride) @@ -275,7 +278,7 @@ class Controls: # self.events.add(EventName.highCpuUsage) # Alert if fan isn't spinning for 5 seconds - if self.sm['peripheralState'].pandaType == PandaType.dos: + if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: self.events.add(EventName.fanMalfunction) @@ -417,16 +420,6 @@ class Controls: if self.sm['liveLocationKalman'].excessiveResets: self.events.add(EventName.localizerMalfunction) - # Only allow engagement with brake pressed when stopped behind another stopped car - speeds = self.sm['longitudinalPlan'].speeds - if len(speeds) > 1: - v_future = speeds[-1] - else: - v_future = 100.0 - if CS.brakePressed and v_future >= self.CP.vEgoStarting \ - and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: - self.events.add(EventName.noTarget) - def data_sample(self): """Receive data from sockets and update carState""" @@ -518,10 +511,7 @@ class Controls: # PRE ENABLING elif self.state == State.preEnabled: - if self.events.any(ET.NO_ENTRY): - self.state = State.disabled - self.current_alert_types.append(ET.NO_ENTRY) - elif not self.events.any(ET.PRE_ENABLE): + if not self.events.any(ET.PRE_ENABLE): self.state = State.enabled else: self.current_alert_types.append(ET.PRE_ENABLE) @@ -582,11 +572,16 @@ class Controls: # Check which actuators can be enabled CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ CS.vEgo > self.CP.minSteerSpeed and not CS.standstill - CC.longActive = self.active and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl + CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state + # Enable blinkers while lane changing + if self.sm['lateralPlan'].laneChangeState != LaneChangeState.off: + CC.leftBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.left + CC.rightBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.right + if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame @@ -635,7 +630,7 @@ class Controls: max_torque = abs(self.last_actuators.steer) > 0.99 if undershooting and turning and good_speed and max_torque: self.events.add(EventName.steerSaturated) - elif lac_log.active and not CS.steeringPressed and lac_log.saturated: + elif lac_log.active and lac_log.saturated: dpath_points = lat_plan.dPathPoints if len(dpath_points): # Check if we deviated from the path @@ -732,7 +727,11 @@ class Controls: self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators - self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ + STEER_ANGLE_SATURATION_THRESHOLD + else: + self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index bdbdb7023a..3d5ec8ac1d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -12,6 +12,7 @@ V_CRUISE_MAX = 145 # kph V_CRUISE_MIN = 8 # kph V_CRUISE_ENABLE_MIN = 40 # kph V_CRUISE_INITIAL = 255 # kph +IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 LAT_MPC_N = 16 @@ -73,8 +74,7 @@ class VCruiseHelper: long_press = False button_type = None - # should be CV.MPH_TO_KPH, but this causes rounding errors - v_cruise_delta = 1. if is_metric else 1.6 + v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT for b in CS.buttonEvents: if b.type.raw in self.button_timers and not b.pressed: @@ -149,6 +149,12 @@ def apply_deadzone(error, deadzone): return error +def apply_center_deadzone(error, deadzone): + if (error > - deadzone) and (error < deadzone): + error = 0. + return error + + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index a761cceecb..3036828662 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -250,10 +250,9 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - gps_integrated = sm['peripheralState'].pandaType in (log.PandaState.PandaType.uno, log.PandaState.PandaType.dos) return Alert( "Poor GPS reception", - "Hardware malfunctioning if sky is visible" if gps_integrated else "Check GPS antenna placement", + "Hardware malfunctioning if sky is visible", AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.) @@ -501,9 +500,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.resumeRequired: { ET.WARNING: Alert( - "STOPPED", - "Press Resume to Go", - AlertStatus.userPrompt, AlertSize.mid, + "Press Resume to Exit Standstill", + "", + AlertStatus.userPrompt, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), }, @@ -616,9 +615,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { visual_alert=VisualAlert.brakePressed), }, - EventName.pedalPressedPreEnable: { + EventName.preEnableStandstill: { ET.PRE_ENABLE: Alert( - "Release Pedal to Engage", + "Release Brake to Engage", "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), @@ -807,9 +806,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.accFaulted: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Faulted"), - ET.PERMANENT: NormalPermanentAlert("Cruise Faulted", ""), - ET.NO_ENTRY: NoEntryAlert("Cruise Faulted"), + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), + ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), }, EventName.accFaultedTemp: { @@ -917,15 +916,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"), }, - EventName.noTarget: { - ET.IMMEDIATE_DISABLE: Alert( - "openpilot Canceled", - "No close lead car", - AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), - ET.NO_ENTRY: NoEntryAlert("No Close Lead Car"), - }, - EventName.speedTooLow: { ET.IMMEDIATE_DISABLE: Alert( "openpilot Canceled", diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 78b59fda59..cefbc1078c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -11,6 +11,7 @@ class LatControl(ABC): self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.sat_count = 0. + self.sat_check_min_speed = 10. # we define the steer torque scale as [-1.0...1.0] self.steer_max = 1.0 @@ -23,7 +24,7 @@ class LatControl(ABC): self.sat_count = 0. def _check_saturation(self, saturated, CS, steer_limited): - if saturated and CS.vEgo > 10. and not steer_limited and not CS.steeringPressed: + if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 0e5be4a977..d692f80b4b 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,6 +7,10 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.sat_check_min_speed = 5. + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -19,7 +23,7 @@ class LatControlAngle(LatControl): angle_steers_des += params.angleOffsetDeg angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited) + angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 92a4f1f99b..545a4c43ff 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -10,12 +10,14 @@ LongCtrlState = car.CarControl.Actuators.LongControlState def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_1sec, brake_pressed, cruise_standstill): + # Ignore cruise standstill if car has a gas interceptor + cruise_standstill = cruise_standstill and not CP.enableGasInterceptor accelerating = v_target_1sec > v_target planned_stop = (v_target < CP.vEgoStopping and v_target_1sec < CP.vEgoStopping and not accelerating) stay_stopped = (v_ego < CP.vEgoStopping and - (brake_pressed or cruise_standstill)) + (brake_pressed or cruise_standstill)) stopping_condition = planned_stop or stay_stopped starting_condition = (v_target_1sec > CP.vEgoStarting and diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 6b79813117..79a9ec4f0c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -352,7 +352,7 @@ class LongitudinalMpc: x_and_cruise = np.column_stack([x, cruise_target]) x = np.min(x_and_cruise, axis=1) - self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise' + self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise' else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index def0a1208a..a0f6318323 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -6,7 +6,6 @@ from common.numpy_fast import clip, interp import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter -from common.params import Params from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longcontrol import LongCtrlState @@ -48,12 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.params = Params() - self.param_read_counter = 0 - self.mpc = LongitudinalMpc() - self.read_param() - self.fcw = False self.a_desired = init_a @@ -65,10 +59,6 @@ class LongitudinalPlanner: self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - def read_param(self): - e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl - self.mpc.mode = 'blended' if e2e else 'acc' - @staticmethod def parse_model(model_msg, model_error): if (len(model_msg.position.x) == 33 and @@ -85,10 +75,8 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm, read=True): - if self.param_read_counter % 50 == 0 and read: - self.read_param() - self.param_read_counter += 1 + def update(self, sm): + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index 3d6f55931e..cd1d31cf07 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -3,17 +3,16 @@ import numpy as np from parameterized import parameterized_class import unittest -from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN +from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT from cereal import car from common.conversions import Conversions as CV -from common.params import Params from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type -def run_cruise_simulation(cruise, t_end=20.): +def run_cruise_simulation(cruise, e2e, t_end=20.): man = Maneuver( '', duration=t_end, @@ -23,6 +22,7 @@ def run_cruise_simulation(cruise, t_end=20.): cruise_values=[cruise], prob_lead_values=[0.0], breakpoints=[0.], + e2e=e2e, ) valid, output = man.evaluate() assert valid @@ -31,14 +31,12 @@ def run_cruise_simulation(cruise, t_end=20.): class TestCruiseSpeed(unittest.TestCase): def test_cruise_speed(self): - params = Params() for e2e in [False, True]: - params.put_bool("ExperimentalMode", e2e) for speed in np.arange(5, 40, 5): print(f'Testing {speed} m/s') cruise_speed = float(speed) - simulation_steady_state = run_cruise_simulation(cruise_speed) + simulation_steady_state = run_cruise_simulation(cruise_speed, e2e) self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s') @@ -110,6 +108,30 @@ class TestVCruiseHelper(unittest.TestCase): should_equal = standstill or pressed self.assertEqual(should_equal, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) + def test_set_gas_pressed(self): + """ + Asserts pressing set while enabled with gas pressed sets + the speed to the maximum of vEgo and current cruise speed. + """ + + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + + # first decrement speed, then perform gas pressed logic + expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT + expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH) # clip to min of vEgo + expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)) + + CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + + # TODO: fix skipping first run due to enabled on rising edge exception + if v_ego == 0.0: + continue + self.assertEqual(expected_v_cruise_kph, self.v_cruise_helper.v_cruise_kph) + def test_initialize_v_cruise(self): """ Asserts allowed cruise speeds on enabling with SET. diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 0535caab84..5185867d2d 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 import unittest import numpy as np -from common.params import Params - from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): man = Maneuver( '', @@ -16,7 +15,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): initial_distance_lead=100, speed_lead_values=[v_lead], breakpoints=[0.], - e2e=e2e + e2e=e2e, ) valid, output = man.evaluate() assert valid @@ -25,13 +24,11 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): class TestFollowingDistance(unittest.TestCase): def test_following_distance(self): - params = Params() for e2e in [False, True]: - params.put_bool("ExperimentalMode", e2e) for speed in np.arange(0, 40, 5): print(f'Testing {speed} m/s') v_lead = float(speed) - simulation_steady_state = run_following_distance_simulation(v_lead) + simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e) correct_steady_state = desired_follow_distance(v_lead, v_lead) err_ratio = 0.2 if e2e else 0.1 self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py index 36535dfdaf..8f263a98e7 100755 --- a/selfdrive/controls/tests/test_state_machine.py +++ b/selfdrive/controls/tests/test_state_machine.py @@ -79,7 +79,7 @@ class TestStateMachine(unittest.TestCase): self.assertEqual(self.controlsd.state, State.disabled) def test_no_entry(self): - # disabled with enable events + # Make sure noEntry keeps us disabled for et in ENABLE_EVENT_TYPES: self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) self.controlsd.state_transition(self.CS) @@ -87,11 +87,11 @@ class TestStateMachine(unittest.TestCase): self.controlsd.events.clear() def test_no_entry_pre_enable(self): - # preEnabled with preEnabled event + # preEnabled with noEntry event self.controlsd.state = State.preEnabled self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) self.controlsd.state_transition(self.CS) - self.assertEqual(self.controlsd.state, State.disabled) + self.assertEqual(self.controlsd.state, State.preEnabled) def test_maintain_states(self): # Given current state's event type, we should maintain state diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 595e25e8c3..ba7d96dba0 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -68,7 +68,7 @@ if __name__ == "__main__": CP = None for msg in lr: if msg.which() == "pandaStates": - if msg.pandaStates[0].pandaType not in ('uno', 'blackPanda', 'dos'): + if msg.pandaStates[0].pandaType in ('unknown', 'whitePanda', 'greyPanda', 'pedal'): print("wrong panda type") break diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index dbccb8d4a9..c03e995497 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -24,6 +24,7 @@ procs = [ NativeProcess("logcatd", "system/logcatd", ["./logcatd"]), NativeProcess("proclogd", "system/proclogd", ["./proclogd"]), PythonProcess("logmessaged", "system.logmessaged", offroad=True), + PythonProcess("micd", "system.micd"), PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True), DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), @@ -31,7 +32,8 @@ procs = [ NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), - # NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"]), + NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False), + NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index f1a8d71881..82338e456b 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -112,3 +112,8 @@ llenv.Program('_modeld', [ "modeld.cc", "models/driving.cc", ]+common_model, LIBS=libs + transformations) + +lenv.Program('_navmodeld', [ + "navmodeld.cc", + "models/nav.cc", + ]+common_model, LIBS=libs + transformations) \ No newline at end of file diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index cfc71a0e27..0a8a8d6358 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -15,6 +15,7 @@ #include "common/util.h" #include "system/hardware/hw.h" #include "selfdrive/modeld/models/driving.h" +#include "selfdrive/modeld/models/nav.h" ExitHandler do_exit; @@ -72,6 +73,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl mat3 model_transform_main = {}; mat3 model_transform_extra = {}; bool live_calib_seen = false; + float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}; + float nav_features[NAV_FEATURE_LEN] = {0}; VisionBuf *buf_main = nullptr; VisionBuf *buf_extra = nullptr; @@ -151,7 +154,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } double mt1 = millis_since_boot(); - ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only); + ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only); double mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 40c82a8c21..1a079da055 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -13,6 +13,7 @@ #endif #include "common/mat.h" +#include "cereal/messaging/messaging.h" #include "selfdrive/modeld/transforms/loadyuv.h" #include "selfdrive/modeld/transforms/transform.h" @@ -21,6 +22,11 @@ const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; void softmax(const float* input, float* output, size_t len); float sigmoid(float input); +template +constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { + return kj::ArrayPtr(arr.data(), arr.size()); +} + class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 4015731c42..ac101bfee7 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -22,11 +22,6 @@ std::array prev_brake_3ms2_probs = {0,0,0}; // #define DUMP_YUV -template -constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { - return kj::ArrayPtr(arr.data(), arr.size()); -} - void model_init(ModelState* s, cl_device_id device_id, cl_context context) { s->frame = new ModelFrame(device_id, context); s->wide_frame = new ModelFrame(device_id, context); @@ -51,10 +46,19 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #ifdef TRAFFIC_CONVENTION s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif + +#ifdef DRIVING_STYLE + s->m->addDrivingStyle(s->driving_style, DRIVING_STYLE_LEN); +#endif + +#ifdef NAV + s->m->addNavFeatures(s->nav_features, NAV_FEATURE_LEN); +#endif + } ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) { #ifdef DESIRE std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN); if (desire_in != NULL) { @@ -72,6 +76,14 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, LOGT("Desire enqueued"); #endif +#ifdef NAV + std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN); +#endif + +#ifdef DRIVING_STYLE + std::memcpy(s->driving_style, driving_style, sizeof(float)*DRIVING_STYLE_LEN); +#endif + int rhd_idx = is_rhd; s->traffic_convention[rhd_idx] = 1.0; s->traffic_convention[1-rhd_idx] = 0.0; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index b23691a56a..5734c68cb9 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -14,6 +14,7 @@ #include "common/modeldata.h" #include "common/util.h" #include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/models/nav.h" #include "selfdrive/modeld/runners/run.h" constexpr int FEATURE_LEN = 128; @@ -21,6 +22,7 @@ constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int DRIVING_STYLE_LEN = 12; constexpr int MODEL_FREQ = 20; constexpr int DISENGAGE_LEN = 5; @@ -259,11 +261,17 @@ struct ModelState { #ifdef TRAFFIC_CONVENTION float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; #endif +#ifdef DRIVING_STYLE + float driving_style[DRIVING_STYLE_LEN] = {}; +#endif +#ifdef NAV + float nav_features[NAV_FEATURE_LEN] = {}; +#endif }; void model_init(ModelState* s, cl_device_id device_id, cl_context context); ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only); + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); void model_free(ModelState* s); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, const ModelOutput &net_outputs, uint64_t timestamp_eof, diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc new file mode 100644 index 0000000000..861795e172 --- /dev/null +++ b/selfdrive/modeld/models/nav.cc @@ -0,0 +1,66 @@ +#include "selfdrive/modeld/models/nav.h" + +#include +#include + +#include "common/mat.h" +#include "common/modeldata.h" +#include "common/timing.h" + + +void navmodel_init(NavModelState* s) { + #ifdef USE_ONNX_MODEL + s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + #else + s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + #endif +} + +NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) { + memcpy(s->net_input_buf, buf->addr, NAV_INPUT_SIZE); + + double t1 = millis_since_boot(); + s->m->addImage((float*)s->net_input_buf, NAV_INPUT_SIZE/sizeof(float)); + s->m->execute(); + double t2 = millis_since_boot(); + + NavModelResult *model_res = (NavModelResult*)&s->output; + model_res->dsp_execution_time = (t2 - t1) / 1000.; + return model_res; +} + +void fill_plan(cereal::NavModelData::Builder &framed, const NavModelOutputPlan &plan) { + std::array pos_x, pos_y; + std::array pos_x_std, pos_y_std; + + for (int i=0; im; +} diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h new file mode 100644 index 0000000000..b2955ad2c5 --- /dev/null +++ b/selfdrive/modeld/models/nav.h @@ -0,0 +1,56 @@ +#pragma once + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_client.h" +#include "common/util.h" +#include "common/modeldata.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" + +constexpr int NAV_INPUT_SIZE = 256*256; +constexpr int NAV_FEATURE_LEN = 64; +constexpr int NAV_DESIRE_LEN = 32; + +struct NavModelOutputXY { + float x; + float y; +}; +static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2); + +struct NavModelOutputPlan { + std::array mean; + std::array std; +}; +static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2); + +struct NavModelOutputDesirePrediction { + std::array values; +}; +static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN); + +struct NavModelOutputFeatures { + std::array values; +}; +static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN); + +struct NavModelResult { + const NavModelOutputPlan plan; + const NavModelOutputDesirePrediction desire_pred; + const NavModelOutputFeatures features; + float dsp_execution_time; +}; +static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float)); + +constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float); +constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1; + +struct NavModelState { + RunModel *m; + uint8_t net_input_buf[NAV_INPUT_SIZE]; + float output[NAV_OUTPUT_SIZE]; +}; + +void navmodel_init(NavModelState* s); +NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf); +void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time); +void navmodel_free(NavModelState* s); diff --git a/selfdrive/modeld/models/navmodel.onnx b/selfdrive/modeld/models/navmodel.onnx new file mode 100644 index 0000000000..397b8d90e2 --- /dev/null +++ b/selfdrive/modeld/models/navmodel.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9028b36a591763724e95205b48f37f09260b4434fb1f3c6f228db1710a401aa8 +size 12258591 diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc new file mode 100644 index 0000000000..984cf0a553 --- /dev/null +++ b/selfdrive/modeld/models/navmodel_q.dlc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc7ade56bb4f9525c84a46df22252ea1e321a0518cbcef8bdfc76ccd8ad10b41 +size 3154304 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index c0db988cf6..8805b3dce8 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:c73998c9f428380dd2282b451de6011469b717498ae83578cbf7aa95948910f7 +oid sha256:db746e3753de84367595fedd089c2bd41b06bd401ea28e085663533d0e63d74b size 45962473 diff --git a/selfdrive/modeld/navmodeld b/selfdrive/modeld/navmodeld new file mode 100755 index 0000000000..079afd9677 --- /dev/null +++ b/selfdrive/modeld/navmodeld @@ -0,0 +1,12 @@ +#!/bin/sh + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +if [ -f /TICI ]; then + export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH" + export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/" +else + export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH" +fi +exec ./_navmodeld diff --git a/selfdrive/modeld/navmodeld.cc b/selfdrive/modeld/navmodeld.cc new file mode 100644 index 0000000000..75f2c62ab3 --- /dev/null +++ b/selfdrive/modeld/navmodeld.cc @@ -0,0 +1,59 @@ +#include +#include + +#include +#include + +#include "cereal/visionipc/visionipc_client.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "selfdrive/modeld/models/nav.h" + +ExitHandler do_exit; + +void run_model(NavModelState &model, VisionIpcClient &vipc_client) { + PubMaster pm({"navModel"}); + + double last_ts = 0; + uint32_t last_frame_id = 0; + VisionIpcBufExtra extra = {}; + + while (!do_exit) { + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; + + double t1 = millis_since_boot(); + NavModelResult *model_res = navmodel_eval_frame(&model, buf); + double t2 = millis_since_boot(); + + // send navmodel packet + navmodel_publish(pm, extra.frame_id, *model_res, (t2 - t1) / 1000.0); + + //printf("navmodel process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last_ts); + last_ts = t1; + last_frame_id = extra.frame_id; + } +} + +int main(int argc, char **argv) { + setpriority(PRIO_PROCESS, 0, -15); + + // init the models + NavModelState model; + navmodel_init(&model); + LOGW("models loaded, navmodeld starting"); + + VisionIpcClient vipc_client = VisionIpcClient("navd", VISION_STREAM_MAP, true); + while (!do_exit && !vipc_client.connect(false)) { + util::sleep_for(100); + } + + // run the models + if (vipc_client.connected) { + LOGW("connected with buffer size: %d", vipc_client.buffers[0].len); + run_model(model, vipc_client); + } + + navmodel_free(&model); + return 0; +} diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 447d90fd7e..5616a6c9c7 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -97,6 +97,16 @@ void ONNXModel::addDesire(float *state, int state_size) { desire_state_size = state_size; } +void ONNXModel::addNavFeatures(float *state, int state_size) { + nav_features_input_buf = state; + nav_features_size = state_size; +} + +void ONNXModel::addDrivingStyle(float *state, int state_size) { + driving_style_input_buf = state; + driving_style_size = state_size; +} + void ONNXModel::addTrafficConvention(float *state, int state_size) { traffic_convention_input_buf = state; traffic_convention_size = state_size; @@ -128,7 +138,13 @@ void ONNXModel::execute() { if (desire_input_buf != NULL) { pwrite(desire_input_buf, desire_state_size); } - if (traffic_convention_input_buf != NULL) { + if (nav_features_input_buf != NULL) { + pwrite(nav_features_input_buf, nav_features_size); + } + if (driving_style_input_buf != NULL) { + pwrite(driving_style_input_buf, driving_style_size); + } + if (traffic_convention_input_buf != NULL) { pwrite(traffic_convention_input_buf, traffic_convention_size); } if (calib_input_buf != NULL) { diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index d5b7bfecf0..9990bf1b45 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -10,6 +10,8 @@ public: ~ONNXModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); + void addNavFeatures(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); void addImage(float *image_buf, int buf_size); @@ -25,6 +27,10 @@ private: int rnn_state_size; float *desire_input_buf = NULL; int desire_state_size; + float *nav_features_input_buf = NULL; + int nav_features_size; + float *driving_style_input_buf = NULL; + int driving_style_size; float *traffic_convention_input_buf = NULL; int traffic_convention_size; float *calib_input_buf = NULL; diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index c607811401..673ddb50b5 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -5,6 +5,8 @@ public: virtual ~RunModel() {} virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} + virtual void addNavFeatures(float *state, int state_size) {} + virtual void addDrivingStyle(float *state, int state_size) {} virtual void addTrafficConvention(float *state, int state_size) {} virtual void addCalib(float *state, int state_size) {} virtual void addImage(float *image_buf, int buf_size) {} diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index ff4adcd8d3..609a7a6657 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -153,6 +153,16 @@ void SNPEModel::addDesire(float *state, int state_size) { desireBuffer = this->addExtra(state, state_size, 1); } +void SNPEModel::addNavFeatures(float *state, int state_size) { + navFeatures = state; + navFeaturesBuffer = this->addExtra(state, state_size, 1); +} + +void SNPEModel::addDrivingStyle(float *state, int state_size) { + drivingStyle = state; + drivingStyleBuffer = this->addExtra(state, state_size, 2); +} + void SNPEModel::addCalib(float *state, int state_size) { calib = state; calibBuffer = this->addExtra(state, state_size, 1); diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 08ae16c2b1..0d84d1d482 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -28,6 +28,8 @@ public: void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); void addDesire(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); + void addNavFeatures(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -75,6 +77,10 @@ private: std::unique_ptr trafficConventionBuffer; float *desire; std::unique_ptr desireBuffer; + float *navFeatures; + std::unique_ptr navFeaturesBuffer; + float *drivingStyle; + std::unique_ptr drivingStyleBuffer; float *calib; std::unique_ptr calibBuffer; }; diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc index 67db01bb95..4fdd7ca466 100644 --- a/selfdrive/modeld/runners/thneedmodel.cc +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -24,6 +24,14 @@ void ThneedModel::addDesire(float *state, int state_size) { desire = state; } +void ThneedModel::addDrivingStyle(float *state, int state_size) { + drivingStyle = state; +} + +void ThneedModel::addNavFeatures(float *state, int state_size) { + navFeatures = state; +} + void ThneedModel::addImage(float *image_input_buf, int buf_size) { input = image_input_buf; } diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index f3f34dc7f4..63712f1d00 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -9,6 +9,8 @@ public: void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); + void addNavFeatures(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -26,6 +28,8 @@ private: // recurrent and desire float *recurrent; float *trafficConvention; + float *drivingStyle; float *desire; + float *navFeatures; }; diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index e3f6a5094d..a2cddc2462 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -156,6 +156,11 @@ class DriverStatus(): self._set_timers(active_monitoring=True) + def _reset_awareness(self): + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + def _set_timers(self, active_monitoring): if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: if active_monitoring: @@ -289,17 +294,17 @@ class DriverStatus(): self.hi_stds = 0 def update_events(self, events, driver_engaged, ctrl_active, standstill): - if (driver_engaged and self.awareness > 0) or not ctrl_active: - # reset only when on disengagement if red reached - self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. + if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or not ctrl_active: # reset only when on disengagement if red reached + self._reset_awareness() return driver_attentive = self.driver_distraction_filter.x < 0.37 awareness_prev = self.awareness if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): + if driver_engaged: + self._reset_awareness() + return # only restore awareness when paying attention and alert is not red self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) if self.awareness == 1.: diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 43b5e7747e..f72b4a3aaa 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -101,11 +101,12 @@ class TestMonitoring(unittest.TestCase): ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0], EventName.driverUnresponsive) # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel - # - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch + # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention def test_normal_driver(self): ds_vector = [msg_DISTRACTED] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ [msg_ATTENTIVE] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ - [msg_DISTRACTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON)) + [msg_DISTRACTED] * int((DISTRACTED_SECONDS_TO_ORANGE+2)/DT_DMON) + \ + [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) @@ -113,7 +114,8 @@ class TestMonitoring(unittest.TestCase): self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) self.assertEqual(len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]), 0) self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) - self.assertEqual(len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)]), 0) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]), 0) # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # driver dodges, and then touches wheel to no avail, disengages and reengages diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript index b10684eef3..8a2c2a8a91 100644 --- a/selfdrive/navd/SConscript +++ b/selfdrive/navd/SConscript @@ -18,5 +18,4 @@ if arch in ['larch64', 'x86_64']: nav_src = ["main.cc", "map_renderer.cc"] qt_env.Program("map_renderer", nav_src, LIBS=qt_libs + ['common', 'json11']) - if GetOption('extras'): - qt_env.SharedLibrary("map_renderer", ["map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) + qt_env.SharedLibrary("map_renderer", ["map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index daf89b2636..91f31d06dc 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -4,25 +4,21 @@ #include #include #include -#include #include "common/util.h" #include "common/timing.h" +#include "common/swaglog.h" #include "selfdrive/ui/qt/maps/map_helpers.h" const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear -const int RENDER_HEIGHT = 512, RENDER_WIDTH = 512; -const int HEIGHT = 256, WIDTH = 256; +const int HEIGHT = 512, WIDTH = 512; const int NUM_VIPC_BUFFERS = 4; const int EARTH_CIRCUMFERENCE_METERS = 40075000; const int PIXELS_PER_TILE = 256; -float get_meters_per_pixel(float lat, float zoom) { - float num_tiles = pow(2, zoom+1); - float meters_per_tile = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / num_tiles; - return meters_per_tile / PIXELS_PER_TILE; -} +const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); +const int LLK_DECIMATION = TEST_MODE ? 1 : 10; float get_zoom_level_for_scale(float lat, float meters_per_pixel) { float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE; @@ -30,14 +26,6 @@ float get_zoom_level_for_scale(float lat, float meters_per_pixel) { return log2(num_tiles) - 1; } -void downsample(uint8_t *src, uint8_t *dst) { - for (int r = 0; r < HEIGHT; r++) { - for (int c = 0; c < WIDTH; c++) { - dst[r*WIDTH + c] = src[(r*2*RENDER_WIDTH + c*2) * 3]; - } - } -} - MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) { QSurfaceFormat fmt; @@ -59,7 +47,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set gl_functions->initializeOpenGLFunctions(); QOpenGLFramebufferObjectFormat fbo_format; - fbo.reset(new QOpenGLFramebufferObject(RENDER_WIDTH, RENDER_HEIGHT, fbo_format)); + fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); std::string style = util::read_file(STYLE_PATH); m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); @@ -69,24 +57,34 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set m_map->resize(fbo->size()); m_map->setFramebufferObject(fbo->handle(), fbo->size()); - gl_functions->glViewport(0, 0, RENDER_WIDTH, RENDER_HEIGHT); + gl_functions->glViewport(0, 0, WIDTH, HEIGHT); + + QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) { + // https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116 + //LOGD("new state %d", change); + }); + + QObject::connect(m_map.data(), &QMapboxGL::mapLoadingFailed, [=](QMapboxGL::MapLoadingFailure err_code, const QString &reason) { + LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str()); + }); if (online) { vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2); vipc_server->start_listener(); - pm.reset(new PubMaster({"navThumbnail"})); - sm.reset(new SubMaster({"liveLocationKalman", "navRoute"})); + pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); + sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"})); timer = new QTimer(this); + timer->setSingleShot(true); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); - timer->start(50); + timer->start(0); } } void MapRenderer::msgUpdate() { - sm->update(0); + sm->update(1000); if (sm->updated("liveLocationKalman")) { auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); @@ -94,8 +92,21 @@ void MapRenderer::msgUpdate() { auto orientation = location.getCalibratedOrientationNED(); bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); - if (localizer_valid) { + if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2])); + + // TODO: use the static rendering mode + if (!loaded() && frame_id > 0) { + for (int i = 0; i < 5 && !loaded(); i++) { + LOGW("map render retry #%d, %d", i+1, m_map.isNull()); + QApplication::processEvents(QEventLoop::AllEvents, 100); + update(); + } + + if (!loaded()) { + LOGE("failed to render map after retry"); + } + } } } @@ -107,6 +118,9 @@ void MapRenderer::msgUpdate() { } updateRoute(route); } + + // schedule next update + timer->start(0); } void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { @@ -114,9 +128,9 @@ void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { return; } - // Choose a zoom level that matches the scale of zoom level 13 at latitude 80deg - float scale_lat80 = get_meters_per_pixel(80, 13); - float zoom = get_zoom_level_for_scale(position.first, scale_lat80); + // Choose a scale that ensures above 13 zoom level up to and above 75deg of lat + float meters_per_pixel = 2; + float zoom = get_zoom_level_for_scale(position.first, meters_per_pixel); m_map->setCoordinate(position); m_map->setBearing(bearing); @@ -129,38 +143,56 @@ bool MapRenderer::loaded() { } void MapRenderer::update() { + double start_t = millis_since_boot(); gl_functions->glClear(GL_COLOR_BUFFER_BIT); m_map->render(); gl_functions->glFlush(); + double end_t = millis_since_boot(); - sendVipc(); + if ((vipc_server != nullptr) && loaded()) { + publish((end_t - start_t) / 1000.0); + } } -void MapRenderer::sendVipc() { - if (!vipc_server || !loaded()) { - return; - } +void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array &buf) { + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initNavThumbnail(); + thumbnaild.setFrameId(frame_id); + thumbnaild.setTimestampEof(ts); + thumbnaild.setThumbnail(buf); + pm->send("navThumbnail", msg); +} +void MapRenderer::publish(const double render_time) { QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); uint64_t ts = nanos_since_boot(); VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); VisionIpcBufExtra extra = { .frame_id = frame_id, - .timestamp_sof = ts, + .timestamp_sof = sm->rcv_time("liveLocationKalman"), .timestamp_eof = ts, }; - assert(cap.sizeInBytes() >= buf->len*4); + assert(cap.sizeInBytes() >= buf->len); uint8_t* dst = (uint8_t*)buf->addr; uint8_t* src = cap.bits(); - // 2x downsample + rgb to grayscale + // RGB to greyscale and crop memset(dst, 128, buf->len); - downsample(src, dst); + for (int r = 0; r < HEIGHT/2; r++) { + for (int c = 0; c < WIDTH/2; c++) { + dst[r*WIDTH/2 + c] = src[((HEIGHT/4 + r)*WIDTH + (c+WIDTH/4)) * 3]; + } + } vipc_server->send(buf, &extra); - if (frame_id % 100 == 0) { + // Send thumbnail + if (TEST_MODE) { + // Full image in thumbnails in test mode + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)cap.bits(), cap.sizeInBytes()); + sendThumbnail(ts, buffer_kj); + } else if (frame_id % 100 == 0) { // Write jpeg into buffer QByteArray buffer_bytes; QBuffer buffer(&buffer_bytes); @@ -168,16 +200,17 @@ void MapRenderer::sendVipc() { cap.save(&buffer, "JPG", 50); kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); - - // Send thumbnail - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initNavThumbnail(); - thumbnaild.setFrameId(frame_id); - thumbnaild.setTimestampEof(ts); - thumbnaild.setThumbnail(buffer_kj); - pm->send("navThumbnail", msg); + sendThumbnail(ts, buffer_kj); } + // Send state msg + MessageBuilder msg; + auto state = msg.initEvent().initMapRenderState(); + state.setLocationMonoTime(sm->rcv_time("liveLocationKalman")); + state.setRenderTime(render_time); + state.setFrameId(frame_id); + pm->send("mapRenderState", msg); + frame_id++; } @@ -187,8 +220,10 @@ uint8_t* MapRenderer::getImage() { uint8_t* src = cap.bits(); uint8_t* dst = new uint8_t[WIDTH * HEIGHT]; - // 2x downsample + rgb to grayscale - downsample(src, dst); + // RGB to greyscale + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } return dst; } diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index 855dc91894..0019030b6d 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -25,7 +25,6 @@ public: bool loaded(); ~MapRenderer(); - private: std::unique_ptr ctx; std::unique_ptr surface; @@ -35,7 +34,8 @@ private: std::unique_ptr vipc_server; std::unique_ptr pm; std::unique_ptr sm; - void sendVipc(); + void publish(const double render_time); + void sendThumbnail(const uint64_t ts, const kj::Array &buf); QMapboxGLSettings m_settings; QScopedPointer m_map; diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py index 9000622928..868307bb63 100755 --- a/selfdrive/navd/map_renderer.py +++ b/selfdrive/navd/map_renderer.py @@ -9,7 +9,8 @@ from cffi import FFI from common.ffi_wrapper import suffix from common.basedir import BASEDIR -HEIGHT = WIDTH = 256 +HEIGHT = WIDTH = SIZE = 512 +METERS_PER_PIXEL = 2 def get_ffi(): diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index 78b3ac526d..78f5d64e79 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -9,20 +9,8 @@ BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Accel::init() { - int ret = 0; - uint8_t buffer[1]; - - ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != BMX055_ACCEL_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); - ret = -1; - goto fail; - } + int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}); + if (ret == -1) return -1; ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE); if (ret < 0) { diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 9d70b9e431..2a938044f3 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -12,20 +12,8 @@ BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Gyro::init() { - int ret = 0; - uint8_t buffer[1]; - - ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != BMX055_GYRO_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID); - ret = -1; - goto fail; - } + int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID}); + if (ret == -1) return -1; ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE); if (ret < 0) { diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 394b1e83d3..2f0d10412c 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -66,8 +66,6 @@ static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Magn::init() { - int ret; - uint8_t buffer[1]; uint8_t trim_x1y1[2] = {0}; uint8_t trim_x2y2[2] = {0}; uint8_t trim_xy1xy2[2] = {0}; @@ -78,25 +76,18 @@ int BMX055_Magn::init() { uint8_t trim_xyz1[2] = {0}; // suspend -> sleep - ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); if(ret < 0) { LOGE("Enabling power failed: %d", ret); goto fail; } util::sleep_for(5); // wait until the chip is powered on - // read chip ID - ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); + ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID}); + if (ret == -1) { goto fail; } - if(buffer[0] != BMX055_MAGN_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID); - return -1; - } - // Load magnetometer trim ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2); if(ret < 0) goto fail; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc index bdb34f1508..95b8068ac1 100644 --- a/selfdrive/sensord/sensors/bmx055_temp.cc +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -9,23 +9,7 @@ BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Temp::init() { - int ret = 0; - uint8_t buffer[1]; - - ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != BMX055_ACCEL_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); - ret = -1; - goto fail; - } - -fail: - return ret; + return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0; } bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index 0de2a98738..08ca6f09cd 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -2,12 +2,12 @@ #include #include - #include "cereal/gen/cpp/log.capnp.h" #include "common/i2c.h" #include "common/gpio.h" +#include "common/swaglog.h" #include "selfdrive/sensord/sensors/constants.h" #include "selfdrive/sensord/sensors/sensor.h" @@ -33,4 +33,18 @@ public: virtual int init() = 0; virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; virtual int shutdown() = 0; + + int verify_chip_id(uint8_t address, const std::vector &expected_ids) { + uint8_t chip_id = 0; + int ret = read_register(address, &chip_id, 1); + if (ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + return -1; + } + for (int i = 0; i < expected_ids.size(); ++i) { + if (chip_id == expected_ids[i]) return chip_id; + } + LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]); + return -1; + } }; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index c19e3de7ed..c8eeb6e5dc 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -118,8 +118,6 @@ int LSM6DS3_Accel::self_test(int test_type) { } int LSM6DS3_Accel::init() { - int ret = 0; - uint8_t buffer[1]; uint8_t value = 0; bool do_self_test = false; @@ -128,19 +126,10 @@ int LSM6DS3_Accel::init() { do_self_test = true; } - ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID && buffer[0] != LSM6DS3TRC_ACCEL_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID); - ret = -1; - goto fail; - } + int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID}); + if (ret == -1) return -1; - if (buffer[0] == LSM6DS3TRC_ACCEL_CHIP_ID) { + if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index f306be0fe8..d9f5b4cb6a 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -107,8 +107,6 @@ int LSM6DS3_Gyro::self_test(int test_type) { } int LSM6DS3_Gyro::init() { - int ret = 0; - uint8_t buffer[1]; uint8_t value = 0; bool do_self_test = false; @@ -117,19 +115,10 @@ int LSM6DS3_Gyro::init() { do_self_test = true; } - ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != LSM6DS3_GYRO_CHIP_ID && buffer[0] != LSM6DS3TRC_GYRO_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID); - ret = -1; - goto fail; - } + int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID}); + if (ret == -1) return -1; - if (buffer[0] == LSM6DS3TRC_GYRO_CHIP_ID) { + if (ret == LSM6DS3TRC_GYRO_CHIP_ID) { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc index 7e1c4d4c81..c2e2c83c1d 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_temp.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -8,27 +8,13 @@ LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} int LSM6DS3_Temp::init() { - int ret = 0; - uint8_t buffer[1]; + int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID}); + if (ret == -1) return -1; - ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != LSM6DS3_TEMP_CHIP_ID && buffer[0] != LSM6DS3TRC_TEMP_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID); - ret = -1; - goto fail; - } - - if (buffer[0] == LSM6DS3TRC_TEMP_CHIP_ID) { + if (ret == LSM6DS3TRC_TEMP_CHIP_ID) { source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; } - -fail: - return ret; + return 0; } bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc index 7a9b7a298b..cdb82a4a12 100644 --- a/selfdrive/sensord/sensors/mmc5603nj_magn.cc +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -8,20 +8,8 @@ MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} int MMC5603NJ_Magn::init() { - int ret = 0; - uint8_t buffer[1]; - - ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1); - if(ret < 0) { - LOGE("Reading chip ID failed: %d", ret); - goto fail; - } - - if(buffer[0] != MMC5603NJ_CHIP_ID) { - LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID); - ret = -1; - goto fail; - } + int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID}); + if (ret == -1) return -1; // Set 100 Hz ret = set_register(MMC5603NJ_I2C_REG_ODR, 100); diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 2279cf2532..fc8dc8620b 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -27,7 +27,6 @@ #define I2C_BUS_IMU 1 ExitHandler do_exit; -std::mutex pm_mutex; uint64_t init_ts = 0; void interrupt_loop(std::vector& sensors, diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 071eaada12..65935f8979 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -2,7 +2,7 @@ import numpy as np from selfdrive.test.longitudinal_maneuvers.plant import Plant -class Maneuver(): +class Maneuver: def __init__(self, title, duration, **kwargs): # Was tempted to make a builder class self.distance_lead = kwargs.get("initial_distance_lead", 200.0) @@ -18,6 +18,7 @@ class Maneuver(): self.only_radar = kwargs.get("only_radar", False) self.ensure_start = kwargs.get("ensure_start", False) self.enabled = kwargs.get("enabled", True) + self.e2e = kwargs.get("e2e", False) self.duration = duration self.title = title @@ -30,6 +31,7 @@ class Maneuver(): enabled=self.enabled, only_lead2=self.only_lead2, only_radar=self.only_radar, + e2e=self.e2e, ) valid = True diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index c3af1eee03..8e150d800c 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -15,7 +15,7 @@ class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - enabled=True, only_lead2=False, only_radar=False): + enabled=True, only_lead2=False, only_radar=False, e2e=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -38,6 +38,7 @@ class Plant: self.enabled = enabled self.only_lead2 = only_lead2 self.only_radar = only_radar + self.e2e = e2e self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate @@ -109,6 +110,7 @@ class Plant: control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off control.controlsState.vCruise = float(v_cruise * 3.6) + control.controlsState.experimentalMode = self.e2e car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index e859952445..686b35e456 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +from parameterized import parameterized import unittest from common.params import Params @@ -8,124 +9,137 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # TODO: make new FCW tests -maneuvers = [ - Maneuver( - 'approach stopped car at 25m/s, initial distance: 120m', - duration=20., - initial_speed=25., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[30., 0.], - breakpoints=[0., 1.], - ), - Maneuver( - 'approach stopped car at 20m/s, initial distance 90m', - duration=20., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=90., - speed_lead_values=[20., 0.], - breakpoints=[0., 1.], - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 35.0], - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 25.0], - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 21.66], - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', - duration=40., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - prob_lead_values=[0., 1., 1.], - cruise_values=[20., 20., 20.], - breakpoints=[2., 2.01, 8.8], - ), - Maneuver( - "approach stopped car at 20m/s, with prob_lead_values", - duration=30., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[0.0, 0., 0.], - prob_lead_values=[0.0, 0., 1.], - cruise_values=[20., 20., 20.], - breakpoints=[0.0, 2., 2.01], - ), - Maneuver( - "approach slower cut-in car at 20m/s", - duration=20., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=50., - speed_lead_values=[15., 15.], - breakpoints=[1., 11.], - only_lead2=True, - ), - Maneuver( - "stay stopped behind radar override lead", - duration=20., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[0., 0.], - prob_lead_values=[0., 0.], - breakpoints=[1., 11.], - only_radar=True, - ), - Maneuver( - "NaN recovery", - duration=30., - initial_speed=15., - lead_relevancy=True, - initial_distance_lead=60., - speed_lead_values=[0., 0., 0.0], - breakpoints=[1., 1.01, 11.], - cruise_values=[float("nan"), 15., 15.], - ), - # controls relies on planner commanding to move for stock-ACC resume spamming - Maneuver( - "resume from a stop", - duration=20., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=STOP_DISTANCE, - speed_lead_values=[0., 0., 2.], - breakpoints=[1., 10., 15.], - ensure_start=True, - ), - Maneuver( - 'cruising at 25 m/s while disabled', - duration=20., - initial_speed=25., - lead_relevancy=False, - enabled=False, - ), -] +def create_maneuvers(e2e): + return [ + Maneuver( + 'approach stopped car at 25m/s, initial distance: 120m', + duration=20., + initial_speed=25., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[30., 0.], + breakpoints=[0., 1.], + e2e=e2e, + ), + Maneuver( + 'approach stopped car at 20m/s, initial distance 90m', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=90., + speed_lead_values=[20., 0.], + breakpoints=[0., 1.], + e2e=e2e, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 35.0], + e2e=e2e, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 25.0], + e2e=e2e, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 21.66], + e2e=e2e, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', + duration=40., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + prob_lead_values=[0., 1., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[2., 2.01, 8.8], + e2e=e2e, + ), + Maneuver( + "approach stopped car at 20m/s, with prob_lead_values", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_lead_values=[0.0, 0., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[0.0, 2., 2.01], + e2e=e2e, + ), + Maneuver( + "approach slower cut-in car at 20m/s", + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + breakpoints=[1., 11.], + only_lead2=True, + e2e=e2e, + ), + Maneuver( + "stay stopped behind radar override lead", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[0., 0.], + prob_lead_values=[0., 0.], + breakpoints=[1., 11.], + only_radar=True, + e2e=e2e, + ), + Maneuver( + "NaN recovery", + duration=30., + initial_speed=15., + lead_relevancy=True, + initial_distance_lead=60., + speed_lead_values=[0., 0., 0.0], + breakpoints=[1., 1.01, 11.], + cruise_values=[float("nan"), 15., 15.], + e2e=e2e, + ), + # controls relies on planner commanding to move for stock-ACC resume spamming + Maneuver( + "resume from a stop", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=STOP_DISTANCE, + speed_lead_values=[0., 0., 2.], + breakpoints=[1., 10., 15.], + ensure_start=True, + e2e=e2e, + ), + Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + e2e=e2e, + ), + ] class LongitudinalControl(unittest.TestCase): @@ -140,29 +154,11 @@ class LongitudinalControl(unittest.TestCase): params.put_bool("Passive", bool(os.getenv("PASSIVE"))) params.put_bool("OpenpilotEnabledToggle", True) - # hack - def test_longitudinal_setup(self): - pass - - -def run_maneuver_worker(k): - def run(self): - params = Params() - - man = maneuvers[k] - params.put_bool("ExperimentalMode", True) - print(man.title, ' in e2e mode') - valid, _ = man.evaluate() - self.assertTrue(valid, msg=man.title) - params.put_bool("ExperimentalMode", False) - print(man.title, ' in acc mode') - valid, _ = man.evaluate() - self.assertTrue(valid, msg=man.title) - return run - -for k in range(len(maneuvers)): - setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}", - run_maneuver_worker(k)) + @parameterized.expand([(man,) for e2e in [True, False] for man in create_maneuvers(e2e)]) + def test_maneuver(self, maneuver): + print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') + valid, _ = maneuver.evaluate() + self.assertTrue(valid, msg=maneuver.title) if __name__ == "__main__": diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index c14956b1b2..c96e624421 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -2,6 +2,7 @@ import bz2 import sys import math +import capnp import numbers import dictdiffer from collections import Counter @@ -30,20 +31,23 @@ def remove_ignored_fields(msg, ignore): continue for k in keys[:-1]: - try: - attr = getattr(msg, k) - except AttributeError: - break - else: - v = getattr(attr, keys[-1]) - if isinstance(v, bool): - val = False - elif isinstance(v, numbers.Number): - val = 0 + # indexing into list + if k.isdigit(): + attr = attr[int(k)] else: - raise NotImplementedError('Error ignoring field') - setattr(attr, keys[-1], val) - return msg.as_reader() + attr = getattr(attr, k) + + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + elif isinstance(v, (list, capnp.lib.capnp._DynamicListBuilder)): + val = [] + else: + raise NotImplementedError(f"Unknown type: {type(v)}") + setattr(attr, keys[-1], val) + return msg def get_field_tolerance(diff_field, field_tolerances): @@ -76,15 +80,15 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non diff = [] for msg1, msg2 in zip(log1, log2): if msg1.which() != msg2.which(): - print(msg1, msg2) + print(msg1.which(), msg2.which()) raise Exception("msgs not aligned between logs") - msg1_bytes = remove_ignored_fields(msg1, ignore_fields).as_builder().to_bytes() - msg2_bytes = remove_ignored_fields(msg2, ignore_fields).as_builder().to_bytes() + msg1 = remove_ignored_fields(msg1, ignore_fields) + msg2 = remove_ignored_fields(msg2, ignore_fields) - if msg1_bytes != msg2_bytes: - msg1_dict = msg1.to_dict(verbose=True) - msg2_dict = msg2.to_dict(verbose=True) + if msg1.to_bytes() != msg2.to_bytes(): + msg1_dict = msg1.as_reader().to_dict(verbose=True) + msg2_dict = msg2.as_reader().to_dict(verbose=True) dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index a63ec6489b..ce861b37e6 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -20,14 +20,17 @@ from system.version import get_commit from tools.lib.framereader import FrameReader from tools.lib.logreader import LogReader -TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" -SEGMENT = 0 +TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30" +SEGMENT = 6 MAX_FRAMES = 100 if PC else 600 +NAV_FRAMES = 50 +NO_NAV = "NO_NAV" in os.environ # TODO: make map renderer work in CI SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD} + def get_log_fn(ref_commit, test_route): return f"{test_route}_model_tici_{ref_commit}.bz2" @@ -39,6 +42,54 @@ def replace_calib(msg, calib): return msg +def nav_model_replay(lr): + sm = messaging.SubMaster(['navModel', 'navThumbnail']) + pm = messaging.PubMaster(['liveLocationKalman', 'navRoute']) + + nav = [m for m in lr if m.which() == 'navRoute'] + llk = [m for m in lr if m.which() == 'liveLocationKalman'] + assert len(nav) > 0 and len(llk) >= NAV_FRAMES + + log_msgs = [] + try: + assert "MAPBOX_TOKEN" in os.environ + os.environ['MAP_RENDER_TEST_MODE'] = '1' + managed_processes['mapsd'].start() + managed_processes['navmodeld'].start() + + # setup position and route + for _ in range(10): + for s in (llk, nav): + pm.send(s[0].which(), s[0].as_builder().to_bytes()) + sm.update(1000) + if sm.updated['navModel']: + break + time.sleep(1) + + if not sm.updated['navModel']: + raise Exception("no navmodeld outputs, failed to initialize") + + # drain + time.sleep(2) + sm.update(0) + + # run replay + for n in range(NAV_FRAMES): + pm.send(llk[n].which(), llk[n].as_builder().to_bytes()) + m = messaging.recv_one(sm.sock['navThumbnail']) + assert m is not None, f"no navThumbnail, frame={n}" + log_msgs.append(m) + + m = messaging.recv_one(sm.sock['navModel']) + assert m is not None, f"no navModel response, frame={n}" + log_msgs.append(m) + finally: + managed_processes['mapsd'].stop() + managed_processes['navmodeld'].stop() + + return log_msgs + + def model_replay(lr, frs): if not PC: spinner = Spinner() @@ -154,8 +205,10 @@ if __name__ == "__main__": 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"), readahead=True) } - # run replay + # run replays log_msgs = model_replay(lr, frs) + if not NO_NAV: + log_msgs += nav_model_replay(lr) # get diff failed = False @@ -164,15 +217,30 @@ if __name__ == "__main__": ref_commit = f.read().strip() log_fn = get_log_fn(ref_commit, TEST_ROUTE) try: - cmp_log = list(LogReader(BASE_URL + log_fn))[:2*MAX_FRAMES] + expected_msgs = 2*MAX_FRAMES + if not NO_NAV: + expected_msgs += NAV_FRAMES*2 + cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs] ignore = [ 'logMonoTime', 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime', 'driverStateV2.modelExecutionTime', - 'driverStateV2.dspExecutionTime' + 'driverStateV2.dspExecutionTime', + 'navModel.dspExecutionTime', + 'navModel.modelExecutionTime', + 'navThumbnail.timestampEof', ] + if PC: + ignore += [ + 'modelV2.laneLines.0.t', + 'modelV2.laneLines.1.t', + 'modelV2.laneLines.2.t', + 'modelV2.laneLines.3.t', + 'modelV2.roadEdges.0.t', + 'modelV2.roadEdges.1.t', + ] # TODO this tolerance is absurdly large tolerance = 2.0 if PC else None results: Any = {TEST_ROUTE: {}} diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index cdd6ed7a6b..aa204e0f8a 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -ca02aa240e629920ad88a6510213cb0a153af92b +4ff972367fdb9546be68ee0ba0d45cf4f839dae7 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4aa9d60ab5..4f06673e97 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -959e63af52d9efdb62156cab4b773f88b154fd75 +67a4bd615017128ce04d0836608d2c7f32432e3e \ No newline at end of file diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index 02c7d76637..9e06b98662 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -29,6 +29,7 @@ sudo abctl --set_success # patch sshd config sudo mount -o rw,remount / +echo tici-$(cat /proc/cmdline | sed -e 's/^.*androidboot.serialno=//' -e 's/ .*$//') | sudo tee /etc/hostname sudo sed -i "s,/data/params/d/GithubSshKeys,/usr/comma/setup_keys," /etc/ssh/sshd_config sudo systemctl daemon-reload sudo systemctl restart ssh diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 7834569088..85e9510eb7 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -1,24 +1,23 @@ import threading from typing import Optional -from cereal import log from common.params import Params, put_nonblocking from common.realtime import sec_since_boot from system.hardware import HARDWARE from system.swaglog import cloudlog from selfdrive.statsd import statlog -CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) +CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1)) -# A C2 uses about 1W while idling, and 30h seens like a good shutoff for most cars # While driving, a battery charges completely in about 30-60 minutes CAR_BATTERY_CAPACITY_uWh = 30e6 CAR_CHARGING_RATE_W = 45 -VBATT_PAUSE_CHARGING = 11.0 # Lower limit on the LPF car battery voltage +VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery voltage VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss MAX_TIME_OFFROAD_S = 30*3600 MIN_ON_TIME_S = 3600 +VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60 class PowerMonitoring: def __init__(self): @@ -39,12 +38,12 @@ class PowerMonitoring: self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) # Calculation tick - def calculate(self, peripheralState, ignition): + def calculate(self, voltage: Optional[int], ignition: bool): try: now = sec_since_boot() # If peripheralState is None, we're probably not in a car, so we don't care - if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown: + if voltage is None: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None @@ -52,8 +51,8 @@ class PowerMonitoring: return # Low-pass battery voltage - self.car_voltage_instant_mV = peripheralState.voltage - self.car_voltage_mV = ((peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + self.car_voltage_instant_mV = voltage + self.car_voltage_mV = ((voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) statlog.gauge("car_voltage", self.car_voltage_mV / 1e3) # Cap the car battery power and save it in a param every 10-ish seconds @@ -115,8 +114,12 @@ class PowerMonitoring: now = sec_since_boot() should_shutdown = False - should_shutdown |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S - should_shutdown |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) + offroad_time = (now - offroad_timestamp) + low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and + self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3) and + offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S) + should_shutdown |= offroad_time > MAX_TIME_OFFROAD_S + should_shutdown |= low_voltage_shutdown should_shutdown |= (self.car_battery_capacity_uWh <= 0) should_shutdown &= not ignition should_shutdown &= (not self.params.get_bool("DisablePowerDown")) diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py index 5d7463d455..d41e92454b 100755 --- a/selfdrive/thermald/tests/test_power_monitoring.py +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -1,10 +1,7 @@ #!/usr/bin/env python3 import unittest from unittest.mock import patch -from parameterized import parameterized -from cereal import log -import cereal.messaging as messaging from common.params import Params params = Params() @@ -21,7 +18,8 @@ with patch("common.realtime.sec_since_boot", new=mock_sec_since_boot): CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING TEST_DURATION_S = 50 -ALL_PANDA_TYPES = [(log.PandaState.PandaType.dos,)] +GOOD_VOLTAGE = 12 * 1e3 +VOLTAGE_BELOW_PAUSE_CHARGING = (VBATT_PAUSE_CHARGING - 1) * 1e3 def pm_patch(name, value, constant=False): if constant: @@ -34,12 +32,6 @@ class TestPowerMonitoring(unittest.TestCase): params.remove("CarBatteryCapacity") params.remove("DisablePowerDown") - def mock_peripheralState(self, hw_type, car_voltage=12): - ps = messaging.new_message('peripheralState').peripheralState - ps.pandaType = hw_type - ps.voltage = car_voltage * 1e3 - return ps - # Test to see that it doesn't do anything when pandaState is None def test_pandaState_present(self): pm = PowerMonitoring() @@ -49,75 +41,68 @@ class TestPowerMonitoring(unittest.TestCase): self.assertEqual(pm.get_car_battery_capacity(), (CAR_BATTERY_CAPACITY_uWh / 10)) # Test to see that it doesn't integrate offroad when ignition is True - @parameterized.expand(ALL_PANDA_TYPES) - def test_offroad_ignition(self, hw_type): + def test_offroad_ignition(self): pm = PowerMonitoring() for _ in range(10): - pm.calculate(self.mock_peripheralState(hw_type), True) + pm.calculate(GOOD_VOLTAGE, True) self.assertEqual(pm.get_power_used(), 0) # Test to see that it integrates with discharging battery - @parameterized.expand(ALL_PANDA_TYPES) - def test_offroad_integration_discharging(self, hw_type): + def test_offroad_integration_discharging(self): POWER_DRAW = 4 with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_peripheralState(hw_type), False) + pm.calculate(GOOD_VOLTAGE, False) expected_power_usage = ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) # Test to check positive integration of car_battery_capacity - @parameterized.expand(ALL_PANDA_TYPES) - def test_car_battery_integration_onroad(self, hw_type): + def test_car_battery_integration_onroad(self): POWER_DRAW = 4 with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() pm.car_battery_capacity_uWh = 0 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_peripheralState(hw_type), True) + pm.calculate(GOOD_VOLTAGE, True) expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) # Test to check positive integration upper limit - @parameterized.expand(ALL_PANDA_TYPES) - def test_car_battery_integration_upper_limit(self, hw_type): + def test_car_battery_integration_upper_limit(self): POWER_DRAW = 4 with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_peripheralState(hw_type), True) + pm.calculate(GOOD_VOLTAGE, True) estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) # Test to check negative integration of car_battery_capacity - @parameterized.expand(ALL_PANDA_TYPES) - def test_car_battery_integration_offroad(self, hw_type): + def test_car_battery_integration_offroad(self): POWER_DRAW = 4 with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_peripheralState(hw_type), False) + pm.calculate(GOOD_VOLTAGE, False) expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) # Test to check negative integration lower limit - @parameterized.expand(ALL_PANDA_TYPES) - def test_car_battery_integration_lower_limit(self, hw_type): + def test_car_battery_integration_lower_limit(self): POWER_DRAW = 4 with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() pm.car_battery_capacity_uWh = 1000 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_peripheralState(hw_type), False) + pm.calculate(GOOD_VOLTAGE, False) estimated_capacity = 0 - ((1/3600) * POWER_DRAW * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S - @parameterized.expand(ALL_PANDA_TYPES) - def test_max_time_offroad(self, hw_type): + def test_max_time_offroad(self): MOCKED_MAX_OFFROAD_TIME = 3600 POWER_DRAW = 0 # To stop shutting down for other reasons with pm_patch("MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True), pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): @@ -125,28 +110,29 @@ class TestPowerMonitoring(unittest.TestCase): pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh start_time = ssb ignition = False - peripheralState = self.mock_peripheralState(hw_type) while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: - pm.calculate(peripheralState, ignition) + pm.calculate(GOOD_VOLTAGE, ignition) if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: self.assertFalse(pm.should_shutdown(ignition, True, start_time, False)) self.assertTrue(pm.should_shutdown(ignition, True, start_time, False)) # Test to check policy of stopping charging when the car voltage is too low - @parameterized.expand(ALL_PANDA_TYPES) - def test_car_voltage(self, hw_type): + def test_car_voltage(self): POWER_DRAW = 0 # To stop shutting down for other reasons TEST_TIME = 100 - with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 50 + with pm_patch("VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S", VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S, constant=True), pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh ignition = False - peripheralState = self.mock_peripheralState(hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + start_time = ssb for i in range(TEST_TIME): - pm.calculate(peripheralState, ignition) + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) if i % 10 == 0: - self.assertEqual(pm.should_shutdown(ignition, True, ssb, True), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3)) - self.assertTrue(pm.should_shutdown(ignition, True, ssb, True)) + self.assertEqual(pm.should_shutdown(ignition, True, start_time, True), + (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3 and + (ssb - start_time) > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S)) + self.assertTrue(pm.should_shutdown(ignition, True, start_time, True)) # Test to check policy of not stopping charging when DisablePowerDown is set def test_disable_power_down(self): @@ -157,9 +143,8 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh ignition = False - peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(peripheralState, ignition) + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) if i % 10 == 0: self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) @@ -172,9 +157,8 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh ignition = True - peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(peripheralState, ignition) + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) if i % 10 == 0: self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) @@ -188,9 +172,8 @@ class TestPowerMonitoring(unittest.TestCase): pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh ignition = False - peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(peripheralState, ignition) + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) if i % 10 == 0: self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 89b81f06ec..eedeff31f1 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -347,7 +347,8 @@ def thermald_thread(end_event, hw_queue): off_ts = sec_since_boot() # Offroad power monitoring - power_monitor.calculate(peripheralState, onroad_conditions["ignition"]) + voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage + power_monitor.calculate(voltage, onroad_conditions["ignition"]) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) current_power_draw = HARDWARE.get_current_power_draw() diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 1377bb3b23..2f366a9b38 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -22,6 +22,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { } void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { + cameraView->stopVipcThread(); emit done(); } @@ -35,7 +36,6 @@ void DriverViewScene::showEvent(QShowEvent* event) { } void DriverViewScene::hideEvent(QHideEvent* event) { - // TODO: stop vipc thread ? params.putBool("IsDriverViewEnabled", false); } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 50f891dd56..95ad813dff 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -31,6 +31,11 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { split->insertWidget(0, arCam); } + if (getenv("MAP_RENDER_VIEW")) { + CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this); + split->insertWidget(0, map_render); + } + stacked_layout->addWidget(split_wrapper); alerts = new OnroadAlerts(this); @@ -204,7 +209,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { v_ego_cluster_seen = true; } float cur_speed = cs_alive ? std::max(0.0, v_ego) : 0.0; - cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; + cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign(); float speed_limit = nav_alive ? sm["navInstruction"].getNavInstruction().getSpeedLimit() : 0.0; diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 347cdb1dca..16aad13437 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -359,7 +359,7 @@ void CameraWidget::vipcThread() { if (!vipc_client || cur_stream != requested_stream_type) { clearFrames(); qDebug() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; - cur_stream = requested_stream_type;; + cur_stream = requested_stream_type; vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); } active_stream_type = cur_stream; diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 7cc3847f99..f8b97be03e 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -35,6 +35,7 @@ public: void setFrameId(int frame_id) { draw_frame_id = frame_id; } void setStreamType(VisionStreamType type) { requested_stream_type = type; } VisionStreamType getStreamType() { return active_stream_type; } + void stopVipcThread(); signals: void clicked(); @@ -51,7 +52,6 @@ protected: void updateCalibration(const mat3 &calib); void vipcThread(); void clearFrames(); - void stopVipcThread(); bool zoomed_view; GLuint frame_vao, frame_vbo, frame_ibo; diff --git a/selfdrive/ui/soundd/sound.cc b/selfdrive/ui/soundd/sound.cc index 6830450d8f..42fc9c4b1a 100644 --- a/selfdrive/ui/soundd/sound.cc +++ b/selfdrive/ui/soundd/sound.cc @@ -12,7 +12,7 @@ // TODO: detect when we can't play sounds // TODO: detect when we can't display the UI -Sound::Sound(QObject *parent) : sm({"carState", "controlsState", "deviceState"}) { +Sound::Sound(QObject *parent) : sm({"controlsState", "deviceState", "microphone"}) { qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName(); for (auto &[alert, fn, loops] : sound_list) { @@ -20,7 +20,6 @@ Sound::Sound(QObject *parent) : sm({"carState", "controlsState", "deviceState"}) QObject::connect(s, &QSoundEffect::statusChanged, [=]() { assert(s->status() != QSoundEffect::Error); }); - s->setVolume(Hardware::MIN_VOLUME); s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn)); sounds[alert] = {s, loops}; } @@ -47,14 +46,11 @@ void Sound::update() { return; } - // scale volume with speed - if (sm.updated("carState")) { - float volume = util::map_val(sm["carState"].getCarState().getVEgo(), 11.f, 20.f, 0.f, 1.0f); + // scale volume using ambient noise level + if (sm.updated("microphone")) { + float volume = util::map_val(sm["microphone"].getMicrophone().getFilteredSoundPressureWeightedDb(), 30.f, 54.f, 0.f, 1.f); volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale); - volume = util::map_val(volume, 0.f, 1.f, Hardware::MIN_VOLUME, Hardware::MAX_VOLUME); - for (auto &[s, loops] : sounds) { - s->setVolume(std::round(100 * volume) / 100); - } + Hardware::set_volume(volume); } setAlert(Alert::get(sm, started_frame)); diff --git a/selfdrive/ui/translations/languages.json b/selfdrive/ui/translations/languages.json index 072d320c13..86d3e62d87 100644 --- a/selfdrive/ui/translations/languages.json +++ b/selfdrive/ui/translations/languages.json @@ -1,5 +1,6 @@ { "English": "main_en", + "Deutsch": "main_de", "Português": "main_pt-BR", "中文（繁體）": "main_zh-CHT", "中文（简体）": "main_zh-CHS", diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts new file mode 100644 index 0000000000..ecb44557c9 --- /dev/null +++ b/selfdrive/ui/translations/main_de.ts @@ -0,0 +1,1113 @@ + + + + + AbstractAlert + + Close + Schließen + + + Snooze Update + Update pausieren + + + Reboot and Update + Aktualisieren und neu starten + + + + AdvancedNetworking + + Back + Zurück + + + Enable Tethering + Tethering aktivieren + + + Tethering Password + Tethering Passwort + + + EDIT + ÄNDERN + + + Enter new tethering password + Neues tethering Passwort eingeben + + + IP Address + IP Adresse + + + Enable Roaming + Roaming aktivieren + + + APN Setting + APN Einstellungen + + + Enter APN + APN eingeben + + + leave blank for automatic configuration + für automatische Konfiguration leer lassen + + + Cellular Metered + Getaktete Verbindung + + + Prevent large data uploads when on a metered connection + Hochladen großer Dateien über getaktete Verbindungen unterbinden + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + MAX + + + SPEED + Geschwindigkeit + + + LIMIT + LIMIT + + + + ConfirmationDialog + + Ok + Ok + + + Cancel + Abbrechen + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + Du musst die Nutzungsbedingungen akzeptieren, um Openpilot zu benutzen. + + + Back + Zurück + + + Decline, uninstall %1 + Ablehnen, deinstallieren %1 + + + + DevicePanel + + Dongle ID + Dongle ID + + + N/A + Nicht verfügbar + + + Serial + Seriennummer + + + Driver Camera + Fahrerkamera + + + PREVIEW + VORSCHAU + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Vorschau der auf den Fahrer gerichteten Kamera, um sicherzustellen, dass die Fahrerüberwachung eine gute Sicht hat. (Fahrzeug muss aus sein) + + + Reset Calibration + Neu kalibrieren + + + RESET + RESET + + + Are you sure you want to reset calibration? + Bist du sicher, dass du die Kalibrierung zurücksetzen möchtest? + + + Review Training Guide + Trainingsanleitung wiederholen + + + REVIEW + TRAINING + + + Review the rules, features, and limitations of openpilot + Wiederhole die Regeln, Fähigkeiten und Limitierungen von Openpilot + + + Are you sure you want to review the training guide? + Bist du sicher, dass du die Trainingsanleitung wiederholen möchtest? + + + Regulatory + Rechtliche Hinweise + + + VIEW + ANSEHEN + + + Change Language + Sprache ändern + + + CHANGE + ÄNDERN + + + Select a language + Sprache wählen + + + Reboot + Neustart + + + Power Off + Ausschalten + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + Damit Openpilot funktioniert, darf die Installationsposition nicht mehr als 4° nach rechts/links, 5° nach oben und 8° nach unten abweichen. Openpilot kalibriert sich durchgehend, ein Zurücksetzen ist selten notwendig. + + + Your device is pointed %1° %2 and %3° %4. + Deine Geräteausrichtung ist %1° %2 und %3° %4. + + + down + unten + + + up + oben + + + left + links + + + right + rechts + + + Are you sure you want to reboot? + Bist du sicher, dass du das Gerät neu starten möchtest? + + + Disengage to Reboot + Für Neustart deaktivieren + + + Are you sure you want to power off? + Bist du sicher, dass du das Gerät ausschalten möchtest? + + + Disengage to Power Off + Zum Ausschalten deaktivieren + + + Reset + Zurücksetzen + + + Review + Überprüfen + + + + DriveStats + + Drives + Fahrten + + + Hours + Stunden + + + ALL TIME + Gesamtzeit + + + PAST WEEK + Letzte Woche + + + KM + KM + + + Miles + Meilen + + + + DriverViewScene + + camera starting + Kamera startet + + + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + EXPERIMENTELLER MODUS AN + + + CHILL MODE ON + ENTSPANNTER MODUS AN + + + + InputDialog + + Cancel + Abbrechen + + + Need at least %n character(s)! + + Mindestens %n Buchstabe benötigt! + Mindestens %n Buchstaben benötigt! + + + + + Installer + + Installing... + Installiere... + + + Receiving objects: + Empfange Objekte: + + + Resolving deltas: + Unterschiede verarbeiten: + + + Updating files: + Dateien aktualisieren: + + + + MapETA + + eta + Ankunft + + + min + min + + + hr + std + + + km + km + + + mi + mi + + + + MapInstructions + + km + km + + + m + m + + + mi + mi + + + ft + fuß + + + + MapPanel + + Current Destination + Aktuelles Ziel + + + CLEAR + LÖSCHEN + + + Recent Destinations + Letzte Ziele + + + Try the Navigation Beta + Beta Navigation ausprobieren + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Erhalte echtzeit Wegführung und mehr mit dem comma prime +Abonnement. Melde dich jetzt an: https://connect.comma.ai + + + No home +location set + Keine Heimadresse gesetzt + + + No work +location set + Keine Arbeitsadresse gesetzt + + + no recent destinations + Keine kürzlich gewählten Ziele + + + + MapWindow + + Map Loading + Karte wird geladen + + + Waiting for GPS + Warten auf GPS + + + + MultiOptionDialog + + Select + Auswählen + + + Cancel + Abbrechen + + + + Networking + + Advanced + Erweitert + + + Enter password + Passwort eingeben + + + for "%1" + für "%1" + + + Wrong password + Falsches Passwort + + + + OffroadHome + + UPDATE + Aktualisieren + + + ALERTS + HINWEISE + + + ALERT + HINWEIS + + + + PairingPopup + + Pair your device to your comma account + Verbinde dein Gerät mit deinem comma Konto + + + Go to https://connect.comma.ai on your phone + Gehe zu https://connect.comma.ai auf deinem Handy + + + Click "add new device" and scan the QR code on the right + Klicke auf "neues Gerät hinzufügen" und scanne den QR code rechts + + + Bookmark connect.comma.ai to your home screen to use it like an app + Füge connect.comma.ai als Lesezeichen auf deinem Homescreen hinzu um es wie eine App zu verwenden + + + + ParamControl + + Cancel + Abbrechen + + + Enable + Aktivieren + + + + PrimeAdWidget + + Upgrade Now + Jetzt abonieren + + + Become a comma prime member at connect.comma.ai + Werde Comma Prime Mitglied auf connect.comma.ai + + + PRIME FEATURES: + PRIME FUNKTIONEN: + + + Remote access + Fernzugriff + + + 1 year of storage + 1 Jahr Speicherplatz + + + Developer perks + Entwickler Vorteile + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ ABBONIERT + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + COMMA PUNKTE + + + + QObject + + Reboot + Neustart + + + Exit + Verlassen + + + dashcam + dashcam + + + openpilot + openpilot + + + %n minute(s) ago + + vor %n Minute + vor %n Minuten + + + + %n hour(s) ago + + vor %n Stunde + vor %n Stunden + + + + %n day(s) ago + + vor %n Tag + vor %n Tagen + + + + + Reset + + Reset failed. Reboot to try again. + Zurücksetzen fehlgeschlagen. Starte das Gerät neu und versuche es wieder. + + + Are you sure you want to reset your device? + Bist du sicher, dass du das Gerät auf Werkseinstellungen zurücksetzen möchtest? + + + Resetting device... + Gerät wird zurückgesetzt... + + + System Reset + System auf Werkseinstellungen zurücksetzen + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Zurücksetzen auf Werkseinstellungen wurde ausgewählt. Drücke Annehmen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um mit dem Starten des Gerätes fortzufahren. + + + Cancel + Abbrechen + + + Reboot + Neustart + + + Confirm + Bestätigen + + + Unable to mount data partition. Press confirm to reset your device. + Datenpartition kann nicht geöffnet werden. Drücke Annehmen, um dein Gerät auf Werkseinstellungen zurückzusetzen. + + + + SettingsWindow + + × + x + + + Device + Gerät + + + Network + Netzwerk + + + Toggles + Schalter + + + Software + Software + + + Navigation + Navigation + + + + Setup + + WARNING: Low Voltage + Warnung: Batteriespannung niedrig + + + Power your device in a car with a harness or proceed at your own risk. + Versorge dein Gerät über einen Kabelbaum im Auto mit Strom, oder fahre auf eigene Gefahr fort. + + + Power off + Ausschalten + + + Continue + Fortsetzen + + + Getting Started + Loslegen + + + Before we get on the road, let’s finish installation and cover some details. + Bevor wir uns auf die Straße begeben, lass uns die Installation fertigstellen und einige Details prüfen. + + + Connect to Wi-Fi + Mit WLAN verbinden + + + Back + Zurück + + + Continue without Wi-Fi + Ohne WLAN fortsetzen + + + Waiting for internet + Auf Internet warten + + + Choose Software to Install + Software zum installieren auswählen + + + Dashcam + Dashcam + + + Custom Software + Spezifische Software + + + Enter URL + URL eingeben + + + for Custom Software + für spezifische Software + + + Downloading... + Herunterladen... + + + Download Failed + Herunterladen fehlgeschlagen + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Stelle sicher, dass die eingegebene URL korrekt ist und dein Gerät eine stabile Internetverbindung hat. + + + Reboot device + Gerät neustarten + + + Start over + Von neuem beginnen + + + + SetupWidget + + Finish Setup + Einrichtung beenden + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Koppele dein Gerät mit Comma Connect (connect.comma.ai) und sichere dir dein Comma Prime Angebot. + + + Pair device + Gerät koppeln + + + + Sidebar + + CONNECT + This is a brand/service name for comma connect, don't translate + CONNECT + + + OFFLINE + OFFLINE + + + ONLINE + ONLINE + + + ERROR + FEHLER + + + TEMP + TEMP + + + HIGH + HOCH + + + GOOD + GUT + + + OK + OK + + + VEHICLE + FAHRZEUG + + + NO + KEIN + + + PANDA + PANDA + + + GPS + GPS + + + SEARCH + SUCHEN + + + -- + -- + + + Wi-Fi + WLAN + + + ETH + LAN + + + 2G + 2G + + + 3G + 3G + + + LTE + LTE + + + 5G + 5G + + + + SoftwarePanel + + UNINSTALL + Too long for UI + DEINSTALL + + + Uninstall %1 + Deinstalliere %1 + + + Are you sure you want to uninstall? + Bist du sicher, dass du Openpilot entfernen möchtest? + + + CHECK + ÜBERPRÜFEN + + + Updates are only downloaded while the car is off. + Updates werden nur heruntergeladen, wenn das Auto aus ist. + + + Current Version + Aktuelle Version + + + Download + Download + + + Install Update + Update installieren + + + INSTALL + INSTALLIEREN + + + Target Branch + Ziel Branch + + + SELECT + AUSWÄHLEN + + + Select a branch + Wähle einen Branch + + + Uninstall + Deinstallieren + + + + SshControl + + SSH Keys + SSH Schlüssel + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Warnung: Dies ermöglicht SSH zugriff für alle öffentlichen Schlüssel in deinen Github Einstellungen. Gib niemals einen anderen Benutzernamen, als deinen Eigenen an. Comma Angestellte fragen dich niemals danach ihren Github Benutzernamen hinzuzufügen. + + + ADD + HINZUFÜGEN + + + Enter your GitHub username + Gib deinen GitHub Benutzernamen ein + + + LOADING + LADEN + + + REMOVE + LÖSCHEN + + + Username '%1' has no keys on GitHub + Benutzername '%1' hat keine Schlüssel auf GitHub + + + Request timed out + Zeitüberschreitung der Anforderung + + + Username '%1' doesn't exist on GitHub + Benutzername '%1' existiert nicht auf GitHub + + + + SshToggle + + Enable SSH + SSH aktivieren + + + + TermsPage + + Terms & Conditions + Benutzungsbedingungen + + + Decline + Ablehnen + + + Scroll to accept + Scrolle herunter um zu akzeptieren + + + Agree + Zustimmen + + + + TogglesPanel + + Enable openpilot + Openpilot aktivieren + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Benutze das Openpilot System als adaptiven Tempomaten und Spurhalteassistenten. Deine Aufmerksamkeit ist jederzeit erforderlich, um diese Funktion zu nutzen. Diese Einstellung wird übernommen, wenn das Auto aus ist. + + + Enable Lane Departure Warnings + Spurverlassenswarnungen aktivieren + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Erhalte Warnungen, zurück in die Spur zu lenken, wenn dein Auto über eine erkannte Fahrstreifenmarkierung ohne aktivierten Blinker mit mehr als 50 km/h fährt. + + + Use Metric System + Benutze das metrische System + + + Display speed in km/h instead of mph. + Zeige die Geschwindigkeit in km/h anstatt von mph. + + + Record and Upload Driver Camera + Fahrerkamera aufnehmen und hochladen + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Lade Daten der Fahreraufmerksamkeitsüberwachungskamera hoch, um die Fahreraufmerksamkeitsüberwachungsalgorithmen zu verbessern. + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Wenn aktiviert, deaktiviert sich Openpilot sobald das Gaspedal betätigt wird. + + + Use 24h format instead of am/pm + Benutze das 24Stunden Format anstatt am/pm + + + Show Map on Left Side of UI + Too long for UI + Zeige die Karte auf der linken Seite + + + Show map on left side when in split screen view. + Zeige die Karte auf der linken Seite der Benutzeroberfläche bei geteilten Bildschirm. + + + Show ETA in 24h Format + Too long for UI + Zeige die Ankunftszeit im 24 Stunden Format + + + Experimental Mode + Experimenteller Modus + + + Experimental openpilot Longitudinal Control + Experimenteller Openpilot Tempomat + + + WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). + WARNUNG: Der Openpilot Tempomat ist für dieses Auto experimentell und deaktiviert den Notbremsassistenten. + + + Disengage on Accelerator Pedal + Bei Gasbetätigung ausschalten + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. + Bei diesem auto wird standardmäßig der im Auto eingebaute adaptive Tempomat anstelle des Openpilot Tempomats benutzt. Aktiviere diesen Schalter, um zum Openpilot Tempomaten zu wechseln. Es ist empfohlen den Experimentellen Modus bei Nutzung des Openpilot Tempomats zu aktivieren. + + + openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: + Openpilot fährt standardmäßig im <b>entspannten Modus</b>. Der Experimentelle Modus aktiviert<b>Alpha-level Funktionen</b>, die noch nicht für den entspannten Modus bereit sind. Die experimentellen Funktionen sind die Folgenden: + + + 🌮 End-to-End Longitudinal Control 🌮 + 🌮 Ende-zu-Ende Tempomat 🌮 + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. + Lass das Fahrmodell Gas und Bremse kontrollieren. Openpilot wird so fahren, wie es dies von einem Menschen erwarten würde; inklusive des Anhaltens für Ampeln und Stoppschildern. Da das Fahrmodell entscheidet wie schnell es fährt stellt die gesetzte Geschwindigkeit lediglich das obere Limit dar. Dies ist ein Alpha-level Funktion. Fehler sind zu erwarten. + + + New Driving Visualization + Neue Fahrvisualisierung + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten zur Straßengewandten Weitwinkelkamera, um manche Kurven besser zu zeigen. Außerdem wird das Experimenteller Modus logo oben rechts angezeigt. + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar, da es den eingebauten adaptiven Tempomaten des Autos benutzt. + + + Enable experimental longitudinal control to allow experimental mode. + Aktiviere den experimentellen Openpilot Tempomaten für experimentelle Funktionen. + + + + Updater + + Update Required + Aktualisierung notwendig + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Eine Aktualisierung des Betriebssystems ist notwendig. Verbinde dein Gerät mit WLAN für ein schnelleres Update. Die Download Größe ist ungefähr 1GB. + + + Connect to Wi-Fi + Mit WLAN verbinden + + + Install + Installieren + + + Back + Zurück + + + Loading... + Laden... + + + Reboot + Neustart + + + Update failed + Aktualisierung fehlgeschlagen + + + + WifiUI + + Scanning for networks... + Suche nach Netzwerken... + + + CONNECTING... + VERBINDEN... + + + FORGET + VERGESSEN + + + Forget Wi-Fi Network "%1"? + WLAN Netzwerk "%1" vergessen? + + + Forget + Vergessen + + + diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index ebf40cc5c5..8226dd59f9 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -285,11 +285,11 @@ ExperimentalModeButton EXPERIMENTAL MODE ON - + 実験モード CHILL MODE ON - + チルモード @@ -1015,15 +1015,15 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - + openpilotはこの車の場合、車に内蔵されているACCを標準で利用します。この機能を有効にすることで実験段階のopenpilotによるアクセル制御を利用できます。実験モードと合わせて利用することをお勧めします。 Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - + この車のACCがアクセル制御を行うため、実験モードを利用することができません。 Enable experimental longitudinal control to allow experimental mode. - + 実験段階のopenpilotによるアクセル制御を有効にしてください。 openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: @@ -1035,15 +1035,15 @@ location set Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. - + openpilotにアクセルとブレーキを任せます。openpilotは赤信号や一時停止サインでの停止を含み、人間と同じように考えて運転を行います。openpilotが運転速度を決定するため、あなたが設定する速度は上限速度になります。この機能は実験段階のため、openpilotの運転ミスに常に備えて注意してください。 New Driving Visualization - + 新しい運転画面 The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - + 新しい運転画面では、低速時に広角カメラの映像を表示することで、曲がる際の道路の視覚を向上します。実験段階を表すマークが右上に表示されます。 diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 4a698947f1..105d6f77f0 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -285,11 +285,11 @@ ExperimentalModeButton EXPERIMENTAL MODE ON - + MODO EXPERIMENTAL ATIVADO CHILL MODE ON - + MODO CHILL ATIVADO @@ -1019,15 +1019,15 @@ trabalho definido On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - + Neste carro o penpilot por padrão utiliza o ACC nativo do veículo ao invés de controlar longitudinalmente. Ative isto para mudar para o controle longitudinal do openpilot. Ativar o Modo Experimental é recomendado quando em uso do controle longitudinal experimental do openpilot. Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - + O Modo Experimental está atualmente indisponível para este carro, já que o ACC original do carro é usado para controle longitudinal. Enable experimental longitudinal control to allow experimental mode. - + Ative o controle longitudinal experimental para permitir o modo experimental. openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: @@ -1039,15 +1039,15 @@ trabalho definido Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. - + Deixe o modelo de IA controlar o acelerador e os freios. O openpilot irá dirigir como pensa que um humano faria, incluindo parar em sinais vermelhos e sinais de parada. Uma vez que o modelo de condução decide a velocidade a conduzir, a velocidade definida apenas funcionará como um limite superior. Este é um recurso de qualidade alfa; erros devem ser esperados. New Driving Visualization - + Nova Visualização de Condução The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - + A visualização da direção fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será exibido no canto superior direito. diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 46fb5578a3..0379e926c4 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -113,7 +113,7 @@ Decline, uninstall %1 - 拒絕並卸載 %1 + 拒絕並解除安裝 %1 @@ -132,7 +132,7 @@ Driver Camera - 駕駛員攝像頭 + 駕駛員監控鏡頭 PREVIEW @@ -285,11 +285,11 @@ ExperimentalModeButton EXPERIMENTAL MODE ON - + 實驗模式 ON CHILL MODE ON - + 輕鬆模式 ON @@ -859,15 +859,15 @@ location set UNINSTALL - 卸載 + 解除安裝 Uninstall %1 - 卸載 %1 + 解除安裝 %1 Are you sure you want to uninstall? - 您確定您要卸載嗎？ + 您確定您要解除安裝嗎？ CHECK @@ -875,7 +875,7 @@ location set Uninstall - 卸載 + 解除安裝 @@ -1015,19 +1015,19 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. - + 在本車輛中，openpilot預設將使用原車內建的ACC系統，而非openpilot縱向控制。開啟此開關來啟用openpilot縱向控制，使用此選項時建議一併啟用實驗模式。 Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - + 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 Enable experimental longitudinal control to allow experimental mode. - + 啟用實驗性縱向控制以使用實驗模式。 openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: - openpilot 默認以 <b>輕鬆模式</b> 駕駛。 實驗模式啟用了尚未準備好進入輕鬆模式的 <b>alpha 級功能</b>。實驗功能如下： + openpilot 預設以 <b>輕鬆模式</b> 駕駛。 實驗模式啟用了尚未準備好進入輕鬆模式的 <b>alpha 級功能</b>。實驗功能如下： 🌮 End-to-End Longitudinal Control 🌮 @@ -1035,15 +1035,15 @@ location set Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. - + 讓駕駛模型來控制油門及煞車。openpilot將會模擬人類的駕駛行為，包含在看見紅燈及停止標示時停車。由於車速將由駕駛模型決定，因此您設定的時速將成為速度上限。本功能仍在早期實驗階段，請預期模型有犯錯的可能性。 New Driving Visualization - + 新的駕駛視覺介面 The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - + 低速行駛時，將會切換成路側廣角鏡頭，以完整顯示轉彎路徑，右上角將出現實驗模式圖案。 diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2d4533afe1..c62d737481 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -55,9 +55,8 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con } void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, - float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { + float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); - QPolygonF left_points, right_points; left_points.reserve(max_idx + 1); right_points.reserve(max_idx + 1); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index d6f5c3e2e0..9e1c54948b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -20,7 +20,7 @@ const int bdr_s = 30; const int header_h = 420; const int footer_h = 280; -const int UI_FREQ = 20; // Hz +const int UI_FREQ = 20; // Hz typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; @@ -78,7 +78,7 @@ typedef enum UIStatus { } UIStatus; const QColor bg_colors [] = { - [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), + [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), [STATUS_WARNING] = QColor(0xDA, 0x6F, 0x25, 0xf1), @@ -152,7 +152,6 @@ private: UIState *uiState(); // device management class - class Device : public QObject { Q_OBJECT diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 2ee06e372a..091b0d91d9 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -96,10 +96,16 @@ const uint32_t ox03c10_analog_gains_reg[] = { const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x +const int ANALOG_GAIN_COST_DELTA_AR0231 = 0; +const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1; +const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0; const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; +const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; +const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.05; +const float ANALOG_GAIN_COST_HIGH_OX03C10 = 0.8; const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms @@ -532,6 +538,9 @@ void CameraState::camera_set_parameters() { analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; + analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; + analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; + analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; for (int i=0; i<=analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; } @@ -548,6 +557,9 @@ void CameraState::camera_set_parameters() { analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; + analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10; + analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10; + analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10; for (int i=0; i<=analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; } @@ -1104,13 +1116,13 @@ void CameraState::set_camera_exposure(float grey_frac) { float score = std::abs(desired_ev - (t * gain)) * 10; // Going below recommended gain needs lower penalty to not overexpose - float m = g > analog_gain_rec_idx ? 5.0 : 0.1; + float m = g > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; score += std::abs(g - (int)analog_gain_rec_idx) * m; // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); // Small penalty on changing gain - score += std::abs(g - gain_idx) * (score + 1.0) / 10.0; + score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (g - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(g - gain_idx) * (score + 1.0) / 10.0; if (score < best_ev_score) { new_t = t; diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 1e25e605c5..9f0c3743f1 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -41,6 +41,9 @@ public: int analog_gain_min_idx; int analog_gain_max_idx; int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; float cur_ev[3]; float min_ev, max_ev; diff --git a/system/hardware/base.h b/system/hardware/base.h index b70948d482..f6e0b42d73 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -20,6 +20,7 @@ public: static void poweroff() {} static void set_brightness(int percent) {} static void set_display_power(bool on) {} + static void set_volume(float volume) {} static bool get_ssh_enabled() { return false; } static void set_ssh_enabled(bool enabled) {} diff --git a/system/hardware/hw.h b/system/hardware/hw.h index f50e94abe1..5599e79186 100644 --- a/system/hardware/hw.h +++ b/system/hardware/hw.h @@ -7,15 +7,7 @@ #include "system/hardware/tici/hardware.h" #define Hardware HardwareTici #else -class HardwarePC : public HardwareNone { -public: - static std::string get_os_version() { return "openpilot for PC"; } - static std::string get_name() { return "pc"; }; - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; - static bool PC() { return true; } - static bool TICI() { return util::getenv("TICI", 0) == 1; } - static bool AGNOS() { return util::getenv("TICI", 0) == 1; } -}; +#include "system/hardware/pc/hardware.h" #define Hardware HardwarePC #endif diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h new file mode 100644 index 0000000000..529b4bfe9d --- /dev/null +++ b/system/hardware/pc/hardware.h @@ -0,0 +1,21 @@ +#pragma once + +#include "system/hardware/base.h" + +class HardwarePC : public HardwareNone { +public: + static std::string get_os_version() { return "openpilot for PC"; } + static std::string get_name() { return "pc"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; + static bool PC() { return true; } + static bool TICI() { return util::getenv("TICI", 0) == 1; } + static bool AGNOS() { return util::getenv("TICI", 0) == 1; } + + static void set_volume(float volume) { + volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); + + char volume_str[6]; + snprintf(volume_str, sizeof(volume_str), "%.3f", volume); + std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); + } +}; diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index dcccb9f3d1..d388f9c48a 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -10,7 +10,7 @@ class HardwareTici : public HardwareNone { public: static constexpr float MAX_VOLUME = 0.9; - static constexpr float MIN_VOLUME = 0.2; + static constexpr float MIN_VOLUME = 0.1; static bool TICI() { return true; } static bool AGNOS() { return true; } static std::string get_os_version() { @@ -38,6 +38,13 @@ public: bl_power_control.close(); } }; + static void set_volume(float volume) { + volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); + + char volume_str[6]; + snprintf(volume_str, sizeof(volume_str), "%.3f", volume); + std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); + } static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index e2fd20c1be..b5f5e00410 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -416,6 +416,7 @@ class Tici(HardwareBase): # *** IRQ config *** affine_irq(5, 565) # kgsl-3d0 + affine_irq(4, 126) # SPI goes on boardd core affine_irq(4, 740) # xhci-hcd:usb1 goes on the boardd core affine_irq(4, 1069) # xhci-hcd:usb3 goes on the boardd core for irq in range(237, 246): diff --git a/system/micd.py b/system/micd.py new file mode 100755 index 0000000000..a56140e3b9 --- /dev/null +++ b/system/micd.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +import sounddevice as sd +import numpy as np + +from cereal import messaging +from common.filter_simple import FirstOrderFilter +from common.realtime import Ratekeeper +from system.swaglog import cloudlog + +RATE = 10 +FFT_SAMPLES = 4096 +REFERENCE_SPL = 2e-5 # newtons/m^2 +SAMPLE_RATE = 44100 +FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES) + + +def calculate_spl(measurements): + # https://www.engineeringtoolbox.com/sound-pressure-d_711.html + sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes + if sound_pressure > 0: + sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB + else: + sound_pressure_level = 0 + return sound_pressure, sound_pressure_level + + +def apply_a_weighting(measurements: np.ndarray) -> np.ndarray: + # Generate a Hanning window of the same length as the audio measurements + measurements_windowed = measurements * np.hanning(len(measurements)) + + # Calculate the frequency axis for the signal + freqs = np.fft.fftfreq(measurements_windowed.size, d=1 / SAMPLE_RATE) + + # Calculate the A-weighting filter + # https://en.wikipedia.org/wiki/A-weighting + A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2))) + A /= np.max(A) # Normalize the filter + + # Apply the A-weighting filter to the signal + return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * A)) + + +class Mic: + def __init__(self, pm): + self.pm = pm + self.rk = Ratekeeper(RATE) + + self.measurements = np.empty(0) + + self.sound_pressure = 0 + self.sound_pressure_weighted = 0 + self.sound_pressure_level_weighted = 0 + + self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + + def update(self): + msg = messaging.new_message('microphone') + msg.microphone.soundPressure = float(self.sound_pressure) + msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted) + + msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted) + msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x) + + self.pm.send('microphone', msg) + self.rk.keep_time() + + def callback(self, indata, frames, time, status): + """ + Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level. + Then apply A-weighting to the raw amplitudes and run the same calculations again. + + Logged A-weighted equivalents are rough approximations of the human-perceived loudness. + """ + + self.measurements = np.concatenate((self.measurements, indata[:, 0])) + + while self.measurements.size >= FFT_SAMPLES: + measurements = self.measurements[:FFT_SAMPLES] + + self.sound_pressure, _ = calculate_spl(measurements) + measurements_weighted = apply_a_weighting(measurements) + self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted) + self.spl_filter_weighted.update(self.sound_pressure_level_weighted) + + self.measurements = self.measurements[FFT_SAMPLES:] + + def micd_thread(self, device=None): + if device is None: + device = "sysdefault" + + with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") + while True: + self.update() + + +def main(pm=None): + if pm is None: + pm = messaging.PubMaster(['microphone']) + + mic = Mic(pm) + mic.micd_thread() + + +if __name__ == "__main__": + main() diff --git a/system/version.py b/system/version.py index f0817b3a9f..6031531556 100644 --- a/system/version.py +++ b/system/version.py @@ -7,7 +7,8 @@ from functools import lru_cache from common.basedir import BASEDIR from system.swaglog import cloudlog -TESTED_BRANCHES = ['devel', 'release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] +RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] +TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging'] training_version: bytes = b"0.2.0" terms_version: bytes = b"2" @@ -96,6 +97,9 @@ def is_comma_remote() -> bool: def is_tested_branch() -> bool: return get_short_branch() in TESTED_BRANCHES +@cache +def is_release_branch() -> bool: + return get_short_branch() in RELEASE_BRANCHES @cache def is_dirty() -> bool: diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 3ff4862800..52fe9e346d 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -17,10 +17,13 @@ cabana_env = qt_env.Clone() prev_moc_path = cabana_env['QT_MOCHPREFIX'] cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_' -cabana_env.Execute('./generate_dbc_json.py --out car_fingerprint_to_dbc.json') cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc', 'dbcmanager.cc', 'canmessages.cc', 'commands.cc', 'messageswidget.cc', 'settings.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) cabana_env.Program('_cabana', ['cabana.cc', cabana_lib], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) if GetOption('test'): cabana_env.Program('tests/_test_cabana', ['tests/test_runner.cc', 'tests/test_cabana.cc', cabana_lib], LIBS=[cabana_libs]) + +def generate_dbc_json(target, source, env): + env.Execute('tools/cabana/generate_dbc_json.py --out tools/cabana/car_fingerprint_to_dbc.json') +cabana_env.Command('generate_dbc_json', [], generate_dbc_json) diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index dae4591976..b98c75d452 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -1,6 +1,5 @@ #include "tools/cabana/binaryview.h" -#include #include #include #include @@ -11,7 +10,7 @@ // BinaryView -const int CELL_HEIGHT = 26; +const int CELL_HEIGHT = 36; inline int get_bit_index(const QModelIndex &index, bool little_endian) { return index.row() * 8 + (little_endian ? 7 - index.column() : index.column()); @@ -24,14 +23,14 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) { setItemDelegate(delegate); horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); verticalHeader()->setSectionsClickable(false); + verticalHeader()->setSectionResizeMode(QHeaderView::Fixed); + verticalHeader()->setDefaultSectionSize(CELL_HEIGHT); horizontalHeader()->hide(); - setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); setFrameShape(QFrame::NoFrame); setShowGrid(false); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); setMouseTracking(true); + setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); } void BinaryView::highlight(const Signal *sig) { @@ -57,7 +56,7 @@ void BinaryView::setSelection(const QRect &rect, QItemSelectionModel::SelectionF } void BinaryView::mousePressEvent(QMouseEvent *event) { - delegate->setSelectionColor(style()->standardPalette().color(QPalette::Active, QPalette::Highlight)); + delegate->selection_color = (palette().color(QPalette::Active, QPalette::Highlight)); if (auto index = indexAt(event->pos()); index.isValid() && index.column() != 8) { anchor_index = index; auto item = (const BinaryViewModel::Item *)anchor_index.internalPointer(); @@ -66,7 +65,7 @@ void BinaryView::mousePressEvent(QMouseEvent *event) { if (bit_idx == s->lsb || bit_idx == s->msb) { anchor_index = model->bitIndex(bit_idx == s->lsb ? s->msb : s->lsb, true); resize_sig = s; - delegate->setSelectionColor(item->bg_color); + delegate->selection_color = item->bg_color; break; } } @@ -74,14 +73,17 @@ void BinaryView::mousePressEvent(QMouseEvent *event) { event->accept(); } -void BinaryView::mouseMoveEvent(QMouseEvent *event) { - if (auto index = indexAt(event->pos()); index.isValid()) { +void BinaryView::highlightPosition(const QPoint &pos) { + if (auto index = indexAt(viewport()->mapFromGlobal(pos)); index.isValid()) { auto item = (BinaryViewModel::Item *)index.internalPointer(); const Signal *sig = item->sigs.isEmpty() ? nullptr : item->sigs.back(); highlight(sig); - sig ? QToolTip::showText(event->globalPos(), sig->name.c_str(), this, rect()) - : QToolTip::hideText(); + QToolTip::showText(pos, sig ? sig->name.c_str() : "", this, rect()); } +} + +void BinaryView::mouseMoveEvent(QMouseEvent *event) { + highlightPosition(event->globalPos()); QTableView::mouseMoveEvent(event); } @@ -116,6 +118,7 @@ void BinaryView::setMessage(const QString &message_id) { anchor_index = QModelIndex(); resize_sig = nullptr; hovered_sig = nullptr; + highlightPosition(QCursor::pos()); updateState(); } @@ -176,15 +179,14 @@ void BinaryViewModel::updateState() { auto prev_items = items; const auto &binary = can->lastMessage(msg_id).dat; // data size may changed. - if (!dbc_msg && binary.size() != row_count) { - beginResetModel(); + if (binary.size() > row_count) { + beginInsertRows({}, row_count, binary.size() - 1); row_count = binary.size(); - items.clear(); items.resize(row_count * column_count); - endResetModel(); + endInsertRows(); } char hex[3] = {'\0'}; - for (int i = 0; i < std::min(binary.size(), row_count); ++i) { + for (int i = 0; i < binary.size(); ++i) { for (int j = 0; j < column_count - 1; ++j) { items[i * column_count + j].val = ((binary[i] >> (7 - j)) & 1) != 0 ? '1' : '0'; } @@ -192,8 +194,13 @@ void BinaryViewModel::updateState() { hex[1] = toHex(binary[i] & 0xf); items[i * column_count + 8].val = hex; } + for (int i = binary.size(); i < row_count; ++i) { + for (int j = 0; j < column_count; ++j) { + items[i * column_count + j].val = "-"; + } + } - for (int i = 0; i < items.size(); ++i) { + for (int i = 0; i < row_count * column_count; ++i) { if (i >= prev_items.size() || prev_items[i].val != items[i].val) { auto idx = index(i / column_count, i % column_count); emit dataChanged(idx, idx); @@ -205,7 +212,7 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i if (orientation == Qt::Vertical) { switch (role) { case Qt::DisplayRole: return section; - case Qt::SizeHintRole: return QSize(30, CELL_HEIGHT); + case Qt::SizeHintRole: return QSize(30, 0); case Qt::TextAlignmentRole: return Qt::AlignCenter; } } @@ -215,16 +222,9 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i // BinaryItemDelegate BinaryItemDelegate::BinaryItemDelegate(QObject *parent) : QStyledItemDelegate(parent) { - // cache fonts and color - small_font.setPointSize(6); + small_font.setPixelSize(8); hex_font = QFontDatabase::systemFont(QFontDatabase::FixedFont); hex_font.setBold(true); - selection_color = QApplication::style()->standardPalette().color(QPalette::Active, QPalette::Highlight); -} - -QSize BinaryItemDelegate::sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const { - QSize sz = QStyledItemDelegate::sizeHint(option, index); - return {sz.width(), CELL_HEIGHT}; } void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { @@ -232,24 +232,20 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op BinaryView *bin_view = (BinaryView *)parent(); painter->save(); - // background - if (option.state & QStyle::State_Selected) { + if (index.column() == 8) { + painter->setFont(hex_font); + } else if (option.state & QStyle::State_Selected) { painter->fillRect(option.rect, selection_color); - } else if (!bin_view->selectionModel()->hasSelection() || !item->sigs.contains(bin_view->resize_sig)) { + painter->setPen(option.palette.color(QPalette::BrightText)); + } else if (!item->sigs.isEmpty() && (!bin_view->selectionModel()->hasSelection() || !item->sigs.contains(bin_view->resize_sig))) { painter->fillRect(option.rect, item->bg_color); + painter->setPen(item->sigs.contains(bin_view->hovered_sig) ? option.palette.color(QPalette::BrightText) : Qt::black); } - // text - if (index.column() == 8) { // hex column - painter->setFont(hex_font); - } else if (option.state & QStyle::State_Selected || (!bin_view->resize_sig && item->sigs.contains(bin_view->hovered_sig))) { - painter->setPen(Qt::white); - } painter->drawText(option.rect, Qt::AlignCenter, item->val); if (item->is_msb || item->is_lsb) { painter->setFont(small_font); painter->drawText(option.rect, Qt::AlignHCenter | Qt::AlignBottom, item->is_msb ? "MSB" : "LSB"); } - painter->restore(); } diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index 0ea111d917..e936efc658 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -8,22 +9,16 @@ #include "tools/cabana/dbcmanager.h" class BinaryItemDelegate : public QStyledItemDelegate { - Q_OBJECT - public: BinaryItemDelegate(QObject *parent); void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; - QSize sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const override; void setSelectionColor(const QColor &color) { selection_color = color; } -private: QFont small_font, hex_font; QColor selection_color; }; class BinaryViewModel : public QAbstractTableModel { - Q_OBJECT - public: BinaryViewModel(QObject *parent) : QAbstractTableModel(parent) {} void setMessage(const QString &message_id); @@ -41,7 +36,7 @@ public: } struct Item { - QColor bg_color = QColor(Qt::white); + QColor bg_color = QApplication::style()->standardPalette().color(QPalette::Base); bool is_msb = false; bool is_lsb = false; QString val = "0"; @@ -51,7 +46,7 @@ public: private: QString msg_id; - const DBCMsg *dbc_msg; + const DBCMsg *dbc_msg = nullptr; int row_count = 0; const int column_count = 9; }; @@ -79,6 +74,7 @@ private: void mouseMoveEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void leaveEvent(QEvent *event) override; + void highlightPosition(const QPoint &pt); QModelIndex anchor_index; BinaryViewModel *model; diff --git a/tools/cabana/cabana b/tools/cabana/cabana index b29dd66e3d..14647b6a10 100755 --- a/tools/cabana/cabana +++ b/tools/cabana/cabana @@ -1,4 +1,4 @@ #!/bin/sh cd "$(dirname "$0")" export LD_LIBRARY_PATH="../../opendbc/can:$LD_LIBRARY_PATH" -exec ./_cabana "$1" +exec ./_cabana "$@" diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 1096487973..51418e293f 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -1,20 +1,20 @@ #include #include -#include +#include "common/prefix.h" #include "selfdrive/ui/qt/util.h" #include "tools/cabana/mainwin.h" int main(int argc, char *argv[]) { initApp(argc, argv); QApplication app(argc, argv); - app.setStyle(QStyleFactory::create("Fusion")); QCommandLineParser cmd_parser; cmd_parser.addHelpOption(); cmd_parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai"); cmd_parser.addOption({"demo", "use a demo route instead of providing your own"}); cmd_parser.addOption({"qcam", "load qcamera"}); + cmd_parser.addOption({"ecam", "load wide road camera"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); cmd_parser.process(app); const QStringList args = cmd_parser.positionalArguments(); @@ -22,12 +22,22 @@ int main(int argc, char *argv[]) { cmd_parser.showHelp(); } + const QString route = args.empty() ? DEMO_ROUTE : args.first(); + uint32_t replay_flags = REPLAY_FLAG_NONE; + if (cmd_parser.isSet("ecam")) { + replay_flags |= REPLAY_FLAG_ECAM; + } else if (cmd_parser.isSet("qcam")) { + replay_flags |= REPLAY_FLAG_QCAMERA; + } + + OpenpilotPrefix op_prefix; CANMessages p(&app); - if (!p.loadRoute(route, cmd_parser.value("data_dir"), cmd_parser.isSet("qcam"))) { - return 0; + int ret = 0; + if (p.loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { + MainWindow w; + w.showMaximized(); + ret = app.exec(); } - MainWindow w; - w.showMaximized(); - return app.exec(); + return ret; } diff --git a/tools/cabana/canmessages.cc b/tools/cabana/canmessages.cc index 3bcaae4bbd..ad3ee80e3d 100644 --- a/tools/cabana/canmessages.cc +++ b/tools/cabana/canmessages.cc @@ -1,7 +1,4 @@ #include "tools/cabana/canmessages.h" - -#include - #include "tools/cabana/dbcmanager.h" CANMessages *can = nullptr; @@ -21,43 +18,25 @@ static bool event_filter(const Event *e, void *opaque) { return c->eventFilter(e); } -bool CANMessages::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) { - replay = new Replay(route, {"can", "roadEncodeIdx", "carParams"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this); +bool CANMessages::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { + replay = new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this); replay->setSegmentCacheLimit(settings.cached_segment_limit); replay->installEventFilter(event_filter, this); + QObject::connect(replay, &Replay::seekedTo, this, &CANMessages::seekedTo); QObject::connect(replay, &Replay::segmentsMerged, this, &CANMessages::eventsMerged); QObject::connect(replay, &Replay::streamStarted, this, &CANMessages::streamStarted); 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; })) { + qWarning() << "no rlogs in route" << route; + return false; + } replay->start(); return true; } return false; } -QList CANMessages::findSignalValues(const QString &id, const Signal *signal, double value, FindFlags flag, int max_count) { - auto evts = events(); - if (!evts) return {}; - - QList ret; - ret.reserve(max_count); - auto [bus, address] = DBCManager::parseId(id); - for (auto &evt : *evts) { - if (evt->which != cereal::Event::Which::CAN) continue; - - for (const auto &c : evt->event.getCan()) { - if (bus == c.getSrc() && address == c.getAddress()) { - double val = get_raw_value((uint8_t *)c.getDat().begin(), c.getDat().size(), *signal); - if ((flag == EQ && val == value) || (flag == LT && val < value) || (flag == GT && val > value)) { - ret.push_back({(evt->mono_time / (double)1e9) - can->routeStartTime(), val}); - if (ret.size() >= max_count) - return ret; - } - } - } - } - return ret; -} - void CANMessages::process(QHash *messages) { for (auto it = messages->begin(); it != messages->end(); ++it) { can_msgs[it.key()] = it.value(); @@ -69,17 +48,13 @@ void CANMessages::process(QHash *messages) { } bool CANMessages::eventFilter(const Event *event) { - static std::unique_ptr> new_msgs; + static std::unique_ptr new_msgs = std::make_unique>(); static double prev_update_ts = 0; if (event->which == cereal::Event::Which::CAN) { - if (!new_msgs) { - new_msgs.reset(new QHash); - new_msgs->reserve(1000); - } - double current_sec = replay->currentSeconds(); if (counters_begin_sec == 0 || counters_begin_sec >= current_sec) { + new_msgs->clear(); counters.clear(); counters_begin_sec = current_sec; } @@ -87,43 +62,33 @@ bool CANMessages::eventFilter(const Event *event) { auto can_events = event->event.getCan(); for (const auto &c : can_events) { QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); - - std::lock_guard lk(lock); - auto &list = received_msgs[id]; - while (list.size() > settings.can_msg_log_size) { - list.pop_back(); - } - CanData &data = list.emplace_front(); + CanData &data = (*new_msgs)[id]; data.ts = current_sec; - data.dat.append((char *)c.getDat().begin(), c.getDat().size()); - + data.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); data.count = ++counters[id]; if (double delta = (current_sec - counters_begin_sec); delta > 0) { data.freq = data.count / delta; } - (*new_msgs)[id] = data; } double ts = millis_since_boot(); - if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing) { + if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing && !new_msgs->isEmpty()) { // delay posting CAN message if UI thread is busy processing = true; prev_update_ts = ts; // use pointer to avoid data copy in queued connection. emit received(new_msgs.release()); + new_msgs.reset(new QHash); + new_msgs->reserve(100); } } return true; } -const std::deque CANMessages::messages(const QString &id) { - std::lock_guard lk(lock); - return received_msgs[id]; -} - void CANMessages::seekTo(double ts) { replay->seekTo(std::max(double(0), ts), false); counters_begin_sec = 0; + emit updated(); } void CANMessages::settingChanged() { diff --git a/tools/cabana/canmessages.h b/tools/cabana/canmessages.h index 4cb0f403a0..4dac4fe9df 100644 --- a/tools/cabana/canmessages.h +++ b/tools/cabana/canmessages.h @@ -1,12 +1,9 @@ #pragma once #include -#include -#include #include #include -#include #include "opendbc/can/common_dbc.h" #include "tools/cabana/settings.h" @@ -23,22 +20,21 @@ class CANMessages : public QObject { Q_OBJECT public: - enum FindFlags{ EQ, LT, GT }; CANMessages(QObject *parent); ~CANMessages(); - bool loadRoute(const QString &route, const QString &data_dir, bool use_qcam); + bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); void seekTo(double ts); - QList findSignalValues(const QString&id, const Signal* signal, double value, FindFlags flag, int max_count); bool eventFilter(const Event *event); - inline QString route() const { return replay->route()->name(); } + inline QString routeName() const { return replay->route()->name(); } inline QString carFingerprint() const { return replay->carFingerprint().c_str(); } + inline VisionStreamType visionStreamType() const { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } inline double totalSeconds() const { return replay->totalSeconds(); } inline double routeStartTime() const { return replay->routeStartTime() / (double)1e9; } inline double currentSec() const { return replay->currentSeconds(); } - const std::deque messages(const QString &id); inline const CanData &lastMessage(const QString &id) { return can_msgs[id]; } + inline const Route* route() const { return replay->route(); } inline const std::vector *events() const { return replay->events(); } inline void setSpeed(float speed) { replay->setSpeed(speed); } inline bool isPaused() const { return replay->isPaused(); } @@ -46,6 +42,7 @@ public: inline const std::vector> getTimeline() { return replay->getTimeline(); } signals: + void seekedTo(double sec); void streamStarted(); void eventsMerged(); void updated(); @@ -60,11 +57,9 @@ protected: void settingChanged(); Replay *replay = nullptr; - std::mutex lock; std::atomic counters_begin_sec = 0; std::atomic processing = false; QHash counters; - QHash> received_msgs; }; inline QString toHex(const QByteArray &dat) { diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 5a129b5f61..bc28666a34 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -1,11 +1,13 @@ #include "tools/cabana/chartswidget.h" +#include +#include #include #include #include -#include #include #include +#include #include // ChartsWidget @@ -19,6 +21,7 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { title_label = new QLabel(); title_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); toolbar->addWidget(title_label); + show_all_values_btn = toolbar->addAction(""); toolbar->addWidget(range_label = new QLabel()); reset_zoom_btn = toolbar->addAction("⟲"); reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); @@ -26,7 +29,6 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { remove_all_btn->setToolTip(tr("Remove all charts")); dock_btn = toolbar->addAction(""); main_layout->addWidget(toolbar); - updateToolBar(); // charts QWidget *charts_container = new QWidget(this); @@ -37,12 +39,16 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { charts_scroll->setWidgetResizable(true); charts_scroll->setWidget(charts_container); charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - main_layout->addWidget(charts_scroll); + max_chart_range = settings.max_chart_x_range; + use_dark_theme = palette().color(QPalette::WindowText).value() > palette().color(QPalette::Background).value(); + updateToolBar(); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &ChartsWidget::removeAll); QObject::connect(can, &CANMessages::eventsMerged, this, &ChartsWidget::eventsMerged); QObject::connect(can, &CANMessages::updated, this, &ChartsWidget::updateState); + QObject::connect(show_all_values_btn, &QAction::triggered, this, &ChartsWidget::showAllData); QObject::connect(remove_all_btn, &QAction::triggered, this, &ChartsWidget::removeAll); QObject::connect(reset_zoom_btn, &QAction::triggered, this, &ChartsWidget::zoomReset); QObject::connect(dock_btn, &QAction::triggered, [this]() { @@ -54,13 +60,25 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { void ChartsWidget::eventsMerged() { if (auto events = can->events(); events && !events->empty()) { - auto it = std::find_if(events->begin(), events->end(), [=](const Event *e) { return e->which == cereal::Event::Which::CAN; }); - event_range.first = it == events->end() ? 0 : (*it)->mono_time / (double)1e9 - can->routeStartTime(); - event_range.second = it == events->end() ? 0 : events->back()->mono_time / (double)1e9 - can->routeStartTime(); - if (display_range.first == 0 && event_range.second == 0) { - display_range.first = event_range.first; - display_range.second = std::min(event_range.first + settings.max_chart_x_range, event_range.second); - } + event_range.first = (events->front()->mono_time / (double)1e9) - can->routeStartTime(); + event_range.second = (events->back()->mono_time / (double)1e9) - can->routeStartTime(); + updateState(); + } +} + +void ChartsWidget::updateDisplayRange() { + auto prev_range = display_range; + double current_sec = can->currentSec(); + if (current_sec < display_range.first || current_sec >= (display_range.second - 5)) { + // reached the end, or seeked to a timestamp out of range. + display_range.first = current_sec - 5; + } + display_range.first = std::floor(std::max(display_range.first, event_range.first) * 10.0) / 10.0; + display_range.second = std::floor(std::min(display_range.first + max_chart_range, event_range.second) * 10.0) / 10.0; + if (prev_range != display_range) { + QFutureSynchronizer future_synchronizer; + for (auto c : charts) + future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::setEventsRange, display_range)); } } @@ -79,34 +97,36 @@ void ChartsWidget::zoomReset() { void ChartsWidget::updateState() { if (charts.isEmpty()) return; - const double current_sec = can->currentSec(); - if (is_zoomed) { - if (current_sec < zoomed_range.first || current_sec >= zoomed_range.second) { - can->seekTo(zoomed_range.first); - } - } else { - auto prev_range = display_range; - if (current_sec < display_range.first || current_sec >= (display_range.second - 5)) { - // line marker reached the end, or seeked to a timestamp out of range. - display_range.first = current_sec - 5; - } - display_range.first = std::max(display_range.first, event_range.first); - display_range.second = std::min(display_range.first + settings.max_chart_x_range, event_range.second); - if (prev_range != display_range) { - QFutureSynchronizer future_synchronizer; - for (auto c : charts) - future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::setEventsRange, display_range)); - } + if (!is_zoomed) { + updateDisplayRange(); + } else if (can->currentSec() < zoomed_range.first || can->currentSec() >= zoomed_range.second) { + can->seekTo(zoomed_range.first); } const auto &range = is_zoomed ? zoomed_range : display_range; + setUpdatesEnabled(false); for (auto c : charts) { c->setDisplayRange(range.first, range.second); c->scene()->invalidate({}, QGraphicsScene::ForegroundLayer); } + setUpdatesEnabled(true); +} + +void ChartsWidget::showAllData() { + bool switch_to_show_all = max_chart_range == settings.max_chart_x_range; + max_chart_range = switch_to_show_all ? settings.cached_segment_limit * 60 + : settings.max_chart_x_range; + max_chart_range = std::min(max_chart_range, (uint32_t)can->totalSeconds()); + updateToolBar(); + updateState(); } void ChartsWidget::updateToolBar() { + int min_range = std::min(settings.max_chart_x_range, (int)can->totalSeconds()); + bool displaying_all = max_chart_range != min_range; + show_all_values_btn->setText(tr("%1 minutes").arg(max_chart_range / 60)); + show_all_values_btn->setToolTip(tr("Click to display %1 data").arg(displaying_all ? tr("%1 minutes").arg(min_range / 60) : tr("ALL cached"))); + show_all_values_btn->setVisible(!is_zoomed); remove_all_btn->setEnabled(!charts.isEmpty()); reset_zoom_btn->setEnabled(is_zoomed); range_label->setText(is_zoomed ? tr("%1 - %2").arg(zoomed_range.first, 0, 'f', 2).arg(zoomed_range.second, 0, 'f', 2) : ""); @@ -122,38 +142,45 @@ ChartView *ChartsWidget::findChart(const QString &id, const Signal *sig) { } void ChartsWidget::showChart(const QString &id, const Signal *sig, bool show, bool merge) { - if (!show) { - if (ChartView *chart = findChart(id, sig)) { - chart->removeSeries(id, sig); - } - } else { + setUpdatesEnabled(false); + if (show) { ChartView *chart = merge && charts.size() > 0 ? charts.back() : nullptr; if (!chart) { chart = new ChartView(this); + chart->chart()->setTheme(use_dark_theme ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); chart->setEventsRange(display_range); + auto range = is_zoomed ? zoomed_range : display_range; + chart->setDisplayRange(range.first, range.second); QObject::connect(chart, &ChartView::remove, [=]() { removeChart(chart); }); QObject::connect(chart, &ChartView::zoomIn, this, &ChartsWidget::zoomIn); QObject::connect(chart, &ChartView::zoomReset, this, &ChartsWidget::zoomReset); - QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::chartClosed); + QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::seriesChanged); + QObject::connect(chart, &ChartView::seriesAdded, this, &ChartsWidget::seriesChanged); charts_layout->insertWidget(0, chart); charts.push_back(chart); } chart->addSeries(id, sig); - emit chartOpened(id, sig); - updateState(); + } else if (ChartView *chart = findChart(id, sig)) { + chart->removeSeries(id, sig); } updateToolBar(); + setUpdatesEnabled(true); } void ChartsWidget::removeChart(ChartView *chart) { charts.removeOne(chart); chart->deleteLater(); updateToolBar(); + emit seriesChanged(); } void ChartsWidget::removeAll() { - for (auto c : charts.toVector()) - removeChart(c); + for (auto c : charts) { + c->deleteLater(); + } + charts.clear(); + updateToolBar(); + emit seriesChanged(); } bool ChartsWidget::eventFilter(QObject *obj, QEvent *event) { @@ -175,18 +202,8 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { chart->addAxis(axis_y, Qt::AlignLeft); chart->legend()->setShowToolTips(true); chart->layout()->setContentsMargins(0, 0, 0, 0); - // top margin for title - chart->setMargins({0, 11, 0, 0}); - - track_line = new QGraphicsLineItem(chart); - track_line->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); - track_ellipse = new QGraphicsEllipseItem(chart); - track_ellipse->setBrush(Qt::darkGray); - value_text = new QGraphicsTextItem(chart); - item_group = scene()->createItemGroup({track_line, track_ellipse, value_text}); - item_group->setZValue(chart->zValue() + 10); - - // title + chart->setMargins(QMargins(20, 11, 11, 11)); + QToolButton *remove_btn = new QToolButton(); remove_btn->setText("X"); remove_btn->setAutoRaise(true); @@ -195,35 +212,31 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { close_btn_proxy->setWidget(remove_btn); close_btn_proxy->setZValue(chart->zValue() + 11); + QToolButton *manage_btn = new QToolButton(); + manage_btn->setText("🔧"); + manage_btn->setAutoRaise(true); + manage_btn->setToolTip(tr("Manage series")); + manage_btn_proxy = new QGraphicsProxyWidget(chart); + manage_btn_proxy->setWidget(manage_btn); + manage_btn_proxy->setZValue(chart->zValue() + 11); + setChart(chart); setRenderHint(QPainter::Antialiasing); setRubberBand(QChartView::HorizontalRubberBand); updateFromSettings(); - QTimer *timer = new QTimer(this); - timer->setInterval(100); - timer->setSingleShot(true); - timer->callOnTimeout(this, &ChartView::adjustChartMargins); - QObject::connect(dbc(), &DBCManager::signalRemoved, this, &ChartView::signalRemoved); QObject::connect(dbc(), &DBCManager::signalUpdated, this, &ChartView::signalUpdated); QObject::connect(dbc(), &DBCManager::msgRemoved, this, &ChartView::msgRemoved); QObject::connect(dbc(), &DBCManager::msgUpdated, this, &ChartView::msgUpdated); QObject::connect(&settings, &Settings::changed, this, &ChartView::updateFromSettings); QObject::connect(remove_btn, &QToolButton::clicked, this, &ChartView::remove); - QObject::connect(chart, &QChart::plotAreaChanged, [=](const QRectF &plotArea) { - // use a singleshot timer to avoid recursion call. - timer->start(); - }); -} - -ChartView::~ChartView() { - for (auto &s : sigs) - emit seriesRemoved(s.msg_id, s.sig); + QObject::connect(manage_btn, &QToolButton::clicked, this, &ChartView::manageSeries); } void ChartView::addSeries(const QString &msg_id, const Signal *sig) { QLineSeries *series = new QLineSeries(this); + series->setUseOpenGL(true); chart()->addSeries(series); series->attachAxis(axis_x); series->attachAxis(axis_y); @@ -231,6 +244,8 @@ void ChartView::addSeries(const QString &msg_id, const Signal *sig) { sigs.push_back({.msg_id = msg_id, .address = address, .source = source, .sig = sig, .series = series}); updateTitle(); updateSeries(sig); + updateAxisY(); + emit seriesAdded(msg_id, sig); } void ChartView::removeSeries(const QString &msg_id, const Signal *sig) { @@ -241,16 +256,16 @@ void ChartView::removeSeries(const QString &msg_id, const Signal *sig) { } bool ChartView::hasSeries(const QString &msg_id, const Signal *sig) const { - auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); - return it != sigs.end(); + return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); } QList::iterator ChartView::removeSeries(const QList::iterator &it) { chart()->removeSeries(it->series); it->series->deleteLater(); - emit seriesRemoved(it->msg_id, it->sig); - + QString msg_id = it->msg_id; + const Signal *sig = it->sig; auto ret = sigs.erase(it); + emit seriesRemoved(msg_id, sig); if (!sigs.isEmpty()) { updateAxisY(); } else { @@ -260,11 +275,11 @@ QList::iterator ChartView::removeSeries(const QListname)); + } + + int ret = dlg.exec(); + if (ret == QDialog::Accepted) { + QList series_list = dlg.series(); + if (series_list.isEmpty()) { + emit remove(); + } else { + for (auto &s : series_list) { + if (auto m = dbc()->msg(s[0])) { + auto it = m->sigs.find(s[2]); + if (it != m->sigs.end() && !hasSeries(s[0], &(it->second))) { + addSeries(s[0], &(it->second)); + } + } + } + for (auto it = sigs.begin(); it != sigs.end(); /**/) { + bool exists = std::any_of(series_list.cbegin(), series_list.cend(), [&](auto &s) { + return s[0] == it->msg_id && s[2] == it->sig->name.c_str(); + }); + it = exists ? ++it : removeSeries(it); + } + } + } +} + void ChartView::resizeEvent(QResizeEvent *event) { QChartView::resizeEvent(event); - close_btn_proxy->setPos(event->size().width() - close_btn_proxy->size().width() - 11, 8); + int x = event->size().width() - close_btn_proxy->size().width() - 11; + close_btn_proxy->setPos(x, 8); + manage_btn_proxy->setPos(x - manage_btn_proxy->size().width() - 5, 8); } void ChartView::updateTitle() { @@ -299,8 +345,6 @@ void ChartView::updateTitle() { void ChartView::updateFromSettings() { setFixedHeight(settings.chart_height); - chart()->setTheme(settings.chart_theme == 0 ? QChart::ChartThemeLight : QChart::QChart::ChartThemeDark); - auto color = chart()->titleBrush().color(); } void ChartView::setEventsRange(const std::pair &range) { @@ -320,18 +364,19 @@ void ChartView::setDisplayRange(double min, double max) { void ChartView::adjustChartMargins() { // TODO: Remove hardcoded aligned_pos const int aligned_pos = 60; - if (chart()->plotArea().left() != aligned_pos) { + if ((int)chart()->plotArea().left() != aligned_pos) { const float left_margin = chart()->margins().left() + aligned_pos - chart()->plotArea().left(); - chart()->setMargins(QMargins(left_margin, 11, 0, 0)); + chart()->setMargins(QMargins(left_margin, 11, 11, 11)); + scene()->invalidate({}, QGraphicsScene::ForegroundLayer); } } void ChartView::updateSeries(const Signal *sig) { auto events = can->events(); - if (!events) return; + if (!events || sigs.isEmpty()) return; - for (int i = 0; i < sigs.size(); ++i) { - if (auto &s = sigs[i]; !sig || s.sig == sig) { + for (auto &s : sigs) { + if (!sig || s.sig == sig) { s.vals.clear(); s.vals.reserve((events_range.second - events_range.first) * 1000); // [n]seconds * 1000hz s.min_y = std::numeric_limits::max(); @@ -357,16 +402,15 @@ void ChartView::updateSeries(const Signal *sig) { } } } - - QLineSeries *series = (QLineSeries *)chart()->series()[i]; - series->replace(s.vals); + s.series->replace(s.vals); } } - updateAxisY(); } // auto zoom on yaxis void ChartView::updateAxisY() { + if (sigs.isEmpty()) return; + double min_y = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); if (events_range == std::pair{axis_x->min(), axis_x->max()}) { @@ -377,27 +421,57 @@ void ChartView::updateAxisY() { } else { 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; }); - if (begin == s.vals.end()) - return; - - auto end = std::upper_bound(s.vals.begin(), s.vals.end(), axis_x->max(), [](double x, auto &p) { return x < p.x(); }); - const auto [min, max] = std::minmax_element(begin, end, [](auto &p1, auto &p2) { return p1.y() < p2.y(); }); - if (min->y() < min_y) min_y = min->y(); - if (max->y() > max_y) max_y = max->y(); + for (auto it = begin; it != s.vals.end() && it->x() <= axis_x->max(); ++it) { + if (it->y() < min_y) min_y = it->y(); + if (it->y() > max_y) max_y = it->y(); + } } } + if (min_y == std::numeric_limits::max()) min_y = 0; + if (max_y == std::numeric_limits::lowest()) max_y = 0; if (max_y == min_y) { axis_y->setRange(min_y - 1, max_y + 1); } else { double range = max_y - min_y; - axis_y->setRange(min_y - range * 0.05, max_y + range * 0.05); - axis_y->applyNiceNumbers(); + applyNiceNumbers(min_y - range * 0.05, max_y + range * 0.05); } + + adjustChartMargins(); +} + +void ChartView::applyNiceNumbers(qreal min, qreal max) { + int tick_count = axis_y->tickCount(); + qreal range = niceNumber((max - min), true); // range with ceiling + qreal step = niceNumber(range / (tick_count - 1), false); + min = qFloor(min / step); + max = qCeil(max / step); + tick_count = int(max - min) + 1; + axis_y->setRange(min * step, max * step); + axis_y->setTickCount(tick_count); +} + +// nice numbers can be expressed as form of 1*10^n, 2* 10^n or 5*10^n +qreal ChartView::niceNumber(qreal x, bool ceiling) { + qreal z = qPow(10, qFloor(std::log10(x))); //find corresponding number of the form of 10^n than is smaller than x + qreal q = x / z; //q<10 && q>=1; + if (ceiling) { + if (q <= 1.0) q = 1; + else if (q <= 2.0) q = 2; + else if (q <= 5.0) q = 5; + else q = 10; + } else { + if (q < 1.5) q = 1; + else if (q < 3.0) q = 2; + else if (q < 7.0) q = 5; + else q = 10; + } + return q * z; } void ChartView::leaveEvent(QEvent *event) { - item_group->setVisible(false); + track_pt = {0, 0}; + scene()->update(); QChartView::leaveEvent(event); } @@ -406,9 +480,8 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton && rubber && rubber->isVisible()) { rubber->hide(); QRectF rect = rubber->geometry().normalized(); - rect.translate(-chart()->plotArea().topLeft()); - double min = axis_x->min() + (rect.left() / chart()->plotArea().width()) * (axis_x->max() - axis_x->min()); - double max = axis_x->min() + (rect.right() / chart()->plotArea().width()) * (axis_x->max() - axis_x->min()); + double min = std::floor(chart()->mapToValue(rect.topLeft()).x() * 10.0) / 10.0; + double max = std::floor(chart()->mapToValue(rect.bottomRight()).x() * 10.0) / 10.0; if (rubber->width() <= 0) { // no rubber dragged, seek to mouse position can->seekTo(min); @@ -429,46 +502,141 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { auto rubber = findChild(); bool is_zooming = rubber && rubber->isVisible(); const auto plot_area = chart()->plotArea(); - + track_pt = {0, 0}; if (!is_zooming && plot_area.contains(ev->pos())) { - double x = std::clamp((double)ev->pos().x(), plot_area.left(), plot_area.right() - 1); - double sec = axis_x->min() + (axis_x->max() - axis_x->min()) * (x - plot_area.left()) / plot_area.width(); QStringList text_list; - QPointF pos = plot_area.bottomRight(); - double tm = 0.0; - + const double sec = chart()->mapToValue(ev->pos()).x(); for (auto &s : sigs) { - auto value = std::upper_bound(s.vals.begin(), s.vals.end(), sec, [](double x, auto &p) { return x < p.x(); }); - if (value != s.vals.end()) { - text_list.push_back(QString(" %1 : %2 ").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value->y())); - tm = value->x(); - auto y_pos = chart()->mapToPosition(*value); - if (y_pos.y() < pos.y()) pos = y_pos; - } - } - - if (!text_list.isEmpty()) { - value_text->setHtml("
 Time: " + - QString::number(tm, 'f', 3) + " 
" + text_list.join("
") + "
"); - track_line->setLine(pos.x(), plot_area.top(), pos.x(), plot_area.bottom()); - int text_x = pos.x() + 8; - QRectF text_rect = value_text->boundingRect(); - if ((text_x + text_rect.width()) > plot_area.right()) { - text_x = pos.x() - text_rect.width() - 8; + QString value = "--"; + // use reverse iterator to find last item <= sec. + auto it = std::lower_bound(s.vals.rbegin(), s.vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); + if (it != s.vals.rend() && it->x() >= axis_x->min()) { + value = QString::number(it->y()); + auto value_pos = chart()->mapToPosition(*it); + if (value_pos.x() > track_pt.x()) track_pt = value_pos; } - value_text->setPos(text_x, pos.y() - text_rect.height() / 2); - track_ellipse->setRect(pos.x() - 5, pos.y() - 5, 10, 10); + text_list.push_back(QString(" %1 : %2 ").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value)); } - item_group->setVisible(!text_list.isEmpty()); + if (track_pt.x() == 0) track_pt = ev->pos(); + QString text = QString("
 Time: %1  
%2
") + .arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3) + .arg(text_list.join("
")); + QPoint pt((int)track_pt.x() + 20, plot_area.top() - 20); + QToolTip::showText(mapToGlobal(pt), text, this, plot_area.toRect()); + scene()->update(); } else { - item_group->setVisible(false); + QToolTip::hideText(); } QChartView::mouseMoveEvent(ev); } void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { - qreal x = chart()->plotArea().left() + - chart()->plotArea().width() * (can->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min()); + qreal x = chart()->mapToPosition(QPointF{can->currentSec(), 0}).x(); + qreal y1 = chart()->plotArea().top() - 2; + qreal y2 = chart()->plotArea().bottom() + 2; painter->setPen(QPen(chart()->titleBrush().color(), 2)); - painter->drawLine(QPointF{x, chart()->plotArea().top() - 2}, QPointF{x, chart()->plotArea().bottom() + 2}); + painter->drawLine(QPointF{x, y1}, QPointF{x, y2}); + if (!track_pt.isNull()) { + painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); + painter->drawLine(QPointF{track_pt.x(), y1}, QPointF{track_pt.x(), y2}); + painter->setBrush(Qt::darkGray); + painter->drawEllipse(track_pt, 5, 5); + } +} + +// SeriesSelector + +SeriesSelector::SeriesSelector(QWidget *parent) { + setWindowTitle(tr("Manage Chart Series")); + QHBoxLayout *contents_layout = new QHBoxLayout(); + + QVBoxLayout *left_layout = new QVBoxLayout(); + left_layout->addWidget(new QLabel(tr("Select Signals:"))); + + msgs_combo = new QComboBox(this); + msgs_combo->setEditable(true); + msgs_combo->lineEdit()->setPlaceholderText(tr("Select Msg")); + msgs_combo->setInsertPolicy(QComboBox::NoInsert); + msgs_combo->completer()->setCompletionMode(QCompleter::PopupCompletion); + msgs_combo->completer()->setFilterMode(Qt::MatchContains); + + left_layout->addWidget(msgs_combo); + sig_list = new QListWidget(this); + sig_list->setSortingEnabled(true); + sig_list->setToolTip(tr("Double click on an item to add signal to chart")); + left_layout->addWidget(sig_list); + + QVBoxLayout *right_layout = new QVBoxLayout(); + right_layout->addWidget(new QLabel(tr("Chart Signals:"))); + chart_series = new QListWidget(this); + chart_series->setSortingEnabled(true); + chart_series->setToolTip(tr("Double click on an item to remove signal from chart")); + right_layout->addWidget(chart_series); + contents_layout->addLayout(left_layout); + contents_layout->addLayout(right_layout); + + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->addLayout(contents_layout); + main_layout->addWidget(buttonBox); + + for (auto it = can->can_msgs.cbegin(); it != can->can_msgs.cend(); ++it) { + if (auto m = dbc()->msg(it.key())) { + msgs_combo->addItem(QString("%1 (%2)").arg(m->name).arg(it.key()), it.key()); + } + } + msgs_combo->model()->sort(0); + + QObject::connect(buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept); + QObject::connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); + QObject::connect(msgs_combo, SIGNAL(currentIndexChanged(int)), SLOT(msgSelected(int))); + QObject::connect(sig_list, &QListWidget::itemDoubleClicked, this, &SeriesSelector::addSignal); + QObject::connect(chart_series, &QListWidget::itemDoubleClicked, [](QListWidgetItem *item) { delete item; }); + + if (int index = msgs_combo->currentIndex(); index >= 0) { + msgSelected(index); + } +} + +void SeriesSelector::msgSelected(int index) { + QString msg_id = msgs_combo->itemData(index).toString(); + sig_list->clear(); + if (auto m = dbc()->msg(msg_id)) { + for (auto &[name, s] : m->sigs) { + QStringList data({msg_id, m->name, name}); + QListWidgetItem *item = new QListWidgetItem(name, sig_list); + item->setData(Qt::UserRole, data); + sig_list->addItem(item); + } + } +} + +void SeriesSelector::addSignal(QListWidgetItem *item) { + QStringList data = item->data(Qt::UserRole).toStringList(); + addSeries(data[0], data[1], data[2]); +} + +void SeriesSelector::addSeries(const QString &id, const QString &msg_name, const QString &sig_name) { + QStringList data({id, msg_name, sig_name}); + for (int i = 0; i < chart_series->count(); ++i) { + if (chart_series->item(i)->data(Qt::UserRole).toStringList() == data) { + return; + } + } + QListWidgetItem *new_item = new QListWidgetItem(chart_series); + new_item->setData(Qt::UserRole, data); + chart_series->addItem(new_item); + QLabel *label = new QLabel(QString("%0 %1 %2").arg(data[2]).arg(data[1]).arg(data[0]), chart_series); + label->setContentsMargins(5, 0, 5, 0); + new_item->setSizeHint(label->sizeHint()); + chart_series->setItemWidget(new_item, label); +} + +QList SeriesSelector::series() { + QList ret; + for (int i = 0; i < chart_series->count(); ++i) { + ret.push_back(chart_series->item(i)->data(Qt::UserRole).toStringList()); + } + return ret; } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 8f0af95b1a..1799a25488 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -1,11 +1,10 @@ #pragma once +#include +#include #include -#include -#include +#include #include -#include -#include #include #include #include @@ -21,7 +20,6 @@ class ChartView : public QChartView { public: ChartView(QWidget *parent = nullptr); - ~ChartView(); void addSeries(const QString &msg_id, const Signal *sig); void removeSeries(const QString &msg_id, const Signal *sig); bool hasSeries(const QString &msg_id, const Signal *sig) const; @@ -42,6 +40,7 @@ public: signals: void seriesRemoved(const QString &id, const Signal *sig); + void seriesAdded(const QString &id, const Signal *sig); void zoomIn(double min, double max); void zoomReset(); void remove(); @@ -51,6 +50,7 @@ private slots: void msgUpdated(uint32_t address); void signalUpdated(const Signal *sig); void signalRemoved(const Signal *sig); + void manageSeries(); private: QList::iterator removeSeries(const QList::iterator &it); @@ -63,14 +63,14 @@ private: void updateTitle(); void updateFromSettings(); void drawForeground(QPainter *painter, const QRectF &rect) override; + void applyNiceNumbers(qreal min, qreal max); + qreal niceNumber(qreal x, bool ceiling); QValueAxis *axis_x; QValueAxis *axis_y; - QGraphicsItemGroup *item_group; - QGraphicsLineItem *track_line; - QGraphicsEllipseItem *track_ellipse; - QGraphicsTextItem *value_text; + QPointF track_pt; QGraphicsProxyWidget *close_btn_proxy; + QGraphicsProxyWidget *manage_btn_proxy; std::pair events_range = {0, 0}; QList sigs; }; @@ -81,35 +81,57 @@ class ChartsWidget : public QWidget { public: ChartsWidget(QWidget *parent = nullptr); void showChart(const QString &id, const Signal *sig, bool show, bool merge); - void removeChart(ChartView *chart); - inline bool isChartOpened(const QString &id, const Signal *sig) { return findChart(id, sig) != nullptr; } + inline bool hasSignal(const QString &id, const Signal *sig) { return findChart(id, sig) != nullptr; } signals: void dock(bool floating); void rangeChanged(double min, double max, bool is_zommed); - void chartOpened(const QString &id, const Signal *sig); - void chartClosed(const QString &id, const Signal *sig); + void seriesChanged(); private: + void removeChart(ChartView *chart); void eventsMerged(); void updateState(); + void updateDisplayRange(); void zoomIn(double min, double max); void zoomReset(); void updateToolBar(); void removeAll(); + void showAllData(); bool eventFilter(QObject *obj, QEvent *event) override; ChartView *findChart(const QString &id, const Signal *sig); QLabel *title_label; QLabel *range_label; bool docking = true; + QAction *show_all_values_btn; QAction *dock_btn; QAction *reset_zoom_btn; QAction *remove_all_btn; QVBoxLayout *charts_layout; QList charts; + uint32_t max_chart_range = 0; bool is_zoomed = false; std::pair event_range; std::pair display_range; std::pair zoomed_range; + bool use_dark_theme = false; +}; + +class SeriesSelector : public QDialog { + Q_OBJECT + +public: + SeriesSelector(QWidget *parent); + void addSeries(const QString &id, const QString& msg_name, const QString &sig_name); + QList series(); + +private slots: + void msgSelected(int index); + void addSignal(QListWidgetItem *item); + +private: + QComboBox *msgs_combo; + QListWidget *sig_list; + QListWidget *chart_series; }; diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index e7d3ead9c6..01bdff17a1 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -32,8 +32,6 @@ void DBCManager::initMsgMap() { } QString DBCManager::generateDBC() { - if (!dbc) return {}; - QString dbc_string; for (auto &[address, m] : msgs) { dbc_string += QString("BO_ %1 %2: %3 XXX\n").arg(address).arg(m.name).arg(m.size); @@ -99,6 +97,7 @@ void DBCManager::removeSignal(const QString &id, const QString &sig_name) { std::pair DBCManager::parseId(const QString &id) { const auto list = id.split(':'); + if (list.size() != 2) return {0, 0}; return {list[0].toInt(), list[1].toUInt(nullptr, 16)}; } @@ -111,7 +110,7 @@ DBCManager *dbc() { std::vector DBCMsg::getSignals() const { std::vector ret; - for (auto &[name, sig] : sigs) ret.push_back(&sig); + for (auto &[_, sig] : sigs) ret.push_back(&sig); std::sort(ret.begin(), ret.end(), [](auto l, auto r) { return l->start_bit < r->start_bit; }); return ret; } diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index 4e0bc91069..c7675121bb 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -28,6 +28,7 @@ public: static std::pair parseId(const QString &id); inline static std::vector allDBCNames() { return get_dbc_names(); } + inline std::map &allMsgs() { return msgs; } inline QString name() const { return dbc ? dbc->name.c_str() : ""; } void updateMsg(const QString &id, const QString &name, uint32_t size); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 52ae530a56..108aa8776e 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -16,16 +16,15 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { undo_stack = new QUndoStack(this); - setMinimumWidth(500); - QVBoxLayout *main_layout = new QVBoxLayout(this); + QWidget *main_widget = new QWidget(this); + QVBoxLayout *main_layout = new QVBoxLayout(main_widget); main_layout->setContentsMargins(0, 0, 0, 0); main_layout->setSpacing(0); // tabbar tabbar = new QTabBar(this); tabbar->setTabsClosable(true); - tabbar->setDrawBase(false); tabbar->setUsesScrollButtons(true); tabbar->setAutoHide(true); tabbar->setContextMenuPolicy(Qt::CustomContextMenu); @@ -57,7 +56,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart QHBoxLayout *warning_hlayout = new QHBoxLayout(warning_widget); warning_hlayout->setContentsMargins(0, 0, 0, 0); QLabel *warning_icon = new QLabel(this); - warning_icon->setPixmap(style()->standardPixmap(QStyle::SP_MessageBoxWarning)); + warning_icon->setPixmap(style()->standardPixmap(QStyle::SP_MessageBoxWarning).scaledToWidth(24, Qt::SmoothTransformation)); warning_hlayout->addWidget(warning_icon, 0, Qt::AlignTop); warning_label = new QLabel(this); warning_hlayout->addWidget(warning_label, 1, Qt::AlignLeft); @@ -86,11 +85,15 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart tab_widget = new QTabWidget(this); tab_widget->setTabPosition(QTabWidget::South); - tab_widget->addTab(scroll, "Msg"); - history_log = new HistoryLog(this); - tab_widget->addTab(history_log, "Logs"); + tab_widget->addTab(scroll, "&Msg"); + history_log = new LogsWidget(this); + tab_widget->addTab(history_log, "&Logs"); main_layout->addWidget(tab_widget); + stacked_layout = new QStackedLayout(this); + stacked_layout->addWidget(new WelcomeWidget(this)); + stacked_layout->addWidget(main_widget); + QObject::connect(binary_view, &BinaryView::signalClicked, this, &DetailWidget::showForm); QObject::connect(binary_view, &BinaryView::resizeSignal, this, &DetailWidget::resizeSignal); QObject::connect(binary_view, &BinaryView::addSignal, this, &DetailWidget::addSignal); @@ -104,8 +107,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart } }); QObject::connect(tabbar, &QTabBar::tabCloseRequested, tabbar, &QTabBar::removeTab); - QObject::connect(charts, &ChartsWidget::chartOpened, [this](const QString &id, const Signal *sig) { updateChartState(id, sig, true); }); - QObject::connect(charts, &ChartsWidget::chartClosed, [this](const QString &id, const Signal *sig) { updateChartState(id, sig, false); }); + QObject::connect(charts, &ChartsWidget::seriesChanged, this, &DetailWidget::updateChartState); QObject::connect(undo_stack, &QUndoStack::indexChanged, [this]() { if (undo_stack->count() > 0) dbcMsgChanged(); @@ -137,6 +139,7 @@ void DetailWidget::setMessage(const QString &message_id) { tabbar->setCurrentIndex(index); dbcMsgChanged(); scroll->verticalScrollBar()->setValue(0); + stacked_layout->setCurrentIndex(1); } void DetailWidget::dbcMsgChanged(int show_form_idx) { @@ -154,10 +157,10 @@ void DetailWidget::dbcMsgChanged(int show_form_idx) { for (auto sig : msg->getSignals()) { SignalEdit *form = i < signal_list.size() ? signal_list[i] : nullptr; if (!form) { - form = new SignalEdit(i); + form = new SignalEdit(i, this); QObject::connect(form, &SignalEdit::remove, this, &DetailWidget::removeSignal); QObject::connect(form, &SignalEdit::save, this, &DetailWidget::saveSignal); - QObject::connect(form, &SignalEdit::showFormClicked, this, &DetailWidget::showFormClicked); + QObject::connect(form, &SignalEdit::showFormClicked, this, &DetailWidget::showForm); QObject::connect(form, &SignalEdit::highlight, binary_view, &BinaryView::highlight); QObject::connect(binary_view, &BinaryView::signalHovered, form, &SignalEdit::signalHovered); QObject::connect(form, &SignalEdit::showChart, charts, &ChartsWidget::showChart); @@ -165,7 +168,7 @@ void DetailWidget::dbcMsgChanged(int show_form_idx) { signal_list.push_back(form); } form->setSignal(msg_id, sig); - form->setChartOpened(charts->isChartOpened(msg_id, sig)); + form->setChartOpened(charts->hasSignal(msg_id, sig)); ++i; } if (msg->size != can->lastMessage(msg_id).dat.size()) @@ -197,23 +200,20 @@ void DetailWidget::updateState(const QHash * msgs) { history_log->updateState(); } -void DetailWidget::showFormClicked() { - auto s = qobject_cast(sender()); - showForm(s->sig); -} - void DetailWidget::showForm(const Signal *sig) { setUpdatesEnabled(false); for (auto f : signal_list) { - f->updateForm(f->sig == sig && !f->isFormVisible()); - if (f->sig == sig) scroll->ensureWidgetVisible(f); + f->updateForm(f->sig == sig && !f->form->isVisible()); + if (f->sig == sig && f->form->isVisible()) { + QTimer::singleShot(0, [=]() { scroll->ensureWidgetVisible(f); }); + } } setUpdatesEnabled(true); } -void DetailWidget::updateChartState(const QString &id, const Signal *sig, bool opened) { +void DetailWidget::updateChartState() { for (auto f : signal_list) - if (f->msg_id == id && f->sig == sig) f->setChartOpened(opened); + f->setChartOpened(charts->hasSignal(f->msg_id, f->sig)); } void DetailWidget::editMsg() { @@ -306,3 +306,30 @@ EditMessageDialog::EditMessageDialog(const QString &msg_id, const QString &title connect(buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept); connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); } + +// WelcomeWidget + +WelcomeWidget::WelcomeWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->addStretch(0); + QLabel *logo = new QLabel("CABANA"); + logo->setAlignment(Qt::AlignCenter); + logo->setStyleSheet("font-size:50px;font-weight:bold;"); + main_layout->addWidget(logo); + + auto newShortcutRow = [](const QString &title, const QString &key) { + QHBoxLayout *hlayout = new QHBoxLayout(); + auto btn = new QToolButton(); + btn->setText(key); + btn->setEnabled(false); + hlayout->addWidget(new QLabel(title), 0, Qt::AlignRight); + hlayout->addWidget(btn, 0, Qt::AlignLeft); + return hlayout; + }; + + main_layout->addLayout(newShortcutRow("Pause", "Space")); + main_layout->addLayout(newShortcutRow("Help", "Alt + H")); + main_layout->addStretch(0); + + setStyleSheet("QLabel{color:darkGray;}"); +} diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 41fa0edd41..18c58ab6bf 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -18,6 +19,11 @@ public: QSpinBox *size_spin; }; +class WelcomeWidget : public QWidget { +public: + WelcomeWidget(QWidget *parent); +}; + class DetailWidget : public QWidget { Q_OBJECT @@ -29,8 +35,7 @@ public: private: void showForm(const Signal *sig); - void showFormClicked(); - void updateChartState(const QString &id, const Signal *sig, bool opened); + void updateChartState(); void showTabBarContextMenu(const QPoint &pt); void addSignal(int start_bit, int size, bool little_endian); void resizeSignal(const Signal *sig, int from, int to); @@ -48,9 +53,10 @@ private: QTabWidget *tab_widget; QToolBar *toolbar; QAction *remove_msg_act; - HistoryLog *history_log; + LogsWidget *history_log; BinaryView *binary_view; QScrollArea *scroll; ChartsWidget *charts; + QStackedLayout *stacked_layout; QList signal_list; }; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 1b0898afbd..3e456e44e9 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -1,23 +1,26 @@ #include "tools/cabana/historylog.h" #include +#include +#include +#include // HistoryLogModel -inline const Signal &get_signal(const DBCMsg *m, int index) { - return std::next(m->sigs.begin(), index)->second; +HistoryLogModel::HistoryLogModel(QObject *parent) : QAbstractTableModel(parent) { + QObject::connect(can, &CANMessages::seekedTo, [this]() { + if (!msg_id.isEmpty()) setMessage(msg_id); + }); } QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { - bool has_signal = dbc_msg && !dbc_msg->sigs.empty(); if (role == Qt::DisplayRole) { const auto &m = messages[index.row()]; if (index.column() == 0) { - return QString::number(m.ts, 'f', 2); + return QString::number((m.mono_time / (double)1e9) - can->routeStartTime(), 'f', 2); } - return has_signal ? QString::number(get_raw_value((uint8_t *)m.dat.begin(), m.dat.size(), get_signal(dbc_msg, index.column() - 1))) - : toHex(m.dat); - } else if (role == Qt::FontRole && index.column() == 1 && !has_signal) { + return !sigs.empty() ? QString::number(m.sig_values[index.column() - 1]) : m.data; + } else if (role == Qt::FontRole && index.column() == 1 && sigs.empty()) { return QFontDatabase::systemFont(QFontDatabase::FixedFont); } return {}; @@ -25,73 +28,193 @@ QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { void HistoryLogModel::setMessage(const QString &message_id) { beginResetModel(); + sigs.clear(); + messages.clear(); + has_more_data = true; + if (auto dbc_msg = dbc()->msg(message_id)) { + sigs = dbc_msg->getSignals(); + } + if (msg_id != message_id || sigs.empty()) { + filter_cmp = nullptr; + } msg_id = message_id; - dbc_msg = dbc()->msg(message_id); - column_count = (dbc_msg && !dbc_msg->sigs.empty() ? dbc_msg->sigs.size() : 1) + 1; - row_count = 0; - endResetModel(); - updateState(); + endResetModel(); } QVariant HistoryLogModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal) { - bool has_signal = dbc_msg && !dbc_msg->sigs.empty(); if (role == Qt::DisplayRole || role == Qt::ToolTipRole) { if (section == 0) { return "Time"; } - return has_signal ? QString::fromStdString(get_signal(dbc_msg, section - 1).name).replace('_', ' ') : "Data"; - } else if (role == Qt::BackgroundRole && section > 0 && has_signal) { + return !sigs.empty() ? QString::fromStdString(sigs[section - 1]->name).replace('_', ' ') : "Data"; + } else if (role == Qt::BackgroundRole && section > 0 && !sigs.empty()) { return QBrush(QColor(getColor(section - 1))); + } else if (role == Qt::ForegroundRole && section > 0 && !sigs.empty()) { + return QBrush(Qt::black); } } return {}; } +void HistoryLogModel::setFilter(int sig_idx, const QString &value, std::function cmp) { + if (sig_idx < sigs.size()) { + filter_sig_idx = sig_idx; + filter_value = value.toDouble(); + filter_cmp = value.isEmpty() ? nullptr : cmp; + beginResetModel(); + messages.clear(); + updateState(); + endResetModel(); + } +} + void HistoryLogModel::updateState() { - if (msg_id.isEmpty()) return; - - int prev_row_count = row_count; - messages = can->messages(msg_id); - row_count = messages.size(); - int delta = row_count - prev_row_count; - if (delta > 0) { - beginInsertRows({}, prev_row_count, row_count - 1); - endInsertRows(); - } else if (delta < 0) { - beginRemoveRows({}, row_count, prev_row_count - 1); - endRemoveRows(); + if (!msg_id.isEmpty()) { + uint64_t last_mono_time = messages.empty() ? 0 : messages.front().mono_time; + auto new_msgs = fetchData(last_mono_time, (can->currentSec() + can->routeStartTime()) * 1e9); + if ((has_more_data = !new_msgs.empty())) { + beginInsertRows({}, 0, new_msgs.size() - 1); + messages.insert(messages.begin(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); + endInsertRows(); + } } - if (row_count > 0) { - emit dataChanged(index(0, 0), index(row_count - 1, column_count - 1), {Qt::DisplayRole}); +} + +void HistoryLogModel::fetchMore(const QModelIndex &parent) { + if (!messages.empty()) { + auto new_msgs = fetchData(0, messages.back().mono_time); + if ((has_more_data = !new_msgs.empty())) { + beginInsertRows({}, messages.size(), messages.size() + new_msgs.size() - 1); + messages.insert(messages.end(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); + endInsertRows(); + } + } +} + +std::deque HistoryLogModel::fetchData(uint64_t min_mono_time, uint64_t max_mono_time) { + auto events = can->events(); + auto it = std::lower_bound(events->begin(), events->end(), max_mono_time, [=](auto &e, uint64_t ts) { + return e->mono_time < ts; + }); + if (it == events->end() || it == events->begin()) + return {}; + + std::deque msgs; + const auto [src, address] = DBCManager::parseId(msg_id); + uint32_t cnt = 0; + QVector values(sigs.size()); + for (--it; it != events->begin() && (*it)->mono_time > min_mono_time; --it) { + if ((*it)->which == cereal::Event::Which::CAN) { + for (const auto &c : (*it)->event.getCan()) { + if (src == c.getSrc() && address == c.getAddress()) { + 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 = toHex(QByteArray((char *)dat.begin(), dat.size())); + m.sig_values = values; + if (++cnt >= batch_size && min_mono_time == 0) + return msgs; + } + } + } + } } + return msgs; } // HeaderView QSize HeaderView::sectionSizeFromContents(int logicalIndex) const { + int default_size = qMax(100, rect().width() / model()->columnCount()); const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString(); - const QRect rect = fontMetrics().boundingRect(QRect(0, 0, sectionSize(logicalIndex), 1000), defaultAlignment(), text); - return rect.size() + QSize{10, 5}; + const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text); + QSize size = rect.size() + QSize{10, 6}; + return {qMax(size.width(), default_size), size.height()}; +} + +void HeaderView::paintSection(QPainter *painter, const QRect &rect, int logicalIndex) const { + auto bg_role = model()->headerData(logicalIndex, Qt::Horizontal, Qt::BackgroundRole); + if (bg_role.isValid()) { + QPen pen(model()->headerData(logicalIndex, Qt::Horizontal, Qt::ForegroundRole).value(), 1); + painter->setPen(pen); + painter->fillRect(rect, bg_role.value()); + } + QString text = model()->headerData(logicalIndex, Qt::Horizontal, Qt::DisplayRole).toString(); + painter->drawText(rect.adjusted(5, 3, -5, -3), defaultAlignment(), text); } // HistoryLog HistoryLog::HistoryLog(QWidget *parent) : QTableView(parent) { - model = new HistoryLogModel(this); - setModel(model); setHorizontalHeader(new HeaderView(Qt::Horizontal, this)); - horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap); - horizontalHeader()->setSectionResizeMode(0, QHeaderView::ResizeToContents); + horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); verticalHeader()->setVisible(false); - setFrameShape(QFrame::NoFrame); setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - setStyleSheet("QTableView::item { border:0px; padding-left:5px; padding-right:5px; }"); } -int HistoryLog::sizeHintForColumn(int column) const { - // sizeHintForColumn is only called for column 0 (ResizeToContents) - return itemDelegate()->sizeHint(viewOptions(), model->index(0, 0)).width() + 1; // +1 for grid +// LogsWidget + +LogsWidget::LogsWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + + filter_container = new QWidget(this); + QHBoxLayout *h = new QHBoxLayout(filter_container); + signals_cb = new QComboBox(this); + h->addWidget(signals_cb); + comp_box = new QComboBox(); + comp_box->addItems({">", "=", "!=", "<"}); + h->addWidget(comp_box); + value_edit = new QLineEdit(this); + value_edit->setClearButtonEnabled(true); + value_edit->setValidator(new QDoubleValidator(-500000, 500000, 6, this)); + h->addWidget(value_edit); + main_layout->addWidget(filter_container); + + model = new HistoryLogModel(this); + logs = new HistoryLog(this); + logs->setModel(model); + main_layout->addWidget(logs); + + QObject::connect(signals_cb, SIGNAL(currentIndexChanged(int)), this, SLOT(setFilter())); + QObject::connect(comp_box, SIGNAL(currentIndexChanged(int)), this, SLOT(setFilter())); + QObject::connect(value_edit, &QLineEdit::textChanged, this, &LogsWidget::setFilter); +} + +void LogsWidget::setMessage(const QString &message_id) { + blockSignals(true); + value_edit->setText(""); + signals_cb->clear(); + comp_box->setCurrentIndex(0); + sigs.clear(); + if (auto dbc_msg = dbc()->msg(message_id)) { + sigs = dbc_msg->getSignals(); + for (auto s : sigs) { + signals_cb->addItem(s->name.c_str()); + } + } + filter_container->setVisible(!sigs.empty()); + model->setMessage(message_id); + blockSignals(false); +} + +static bool not_equal(double l, double r) { + return l != r; +} + +void LogsWidget::setFilter() { + std::function cmp; + switch (comp_box->currentIndex()) { + case 0: cmp = std::greater{}; break; + case 1: cmp = std::equal_to{}; break; + case 2: cmp = not_equal; break; + case 3: cmp = std::less{}; break; + } + model->setFilter(signals_cb->currentIndex(), value_edit->text(), cmp); } diff --git a/tools/cabana/historylog.h b/tools/cabana/historylog.h index 21be8fc129..d5ae47192e 100644 --- a/tools/cabana/historylog.h +++ b/tools/cabana/historylog.h @@ -1,6 +1,9 @@ #pragma once +#include +#include #include +#include #include #include "tools/cabana/canmessages.h" @@ -10,36 +13,63 @@ class HeaderView : public QHeaderView { public: HeaderView(Qt::Orientation orientation, QWidget *parent = nullptr) : QHeaderView(orientation, parent) {} QSize sectionSizeFromContents(int logicalIndex) const override; + void paintSection(QPainter *painter, const QRect &rect, int logicalIndex) const; }; class HistoryLogModel : public QAbstractTableModel { - Q_OBJECT - public: - HistoryLogModel(QObject *parent) : QAbstractTableModel(parent) {} + HistoryLogModel(QObject *parent); void setMessage(const QString &message_id); void updateState(); + void setFilter(int sig_idx, const QString &value, std::function cmp); QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; - int rowCount(const QModelIndex &parent = QModelIndex()) const override { return row_count; } - int columnCount(const QModelIndex &parent = QModelIndex()) const override { return column_count; } + void fetchMore(const QModelIndex &parent) override; + inline bool canFetchMore(const QModelIndex &parent) const override { return has_more_data; } + int rowCount(const QModelIndex &parent = QModelIndex()) const override { return messages.size(); } + int columnCount(const QModelIndex &parent = QModelIndex()) const override { return std::max(1ul, sigs.size()) + 1; } -private: + struct Message { + uint64_t mono_time = 0; + QVector sig_values; + QString data; + }; + + std::deque fetchData(uint64_t min_mono_time, uint64_t max_mono_time); QString msg_id; - int row_count = 0; - int column_count = 2; - const DBCMsg *dbc_msg = nullptr; - std::deque messages; + bool has_more_data = true; + const int batch_size = 50; + int filter_sig_idx = -1; + double filter_value = 0; + std::function filter_cmp = nullptr; + std::deque messages; + std::vector sigs; }; class HistoryLog : public QTableView { +public: + HistoryLog(QWidget *parent); + int sizeHintForColumn(int column) const override { return -1; }; +}; + +class LogsWidget : public QWidget { Q_OBJECT public: - HistoryLog(QWidget *parent); - void setMessage(const QString &message_id) { model->setMessage(message_id); } + LogsWidget(QWidget *parent); + void setMessage(const QString &message_id); void updateState() { model->updateState(); } + +private slots: + void setFilter(); + private: - int sizeHintForColumn(int column) const override; + void showEvent(QShowEvent *event) override { model->setMessage(model->msg_id); }; + + HistoryLog *logs; HistoryLogModel *model; + QWidget *filter_container; + QComboBox *signals_cb, *comp_box; + QLineEdit *value_edit; + std::vector sigs; }; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 324323ac44..f14d522c20 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -11,14 +11,13 @@ #include #include #include +#include #include #include #include #include #include -#include "tools/replay/util.h" - static MainWindow *main_win = nullptr; void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; @@ -35,19 +34,13 @@ MainWindow::MainWindow() : QMainWindow() { splitter = new QSplitter(Qt::Horizontal, this); splitter->setHandleWidth(11); - // DBC file selector QWidget *messages_container = new QWidget(this); QVBoxLayout *messages_layout = new QVBoxLayout(messages_container); messages_layout->setContentsMargins(0, 0, 0, 0); - dbc_combo = new QComboBox(this); - auto dbc_names = dbc()->allDBCNames(); - for (const auto &name : dbc_names) { - dbc_combo->addItem(QString::fromStdString(name)); - } - dbc_combo->model()->sort(0); - dbc_combo->setInsertPolicy(QComboBox::NoInsert); - messages_layout->addWidget(dbc_combo); + // left panel + dbc_combo = createDBCSelector(); + messages_layout->addWidget(dbc_combo); messages_widget = new MessagesWidget(this); messages_layout->addWidget(messages_widget); splitter->addWidget(messages_container); @@ -70,7 +63,7 @@ MainWindow::MainWindow() : QMainWindow() { right_hlayout->addWidget(fingerprint_label, 0, Qt::AlignLeft); // TODO: click to select another route. - right_hlayout->addWidget(new QLabel(can->route()), 0, Qt::AlignRight); + right_hlayout->addWidget(new QLabel(can->routeName()), 0, Qt::AlignRight); r_layout->addLayout(right_hlayout); video_widget = new VideoWidget(this); @@ -82,6 +75,7 @@ MainWindow::MainWindow() : QMainWindow() { setCentralWidget(central_widget); createActions(); createStatusBar(); + createShortcuts(); qRegisterMetaType("uint64_t"); qRegisterMetaType("ReplyMsgType"); @@ -107,11 +101,7 @@ MainWindow::MainWindow() : QMainWindow() { QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); QObject::connect(charts_widget, &ChartsWidget::rangeChanged, video_widget, &VideoWidget::rangeChanged); QObject::connect(can, &CANMessages::streamStarted, this, &MainWindow::loadDBCFromFingerprint); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, [this]() { - detail_widget->undo_stack->clear(); - dbc_combo->setCurrentText(QFileInfo(dbc()->name()).baseName()); - setWindowTitle(tr("%1 - Cabana").arg(dbc()->name())); - }); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); QObject::connect(detail_widget->undo_stack, &QUndoStack::indexChanged, [this](int index) { setWindowTitle(tr("%1%2 - Cabana").arg(index > 0 ? "* " : "").arg(dbc()->name())); }); @@ -147,6 +137,23 @@ void MainWindow::createActions() { help_menu->addAction(tr("About &Qt"), qApp, &QApplication::aboutQt); } +QComboBox *MainWindow::createDBCSelector() { + QComboBox *c = new QComboBox(this); + c->setEditable(true); + c->lineEdit()->setPlaceholderText(tr("Select from an existing DBC file")); + c->setInsertPolicy(QComboBox::NoInsert); + c->completer()->setCompletionMode(QCompleter::PopupCompletion); + c->completer()->setFilterMode(Qt::MatchContains); + + auto dbc_names = dbc()->allDBCNames(); + std::sort(dbc_names.begin(), dbc_names.end()); + for (const auto &name : dbc_names) { + c->addItem(QString::fromStdString(name)); + } + c->setCurrentIndex(-1); + return c; +} + void MainWindow::createStatusBar() { progress_bar = new QProgressBar(); progress_bar->setRange(0, 100); @@ -156,9 +163,23 @@ void MainWindow::createStatusBar() { statusBar()->addPermanentWidget(progress_bar); } +void MainWindow::createShortcuts() { + auto shortcut = new QShortcut(QKeySequence(Qt::Key_Space), this, nullptr, nullptr, Qt::ApplicationShortcut); + QObject::connect(shortcut, &QShortcut::activated, []() { can->pause(!can->isPaused()); }); + // TODO: add more shortcuts here. +} + +void MainWindow::DBCFileChanged() { + detail_widget->undo_stack->clear(); + int index = dbc_combo->findText(QFileInfo(dbc()->name()).baseName()); + dbc_combo->setCurrentIndex(index); + setWindowTitle(tr("%1 - Cabana").arg(dbc()->name())); +} + void MainWindow::loadDBCFromName(const QString &name) { - if (name != dbc()->name()) + if (name != dbc()->name()) { dbc()->open(name); + } } void MainWindow::loadDBCFromFile() { @@ -181,13 +202,15 @@ void MainWindow::loadDBCFromClipboard() { void MainWindow::loadDBCFromFingerprint() { auto fingerprint = can->carFingerprint(); - fingerprint_label->setText(fingerprint); - if (!fingerprint.isEmpty() && dbc()->name().isEmpty()) { + fingerprint_label->setText(fingerprint.isEmpty() ? tr("Unknown Car") : fingerprint); + if (!fingerprint.isEmpty()) { auto dbc_name = fingerprint_to_dbc[fingerprint]; if (dbc_name != QJsonValue::Undefined) { loadDBCFromName(dbc_name.toString()); + return; } } + dbc()->open("New_DBC", ""); } void MainWindow::saveDBCToFile() { @@ -225,7 +248,8 @@ void MainWindow::dockCharts(bool dock) { floating_window->deleteLater(); floating_window = nullptr; } else if (!dock && !floating_window) { - floating_window = new QWidget(nullptr); + floating_window = new QWidget(this); + floating_window->setWindowFlags(Qt::Window); floating_window->setWindowTitle("Charts - Cabana"); floating_window->setLayout(new QVBoxLayout()); floating_window->layout()->addWidget(charts_widget); diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index bb9280c3ea..17c513d809 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -34,8 +34,11 @@ signals: protected: void createActions(); + QComboBox *createDBCSelector(); void createStatusBar(); + void createShortcuts(); void closeEvent(QCloseEvent *event) override; + void DBCFileChanged(); void updateDownloadProgress(uint64_t cur, uint64_t total, bool success); void setOption(); diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index be806aa705..63e26f3808 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -15,10 +15,8 @@ Settings::Settings() { void Settings::save() { QSettings s("settings", QSettings::IniFormat); s.setValue("fps", fps); - s.setValue("log_size", can_msg_log_size); s.setValue("cached_segment", cached_segment_limit); s.setValue("chart_height", chart_height); - s.setValue("chart_theme", chart_theme); s.setValue("max_chart_x_range", max_chart_x_range); s.setValue("last_dir", last_dir); s.setValue("splitter_state", splitter_state); @@ -27,10 +25,8 @@ void Settings::save() { void Settings::load() { QSettings s("settings", QSettings::IniFormat); fps = s.value("fps", 10).toInt(); - can_msg_log_size = s.value("log_size", 50).toInt(); cached_segment_limit = s.value("cached_segment", 3).toInt(); chart_height = s.value("chart_height", 200).toInt(); - chart_theme = s.value("chart_theme", 0).toInt(); max_chart_x_range = s.value("max_chart_x_range", 3 * 60).toInt(); last_dir = s.value("last_dir", QDir::homePath()).toString(); splitter_state = s.value("splitter_state").toByteArray(); @@ -48,12 +44,6 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { fps->setValue(settings.fps); form_layout->addRow("FPS", fps); - log_size = new QSpinBox(this); - log_size->setRange(50, 500); - log_size->setSingleStep(10); - log_size->setValue(settings.can_msg_log_size); - form_layout->addRow(tr("Signal history log size"), log_size); - cached_segment = new QSpinBox(this); cached_segment->setRange(3, 60); cached_segment->setSingleStep(1); @@ -72,11 +62,6 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { chart_height->setValue(settings.chart_height); form_layout->addRow(tr("Chart height"), chart_height); - chart_theme = new QComboBox(); - chart_theme->addItems({"Light", "Dark"}); - chart_theme->setCurrentIndex(settings.chart_theme == 1 ? 1 : 0); - form_layout->addRow(tr("Chart theme"), chart_theme); - auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); form_layout->addRow(buttonBox); @@ -87,10 +72,8 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { void SettingsDlg::save() { settings.fps = fps->value(); - settings.can_msg_log_size = log_size->value(); settings.cached_segment_limit = cached_segment->value(); settings.chart_height = chart_height->value(); - settings.chart_theme = chart_theme->currentIndex(); settings.max_chart_x_range = max_chart_x_range->value() * 60; settings.save(); accept(); diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index 624a1ce33d..1db92fe231 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -14,10 +14,8 @@ public: void load(); int fps = 10; - int can_msg_log_size = 50; int cached_segment_limit = 3; int chart_height = 200; - int chart_theme = 0; int max_chart_x_range = 3 * 60; // 3 minutes QString last_dir; QByteArray splitter_state; @@ -33,10 +31,8 @@ public: SettingsDlg(QWidget *parent); void save(); QSpinBox *fps; - QSpinBox *log_size ; QSpinBox *cached_segment; QSpinBox *chart_height; - QComboBox *chart_theme; QSpinBox *max_chart_x_range; }; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index 93b7aa88f7..b2491d5b1f 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -4,12 +4,8 @@ #include #include #include -#include -#include #include -#include "selfdrive/ui/qt/util.h" - // SignalForm SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { @@ -94,11 +90,6 @@ SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(pa plot_btn->setCheckable(true); plot_btn->setAutoRaise(true); title_layout->addWidget(plot_btn); - auto seek_btn = new QToolButton(this); - seek_btn->setIcon(QIcon::fromTheme("edit-find")); - seek_btn->setAutoRaise(true); - seek_btn->setToolTip(tr("Find signal values")); - title_layout->addWidget(seek_btn); auto remove_btn = new QToolButton(this); remove_btn->setAutoRaise(true); remove_btn->setText("x"); @@ -122,11 +113,10 @@ SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(pa save_timer->setSingleShot(true); save_timer->callOnTimeout(this, &SignalEdit::saveSignal); - QObject::connect(title, &ElidedLabel::clicked, this, &SignalEdit::showFormClicked); + QObject::connect(title, &ElidedLabel::clicked, [this]() { emit showFormClicked(sig); }); QObject::connect(plot_btn, &QToolButton::clicked, [this](bool checked) { emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); }); - QObject::connect(seek_btn, &QToolButton::clicked, [this]() { SignalFindDlg(msg_id, sig, this).exec(); }); QObject::connect(remove_btn, &QToolButton::clicked, [this]() { emit remove(sig); }); QObject::connect(form, &SignalForm::changed, [this]() { save_timer->start(); }); setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); @@ -137,7 +127,7 @@ void SignalEdit::setSignal(const QString &message_id, const Signal *signal) { updateForm(msg_id == message_id && form->isVisible()); msg_id = message_id; color_label->setText(QString::number(form_idx + 1)); - color_label->setStyleSheet(QString("background-color:%1").arg(getColor(form_idx))); + color_label->setStyleSheet(QString("color:black; background-color:%2").arg(getColor(form_idx))); title->setText(sig->name.c_str()); show(); } @@ -212,68 +202,3 @@ void SignalEdit::leaveEvent(QEvent *event) { emit highlight(nullptr); QWidget::leaveEvent(event); } - -// SignalFindDlg - -SignalFindDlg::SignalFindDlg(const QString &id, const Signal *signal, QWidget *parent) : QDialog(parent) { - setWindowTitle(tr("Find signal values")); - QVBoxLayout *main_layout = new QVBoxLayout(this); - - QHBoxLayout *h = new QHBoxLayout(); - h->addWidget(new QLabel(signal->name.c_str())); - QComboBox *comp_box = new QComboBox(); - comp_box->addItems({">", "=", "<"}); - h->addWidget(comp_box); - QLineEdit *value_edit = new QLineEdit("0", this); - value_edit->setValidator(new QDoubleValidator(-500000, 500000, 6, this)); - h->addWidget(value_edit, 1); - QPushButton *search_btn = new QPushButton(tr("Find"), this); - h->addWidget(search_btn); - main_layout->addLayout(h); - - QWidget *container = new QWidget(this); - QVBoxLayout *signals_layout = new QVBoxLayout(container); - QScrollArea *scroll = new QScrollArea(this); - scroll->setWidget(container); - scroll->setWidgetResizable(true); - scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - main_layout->addWidget(scroll); - - QObject::connect(search_btn, &QPushButton::clicked, [=]() { - clearLayout(signals_layout); - - CANMessages::FindFlags comp = CANMessages::EQ; - if (comp_box->currentIndex() == 0) { - comp = CANMessages::GT; - } else if (comp_box->currentIndex() == 2) { - comp = CANMessages::LT; - } - double value = value_edit->text().toDouble(); - - const int limit_results = 50; - auto values = can->findSignalValues(id, signal, value, comp, limit_results); - for (auto &v : values) { - QHBoxLayout *item_layout = new QHBoxLayout(); - item_layout->addWidget(new QLabel(QString::number(v.x(), 'f', 2))); - item_layout->addWidget(new QLabel(QString::number(v.y()))); - item_layout->addStretch(1); - - QPushButton *goto_btn = new QPushButton(tr("Goto"), this); - QObject::connect(goto_btn, &QPushButton::clicked, [sec = v.x()]() { can->seekTo(sec); }); - item_layout->addWidget(goto_btn); - signals_layout->addLayout(item_layout); - } - if (values.size() == limit_results) { - QFrame *hline = new QFrame(); - hline->setFrameShape(QFrame::HLine); - hline->setFrameShadow(QFrame::Sunken); - signals_layout->addWidget(hline); - QLabel *info = new QLabel(tr("Only display the first %1 results").arg(limit_results)); - info->setAlignment(Qt::AlignCenter); - signals_layout->addWidget(info); - } - if (values.size() * 30 > container->height()) { - scroll->setFixedHeight(std::min(values.size() * 30, 300)); - } - }); -} diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index f035797e72..eec943226e 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include #include @@ -9,7 +8,6 @@ #include #include "selfdrive/ui/qt/widgets/controls.h" - #include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" @@ -36,8 +34,8 @@ public: void setChartOpened(bool opened); void signalHovered(const Signal *sig); void updateForm(bool show); - inline bool isFormVisible() const { return form->isVisible(); } const Signal *sig = nullptr; + SignalForm *form = nullptr; QString msg_id; signals: @@ -45,14 +43,13 @@ signals: void showChart(const QString &name, const Signal *sig, bool show, bool merge); void remove(const Signal *sig); void save(const Signal *sig, const Signal &new_sig); - void showFormClicked(); + void showFormClicked(const Signal *sig); protected: void enterEvent(QEvent *event) override; void leaveEvent(QEvent *event) override; void saveSignal(); - SignalForm *form = nullptr; ElidedLabel *title; QLabel *color_label; QLabel *icon; @@ -60,8 +57,3 @@ protected: QToolButton *plot_btn; QTimer *save_timer; }; - -class SignalFindDlg : public QDialog { -public: - SignalFindDlg(const QString &id, const Signal *signal, QWidget *parent); -}; diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index d85b23b7e6..6cd173b514 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -1,13 +1,17 @@ #include "tools/cabana/videowidget.h" +#include #include #include #include #include #include +#include #include #include +#include #include +#include inline QString formatTime(int seconds) { return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss"); @@ -17,7 +21,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(0, 0, 0, 0); - cam_widget = new CameraWidget("camerad", VISION_STREAM_ROAD, false, this); + cam_widget = new CameraWidget("camerad", can->visionStreamType(), false, this); cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); main_layout->addWidget(cam_widget); @@ -92,7 +96,50 @@ Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { timeline = can->getTimeline(); update(); }); + setMouseTracking(true); + QObject::connect(can, SIGNAL(streamStarted()), timer, SLOT(start())); + QObject::connect(can, &CANMessages::streamStarted, this, &Slider::streamStarted); +} + +void Slider::streamStarted() { + abort_load_thumbnail = true; + thumnail_future.waitForFinished(); + abort_load_thumbnail = false; + thumbnails.clear(); + thumnail_future = QtConcurrent::run(this, &Slider::loadThumbnails); +} + +void Slider::loadThumbnails() { + 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()) { + LogReader log; + if (log.load(qlog, &abort_load_thumbnail, {cereal::Event::Which::THUMBNAIL}, true, 0, 3)) { + for (auto ev = log.events.cbegin(); ev != log.events.cend() && !abort_load_thumbnail; ++ev) { + auto thumb = (*ev)->event.getThumbnail(); + QString str = getThumbnailString(thumb.getThumbnail()); + std::lock_guard lk(thumbnail_lock); + thumbnails[thumb.getTimestampEof()] = str; + } + } + } + } +} + +QString Slider::getThumbnailString(const capnp::Data::Reader &data) { + QPixmap thumb; + if (thumb.loadFromData(data.begin(), data.size(), "jpeg")) { + thumb = thumb.scaled({thumb.width()/3, thumb.height()/3}, Qt::KeepAspectRatio); + thumbnail_size = thumb.size(); + QByteArray bytes; + QBuffer buffer(&bytes); + buffer.open(QIODevice::WriteOnly); + thumb.save(&buffer, "png"); + return QString("").arg(QString(bytes.toBase64())); + } + return {}; } void Slider::sliderChange(QAbstractSlider::SliderChange change) { @@ -146,3 +193,18 @@ void Slider::mousePressEvent(QMouseEvent *e) { emit sliderReleased(); } } + +void Slider::mouseMoveEvent(QMouseEvent *e) { + QString thumb; + { + double seconds = (minimum() + e->pos().x() * ((maximum() - minimum()) / (double)width())) / 1000.0; + std::lock_guard lk(thumbnail_lock); + auto it = thumbnails.lowerBound((seconds + can->routeStartTime()) * 1e9); + if (it != thumbnails.end()) { + thumb = it.value(); + } + } + QPoint pt = mapToGlobal({e->pos().x() - thumbnail_size.width() / 2, -thumbnail_size.height() - 30}); + QToolTip::showText(pt, thumb, this, rect()); + QSlider::mouseMoveEvent(e); +} diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index 16f60b0b03..ea62081a91 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -1,5 +1,9 @@ #pragma once +#include +#include + +#include #include #include #include @@ -12,12 +16,23 @@ class Slider : public QSlider { public: Slider(QWidget *parent); + +private: void mousePressEvent(QMouseEvent *e) override; + void mouseMoveEvent(QMouseEvent *e) override; void sliderChange(QAbstractSlider::SliderChange change) override; void paintEvent(QPaintEvent *ev) override; + void streamStarted(); + void loadThumbnails(); + QString getThumbnailString(const capnp::Data::Reader &data); int slider_x = -1; std::vector> timeline; + std::mutex thumbnail_lock; + std::atomic abort_load_thumbnail = false; + QMap thumbnails; + QFuture thumnail_future; + QSize thumbnail_size = {}; }; class VideoWidget : public QWidget { diff --git a/tools/gpstest/.gitignore b/tools/gpstest/.gitignore index f11597286e..992088ef34 100644 --- a/tools/gpstest/.gitignore +++ b/tools/gpstest/.gitignore @@ -1,2 +1,4 @@ LimeGPS/ -LimeSuite/ \ No newline at end of file +LimeSuite/ +hackrf/ +gps-sdr-sim/ diff --git a/tools/gpstest/patches/hackrf.patch b/tools/gpstest/patches/hackrf.patch new file mode 100644 index 0000000000..afc9ac437b --- /dev/null +++ b/tools/gpstest/patches/hackrf.patch @@ -0,0 +1,44 @@ +diff --git a/host/hackrf-tools/src/CMakeLists.txt b/host/hackrf-tools/src/CMakeLists.txt +index 7115151c..a51388ba 100644 +--- a/host/hackrf-tools/src/CMakeLists.txt ++++ b/host/hackrf-tools/src/CMakeLists.txt +@@ -23,20 +23,20 @@ + + set(INSTALL_DEFAULT_BINDIR "bin" CACHE STRING "Appended to CMAKE_INSTALL_PREFIX") + +-find_package(FFTW REQUIRED) +-include_directories(${FFTW_INCLUDES}) +-get_filename_component(FFTW_LIBRARY_DIRS ${FFTW_LIBRARIES} DIRECTORY) +-link_directories(${FFTW_LIBRARY_DIRS}) ++#find_package(FFTW REQUIRED) ++#include_directories(${FFTW_INCLUDES}) ++#get_filename_component(FFTW_LIBRARY_DIRS ${FFTW_LIBRARIES} DIRECTORY) ++#link_directories(${FFTW_LIBRARY_DIRS}) + + SET(TOOLS + hackrf_transfer +- hackrf_spiflash +- hackrf_cpldjtag ++ #hackrf_spiflash ++ #hackrf_cpldjtag + hackrf_info +- hackrf_debug +- hackrf_clock +- hackrf_sweep +- hackrf_operacake ++ #hackrf_debug ++ #hackrf_clock ++ #hackrf_sweep ++ #hackrf_operacake + ) + + if(MSVC) +@@ -45,7 +45,7 @@ if(MSVC) + ) + LIST(APPEND TOOLS_LINK_LIBS ${FFTW_LIBRARIES}) + else() +- LIST(APPEND TOOLS_LINK_LIBS m fftw3f) ++ LIST(APPEND TOOLS_LINK_LIBS m)# fftw3f) + endif() + + if(NOT libhackrf_SOURCE_DIR) diff --git a/tools/gpstest/setup_hackrf.sh b/tools/gpstest/setup_hackrf.sh new file mode 100755 index 0000000000..e504ec9447 --- /dev/null +++ b/tools/gpstest/setup_hackrf.sh @@ -0,0 +1,21 @@ +#!/bin/bash +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +cd $DIR + +if [ ! -d gps-sdr-sim ]; then + git clone https://github.com/osqzss/gps-sdr-sim.git + cd gps-sdr-sim + make + cd .. +fi + +if [ ! -d hackrf ]; then + git clone https://github.com/greatscottgadgets/hackrf.git + cd hackrf/host + git apply ../../patches/hackrf.patch + cmake . + make +fi + diff --git a/tools/gpstest/simulate_gps_signal.py b/tools/gpstest/simulate_gps_signal.py index f1e5ad2028..a6aca1c404 100755 --- a/tools/gpstest/simulate_gps_signal.py +++ b/tools/gpstest/simulate_gps_signal.py @@ -36,38 +36,7 @@ def get_random_coords(lat, lon) -> Tuple[int, int]: # jump around the world return get_coords(lat, lon, 20, 20, 10, 20) -def check_availability() -> bool: - cmd = ["LimeSuite/builddir/LimeUtil/LimeUtil", "--find"] - output = sp.check_output(cmd) - - if output.strip() == b"": - return False - - print(f"Device: {output.strip().decode('utf-8')}") - return True - -def main(lat, lon, jump_sim, contin_sim): - if not os.path.exists('LimeGPS'): - print("LimeGPS not found run 'setup.sh' first") - return - - if not os.path.exists('LimeSuite'): - print("LimeSuite not found run 'setup.sh' first") - return - - if not check_availability(): - print("No limeSDR device found!") - return - - rinex_file = download_rinex() - - if lat == 0 and lon == 0: - lat, lon = get_random_coords(47.2020, 15.7403) - - timeout = None - if jump_sim: - timeout = 30 - +def run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout): while True: try: print(f"starting LimeGPS, Location: {lat},{lon}") @@ -86,17 +55,92 @@ def main(lat, lon, jump_sim, contin_sim): print(f"LimeGPS crashed: {str(e)}") print(f"stderr:\n{e.stderr.decode('utf-8')}")# pylint:disable=no-member + return if contin_sim: lat, lon = get_continuous_coords(lat, lon) else: lat, lon = get_random_coords(lat, lon) +def run_hackRF_loop(lat, lon, rinex_file, timeout): + + if timeout is not None: + print("no jump mode for hackrf!") + return + + try: + print(f"starting gps-sdr-sim, Location: {lat},{lon}") + # create 30second file and replay with hackrf endless + cmd = ["gps-sdr-sim/gps-sdr-sim", "-e", rinex_file, "-l", f"{lat},{lon},100", "-d", "30"] + sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) + # created in current working directory + except Exception: + print("Failed to generate gpssim.bin") + + try: + print("starting hackrf_transfer") + # create 30second file and replay with hackrf endless + cmd = ["hackrf/host/hackrf-tools/src/hackrf_transfer", "-t", "gpssim.bin", + "-f", "1575420000", "-s", "2600000", "-a", "1", "-R"] + sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) + except KeyboardInterrupt: + print("stopping hackrf_transfer") + return + except Exception as e: + print(f"hackrf_transfer crashed:{str(e)}") + + +def main(lat, lon, jump_sim, contin_sim, hackrf_mode): + + if hackrf_mode: + if not os.path.exists('hackrf'): + print("hackrf not found run 'setup_hackrf.sh' first") + return + + if not os.path.exists('gps-sdr-sim'): + print("gps-sdr-sim not found run 'setup_hackrf.sh' first") + return + + output = sp.check_output(["hackrf/host/hackrf-tools/src/hackrf_info"]) + if output.strip() == b"" or b"No HackRF boards found." in output: + print("No HackRF boards found!") + return + + else: + if not os.path.exists('LimeGPS'): + print("LimeGPS not found run 'setup.sh' first") + return + + if not os.path.exists('LimeSuite'): + print("LimeSuite not found run 'setup.sh' first") + return + + output = sp.check_output(["LimeSuite/builddir/LimeUtil/LimeUtil", "--find"]) + if output.strip() == b"": + print("No LimeSDR device found!") + return + print(f"Device: {output.strip().decode('utf-8')}") + + if lat == 0 and lon == 0: + lat, lon = get_random_coords(47.2020, 15.7403) + + rinex_file = download_rinex() + + timeout = None + if jump_sim: + timeout = 30 + + if not hackrf_mode: + run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout) + else: + run_hackRF_loop(lat, lon, rinex_file, timeout) + if __name__ == "__main__": parser = argparse.ArgumentParser(description="Simulate static [or random jumping] GPS signal.") parser.add_argument("lat", type=float, nargs='?', default=0) parser.add_argument("lon", type=float, nargs='?', default=0) parser.add_argument("--jump", action="store_true", help="signal that jumps around the world") parser.add_argument("--contin", action="store_true", help="continuously/slowly moving around the world") + parser.add_argument("--hackrf", action="store_true", help="hackrf mode (DEFAULT: LimeSDR)") args = parser.parse_args() - main(args.lat, args.lon, args.jump, args.contin) + main(args.lat, args.lon, args.jump, args.contin, args.hackrf) diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc index 5f165ac312..77357a2873 100644 --- a/tools/replay/consoleui.cc +++ b/tools/replay/consoleui.cc @@ -156,13 +156,13 @@ void ConsoleUI::timerEvent(QTimerEvent *ev) { } void ConsoleUI::updateStatus() { - auto write_item = [this](int y, int x, const char *key, const std::string &value, const char *unit, + auto write_item = [this](int y, int x, const char *key, const std::string &value, const std::string &unit, bool bold = false, Color color = Color::BrightWhite) { auto win = w[Win::CarState]; wmove(win, y, x); add_str(win, key); add_str(win, value.c_str(), color, bold); - add_str(win, unit); + add_str(win, unit.c_str()); }; static const std::pair status_text[] = { {"loading...", Color::Red}, @@ -177,9 +177,8 @@ void ConsoleUI::updateStatus() { } auto [status_str, status_color] = status_text[status]; write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); - std::string suffix = util::string_format(" / %s [%d/%d] ", format_seconds(replay->totalSeconds()).c_str(), - replay->currentSeconds() / 60, replay->route()->segments().size()); - write_item(0, 25, "TIME: ", format_seconds(replay->currentSeconds()), suffix.c_str(), true); + std::string suffix = " / " + format_seconds(replay->totalSeconds()); + write_item(0, 25, "TIME: ", format_seconds(replay->currentSeconds()), suffix, true); auto p = sm["liveParameters"].getLiveParameters(); write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); diff --git a/tools/replay/main.cc b/tools/replay/main.cc index 40dace0c91..8621a9b978 100644 --- a/tools/replay/main.cc +++ b/tools/replay/main.cc @@ -23,6 +23,7 @@ int main(int argc, char *argv[]) { parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai"); parser.addOption({{"a", "allow"}, "whitelist of services to send", "allow"}); parser.addOption({{"b", "block"}, "blacklist of services to send", "block"}); + parser.addOption({{"c", "cache"}, "cache segments in memory. default is 5", "n"}); parser.addOption({{"s", "start"}, "start from ", "seconds"}); parser.addOption({"demo", "use a demo route instead of providing your own"}); parser.addOption({"data_dir", "local directory with routes", "data_dir"}); @@ -47,6 +48,9 @@ int main(int argc, char *argv[]) { } } Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); + if (!parser.value("c").isEmpty()) { + replay->setSegmentCacheLimit(parser.value("c").toInt()); + } if (!replay->load()) { return 0; } diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 1337a4ef2c..a01371abe1 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -51,9 +51,9 @@ void Replay::stop() { stream_thread_->wait(); stream_thread_ = nullptr; } - segments_.clear(); camera_server_.reset(nullptr); timeline_future.waitForFinished(); + segments_.clear(); rInfo("shutdown: done"); } @@ -109,6 +109,7 @@ void Replay::seekTo(double seconds, bool relative) { cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); }); + emit seekedTo(seconds); queueSegment(); } @@ -123,9 +124,9 @@ void Replay::buildTimeline() { uint64_t alert_begin = 0; TimelineType alert_type = TimelineType::None; - for (int i = 0; i < segments_.size() && !exit_; ++i) { + for (auto it = segments_.cbegin(); it != segments_.cend() && !exit_; ++it) { LogReader log; - if (!log.load(route_->at(i).qlog.toStdString(), &exit_, + if (!log.load(route_->at(it->first).qlog.toStdString(), &exit_, {cereal::Event::Which::CONTROLS_STATE, cereal::Event::Which::USER_FLAG}, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; @@ -209,11 +210,17 @@ void Replay::segmentLoadFinished(bool success) { void Replay::queueSegment() { if (segments_.empty()) return; - SegmentMap::iterator cur, end; - cur = end = segments_.lower_bound(std::min(current_segment_.load(), segments_.rbegin()->first)); - for (int i = 0; end != segments_.end() && i <= segment_cache_limit + FORWARD_FETCH_SEGS; ++i) { + SegmentMap::iterator begin, cur; + begin = cur = segments_.lower_bound(std::min(current_segment_.load(), segments_.rbegin()->first)); + int distance = std::max(std::ceil(segment_cache_limit / 2.0) - 1, segment_cache_limit - std::distance(cur, segments_.end())); + for (int i = 0; begin != segments_.begin() && i < distance; ++i) { + --begin; + } + auto end = begin; + for (int i = 0; end != segments_.end() && i < segment_cache_limit; ++i) { ++end; } + // load one segment at a time for (auto it = cur; it != end; ++it) { auto &[n, seg] = *it; @@ -227,12 +234,6 @@ void Replay::queueSegment() { } } - const auto &cur_segment = cur->second; - // merge the previous adjacent segment if it's loaded - auto begin = segments_.find(cur_segment->seg_num - 1); - if (begin == segments_.end() || !(begin->second && begin->second->isLoaded())) { - begin = cur; - } mergeSegments(begin, end); // free segments out of current semgnt window. @@ -240,6 +241,7 @@ void Replay::queueSegment() { std::for_each(end, segments_.end(), [](auto &e) { e.second.reset(nullptr); }); // start stream thread + const auto &cur_segment = cur->second; if (stream_thread_ == nullptr && cur_segment->isLoaded()) { startStream(cur_segment.get()); emit streamStarted(); @@ -247,12 +249,13 @@ void Replay::queueSegment() { } void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end) { - // merge 3 segments in sequence. std::vector segments_need_merge; size_t new_events_size = 0; - for (auto it = begin; it != end && it->second && it->second->isLoaded() && segments_need_merge.size() < segment_cache_limit; ++it) { - segments_need_merge.push_back(it->first); - new_events_size += it->second->log->events.size(); + for (auto it = begin; it != end; ++it) { + if (it->second && it->second->isLoaded()) { + segments_need_merge.push_back(it->first); + new_events_size += it->second->log->events.size(); + } } if (segments_need_merge != segments_merged_) { @@ -358,6 +361,7 @@ void Replay::publishFrame(const Event *e) { void Replay::stream() { cereal::Event::Which cur_which = cereal::Event::Which::INIT_DATA; + double prev_replay_speed = 1.0; std::unique_lock lk(stream_lock_); while (true) { @@ -397,10 +401,11 @@ void Replay::stream() { long rtime = nanos_since_boot() - loop_start_ts; long behind_ns = etime - rtime; // if behind_ns is greater than 1 second, it means that an invalid segemnt is skipped by seeking/replaying - if (behind_ns >= 1 * 1e9) { - // reset start times + if (behind_ns >= 1 * 1e9 || speed_ != prev_replay_speed) { + // reset event start times evt_start_ts = cur_mono_time_; loop_start_ts = nanos_since_boot(); + prev_replay_speed = speed_; } else if (behind_ns > 0 && !hasFlag(REPLAY_FLAG_FULL_SPEED)) { precise_nano_sleep(behind_ns); } diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 88c285125a..2c68443df0 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -10,7 +10,7 @@ const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; // one segment uses about 100M of memory -constexpr int FORWARD_FETCH_SEGS = 3; +constexpr int MIN_SEGMENTS_CACHE = 5; enum REPLAY_FLAGS { REPLAY_FLAG_NONE = 0x0000, @@ -58,7 +58,7 @@ public: event_filter = filter; } inline int segmentCacheLimit() const { return segment_cache_limit; } - inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(3, n); } + inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(MIN_SEGMENTS_CACHE, n); } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } @@ -79,6 +79,7 @@ public: signals: void streamStarted(); void segmentsMerged(); + void seekedTo(double sec); protected slots: void segmentLoadFinished(bool success); @@ -133,5 +134,5 @@ protected: float speed_ = 1.0; replayEventFilter event_filter = nullptr; void *filter_opaque = nullptr; - int segment_cache_limit = 3; + int segment_cache_limit = MIN_SEGMENTS_CACHE; }; diff --git a/tools/replay/ui.py b/tools/replay/ui.py index bbcd49061e..43cbef5bd6 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -128,8 +128,8 @@ def ui_thread(addr): sm.update(0) w = sm['controlsState'].lateralControlState.which() - if w == 'lqrState': - angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steeringAngleDeg + if w == 'lqrStateDEPRECATED': + angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg elif w == 'indiState': angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg else: diff --git a/tools/sim/README.md b/tools/sim/README.md index 40603f3f71..69a89aefab 100644 --- a/tools/sim/README.md +++ b/tools/sim/README.md @@ -6,7 +6,7 @@ openpilot implements a [bridge](bridge.py) that allows it to run in the [CARLA s ## System Requirements openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system. -For this case, we have a the simulator in low quality by default. +For this case, we have the simulator in low quality by default. You can also check out the [CARLA python documentation](https://carla.readthedocs.io/en/latest/python_api/) to find more parameters to tune that might increase performance on your system. diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index adabc40c2e..2100f05b67 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -6,7 +6,7 @@ export SIMULATION="1" export SKIP_FW_QUERY="1" export FINGERPRINT="HONDA CIVIC 2016" -export BLOCK="camerad,loggerd,encoderd" +export BLOCK="camerad,loggerd,encoderd,micd" if [[ "$CI" ]]; then # TODO: offscreen UI should work export BLOCK="${BLOCK},ui" diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index 7abfb6358a..4819be770f 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -8,24 +8,13 @@ import signal from collections import defaultdict import cereal.messaging as messaging +from tools.lib.logreader import logreader_from_route_or_segment def sigint_handler(signal, frame): - print("handler!") exit(0) signal.signal(signal.SIGINT, sigint_handler) -if __name__ == "__main__": - - parser = argparse.ArgumentParser(description='Sniff a communication socket') - parser.add_argument('control_type', help="[pid|indi|lqr|angle]") - parser.add_argument('--addr', default='127.0.0.1', help="IP address for optional ZMQ listener, default to msgq") - parser.add_argument('--group', default='all', help="speed group to display, [crawl|slow|medium|fast|veryfast|germany|all], default to all") - args = parser.parse_args() - - if args.addr != "127.0.0.1": - os.environ["ZMQ"] = "1" - messaging.context = messaging.Context() - +class SteeringAccuracyTool: all_groups = {"germany": (45, "45 - up m/s // 162 - up km/h // 101 - up mph"), "veryfast": (35, "35 - 45 m/s // 126 - 162 km/h // 78 - 101 mph"), "fast": (25, "25 - 35 m/s // 90 - 126 km/h // 56 - 78 mph"), @@ -33,39 +22,28 @@ if __name__ == "__main__": "slow": (5, " 5 - 15 m/s // 18 - 54 km/h // 11 - 34 mph"), "crawl": (0, " 0 - 5 m/s // 0 - 18 km/h // 0 - 11 mph")} - if args.group == "all": - display_groups = all_groups.keys() - elif args.group in all_groups.keys(): - display_groups = [args.group] - else: - raise ValueError("invalid speed group, see help") - - speed_group_stats = {} - for group in all_groups: - speed_group_stats[group] = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0, "steer": 0, "limited": 0, "saturated": 0, "dpp": 0}) - - carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) - sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr) - time.sleep(1) # Make sure all submaster data is available before going further - - msg_cnt = 0 - cnt = 0 - total_error = 0 - - while messaging.recv_one(carControl): - sm.update() - msg_cnt += 1 - - if args.control_type == "pid": - control_state = sm['controlsState'].lateralControlState.pidState - elif args.control_type == "indi": - control_state = sm['controlsState'].lateralControlState.indiState - elif args.control_type == "lqr": - control_state = sm['controlsState'].lateralControlState.lqrState - elif args.control_type == "angle": - control_state = sm['controlsState'].lateralControlState.angleState + def __init__(self, args): + self.msg_cnt = 0 + self.cnt = 0 + self.total_error = 0 + + if args.group == "all": + self.display_groups = self.all_groups.keys() + elif args.group in self.all_groups.keys(): + self.display_groups = [args.group] else: - raise ValueError("invalid lateral control type, see help") + raise ValueError("invalid speed group, see help") + + self.speed_group_stats = {} + for group in self.all_groups: + self.speed_group_stats[group] = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0, "steer": 0, "limited": 0, "saturated": 0, "dpp": 0}) + + def update(self, sm): + self.msg_cnt += 1 + + lateralControlState = sm['controlsState'].lateralControlState + control_type = list(lateralControlState.to_dict().keys())[0] + control_state = lateralControlState.__getattr__(control_type) v_ego = sm['carState'].vEgo active = sm['controlsState'].active @@ -77,10 +55,10 @@ if __name__ == "__main__": d_path_points = sm['lateralPlan'].dPathPoints # must be engaged, not at standstill, not overriding steering, and not changing lanes if active and not standstill and not overriding and not changing_lanes: - cnt += 1 + self.cnt += 1 # wait 5 seconds after engage / standstill / override / lane change - if cnt >= 500: + if self.cnt >= 500: actual_angle = control_state.steeringAngleDeg desired_angle = control_state.steeringAngleDesiredDeg @@ -91,41 +69,88 @@ if __name__ == "__main__": angle_error = round(angle_error, 2) angle_abs = int(abs(round(desired_angle, 0))) - for group, group_props in all_groups.items(): + for group, group_props in self.all_groups.items(): if v_ego > group_props[0]: # collect stats - speed_group_stats[group][angle_abs]["cnt"] += 1 - speed_group_stats[group][angle_abs]["err"] += angle_error - speed_group_stats[group][angle_abs]["steer"] += abs(steer) + self.speed_group_stats[group][angle_abs]["cnt"] += 1 + self.speed_group_stats[group][angle_abs]["err"] += angle_error + self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) if len(d_path_points): - speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0]) + self.speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0]) if steer_limited: - speed_group_stats[group][angle_abs]["limited"] += 1 + self.speed_group_stats[group][angle_abs]["limited"] += 1 if control_state.saturated: - speed_group_stats[group][angle_abs]["saturated"] += 1 + self.speed_group_stats[group][angle_abs]["saturated"] += 1 if actual_angle == desired_angle: - speed_group_stats[group][angle_abs]["="] += 1 + self.speed_group_stats[group][angle_abs]["="] += 1 else: if desired_angle == 0.: overshoot = True else: overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle - speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1 + self.speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1 break else: - cnt = 0 + self.cnt = 0 - if msg_cnt % 100 == 0: + if self.msg_cnt % 100 == 0: print(chr(27) + "[2J") - if cnt != 0: + if self.cnt != 0: print("COLLECTING ...\n") else: print("DISABLED (not active, standstill, steering override, or lane change)\n") - for group in display_groups: - if len(speed_group_stats[group]) > 0: - print(f"speed group: {group:10s} {all_groups[group][1]:>96s}") + for group in self.display_groups: + if len(self.speed_group_stats[group]) > 0: + print(f"speed group: {group:10s} {self.all_groups[group][1]:>96s}") print(f" {'-'*118}") - for k in sorted(speed_group_stats[group].keys()): - v = speed_group_stats[group][k] + for k in sorted(self.speed_group_stats[group].keys()): + v = self.speed_group_stats[group][k] print(f' {k:#2}° | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% | error: {round(v["err"] / v["cnt"], 2):2.2f}° | -:{int(v["-"] / v["cnt"] * 100):#3}% | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#3}% | lim:{v["limited"]:#5} | sat:{v["saturated"]:#5} | path dev: {round(v["dpp"] / v["cnt"], 2):2.2f}m | total: {v["cnt"]:#5}') print("") + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Steering accuracy measurement tool') + parser.add_argument('--route', help="route name") + parser.add_argument('--addr', default='127.0.0.1', help="IP address for optional ZMQ listener, default to msgq") + parser.add_argument('--group', default='all', help="speed group to display, [crawl|slow|medium|fast|veryfast|germany|all], default to all") + parser.add_argument('--cache', default=False, action='store_true', help="use cached data, default to False") + args = parser.parse_args() + + if args.cache: + os.environ['FILEREADER_CACHE'] = '1' + + tool = SteeringAccuracyTool(args) + + if args.route is not None: + print(f"loading {args.route}...") + lr = logreader_from_route_or_segment(args.route, sort_by_time=True) + + sm = {} + for msg in lr: + if msg.which() == 'carState': + sm['carState'] = msg.carState + elif msg.which() == 'carControl': + sm['carControl'] = msg.carControl + elif msg.which() == 'controlsState': + sm['controlsState'] = msg.controlsState + elif msg.which() == 'lateralPlan': + sm['lateralPlan'] = msg.lateralPlan + + if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'lateralPlan' in sm: + tool.update(sm) + + else: + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) + sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr) + time.sleep(1) # Make sure all submaster data is available before going further + + print("waiting for messages...") + while messaging.recv_one(carControl): + sm.update() + tool.update(sm) diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 7e021bcc23..89d33a6127 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -4,13 +4,23 @@ set -e DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" ROOT="$(cd $DIR/../ && pwd)" +SUDO="" # NOTE: this is used in a docker build, so do not run any scripts here. +# Use sudo if not root +if [[ ! $(id -u) -eq 0 ]]; then + if [[ -z $(which sudo) ]]; then + echo "Please install sudo or run as root" + exit 1 + fi + SUDO="sudo" +fi + # Install packages present in all supported versions of Ubuntu function install_ubuntu_common_requirements() { - sudo apt-get update - sudo apt-get install -y --no-install-recommends \ + $SUDO apt-get update + $SUDO apt-get install -y --no-install-recommends \ autoconf \ build-essential \ ca-certificates \ @@ -46,6 +56,7 @@ function install_ubuntu_common_requirements() { libomp-dev \ libopencv-dev \ libpng16-16 \ + libportaudio2 \ libssl-dev \ libsqlite3-dev \ libusb-1.0-0-dev \ @@ -74,7 +85,7 @@ function install_ubuntu_common_requirements() { function install_ubuntu_jammy_requirements() { install_ubuntu_common_requirements - sudo apt-get install -y --no-install-recommends \ + $SUDO apt-get install -y --no-install-recommends \ qtbase5-dev \ qtchooser \ qt5-qmake \ @@ -86,7 +97,7 @@ function install_ubuntu_jammy_requirements() { function install_ubuntu_focal_requirements() { install_ubuntu_common_requirements - sudo apt-get install -y --no-install-recommends \ + $SUDO apt-get install -y --no-install-recommends \ libavresample-dev \ qt5-default \ python-dev diff --git a/tools/webcam/README.md b/tools/webcam/README.md index f896e19b86..237e07cdb6 100644 --- a/tools/webcam/README.md +++ b/tools/webcam/README.md @@ -18,7 +18,7 @@ git clone https://github.com/commaai/openpilot.git - Follow [this readme](https://github.com/commaai/openpilot/tree/master/tools) to install the requirements - Add line "export PYTHONPATH=$HOME/openpilot" to your ~/.bashrc - Install tensorflow 2.2 and nvidia drivers: nvidia-xxx/cuda10.0/cudnn7.6.5 -- Install [OpenCL Driver](http://registrationcenter-download.intel.com/akdlm/irc_nas/vcp/15532/l_opencl_p_18.1.0.015.tgz) +- Install [OpenCL Driver](https://registrationcenter-download.intel.com/akdlm/irc_nas/vcp/15532/l_opencl_p_18.1.0.015.tgz) - Install [OpenCV4](https://www.pyimagesearch.com/2018/08/15/how-to-install-opencv-4-on-ubuntu/) (ignore the Python part) ## Build openpilot for webcam diff --git a/update_requirements.sh b/update_requirements.sh index 8511a0a4d6..e5dedea7a6 100755 --- a/update_requirements.sh +++ b/update_requirements.sh @@ -1,4 +1,4 @@ -#!/bin/bash +#!/usr/bin/env bash set -e DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" @@ -22,8 +22,13 @@ if [ -z "\$PYENV_ROOT" ]; then eval "\$(pyenv virtualenv-init -)" fi EOF + + # setup now without restarting shell + export PATH=$HOME/.pyenv/bin:$HOME/.pyenv/shims:$PATH + export PYENV_ROOT="$HOME/.pyenv" + eval "$(pyenv init -)" + eval "$(pyenv virtualenv-init -)" fi -source $RC_FILE export MAKEFLAGS="-j$(nproc)" @@ -40,7 +45,7 @@ fi eval "$(pyenv init --path)" echo "update pip" -pip install pip==22.3 +pip install pip==22.3.1 pip install poetry==1.2.2 poetry config virtualenvs.prefer-active-python true --local