diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index acc0fcc784cfab..51907b7a44ba60 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -2,7 +2,7 @@ FROM ghcr.io/commaai/openpilot-base:latest ENV PYTHONUNBUFFERED 1 -ENV OPENPILOT_PATH /home/batman/openpilot/ +ENV OPENPILOT_PATH /home/batman/openpilot ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} RUN mkdir -p ${OPENPILOT_PATH} @@ -23,5 +23,6 @@ COPY ./cereal ${OPENPILOT_PATH}/cereal COPY ./panda ${OPENPILOT_PATH}/panda COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive COPY ./system ${OPENPILOT_PATH}/system +COPY ./body ${OPENPILOT_PATH}/body RUN scons --cache-readonly -j$(nproc) diff --git a/Jenkinsfile b/Jenkinsfile index 88f98f33e94a0f..8a2f83be2f5ba5 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -19,6 +19,9 @@ source ~/.bash_profile if [ -f /TICI ]; then source /etc/profile fi +if [ -f /data/openpilot/launch_env.sh ]; then + source /data/openpilot/launch_env.sh +fi ln -snf ${env.TEST_DIR} /data/pythonpath @@ -181,7 +184,7 @@ pipeline { } } - stage('camerad-ar') { + stage('camerad') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { phone_steps("tici-ar0231", [ @@ -189,12 +192,6 @@ pipeline { ["test camerad", "python system/camerad/test/test_camerad.py"], ["test exposure", "python system/camerad/test/test_exposure.py"], ]) - } - } - - stage('camerad-ox') { - agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } - steps { phone_steps("tici-ox03c10", [ ["build", "cd selfdrive/manager && ./build.py"], ["test camerad", "python system/camerad/test/test_camerad.py"], diff --git a/README.md b/README.md index a71a9e671ed66b..5428ea7a9599da 100755 --- a/README.md +++ b/README.md @@ -42,7 +42,7 @@ To use openpilot in a car, you need four things * A supported device to run this software: a [comma three](https://comma.ai/shop/products/three). * This software. The setup procedure of the comma three allows the user to enter a URL for custom software. The URL, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another GitHub username can install a fork. -* One of [the 200+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot. +* One of [the 250+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot. * A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car. We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup). @@ -67,7 +67,7 @@ Documentation related to openpilot development can be found on [docs.comma.ai](h You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel. -Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/). +Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs#open-positions). And [follow us on Twitter](https://twitter.com/comma_ai). diff --git a/RELEASES.md b/RELEASES.md index 4f5708cc1a35c3..a4b2a76875a65a 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,14 +1,30 @@ -Version 0.9.2 (2023-03-XX) +Version 0.9.2 (2023-05-22) ======================== -* New driving model, trained on a new dataset -* Draw MPC path instead of model predicted path, this is a more accurate representation of what the car will do. +* New driving model + * Reduced turn diving + * Trained on a new dataset +* UI updates + * New experimental mode visualization + * Draw MPC path instead of model-predicted path +* AGNOS 7 + * Faster boot time + * Fixes rare no sounds bug + * Fixes bootsplash bug at extreme temperatures * Buick LaCrosse 2017-19 support thanks to koch-cf! * Chevrolet Trailblazer 2021-22 support thanks to TurboCE! +* Ford Bronco Sport 2021-22 support +* Ford Escape 2020-22 support +* Ford Explorer 2020-22 support +* Ford Kuga 2020-22 support +* Ford Maverick 2022-23 support +* Genesis GV80 2023 support thanks to JWingate80! * Honda HR-V 2023 support thanks to AlexandreSato and galegozi! * Kia Niro EV 2023 support thanks to JosselinLecocq! * Lexus ES 2017-18 support +* Lincoln Aviator 2021 support * Škoda Fabia 2022-23 support thanks to jyoung8607! + Version 0.9.1 (2023-02-28) ======================== * New driving model diff --git a/SConstruct b/SConstruct index 320590b05882c5..45b6fd8fdf8511 100644 --- a/SConstruct +++ b/SConstruct @@ -405,13 +405,17 @@ SConscript(['rednose/SConscript']) # Build system services SConscript([ - 'system/camerad/SConscript', 'system/clocksd/SConscript', 'system/proclogd/SConscript', 'system/ubloxd/SConscript', + 'system/loggerd/SConscript', ]) if arch != "Darwin": - SConscript(['system/logcatd/SConscript']) + SConscript([ + 'system/camerad/SConscript', + 'system/sensord/SConscript', + 'system/logcatd/SConscript', + ]) # Build openpilot @@ -428,19 +432,13 @@ SConscript(['third_party/SConscript']) SConscript(['common/kalman/SConscript']) SConscript(['common/transformations/SConscript']) -SConscript(['selfdrive/modeld/SConscript']) - +SConscript(['selfdrive/boardd/SConscript']) SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) - -SConscript(['selfdrive/boardd/SConscript']) - -SConscript(['system/loggerd/SConscript']) - SConscript(['selfdrive/locationd/SConscript']) -SConscript(['system/sensord/SConscript']) -SConscript(['selfdrive/ui/SConscript']) SConscript(['selfdrive/navd/SConscript']) +SConscript(['selfdrive/modeld/SConscript']) +SConscript(['selfdrive/ui/SConscript']) if (arch in ['x86_64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'): SConscript(['tools/replay/SConscript']) diff --git a/cereal b/cereal index dab31315f49389..9acc99c3fbf73b 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit dab31315f493890ebe4ffcdc473772c8226244b7 +Subproject commit 9acc99c3fbf73b1a00125bc89c14ee08718b2d73 diff --git a/common/params.cc b/common/params.cc index adf1e6c70c0120..86678d542864a3 100644 --- a/common/params.cc +++ b/common/params.cc @@ -92,15 +92,15 @@ std::unordered_map keys = { {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, {"CameraDebugExpTime", CLEAR_ON_MANAGER_START}, {"CarBatteryCapacity", PERSISTENT}, - {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, {"CarParamsPersistent", PERSISTENT}, - {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CompletedTrainingVersion", PERSISTENT}, - {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CurrentBootlog", PERSISTENT}, - {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisablePowerDown", PERSISTENT}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, @@ -111,7 +111,7 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, @@ -137,7 +137,7 @@ std::unordered_map keys = { {"IsTestedBranch", CLEAR_ON_MANAGER_START}, {"IsReleaseBranch", CLEAR_ON_MANAGER_START}, {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, - {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"LaikadEphemerisV3", PERSISTENT | DONT_LOG}, {"LanguageSetting", PERSISTENT}, {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, @@ -150,30 +150,30 @@ std::unordered_map keys = { {"LiveParameters", PERSISTENT}, {"LiveTorqueCarParams", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, - {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, - {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavSettingTime24h", PERSISTENT}, {"NavSettingLeftSide", PERSISTENT}, {"NavdRender", PERSISTENT}, - {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"OpenpilotEnabledToggle", PERSISTENT}, - {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet - {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, - {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"SshEnabled", PERSISTENT}, {"SubscriberInfo", PERSISTENT}, {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, @@ -191,17 +191,18 @@ std::unordered_map keys = { {"ApiCache_NavDestinations", PERSISTENT}, {"ApiCache_Owner", PERSISTENT}, {"Offroad_BadNvme", CLEAR_ON_MANAGER_START}, - {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, - {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START}, {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + {"Offroad_Recalibration", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"AccMadsCombo", PERSISTENT}, {"AmapKey1", PERSISTENT}, @@ -387,14 +388,19 @@ std::map Params::readAll() { void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - if (key_type == ALL) { - util::remove_files_in_dir(getParamPath()); - } else { - for (auto &[key, type] : keys) { - if (type & key_type) { - unlink(getParamPath(key).c_str()); + // 1) delete params of key_type + // 2) delete files that are not defined in the keys. + if (DIR *d = opendir(getParamPath().c_str())) { + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (de->d_type != DT_DIR) { + auto it = keys.find(de->d_name); + if (it == keys.end() || (it->second & key_type)) { + unlink(getParamPath(de->d_name).c_str()); + } } } + closedir(d); } fsync_dir(getParamPath()); diff --git a/common/params.h b/common/params.h index aecb3ee471e4c6..24b1bffeb1a88f 100644 --- a/common/params.h +++ b/common/params.h @@ -7,8 +7,8 @@ enum ParamKeyType { PERSISTENT = 0x02, CLEAR_ON_MANAGER_START = 0x04, - CLEAR_ON_IGNITION_ON = 0x08, - CLEAR_ON_IGNITION_OFF = 0x10, + CLEAR_ON_ONROAD_TRANSITION = 0x08, + CLEAR_ON_OFFROAD_TRANSITION = 0x10, DONT_LOG = 0x20, ALL = 0xFFFFFFFF }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 9d8933609fffcb..abb3199d059073 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -9,8 +9,8 @@ cdef extern from "common/params.h": cpdef enum ParamKeyType: PERSISTENT CLEAR_ON_MANAGER_START - CLEAR_ON_IGNITION_ON - CLEAR_ON_IGNITION_OFF + CLEAR_ON_ONROAD_TRANSITION + CLEAR_ON_OFFROAD_TRANSITION ALL cdef cppclass c_Params "Params": diff --git a/common/tests/test_params.py b/common/tests/test_params.py index ec13e822179f4c..d432218c8a7c13 100644 --- a/common/tests/test_params.py +++ b/common/tests/test_params.py @@ -1,7 +1,9 @@ +import os import threading import time import tempfile import shutil +import uuid import unittest from common.params import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking @@ -28,9 +30,16 @@ def test_params_get_cleared_manager_start(self): self.params.put("CarParams", "test") self.params.put("DongleId", "cb38263377b873ee") assert self.params.get("CarParams") == b"test" + + undefined_param = self.params.get_param_path(uuid.uuid4().hex) + with open(undefined_param, "w") as f: + f.write("test") + assert os.path.isfile(undefined_param) + self.params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) assert self.params.get("CarParams") is None assert self.params.get("DongleId") is not None + assert not os.path.isfile(undefined_param) def test_params_two_things(self): self.params.put("DongleId", "bob") diff --git a/common/tests/test_util.cc b/common/tests/test_util.cc index b70cc9044afc9d..25ecf09aa9db83 100644 --- a/common/tests/test_util.cc +++ b/common/tests/test_util.cc @@ -143,20 +143,3 @@ TEST_CASE("util::create_directories") { REQUIRE(util::create_directories("", 0755) == false); } } - -TEST_CASE("util::remove_files_in_dir") { - std::string tmp_dir = "/tmp/test_remove_all_in_dir"; - system("rm /tmp/test_remove_all_in_dir -rf"); - REQUIRE(util::create_directories(tmp_dir, 0755)); - const int tmp_file_cnt = 10; - for (int i = 0; i < tmp_file_cnt; ++i) { - std::string tmp_file = tmp_dir + "/test_XXXXXX"; - int fd = mkstemp((char*)tmp_file.c_str()); - close(fd); - REQUIRE(util::file_exists(tmp_file.c_str())); - } - - REQUIRE(util::read_files_in_dir(tmp_dir).size() == tmp_file_cnt); - util::remove_files_in_dir(tmp_dir); - REQUIRE(util::read_files_in_dir(tmp_dir).empty()); -} diff --git a/common/util.cc b/common/util.cc index 10dff6a9ea29bf..ee1080cbdf2a82 100644 --- a/common/util.cc +++ b/common/util.cc @@ -99,22 +99,6 @@ std::map read_files_in_dir(const std::string &path) { return ret; } -void remove_files_in_dir(const std::string &path) { - DIR *d = opendir(path.c_str()); - if (!d) return; - - std::string fn; - struct dirent *de = NULL; - while ((de = readdir(d))) { - if (de->d_type != DT_DIR) { - fn = path + "/" + de->d_name; - unlink(fn.c_str()); - } - } - - closedir(d); -} - int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { int fd = HANDLE_EINTR(open(path, flags, mode)); if (fd == -1) { diff --git a/common/util.h b/common/util.h index 028074384e19eb..c80dc234f2a20c 100644 --- a/common/util.h +++ b/common/util.h @@ -81,7 +81,6 @@ std::string dir_name(std::string const& path); // **** file fhelpers ***** std::string read_file(const std::string& fn); std::map read_files_in_dir(const std::string& path); -void remove_files_in_dir(const std::string& path); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); FILE* safe_fopen(const char* filename, const char* mode); diff --git a/docs/CARS.md b/docs/CARS.md index 28f9e4eb2f8d36..46f8bd2787f869 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,253 +4,262 @@ 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. Supported vehicles reference the US market unless otherwise specified. -# 243 Supported Cars +# 252 Supported Cars -|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| +|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness Kit
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| -|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,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|| -|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|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|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|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|7 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|| -|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| +|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| -|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|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|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| -|Genesis|G80 2018-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|GV60 (Advanced Trim) 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 A|| -|Genesis|GV60 (Performance Trim) 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|7 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|| -|Honda|Accord Hybrid 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|| -|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|Insight 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|| -|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Hyundai|Elantra 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|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 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 (Southeast Asia only) 2022-23[5](#footnotes)|All|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 (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|| -|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Kona 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 B|| -|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|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 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 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|| -|Hyundai|Santa Fe Plug-in 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|| -|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Sonata 2020-23|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[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 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (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 P|| -|Kia|EV6 (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 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|K5 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 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|| -|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro EV 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 A|| -|Kia|Niro Hybrid 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 F|| -|Kia|Niro 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 A|| -|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| -|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Stinger 2018-20|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 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|| -|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|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|| -|MAN|eTGE 2020-23|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|| -|MAN|TGE 2017-23|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|| -|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-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-23|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-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram|| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|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-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|| -|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B|| -|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|Fabia 2022-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[10](#footnotes)|| -|Š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 2017-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|| -|Š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|| -|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|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-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2021-22|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[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry 2021-23|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-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|| -|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 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|| -|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|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,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|Crafter 2017-23|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|e-Crafter 2018-23|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|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|Grand California 2019-23|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|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 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[10](#footnotes)|| -|Volkswagen|Polo GTI 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[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 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|Tiguan eHybrid 2021-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|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|| +|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Ford|Maverick 2022-23|Co-Pilot360 Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Passport 2019-22|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Elantra 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)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona 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)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai O connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai I connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Cruz 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)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Hyundai|Tucson Hybrid 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (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)](##)|
View- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|EV6 (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)](##)|
View- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Forte 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)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|K5 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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Hybrid 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)](##)|
View- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro 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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|MAN|eTGE 2020-23|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|MAN|TGE 2017-23|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan B connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Ram connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Škoda|Fabia 2022-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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Š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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Škoda|Kodiaq 2017-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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Š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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Š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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Š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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry 2021-23|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 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)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Crafter 2017-23|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|e-Crafter 2018-23|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Grand California 2019-23|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Polo 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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[10](#footnotes)|| +|Volkswagen|Polo GTI 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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
[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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Tiguan 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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|Volkswagen|Tiguan eHybrid 2021-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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| +|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)](##)|
View- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable
|| 1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
@@ -281,6 +290,7 @@ If your car has the following packages or features, then it's a good candidate f | Make | Required Package/Features | | ---- | ------------------------- | | Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | +| Ford | Any car with Lane Centering will likely work. | | Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | | Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | | Nissan | Any car with ProPILOT will likely work. | diff --git a/launch_env.sh b/launch_env.sh index 3059ec268e7cc9..c1ecbe3b31e0f8 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="6.2" + export AGNOS_VERSION="7.1" fi if [ -z "$PASSIVE" ]; then diff --git a/panda b/panda index d8d5012ab7735f..ea44b41f918dc3 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit d8d5012ab7735f2255fe55620850885a424cde8d +Subproject commit ea44b41f918dc3dddabf56320779126400f75f08 diff --git a/poetry.lock b/poetry.lock index 5d60353f80c4ba..e2d894d3c3b8f2 100644 --- a/poetry.lock +++ b/poetry.lock @@ -887,7 +887,7 @@ pgp = ["gpg"] [[package]] name = "efficientnet-pytorch" -version = "0.6.3" +version = "0.7.1" description = "EfficientNet implemented in PyTorch." category = "dev" optional = false @@ -1126,6 +1126,38 @@ category = "dev" optional = false python-versions = ">=3.7" +[[package]] +name = "fsspec" +version = "2023.5.0" +description = "File-system specification" +category = "dev" +optional = false +python-versions = ">=3.8" + +[package.extras] +abfs = ["adlfs"] +adl = ["adlfs"] +arrow = ["pyarrow (>=1)"] +dask = ["dask", "distributed"] +devel = ["pytest", "pytest-cov"] +dropbox = ["dropbox", "dropboxdrivefs", "requests"] +full = ["adlfs", "aiohttp (!=4.0.0a0,!=4.0.0a1)", "dask", "distributed", "dropbox", "dropboxdrivefs", "fusepy", "gcsfs", "libarchive-c", "ocifs", "panel", "paramiko", "pyarrow (>=1)", "pygit2", "requests", "s3fs", "smbprotocol", "tqdm"] +fuse = ["fusepy"] +gcs = ["gcsfs"] +git = ["pygit2"] +github = ["requests"] +gs = ["gcsfs"] +gui = ["panel"] +hdfs = ["pyarrow (>=1)"] +http = ["aiohttp (!=4.0.0a0,!=4.0.0a1)", "requests"] +libarchive = ["libarchive-c"] +oci = ["ocifs"] +s3 = ["s3fs"] +sftp = ["paramiko"] +smb = ["smbprotocol"] +ssh = ["paramiko"] +tqdm = ["tqdm"] + [[package]] name = "ft4222" version = "1.6.0" @@ -1299,6 +1331,34 @@ chardet = ["chardet (>=2.2)"] genshi = ["genshi"] lxml = ["lxml"] +[[package]] +name = "huggingface-hub" +version = "0.14.1" +description = "Client library to download and publish models, datasets and other repos on the huggingface.co hub" +category = "dev" +optional = false +python-versions = ">=3.7.0" + +[package.dependencies] +filelock = "*" +fsspec = "*" +packaging = ">=20.9" +pyyaml = ">=5.1" +requests = "*" +tqdm = ">=4.42.1" +typing-extensions = ">=3.7.4.3" + +[package.extras] +all = ["InquirerPy (==0.3.4)", "Jinja2", "Pillow", "black (>=23.1,<24.0)", "gradio", "jedi", "mypy (==0.982)", "pytest", "pytest-cov", "pytest-env", "pytest-xdist", "ruff (>=0.0.241)", "soundfile", "types-PyYAML", "types-requests", "types-simplejson", "types-toml", "types-tqdm", "types-urllib3"] +cli = ["InquirerPy (==0.3.4)"] +dev = ["InquirerPy (==0.3.4)", "Jinja2", "Pillow", "black (>=23.1,<24.0)", "gradio", "jedi", "mypy (==0.982)", "pytest", "pytest-cov", "pytest-env", "pytest-xdist", "ruff (>=0.0.241)", "soundfile", "types-PyYAML", "types-requests", "types-simplejson", "types-toml", "types-tqdm", "types-urllib3"] +fastai = ["fastai (>=2.4)", "fastcore (>=1.3.27)", "toml"] +quality = ["black (>=23.1,<24.0)", "mypy (==0.982)", "ruff (>=0.0.241)"] +tensorflow = ["graphviz", "pydot", "tensorflow"] +testing = ["InquirerPy (==0.3.4)", "Jinja2", "Pillow", "gradio", "jedi", "pytest", "pytest-cov", "pytest-env", "pytest-xdist", "soundfile"] +torch = ["torch"] +typing = ["types-PyYAML", "types-requests", "types-simplejson", "types-toml", "types-tqdm", "types-urllib3"] + [[package]] name = "humanfriendly" version = "10.0" @@ -3793,20 +3853,22 @@ jeepney = ">=0.6" [[package]] name = "segmentation-models-pytorch" -version = "0.2.1" +version = "0.3.2" description = "Image segmentation models with pre-trained backbones. PyTorch." category = "dev" optional = false -python-versions = ">=3.0.0" +python-versions = ">=3.7.0" [package.dependencies] -efficientnet-pytorch = "0.6.3" +efficientnet-pytorch = "0.7.1" +pillow = "*" pretrainedmodels = "0.7.4" -timm = "0.4.12" +timm = "0.6.12" torchvision = ">=0.5.0" +tqdm = "*" [package.extras] -test = ["pytest"] +test = ["black (==22.3.0)", "flake8 (==4.0.1)", "flake8-docstrings (==1.6.0)", "mock", "pre-commit", "pytest"] [[package]] name = "send2trash" @@ -4302,14 +4364,16 @@ numba = ["numba (>=0.55.2,<0.56.0)"] [[package]] name = "timm" -version = "0.4.12" +version = "0.6.12" description = "(Unofficial) PyTorch Image Models" category = "dev" optional = false python-versions = ">=3.6" [package.dependencies] -torch = ">=1.4" +huggingface-hub = "*" +pyyaml = "*" +torch = ">=1.7" torchvision = "*" [[package]] @@ -4740,7 +4804,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "774e90b7d2bef68c6d219c8afc3d5717a104a04b9cd7b1b215655eb48fa62d04" +content-hash = "0f8861fe679c2ba9a2a6365498a02ee353023e1a3ef882b117516fb7ea28fb58" [metadata.files] adal = [ @@ -5625,7 +5689,7 @@ dulwich = [ {file = "dulwich-0.20.46.tar.gz", hash = "sha256:4f0e88ffff5db1523d93d92f1525fe5fa161318ffbaad502c1b9b3be7a067172"}, ] efficientnet-pytorch = [ - {file = "efficientnet_pytorch-0.6.3.tar.gz", hash = "sha256:6667459336893e9bf6367de3788ba449fed97f65da3b6782bf2204b6273a319f"}, + {file = "efficientnet_pytorch-0.7.1.tar.gz", hash = "sha256:00b9b261effce59d2d47aae2ad238c29a2a65175470f41ada7ecac439b7c1ee1"}, ] einops = [ {file = "einops-0.5.0-py3-none-any.whl", hash = "sha256:055de7eeb3cb9e9710ef3085a811090c6b52e809b7044e8785824ed185f486d1"}, @@ -5842,6 +5906,10 @@ frozenlist = [ {file = "frozenlist-1.3.1-cp39-cp39-win_amd64.whl", hash = "sha256:625d8472c67f2d96f9a4302a947f92a7adbc1e20bedb6aff8dbc8ff039ca6189"}, {file = "frozenlist-1.3.1.tar.gz", hash = "sha256:3a735e4211a04ccfa3f4833547acdf5d2f863bfeb01cfd3edaffbc251f15cec8"}, ] +fsspec = [ + {file = "fsspec-2023.5.0-py3-none-any.whl", hash = "sha256:51a4ad01a5bb66fcc58036e288c0d53d3975a0df2a5dc59a93b59bade0391f2a"}, + {file = "fsspec-2023.5.0.tar.gz", hash = "sha256:b3b56e00fb93ea321bc9e5d9cf6f8522a0198b20eb24e02774d329e9c6fb84ce"}, +] ft4222 = [ {file = "ft4222-1.6.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.manylinux_2_24_aarch64.whl", hash = "sha256:64c80402e19ada10f142cf9d5f5b343a121689b94dfc31fafc7864db13ac7f79"}, {file = "ft4222-1.6.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_24_x86_64.whl", hash = "sha256:5c713b6527513a77e674a6db60d97f67b18ce9f85727168ecbeef82557f2b2d1"}, @@ -6125,6 +6193,10 @@ html5lib = [ {file = "html5lib-1.1-py2.py3-none-any.whl", hash = "sha256:0d78f8fde1c230e99fe37986a60526d7049ed4bf8a9fadbad5f00e22e58e041d"}, {file = "html5lib-1.1.tar.gz", hash = "sha256:b2e5b40261e20f354d198eae92afc10d750afb487ed5e50f9c4eaf07c184146f"}, ] +huggingface-hub = [ + {file = "huggingface_hub-0.14.1-py3-none-any.whl", hash = "sha256:9fc619170d800ff3793ad37c9757c255c8783051e1b5b00501205eb43ccc4f27"}, + {file = "huggingface_hub-0.14.1.tar.gz", hash = "sha256:9ab899af8e10922eac65e290d60ab956882ab0bf643e3d990b1394b6b47b7fbc"}, +] humanfriendly = [ {file = "humanfriendly-10.0-py2.py3-none-any.whl", hash = "sha256:1697e1a8a8f550fd43c2865cd84542fc175a61dcb779b6fee18cf6b6ccba1477"}, {file = "humanfriendly-10.0.tar.gz", hash = "sha256:6b0b831ce8f15f7300721aa49829fc4e83921a9a301cc7f606be6686a2288ddc"}, @@ -8078,8 +8150,8 @@ secretstorage = [ {file = "SecretStorage-3.3.3.tar.gz", hash = "sha256:2403533ef369eca6d2ba81718576c5e0f564d5cca1b58f73a8b23e7d4eeebd77"}, ] segmentation-models-pytorch = [ - {file = "segmentation_models_pytorch-0.2.1-py3-none-any.whl", hash = "sha256:98822571470867fb0f416c112c32f7f1d21702dd32195ec8f7736932c2de0486"}, - {file = "segmentation_models_pytorch-0.2.1.tar.gz", hash = "sha256:86744552b04c6bedf7e10f7928791894d8d9b399b9ed58ed1a3236d2bf69ead6"}, + {file = "segmentation_models_pytorch-0.3.2-py3-none-any.whl", hash = "sha256:dba48e7ead5d34fcb6e5c6d04d6d7c5a61a53fa84264e5481df788a22a1bd66b"}, + {file = "segmentation_models_pytorch-0.3.2.tar.gz", hash = "sha256:8372733e57a10cb8f6b9e18a20577fbb3fb83549b6945664dc774a9b6d3ecd13"}, ] send2trash = [ {file = "Send2Trash-1.8.0-py3-none-any.whl", hash = "sha256:f20eaadfdb517eaca5ce077640cb261c7d2698385a6a0f072a4a5447fd49fa08"}, @@ -8443,8 +8515,8 @@ timezonefinder = [ {file = "timezonefinder-6.1.3.tar.gz", hash = "sha256:f2ee561b1e7692b933fcd914df38800e93db7caf278e7328de7328829b04f275"}, ] timm = [ - {file = "timm-0.4.12-py3-none-any.whl", hash = "sha256:dba6b1702b7d24bf9f0f1c2fc394b4ee28f93cde5404f1dc732d63ccd00533b6"}, - {file = "timm-0.4.12.tar.gz", hash = "sha256:b14be70dbd4528b5ca8657cf5bc2672c7918c3d9ebfbffe80f4785b54e884b1e"}, + {file = "timm-0.6.12-py3-none-any.whl", hash = "sha256:3dfa19b82afa707acc0c2392a84c0e549dd9ea626c285fb2e8d9e4073b58dbd1"}, + {file = "timm-0.6.12.tar.gz", hash = "sha256:8f1747121598e06a1ea2d00df16d332cc284cdd4596bdc136b490a2122d3aa91"}, ] tinycss2 = [ {file = "tinycss2-1.2.1-py3-none-any.whl", hash = "sha256:2b80a96d41e7c3914b8cda8bc7f705a4d9c49275616e886103dd839dfc847847"}, diff --git a/pyproject.toml b/pyproject.toml index 25b61eb50b5ad7..2963337733bc64 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -164,7 +164,7 @@ redis = "^4.3.4" s2sphere = "^0.2.5" scikit-image = "^0.19.3" scikit-learn = "^1.1.1" -segmentation-models-pytorch = "==0.2.1" +segmentation-models-pytorch = "==0.3.2" simplejson = "^3.17.6" SQLAlchemy = "^1.4.39" torch = { url = "https://download.pytorch.org/whl/cu118/torch-2.0.0%2Bcu118-cp38-cp38-linux_x86_64.whl" } diff --git a/selfdrive/assets/training/step8.png b/selfdrive/assets/training/step8.png index c4e8668332cddc..77ff9d73685551 100644 Binary files a/selfdrive/assets/training/step8.png and b/selfdrive/assets/training/step8.png differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 7cb6d5b845f863..8e335c2b581b30 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -517,6 +517,11 @@ def getSshAuthorizedKeys() -> str: return Params().get("GithubSshKeys", encoding='utf8') or '' +@dispatcher.add_method +def getGithubUsername() -> str: + return Params().get("GithubUsername", encoding='utf8') or '' + + @dispatcher.add_method def getSimInfo(): return HARDWARE.get_sim_info() diff --git a/selfdrive/athena/tests/helpers.py b/selfdrive/athena/tests/helpers.py index a43527c2601941..247aedd67a6eb4 100644 --- a/selfdrive/athena/tests/helpers.py +++ b/selfdrive/athena/tests/helpers.py @@ -53,6 +53,7 @@ class MockParams(): default_params = { "DongleId": b"0000000000000000", "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501 + "GithubUsername": b"commaci", "GsmMetered": True, "AthenadUploadQueue": '[]', } diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 128fde0319285e..948fcc07a52537 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -377,6 +377,10 @@ def test_getSshAuthorizedKeys(self): keys = dispatcher["getSshAuthorizedKeys"]() self.assertEqual(keys, MockParams().params["GithubSshKeys"].decode('utf-8')) + def test_getGithubUsername(self): + keys = dispatcher["getGithubUsername"]() + self.assertEqual(keys, MockParams().params["GithubUsername"].decode('utf-8')) + def test_getVersion(self): resp = dispatcher["getVersion"]() keys = ["version", "remote", "branch", "commit"] diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e56e1c21752726..6248da7d4b40d0 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -232,6 +232,8 @@ void can_send_thread(std::vector pandas, bool fake_send) { panda->can_send(event.getSendcan()); LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); } + } else { + LOGE("sendcan too old to send: %llu, %llu", nanos_since_boot(), event.getLogMonoTime()); } } } @@ -388,6 +390,8 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setFanStallCount(health.fan_stall_count); ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid)); ps.setSpiChecksumErrorCount(health.spi_checksum_error_count); + ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); + ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; @@ -464,7 +468,8 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin SubMaster sm({"controlsState"}); Panda *peripheral_panda = pandas[0]; - bool ignition_last = false; + bool is_onroad = false; + bool is_onroad_last = false; std::future safety_future; LOGD("start panda state thread"); @@ -483,27 +488,40 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin ignition = *ignition_opt; - // TODO: make this check fast, currently takes 16ms - // check if we have new pandas and are offroad - if (!ignition && (pandas.size() != Panda::list().size())) { - LOGW("Reconnecting to changed amount of pandas!"); - do_exit = true; - break; + // check if we should have pandad reconnect + if (!ignition) { + bool comms_healthy = true; + for (const auto &panda : pandas) { + comms_healthy &= panda->comms_healthy(); + } + + if (!comms_healthy) { + LOGE("Reconnecting, communication to pandas not healthy"); + do_exit = true; + + // TODO: list is slow, takes 16ms + } else if (pandas.size() != Panda::list().size()) { + LOGW("Reconnecting to changed amount of pandas!"); + do_exit = true; + } + + if (do_exit) { + break; + } } - // clear ignition-based params and set new safety on car start - if (ignition && !ignition_last) { - params.clearAll(CLEAR_ON_IGNITION_ON); + is_onroad = params.getBool("IsOnroad"); + + // set new safety on onroad transition, after params are cleared + if (is_onroad && !is_onroad_last) { if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) { safety_future = std::async(std::launch::async, safety_setter_thread, pandas); } else { LOGW("Safety setter thread already running"); } - } else if (!ignition && ignition_last) { - params.clearAll(CLEAR_ON_IGNITION_OFF); } - ignition_last = ignition; + is_onroad_last = is_onroad; sm.update(0); const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled(); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 4873040f37d21c..1e7b510eca2657 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -13,9 +13,11 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { // try USB first, then SPI try { handle = std::make_unique(serial); + LOGW("conntected to %s over USB", serial.c_str()); } catch (std::exception &e) { #ifndef __APPLE__ handle = std::make_unique(serial); + LOGW("conntected to %s over SPI", serial.c_str()); #endif } diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index 506b96b3722519..f93d2921709ebd 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -1,8 +1,9 @@ #pragma once -#include #include #include +#include +#include #include #ifndef __APPLE__ diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index f0ab76e1dd7398..50027f340f03e0 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -26,7 +26,7 @@ def flash_panda(panda_serial: str) -> Panda: panda = Panda(panda_serial) fw_signature = get_expected_signature(panda) - internal_panda = panda.is_internal() and not panda.bootstub + internal_panda = panda.is_internal() panda_version = "bootstub" if panda.bootstub else panda.get_version() panda_signature = b"" if panda.bootstub else panda.get_signature() @@ -39,7 +39,7 @@ def flash_panda(panda_serial: str) -> Panda: if panda.bootstub: bootstub_version = panda.get_version() - cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}") + cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. {bootstub_version=}, {internal_panda=}") if internal_panda: HARDWARE.recover_internal_panda() panda.recover(reset=(not internal_panda)) @@ -76,23 +76,28 @@ def panda_sort_cmp(a: Panda, b: Panda): def main() -> NoReturn: + count = 0 first_run = True params = Params() while True: try: + count += 1 + cloudlog.event("pandad.flash_and_connect", count=count) params.remove("PandaSignatures") # Flash all Pandas in DFU mode - for serial in PandaDFU.list(): - cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}") - PandaDFU(serial).recover() - time.sleep(1) + dfu_serials = PandaDFU.list() + if len(dfu_serials) > 0: + for serial in dfu_serials: + cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}") + PandaDFU(serial).recover() + time.sleep(1) panda_serials = Panda.list() if len(panda_serials) == 0: if first_run: - cloudlog.info("Resetting internal panda") + cloudlog.info("No pandas found, resetting internal panda") HARDWARE.reset_internal_panda() time.sleep(2) # wait to come back up continue @@ -104,17 +109,6 @@ def main() -> NoReturn: for serial in panda_serials: pandas.append(flash_panda(serial)) - # check health for lost heartbeat - for panda in pandas: - health = panda.health() - if health["heartbeat_lost"]: - params.put_bool("PandaHeartbeatLost", True) - cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) - - if first_run: - cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - panda.reset() - # Ensure internal panda is present if expected internal_pandas = [panda for panda in pandas if panda.is_internal()] if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: @@ -130,13 +124,30 @@ def main() -> NoReturn: # log panda fw versions params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) - # close all pandas + for panda in pandas: + # check health for lost heartbeat + health = panda.health() + if health["heartbeat_lost"]: + params.put_bool("PandaHeartbeatLost", True) + cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) + + if first_run: + cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") + if panda.is_internal(): + HARDWARE.reset_internal_panda() + else: + panda.reset(reconnect=False) + for p in pandas: p.close() + # TODO: wrap all panda exceptions in a base panda exception except (usb1.USBErrorNoDevice, usb1.USBErrorPipe): # a panda was disconnected while setting everything up. let's try again cloudlog.exception("Panda USB exception while setting up") continue + except Exception: + cloudlog.exception("pandad.uncaught_exception") + continue first_run = False diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index d418d2bdac58c2..82a95f77590696 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -283,7 +283,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } // Wait for (N)ACK - tx_buf[0] = 0x12; + tx_buf[0] = 0x11; transfer.len = 1; ret = wait_for_ack(transfer, SPI_HACK); if (ret < 0) { @@ -303,7 +303,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } // Wait for (N)ACK - tx_buf[0] = 0xab; + tx_buf[0] = 0x13; transfer.len = 1; ret = wait_for_ack(transfer, SPI_DACK); if (ret < 0) { diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 6217561bd1affa..4be5b3f7e8e7b9 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -6,7 +6,7 @@ from collections import defaultdict import cereal.messaging as messaging -from cereal import car +from cereal import car, log from common.params import Params from common.spinner import Spinner from common.timeout import Timeout @@ -31,12 +31,13 @@ def tearDownClass(cls): @phone_only @with_processes(['pandad']) def test_loopback(self): - # wait for boardd to init - time.sleep(2) + params = Params() + params.put_bool("IsOnroad", False) with Timeout(60, "boardd didn't start"): sm = messaging.SubMaster(['pandaStates']) - while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0: + while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ + any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): sm.update(1000) num_pandas = len(sm['pandaStates']) @@ -44,21 +45,21 @@ def test_loopback(self): self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \ connect another panda for multipanda tests.") - # boardd blocks on CarVin and CarParams + # boardd safety setting relies on these params cp = car.CarParams.new_message() safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.allOutput cp.safetyConfigs = [safety_config]*num_pandas - params = Params() + params.put_bool("IsOnroad", True) params.put_bool("FirmwareQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) sendcan = messaging.pub_sock('sendcan') can = messaging.sub_sock('can', conflate=False, timeout=100) - time.sleep(0.2) + time.sleep(0.5) n = 200 for i in range(n): diff --git a/selfdrive/boardd/tests/test_pandad.py b/selfdrive/boardd/tests/test_pandad.py index 09dba6ec7a42dc..50d24f4fe35176 100755 --- a/selfdrive/boardd/tests/test_pandad.py +++ b/selfdrive/boardd/tests/test_pandad.py @@ -3,8 +3,9 @@ import unittest import cereal.messaging as messaging -from panda import Panda +from cereal import log from common.gpio import gpio_set, gpio_init +from panda import Panda from selfdrive.test.helpers import phone_only from selfdrive.manager.process_config import managed_processes from system.hardware import HARDWARE @@ -20,10 +21,10 @@ def _wait_for_boardd(self, timeout=30): sm = messaging.SubMaster(['peripheralState']) for _ in range(timeout): sm.update(1000) - if sm.updated['peripheralState']: + if sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: break - if not sm.updated['peripheralState']: + if sm['peripheralState'].pandaType == log.PandaState.PandaType.unknown: raise Exception("boardd failed to start") @phone_only @@ -54,6 +55,18 @@ def test_internal_panda_reset(self): assert any(Panda(s).is_internal() for s in Panda.list()) + @phone_only + def test_best_case_startup_time(self): + # run once so we're setup + managed_processes['pandad'].start() + self._wait_for_boardd() + managed_processes['pandad'].stop() + + # should be fast this time + managed_processes['pandad'].start() + self._wait_for_boardd(8) + + #def test_out_of_date_fw(self): # pass diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md index 8a879954fa1c74..d1d18b6e3df9cf 100644 --- a/selfdrive/car/CARS_template.md +++ b/selfdrive/car/CARS_template.md @@ -1,6 +1,10 @@ -{% set footnote_tag = '[{}](#footnotes)' -%} -{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' -%} -{% set video_icon = '' -%} +{% set footnote_tag = '[{}](#footnotes)' %} +{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' %} +{% set video_icon = '' %} +{# Force harness column wider by using a blank image with max width. #} +{% set width_tag = '%s
 ' %} +{% set harness_col_name = 'Harness Kit' %} +{% set wide_harness_col_name = width_tag|format(harness_col_name) -%} @@ -10,7 +14,7 @@ A supported vehicle is one that just works when you install a comma three. All s # {{all_car_info | length}} Supported Cars -|{{Column | map(attribute='value') | join('|')}}| +|{{Column | map(attribute='value') | join('|') | replace(harness_col_name, wide_harness_col_name)}}| |---|---|---|{% for _ in range((Column | length) - 3) %}{{':---:|'}}{% endfor +%} {% for car_info in all_car_info %} |{% for column in Column %}{{car_info.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %} @@ -39,6 +43,7 @@ If your car has the following packages or features, then it's a good candidate f | Make | Required Package/Features | | ---- | ------------------------- | | Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | +| Ford | Any car with Lane Centering will likely work. | | Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | | Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | | Nissan | Any car with ProPILOT will likely work. | diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 8799f073c956d3..17ba59f964099a 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,12 +1,11 @@ from enum import IntFlag from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -61,13 +60,13 @@ def __init__(self, CP): @dataclass class ChryslerCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC)" - harness: Enum = Harness.fca + harness_kit: HarnessKit = HarnessKit(Harness.fca) CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"), CAR.PACIFICA_2018_HYBRID: None, # same platforms - CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-22"), + CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-23"), CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), CAR.PACIFICA_2020: [ ChryslerCarInfo("Chrysler Pacifica 2019-20"), @@ -75,10 +74,10 @@ class ChryslerCarInfo(CarInfo): ], CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), - CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness=Harness.ram), + CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness_kit=HarnessKit(Harness.ram)), CAR.RAM_HD: [ - ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram), - ChryslerCarInfo("Ram 3500 2019-22", harness=Harness.ram), + ChryslerCarInfo("Ram 2500 2020-22", harness_kit=HarnessKit(Harness.ram)), + ChryslerCarInfo("Ram 3500 2019-22", harness_kit=HarnessKit(Harness.ram)), ], } @@ -127,6 +126,9 @@ class ChryslerCarInfo(CarInfo): # Based on "8190c7275a24557b|2020-02-24--09-57-23" { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }, + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 450: 8, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2018: 8, 2020: 8, 2026: 8, 2028: 8 }], CAR.JEEP_CHEROKEE: [{ 55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 @@ -224,6 +226,7 @@ class ChryslerCarInfo(CarInfo): b'68453514AD', b'68510283AG', b'68527375AD', + b'68527346AE', ], (Ecu.srs, 0x744, None): [ b'68428609AB', @@ -246,6 +249,7 @@ class ChryslerCarInfo(CarInfo): b'68535469AB', b'68535470AC', b'68586307AB', + b'68548900AB', ], (Ecu.fwdRadar, 0x753, None): [ b'04672892AB', @@ -273,6 +277,7 @@ class ChryslerCarInfo(CarInfo): b'68552788AA', b'68552790AA', b'68585112AB', + b'68552789AA', ], (Ecu.engine, 0x7e0, None): [ b'05036065AE ', @@ -284,6 +289,7 @@ class ChryslerCarInfo(CarInfo): b'68500630AD', b'68500630AE', b'68539650AD', + b'05149846AA ', ], (Ecu.transmission, 0x7e1, None): [ b'68360078AL', @@ -296,6 +302,7 @@ class ChryslerCarInfo(CarInfo): b'68484467AC', b'68502994AD', b'68540431AB', + b'68520867AE', ], }, diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py index cd3e93fa80c1bd..ed98e14dc146c2 100755 --- a/selfdrive/car/disable_ecu.py +++ b/selfdrive/car/disable_ecu.py @@ -31,8 +31,8 @@ def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01' except Exception: cloudlog.exception("ecu disable exception") - print(f"ecu disable retry ({i+1}) ...") - cloudlog.warning("ecu disable failed") + cloudlog.error(f"ecu disable retry ({i + 1}) ...") + cloudlog.error("ecu disable failed") return False diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index fa95b03fd8c870..bfb182c5e91928 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -1,5 +1,6 @@ import re from collections import namedtuple +import copy from dataclasses import dataclass, field from enum import Enum from typing import Dict, List, Optional, Tuple, Union @@ -20,7 +21,7 @@ class Column(Enum): FSR_STEERING = "No ALC below" STEERING_TORQUE = "Steering Torque" AUTO_RESUME = "Resume from stop" - HARNESS = "Harness" + HARNESS = "Harness Kit" VIDEO = "Video" @@ -69,6 +70,23 @@ class Harness(Enum): none = "None" +class HarnessPart(Enum): + harness_box = "harness box" + comma_power_v2 = "comma power v2" + rj45_cable = "RJ45 cable (7 ft)" + long_obdc_cable = "long OBD-C cable" + usbc_coupler = "USB-C coupler" + + +DEFAULT_HARNESS_PARTS: List[HarnessPart] = [HarnessPart.harness_box, HarnessPart.comma_power_v2, HarnessPart.rj45_cable] + + +@dataclass +class HarnessKit: + connector: Harness = Harness.none + parts: List[HarnessPart] = field(default_factory=lambda: copy.copy(DEFAULT_HARNESS_PARTS)) + + CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False)) @@ -135,7 +153,9 @@ class CarInfo: footnotes: List[Enum] = field(default_factory=list) min_steer_speed: Optional[float] = None min_enable_speed: Optional[float] = None - harness: Enum = Harness.none + + # harness connectors + all the parts needed + harness_kit: HarnessKit = HarnessKit() def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): self.car_name = CP.carName @@ -165,12 +185,14 @@ def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): self.min_enable_speed = CP.minEnableSpeed # harness column - harness_col = self.harness.value - if self.harness is not Harness.none: + harness_col = self.harness_kit.connector.value + if self.harness_kit.connector is not Harness.none: model_years = self.model + (' ' + self.years if self.years else '') - harness_col = f'{harness_col}' + harness_connector = f'- 1 {harness_col} connector' + harness_parts = '
'.join([f"- {self.harness_kit.parts.count(part)} {part.value}" for part in sorted(set(self.harness_kit.parts), key=lambda part: part.value)]) + harness_col = f'
View{harness_connector}
{harness_parts}
' - self.row = { + self.row: Dict[Enum, Union[str, Star]] = { Column.MAKE: self.make, Column.MODEL: self.model, Column.PACKAGE: self.package, diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 99072ae9756d85..d9a9ae6bc0902f 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -2,13 +2,26 @@ from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits -from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ +from selfdrive.car.ford.fordcan import create_acc_msg, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams +LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert +def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw): + # No blending at low speed due to lack of torque wind-up and inaccurate current curvature + if v_ego_raw > 9: + apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, + current_curvature + CarControllerParams.CURVATURE_ERROR) + + # Curvature rate limit after driver torque limit + apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams) + + return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) + + class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -43,17 +56,16 @@ def update(self, CC, CS, now_nanos): can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### - # send steering commands at 20Hz + # send steer msg at 20Hz if (self.frame % CarControllerParams.STEER_STEP) == 0: if CC.latActive: - # apply limits to curvature and clip to signal range - apply_curvature = apply_std_steer_angle_limits(actuators.curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams) - apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) + # apply rate limits, curvature error limit, and clip to signal range + current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + apply_curvature = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw) else: apply_curvature = 0. self.apply_curvature_last = apply_curvature - can_sends.append(create_lka_msg(self.packer)) if self.CP.carFingerprint in CANFD_CARS: # TODO: extended mode @@ -63,31 +75,32 @@ def update(self, CC, CS, now_nanos): else: can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + # send lka msg at 33Hz + if (self.frame % CarControllerParams.LKA_STEP) == 0: + can_sends.append(create_lka_msg(self.packer)) + ### longitudinal control ### - # send acc command at 50Hz + # send acc msg at 50Hz if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + # Both gas and accel are in m/s^2, accel is used solely for braking accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + gas = accel + if not CC.longActive or gas < CarControllerParams.MIN_GAS: + gas = CarControllerParams.INACTIVE_GAS - precharge_brake = accel < -0.1 - if accel > -0.5: - gas = accel - decel = False - else: - gas = -5.0 - decel = True - - can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) + stopping = CC.actuators.longControlState == LongCtrlState.stopping + can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, stopping)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) - - # send lkas ui command at 1Hz or if ui state changes + # send lkas ui msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) - - # send acc ui command at 20Hz or if ui state changes + # send acc ui msg at 5Hz or if ui state changes if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + can_sends.append(create_acc_ui_msg(self.packer, self.CP, main_on, CC.latActive, + CS.out.cruiseState.standstill, hud_control, + CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 21af1062f3646a..9be2c7637cae0a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -16,9 +16,21 @@ def __init__(self, CP): if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] + self.vehicle_sensors_valid = False + self.hybrid_platform = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() + # Hybrid variants experience a bug where a message from the PCM sends invalid checksums, + # we do not support these cars at this time. + # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants + self.hybrid_platform = cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 + + # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement + # The vehicle usually recovers out of this state within a minute of normal driving + self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3 + # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) @@ -48,6 +60,7 @@ def update(self, cp, cp_cam): ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 + ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2) # gear if self.CP.transmissionType == TransmissionType.automatic: @@ -62,7 +75,7 @@ def update(self, cp, cp_cam): # safety ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) - ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled + ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"]) # button presses ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 @@ -92,6 +105,8 @@ def update(self, cp, cp_cam): def get_can_parser(CP): signals = [ # sig_name, sig_address + ("TrnAinTq_D_Qf", "VehicleOperatingModes"), # Used to detect hybrid or ICE platform variant + ("Veh_V_ActlBrk", "BrakeSysFeatures"), # ABS vehicle speed (kph) ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped @@ -105,6 +120,7 @@ def get_can_parser(CP): ("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) + ("StePinCompAnEst_D_Qf", "SteeringPinion_Data"), # PSCM estimated steering angle (quality flag) # Calculates steering angle (and offset) from pinion # angle and driving measurements. # StePinRelInit_An_Sns is the pinion angle, initialised @@ -125,7 +141,6 @@ def get_can_parser(CP): ("AccButtnGapIncPress", "Steering_Data_FD1"), ("AslButtnOnOffCnclPress", "Steering_Data_FD1"), ("AslButtnOnOffPress", "Steering_Data_FD1"), - ("CcAslButtnCnclPress", "Steering_Data_FD1"), ("LaSwtchPos_D_Stat", "Steering_Data_FD1"), ("CcAslButtnCnclResPress", "Steering_Data_FD1"), ("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"), @@ -139,7 +154,6 @@ def get_can_parser(CP): ("CcAslButtnSetDecPress", "Steering_Data_FD1"), ("CcAslButtnSetIncPress", "Steering_Data_FD1"), ("CcAslButtnSetPress", "Steering_Data_FD1"), - ("CcAsllButtnResPress", "Steering_Data_FD1"), ("CcButtnOffPress", "Steering_Data_FD1"), ("CcButtnOnOffCnclPress", "Steering_Data_FD1"), ("CcButtnOnOffPress", "Steering_Data_FD1"), @@ -154,6 +168,7 @@ def get_can_parser(CP): checks = [ # sig_address, frequency + ("VehicleOperatingModes", 100), ("BrakeSysFeatures", 50), ("Yaw_Data_FD1", 100), ("DesiredTorqBrk", 50), @@ -202,6 +217,8 @@ def get_can_parser(CP): def get_cam_can_parser(CP): signals = [ # sig_name, sig_address + ("CmbbBrkDecel_B_Rq", "ACCDATA_2"), # AEB actuation request bit + ("HaDsply_No_Cs", "ACCDATA_3"), ("HaDsply_No_Cnt", "ACCDATA_3"), ("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message @@ -216,7 +233,7 @@ def get_cam_can_parser(CP): ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting ("CadsAlignIncplt_B_Actl", "ACCDATA_3"), - ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting + ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC lead indicator ("CadsRadrBlck_B_Actl", "ACCDATA_3"), ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting @@ -233,9 +250,7 @@ def get_cam_can_parser(CP): ("FeatNoIpmaActl", "IPMA_Data"), ("PersIndexIpma_D_Actl", "IPMA_Data"), ("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping - ("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) ("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error - ("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime ("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? ("CamraStats_D_Dsply", "IPMA_Data"), # Camera status ("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level @@ -248,6 +263,7 @@ def get_cam_can_parser(CP): checks = [ # sig_address, frequency + ("ACCDATA_2", 50), ("ACCDATA_3", 5), ("IPMA_Data", 1), ] diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index d2fb6fad5552b8..97a8c025d4c914 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -5,11 +5,15 @@ def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + curvature = (dat[2] << 3) | ((dat[3]) >> 5) + curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5) + path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2) + path_offset = ((dat[4] & 0x3) << 8) | dat[5] + checksum = mode + counter - checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature - checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate - checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle - checksum += (dat[4] & 0x3) + dat[5] # path offset + for sig_val in (curvature, curvature_rate, path_angle, path_offset): + checksum += sig_val + (sig_val >> 8) + return 0xFF - (checksum & 0xFF) @@ -19,7 +23,7 @@ def create_lka_msg(packer): This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout. - Frequency is 20Hz. + Frequency is 33Hz. """ return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) @@ -97,7 +101,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) -def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): +def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping: bool): """ Creates a CAN message for the Ford ACC Command. @@ -107,16 +111,95 @@ def create_acc_command(packer, long_active: bool, gas: float, accel: float, prec Frequency is 50Hz. """ + decel = accel < 0 and long_active values = { "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 - "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccResumEnbl_B_Rq": 1 if long_active else 0, + # TODO: we may be able to improve braking response by utilizing pre-charging better + "AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + "AccStopStat_B_Rq": 1 if stopping else 0, } return packer.make_can_msg("ACCDATA", CANBUS.main, values) +def create_acc_ui_msg(packer, CP, main_on: bool, enabled: bool, standstill: bool, hud_control, + stock_values: dict): + """ + Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam + assist status. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 5Hz. + """ + + # Tja_D_Stat + if enabled: + if hud_control.leftLaneDepart: + status = 3 # ActiveInterventionLeft + elif hud_control.rightLaneDepart: + status = 4 # ActiveInterventionRight + else: + status = 2 # Active + elif main_on: + if hud_control.leftLaneDepart: + status = 5 # ActiveWarningLeft + elif hud_control.rightLaneDepart: + status = 6 # ActiveWarningRight + else: + status = 1 # Standby + else: + status = 0 # Off + + values = {s: stock_values[s] for s in [ + "HaDsply_No_Cs", + "HaDsply_No_Cnt", + "AccStopStat_D_Dsply", # ACC stopped status message + "AccTrgDist2_D_Dsply", # ACC target distance + "AccStopRes_B_Dsply", + "TjaWarn_D_Rq", # TJA warning + "TjaMsgTxt_D_Dsply", # TJA text + "IaccLamp_D_Rq", # iACC status icon + "AccMsgTxt_D2_Rq", # ACC text + "FcwDeny_B_Dsply", # FCW disabled + "FcwMemStat_B_Actl", # FCW enabled setting + "AccTGap_B_Dsply", # ACC time gap display setting + "CadsAlignIncplt_B_Actl", + "AccFllwMde_B_Dsply", # ACC follow mode display setting + "CadsRadrBlck_B_Actl", + "CmbbPostEvnt_B_Dsply", # AEB event status + "AccStopMde_B_Dsply", # ACC stop mode display setting + "FcwMemSens_D_Actl", # FCW sensitivity setting + "FcwMsgTxt_D_Rq", # FCW text + "AccWarn_D_Dsply", # ACC warning + "FcwVisblWarn_B_Rq", # FCW visible alert + "FcwAudioWarn_B_Rq", # FCW audio alert + "AccTGap_D_Dsply", # ACC time gap + "AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting + "FdaMem_B_Stat", # FDA enabled setting + ]} + + values.update({ + "Tja_D_Stat": status, # TJA status + }) + + if CP.openpilotLongitudinalControl: + values.update({ + "AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text + "AccMsgTxt_D2_Rq": 0, # ACC text + "AccTGap_B_Dsply": 0, # Show time gap control UI + "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator + "AccStopMde_B_Dsply": 1 if standstill else 0, + "AccWarn_D_Dsply": 0, # ACC warning + "AccTGap_D_Dsply": 4, # Fixed time gap in UI + }) + + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) + + def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. @@ -160,60 +243,76 @@ def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hands_on_wheel_dsply = 1 if steer_alert else 0 - values = { - **stock_values, + values = {s: stock_values[s] for s in [ + "FeatConfigIpmaActl", + "FeatNoIpmaActl", + "PersIndexIpma_D_Actl", + "AhbcRampingV_D_Rq", # AHB ramping + "LaDenyStats_B_Dsply", # LKAS error + "CamraDefog_B_Req", # Windshield heater? + "CamraStats_D_Dsply", # Camera status + "DasAlrtLvl_D_Dsply", # DAS alert level + "DasStats_D_Dsply", # DAS status + "DasWarn_D_Dsply", # DAS warning + "AhbHiBeam_D_Rq", # AHB status + "Passthru_63", + "Passthru_48", + ]} + + values.update({ "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed - } + }) return packer.make_can_msg("IPMA_Data", CANBUS.main, values) -def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): - """ - Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. - - Stock functionality is maintained by passing through unmodified signals. - - Frequency is 20Hz. - """ - - # Tja_D_Stat - if enabled: - if hud_control.leftLaneDepart: - status = 3 # ActiveInterventionLeft - elif hud_control.rightLaneDepart: - status = 4 # ActiveInterventionRight - else: - status = 2 # Active - elif main_on: - if hud_control.leftLaneDepart: - status = 5 # ActiveWarningLeft - elif hud_control.rightLaneDepart: - status = 6 # ActiveWarningRight - else: - status = 1 # Standby - else: - status = 0 # Off - - values = { - **stock_values, - "Tja_D_Stat": status, - } - return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) - - def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False, bus: int = CANBUS.camera): """ Creates a CAN message for the Ford SCCM buttons/switches. Includes cruise control buttons, turn lights and more. + + Frequency is 10Hz. """ - values = { - **stock_values, + values = {s: stock_values[s] for s in [ + "HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons + "TurnLghtSwtch_D_Stat", # SCCM Turn signal switch + "WiprFront_D_Stat", + "LghtAmb_D_Sns", + "AccButtnGapDecPress", + "AccButtnGapIncPress", + "AslButtnOnOffCnclPress", + "AslButtnOnOffPress", + "LaSwtchPos_D_Stat", + "CcAslButtnCnclResPress", + "CcAslButtnDeny_B_Actl", + "CcAslButtnIndxDecPress", + "CcAslButtnIndxIncPress", + "CcAslButtnOffCnclPress", + "CcAslButtnOnOffCncl", + "CcAslButtnOnPress", + "CcAslButtnResDecPress", + "CcAslButtnResIncPress", + "CcAslButtnSetDecPress", + "CcAslButtnSetIncPress", + "CcAslButtnSetPress", + "CcButtnOffPress", + "CcButtnOnOffCnclPress", + "CcButtnOnOffPress", + "CcButtnOnPress", + "HeadLghtHiFlash_D_Actl", + "HeadLghtHiOn_B_StatAhb", + "AhbStat_B_Dsply", + "AccButtnGapTogglePress", + "WiprFrontSwtch_D_Stat", + "HeadLghtHiCtrl_D_RqAhb", + ]} + + values.update({ "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button "CcAsllButtnResPress": 1 if resume else 0, # CC resume button - "TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button - } + "TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button + }) return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 60571b42efd023..937980899fa77a 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -15,8 +15,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] - # These cars are dashcam only until the port is finished - ret.dashcamOnly = True + # These cars are dashcam only for lack of test coverage. + # 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.FOCUS_MK4} ret.radarUnavailable = True ret.steerControlType = car.CarParams.SteerControlType.angle @@ -53,7 +55,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] - if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]: + if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0] or docs: ret.transmissionType = TransmissionType.automatic else: ret.transmissionType = TransmissionType.manual @@ -74,6 +76,11 @@ def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) + if not self.CS.vehicle_sensors_valid: + events.add(car.CarEvent.EventName.vehicleSensorsInvalid) + if self.CS.hybrid_platform: + events.add(car.CarEvent.EventName.startupNoControl) + ret.events = events.to_msg() return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index c3652443c202c6..6425179a27b108 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,27 +1,22 @@ from collections import defaultdict from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Set, Union from cereal import car from selfdrive.car import AngleRateLimit, dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu class CarControllerParams: - # Messages: Lane_Assist_Data1, LateralMotionControl - STEER_STEP = 5 - # Message: ACCDATA - ACC_CONTROL_STEP = 2 - # Message: IPMA_Data - LKAS_UI_STEP = 100 - # Message: ACCDATA_3 - ACC_UI_STEP = 5 - # Message: Steering_Data_FD1, but send twice as fast - BUTTONS_STEP = 10 / 2 + STEER_STEP = 5 # LateralMotionControl, 20Hz + LKA_STEP = 3 # Lane_Assist_Data1, 33Hz + ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz + LKAS_UI_STEP = 100 # IPMA_Data, 1Hz + ACC_UI_STEP = 20 # ACCDATA_3, 5Hz + BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm @@ -32,9 +27,12 @@ class CarControllerParams: # Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001]) ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015]) + CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s ACCEL_MAX = 2.0 # m/s^s max acceleration ACCEL_MIN = -3.5 # m/s^s max deceleration + MIN_GAS = -0.5 + INACTIVE_GAS = -5.0 def __init__(self, CP): pass @@ -68,23 +66,20 @@ class RADAR: @dataclass class FordCarInfo(CarInfo): package: str = "Co-Pilot360 Assist+" - harness: Enum = Harness.ford_q3 + harness_kit: HarnessKit = HarnessKit(Harness.ford_q3) CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"), CAR.ESCAPE_MK4: [ FordCarInfo("Ford Escape 2020-22"), - FordCarInfo("Ford Escape Plug-in Hybrid 2020-22"), - FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), - FordCarInfo("Ford Kuga Plug-in Hybrid 2020-22", "Driver Assistance Pack"), + FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"), ], CAR.EXPLORER_MK6: [ FordCarInfo("Ford Explorer 2020-22"), FordCarInfo("Lincoln Aviator 2021", "Co-Pilot360 Plus"), - FordCarInfo("Lincoln Aviator Plug-in Hybrid 2021", "Co-Pilot360 Plus"), ], - CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), + CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2018", "Adaptive Cruise Control with Lane Centering"), CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"), } @@ -162,11 +157,13 @@ class FordCarInfo(CarInfo): }, CAR.EXPLORER_MK6: { (Ecu.eps, 0x730, None): [ + b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -176,11 +173,13 @@ class FordCarInfo(CarInfo): b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ + b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7E0, None): [ + b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index bc2858a6675a00..865ba8ed0f827a 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -5,7 +5,7 @@ from cereal import car from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart Ecu = car.CarParams.Ecu @@ -89,9 +89,9 @@ class GMCarInfo(CarInfo): def init_make(self, CP: car.CarParams): if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera: - self.harness = Harness.gm + self.harness_kit = HarnessKit(Harness.gm) else: - self.harness = Harness.obd_ii + self.harness_kit = HarnessKit(Harness.obd_ii, parts=[HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler]) self.footnotes.append(Footnote.OBD_II) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 951d3c82184a35..05c6f25432f855 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -6,7 +6,7 @@ from common.conversions import Conversions as CV from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -110,9 +110,9 @@ class HondaCarInfo(CarInfo): 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 + self.harness_kit = HarnessKit(Harness.bosch_b) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else HarnessKit(Harness.bosch_a) else: - self.harness = Harness.nidec + self.harness_kit = HarnessKit(Harness.nidec) CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { @@ -146,7 +146,7 @@ def init_make(self, CP: car.CarParams): 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), - HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarInfo("Honda Passport 2019-22", "All", min_steer_speed=12. * CV.MPH_TO_MS), ], CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), @@ -1114,6 +1114,7 @@ def init_make(self, CP: car.CarParams): b'28101-5EZ-A210\x00\x00', b'28101-5EZ-A600\x00\x00', b'28101-5EZ-A430\x00\x00', + b'28101-5EZ-A700\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-RLV-4060\x00\x00', @@ -1128,6 +1129,7 @@ def init_make(self, CP: car.CarParams): b'37805-RLV-B220\x00\x00', b'37805-RLV-B210\x00\x00', b'37805-RLV-L160\x00\x00', + b'37805-RLV-B420\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', @@ -1141,6 +1143,7 @@ def init_make(self, CP: car.CarParams): b'39990-TG7-A060\x00\x00', b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', + b'39990-TGS-A320\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TG7-A310\x00\x00', @@ -1161,6 +1164,7 @@ def init_make(self, CP: car.CarParams): b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', b'36161-TGS-A030\x00\x00', + b'36161-TGS-A220\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TG7-A020\x00\x00', @@ -1168,6 +1172,7 @@ def init_make(self, CP: car.CarParams): b'77959-TG7-A210\x00\x00', b'77959-TG7-Y210\x00\x00', b'77959-TGS-A010\x00\x00', + b'77959-TGS-A110\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TG7-A040\x00\x00', @@ -1201,6 +1206,7 @@ def init_make(self, CP: car.CarParams): b'78109-TGS-AT20\x00\x00', b'78109-TGS-AX20\x00\x00', b'78109-TGS-AJ20\x00\x00', + b'78109-TGS-AC10\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A130\x00\x00', diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index fbd2da067853eb..ded3e9bb36d472 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -232,6 +232,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.mass = 2200 ret.wheelbase = 3.15 ret.steerRatio = 12.069 + elif candidate == CAR.GENESIS_GV80: + ret.mass = 2258. + STD_CARGO_KG + ret.wheelbase = 2.95 + ret.steerRatio = 14.14 # *** longitudinal control *** if candidate in CANFD_CAR: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index c107c0c004bb03..0ad8546331f16a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -6,7 +6,7 @@ from panda.python import uds from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -132,6 +132,7 @@ class CAR: GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" GENESIS_G80 = "GENESIS G80 2017" GENESIS_G90 = "GENESIS G90 2017" + GENESIS_GV80 = "GENESIS GV80 2023" class Footnote(Enum): @@ -153,107 +154,114 @@ def init_make(self, CP: car.CarParams): 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 2017-19", harness=Harness.hyundai_e), + HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_b)), + HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)), + HyundaiCarInfo("Hyundai i30 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)), ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", 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), + CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)), CAR.HYUNDAI_GENESIS: [ - HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages - HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)), # TODO: check 2015 packages + HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)), ], - CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), - CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h), # TODO: confirm 2020-21 harness - CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c), - CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness=Harness.hyundai_h), - CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c), - CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", harness=Harness.hyundai_h), - CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness=Harness.hyundai_b), - CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness=Harness.hyundai_g), - CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness=Harness.hyundai_o), - CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i), # TODO: check packages - CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d), - CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l), - CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l), - CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l), - CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), - CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e), + CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness_kit=HarnessKit(Harness.hyundai_h)), # TODO: confirm 2020-21 harness + CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness_kit=HarnessKit(Harness.hyundai_b)), + CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness_kit=HarnessKit(Harness.hyundai_g)), + CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness_kit=HarnessKit(Harness.hyundai_o)), + CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness_kit=HarnessKit(Harness.hyundai_i)), # TODO: check packages + CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness_kit=HarnessKit(Harness.hyundai_d)), + CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness_kit=HarnessKit(Harness.hyundai_e)), CAR.TUCSON: [ - HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), - HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l), + HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness_kit=HarnessKit(Harness.hyundai_l)), ], CAR.PALISADE: [ - HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Telluride 2020-22", "All", harness=Harness.hyundai_h), + HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness_kit=HarnessKit(Harness.hyundai_h)), + HyundaiCarInfo("Kia Telluride 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)), ], - 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.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_e)), + CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_a)), CAR.IONIQ_5: [ - HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_q), - 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), + HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_q)), + HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_k)), + HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_q)), ], CAR.TUCSON_4TH_GEN: [ - HyundaiCarInfo("Hyundai Tucson 2022", harness=Harness.hyundai_n), - HyundaiCarInfo("Hyundai Tucson 2023", "All", harness=Harness.hyundai_n), + HyundaiCarInfo("Hyundai Tucson 2022", harness_kit=HarnessKit(Harness.hyundai_n)), + HyundaiCarInfo("Hyundai Tucson 2023", "All", harness_kit=HarnessKit(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 2022-23", harness=Harness.hyundai_n), + CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_n)), + CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", harness_kit=HarnessKit(Harness.hyundai_n)), # Kia - CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a), - CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness=Harness.hyundai_a), + CAR.KIA_FORTE: [ + HyundaiCarInfo("Kia Forte 2019-21", harness_kit=HarnessKit(Harness.hyundai_g)), + HyundaiCarInfo("Kia Forte 2023", harness_kit=HarnessKit(Harness.hyundai_e)), + ], + CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness_kit=HarnessKit(Harness.hyundai_a)), CAR.KIA_NIRO_EV: [ - HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), - HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), - HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), + HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)), + HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_f)), + HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)), ], - CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", harness=Harness.hyundai_a), + CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)), CAR.KIA_NIRO_PHEV: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness=Harness.hyundai_d), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness_kit=HarnessKit(Harness.hyundai_d)), ], CAR.KIA_NIRO_HEV_2021: [ - HyundaiCarInfo("Kia Niro Hybrid 2021-22", harness=Harness.hyundai_f), # TODO: 2021 could be hyundai_d, verify + HyundaiCarInfo("Kia Niro Hybrid 2021-22", harness_kit=HarnessKit(Harness.hyundai_f)), # TODO: 2021 could be hyundai_d, verify ], - CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness=Harness.hyundai_a), - CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 - CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), + CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness_kit=HarnessKit(Harness.hyundai_b)), # TODO: may support 2016, 2018 + CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness_kit=HarnessKit(Harness.hyundai_g)), CAR.KIA_OPTIMA_H: [ HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years 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", harness=Harness.hyundai_n), + CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness_kit=HarnessKit(Harness.hyundai_n)), CAR.KIA_SORENTO: [ - HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="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), + HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_c)), + HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_e)), ], - CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness=Harness.hyundai_k), - CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness=Harness.hyundai_a), - CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness=Harness.hyundai_n), - CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c), - CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness=Harness.hyundai_k), - CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e), + CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness_kit=HarnessKit(Harness.hyundai_a)), + CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_n)), + CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness_kit=HarnessKit(Harness.hyundai_k)), + CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness_kit=HarnessKit(Harness.hyundai_e)), CAR.KIA_EV6: [ - HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_p), - HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_l), - HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_p) + HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_p)), + HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_p)) ], # Genesis CAR.GENESIS_GV60_EV_1ST_GEN: [ - HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness=Harness.hyundai_a), - HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness=Harness.hyundai_k), + HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)), + HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_k)), + ], + CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_f)), + CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness_kit=HarnessKit(Harness.hyundai_f)), + CAR.GENESIS_GV70_1ST_GEN: [ + HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_l)), + HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_m)), ], - 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), - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h), - CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), + CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_h)), + CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness_kit=HarnessKit(Harness.hyundai_c)), + CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", harness_kit=HarnessKit(Harness.hyundai_m)), } class Buttons: @@ -573,6 +581,7 @@ class Buttons: b'\xf1\x82DNCVN5GMCCXXXG2B', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A', + b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M10', b'\xf1\x87391162M003', b'\xf1\x87391162M013', b'\xf1\x87391162M023', @@ -586,6 +595,7 @@ class Buttons: ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102', b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', @@ -623,6 +633,7 @@ class Buttons: b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:', b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB4\x00\x00\x00\x00\x00\x00g!l[', + b'\xf1\x00HT6WA280BLHT6WAE10A1SDN8G25NB5\x00\x00\x00\x00\x00\x00\xe0t\xa9\xba', b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE', b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSCF\xe4!Y', @@ -789,10 +800,10 @@ class Buttons: CAR.SANTA_FE_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', - b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', b'\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', ], (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00TM ESC \x01 102!\x04\x03 58910-S2DA0', b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', @@ -803,6 +814,8 @@ class Buttons: b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0', ], (Ecu.engine, 0x7e0, None): [ + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_L50', + b'\xf1\x81HM6M1_0a0_H00', b'\xf1\x82TACVN5GMI3XXXH0A', b'\xf1\x82TMBZN5TMD3XXXG2E', b'\xf1\x82TACVN5GSI3XXXH0A', @@ -823,6 +836,8 @@ class Buttons: b'\xf1\x00TMA MFC AT USA LHD 1.00 1.01 99211-S2500 210205', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00HT6WA280BLHT6WAD00A1STM2G25NH2\x00\x00\x00\x00\x00\x00\xf8\xc0\xc3\xaa', + b'\xf1\x00HT6WA280BLHT6WAD00A1STM4G25NH1\x00\x00\x00\x00\x00\x00\x9cl\x04\xbc', b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NSA\xf3\xf4Uj', b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', @@ -1177,21 +1192,27 @@ class Buttons: b'\xf1\x00BD MDPS C 1.00 1.02 56310-XX000 4BD2C102', b'\xf1\x00BD MDPS C 1.00 1.08 56310/M6300 4BDDC108', b'\xf1\x00BD MDPS C 1.00 1.08 56310M6300\x00 4BDDC108', + b'\xf1\x00BDm MDPS C A.01 1.03 56310M7800\x00 4BPMC103', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33', + b'\xf1\x00BDP LKAS AT USA LHD 1.00 1.05 99211-M6500 744', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00BD__ SCC H-CUP 1.00 1.02 99110-M6000 ', + b'\xf1\x00BDPE_SCC FHCUPC 1.00 1.04 99110-M6500\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x01TBDM1NU06F200H01', b'391182B945\x00', + b'\xf1\x81616F2051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x8758900-M7AB0 \xf1\x816VQRAD00127.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006V2B0_C2\x00\x006V2C6051\x00\x00CBD0N20NL1\x00\x00\x00\x00', b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\x00\x00\x00\x00', b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3", ], @@ -1374,6 +1395,7 @@ class Buttons: ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00SG2EMFC AT EUR LHD 1.01 1.09 99211-AT000 220801', + b'\xf1\x00SG2EMFC AT USA LHD 1.01 1.09 99211-AT000 220801', ], }, CAR.KIA_NIRO_PHEV: { @@ -1575,6 +1597,7 @@ class Buttons: b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.07 99210-AA000 220426', b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.08 99210-AA000 220728', + b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.09 99210-AA000 221108', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', @@ -1687,6 +1710,7 @@ class Buttons: b'\xf1\x00CV1 MFC AT EUR RHD 1.00 1.00 99210-CV100 220630', b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630', b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.04 99210-CV000 210823', + b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.05 99210-CV000 211027', b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.06 99210-CV000 220328', ], }, @@ -1721,9 +1745,11 @@ class Buttons: 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', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9250 14W', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', + b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', ], }, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: { @@ -1753,16 +1779,19 @@ class Buttons: (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', b'\xf1\x00NQ5__ 1.00 1.03 99110-P1000 ', + b'\xf1\x00NQ5__ 1.01 1.03 99110-P1000 ', ], }, CAR.GENESIS_GV70_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.04 99211-AR000 210204', b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR200 220125', + b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR300 220125', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ', b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR200 ', + b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR300 ', ], }, CAR.GENESIS_GV60_EV_1ST_GEN: { @@ -1794,6 +1823,14 @@ class Buttons: b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ', ], }, + CAR.GENESIS_GV80: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JX1 MFC AT USA LHD 1.00 1.02 99211-T6110 220513', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JX1_ SCC FHCUP 1.00 1.01 99110-T6100 ', + ], + }, } CHECKSUM = { @@ -1808,10 +1845,10 @@ class Buttons: "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020}, } -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, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_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, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, CAR.GENESIS_GV80} # The radar does SCC on these cars when HDA I, rather than the camera -CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN} +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } @@ -1889,4 +1926,5 @@ class Buttons: CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), + CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 8f993e26514a30..79f07a1fde157e 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,10 +1,9 @@ from dataclasses import dataclass -from enum import Enum from typing import Dict, List, Union from cereal import car from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -38,7 +37,7 @@ class CAR: @dataclass class MazdaCarInfo(CarInfo): package: str = "All" - harness: Enum = Harness.mazda + harness_kit: HarnessKit = HarnessKit(Harness.mazda) CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 45c3dd720c3060..5064e8c457e52d 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -26,13 +26,11 @@ def update(self, CC, CS, now_nanos): can_sends = [] ### STEER ### - lkas_hud_msg = CS.lkas_hud_msg - lkas_hud_info_msg = CS.lkas_hud_info_msg steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 if CC.latActive: # windup slower - apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CarControllerParams) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): @@ -63,16 +61,16 @@ def update(self, CC, CS, now_nanos): can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) + self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque)) - if lkas_hud_msg and lkas_hud_info_msg: + if self.CP.carFingerprint != CAR.ALTIMA: if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) if self.frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( - self.packer, lkas_hud_info_msg, steer_hud_alert + self.packer, CS.lkas_hud_info_msg, steer_hud_alert )) new_actuators = actuators.copy() diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index d6b6d17d55a93d..bbba92ddeb0f79 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -14,8 +14,8 @@ def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - self.lkas_hud_msg = None - self.lkas_hud_info_msg = None + self.lkas_hud_msg = {} + self.lkas_hud_info_msg = {} self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index d0f0aa93c9c1ef..cbe62645d53e4f 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,11 +1,10 @@ from dataclasses import dataclass from typing import Dict, List, Optional, Union -from enum import Enum from cereal import car from panda.python import uds from selfdrive.car import AngleRateLimit, dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit, HarnessPart from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -31,10 +30,13 @@ class CAR: ALTIMA = "NISSAN ALTIMA 2020" +NISSAN_HARNESS_PARTS = [HarnessPart.harness_box, HarnessPart.rj45_cable, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler] + + @dataclass class NissanCarInfo(CarInfo): package: str = "ProPILOT Assist" - harness: Enum = Harness.nissan_a + harness_kit: HarnessKit = HarnessKit(Harness.nissan_a, parts=NISSAN_HARNESS_PARTS) CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { @@ -42,7 +44,7 @@ class NissanCarInfo(CarInfo): CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"), CAR.LEAF_IC: None, # same platforms CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), - CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), + CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness_kit=HarnessKit(Harness.nissan_b, parts=NISSAN_HARNESS_PARTS)), } FINGERPRINTS = { diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 51f5c45f9096b2..15eecd19b15dcb 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -264,7 +264,7 @@ def get_cam_can_parser(CP): ("LKAS_State_Msg", "ES_DashStatus"), ("Signal2", "ES_DashStatus"), ("Cruise_Soft_Disable", "ES_DashStatus"), - ("EyeSight_Status_Msg", "ES_DashStatus"), + ("Cruise_Status_Msg", "ES_DashStatus"), ("Signal3", "ES_DashStatus"), ("Cruise_Distance", "ES_DashStatus"), ("Signal4", "ES_DashStatus"), diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index a8ab6f181d7cde..60c3c9dd22127e 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -20,7 +20,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): if candidate in PREGLOBAL_CARS: ret.enableBsm = 0x25c in fingerprint[0] - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)] else: ret.enableBsm = 0x228 in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index d5d108ae79da73..595c9835a3aeb5 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -3,6 +3,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert + def create_steering_control(packer, apply_steer): values = { "LKAS_Output": apply_steer, @@ -11,9 +12,11 @@ def create_steering_control(packer, apply_steer): } return packer.make_can_msg("ES_LKAS", 0, values) + def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) + def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): values = copy.copy(es_distance_msg) values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 @@ -21,8 +24,8 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): values["Cruise_Cancel"] = 1 return packer.make_can_msg("ES_Distance", bus, values) -def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): +def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): values = copy.copy(es_lkas_state_msg) # Filter the stock LKAS "Keep hands on wheel" alert @@ -52,18 +55,19 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ # Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW) if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0: if left_lane_depart: - values["LKAS_Alert"] = 12 # Left lane departure dash alert + values["LKAS_Alert"] = 12 # Left lane departure dash alert elif right_lane_depart: - values["LKAS_Alert"] = 11 # Right lane departure dash alert + values["LKAS_Alert"] = 11 # Right lane departure dash alert - values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines - values["LKAS_Dash_State"] = 2 if enabled else 0 # Green enabled indicator + values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines + values["LKAS_Dash_State"] = 2 if enabled else 0 # Green enabled indicator values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line) return packer.make_can_msg("ES_LKAS_State", 0, values) + def create_es_dashstatus(packer, dashstatus_msg): values = copy.copy(dashstatus_msg) @@ -73,6 +77,7 @@ def create_es_dashstatus(packer, dashstatus_msg): return packer.make_can_msg("ES_DashStatus", 0, values) + def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4): @@ -88,12 +93,14 @@ def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg) + # *** Subaru Pre-global *** def subaru_preglobal_checksum(packer, values, addr): dat = packer.make_can_msg(addr, 0, values)[2] return (sum(dat[:7])) % 256 + def create_preglobal_steering_control(packer, apply_steer, frame, steer_step): idx = (frame / steer_step) % 8 @@ -107,8 +114,8 @@ def create_preglobal_steering_control(packer, apply_steer, frame, steer_step): return packer.make_can_msg("ES_LKAS", 0, values) -def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): +def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): values = copy.copy(es_distance_msg) values["Cruise_Button"] = cruise_button diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index c191c4f6e19a50..f90f015bfb8529 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,11 +1,11 @@ from dataclasses import dataclass -from enum import Enum, IntFlag +from enum import IntFlag from typing import Dict, List, Union from cereal import car from panda.python import uds from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -53,13 +53,13 @@ class CAR: @dataclass class SubaruCarInfo(CarInfo): package: str = "EyeSight Driver Assistance" - harness: Enum = Harness.subaru_a + harness_kit: HarnessKit = HarnessKit(Harness.subaru_a) CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"), - CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness=Harness.subaru_b), - CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness=Harness.subaru_b), + CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)), + CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)), CAR.IMPREZA: [ SubaruCarInfo("Subaru Impreza 2017-19"), SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), @@ -247,7 +247,7 @@ class SubaruCarInfo(CarInfo): ], (Ecu.fwdCamera, 0x787, None): [ b'\000\000eb\037@ \"', - b'\000\000e\x8f\037@ )', + b'\x00\x00e\x8f\x1f@ )', b'\x00\x00eq\x1f@ "', b'\x00\x00eq\x00\x00\x00\x00', b'\x00\x00e\x8f\x00\x00\x00\x00', @@ -262,6 +262,7 @@ class SubaruCarInfo(CarInfo): b'\xca!fp\x07', b'\xf3"f@\x07', b'\xe6!fp\x07', + b'\xf3"fp\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe6\xf5\004\000\000', diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 3935461e612dfa..74834065fbc440 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -16,9 +16,7 @@ # TODO: add routes for these cars non_tested_cars = [ - FORD.ESCAPE_MK4, FORD.FOCUS_MK4, - FORD.MAVERICK_MK1, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU, @@ -47,7 +45,9 @@ CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD, segment=6), CarTestRoute("54827bf84c38b14f|2023-01-25--14-14-11", FORD.BRONCO_SPORT_MK1), + CarTestRoute("f8eaaccd2a90aef8|2023-05-04--15-10-09", FORD.ESCAPE_MK4), CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6), + CarTestRoute("bd37e43731e5964b|2023-04-30--10-42-26", FORD.MAVERICK_MK1), #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), @@ -144,6 +144,7 @@ CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.ELANTRA_HEV_2021), CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.SONATA_HYBRID), CarTestRoute("715ac05b594e9c59|2021-10-27--23-24-56", HYUNDAI.GENESIS_G70_2020), + CarTestRoute("6b0d44d22df18134|2023-05-06--10-36-55", HYUNDAI.GENESIS_GV80), CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.ALPHARD_TSS2), CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARDH_TSS2), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 8e487224823595..7198218d6a2fbe 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -6,9 +6,9 @@ from cereal import car from selfdrive.car import gen_empty_fingerprint -from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces -from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS +from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS, all_known_cars + class TestCarInterfaces(unittest.TestCase): @@ -51,7 +51,7 @@ def test_car_interfaces(self, car_name): elif tune.which() == 'torque': self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0) - self.assertTrue(not math.isnan(tune.torque.friction)) + self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0) elif tune.which() == 'indi': self.assertTrue(len(tune.indi.outerLoopGainV)) diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index e56f98f7a8ee39..e36bdc5dedffdb 100755 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -74,7 +74,8 @@ def test_harnesses(self): if car.name == "comma body": raise unittest.SkipTest - self.assertNotIn(car.harness, [None, Harness.none], f"Need to specify car harness: {car.name}") + self.assertNotIn(car.harness_kit.connector, [None, Harness.none], f"Need to specify car harness: {car.name}") + self.assertTrue(car.harness_kit.parts, f"Need to specify harness parts: {car.name} with {car.harness_kit.connector.value} connector") if __name__ == "__main__": diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 2aa049a89dfe39..afd4624a8173d6 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -27,8 +27,8 @@ FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan] COMMA BODY: [.nan, 1000, .nan] # Totally new cars -RAM 1500 5TH GEN: [2.0, 2.0, 0.0] -RAM HD 5TH GEN: [1.4, 1.4, 0.0] +RAM 1500 5TH GEN: [2.0, 2.0, 0.05] +RAM HD 5TH GEN: [1.4, 1.4, 0.05] SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11] CADILLAC ESCALADE 2017: [1.899999976158142, 1.842270016670227, 0.1120000034570694] CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] @@ -46,6 +46,7 @@ GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1] KIA NIRO HYBRID 2ND GEN: [2.42, 2.5, 0.12] KIA NIRO EV 2ND GEN: [2.05, 2.5, 0.14] +GENESIS GV80 2023: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index bc4816fdb004ee..88c0fd3cbc2baa 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -99,6 +99,13 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep # lane sway functionality # not all cars have LKAS_HUD — update with camera values if available - values.update(stock_lkas_hud) + if len(stock_lkas_hud): + values.update({s: stock_lkas_hud[s] for s in [ + "LANE_SWAY_FLD", + "LANE_SWAY_BUZZER", + "LANE_SWAY_WARNING", + "LANE_SWAY_SENSITIVITY", + "LANE_SWAY_TOGGLE", + ]}) return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2fe60e8fb6403c..00552485a88d03 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -102,7 +102,7 @@ class Footnote(Enum): @dataclass class ToyotaCarInfo(CarInfo): package: str = "All" - harness: Enum = Harness.toyota + harness_kit: HarnessKit = HarnessKit(Harness.toyota) CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { @@ -1076,6 +1076,7 @@ class ToyotaCarInfo(CarInfo): b'\x01896630E62200\x00\x00\x00\x00', b'\x01896630E64100\x00\x00\x00\x00', b'\x01896630E64200\x00\x00\x00\x00', + b'\x01896630E64400\x00\x00\x00\x00', b'\x01896630EB1000\x00\x00\x00\x00', b'\x01896630EB1100\x00\x00\x00\x00', b'\x01896630EB1200\x00\x00\x00\x00', @@ -2017,6 +2018,7 @@ class ToyotaCarInfo(CarInfo): b'\x01896630ED0000\x00\x00\x00\x00', b'\x01896630ED0100\x00\x00\x00\x00', b'\x01896630ED6000\x00\x00\x00\x00', + b'\x018966348T8000\x00\x00\x00\x00', b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', b'\x01896634D12000\x00\x00\x00\x00', @@ -2024,6 +2026,7 @@ class ToyotaCarInfo(CarInfo): b'\x01896634D43000\x00\x00\x00\x00', b'\x01896634D44000\x00\x00\x00\x00', b'\x018966348X0000\x00\x00\x00\x00', + b'\x01896630ED5000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F15260E031\x00\x00\x00\x00\x00\x00', @@ -2049,11 +2052,12 @@ class ToyotaCarInfo(CarInfo): }, CAR.LEXUS_RXH_TSS2: { (Ecu.engine, 0x7e0, None): [ + b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348X5000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', - b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152648831\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index dd2d42797d4bc6..db28a62439e557 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -7,7 +7,7 @@ from panda.python import uds from opendbc.can.can_define import CANDefine from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -166,7 +166,7 @@ class Footnote(Enum): @dataclass class VWCarInfo(CarInfo): package: str = "Adaptive Cruise Control (ACC) & Lane Assist" - harness: Enum = Harness.j533 + harness_kit: HarnessKit = HarnessKit(Harness.j533, parts=[HarnessPart.harness_box, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler]) def init_make(self, CP: car.CarParams): self.footnotes.insert(0, Footnote.VW_EXP_LONG) @@ -394,6 +394,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704E906016A \xf1\x897697', b'\xf1\x8704E906016AD\xf1\x895758', b'\xf1\x8704E906016CE\xf1\x899096', + b'\xf1\x8704E906016CH\xf1\x899226', b'\xf1\x8704E906023AG\xf1\x891726', b'\xf1\x8704E906023BN\xf1\x894518', b'\xf1\x8704E906024K \xf1\x896811', @@ -442,6 +443,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8709G927749AP\xf1\x892943', b'\xf1\x8709S927158A \xf1\x893585', b'\xf1\x870CW300040H \xf1\x890606', + b'\xf1\x870CW300041D \xf1\x891004', b'\xf1\x870CW300041H \xf1\x891010', b'\xf1\x870CW300042F \xf1\x891604', b'\xf1\x870CW300043B \xf1\x891601', @@ -450,6 +452,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300045 \xf1\x894531', b'\xf1\x870CW300047D \xf1\x895261', + b'\xf1\x870CW300047E \xf1\x895261', b'\xf1\x870CW300048J \xf1\x890611', b'\xf1\x870CW300049H \xf1\x890905', b'\xf1\x870D9300012 \xf1\x894904', @@ -482,6 +485,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114317121C111C9113', b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114417121411149113', b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120053114317121C111C9113', + b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\x13141500111233003142114A2131219333313100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100', @@ -615,6 +619,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906264 \xf1\x890004', ], (Ecu.transmission, 0x7e1, None): [ @@ -625,7 +630,10 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870D9300041A \xf1\x894801', b'\xf1\x870DD300045T \xf1\x891601', b'\xf1\x870DL300011H \xf1\x895201', + b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870GC300042H \xf1\x891404', + b'\xf1\x870D9300018C \xf1\x895297', + b'\xf1\x870GC300043 \xf1\x892301', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', @@ -633,7 +641,10 @@ def init_make(self, CP: car.CarParams): b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111', b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', + b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e5915005914001305701311052900', + b'\xf1\x873Q0959655BG\xf1\x890712\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900', + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e5915005914001354701311542900', b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', ], @@ -646,6 +657,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1', + b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563B0000600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020600', ], (Ecu.fwdRadar, 0x757, None): [ @@ -680,19 +692,24 @@ def init_make(self, CP: car.CarParams): CAR.POLO_MK6: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704C906025H \xf1\x895177', + b'\xf1\x8705C906032J \xf1\x891702', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300042D \xf1\x891612', b'\xf1\x870CW300050D \xf1\x891908', + b'\xf1\x870CW300051G \xf1\x891909', ], (Ecu.srs, 0x715, None): [ b'\xf1\x872Q0959655AG\xf1\x890248\xf1\x82\x1218130411110411--04040404231811152H14', b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1248130411110416--04040404784811152H14', + b'\xf1\x872Q0959655AS\xf1\x890411\xf1\x82\x1384830511110516041405820599841215391471', ], (Ecu.eps, 0x712, None): [ b'\xf1\x872Q1909144M \xf1\x896041', + b'\xf1\x872Q2909144AB\xf1\x896050', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572R \xf1\x890372', ], }, @@ -754,6 +771,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704L906027G \xf1\x899893', b'\xf1\x875N0906259 \xf1\x890002', b'\xf1\x875NA906259H \xf1\x890002', + b'\xf1\x875NA907115E \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115B \xf1\x890005', b'\xf1\x8783A907115F \xf1\x890002', @@ -770,6 +788,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', + b'\xf1\x870DL300012M \xf1\x892107', b'\xf1\x870DL300012P \xf1\x892103', b'\xf1\x870DL300013A \xf1\x893005', b'\xf1\x870DL300013G \xf1\x892119', @@ -780,6 +799,7 @@ def init_make(self, CP: car.CarParams): (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100', b'\xf1\x875Q0959655BJ\xf1\x890336\xf1\x82\x1312110031333300314232583732379333423100', + b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1331310031333334313132013730379333423100', b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', @@ -791,6 +811,7 @@ def init_make(self, CP: car.CarParams): ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527A6050705', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6000600', b'\xf1\x875QF909144A \xf1\x895581\xf1\x82\x0571A60834A1', @@ -814,18 +835,23 @@ def init_make(self, CP: car.CarParams): CAR.TOURAN_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906026HM\xf1\x893017', + b'\xf1\x8705E906018CQ\xf1\x890808', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300041E \xf1\x891005', + b'\xf1\x870CW300051M \xf1\x891926', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\023363500213533353141324C4732479333313100', + b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1336350021353336314740025250529333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A8090400', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x872Q0907572AA\xf1\x890396', ], }, CAR.TRANSPORTER_T61: { @@ -1219,6 +1245,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704L906026MT\xf1\x893076', b'\xf1\x873G0906259 \xf1\x890004', b'\xf1\x873G0906259B \xf1\x890002', + b'\xf1\x873G0906259L \xf1\x890003', b'\xf1\x873G0906264A \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ @@ -1227,6 +1254,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870D9300012 \xf1\x894940', b'\xf1\x870D9300013A \xf1\x894905', b'\xf1\x870D9300041H \xf1\x894905', + b'\xf1\x870GC300014M \xf1\x892801', b'\xf1\x870GC300043 \xf1\x892301', b'\xf1\x870D9300043F \xf1\x895202', ], @@ -1236,6 +1264,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100', b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1331310031313100313151013141319331423100', + b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152025350539331463100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203', @@ -1243,12 +1272,14 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700', b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070600', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070700', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572B \xf1\x890194', b'\xf1\x873Q0907572C \xf1\x890195', b'\xf1\x875Q0907572R \xf1\x890771', + b'\xf1\x875Q0907572S \xf1\x890780', ], }, } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b043388aeb01fd..6558eb7f5ca604 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -26,9 +26,7 @@ from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.locationd.calibrationd import Calibration from system.hardware import HARDWARE -from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -37,9 +35,7 @@ REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ NOSENSOR = "NOSENSOR" in os.environ -IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", - "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"} | \ - {k for k, v in managed_processes.items() if not v.enabled} +IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState @@ -289,9 +285,12 @@ def update_events(self, CS): # Handle calibration status cal_status = self.sm['liveCalibration'].calStatus - if cal_status != Calibration.CALIBRATED: - if cal_status == Calibration.UNCALIBRATED: + if cal_status != log.LiveCalibrationData.Status.calibrated: + if cal_status == log.LiveCalibrationData.Status.uncalibrated: self.events.add(EventName.calibrationIncomplete) + elif cal_status == log.LiveCalibrationData.Status.recalibrating: + set_offroad_alert("Offroad_Recalibration", True) + self.events.add(EventName.calibrationRecalibrating) else: self.events.add(EventName.calibrationInvalid) @@ -343,9 +342,9 @@ def update_events(self, CS): self.events.add(EventName.cameraMalfunction) elif not self.sm.all_freq_ok(self.camera_packets): self.events.add(EventName.cameraFrameRate) - if self.rk.lagging: + if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) - if len(self.sm['radarState'].radarErrors) or not self.sm.all_checks(['radarState']): + if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) @@ -418,7 +417,7 @@ def update_events(self, CS): # TODO: fix simulator if not SIMULATION: if not NOSENSOR: - if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000): + if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000): # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) @@ -707,15 +706,15 @@ def publish_logs(self, CS, start_time, CC, lac_log): recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ - and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + and not CC.latActive and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: right_lane_visible = model_v2.laneLineProbs[2] > 0.5 left_lane_visible = model_v2.laneLineProbs[1] > 0.5 - l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1] - r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1] + l_lane_change_prob = desire_prediction[Desire.laneChangeLeft] + r_lane_change_prob = desire_prediction[Desire.laneChangeRight] lane_lines = model_v2.laneLines l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET)) diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 2f85ea917ae6b4..42d2512e518e0c 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -48,5 +48,9 @@ "Offroad_NoFirmware": { "text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.", "severity": 0 + }, + "Offroad_Recalibration": { + "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.", + "severity": 0 } } diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b7f7244f8ab98d..568186d8a87b20 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,6 +1,6 @@ import math -from cereal import car +from cereal import car, log from common.conversions import Conversions as CV from common.numpy_fast import clip, interp from common.realtime import DT_MDL @@ -23,6 +23,8 @@ # EU guidelines MAX_LATERAL_JERK = 5.0 +MAX_VEL_ERR = 5.0 + ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type CRUISE_LONG_PRESS = 50 @@ -211,3 +213,11 @@ def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, fric ) friction = float(friction_interp) if friction_compensation else 0.0 return friction + + +def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: + # ToDo: Try relative error, and absolute speed + if len(modelV2.temporalPose.trans): + vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) + return float(vel_err) + return 0.0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d1a75146e2d02d..6b043d7fdfca57 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -242,8 +242,9 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration' return Alert( - "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, + f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%", f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) @@ -368,6 +369,7 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. # Car is recognized, but marked as dashcam only EventName.startupNoControl: { ET.PERMANENT: StartupAlert("Dashcam mode"), + ET.NO_ENTRY: NoEntryAlert("Dashcam mode"), }, # Car is not recognized @@ -761,9 +763,15 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. EventName.calibrationIncomplete: { ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Device remount detected: recalibrating"), + ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), }, + + EventName.calibrationRecalibrating: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), + ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), + }, EventName.doorOpen: { ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), @@ -987,4 +995,10 @@ def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), }, + EventName.vehicleSensorsInvalid: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), + ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), + }, + } diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 717f06de529883..ca0711bf39564d 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -4,7 +4,7 @@ from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N -from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED +from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log @@ -39,7 +39,11 @@ def __init__(self, CP): self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) - self.y_pts = np.zeros(TRAJECTORY_SIZE) + self.y_pts = np.zeros((TRAJECTORY_SIZE,)) + self.v_plan = np.zeros((TRAJECTORY_SIZE,)) + self.v_ego = 0.0 + self.l_lane_change_prob = 0.0 + self.r_lane_change_prob = 0.0 self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.lat_mpc = LateralMpc() @@ -52,6 +56,7 @@ def reset_mpc(self, x0=np.zeros(4)): def update(self, sm): # clip speed , lateral planning is not possible at 0 speed measured_curvature = sm['controlsState'].curvature + v_ego_car = sm['carState'].vEgo # Parse model predictions md = sm['modelV2'] @@ -61,7 +66,7 @@ def update(self, sm): self.plan_yaw = np.array(md.orientation.z) self.plan_yaw_rate = np.array(md.orientationRate.z) self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) - car_speed = np.linalg.norm(self.velocity_xyz, axis=1) + car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) self.v_ego = self.v_plan[0] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e4fc95279216b1..0f1db851098565 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error from selfdrive.controls.lib.vision_turn_controller import VisionTurnController from selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver from selfdrive.controls.lib.turn_speed_controller import TurnSpeedController @@ -116,8 +116,7 @@ def update(self, sm): # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error - if len(sm['modelV2'].temporalPose.trans): - self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego + self.v_model_error = get_speed_error(sm['modelV2'], v_ego) if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py index 93dd5bdc470f25..42b671e5e33dd2 100755 --- a/selfdrive/debug/count_events.py +++ b/selfdrive/debug/count_events.py @@ -5,7 +5,7 @@ from collections import Counter from pprint import pprint from tqdm import tqdm -from typing import cast +from typing import List, Tuple, cast from cereal.services import service_list from tools.lib.route import Route @@ -20,6 +20,7 @@ cams = [s for s in service_list if s.endswith('CameraState')] cnt_cameras = dict.fromkeys(cams, 0) + alerts: List[Tuple[float, str]] = [] start_time = math.inf end_time = -math.inf for q in tqdm(r.qlog_paths()): @@ -27,18 +28,22 @@ continue lr = list(LogReader(q)) for msg in lr: + end_time = max(end_time, msg.logMonoTime) + start_time = min(start_time, msg.logMonoTime) + if msg.which() == 'carEvents': for e in msg.carEvents: cnt_events[e.name] += 1 + elif msg.which() == 'controlsState': + if len(alerts) == 0 or alerts[-1][1] != msg.controlsState.alertType: + t = (msg.logMonoTime - start_time) / 1e9 + alerts.append((t, msg.controlsState.alertType)) elif msg.which() in cams: cnt_cameras[msg.which()] += 1 if not msg.valid: cnt_valid[msg.which()] += 1 - end_time = max(end_time, msg.logMonoTime) - start_time = min(start_time, msg.logMonoTime) - duration = (end_time - start_time) / 1e9 print("Events") @@ -55,5 +60,10 @@ expected_frames = int(s.frequency * duration / cast(float, s.decimation)) print(" ", k.ljust(20), f"{v}, {v/expected_frames:.1%} of expected") + print("\n") + print("Alerts") + for t, a in alerts: + print(f"{t:8.2f} {a}") + print("\n") print("Route duration", datetime.timedelta(seconds=duration)) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 8952405b8eb500..6b5ec36935f07d 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -49,7 +49,7 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): sw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER).decode("utf-8") sw_ver = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER).decode("utf-8") component = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.SYSTEM_NAME_OR_ENGINE_TYPE).decode("utf-8") - odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8") + odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8").rstrip('\x00') current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) # type: ignore coding_text = current_coding.hex() @@ -70,14 +70,14 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_length = len(current_coding) - # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS) - if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"): - coding_variant = "ZF" + # EPS_MQB_ZFLS + if odx_file in ("EV_SteerAssisMQB", "EV_SteerAssisMNB"): + coding_variant = "ZFLS" coding_byte = 0 coding_bit = 4 - # APA racks (MQB_PP_APA) have a different coding layout - elif odx_file == "EV_SteerAssisVWBSMQBA\x00\x00\x00\x00": + # MQB_PP_APA, MQB_VWBS_GEN2 + elif odx_file in ("EV_SteerAssisVWBSMQBA", "EV_SteerAssisVWBSMQBGen2"): coding_variant = "APA" coding_byte = 3 coding_bit = 0 @@ -111,8 +111,8 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant in ("ZF", "APA")) - # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the + assert(coding_variant in ("ZFLS", "APA")) + # ZFLS EPS config coding length can be anywhere from 1 to 4 bytes, but the # bit we care about is always in the same place in the first byte if args.action == "enable": new_byte = current_coding_array[coding_byte] | (1 << coding_bit) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 091f21117517d1..dc4ffbb1f72446 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -39,12 +39,6 @@ DEBUG = os.getenv("DEBUG") is not None -class Calibration: - UNCALIBRATED = 0 - CALIBRATED = 1 - INVALID = 2 - - def is_calibration_valid(rpy: np.ndarray) -> bool: return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore @@ -69,6 +63,7 @@ def __init__(self, param_put: bool = False): rpy_init = RPY_INIT wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 + self.cal_status = log.LiveCalibrationData.Status.uncalibrated if param_put and calibration_params: try: @@ -134,16 +129,20 @@ def update_status(self) -> None: self.calib_spread = np.zeros(3) if self.valid_blocks < INPUTS_NEEDED: - self.cal_status = Calibration.UNCALIBRATED + if self.cal_status == log.LiveCalibrationData.Status.recalibrating: + self.cal_status = log.LiveCalibrationData.Status.recalibrating + else: + self.cal_status = log.LiveCalibrationData.Status.uncalibrated elif is_calibration_valid(self.rpy): - self.cal_status = Calibration.CALIBRATED + self.cal_status = log.LiveCalibrationData.Status.calibrated else: - self.cal_status = Calibration.INVALID + self.cal_status = log.LiveCalibrationData.Status.invalid # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. - if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: + if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated: self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) + self.cal_status = log.LiveCalibrationData.Status.recalibrating write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: @@ -210,7 +209,7 @@ def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder: if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED - liveCalibration.calStatus = Calibration.CALIBRATED + liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated liveCalibration.calPerc = 100. liveCalibration.rpyCalib = [0, 0, 0] liveCalibration.rpyCalibSpread = self.calib_spread.tolist() diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 8b07c46aaa1494..71b81cc307c5fa 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -5,7 +5,6 @@ import shutil from collections import defaultdict from concurrent.futures import Future, ProcessPoolExecutor -from datetime import datetime from enum import IntEnum from typing import List, Optional, Dict, Any @@ -88,7 +87,6 @@ def __init__(self, valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), a self.auto_fetch_navs = auto_fetch_navs self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None - self.got_first_gnss_msg = False self.last_report_time = GPSTime(0, 0) self.last_fetch_navs_t = GPSTime(0, 0) @@ -178,17 +176,32 @@ def get_lsq_fix(self, t, measurements): return position_estimate, position_std, velocity_estimate, velocity_std + def gps_time_from_qcom_report(self, gnss_msg): + report = gnss_msg.drMeasurementReport + if report.source == log.QcomGnss.MeasurementSource.gps: + report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) + elif report.source == log.QcomGnss.MeasurementSource.sbas: + report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) + elif report.source == log.QcomGnss.MeasurementSource.glonass: + report_time = GPSTime.from_glonass(report.glonassYear, + report.glonassDay, + report.glonassMilliseconds / 1000.0) + else: + raise NotImplementedError(f'Unknownconstellation {report.source}') + return report_time + def is_good_report(self, gnss_msg): if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) # TODO: Understand and use remaining unknown constellations try: - good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] + good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] except NotImplementedError: good_constellation = False - # gpsWeek 65535 is received rarely from quectel, this cannot be - # passed to GnssMeasurements's gpsWeek (Int16) - good_week = not getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max + # Garbage timestamps with week > 32767 are sometimes sent by module. + # This is an issue with gpsTime and GLONASS time. + report_time = self.gps_time_from_qcom_report(gnss_msg) + good_week = report_time.week < np.iinfo(np.int16).max return good_constellation and good_week elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: return True @@ -197,17 +210,26 @@ def is_good_report(self, gnss_msg): def read_report(self, gnss_msg): if self.use_qcom: + # QCOM reports are per constellation, should always send 3 reports report = gnss_msg.drMeasurementReport - week = report.gpsWeek - tow = report.gpsMilliseconds / 1000.0 - new_meas = read_raw_qcom(report) + report_time = self.gps_time_from_qcom_report(gnss_msg) + + if report_time - self.last_report_time > 0: + self.qcom_reports = [report] + else: + self.qcom_reports.append(report) + self.last_report_time = report_time + + new_meas = [] + if len(self.qcom_reports) == 3: + for report in self.qcom_reports: + new_meas.extend(read_raw_qcom(report)) + else: report = gnss_msg.measurementReport - week = report.gpsWeek - tow = report.rcvTow + self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) new_meas = read_raw_ublox(report) - self.last_report_time = GPSTime(week, tow) - return week, tow, new_meas + return self.last_report_time, new_meas def is_ephemeris(self, gnss_msg): if self.use_qcom: @@ -281,13 +303,11 @@ def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_ephemeris(gnss_msg): self.read_ephemeris(gnss_msg) elif self.is_good_report(gnss_msg): - week, tow, new_meas = self.read_report(gnss_msg) - self.gps_week = week - if week > 0: - self.got_first_gnss_msg = True - latest_msg_t = GPSTime(week, tow) + report_t, new_meas = self.read_report(gnss_msg) + self.gps_week = report_t.week + if report_t.week > 0: if self.auto_fetch_navs: - self.fetch_navs(latest_msg_t, block) + self.fetch_navs(report_t, block) corrected_measurements = self.process_report(new_meas, t) msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements] @@ -432,10 +452,7 @@ def main(sm=None, pm=None): raw_name = "qcomGnss" else: raw_name = "ubloxGnss" - raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False, timeout=1000) - - if sm is None: - sm = messaging.SubMaster(['clocks',]) + raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False) if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) @@ -443,23 +460,16 @@ def main(sm=None, pm=None): use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ replay = "REPLAY" in os.environ - if replay or "CI" in os.environ: + if replay: use_internet = True laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: - for in_msg in messaging.drain_sock(raw_gnss_sock): + for in_msg in messaging.drain_sock(raw_gnss_sock, wait_for_one=True): out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay) pm.send('gnssMeasurements', out_msg) - sm.update(0) - if not laikad.got_first_gnss_msg and sm.updated['clocks']: - clocks_msg = sm['clocks'] - t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9)) - if laikad.auto_fetch_navs: - laikad.fetch_navs(t, block=replay) - if __name__ == "__main__": main() diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 6c0307bd64b55f..9ee9e33fb44ebc 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -507,7 +507,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra this->calib = live_calib; this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); - this->calibrated = log.getCalStatus() == 1; + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; this->observation_values_invalid["liveCalibration"] *= DECAY; } } diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 2c6508fd42f0c7..18125cc43f2fb1 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -5,6 +5,7 @@ import numpy as np import cereal.messaging as messaging +from cereal import log from common.params import Params from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES @@ -96,6 +97,7 @@ def test_calibration_auto_reset(self): [0.0, 0.0, 0.0], [1e-3, 1e-3, 1e-3]) self.assertEqual(c.valid_blocks, 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2) if __name__ == "__main__": diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py index 983c7cc0b16c56..f8607fffc66b33 100644 --- a/selfdrive/manager/helpers.py +++ b/selfdrive/manager/helpers.py @@ -36,3 +36,8 @@ def unblock_stdout() -> None: # whose low byte is the signal number and whose high byte is the exit status exit_status = os.wait()[1] >> 8 os._exit(exit_status) + + +def write_onroad_params(started, params): + params.put_bool("IsOnroad", started) + params.put_bool("IsOffroad", not started) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index d5b7cde64f885e..f1d87d2a0197a5 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -14,7 +14,7 @@ from common.text_window import TextWindow from selfdrive.boardd.set_time import set_time from system.hardware import HARDWARE, PC -from selfdrive.manager.helpers import unblock_stdout +from selfdrive.manager.helpers import unblock_stdout, write_onroad_params from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID @@ -199,12 +199,27 @@ def manager_thread() -> None: sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) pm = messaging.PubMaster(['managerState']) + write_onroad_params(False, params) ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) + started_prev = False + while True: sm.update() started = sm['deviceState'].started + + if started and not started_prev: + params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) + elif not started and started_prev: + params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + + # update onroad params, which drives boardd's safety setter thread + if started != started_prev: + write_onroad_params(started, params) + + started_prev = started + ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) diff --git a/selfdrive/modeld/models/README.md b/selfdrive/modeld/models/README.md index 1bfca8e95a6a54..80d6dc0e02a453 100644 --- a/selfdrive/modeld/models/README.md +++ b/selfdrive/modeld/models/README.md @@ -32,28 +32,30 @@ Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d0 * .dlc file is a pre-quantized model and only runs on qualcomm DSPs ### input format -* single image (640 * 320 * 3 in RGB): - * full input size is 6 * 640/2 * 320/2 = 307200 - * represented in YUV420 with 6 channels: - * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2] - * Channel 4 represents the half-res U channel - * Channel 5 represents the half-res V channel - * normalized, ranging from -1.0 to 1.0 +* single image W = 1440 H = 960 represented in planar YUV420 format: + * full input size is 1440 * 960 = 1382400 + * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner) ### output format -* 39 x float32 outputs ([parsing example](https://github.com/commaai/openpilot/blob/master/selfdrive/modeld/models/dmonitoring.cc#L165)) - * face pose: 12 = 6 + 6 - * face orientation [pitch, yaw, roll] in camera frame: 3 - * face position [dx, dy] relative to image center: 2 - * normalized face size: 1 - * standard deviations for above outputs: 6 - * face visible probability: 1 - * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1 - * eye position and size, and their standard deviations: 8 - * eye visible probability: 1 - * eye closed probability: 1 - * wearing sunglasses probability: 1 - * poor camera vision probability: 1 - * face partially out-of-frame probability: 1 - * (deprecated) distracted probabilities: 2 - * face covered probability: 1 +* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33)) + * for each person in the front seats (2 * 41) + * face pose: 12 = 6 + 6 + * face orientation [pitch, yaw, roll] in camera frame: 3 + * face position [dx, dy] relative to image center: 2 + * normalized face size: 1 + * standard deviations for above outputs: 6 + * face visible probability: 1 + * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1 + * eye position and size, and their standard deviations: 8 + * eye visible probability: 1 + * eye closed probability: 1 + * wearing sunglasses probability: 1 + * face occluded probability: 1 + * touching wheel probability: 1 + * paying attention probability: 1 + * (deprecated) distracted probabilities: 2 + * using phone probability: 1 + * distracted probability: 1 + * common outputs 2 + * poor camera vision probability: 1 + * left hand drive probability: 1 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 13759c62f8a078..d8a8b5ef13acfc 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:5121deb0d5c683b0fbee4c1cad7bc625953bf127b1383fb7599a6b644efd0aea +oid sha256:0007958c9bad4a87eb5c28080b5bfdc24834958fbcbaff448674fa570b0196da size 46011200 diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 9e9cf3a6e9202a..e4ef6e308d5631 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -3,10 +3,10 @@ import cereal.messaging as messaging from cereal import car +from cereal import log from common.params import Params, put_bool_nonblocking from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events -from selfdrive.locationd.calibrationd import Calibration from selfdrive.monitoring.driver_monitor import DriverStatus from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus @@ -24,7 +24,7 @@ def dmonitoringd_thread(sm=None, pm=None): driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) hands_on_wheel_status = HandsOnWheelStatus() - sm['liveCalibration'].calStatus = Calibration.INVALID + sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].buttonEvents = [] sm['carState'].standstill = True diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc index b6eec10328bc10..8cef07edcf8dc6 100644 --- a/selfdrive/navd/main.cc +++ b/selfdrive/navd/main.cc @@ -7,22 +7,12 @@ #include "selfdrive/navd/map_renderer.h" #include "system/hardware/hw.h" - - -void sigHandler(int s) { - qInfo() << "Shutting down"; - std::signal(s, SIG_DFL); - - qApp->quit(); -} - - int main(int argc, char *argv[]) { qInstallMessageHandler(swagLogMessageHandler); QApplication app(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); MapRenderer * m = new MapRenderer(get_mapbox_settings()); assert(m); diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 51676bb3a37c51..203470bb4293d1 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -11,7 +11,7 @@ #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 HEIGHT = 512, WIDTH = 512; +const int HEIGHT = 256, WIDTH = 256; const int NUM_VIPC_BUFFERS = 4; const int EARTH_CIRCUMFERENCE_METERS = 40075000; @@ -177,12 +177,10 @@ void MapRenderer::publish(const double render_time) { uint8_t* dst = (uint8_t*)buf->addr; uint8_t* src = cap.bits(); - // RGB to greyscale and crop + // RGB to greyscale memset(dst, 128, buf->len); - 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]; - } + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; } vipc_server->send(buf, &extra); diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py index 3239470b23ece5..aa5682169f34b5 100755 --- a/selfdrive/navd/map_renderer.py +++ b/selfdrive/navd/map_renderer.py @@ -10,12 +10,12 @@ from common.ffi_wrapper import suffix from common.basedir import BASEDIR -HEIGHT = WIDTH = SIZE = 512 +HEIGHT = WIDTH = SIZE = 256 METERS_PER_PIXEL = 2 def get_ffi(): - lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmap_renderer" + suffix()) + lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmaprender" + suffix()) ffi = FFI() ffi.cdef(""" diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 8cc996c28dbf35..281c902933d933 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -2,8 +2,9 @@ import time from functools import wraps -from system.hardware import PC +import cereal.messaging as messaging from selfdrive.manager.process_config import managed_processes +from system.hardware import PC from system.version import training_version, terms_version @@ -15,6 +16,11 @@ def set_params_enabled(): params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) + # valid calib + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = 20 + msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + params.put("CalibrationParams", msg.to_bytes()) def phone_only(f): @wraps(f) diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index c96e624421ba9f..821429a70f2dd3 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -70,7 +70,10 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non field_tolerances = {} default_tolerance = EPSILON if tolerance is None else tolerance - log1, log2 = (list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)) + log1, log2 = ( + sorted((m for m in log if m.which() not in ignore_msgs), key=lambda m: (m.logMonoTime, m.which())) + for log in (log1, log2) + ) if len(log1) != len(log2): cnt1 = Counter(m.which() for m in log1) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index bd026f5710b879..b3cc50e559a03b 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -82db08d52b155336e9a1dadd11485d5acdf2eba0 +9d3cd2e7d5fceaaf0e8a4bd798a24fcf470da7c2 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 3072ecd0c98ee5..4ab2d3031561d9 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -372,7 +372,6 @@ def ublox_rcv_callback(msg, CP, cfg, fsm): pub_sub={ "ubloxGnss": ["gnssMeasurements"], "qcomGnss": ["gnssMeasurements"], - "clocks": [] }, ignore=["logMonoTime"], init_callback=get_car_params, @@ -421,11 +420,7 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None, lr=None): if lr is not None: services = {m.which() for m in lr} params.put_bool("UbloxAvailable", "ubloxGnss" in services) - - if lr is not None: - services = {m.which() for m in lr} - params.put_bool("UbloxAvailable", "ubloxGnss" in services) - + if cfg is not None: # Clear all custom processConfig environment variables for config in CONFIGS: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2e1c439698c29f..4dee64009f6a2f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -627aa0f54e377d1f3954c58e37c0a15b555e20b3 +658e4dd36d92ae7a973a090aaf3fab62fdf701b6 diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 8de4eb43fc11b6..dea9a737e8ebde 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -19,7 +19,7 @@ from selfdrive.car.toyota.values import EPS_SCALE from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes -from selfdrive.test.process_replay.process_replay import FAKEDATA, setup_env, check_enabled +from selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, setup_env, check_enabled from selfdrive.test.update_ci_routes import upload_route from tools.lib.route import Route from tools.lib.framereader import FrameReader @@ -233,7 +233,11 @@ def migrate_sensorEvents(lr, old_logtime=False): return all_msgs -def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): + +def regen_segment(lr, frs=None, daemons="all", outdir=FAKEDATA, disable_tqdm=False): + if not isinstance(daemons, str) and not hasattr(daemons, "__iter__"): + raise ValueError("whitelist_proc must be a string or iterable") + lr = migrate_carparams(list(lr)) lr = migrate_sensorEvents(list(lr)) if frs is None: @@ -262,33 +266,68 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), ], - 'managerState': [ + 'manager': [ multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), ], 'thermald': [ multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), ], + 'rawgpsd': [ + multiprocessing.Process(target=replay_service, args=('qcomGnss', lr)), + multiprocessing.Process(target=replay_service, args=('gpsLocation', lr)), + ], 'camerad': [ *cam_procs, ], } + # TODO add configs for modeld, dmonitoringmodeld + fakeable_daemons = {} + for config in CONFIGS: + replayable_messages = set([msg for sub in config.pub_sub.values() for msg in sub]) + processes = [ + multiprocessing.Process(target=replay_service, args=(msg, lr)) + for msg in replayable_messages + ] + fakeable_daemons[config.proc_name] = processes + + additional_fake_daemons = {} + if daemons != "all": + additional_fake_daemons = fakeable_daemons + if isinstance(daemons, str): + raise ValueError(f"Invalid value for daemons: {daemons}") + + for d in daemons: + if d in fake_daemons: + raise ValueError(f"Running daemon {d} is not supported!") + + if d in fakeable_daemons: + del additional_fake_daemons[d] + + all_fake_daemons = {**fake_daemons, **additional_fake_daemons} try: # TODO: make first run of onnxruntime CUDA provider fast - managed_processes["modeld"].start() - managed_processes["dmonitoringmodeld"].start() + if "modeld" not in all_fake_daemons: + managed_processes["modeld"].start() + if "dmonitoringmodeld" not in all_fake_daemons: + managed_processes["dmonitoringmodeld"].start() time.sleep(5) # start procs up - ignore = list(fake_daemons.keys()) + ['ui', 'manage_athenad', 'uploader', 'soundd'] + ignore = list(all_fake_daemons.keys()) \ + + ['ui', 'manage_athenad', 'uploader', 'soundd', 'micd', 'navd'] + + print("Faked daemons:", ", ".join(all_fake_daemons.keys())) + print("Running daemons:", ", ".join([key for key in managed_processes.keys() if key not in ignore])) + ensure_running(managed_processes.values(), started=True, params=Params(), CP=car.CarParams(), not_run=ignore) - for procs in fake_daemons.values(): + for procs in all_fake_daemons.values(): for p in procs: p.start() for _ in tqdm(range(60), disable=disable_tqdm): # ensure all procs are running - for d, procs in fake_daemons.items(): + for d, procs in all_fake_daemons.items(): for p in procs: if not p.is_alive(): raise Exception(f"{d}'s {p.name} died") @@ -297,7 +336,7 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): # kill everything for p in managed_processes.values(): p.stop() - for procs in fake_daemons.values(): + for procs in all_fake_daemons.values(): for p in procs: p.terminate() @@ -312,7 +351,7 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): return seg_path -def regen_and_save(route, sidx, upload=False, use_route_meta=False, outdir=FAKEDATA, disable_tqdm=False): +def regen_and_save(route, sidx, daemons="all", upload=False, use_route_meta=False, outdir=FAKEDATA, disable_tqdm=False): if use_route_meta: r = Route(route) lr = LogReader(r.log_paths()[sidx]) @@ -333,7 +372,7 @@ def regen_and_save(route, sidx, upload=False, use_route_meta=False, outdir=FAKED frs = {'roadCameraState': fr} if wfr is not None: frs['wideRoadCameraState'] = wfr - rpath = regen_segment(lr, frs, outdir=outdir, disable_tqdm=disable_tqdm) + rpath = regen_segment(lr, frs, daemons, outdir=outdir, disable_tqdm=disable_tqdm) # compress raw rlog before uploading with open(os.path.join(rpath, "rlog"), "rb") as f: @@ -356,10 +395,18 @@ def regen_and_save(route, sidx, upload=False, use_route_meta=False, outdir=FAKED if __name__ == "__main__": + def comma_separated_list(string): + if string == "all": + return string + return string.split(",") + parser = argparse.ArgumentParser(description="Generate new segments from old ones") parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") parser.add_argument("--outdir", help="log output dir", default=FAKEDATA) + parser.add_argument("--whitelist-procs", type=comma_separated_list, default="all", + help="Comma-separated whitelist of processes to regen (e.g. controlsd). Pass 'all' to whitelist all processes.") parser.add_argument("route", type=str, help="The source route") parser.add_argument("seg", type=int, help="Segment in source route") args = parser.parse_args() - regen_and_save(args.route, args.seg, args.upload, outdir=args.outdir) + + regen_and_save(args.route, args.seg, args.whitelist_procs, args.upload, outdir=args.outdir) diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py index 28e0f70589cf57..29fdf641f13f99 100755 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -92,7 +92,6 @@ def floats(**kwargs): 'speedAccuracy': floats(width=32), }) r['LiveCalibration'] = st.fixed_dictionaries({ - 'calStatus': st.integers(min_value=0, max_value=1), 'rpyCalib': st.lists(floats(width=32), min_size=3, max_size=3), }) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index a2dd938e2f4870..deed1ec48f5c76 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -25,41 +25,42 @@ ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA_2018_HYBRID - ("RAM", "2f4452b03ccb98f0|2022-09-07--13-55-08--10"), # CHRYSLER.RAM_1500 + ("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), # CHRYSLER.RAM_1500 ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.OUTBACK ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.BOLT_EUV ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021 + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1 # Enable when port is tested and dashcamOnly is no longer set - #("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1 #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.PASSAT_NMS ] segments = [ - ("BODY", "regenFA002A80700|2022-09-27--15-37-02--0"), - ("HYUNDAI", "regenBE53A59065B|2022-09-27--16-52-03--0"), - ("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), - ("TOYOTA", "regen929C5790007|2022-09-27--16-27-47--0"), - ("TOYOTA2", "regenEA3950D7F22|2022-09-27--15-43-24--0"), - ("TOYOTA3", "regen89026F6BD8D|2022-09-27--15-45-37--0"), - ("HONDA", "regenC7D5645EB17|2022-09-27--15-47-29--0"), - ("HONDA2", "regenCC2ECCE5742|2022-09-27--16-18-01--0"), - ("CHRYSLER", "regenC253C4DAC90|2022-09-27--15-51-03--0"), - ("RAM", "regen20490083AE7|2022-09-27--15-53-15--0"), - ("SUBARU", "regen1E72BBDCED5|2022-09-27--15-55-31--0"), - ("GM", "regen45B05A80EF6|2022-09-27--15-57-22--0"), - ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), - ("NISSAN", "regenC19D899B46D|2022-09-27--15-59-13--0"), - ("VOLKSWAGEN", "regenD8F7AC4BD0D|2022-09-27--16-41-45--0"), - ("MAZDA", "regenFC3F9ECBB64|2022-09-27--16-03-09--0"), -] + ("BODY", "aregenECF15D9E559|2023-05-10--14-26-40--0"), + ("HYUNDAI", "aregenAB9F543F70A|2023-05-10--14-28-25--0"), + ("HYUNDAI2", "aregen39F5A028F96|2023-05-10--14-31-00--0"), + ("TOYOTA", "aregen8D6A8B36E8D|2023-05-10--14-32-38--0"), + ("TOYOTA2", "aregenB1933C49809|2023-05-10--14-34-14--0"), + ("TOYOTA3", "aregen5D9915223DC|2023-05-10--14-36-43--0"), + ("HONDA", "aregen484B732B675|2023-05-10--14-38-23--0"), + ("HONDA2", "aregenAF6ACED4713|2023-05-10--14-40-01--0"), + ("CHRYSLER", "aregen99B094E1E2E|2023-05-10--14-41-40--0"), + ("RAM", "aregen5C2487E1EEB|2023-05-10--14-44-09--0"), + ("SUBARU", "aregen98D277B792E|2023-05-10--14-46-46--0"), + ("GM", "aregen377BA28D848|2023-05-10--14-48-28--0"), + ("GM2", "aregen7CA0CC0F0C2|2023-05-10--14-51-00--0"), + ("NISSAN", "aregen7097BF01563|2023-05-10--14-52-43--0"), + ("VOLKSWAGEN", "aregen765AF3D2CB5|2023-05-10--14-54-23--0"), + ("MAZDA", "aregen3053762FF2E|2023-05-10--14-56-53--0"), + ("FORD", "aregenDDE0F89FA1E|2023-05-10--14-59-26--0"), + ] # dashcamOnly makes don't need to be tested until a full port is done -excluded_interfaces = ["mock", "ford", "mazda", "tesla"] +excluded_interfaces = ["mock", "mazda", "tesla"] BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 81d68aa8778a1d..22ddb6e9cc57f8 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -1,11 +1,14 @@ #!/usr/bin/env python3 +import math import json import os +import shutil import subprocess import time import numpy as np import unittest from collections import Counter, defaultdict +from functools import cached_property from pathlib import Path from cereal import car @@ -45,6 +48,15 @@ "./ubloxd": 0.02, "selfdrive.tombstoned": 0, "./logcatd": 0, + "system.micd": 10.0, + "system.timezoned": 0, + "system.sensord.pigeond": 6.0, + "selfdrive.boardd.pandad": 0, + "selfdrive.statsd": 0.4, + "selfdrive.navd.navd": 0.4, + "system.loggerd.uploader": 4.0, + "system.loggerd.deleter": 0.1, + "selfdrive.locationd.laikad": None, # TODO: laikad cpu usage is sporadic } TIMINGS = { @@ -71,48 +83,6 @@ def cputime_total(ct): return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem -def check_cpu_usage(proclogs): - result = "\n" - result += "------------------------------------------------\n" - result += "------------------ CPU Usage -------------------\n" - result += "------------------------------------------------\n" - - plogs_by_proc = defaultdict(list) - for pl in proclogs: - for x in pl.procLog.procs: - if len(x.cmdline) > 0: - n = list(x.cmdline)[0] - plogs_by_proc[n].append(x) - - print(plogs_by_proc.keys()) - - r = True - dt = (proclogs[-1].logMonoTime - proclogs[0].logMonoTime) / 1e9 - for proc_name, expected_cpu in PROCS.items(): - err = "" - cpu_usage = 0. - x = plogs_by_proc[proc_name] - if len(x) > 2: - cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) - cpu_usage = cpu_time / dt * 100. - if cpu_usage > max(expected_cpu * 1.15, expected_cpu + 5.0): - # cpu usage is high while playing sounds - if not (proc_name == "./_soundd" and cpu_usage < 65.): - err = "using more CPU than normal" - elif cpu_usage < min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)): - err = "using less CPU than normal" - else: - err = "NO METRICS FOUND" - - result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu:5.2f}%) {err}\n" - if len(err) > 0: - r = False - - result += "------------------------------------------------\n" - print(result) - return r - - class TestOnroad(unittest.TestCase): @classmethod @@ -120,19 +90,22 @@ def setUpClass(cls): if "DEBUG" in os.environ: segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(ROOT).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) - print(segs[-2]) - cls.lr = list(LogReader(os.path.join(segs[-2], "rlog"))) + print(segs[-3]) + cls.lr = list(LogReader(os.path.join(segs[-3], "rlog"))) return # setup env + os.environ['PASSIVE'] = "0" os.environ['REPLAY'] = "1" os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" - os.environ['LOGPRINT'] = 'debug' + os.environ['LOGPRINT'] = "debug" params = Params() params.clear_all() set_params_enabled() + if os.path.exists(ROOT): + shutil.rmtree(ROOT) # Make sure athena isn't running os.system("pkill -9 -f athena") @@ -177,6 +150,25 @@ def setUpClass(cls): # use the second segment by default as it's the first full segment cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog"))) + @cached_property + def service_msgs(self): + msgs = defaultdict(list) + for m in self.lr: + msgs[m.which()].append(m) + return msgs + + def test_service_frequencies(self): + for s, msgs in self.service_msgs.items(): + if s in ('initData', 'sentinel'): + continue + + # skip gps services for now + if s in ('ubloxGnss', 'ubloxRaw', 'gnssMeasurements'): + continue + + with self.subTest(service=s): + assert len(msgs) >= math.floor(service_list[s].frequency*55) + def test_cloudlog_size(self): msgs = [m for m in self.lr if m.which() == 'logMessage'] @@ -193,7 +185,7 @@ def test_ui_timings(self): result += "-------------- UI Draw Timing ------------------\n" result += "------------------------------------------------\n" - ts = [m.uiDebug.drawTimeMillis for m in self.lr if m.which() == 'uiDebug'] + ts = [m.uiDebug.drawTimeMillis for m in self.service_msgs['uiDebug']] result += f"min {min(ts):.2f}ms\n" result += f"max {max(ts):.2f}ms\n" result += f"std {np.std(ts):.2f}ms\n" @@ -201,15 +193,60 @@ def test_ui_timings(self): result += "------------------------------------------------\n" print(result) - self.assertGreater(len(ts), 20*50, "insufficient samples") #self.assertLess(max(ts), 30.) self.assertLess(np.mean(ts), 10.) #self.assertLess(np.std(ts), 5.) def test_cpu_usage(self): - proclogs = [m for m in self.lr if m.which() == 'procLog'] - self.assertGreater(len(proclogs), service_list['procLog'].frequency * 45, "insufficient samples") - cpu_ok = check_cpu_usage(proclogs) + result = "\n" + result += "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + plogs_by_proc = defaultdict(list) + for pl in self.service_msgs['procLog']: + for x in pl.procLog.procs: + if len(x.cmdline) > 0: + n = list(x.cmdline)[0] + plogs_by_proc[n].append(x) + print(plogs_by_proc.keys()) + + cpu_ok = True + dt = (self.service_msgs['procLog'][-1].logMonoTime - self.service_msgs['procLog'][0].logMonoTime) / 1e9 + for proc_name, expected_cpu in PROCS.items(): + + err = "" + cpu_usage = 0. + x = plogs_by_proc[proc_name] + if len(x) > 2: + cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) + cpu_usage = cpu_time / dt * 100. + + if expected_cpu is None: + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu}) SKIPPED\n" + continue + elif cpu_usage > max(expected_cpu * 1.15, expected_cpu + 5.0): + # cpu usage is high while playing sounds + if not (proc_name == "./_soundd" and cpu_usage < 65.): + err = "using more CPU than normal" + elif cpu_usage < min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)): + err = "using less CPU than normal" + else: + err = "NO METRICS FOUND" + + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu:5.2f}%) {err}\n" + if len(err) > 0: + cpu_ok = False + + # Ensure there's no missing procs + all_procs = set([p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning]) + for p in all_procs: + with self.subTest(proc=p): + assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}" + + result += "------------------------------------------------\n" + print(result) + self.assertTrue(cpu_ok) def test_camera_processing_time(self): @@ -234,7 +271,7 @@ def test_mpc_execution_timings(self): cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] + ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.service_msgs[s]] self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" @@ -254,7 +291,7 @@ def test_model_execution_timings(self): ("driverStateV2", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] + ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.service_msgs[s]] self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" @@ -270,7 +307,7 @@ def test_timings(self): result += "----------------- Service Timings --------------\n" result += "------------------------------------------------\n" for s, (maxmin, rsd) in TIMINGS.items(): - msgs = [m.logMonoTime for m in self.lr if m.which() == s] + msgs = [m.logMonoTime for m in self.service_msgs[s]] if not len(msgs): raise Exception(f"missing {s}") @@ -305,6 +342,17 @@ def test_startup(self): expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1 self.assertEqual(startup_alert, expected, "wrong startup alert") + def test_engagable(self): + no_entries = Counter() + for m in self.service_msgs['carEvents']: + for evt in m.carEvents: + if evt.noEntry: + no_entries[evt.name] += 1 + + eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']] + assert all(eng), \ + f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}" + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index 2094faeaa73c04..f3e822da519a99 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -8,7 +8,7 @@ class BaseFanController(ABC): @abstractmethod - def update(self, max_cpu_temp: float, ignition: bool) -> int: + def update(self, cur_temp: float, ignition: bool) -> int: pass @@ -18,19 +18,19 @@ def __init__(self) -> None: cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML)) - def update(self, max_cpu_temp: float, ignition: bool) -> int: - self.controller.neg_limit = -(80 if ignition else 30) + def update(self, cur_temp: float, ignition: bool) -> int: + self.controller.neg_limit = -(100 if ignition else 30) self.controller.pos_limit = -(30 if ignition else 0) if ignition != self.last_ignition: self.controller.reset() - error = 70 - max_cpu_temp + error = 70 - cur_temp fan_pwr_out = -int(self.controller.update( error=error, - feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80]) + feedforward=interp(cur_temp, [60.0, 100.0], [0, -100]) )) self.last_ignition = ignition diff --git a/selfdrive/thermald/tests/test_fan_controller.py b/selfdrive/thermald/tests/test_fan_controller.py index 857866f64eac60..22d618485ca5b7 100755 --- a/selfdrive/thermald/tests/test_fan_controller.py +++ b/selfdrive/thermald/tests/test_fan_controller.py @@ -14,43 +14,43 @@ def patched_controller(controller_class): class TestFanController(unittest.TestCase): def wind_up(self, controller, ignition=True): for _ in range(1000): - controller.update(max_cpu_temp=100, ignition=ignition) + controller.update(100, ignition) def wind_down(self, controller, ignition=False): for _ in range(1000): - controller.update(max_cpu_temp=10, ignition=ignition) + controller.update(10, ignition) @parameterized.expand(ALL_CONTROLLERS) def test_hot_onroad(self, controller_class): controller = patched_controller(controller_class) self.wind_up(controller) - self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 70) + self.assertGreaterEqual(controller.update(100, True), 70) @parameterized.expand(ALL_CONTROLLERS) def test_offroad_limits(self, controller_class): controller = patched_controller(controller_class) self.wind_up(controller) - self.assertLessEqual(controller.update(max_cpu_temp=100, ignition=False), 30) + self.assertLessEqual(controller.update(100, False), 30) @parameterized.expand(ALL_CONTROLLERS) def test_no_fan_wear(self, controller_class): controller = patched_controller(controller_class) self.wind_down(controller) - self.assertEqual(controller.update(max_cpu_temp=10, ignition=False), 0) + self.assertEqual(controller.update(10, False), 0) @parameterized.expand(ALL_CONTROLLERS) def test_limited(self, controller_class): controller = patched_controller(controller_class) - self.wind_up(controller, ignition=True) - self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 80) + self.wind_up(controller, True) + self.assertEqual(controller.update(100, True), 100) @parameterized.expand(ALL_CONTROLLERS) def test_windup_speed(self, controller_class): controller = patched_controller(controller_class) - self.wind_down(controller, ignition=True) + self.wind_down(controller, True) for _ in range(10): - controller.update(max_cpu_temp=90, ignition=True) - self.assertGreaterEqual(controller.update(max_cpu_temp=90, ignition=True), 60) + controller.update(90, True) + self.assertGreaterEqual(controller.update(90, True), 60) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 531e0b04d186b2..57387e5186e72a 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -313,9 +313,6 @@ def thermald_thread(end_event, hw_queue): should_start = should_start and all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): - params.put_bool("IsOnroad", should_start) - params.put_bool("IsOffroad", not should_start) - params.put_bool("IsEngaged", False) engaged_prev = False HARDWARE.set_power_save(not should_start) diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc index 622dcdd3524d48..64cb3579945357 100644 --- a/selfdrive/ui/qt/offroad/onboarding.cc +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -40,8 +40,6 @@ void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) { } void TrainingGuide::showEvent(QShowEvent *event) { - boundingRect = width() == WIDE_WIDTH ? boundingRectWide : boundingRectStandard; - currentIndex = 0; image.load(img_path + "step0.png"); click_timer.start(); diff --git a/selfdrive/ui/qt/offroad/onboarding.h b/selfdrive/ui/qt/offroad/onboarding.h index 1be1d8f397e349..d347d1e613d9f8 100644 --- a/selfdrive/ui/qt/offroad/onboarding.h +++ b/selfdrive/ui/qt/offroad/onboarding.h @@ -25,56 +25,31 @@ class TrainingGuide : public QFrame { int currentIndex = 0; // Bounding boxes for each training guide step - const QRect continueBtnStandard = {1620, 0, 300, 1080}; - QVector boundingRectStandard { - QRect(112, 804, 619, 166), - continueBtnStandard, - continueBtnStandard, - QRect(1476, 565, 253, 308), - QRect(1501, 529, 184, 108), - continueBtnStandard, - QRect(1613, 665, 178, 153), - QRect(1220, 0, 420, 730), - QRect(1335, 499, 440, 147), - QRect(112, 820, 996, 148), - QRect(1412, 199, 316, 333), - continueBtnStandard, - QRect(1237, 63, 683, 1017), - continueBtnStandard, - QRect(1455, 110, 313, 860), - QRect(1253, 519, 383, 228), - continueBtnStandard, - continueBtnStandard, - QRect(630, 804, 626, 164), - QRect(108, 804, 426, 164), - }; - - const QRect continueBtnWide = {1840, 0, 320, 1080}; - QVector boundingRectWide { + const QRect continueBtn = {1840, 0, 320, 1080}; + QVector boundingRect { QRect(112, 804, 618, 164), - continueBtnWide, - continueBtnWide, + continueBtn, + continueBtn, QRect(1641, 558, 210, 313), QRect(1662, 528, 184, 108), - continueBtnWide, + continueBtn, QRect(1814, 621, 211, 170), QRect(1350, 0, 497, 755), - QRect(1553, 516, 406, 112), + QRect(1540, 386, 468, 238), QRect(112, 804, 1126, 164), QRect(1598, 199, 316, 333), - continueBtnWide, + continueBtn, QRect(1364, 90, 796, 990), - continueBtnWide, + continueBtn, QRect(1593, 114, 318, 853), QRect(1379, 511, 391, 243), - continueBtnWide, - continueBtnWide, + continueBtn, + continueBtn, QRect(630, 804, 626, 164), QRect(108, 804, 426, 164), }; const QString img_path = "../assets/training/"; - QVector boundingRect; QElapsedTimer click_timer; signals: diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index fd14ed15e2d33c..b9fec9d62db083 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -273,7 +273,7 @@ void DevicePanel::updateCalibDescription() { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size())); auto calib = cmsg.getRoot().getLiveCalibration(); - if (calib.getCalStatus() != 0) { + if (calib.getCalStatus() != cereal::LiveCalibrationData::Status::UNCALIBRATED) { double pitch = calib.getRpyCalib()[1] * (180 / M_PI); double yaw = calib.getRpyCalib()[2] * (180 / M_PI); desc += tr(" Your device is pointed %1° %2 and %3° %4.") @@ -343,7 +343,6 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QPushButton { font-size: 140px; padding-bottom: 20px; - font-weight: bold; border 1px grey solid; border-radius: 100px; background-color: #292929; @@ -379,22 +378,18 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QObject::connect(map_panel, &MapPanel::closeSettings, this, &SettingsWindow::closeSettings); #endif - const int padding = panels.size() > 3 ? 25 : 35; - nav_btns = new QButtonGroup(this); for (auto &[name, panel] : panels) { QPushButton *btn = new QPushButton(name); btn->setCheckable(true); btn->setChecked(nav_btns->buttons().size() == 0); - btn->setStyleSheet(QString(R"( + btn->setStyleSheet(R"( QPushButton { color: grey; border: none; background: none; font-size: 65px; font-weight: 500; - padding-top: %1px; - padding-bottom: %1px; } QPushButton:checked { color: white; @@ -402,8 +397,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QPushButton:pressed { color: #ADADAD; } - )").arg(padding)); - + )"); + btn->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); nav_btns->addButton(btn); sidebar_layout->addWidget(btn, 0, Qt::AlignRight); diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index fde8645586e97f..be9da34d457064 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -415,7 +415,7 @@ void WifiManager::addTetheringConnection() { } void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { - int prime_type = uiState()->prime_type; + int prime_type = uiState()->primeType(); int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); if (!ipv4_forward) { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 73a28225351155..2030c36046a415 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -154,7 +154,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { void OnroadWindow::offroadTransition(bool offroad) { #ifdef ENABLE_MAPS if (!offroad) { - if (map == nullptr && (uiState()->prime_type || !MAPBOX_TOKEN.isEmpty())) { + if (map == nullptr && (uiState()->primeType() || !MAPBOX_TOKEN.isEmpty())) { MapWindow * m = new MapWindow(get_mapbox_settings()); map = m; @@ -525,6 +525,7 @@ void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QB p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); p.setOpacity(opacity); p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img); + p.setOpacity(1.0); } @@ -576,24 +577,34 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } // paint path - QLinearGradient bg(0, height(), 0, height() / 4); - float start_hue, end_hue; + QLinearGradient bg(0, height(), 0, 0); if (sm["controlsState"].getControlsState().getExperimentalMode()) { - const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration(); - float acceleration_future = 0; - if (acceleration.getZ().size() > 16) { - acceleration_future = acceleration.getX()[16]; // 2.5 seconds + // The first half of track_vertices are the points for the right side of the path + // and the indices match the positions of accel from uiPlan + const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); + const int max_len = std::min(scene.track_vertices.length() / 2, acceleration.size()); + + for (int i = 0; i < max_len; ++i) { + // Some points are out of frame + if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue; + + // Flip so 0 is bottom of frame + float lin_grad_point = (height() - scene.track_vertices[i].y()) / height(); + + // speed up: 120, slow down: 0 + float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0); + // FIXME: painter.drawPolygon can be slow if hue is not rounded + path_hue = int(path_hue * 100 + 0.5) / 100; + + float saturation = fmin(fabs(acceleration[i] * 1.5), 1); + float lightness = util::map_val(saturation, 0.0f, 1.0f, 0.95f, 0.62f); // lighter when grey + float alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, 0.4f, 0.0f); // matches previous alpha fade + bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha)); + + // Skip a point, unless next is last + i += (i + 2) < max_len ? 1 : 0; } - start_hue = 60; - // speed up: 120, slow down: 0 - end_hue = fmax(fmin(start_hue + acceleration_future * 45, 148), 0); - - // FIXME: painter.drawPolygon can be slow if hue is not rounded - end_hue = int(end_hue * 100 + 0.5) / 100; - bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); - bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); - bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); } else { bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); bg.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.35)); @@ -615,13 +626,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) int x = rightHandDM ? rect().right() - (btn_size - 24) / 2 - (bdr_s * 2) : (btn_size - 24) / 2 + (bdr_s * 2); int y = rect().bottom() - footer_h / 2; float opacity = dmActive ? 0.65 : 0.2; - drawIcon(painter, x, y, dm_img, blackColor(0), opacity); - - // circle background - painter.setOpacity(1.0); - painter.setPen(Qt::NoPen); - painter.setBrush(blackColor(70)); - painter.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); + drawIcon(painter, x, y, dm_img, blackColor(70), opacity); // face QPointF face_kpts_draw[std::size(default_face_kpts_3d)]; diff --git a/selfdrive/ui/qt/qt_window.cc b/selfdrive/ui/qt/qt_window.cc index d630b560bb967f..b83db706711e84 100644 --- a/selfdrive/ui/qt/qt_window.cc +++ b/selfdrive/ui/qt/qt_window.cc @@ -19,6 +19,10 @@ void setMainWindow(QWidget *w) { wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); wl_surface_commit(s); w->showFullScreen(); + + // ensure we have a valid eglDisplay, otherwise the ui will silently fail + void *egl = native->nativeResourceForWindow("egldisplay", w->windowHandle()); + assert(egl != nullptr); #endif } diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 1a4a66dc580a5b..b107bc11306401 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -59,7 +59,8 @@ void configFont(QPainter &p, const QString &family, int size, const QString &sty } void clearLayout(QLayout* layout) { - while (QLayoutItem* item = layout->takeAt(0)) { + while (layout->count() > 0) { + QLayoutItem* item = layout->takeAt(0); if (QWidget* widget = item->widget()) { widget->deleteLater(); } diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 5b66ec9fa48498..b86bbcf8995f24 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -17,6 +17,7 @@ QMap getSupportedLanguages(); void configFont(QPainter &p, const QString &family, int size, const QString &style); void clearLayout(QLayout* layout); void setQtSurfaceFormat(); +void sigTermHandler(int s); QString timeAgo(const QDateTime &date); void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg); void initApp(int argc, char *argv[], bool disable_hidpi = true); diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index da2f4e60d19731..7f88b049aecdae 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -277,7 +277,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { primeUser = new PrimeUserWidget; mainLayout->addWidget(primeUser); - mainLayout->setCurrentWidget(uiState()->prime_type ? (QWidget*)primeUser : (QWidget*)primeAd); + mainLayout->setCurrentWidget(uiState()->primeType() ? (QWidget*)primeUser : (QWidget*)primeAd); setFixedWidth(750); setStyleSheet(R"( @@ -312,7 +312,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) { QJsonObject json = doc.object(); int prime_type = json["prime_type"].toInt(); - uiState()->prime_type = prime_type; + uiState()->setPrimeType(prime_type); if (!json["is_paired"].toBool()) { mainLayout->setCurrentIndex(0); diff --git a/selfdrive/ui/soundd/main.cc b/selfdrive/ui/soundd/main.cc index 64088deff83d21..c6c7434ca44393 100644 --- a/selfdrive/ui/soundd/main.cc +++ b/selfdrive/ui/soundd/main.cc @@ -5,17 +5,13 @@ #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/soundd/sound.h" -void sigHandler(int s) { - qApp->quit(); -} - int main(int argc, char **argv) { qInstallMessageHandler(swagLogMessageHandler); setpriority(PRIO_PROCESS, 0, -20); QApplication a(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); Sound sound; return a.exec(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 828dc1901978a6..184c668825bce9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -179,7 +179,7 @@ static void update_state(UIState *s) { scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j); } } - scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; scene.calibration_wide_valid = wfde_list.size() == 3; } if (sm.updated("pandaStates")) { @@ -242,13 +242,6 @@ void UIState::updateStatus() { started_prev = scene.started; emit offroadTransition(!scene.started); } - - // Handle prime type change - if (prime_type != prime_type_prev) { - prime_type_prev = prime_type; - emit primeTypeChanged(prime_type); - Params().put("PrimeType", std::to_string(prime_type)); - } } UIState::UIState(QObject *parent) : QObject(parent) { @@ -279,6 +272,14 @@ void UIState::update() { emit uiUpdate(*this); } +void UIState::setPrimeType(int type) { + if (type != prime_type) { + prime_type = type; + Params().put("PrimeType", std::to_string(prime_type)); + emit primeTypeChanged(prime_type); + } +} + Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) { setAwake(true); resetInteractiveTimout(); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index be2353d76e6fe8..cd67c51469db5a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -154,10 +154,13 @@ class UIState : public QObject { void updateStatus(); inline bool worldObjectsVisible() const { return sm->rcv_frame("liveCalibration") > scene.started_frame; - }; + } inline bool engaged() const { return scene.started && (*sm)["controlsState"].getControlsState().getEnabled(); - }; + } + + void setPrimeType(int type); + inline int primeType() const { return prime_type; } int fb_w = 0, fb_h = 0; @@ -167,7 +170,6 @@ class UIState : public QObject { UIScene scene = {}; bool awake; - int prime_type; QString language; QTransform car_space_transform; @@ -183,7 +185,7 @@ private slots: private: QTimer *timer; bool started_prev = false; - int prime_type_prev = -1; + int prime_type = -1; }; UIState *uiState(); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index e8b8bdc77a7411..4325eccde52a25 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -1179,19 +1179,14 @@ void CameraState::set_camera_exposure(float grey_frac) { uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; - uint32_t min_gain = ox03c10_analog_gains_reg[0]; - uint32_t spd_gain = 0xF00; struct i2c_random_wr_payload exp_reg_array[] = { {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, - {0x35c1, vs_time>>8}, {0x35c2, vs_time&0xFF}, + {0x35c2, vs_time&0xFF}, {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, - {0x3588, min_gain>>8}, {0x3589, min_gain&0xFF}, - {0x3548, spd_gain>>8}, {0x3549, spd_gain&0xFF}, - {0x35c8, min_gain>>8}, {0x35c9, min_gain&0xFF}, }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 4db4bfd4d856e1..9170c5183a58c0 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -752,6 +752,12 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x5486, 0x08}, {0x5487, 0xDE}, {0x5686, 0x08}, {0x5687, 0xDE}, {0x5886, 0x08}, {0x5887, 0xDE}, + + // fixed gains + {0x3588, 0x01}, {0x3589, 0x00}, + {0x35c8, 0x01}, {0x35c9, 0x00}, + {0x3548, 0x0F}, {0x3549, 0x00}, + {0x35c1, 0x00}, }; struct i2c_random_wr_payload init_array_ar0231[] = { diff --git a/system/hardware/base.h b/system/hardware/base.h index 6cfc1d8743ea2f..5460099723c20d 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -18,6 +18,10 @@ class HardwareNone { static std::string get_serial() { return "cccccc"; } + static std::map get_init_logs() { + return {}; + } + static void reboot() {} static void poweroff() {} static void set_brightness(int percent) {} diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 7876b1af1f79b3..6730f827061cc9 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,19 +1,19 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a.img.xz", - "hash": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", - "hash_raw": "72662ec5d586c7a22659a1c8b140932d5472914176020fe76ba4204edbbb214a", - "size": 14780416, + "url": "https://commadist.azureedge.net/agnosupdate/boot-7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c.img.xz", + "hash": "7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c", + "hash_raw": "7d953f5e1bc606984e4d49c6f957421a4172f72b4ebd359baa689ef43b7e911c", + "size": 15153152, "sparse": false, "full_check": true, "has_ab": true }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82.img.xz", - "hash": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", - "hash_raw": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", + "url": "https://commadist.azureedge.net/agnosupdate/abl-50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602.img.xz", + "hash": "50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602", + "hash_raw": "50329ac734ff7a6c20c3f552dce9b13f84b3eb2e73faa64b9810049d9b406602", "size": 274432, "sparse": false, "full_check": true, @@ -21,29 +21,49 @@ }, { "name": "xbl", - "url": "https://commadist.azureedge.net/agnosupdate/xbl-2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35.img.xz", - "hash": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", - "hash_raw": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", - "size": 3670016, + "url": "https://commadist.azureedge.net/agnosupdate/xbl-dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15.img.xz", + "hash": "dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15", + "hash_raw": "dc297986b38f50c47584bd8549b188b37b1d6a0c77b3255859dd675c177b5c15", + "size": 3282672, "sparse": false, "full_check": true, "has_ab": true }, { "name": "xbl_config", - "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad.img.xz", - "hash": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", - "hash_raw": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", - "size": 364544, + "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40.img.xz", + "hash": "b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40", + "hash_raw": "b73fbbb42934aabc6d4f16ce84ac6c8c0205bc70e0a85412a771f3cc1d62cc40", + "size": 98124, + "sparse": false, + "full_check": true, + "has_ab": true + }, + { + "name": "devcfg", + "url": "https://commadist.azureedge.net/agnosupdate/devcfg-2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca.img.xz", + "hash": "2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca", + "hash_raw": "2d3063d106813006ac9ceeaf8818a31d4b33996873e81178ac5129f5e1b82bca", + "size": 40336, + "sparse": false, + "full_check": true, + "has_ab": true + }, + { + "name": "aop", + "url": "https://commadist.azureedge.net/agnosupdate/aop-d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e.img.xz", + "hash": "d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e", + "hash_raw": "d69450d5438b3e5e2ba5b77db1ae49e1cf9cab17836f563aa57192b5b3a4ac3e", + "size": 184364, "sparse": false, "full_check": true, "has_ab": true }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e.img.xz", - "hash": "48209ce7e8cc2fff4ec024f0cd82fc2e3e097b5c0629be2b292acf64e6701449", - "hash_raw": "9efdc9368f05e06008a7a1dbbee21b564e89988dc94d6ddee3a3a88e42268f0e", + "url": "https://commadist.azureedge.net/agnosupdate/system-4a8311dd591006e0c2a6f60060d6ef579ceec9b3d688e8438a9aef4e230ae028.img.xz", + "hash": "23c9f111f81fc3ee83f85016cb320e03a46aad6721a85e1b4a3f04b6a764e934", + "hash_raw": "4a8311dd591006e0c2a6f60060d6ef579ceec9b3d688e8438a9aef4e230ae028", "size": 10737418240, "sparse": true, "full_check": false, diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py index 979bc757f67d20..5b656a40fa88b1 100755 --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -124,12 +124,13 @@ def _set_configs(self, configs: List[AmpConfig]) -> None: def set_configs(self, configs: List[AmpConfig]) -> bool: # retry in case panda is using the amp - for _ in range(10): + tries = 15 + for i in range(15): try: self._set_configs(configs) return True except OSError: - print("Failed to set amp config, retrying...") + print(f"Failed to set amp config, {tries - i - 1} retries left") time.sleep(0.02) return False diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index 5f6fb2dc50d532..fba67d96c872ee 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -16,8 +16,16 @@ class HardwareTici : public HardwareNone { static std::string get_os_version() { return "AGNOS " + util::read_file("/VERSION"); }; - static std::string get_name() { return "tici"; }; - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; }; + + static std::string get_name() { + std::string devicetree_model = util::read_file("/sys/firmware/devicetree/base/model"); + return (devicetree_model.find("tizi") != std::string::npos) ? "tizi" : "tici"; + }; + + static cereal::InitData::DeviceType get_device_type() { + return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : cereal::InitData::DeviceType::TICI; + }; + static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; @@ -65,6 +73,31 @@ class HardwareTici : public HardwareNone { std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); } + + static std::map get_init_logs() { + std::map ret = { + {"/BUILD", util::read_file("/BUILD")}, + }; + + std::string bs = util::check_output("abctl --boot_slot"); + ret["boot slot"] = bs.substr(0, bs.find_first_of("\n")); + + std::string temp = util::read_file("/dev/disk/by-partlabel/ssd"); + temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1); + ret["boot temp"] = temp; + + // TODO: log something from system and boot + for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) { + for (std::string slot : {"a", "b"}) { + std::string partition = part + "_" + slot; + std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition); + ret[partition] = hash.substr(0, hash.find_first_of(" ")); + } + } + + return ret; + } + 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 ee1d5f52793282..d52710e9508977 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -4,7 +4,7 @@ import subprocess import time from enum import IntEnum -from functools import cached_property +from functools import cached_property, lru_cache from pathlib import Path from cereal import log @@ -91,8 +91,12 @@ def mm(self): def amplifier(self): return Amplifier() - @cached_property - def model(self): + def get_os_version(self): + with open("/VERSION") as f: + return f.read().strip() + + @lru_cache + def get_device_type(self): with open("/sys/firmware/devicetree/base/model") as f: model = f.read().strip('\x00') model = model.split('comma ')[-1] @@ -101,13 +105,6 @@ def model(self): model = 'tici' return model - def get_os_version(self): - with open("/VERSION") as f: - return f.read().strip() - - def get_device_type(self): - return "tici" - def get_sound_card_online(self): return (os.path.isfile('/proc/asound/card0/state') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') @@ -424,7 +421,7 @@ def set_power_save(self, powersave_enabled): # amplifier, 100mW at idle self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) if not powersave_enabled: - self.amplifier.initialize_configuration(self.model) + self.amplifier.initialize_configuration(self.get_device_type()) # *** CPU config *** @@ -445,7 +442,7 @@ def set_power_save(self, powersave_enabled): # boardd core affine_irq(4, "spi_geni") # SPI affine_irq(4, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB) - if "tici" in self.model: + if "tici" in self.get_device_type(): affine_irq(4, "xhci-hcd:usb1") # internal panda USB # camerad core @@ -461,16 +458,14 @@ def get_gpu_usage_percent(self): return 0 def initialize_hardware(self): - self.amplifier.initialize_configuration(self.model) + self.amplifier.initialize_configuration(self.get_device_type()) # Allow thermald to write engagement status to kmsg os.system("sudo chmod a+w /dev/kmsg") - # TODO: remove the if once agnos 7 ships # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL - if os.path.exists('/sys/class/gpio/gpio49/'): - gpio_init(GPIO.SOM_ST_IO, True) - gpio_set(GPIO.SOM_ST_IO, 1) + gpio_init(GPIO.SOM_ST_IO, True) + gpio_set(GPIO.SOM_ST_IO, 1) # *** IRQ config *** @@ -581,8 +576,9 @@ def recover_internal_panda(self): gpio_set(GPIO.STM_RST_N, 1) gpio_set(GPIO.STM_BOOT0, 1) - time.sleep(2) + time.sleep(1) gpio_set(GPIO.STM_RST_N, 0) + time.sleep(1) gpio_set(GPIO.STM_BOOT0, 0) diff --git a/system/hardware/tici/tests/test_amplifier.py b/system/hardware/tici/tests/test_amplifier.py index 1cad6cfd6b23c0..5d5a86c512e2d0 100755 --- a/system/hardware/tici/tests/test_amplifier.py +++ b/system/hardware/tici/tests/test_amplifier.py @@ -38,7 +38,7 @@ def _check_for_i2c_errors(self, expected): def test_init(self): amp = Amplifier(debug=True) - r = amp.initialize_configuration(Tici().model) + r = amp.initialize_configuration(Tici().get_device_type()) assert r self._check_for_i2c_errors(False) @@ -59,7 +59,7 @@ def test_init_while_siren_play(self): time.sleep(random.randint(0, 5)) amp = Amplifier(debug=True) - r = amp.initialize_configuration(Tici().model) + r = amp.initialize_configuration(Tici().get_device_type()) assert r # make sure we're a good test diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index f563933285d608..df4852183e584d 100755 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -16,11 +16,11 @@ class Proc: name: str power: float rtol: float = 0.05 - atol: float = 0.1 + atol: float = 0.12 warmup: float = 6. PROCS = [ - Proc('camerad', 2.15), + Proc('camerad', 2.1), Proc('modeld', 0.93, atol=0.2), Proc('dmonitoringmodeld', 0.4), Proc('encoderd', 0.23), @@ -60,7 +60,7 @@ def test_camera_procs(self): manager_cleanup() tab = [] - tab.append(['process', 'expected (W)', 'current (W)']) + tab.append(['process', 'expected (W)', 'measured (W)']) for proc in PROCS: cur = used[proc.name] expected = proc.power diff --git a/system/loggerd/logger.cc b/system/loggerd/logger.cc index 0599aa1e54c013..a3152aa88ae6a0 100644 --- a/system/loggerd/logger.cc +++ b/system/loggerd/logger.cc @@ -70,7 +70,9 @@ kj::Array logger_build_init_data() { "df -h", // usage for all filesystems }; - auto commands = init.initCommands().initEntries(log_commands.size()); + auto hw_logs = Hardware::get_init_logs(); + + auto commands = init.initCommands().initEntries(log_commands.size() + hw_logs.size()); for (int i = 0; i < log_commands.size(); i++) { auto lentry = commands[i]; @@ -80,6 +82,14 @@ kj::Array logger_build_init_data() { lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); } + int i = log_commands.size(); + for (auto [key, value] : hw_logs) { + auto lentry = commands[i]; + lentry.setKey(key); + lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size())); + i++; + } + return capnp::messageToFlatArray(msg); } diff --git a/system/loggerd/tests/loggerd_tests_common.py b/system/loggerd/tests/loggerd_tests_common.py index 6aca83858bdab7..780316ad0c44b5 100644 --- a/system/loggerd/tests/loggerd_tests_common.py +++ b/system/loggerd/tests/loggerd_tests_common.py @@ -7,7 +7,7 @@ import system.loggerd.uploader as uploader -def create_random_file(file_path, size_mb, lock=False): +def create_random_file(file_path, size_mb, lock=False, xattr=None): try: os.mkdir(os.path.dirname(file_path)) except OSError: @@ -25,6 +25,9 @@ def create_random_file(file_path, size_mb, lock=False): for _ in range(chunks): f.write(data) + if xattr is not None: + uploader.setxattr(file_path, uploader.UPLOAD_ATTR_NAME, xattr) + class MockResponse(): def __init__(self, text, status_code): self.text = text @@ -95,8 +98,8 @@ def tearDown(self): if e.errno != errno.ENOENT: raise - def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False): + def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False, xattr=None): file_path = os.path.join(self.root, f_dir, fn) - create_random_file(file_path, size_mb, lock) + create_random_file(file_path, size_mb, lock, xattr) return file_path diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py index f39b8bb9d6ed3e..a2166016e024c3 100755 --- a/system/loggerd/tests/test_loggerd.py +++ b/system/loggerd/tests/test_loggerd.py @@ -133,6 +133,7 @@ def test_rotation(self): os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length) managed_processes["loggerd"].start() managed_processes["encoderd"].start() + time.sleep(1) fps = 20.0 for n in range(1, int(num_segs*length*fps)+1): diff --git a/system/loggerd/tests/test_uploader.py b/system/loggerd/tests/test_uploader.py index 11b273cec8a582..df99651447133e 100755 --- a/system/loggerd/tests/test_uploader.py +++ b/system/loggerd/tests/test_uploader.py @@ -7,7 +7,7 @@ import json from system.swaglog import cloudlog -import system.loggerd.uploader as uploader +from system.loggerd.uploader import uploader_fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE from system.loggerd.tests.loggerd_tests_common import UploaderTestCase @@ -42,7 +42,7 @@ def setUp(self): def start_thread(self): self.end_event = threading.Event() - self.up_thread = threading.Thread(target=uploader.uploader_fn, args=[self.end_event]) + self.up_thread = threading.Thread(target=uploader_fn, args=[self.end_event]) self.up_thread.daemon = True self.up_thread.start() @@ -50,13 +50,13 @@ def join_thread(self): self.end_event.set() self.up_thread.join() - def gen_files(self, lock=False, boot=True): + def gen_files(self, lock=False, xattr=None, boot=True): f_paths = list() for t in ["qlog", "rlog", "dcamera.hevc", "fcamera.hevc"]: - f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock)) + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock, xattr=xattr)) if boot: - f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock)) + f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock, xattr=xattr)) return f_paths def gen_order(self, seg1, seg2, boot=True): @@ -82,7 +82,25 @@ def test_upload(self): self.assertFalse(len(log_handler.upload_order) < len(exp_order), "Some files failed to upload") self.assertFalse(len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice") for f_path in exp_order: - self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not uploaded") + self.assertEqual(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), UPLOAD_ATTR_NAME), UPLOAD_ATTR_VALUE, "All files not uploaded") + + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_upload_with_wrong_xattr(self): + self.gen_files(lock=False, xattr=b'0') + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + self.assertTrue(len(log_handler.upload_ignored) == 0, "Some files were ignored") + self.assertFalse(len(log_handler.upload_order) < len(exp_order), "Some files failed to upload") + self.assertFalse(len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice") + for f_path in exp_order: + self.assertEqual(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), UPLOAD_ATTR_NAME), UPLOAD_ATTR_VALUE, "All files not uploaded") self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") @@ -101,7 +119,7 @@ def test_upload_ignored(self): self.assertFalse(len(log_handler.upload_ignored) < len(exp_order), "Some files failed to ignore") self.assertFalse(len(log_handler.upload_ignored) > len(exp_order), "Some files were ignored twice") for f_path in exp_order: - self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not ignored") + self.assertEqual(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), UPLOAD_ATTR_NAME), UPLOAD_ATTR_VALUE, "All files not ignored") self.assertTrue(log_handler.upload_ignored == exp_order, "Files ignored in wrong order") @@ -126,7 +144,7 @@ def test_upload_files_in_create_order(self): self.assertFalse(len(log_handler.upload_order) < len(exp_order), "Some files failed to upload") self.assertFalse(len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice") for f_path in exp_order: - self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not uploaded") + self.assertEqual(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), UPLOAD_ATTR_NAME), UPLOAD_ATTR_VALUE, "All files not uploaded") self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") @@ -141,9 +159,20 @@ def test_no_upload_with_lock_file(self): self.join_thread() for f_path in f_paths: - uploaded = uploader.UPLOAD_ATTR_NAME in os.listxattr(f_path.replace('.bz2', '')) + fn = f_path.replace('.bz2', '') + uploaded = UPLOAD_ATTR_NAME in os.listxattr(fn) and os.getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE self.assertFalse(uploaded, "File upload when locked") + def test_no_upload_with_xattr(self): + self.gen_files(lock=False, xattr=UPLOAD_ATTR_VALUE) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + self.assertEqual(len(log_handler.upload_order), 0, "File uploaded again") + def test_clear_locks_on_startup(self): f_paths = self.gen_files(lock=True, boot=False) self.start_thread() diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 6f3aae73a5bc46..34c60112f3f177 100644 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -33,9 +33,15 @@ fake_upload = os.getenv("FAKEUPLOAD") is not None +class FakeRequest: + def __init__(self): + self.headers = {"Content-Length": "0"} + + class FakeResponse: def __init__(self): self.status_code = 200 + self.request = FakeRequest() UploadResponse = Union[requests.Response, FakeResponse] @@ -110,7 +116,7 @@ def list_upload_files(self) -> Iterator[Tuple[str, str, str]]: fn = os.path.join(path, name) # skip files already uploaded try: - is_uploaded = bool(getxattr(fn, UPLOAD_ATTR_NAME)) + is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE except OSError: cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn) is_uploaded = True # deleter could have deleted @@ -200,9 +206,14 @@ def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) if stat is not None and stat.status_code in (200, 201, 401, 403, 412): self.last_filename = fn self.last_time = time.monotonic() - start_time - self.last_speed = (sz / 1e6) / self.last_time + if stat.status_code == 412: + self.last_speed = 0 + cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + else: + content_length = int(stat.request.headers.get("Content-Length", 0)) + self.last_speed = (content_length / 1e6) / self.last_time + cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length, network_type=network_type, metered=metered, speed=self.last_speed) success = True - cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) else: success = False cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) diff --git a/tools/cabana/README.md b/tools/cabana/README.md index 921decff3c22bf..cfda056636647c 100644 --- a/tools/cabana/README.md +++ b/tools/cabana/README.md @@ -1,7 +1,5 @@ # Cabana - - Cabana is a tool developed to view raw CAN data. One use for this is creating and editing [CAN Dictionaries](http://socialledge.com/sjsu/index.php/DBC_Format) (DBC files), and the tool provides direct integration with [commaai/opendbc](https://github.com/commaai/opendbc) (a collection of DBC files), allowing you to load the DBC files direct from source, and save to your fork. In addition, you can load routes from [comma connect](https://connect.comma.ai). ## Usage Instructions diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index a735dc438e9709..f12888b792aa56 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -32,7 +32,7 @@ cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_' cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/pandastream.cc', 'streams/devicestream.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc', 'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc', 'chart/chartswidget.cc', 'chart/chart.cc', 'chart/signalselector.cc', 'chart/tiplabel.cc', 'chart/sparkline.cc', - 'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) + 'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc', 'tools/findsignal.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) cabana_env.Program('cabana', ['cabana.cc', cabana_lib, assets], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) if GetOption('test'): diff --git a/tools/cabana/chart/chart.cc b/tools/cabana/chart/chart.cc index bd051072975cca..7840ffd673628a 100644 --- a/tools/cabana/chart/chart.cc +++ b/tools/cabana/chart/chart.cc @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -17,6 +18,8 @@ #include "tools/cabana/chart/chartswidget.h" +// ChartAxisElement's padding is 4 (https://codebrowser.dev/qt5/qtcharts/src/charts/axis/chartaxiselement_p.h.html) +const int AXIS_X_TOP_MARGIN = 4; static inline bool xLessThan(const QPointF &p, float x) { return p.x() < x; } ChartView::ChartView(const std::pair &x_range, ChartsWidget *parent) : charts_widget(parent), tip_label(this), QChartView(nullptr, parent) { @@ -39,6 +42,7 @@ ChartView::ChartView(const std::pair &x_range, ChartsWidget *par setRubberBand(QChartView::HorizontalRubberBand); setMouseTracking(true); setTheme(settings.theme == DARK_THEME ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); + signal_value_font.setPointSize(9); QObject::connect(axis_y, &QValueAxis::rangeChanged, [this]() { resetChartCache(); }); QObject::connect(axis_y, &QAbstractAxis::titleTextChanged, [this]() { resetChartCache(); }); @@ -119,7 +123,7 @@ void ChartView::addSignal(const MessageId &msg_id, const cabana::Signal *sig) { } bool ChartView::hasSignal(const MessageId &msg_id, const cabana::Signal *sig) const { - return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); + return std::any_of(sigs.cbegin(), sigs.cend(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); } void ChartView::removeIf(std::function predicate) { @@ -143,14 +147,14 @@ void ChartView::removeIf(std::function predicate) { } void ChartView::signalUpdated(const cabana::Signal *sig) { - if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.sig == sig; })) { + if (std::any_of(sigs.cbegin(), sigs.cend(), [=](auto &s) { return s.sig == sig; })) { updateTitle(); updateSeries(sig); } } void ChartView::msgUpdated(MessageId id) { - if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.msg_id == id; })) + if (std::any_of(sigs.cbegin(), sigs.cend(), [=](auto &s) { return s.msg_id == id; })) updateTitle(); } @@ -189,10 +193,16 @@ void ChartView::updatePlotArea(int left_pos, bool force) { qreal left, top, right, bottom; chart()->layout()->getContentsMargins(&left, &top, &right, &bottom); - chart()->legend()->setGeometry({move_icon->sceneBoundingRect().topRight(), manage_btn_proxy->sceneBoundingRect().bottomLeft()}); + QSizeF legend_size = chart()->legend()->layout()->minimumSize(); + legend_size.setWidth(manage_btn_proxy->sceneBoundingRect().left() - move_icon->sceneBoundingRect().right()); + chart()->legend()->setGeometry({move_icon->sceneBoundingRect().topRight(), legend_size}); + + // add top space for signal value + int adjust_top = chart()->legend()->geometry().height() + QFontMetrics(signal_value_font).height() + 3; + adjust_top = std::max(adjust_top, manage_btn_proxy->sceneBoundingRect().height() + style()->pixelMetric(QStyle::PM_LayoutTopMargin)); + // add right space for x-axis label QSizeF x_label_size = QFontMetrics(axis_x->labelsFont()).size(Qt::TextSingleLine, QString::number(axis_x->max(), 'f', 2)); x_label_size += QSizeF{5, 5}; - int adjust_top = chart()->legend()->geometry().height() + style()->pixelMetric(QStyle::PM_LayoutTopMargin); chart()->setPlotArea(rect().adjusted(align_to + left, adjust_top + top, -x_label_size.width() / 2 - right, -x_label_size.height() - bottom)); chart()->layout()->invalidate(); resetChartCache(); @@ -229,11 +239,11 @@ void ChartView::updatePlot(double cur, double min, double max) { void ChartView::updateSeriesPoints() { // Show points when zoomed in enough for (auto &s : sigs) { - auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan); - auto end = std::lower_bound(begin, s.vals.end(), axis_x->max(), xLessThan); + auto begin = std::lower_bound(s.vals.cbegin(), s.vals.cend(), axis_x->min(), xLessThan); + auto end = std::lower_bound(begin, s.vals.cend(), axis_x->max(), xLessThan); if (begin != end) { int num_points = std::max((end - begin), 1); - QPointF right_pt = end == s.vals.end() ? s.vals.back() : *end; + QPointF right_pt = end == s.vals.cend() ? s.vals.back() : *end; double pixels_per_point = (chart()->mapToPosition(right_pt).x() - chart()->mapToPosition(*begin).x()) / num_points; if (series_type == SeriesType::Scatter) { @@ -249,10 +259,10 @@ void ChartView::updateSeriesPoints() { } } -void ChartView::updateSeries(const cabana::Signal *sig) { +void ChartView::updateSeries(const cabana::Signal *sig, bool clear) { for (auto &s : sigs) { if (!sig || s.sig == sig) { - if (!can->liveStreaming()) { + if (clear) { s.vals.clear(); s.step_vals.clear(); s.last_value_mono_time = 0; @@ -260,15 +270,12 @@ void ChartView::updateSeries(const cabana::Signal *sig) { s.series->setColor(getColor(s.sig)); const auto &msgs = can->events(s.msg_id); + s.vals.reserve(msgs.capacity()); + s.step_vals.reserve(msgs.capacity() * 2); + auto first = std::upper_bound(msgs.cbegin(), msgs.cend(), s.last_value_mono_time, [](uint64_t ts, auto e) { return ts < e->mono_time; }); - int new_size = std::max(s.vals.size() + std::distance(first, msgs.cend()), settings.max_cached_minutes * 60 * 100); - if (s.vals.capacity() <= new_size) { - s.vals.reserve(new_size * 2); - s.step_vals.reserve(new_size * 4); - } - const double route_start_time = can->routeStartTime(); for (auto end = msgs.cend(); first != end; ++first) { const CanEvent *e = *first; @@ -308,8 +315,8 @@ void ChartView::updateAxisY() { unit.clear(); } - auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan); - auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), xLessThan); + auto first = std::lower_bound(s.vals.cbegin(), s.vals.cend(), axis_x->min(), xLessThan); + auto last = std::lower_bound(first, s.vals.cend(), axis_x->max(), xLessThan); s.min = std::numeric_limits::max(); s.max = std::numeric_limits::lowest(); if (can->liveStreaming()) { @@ -318,7 +325,7 @@ void ChartView::updateAxisY() { if (it->y() > s.max) s.max = it->y(); } } else { - auto [min_y, max_y] = s.segment_tree.minmax(std::distance(s.vals.begin(), first), std::distance(s.vals.begin(), last)); + auto [min_y, max_y] = s.segment_tree.minmax(std::distance(s.vals.cbegin(), first), std::distance(s.vals.cbegin(), last)); s.min = min_y; s.max = max_y; } @@ -527,14 +534,14 @@ void ChartView::showTip(double sec) { } tooltip_x = chart()->mapToPosition({sec, 0}).x(); - qreal x = tooltip_x; - QStringList text_list(QString::number(chart()->mapToValue({x, 0}).x(), 'f', 3)); + qreal x = -1; + QStringList text_list; for (auto &s : sigs) { if (s.series->isVisible()) { 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()) { + auto it = std::lower_bound(s.vals.crbegin(), s.vals.crend(), sec, [](auto &p, double x) { return p.x() > x; }); + if (it != s.vals.crend() && it->x() >= axis_x->min()) { value = QString::number(it->y()); s.track_pt = *it; x = std::max(x, chart()->mapToPosition(*it).x()); @@ -546,7 +553,11 @@ void ChartView::showTip(double sec) { .arg(s.series->color().name(), name, value, min, max); } } + if (x < 0) { + x = tooltip_x; + } QPoint pt(x, chart()->plotArea().top()); + text_list.push_front(QString::number(chart()->mapToValue({x, 0}).x(), 'f', 3)); QString text = "

" % text_list.join("
") % "

"; tip_label.showText(pt, text, this, visible_rect); viewport()->update(); @@ -643,14 +654,7 @@ void ChartView::drawBackground(QPainter *painter, const QRectF &rect) { } void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { - // draw time line - qreal x = chart()->mapToPosition(QPointF{cur_sec, 0}).x(); - x = std::clamp(x, chart()->plotArea().left(), chart()->plotArea().right()); - qreal y1 = chart()->plotArea().top() - 2; - qreal y2 = chart()->plotArea().bottom() + 2; - painter->setPen(QPen(chart()->titleBrush().color(), 2)); - painter->drawLine(QPointF{x, y1}, QPointF{x, y2}); - + drawTimeline(painter); // draw track points painter->setPen(Qt::NoPen); qreal track_line_x = -1; @@ -663,16 +667,17 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { } } if (track_line_x > 0) { + auto plot_area = chart()->plotArea(); painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); - painter->drawLine(QPointF{track_line_x, y1}, QPointF{track_line_x, y2}); + painter->drawLine(QPointF{track_line_x, plot_area.top()}, QPointF{track_line_x, plot_area.bottom()}); } // paint points. OpenGL mode lacks certain features (such as showing points) painter->setPen(Qt::NoPen); for (auto &s : sigs) { if (s.series->useOpenGL() && s.series->isVisible() && s.series->pointsVisible()) { - auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan); - auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), xLessThan); + auto first = std::lower_bound(s.vals.cbegin(), s.vals.cend(), axis_x->min(), xLessThan); + auto last = std::lower_bound(first, s.vals.cend(), axis_x->max(), xLessThan); painter->setBrush(s.series->color()); for (auto it = first; it != last; ++it) { painter->drawEllipse(chart()->mapToPosition(*it), 4, 4); @@ -686,9 +691,8 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { painter->setPen(Qt::white); auto rubber_rect = rubber->geometry().normalized(); for (const auto &pt : {rubber_rect.bottomLeft(), rubber_rect.bottomRight()}) { - QString sec = QString::number(chart()->mapToValue(pt).x(), 'f', 1); - // ChartAxisElement's padding is 4 (https://codebrowser.dev/qt5/qtcharts/src/charts/axis/chartaxiselement_p.h.html) - auto r = painter->fontMetrics().boundingRect(sec).adjusted(-6, -4, 6, 4); + QString sec = QString::number(chart()->mapToValue(pt).x(), 'f', 2); + auto r = painter->fontMetrics().boundingRect(sec).adjusted(-6, -AXIS_X_TOP_MARGIN, 6, AXIS_X_TOP_MARGIN); pt == rubber_rect.bottomLeft() ? r.moveTopRight(pt + QPoint{0, 2}) : r.moveTopLeft(pt + QPoint{0, 2}); painter->fillRect(r, Qt::gray); painter->drawText(r, Qt::AlignCenter, sec); @@ -696,6 +700,48 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { } } +void ChartView::drawTimeline(QPainter *painter) { + const auto plot_area = chart()->plotArea(); + // draw line + qreal x = std::clamp(chart()->mapToPosition(QPointF{cur_sec, 0}).x(), plot_area.left(), plot_area.right()); + painter->setPen(QPen(chart()->titleBrush().color(), 2)); + painter->drawLine(QPointF{x, plot_area.top()}, QPointF{x, plot_area.bottom() + 1}); + + // draw current time + QString time_str = QString::number(cur_sec, 'f', 2); + QSize time_str_size = QFontMetrics(axis_x->labelsFont()).size(Qt::TextSingleLine, time_str) + QSize(8, 2); + QRect time_str_rect(QPoint(x - time_str_size.width() / 2, plot_area.bottom() + AXIS_X_TOP_MARGIN), time_str_size); + QPainterPath path; + path.addRoundedRect(time_str_rect, 3, 3); + painter->fillPath(path, settings.theme == DARK_THEME ? Qt::darkGray : Qt::gray); + painter->setPen(palette().color(QPalette::BrightText)); + painter->setFont(axis_x->labelsFont()); + painter->drawText(time_str_rect, Qt::AlignCenter, time_str); + + // draw signal value + auto item_group = qgraphicsitem_cast(chart()->legend()->childItems()[0]); + assert(item_group != nullptr); + auto legend_markers = item_group->childItems(); + assert(legend_markers.size() == sigs.size()); + + painter->setFont(signal_value_font); + painter->setPen(chart()->legend()->labelColor()); + int i = 0; + for (auto &s : sigs) { + QString value = "--"; + if (s.series->isVisible()) { + auto it = std::lower_bound(s.vals.crbegin(), s.vals.crend(), cur_sec, [](auto &p, double x) { return p.x() > x; }); + if (it != s.vals.crend() && it->x() >= axis_x->min()) { + value = s.sig->formatValue(it->y()); + } + } + QRectF marker_rect = legend_markers[i++]->sceneBoundingRect(); + QRectF value_rect(marker_rect.bottomLeft() - QPoint(0, 1), marker_rect.size()); + QString elided_val = painter->fontMetrics().elidedText(value, Qt::ElideRight, value_rect.width()); + painter->drawText(value_rect, Qt::AlignHCenter | Qt::AlignTop, elided_val); + } +} + QXYSeries *ChartView::createSeries(SeriesType type, QColor color) { QXYSeries *series = nullptr; if (type == SeriesType::Line) { diff --git a/tools/cabana/chart/chart.h b/tools/cabana/chart/chart.h index fda9271560e995..3bb191e5cdda7d 100644 --- a/tools/cabana/chart/chart.h +++ b/tools/cabana/chart/chart.h @@ -27,7 +27,7 @@ class ChartView : public QChartView { ChartView(const std::pair &x_range, ChartsWidget *parent = nullptr); void addSignal(const MessageId &msg_id, const cabana::Signal *sig); bool hasSignal(const MessageId &msg_id, const cabana::Signal *sig) const; - void updateSeries(const cabana::Signal *sig = nullptr); + void updateSeries(const cabana::Signal *sig = nullptr, bool clear = true); void updatePlot(double cur, double min, double max); void setSeriesType(SeriesType type); void updatePlotArea(int left, bool force = false); @@ -80,6 +80,7 @@ private slots: void drawForeground(QPainter *painter, const QRectF &rect) override; void drawBackground(QPainter *painter, const QRectF &rect) override; void drawDropIndicator(bool draw) { if (std::exchange(can_drop, draw) != can_drop) viewport()->update(); } + void drawTimeline(QPainter *painter); std::tuple getNiceAxisNumbers(qreal min, qreal max, int tick_count); qreal niceNumber(qreal x, bool ceiling); QXYSeries *createSeries(SeriesType type, QColor color); @@ -104,6 +105,7 @@ private slots: QPixmap chart_pixmap; bool can_drop = false; double tooltip_x = -1; + QFont signal_value_font; ChartsWidget *charts_widget; friend class ChartsWidget; }; diff --git a/tools/cabana/chart/chartswidget.cc b/tools/cabana/chart/chartswidget.cc index ffb354aa3a532a..d244b0c6cc758a 100644 --- a/tools/cabana/chart/chartswidget.cc +++ b/tools/cabana/chart/chartswidget.cc @@ -60,18 +60,17 @@ ChartsWidget::ChartsWidget(QWidget *parent) : align_timer(this), auto_scroll_tim reset_zoom_action = toolbar->addWidget(reset_zoom_btn = new ToolButton("zoom-out", tr("Reset Zoom"))); reset_zoom_btn->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); - toolbar->addWidget(remove_all_btn = new ToolButton("x", tr("Remove all charts"))); + toolbar->addWidget(remove_all_btn = new ToolButton("x-square", tr("Remove all charts"))); toolbar->addWidget(dock_btn = new ToolButton("")); main_layout->addWidget(toolbar); // tabbar - tabbar = new QTabBar(this); + tabbar = new TabBar(this); tabbar->setAutoHide(true); tabbar->setExpanding(false); tabbar->setDrawBase(true); tabbar->setAcceptDrops(true); tabbar->setChangeCurrentOnDrag(true); - tabbar->setTabsClosable(true); tabbar->setUsesScrollButtons(true); main_layout->addWidget(tabbar); @@ -150,8 +149,9 @@ void ChartsWidget::updateTabBar() { void ChartsWidget::eventsMerged() { QFutureSynchronizer future_synchronizer; + bool clear = !can->liveStreaming(); for (auto c : charts) { - future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr)); + future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr, clear)); } } @@ -188,7 +188,7 @@ void ChartsWidget::updateState() { if (pos < 0 || pos > 0.8) { display_range.first = std::max(0.0, cur_sec - max_chart_range * 0.1); } - double max_sec = std::min(std::floor(display_range.first + max_chart_range), can->lastEventSecond()); + double max_sec = std::min(std::floor(display_range.first + max_chart_range), can->totalSeconds()); display_range.first = std::max(0.0, max_sec - max_chart_range); display_range.second = display_range.first + max_chart_range; } else if (cur_sec < (zoomed_range.first - 0.1) || cur_sec >= zoomed_range.second) { @@ -264,6 +264,7 @@ void ChartsWidget::showChart(const MessageId &id, const cabana::Signal *sig, boo if (show && !chart) { chart = merge && currentCharts().size() > 0 ? currentCharts().front() : createChart(); chart->addSignal(id, sig); + updateState(); } else if (!show && chart) { chart->removeIf([&](auto &s) { return s.msg_id == id && s.sig == sig; }); } diff --git a/tools/cabana/chart/chartswidget.h b/tools/cabana/chart/chartswidget.h index 25e5b8e1941987..ee0c07292da6ce 100644 --- a/tools/cabana/chart/chartswidget.h +++ b/tools/cabana/chart/chartswidget.h @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -95,7 +94,7 @@ public slots: ToolButton *remove_all_btn; QList charts; std::unordered_map> tab_charts; - QTabBar *tabbar; + TabBar *tabbar; ChartsContainer *charts_container; QScrollArea *charts_scroll; uint32_t max_chart_range = 0; diff --git a/tools/cabana/chart/tiplabel.cc b/tools/cabana/chart/tiplabel.cc index 3ee5f00978b183..f34d7e8dfed012 100644 --- a/tools/cabana/chart/tiplabel.cc +++ b/tools/cabana/chart/tiplabel.cc @@ -9,6 +9,9 @@ TipLabel::TipLabel(QWidget *parent) : QLabel(parent, Qt::ToolTip | Qt::FramelessWindowHint) { setForegroundRole(QPalette::ToolTipText); setBackgroundRole(QPalette::ToolTipBase); + QFont font; + font.setPointSizeF(8.34563465); + setFont(font); auto palette = QToolTip::palette(); if (settings.theme != DARK_THEME) { palette.setColor(QPalette::ToolTipBase, QApplication::palette().color(QPalette::Base)); @@ -27,9 +30,9 @@ void TipLabel::showText(const QPoint &pt, const QString &text, QWidget *w, const if (!text.isEmpty()) { QSize extra(1, 1); resize(sizeHint() + extra); - QPoint tip_pos(pt.x() + 12, rect.top() + 2); + QPoint tip_pos(pt.x() + 8, rect.top() + 2); if (tip_pos.x() + size().width() >= rect.right()) { - tip_pos.rx() = pt.x() - size().width() - 12; + tip_pos.rx() = pt.x() - size().width() - 8; } if (rect.contains({tip_pos, size()})) { move(w->mapToGlobal(tip_pos)); diff --git a/tools/cabana/dbc/dbc.cc b/tools/cabana/dbc/dbc.cc index 46302ad789b475..ac91922e014abc 100644 --- a/tools/cabana/dbc/dbc.cc +++ b/tools/cabana/dbc/dbc.cc @@ -13,10 +13,45 @@ std::vector cabana::Msg::getSignals() const { return ret; } +void cabana::Msg::updateMask() { + mask = QVector(size, 0x00).toList(); + for (auto &sig : sigs) { + int i = sig.msb / 8; + int bits = sig.size; + while (i >= 0 && i < size && bits > 0) { + int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i * 8; + int msb = (int)(sig.msb / 8) == i ? sig.msb : (i + 1) * 8 - 1; + + int sz = msb - lsb + 1; + int shift = (lsb - (i * 8)); + + mask[i] |= ((1ULL << sz) - 1) << shift; + + bits -= size; + i = sig.is_little_endian ? i - 1 : i + 1; + } + } +} + void cabana::Signal::updatePrecision() { precision = std::max(num_decimals(factor), num_decimals(offset)); } +QString cabana::Signal::formatValue(double value) const { + // Show enum string + for (auto &[val, desc] : val_desc) { + if (std::abs(value - val.toInt()) < 1e-6) { + return desc; + } + } + + QString val_str = QString::number(value, 'f', precision); + if (!unit.isEmpty()) { + val_str += " " + unit; + } + return val_str; +} + // helper functions static QVector BIG_ENDIAN_START_BITS = []() { @@ -78,5 +113,3 @@ std::pair getSignalRange(const cabana::Signal *s) { int to = from + s->size - 1; return {from, to}; } - -std::vector allDBCNames() { return get_dbc_names(); } diff --git a/tools/cabana/dbc/dbc.h b/tools/cabana/dbc/dbc.h index 8b1e5a16e9370b..7daa258ecd210d 100644 --- a/tools/cabana/dbc/dbc.h +++ b/tools/cabana/dbc/dbc.h @@ -1,9 +1,7 @@ #pragma once -#include #include #include -#include #include #include "opendbc/can/common_dbc.h" @@ -57,13 +55,18 @@ namespace cabana { ValueDescription val_desc; int precision = 0; void updatePrecision(); + QString formatValue(double value) const; }; struct Msg { + uint32_t address; QString name; uint32_t size; QList sigs; + QList mask; + void updateMask(); + std::vector getSignals() const; const cabana::Signal *sig(const QString &sig_name) const { auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.name == sig_name; }); @@ -81,4 +84,4 @@ int bigEndianStartBitsIndex(int start_bit); int bigEndianBitIndex(int index); void updateSigSizeParamsFromRange(cabana::Signal &s, int start_bit, int size); std::pair getSignalRange(const cabana::Signal *s); -std::vector allDBCNames(); +inline std::vector allDBCNames() { return get_dbc_names(); } diff --git a/tools/cabana/dbc/dbcfile.cc b/tools/cabana/dbc/dbcfile.cc index 0534ce39ff717c..fd20c0af193693 100644 --- a/tools/cabana/dbc/dbcfile.cc +++ b/tools/cabana/dbc/dbcfile.cc @@ -1,26 +1,21 @@ #include "tools/cabana/dbc/dbcfile.h" -#include - #include #include #include #include -#include #include +#include #include - DBCFile::DBCFile(const QString &dbc_file_name, QObject *parent) : QObject(parent) { QFile file(dbc_file_name); if (file.open(QIODevice::ReadOnly)) { name_ = QFileInfo(dbc_file_name).baseName(); - + filename = dbc_file_name; // Remove auto save file extension if (dbc_file_name.endsWith(AUTO_SAVE_EXTENSION)) { - filename = dbc_file_name.left(dbc_file_name.length() - AUTO_SAVE_EXTENSION.length()); - } else { - filename = dbc_file_name; + filename.chop(AUTO_SAVE_EXTENSION.length()); } open(file.readAll()); } else { @@ -39,6 +34,7 @@ void DBCFile::open(const QString &content) { msgs.clear(); for (auto &msg : dbc->msgs) { auto &m = msgs[msg.address]; + m.address = msg.address; m.name = msg.name.c_str(); m.size = msg.size; for (auto &s : msg.sigs) { @@ -55,6 +51,7 @@ void DBCFile::open(const QString &content) { sig.is_little_endian = s.is_little_endian; sig.updatePrecision(); } + m.updateMask(); } parseExtraInfo(content); @@ -66,9 +63,8 @@ bool DBCFile::save() { if (writeContents(filename)) { cleanupAutoSaveFile(); return true; - } else { - return false; } + return false; } bool DBCFile::saveAs(const QString &new_filename) { @@ -77,11 +73,7 @@ bool DBCFile::saveAs(const QString &new_filename) { } bool DBCFile::autoSave() { - if (!filename.isEmpty()) { - return writeContents(filename + AUTO_SAVE_EXTENSION); - } else { - return false; - } + return !filename.isEmpty() && writeContents(filename + AUTO_SAVE_EXTENSION); } void DBCFile::cleanupAutoSaveFile() { @@ -95,17 +87,16 @@ bool DBCFile::writeContents(const QString &fn) { if (file.open(QIODevice::WriteOnly)) { file.write(generateDBC().toUtf8()); return true; - } else { - return false; } + return false; } cabana::Signal *DBCFile::addSignal(const MessageId &id, const cabana::Signal &sig) { if (auto m = const_cast(msg(id.address))) { m->sigs.push_back(sig); + m->updateMask(); return &m->sigs.last(); } - return nullptr; } @@ -113,34 +104,31 @@ cabana::Signal *DBCFile::addSignal(const MessageId &id, const cabana::Signal &si if (auto m = const_cast(msg(id))) { if (auto s = (cabana::Signal *)m->sig(sig_name)) { *s = sig; + m->updateMask(); return s; } } - return nullptr; } cabana::Signal *DBCFile::getSignal(const MessageId &id, const QString &sig_name) { - if (auto m = const_cast(msg(id))) { - auto it = std::find_if(m->sigs.begin(), m->sigs.end(), [&](auto &s) { return s.name == sig_name; }); - if (it != m->sigs.end()) { - return &(*it); - } - } - return nullptr; - } + auto m = msg(id); + return m ? (cabana::Signal *)m->sig(sig_name) : nullptr; +} void DBCFile::removeSignal(const MessageId &id, const QString &sig_name) { if (auto m = const_cast(msg(id))) { auto it = std::find_if(m->sigs.begin(), m->sigs.end(), [&](auto &s) { return s.name == sig_name; }); if (it != m->sigs.end()) { m->sigs.erase(it); + m->updateMask(); } } } void DBCFile::updateMsg(const MessageId &id, const QString &name, uint32_t size) { auto &m = msgs[id.address]; + m.address = id.address; m.name = name; m.size = size; } @@ -158,21 +146,16 @@ QString DBCFile::newSignalName(const MessageId &id) { assert(m != nullptr); QString name; - for (int i = 1; /**/; ++i) { name = QString("NEW_SIGNAL_%1").arg(i); if (m->sig(name) == nullptr) break; } - return name; } -std::map DBCFile::getMessages() { - return msgs; -} - -const cabana::Msg *DBCFile::msg(const MessageId &id) const { - return msg(id.address); +const QList& DBCFile::mask(const MessageId &id) const { + auto m = msg(id); + return m ? m->mask : empty_mask; } const cabana::Msg *DBCFile::msg(uint32_t address) const { @@ -181,16 +164,10 @@ const cabana::Msg *DBCFile::msg(uint32_t address) const { } const cabana::Msg* DBCFile::msg(const QString &name) { - for (auto &[_, msg] : msgs) { - if (msg.name == name) { - return &msg; - } - } - - return nullptr; + auto it = std::find_if(msgs.cbegin(), msgs.cend(), [&name](auto &m) { return m.second.name == name; }); + return it != msgs.cend() ? &(it->second) : nullptr; } - QStringList DBCFile::signalNames() const { // Used for autocompletion QStringList ret; @@ -210,23 +187,7 @@ int DBCFile::signalCount(const MessageId &id) const { } int DBCFile::signalCount() const { - int total = 0; - for (auto const& [_, msg] : msgs) { - total += msg.sigs.size(); - } - return total; -} - -int DBCFile::msgCount() const { - return msgs.size(); -} - -QString DBCFile::name() const { - return name_.isEmpty() ? "untitled" : name_; -} - -bool DBCFile::isEmpty() const { - return (signalCount() == 0) && name_.isEmpty(); + return std::accumulate(msgs.cbegin(), msgs.cend(), 0, [](int &n, const auto &m) { return n + m.second.sigs.size(); }); } void DBCFile::parseExtraInfo(const QString &content) { diff --git a/tools/cabana/dbc/dbcfile.h b/tools/cabana/dbc/dbcfile.h index e048dc58397a96..9f140f44d90233 100644 --- a/tools/cabana/dbc/dbcfile.h +++ b/tools/cabana/dbc/dbcfile.h @@ -2,17 +2,13 @@ #include #include -#include #include #include -#include -#include #include "tools/cabana/dbc/dbc.h" const QString AUTO_SAVE_EXTENSION = ".tmp"; - class DBCFile : public QObject { Q_OBJECT @@ -41,16 +37,19 @@ class DBCFile : public QObject { QString newMsgName(const MessageId &id); QString newSignalName(const MessageId &id); - std::map getMessages(); - const cabana::Msg *msg(const MessageId &id) const; + const QList& mask(const MessageId &id) const; + + inline std::map getMessages() const { return msgs; } const cabana::Msg *msg(uint32_t address) const; const cabana::Msg* msg(const QString &name); + inline const cabana::Msg *msg(const MessageId &id) const { return msg(id.address); }; + QStringList signalNames() const; int signalCount(const MessageId &id) const; int signalCount() const; - int msgCount() const; - QString name() const; - bool isEmpty() const; + inline int msgCount() const { return msgs.size(); } + inline QString name() const { return name_.isEmpty() ? "untitled" : name_; } + inline bool isEmpty() const { return (signalCount() == 0) && name_.isEmpty(); } QString filename; @@ -58,4 +57,5 @@ class DBCFile : public QObject { void parseExtraInfo(const QString &content); std::map msgs; QString name_; + QList empty_mask; }; diff --git a/tools/cabana/dbc/dbcmanager.cc b/tools/cabana/dbc/dbcmanager.cc index 390ded8512ad1b..9667dd8f2add03 100644 --- a/tools/cabana/dbc/dbcmanager.cc +++ b/tools/cabana/dbc/dbcmanager.cc @@ -119,6 +119,7 @@ void DBCManager::addSignal(const MessageId &id, const cabana::Signal &sig) { cabana::Signal *s = dbc_file->addSignal(id, sig); if (s != nullptr) { + dbc_sources.insert(id.source); for (uint8_t source : dbc_sources) { emit signalAdded({.source = source, .address = id.address}, s); } @@ -188,6 +189,15 @@ QString DBCManager::newSignalName(const MessageId &id) { return dbc_file->newSignalName(id); } +const QList& DBCManager::mask(const MessageId &id) const { + auto sources_dbc_file = findDBCFile(id); + if (!sources_dbc_file) { + return empty_mask; + } + auto [_, dbc_file] = *sources_dbc_file; + return dbc_file->mask(id); +} + std::map DBCManager::getMessages(uint8_t source) { std::map ret; diff --git a/tools/cabana/dbc/dbcmanager.h b/tools/cabana/dbc/dbcmanager.h index 96d9380dbdb6d8..699c280b4c4711 100644 --- a/tools/cabana/dbc/dbcmanager.h +++ b/tools/cabana/dbc/dbcmanager.h @@ -15,6 +15,7 @@ typedef QSet SourceSet; const SourceSet SOURCE_ALL = {}; +const int INVALID_SOURCE = 0xff; class DBCManager : public QObject { Q_OBJECT @@ -39,6 +40,8 @@ class DBCManager : public QObject { QString newMsgName(const MessageId &id); QString newSignalName(const MessageId &id); + const QList& mask(const MessageId &id) const; + std::map getMessages(uint8_t source); const cabana::Msg *msg(const MessageId &id) const; const cabana::Msg* msg(uint8_t source, const QString &name); @@ -57,6 +60,7 @@ class DBCManager : public QObject { private: SourceSet sources; + QList empty_mask; public slots: void updateSources(const SourceSet &s); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index dd12d4c8a34d95..0feb4642f4b2a3 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -13,8 +13,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart main_layout->setContentsMargins(0, 0, 0, 0); // tabbar - tabbar = new QTabBar(this); - tabbar->setTabsClosable(true); + tabbar = new TabBar(this); tabbar->setUsesScrollButtons(true); tabbar->setAutoHide(true); tabbar->setContextMenuPolicy(Qt::CustomContextMenu); @@ -130,7 +129,9 @@ void DetailWidget::refresh() { QStringList warnings; auto msg = dbc()->msg(msg_id); if (msg) { - if (msg->size != can->lastMessage(msg_id).dat.size()) { + if (msg_id.source == INVALID_SOURCE) { + warnings.push_back(tr("No messages received.")); + } else if (msg->size != can->lastMessage(msg_id).dat.size()) { warnings.push_back(tr("Message size (%1) is incorrect.").arg(msg->size)); } for (auto s : binary_view->getOverlappingSignals()) { diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 074ce3b4d34639..ce65adb34ea168 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -41,7 +41,7 @@ class DetailWidget : public QWidget { QLabel *time_label, *warning_icon, *warning_label; ElidedLabel *name_label; QWidget *warning_widget; - QTabBar *tabbar; + TabBar *tabbar; QTabWidget *tab_widget; QToolButton *remove_btn; LogsWidget *history_log; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 4c193666dc99a4..13e8f70a8f7cde 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -138,6 +138,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I } std::deque HistoryLogModel::fetchData(uint64_t from_time, uint64_t min_time) { + const QList mask; const auto &events = can->events(msg_id); const auto freq = can->lastMessage(msg_id).freq; const bool update_colors = !display_signals_mode || sigs.empty(); @@ -150,7 +151,7 @@ std::deque HistoryLogModel::fetchData(uint64_t from_ti auto msgs = fetchData(first, events.rend(), min_time); if (update_colors && (min_time > 0 || messages.empty())) { for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { - hex_colors.compute(it->data.data(), it->data.size(), it->mono_time / (double)1e9, speed, freq); + hex_colors.compute(it->data.data(), it->data.size(), it->mono_time / (double)1e9, speed, mask, freq); it->colors = hex_colors.colors; } } @@ -163,7 +164,7 @@ std::deque HistoryLogModel::fetchData(uint64_t from_ti auto msgs = fetchData(first, events.cend(), 0); if (update_colors) { for (auto it = msgs.begin(); it != msgs.end(); ++it) { - hex_colors.compute(it->data.data(), it->data.size(), it->mono_time / (double)1e9, speed, freq); + hex_colors.compute(it->data.data(), it->data.size(), it->mono_time / (double)1e9, speed, mask, freq); it->colors = hex_colors.colors; } } diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 891bfc4bb13087..7b2d23a3f9ebb1 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -19,6 +19,7 @@ #include "tools/cabana/commands.h" #include "tools/cabana/streamselector.h" #include "tools/cabana/streams/replaystream.h" +#include "tools/cabana/tools/findsignal.h" static MainWindow *main_win = nullptr; void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { @@ -55,12 +56,9 @@ MainWindow::MainWindow() : QMainWindow() { main_win = this; qInstallMessageHandler(qLogMessageHandler); - for (const QString &fn : {"./dbc/car_fingerprint_to_dbc.json", "./tools/cabana/dbc/car_fingerprint_to_dbc.json"}) { - QFile json_file(fn); - if (json_file.open(QIODevice::ReadOnly)) { - fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); - break; - } + QFile json_file(QApplication::applicationDirPath() + "/dbc/car_fingerprint_to_dbc.json"); + if (json_file.open(QIODevice::ReadOnly)) { + fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); } setStyleSheet(QString(R"(QMainWindow::separator { @@ -72,11 +70,10 @@ MainWindow::MainWindow() : QMainWindow() { QObject::connect(this, &MainWindow::updateProgressBar, this, &MainWindow::updateDownloadProgress); QObject::connect(messages_widget, &MessagesWidget::msgSelectionChanged, center_widget, &CenterWidget::setMessage); QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); - QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::loadDBCFromFingerprint); - QObject::connect(can, &AbstractStream::eventsMerged, this, &MainWindow::updateStatus); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); + QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::streamStarted); QObject::connect(can, &AbstractStream::sourcesUpdated, dbc(), &DBCManager::updateSources); - QObject::connect(can, &AbstractStream::sourcesUpdated, this, &MainWindow::updateSources); + QObject::connect(can, &AbstractStream::sourcesUpdated, this, &MainWindow::updateLoadSaveMenus); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); QObject::connect(UndoStack::instance(), &QUndoStack::cleanChanged, this, &MainWindow::undoStackCleanChanged); QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &MainWindow::undoStackIndexChanged); QObject::connect(&settings, &Settings::changed, this, &MainWindow::updateStatus); @@ -89,8 +86,8 @@ void MainWindow::createActions() { file_menu->addSeparator(); } - file_menu->addAction(tr("New DBC File"), this, &MainWindow::newFile)->setShortcuts(QKeySequence::New); - file_menu->addAction(tr("Open DBC File..."), this, &MainWindow::openFile)->setShortcuts(QKeySequence::Open); + file_menu->addAction(tr("New DBC File"), [this]() { newFile(); })->setShortcuts(QKeySequence::New); + file_menu->addAction(tr("Open DBC File..."), [this]() { openFile(); })->setShortcuts(QKeySequence::Open); manage_dbcs_menu = file_menu->addMenu(tr("Manage &DBC Files")); @@ -109,7 +106,8 @@ void MainWindow::createActions() { auto dbc_names = allDBCNames(); std::sort(dbc_names.begin(), dbc_names.end()); for (const auto &name : dbc_names) { - load_opendbc_menu->addAction(QString::fromStdString(name), this, &MainWindow::openOpendbcFile); + QString dbc_name = QString::fromStdString(name); + load_opendbc_menu->addAction(dbc_name, [=]() { loadDBCFromOpendbc(dbc_name); }); } file_menu->addAction(tr("Load DBC From Clipboard"), [=]() { loadFromClipboard(); }); @@ -139,14 +137,13 @@ void MainWindow::createActions() { edit_menu->addSeparator(); QMenu *commands_menu = edit_menu->addMenu(tr("Command &List")); - auto undo_view = new QUndoView(UndoStack::instance()); - undo_view->setWindowTitle(tr("Command List")); QWidgetAction *commands_act = new QWidgetAction(this); - commands_act->setDefaultWidget(undo_view); + commands_act->setDefaultWidget(new QUndoView(UndoStack::instance())); commands_menu->addAction(commands_act); QMenu *tools_menu = menuBar()->addMenu(tr("&Tools")); tools_menu->addAction(tr("Find &Similar Bits"), this, &MainWindow::findSimilarBits); + tools_menu->addAction(tr("&Find Signal"), this, &MainWindow::findSignal); QMenu *help_menu = menuBar()->addMenu(tr("&Help")); help_menu->addAction(tr("Help"), this, &MainWindow::onlineHelp)->setShortcuts(QKeySequence::HelpContents); @@ -257,39 +254,24 @@ void MainWindow::openRoute() { } } -void MainWindow::newFile() { - remindSaveChanges(); - dbc()->closeAll(); - dbc()->open(SOURCE_ALL, "", ""); - updateLoadSaveMenus(); +void MainWindow::newFile(SourceSet s) { + closeFile(s); + dbc()->open(s, "", ""); } -void MainWindow::openFile() { +void MainWindow::openFile(SourceSet s) { remindSaveChanges(); QString fn = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); if (!fn.isEmpty()) { - loadFile(fn); + loadFile(fn, s); } } -void MainWindow::openFileForSource(SourceSet s) { - QString fn = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); +void MainWindow::loadFile(const QString &fn, SourceSet s) { if (!fn.isEmpty()) { - loadFile(fn, s, false); - } -} + closeFile(s); -void MainWindow::newFileForSource(SourceSet s) { - remindSaveChanges(); - - dbc()->close(s); - dbc()->open(s, "", ""); -} - -void MainWindow::loadFile(const QString &fn, SourceSet s, bool close_all) { - if (!fn.isEmpty()) { QString dbc_fn = fn; - // Prompt user to load auto saved file if it exists. if (QFile::exists(fn + AUTO_SAVE_EXTENSION)) { auto ret = QMessageBox::question(this, tr("Auto saved DBC found"), tr("Auto saved DBC file from previous session found. Do you want to load it instead?")); @@ -299,16 +281,8 @@ void MainWindow::loadFile(const QString &fn, SourceSet s, bool close_all) { } } - auto dbc_name = QFileInfo(fn).baseName(); QString error; - - if (close_all) { - dbc()->closeAll(); - } - - dbc()->close(s); - bool ret = dbc()->open(s, dbc_fn, &error); - if (ret) { + if (dbc()->open(s, dbc_fn, &error)) { updateRecentFiles(fn); statusBar()->showMessage(tr("DBC File %1 loaded").arg(fn), 2000); } else { @@ -316,79 +290,54 @@ void MainWindow::loadFile(const QString &fn, SourceSet s, bool close_all) { msg_box.setDetailedText(error); msg_box.exec(); } - - updateLoadSaveMenus(); - } -} - -void MainWindow::openOpendbcFile() { - if (auto action = qobject_cast(sender())) { - remindSaveChanges(); - loadDBCFromOpendbc(action->text()); } } void MainWindow::openRecentFile() { if (auto action = qobject_cast(sender())) { - remindSaveChanges(); loadFile(action->data().toString()); } } void MainWindow::loadDBCFromOpendbc(const QString &name) { - remindSaveChanges(); - QString opendbc_file_path = QString("%1/%2.dbc").arg(OPENDBC_FILE_PATH, name); - - dbc()->closeAll(); - dbc()->open(SOURCE_ALL, opendbc_file_path); - - updateLoadSaveMenus(); + loadFile(opendbc_file_path); } void MainWindow::loadFromClipboard(SourceSet s, bool close_all) { - remindSaveChanges(); + closeFile(s); + QString dbc_str = QGuiApplication::clipboard()->text(); QString error; - - if (close_all) { - dbc()->closeAll(); - } - - dbc()->close(s); bool ret = dbc()->open(s, "", dbc_str, &error); if (ret && dbc()->msgCount() > 0) { QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!")); } else { QMessageBox msg_box(QMessageBox::Warning, tr("Failed to load DBC from clipboard"), tr("Make sure that you paste the text with correct format.")); - if (!error.isEmpty()) { - msg_box.setDetailedText(error); - } + msg_box.setDetailedText(error); msg_box.exec(); } } -void MainWindow::loadDBCFromFingerprint() { - // Don't overwrite already loaded DBC - if (dbc()->msgCount()) { - return; - } - - remindSaveChanges(); +void MainWindow::streamStarted() { auto fingerprint = can->carFingerprint(); if (can->liveStreaming()) { video_dock->setWindowTitle(can->routeName()); } else { video_dock->setWindowTitle(tr("ROUTE: %1 FINGERPRINT: %2").arg(can->routeName()).arg(fingerprint.isEmpty() ? tr("Unknown Car") : fingerprint)); } - if (!fingerprint.isEmpty()) { - auto dbc_name = fingerprint_to_dbc[fingerprint]; - if (dbc_name != QJsonValue::Undefined) { - loadDBCFromOpendbc(dbc_name.toString()); - return; + + // Don't overwrite already loaded DBC + if (!dbc()->msgCount()) { + if (!fingerprint.isEmpty()) { + auto dbc_name = fingerprint_to_dbc[fingerprint]; + if (dbc_name != QJsonValue::Undefined) { + loadDBCFromOpendbc(dbc_name.toString()); + return; + } } + newFile(); } - newFile(); } void MainWindow::save() { @@ -407,7 +356,6 @@ void MainWindow::saveAs() { } } - void MainWindow::autoSave() { if (!UndoStack::instance()->isClean()) { for (auto &[_, dbc_file] : dbc()->dbc_files) { @@ -424,12 +372,19 @@ void MainWindow::cleanupAutoSaveFile() { } } +void MainWindow::closeFile(SourceSet s) { + remindSaveChanges(); + if (s == SOURCE_ALL) { + dbc()->closeAll(); + } else { + dbc()->close(s); + } +} + void MainWindow::closeFile(DBCFile *dbc_file) { assert(dbc_file != nullptr); remindSaveChanges(); - dbc()->close(dbc_file); - // Ensure we always have at least one file open if (dbc()->dbcCount() == 0) { newFile(); @@ -438,43 +393,21 @@ void MainWindow::closeFile(DBCFile *dbc_file) { void MainWindow::saveFile(DBCFile *dbc_file) { assert(dbc_file != nullptr); - - SourceSet s; - for (auto &[s_, dbc_file_] : dbc()->dbc_files) { - if (dbc_file_ == dbc_file) { - s = s_; - break; - } - } - if (!dbc_file->filename.isEmpty()) { dbc_file->save(); - updateRecentFiles(dbc_file->filename); + updateLoadSaveMenus(); } else if (!dbc_file->isEmpty()) { - QString fn = QFileDialog::getSaveFileName(this, tr("Save File (bus: %1)").arg(toString(s)), QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); - if (!fn.isEmpty()) { - dbc_file->saveAs(fn); - updateRecentFiles(fn); - } + saveFileAs(dbc_file); } - UndoStack::instance()->setClean(); statusBar()->showMessage(tr("File saved"), 2000); - updateLoadSaveMenus(); } void MainWindow::saveFileAs(DBCFile *dbc_file) { - assert(dbc_file != nullptr); - - SourceSet s; - for (auto &[s_, dbc_file_] : dbc()->dbc_files) { - if (dbc_file_ == dbc_file) { - s = s_; - break; - } - } - - QString fn = QFileDialog::getSaveFileName(this, tr("Save File (bus: %1)").arg(toString(s)), QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); + auto it = std::find_if(dbc()->dbc_files.begin(), dbc()->dbc_files.end(), [=](auto &f) { return f.second == dbc_file; }); + assert(it != dbc()->dbc_files.end()); + QString title = tr("Save File (bus: %1)").arg(toString(it->first)); + QString fn = QFileDialog::getSaveFileName(this, title, QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); if (!fn.isEmpty()) { dbc_file->saveAs(fn); updateRecentFiles(fn); @@ -486,10 +419,8 @@ void MainWindow::removeBusFromFile(DBCFile *dbc_file, uint8_t source) { assert(dbc_file != nullptr); SourceSet ss = {source, uint8_t(source + 128), uint8_t(source + 192)}; dbc()->removeSourcesFromFile(dbc_file, ss); - updateLoadSaveMenus(); } - void MainWindow::saveToClipboard() { // Copy all open DBC files to clipboard. Should not be called with more than 1 file open for (auto &[s, dbc_file] : dbc()->dbc_files) { @@ -504,28 +435,20 @@ void MainWindow::saveFileToClipboard(DBCFile *dbc_file) { QMessageBox::information(this, tr("Copy To Clipboard"), tr("DBC Successfully copied!")); } -void MainWindow::updateSources(const SourceSet &s) { - sources = s; - updateLoadSaveMenus(); -} - void MainWindow::updateLoadSaveMenus() { int cnt = dbc()->nonEmptyDBCCount(); - save_dbc->setEnabled(cnt > 0); - if (cnt > 1) { save_dbc->setText(tr("Save %1 DBCs...").arg(dbc()->dbcCount())); } else { save_dbc->setText(tr("Save DBC...")); } - + save_dbc->setEnabled(cnt > 0); save_dbc_as->setEnabled(cnt == 1); // TODO: Support clipboard for multiple files copy_dbc_to_clipboard->setEnabled(cnt == 1); - - QList sources_sorted = sources.toList(); + QList sources_sorted = can->sources.toList(); std::sort(sources_sorted.begin(), sources_sorted.end()); manage_dbcs_menu->clear(); @@ -536,24 +459,9 @@ void MainWindow::updateLoadSaveMenus() { SourceSet ss = {source, uint8_t(source + 128), uint8_t(source + 192)}; QMenu *bus_menu = new QMenu(this); - - // New - QAction *new_action = new QAction(this); - new_action->setText(tr("New DBC File...")); - QObject::connect(new_action, &QAction::triggered, [=]() { newFileForSource(ss); }); - bus_menu->addAction(new_action); - - // Open - QAction *open_action = new QAction(this); - open_action->setText(tr("Open DBC File...")); - QObject::connect(open_action, &QAction::triggered, [=]() { openFileForSource(ss); }); - bus_menu->addAction(open_action); - - // Open - QAction *load_clipboard_action = new QAction(this); - load_clipboard_action->setText(tr("Load DBC From Clipboard...")); - QObject::connect(load_clipboard_action, &QAction::triggered, [=]() { loadFromClipboard(ss, false); }); - bus_menu->addAction(load_clipboard_action); + bus_menu->addAction(tr("New DBC File..."), [=]() { newFile(ss); }); + bus_menu->addAction(tr("Open DBC File..."), [=]() { openFile(ss); }); + bus_menu->addAction(tr("Load DBC From Clipboard..."), [=]() { loadFromClipboard(ss, false); }); // Show sub-menu for each dbc for this source. QStringList bus_menu_fns; @@ -563,46 +471,15 @@ void MainWindow::updateLoadSaveMenus() { continue; } - QString fn = dbc_file->filename.isEmpty() ? "untitled" : QFileInfo(dbc_file->filename).baseName(); - // QMenu *manage_menu = bus_menu; - bus_menu->addSeparator(); - QAction *fn_action = new QAction(this); - fn_action->setText(fn + " (" + toString(src) + ")"); - fn_action->setEnabled(false); - bus_menu->addAction(fn_action); - - // Save - QAction *save_action = new QAction(this); - save_action->setText(tr("Save...")); - bus_menu->addAction(save_action); - QObject::connect(save_action, &QAction::triggered, [=](){ saveFile(it.second); }); - - // Save as - QAction *save_as_action = new QAction(this); - save_as_action->setText(tr("Save As...")); - bus_menu->addAction(save_as_action); - QObject::connect(save_as_action, &QAction::triggered, [=](){ saveFileAs(it.second); }); - - // Copy to clipboard - QAction *save_clipboard_action = new QAction(this); - save_clipboard_action->setText(tr("Copy to Clipboard...")); - bus_menu->addAction(save_clipboard_action); - QObject::connect(save_clipboard_action, &QAction::triggered, [=](){ saveFileToClipboard(it.second); }); - - // Remove from this bus - QAction *remove_action = new QAction(this); - remove_action->setText(tr("Remove from this bus...")); - bus_menu->addAction(remove_action); - QObject::connect(remove_action, &QAction::triggered, [=](){ removeBusFromFile(it.second, source); }); - - // Close/Remove from all buses - QAction *close_action = new QAction(this); - close_action->setText(tr("Remove from all buses...")); - bus_menu->addAction(close_action); - QObject::connect(close_action, &QAction::triggered, [=](){ closeFile(it.second); }); - - bus_menu_fns << fn; + bus_menu->addAction(dbc_file->name() + " (" + toString(src) + ")")->setEnabled(false); + bus_menu->addAction(tr("Save..."), [=]() { saveFile(it.second); }); + bus_menu->addAction(tr("Save As..."), [=]() { saveFileAs(it.second); }); + bus_menu->addAction(tr("Copy to Clipboard..."), [=]() { saveFileToClipboard(it.second); }); + bus_menu->addAction(tr("Remove from this bus..."), [=]() { removeBusFromFile(it.second, source); }); + bus_menu->addAction(tr("Remove from all buses..."), [=]() { closeFile(it.second); }); + + bus_menu_fns << dbc_file->name(); } manage_dbcs_menu->addMenu(bus_menu); @@ -612,8 +489,7 @@ void MainWindow::updateLoadSaveMenus() { QStringList title; for (auto &[src, dbc_file] : dbc()->dbc_files) { - QString fn = dbc_file->filename.isEmpty() ? "untitled" : QFileInfo(dbc_file->filename).baseName(); - title.push_back(tr("(%1) %2").arg(toString(src)).arg(fn)); + title.push_back(tr("(%1) %2").arg(toString(src), dbc_file->name())); } setWindowFilePath(title.join(" | ")); } @@ -646,9 +522,8 @@ void MainWindow::updateRecentFileActions() { void MainWindow::remindSaveChanges() { bool discard_changes = false; while (!UndoStack::instance()->isClean() && !discard_changes) { - int ret = (QMessageBox::question(this, tr("Unsaved Changes"), - tr("You have unsaved changes. Press ok to save them, cancel to discard."), - QMessageBox::Ok | QMessageBox::Cancel)); + QString text = tr("You have unsaved changes. Press ok to save them, cancel to discard."); + int ret = (QMessageBox::question(this, tr("Unsaved Changes"), text, QMessageBox::Ok | QMessageBox::Cancel)); if (ret == QMessageBox::Ok) { save(); } else { @@ -719,6 +594,12 @@ void MainWindow::findSimilarBits() { dlg->show(); } +void MainWindow::findSignal() { + FindSignalDlg *dlg = new FindSignalDlg(this); + QObject::connect(dlg, &FindSignalDlg::openMessage, messages_widget, &MessagesWidget::selectMessage); + dlg->show(); +} + void MainWindow::onlineHelp() { if (auto help = findChild()) { help->close(); diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index 5e76d86e5ceb02..9db999e6c96d61 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -21,20 +21,18 @@ class MainWindow : public QMainWindow { MainWindow(); void dockCharts(bool dock); void showStatusMessage(const QString &msg, int timeout = 0) { statusBar()->showMessage(msg, timeout); } - void loadFile(const QString &fn, SourceSet s = SOURCE_ALL, bool close_all = true); + void loadFile(const QString &fn, SourceSet s = SOURCE_ALL); public slots: void openRoute(); - void newFile(); - void openFile(); + void newFile(SourceSet s = SOURCE_ALL); + void openFile(SourceSet s = SOURCE_ALL); void openRecentFile(); - void openOpendbcFile(); void loadDBCFromOpendbc(const QString &name); - void loadDBCFromFingerprint(); + void streamStarted(); void save(); void saveAs(); void saveToClipboard(); - void updateSources(const SourceSet &s); signals: void showMessage(const QString &msg, int timeout); @@ -42,14 +40,13 @@ public slots: protected: void remindSaveChanges(); + void closeFile(SourceSet s = SOURCE_ALL); void closeFile(DBCFile *dbc_file); void saveFile(DBCFile *dbc_file); void saveFileAs(DBCFile *dbc_file); void saveFileToClipboard(DBCFile *dbc_file); void removeBusFromFile(DBCFile *dbc_file, uint8_t source); void loadFromClipboard(SourceSet s = SOURCE_ALL, bool close_all = true); - void openFileForSource(SourceSet s); - void newFileForSource(SourceSet s); void autoSave(); void cleanupAutoSaveFile(); void updateRecentFiles(const QString &fn); @@ -63,6 +60,7 @@ public slots: void updateDownloadProgress(uint64_t cur, uint64_t total, bool success); void setOption(); void findSimilarBits(); + void findSignal(); void undoStackCleanChanged(bool clean); void undoStackIndexChanged(int index); void onlineHelp(); @@ -90,7 +88,6 @@ public slots: QAction *copy_dbc_to_clipboard = nullptr; int prev_undostack_index = 0; int prev_undostack_count = 0; - SourceSet sources; friend class OnlineHelp; }; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index e74b8526175f68..60aadb3673aaa5 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,43 +1,53 @@ #include "tools/cabana/messageswidget.h" - #include #include #include +#include #include MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(0 ,0, 0, 0); - // message filter QHBoxLayout *title_layout = new QHBoxLayout(); - title_layout->addWidget(filter = new QLineEdit(this)); - QRegularExpression re("\\S+"); - filter->setValidator(new QRegularExpressionValidator(re, this)); - filter->setClearButtonEnabled(true); - filter->setPlaceholderText(tr("filter messages")); - title_layout->addWidget(multiple_lines_bytes = new QCheckBox(tr("Multiple Lines Bytes"), this)); + num_msg_label = new QLabel(this); + title_layout->addSpacing(10); + title_layout->addWidget(num_msg_label); + + title_layout->addStretch(); + title_layout->addWidget(multiple_lines_bytes = new QCheckBox(tr("Multiple Lines &Bytes"), this)); multiple_lines_bytes->setToolTip(tr("Display bytes in multiple lines")); multiple_lines_bytes->setChecked(settings.multiple_lines_bytes); + QPushButton *clear_filters = new QPushButton(tr("&Clear Filters")); + clear_filters->setEnabled(false); + title_layout->addWidget(clear_filters); main_layout->addLayout(title_layout); // message table view = new MessageView(this); model = new MessageListModel(this); + header = new MessageViewHeader(this, model); auto delegate = new MessageBytesDelegate(view, settings.multiple_lines_bytes); + view->setItemDelegate(delegate); view->setModel(model); view->setSortingEnabled(true); - view->sortByColumn(0, Qt::AscendingOrder); + view->sortByColumn(MessageListModel::Column::NAME, Qt::AscendingOrder); view->setAllColumnsShowFocus(true); view->setEditTriggers(QAbstractItemView::NoEditTriggers); view->setItemsExpandable(false); view->setIndentation(0); view->setRootIsDecorated(false); + view->setHeader(header); + // Must be called before setting any header parameters to avoid overriding restoreHeaderState(settings.message_header_state); - view->header()->setSectionsMovable(true); + view->header()->setSectionResizeMode(MessageListModel::Column::DATA, QHeaderView::Fixed); + + // Header context menu + view->header()->setContextMenuPolicy(Qt::CustomContextMenu); + QObject::connect(view->header(), &QHeaderView::customContextMenuRequested, view, &MessageView::headerContextMenuEvent); main_layout->addWidget(view); @@ -47,21 +57,37 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { suppress_clear = new QPushButton(); suppress_layout->addWidget(suppress_add); suppress_layout->addWidget(suppress_clear); + QCheckBox *suppress_defined_signals = new QCheckBox(tr("Suppress Defined Signals"), this); + suppress_defined_signals->setChecked(settings.suppress_defined_signals); + suppress_layout->addWidget(suppress_defined_signals); main_layout->addLayout(suppress_layout); // signals/slots - QObject::connect(filter, &QLineEdit::textEdited, model, &MessageListModel::setFilterString); + QObject::connect(header, &MessageViewHeader::filtersUpdated, model, &MessageListModel::setFilterStrings); + QObject::connect(header, &MessageViewHeader::filtersUpdated, [=](const QMap &filters) { + clear_filters->setEnabled(!filters.isEmpty()); + }); + QObject::connect(view->horizontalScrollBar(), &QScrollBar::valueChanged, header, &MessageViewHeader::updateHeaderPositions); + QObject::connect(clear_filters, &QPushButton::clicked, header, &MessageViewHeader::clearFilters); QObject::connect(multiple_lines_bytes, &QCheckBox::stateChanged, [=](int state) { settings.multiple_lines_bytes = (state == Qt::Checked); delegate->setMultipleLines(settings.multiple_lines_bytes); view->setUniformRowHeights(!settings.multiple_lines_bytes); - model->sortMessages(); + + // Reset model to force recalculation of the width of the bytes column + model->forceResetModel(); + }); + QObject::connect(suppress_defined_signals, &QCheckBox::stateChanged, [=](int state) { + settings.suppress_defined_signals = (state == Qt::Checked); }); QObject::connect(can, &AbstractStream::msgsReceived, model, &MessageListModel::msgsReceived); QObject::connect(can, &AbstractStream::streamStarted, this, &MessagesWidget::reset); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, model, &MessageListModel::sortMessages); - QObject::connect(dbc(), &DBCManager::msgUpdated, model, &MessageListModel::sortMessages); - QObject::connect(dbc(), &DBCManager::msgRemoved, model, &MessageListModel::sortMessages); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MessagesWidget::dbcModified); + QObject::connect(dbc(), &DBCManager::msgUpdated, this, &MessagesWidget::dbcModified); + QObject::connect(dbc(), &DBCManager::msgRemoved, this, &MessagesWidget::dbcModified); + QObject::connect(dbc(), &DBCManager::signalAdded, this, &MessagesWidget::dbcModified); + QObject::connect(dbc(), &DBCManager::signalRemoved, this, &MessagesWidget::dbcModified); + QObject::connect(dbc(), &DBCManager::signalUpdated, this, &MessagesWidget::dbcModified); QObject::connect(model, &MessageListModel::modelReset, [this]() { if (current_msg_id) { selectMessage(*current_msg_id); @@ -98,9 +124,15 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { )")); } +void MessagesWidget::dbcModified() { + num_msg_label->setText(tr("%1 Messages, %2 Signals").arg(dbc()->msgCount()).arg(dbc()->signalCount())); + model->dbcModified(); +} + void MessagesWidget::selectMessage(const MessageId &msg_id) { - if (int row = model->msgs.indexOf(msg_id); row != -1) { - view->selectionModel()->setCurrentIndex(model->index(row, 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); + auto it = std::find(model->msgs.cbegin(), model->msgs.cend(), msg_id); + if (it != model->msgs.cend()) { + view->selectionModel()->setCurrentIndex(model->index(std::distance(model->msgs.cbegin(), it), 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); } } @@ -118,7 +150,6 @@ void MessagesWidget::reset() { current_msg_id = std::nullopt; view->selectionModel()->clear(); model->reset(); - filter->clear(); updateSuppressedButtons(); } @@ -127,8 +158,14 @@ void MessagesWidget::reset() { QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { - static const QString titles[] = {"Name", "Bus", "ID", "Freq", "Count", "Bytes"}; - return titles[section]; + switch (section) { + case Column::NAME: return tr("Name"); + case Column::SOURCE: return tr("Bus"); + case Column::ADDRESS: return tr("ID"); + case Column::FREQ: return tr("Freq"); + case Column::COUNT: return tr("Count"); + case Column::DATA: return tr("Bytes"); + } } return {}; } @@ -147,12 +184,12 @@ QVariant MessageListModel::data(const QModelIndex &index, int role) const { if (role == Qt::DisplayRole) { switch (index.column()) { - case 0: return msgName(id); - case 1: return id.source; - case 2: return QString::number(id.address, 16); - case 3: return getFreq(can_data); - case 4: return can_data.count; - case 5: return toHex(can_data.dat); + case Column::NAME: return msgName(id); + case Column::SOURCE: return id.source != INVALID_SOURCE ? QString::number(id.source) : "N/A" ; + case Column::ADDRESS: return QString::number(id.address, 16); + case Column::FREQ: return id.source != INVALID_SOURCE ? getFreq(can_data) : "N/A"; + case Column::COUNT: return id.source != INVALID_SOURCE ? QString::number(can_data.count) : "N/A"; + case Column::DATA: return id.source != INVALID_SOURCE ? toHex(can_data.dat) : "N/A"; } } else if (role == ColorsRole) { QVector colors = can_data.colors; @@ -164,83 +201,193 @@ QVariant MessageListModel::data(const QModelIndex &index, int role) const { } } return QVariant::fromValue(colors); - } else if (role == BytesRole && index.column() == 5) { + } else if (role == BytesRole && index.column() == Column::DATA && id.source != INVALID_SOURCE) { return can_data.dat; } return {}; } -void MessageListModel::setFilterString(const QString &string) { - auto contains = [](const MessageId &id, const QString &txt) { - auto cs = Qt::CaseInsensitive; - if (id.toString().contains(txt, cs) || msgName(id).contains(txt, cs)) return true; - // Search by signal name - if (const auto msg = dbc()->msg(id)) { - for (auto s : msg->getSignals()) { - if (s->name.contains(txt, cs)) return true; - } - } - return false; - }; +void MessageListModel::setFilterStrings(const QMap &filters) { + filter_str = filters; + fetchData(); +} - filter_str = string; - msgs.clear(); - for (auto it = can->last_msgs.begin(); it != can->last_msgs.end(); ++it) { - if (filter_str.isEmpty() || contains(it.key(), filter_str)) { - msgs.push_back(it.key()); - } +void MessageListModel::dbcModified() { + dbc_address.clear(); + for (const auto &[_, m] : dbc()->getMessages(0)) { + dbc_address.insert(m.address); } - sortMessages(); + fetchData(); } -void MessageListModel::sortMessages() { - beginResetModel(); - if (sort_column == 0) { - std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { +void MessageListModel::sortMessages(std::vector &new_msgs) { + if (sort_column == Column::NAME) { + std::sort(new_msgs.begin(), new_msgs.end(), [=](auto &l, auto &r) { auto ll = std::pair{msgName(l), l}; auto rr = std::pair{msgName(r), r}; return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); - } else if (sort_column == 1) { - std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { + } else if (sort_column == Column::SOURCE) { + std::sort(new_msgs.begin(), new_msgs.end(), [=](auto &l, auto &r) { auto ll = std::pair{l.source, l}; auto rr = std::pair{r.source, r}; return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); - } else if (sort_column == 2) { - std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { + } else if (sort_column == Column::ADDRESS) { + std::sort(new_msgs.begin(), new_msgs.end(), [=](auto &l, auto &r) { auto ll = std::pair{l.address, l}; auto rr = std::pair{r.address, r}; return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); - } else if (sort_column == 3) { - std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { + } else if (sort_column == Column::FREQ) { + std::sort(new_msgs.begin(), new_msgs.end(), [=](auto &l, auto &r) { auto ll = std::pair{can->lastMessage(l).freq, l}; auto rr = std::pair{can->lastMessage(r).freq, r}; return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); - } else if (sort_column == 4) { - std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { + } else if (sort_column == Column::COUNT) { + std::sort(new_msgs.begin(), new_msgs.end(), [=](auto &l, auto &r) { auto ll = std::pair{can->lastMessage(l).count, l}; auto rr = std::pair{can->lastMessage(r).count, r}; return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } - endResetModel(); } -void MessageListModel::msgsReceived(const QHash *new_msgs) { - int prev_row_count = msgs.size(); - if (filter_str.isEmpty() && msgs.size() != can->last_msgs.size()) { - msgs = can->last_msgs.keys(); +static std::pair parseRange(const QString &filter, bool *ok = nullptr, int base = 10) { + // Parse out filter string into a range (e.g. "1" -> {1, 1}, "1-3" -> {1, 3}, "1-" -> {1, inf}) + bool ok1 = true, ok2 = true; + unsigned int parsed1 = std::numeric_limits::min(); + unsigned int parsed2 = std::numeric_limits::max(); + + auto s = filter.split('-'); + if (s.size() == 1) { + parsed1 = s[0].toUInt(ok, base); + return {parsed1, parsed1}; + } else if (s.size() == 2) { + if (!s[0].isEmpty()) parsed1 = s[0].toUInt(&ok1, base); + if (!s[1].isEmpty()) parsed2 = s[1].toUInt(&ok2, base); + + *ok = ok1 & ok2; + return {parsed1, parsed2}; + } else { + *ok = false; + return {0, 0}; + } +} + +bool MessageListModel::matchMessage(const MessageId &id, const CanData &data, const QMap &filters) { + auto cs = Qt::CaseInsensitive; + bool match = true; + bool convert_ok; + + for (int column = Column::NAME; column <= Column::DATA; column++) { + if (!filters.contains(column)) continue; + const QString &txt = filters[column]; + + QRegularExpression re(txt, QRegularExpression::CaseInsensitiveOption | QRegularExpression::DotMatchesEverythingOption); + + switch (column) { + case Column::NAME: + { + bool name_match = re.match(msgName(id)).hasMatch(); + + // Message signals + if (const auto msg = dbc()->msg(id)) { + for (auto s : msg->getSignals()) { + if (re.match(s->name).hasMatch()) { + name_match = true; + break; + } + } + } + if (!name_match) match = false; + } + break; + case Column::SOURCE: + { + auto source = parseRange(txt, &convert_ok); + bool source_match = convert_ok && (id.source >= source.first && id.source <= source.second); + if (!source_match) match = false; + } + break; + case Column::ADDRESS: + { + bool address_re_match = re.match(QString::number(id.address, 16)).hasMatch(); + + auto address = parseRange(txt, &convert_ok, 16); + bool address_match = convert_ok && (id.address >= address.first && id.address <= address.second); + + if (!address_re_match && !address_match) match = false; + } + break; + case Column::FREQ: + { + // TODO: Hide stale messages? + auto freq = parseRange(txt, &convert_ok); + bool freq_match = convert_ok && (data.freq >= freq.first && data.freq <= freq.second); + if (!freq_match) match = false; + } + break; + case Column::COUNT: + { + auto count = parseRange(txt, &convert_ok); + bool count_match = convert_ok && (data.count >= count.first && data.count <= count.second); + if (!count_match) match = false; + } + break; + case Column::DATA: + { + bool data_match = false; + data_match |= QString(data.dat.toHex()).contains(txt, cs); + data_match |= re.match(QString(data.dat.toHex())).hasMatch(); + data_match |= re.match(QString(data.dat.toHex(' '))).hasMatch(); + + if (!data_match) match = false; + } + break; + } + } + return match; +} + + +void MessageListModel::fetchData() { + std::vector new_msgs; + new_msgs.reserve(can->last_msgs.size() + dbc_address.size()); + + auto address = dbc_address; + for (auto it = can->last_msgs.cbegin(); it != can->last_msgs.cend(); ++it) { + if (filter_str.isEmpty() || matchMessage(it.key(), it.value(), filter_str)) { + new_msgs.push_back(it.key()); + } + address.remove(it.key().address); + } + + // merge all DBC messages + for (auto &addr : address) { + MessageId id{.source = INVALID_SOURCE, .address = addr}; + if (filter_str.isEmpty() || matchMessage(id, {}, filter_str)) { + new_msgs.push_back(id); + } + } + + sortMessages(new_msgs); + + if (msgs != new_msgs) { + beginResetModel(); + msgs = std::move(new_msgs); + endResetModel(); } - if (msgs.size() != prev_row_count) { - sortMessages(); - return; +} + +void MessageListModel::msgsReceived(const QHash *new_msgs, bool has_new_ids) { + if (has_new_ids || filter_str.contains(Column::FREQ) || filter_str.contains(Column::COUNT) || filter_str.contains(Column::DATA)) { + fetchData(); } for (int i = 0; i < msgs.size(); ++i) { if (new_msgs->contains(msgs[i])) { - for (int col = 3; col < columnCount(); ++col) + for (int col = Column::FREQ; col < columnCount(); ++col) emit dataChanged(index(i, col), index(i, col), {Qt::DisplayRole}); } } @@ -250,7 +397,7 @@ void MessageListModel::sort(int column, Qt::SortOrder order) { if (column != columnCount() - 1) { sort_column = column; sort_order = order; - sortMessages(); + fetchData(); } } @@ -274,12 +421,17 @@ void MessageListModel::clearSuppress() { void MessageListModel::reset() { beginResetModel(); - filter_str = ""; + filter_str.clear(); msgs.clear(); clearSuppress(); endResetModel(); } +void MessageListModel::forceResetModel() { + beginResetModel(); + endResetModel(); +} + // MessageView void MessageView::drawRow(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { @@ -319,3 +471,105 @@ void MessageView::updateBytesSectionSize() { header()->resizeSection(5, width); } } + +void MessageView::headerContextMenuEvent(const QPoint &pos) { + QMenu *menu = new QMenu(this); + int cur_index = header()->logicalIndexAt(pos); + + QString column_name; + QAction *action; + for (int visual_index = 0; visual_index < header()->count(); visual_index++) { + int logical_index = header()->logicalIndex(visual_index); + column_name = model()->headerData(logical_index, Qt::Horizontal).toString(); + + // Hide show action + if (header()->isSectionHidden(logical_index)) { + action = menu->addAction(tr("  %1").arg(column_name), [=]() { header()->showSection(logical_index); }); + } else { + action = menu->addAction(tr("✓ %1").arg(column_name), [=]() { header()->hideSection(logical_index); }); + } + + // Can't hide the name column + action->setEnabled(logical_index > 0); + + // Make current column bold + if (logical_index == cur_index) { + QFont font = action->font(); + font.setBold(true); + action->setFont(font); + } + } + + menu->popup(header()->mapToGlobal(pos)); +} + +MessageViewHeader::MessageViewHeader(QWidget *parent, MessageListModel *model) : model(model), QHeaderView(Qt::Horizontal, parent) { + QObject::connect(this, &QHeaderView::sectionResized, this, &MessageViewHeader::updateHeaderPositions); + QObject::connect(this, &QHeaderView::sectionMoved, this, &MessageViewHeader::updateHeaderPositions); +} + +void MessageViewHeader::showEvent(QShowEvent *e) { + + for (int i = 0; i < count(); i++) { + if (!editors[i]) { + QString column_name = model->headerData(i, Qt::Horizontal, Qt::DisplayRole).toString(); + editors[i] = new QLineEdit(this); + editors[i]->setClearButtonEnabled(true); + editors[i]->setPlaceholderText(tr("Filter %1").arg(column_name)); + + QObject::connect(editors[i], &QLineEdit::textChanged, this, &MessageViewHeader::updateFilters); + } + editors[i]->show(); + } + QHeaderView::showEvent(e); +} + +void MessageViewHeader::updateFilters() { + QMap filters; + for (int i = 0; i < count(); i++) { + if (editors[i]) { + QString filter = editors[i]->text(); + if (!filter.isEmpty()) { + filters[i] = filter; + } + } + } + emit filtersUpdated(filters); +} + +void MessageViewHeader::clearFilters() { + for (QLineEdit *editor : editors) { + editor->clear(); + } +} + +void MessageViewHeader::updateHeaderPositions() { + QSize sz = QHeaderView::sizeHint(); + for (int i = 0; i < count(); i++) { + if (editors[i]) { + int h = editors[i]->sizeHint().height(); + editors[i]->move(sectionViewportPosition(i), sz.height()); + editors[i]->resize(sectionSize(i), h); + editors[i]->setHidden(isSectionHidden(i)); + } + } +} + +void MessageViewHeader::updateGeometries() { + if (editors[0]) { + setViewportMargins(0, 0, 0, editors[0]->sizeHint().height()); + } else { + setViewportMargins(0, 0, 0, 0); + } + QHeaderView::updateGeometries(); + updateHeaderPositions(); +} + + +QSize MessageViewHeader::sizeHint() const { + QSize sz = QHeaderView::sizeHint(); + if (editors[0]) { + sz.setHeight(sz.height() + editors[0]->minimumSizeHint().height() + 1); + } + return sz; +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 2dc0d1d3169b09..036758e6367cf8 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -2,8 +2,11 @@ #include #include +#include #include +#include #include +#include #include #include @@ -14,23 +17,39 @@ class MessageListModel : public QAbstractTableModel { Q_OBJECT public: + + enum Column { + NAME = 0, + SOURCE, + ADDRESS, + FREQ, + COUNT, + DATA, + }; + MessageListModel(QObject *parent) : QAbstractTableModel(parent) {} QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; - int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 6; } + int columnCount(const QModelIndex &parent = QModelIndex()) const override { return Column::DATA + 1; } QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const; int rowCount(const QModelIndex &parent = QModelIndex()) const override { return msgs.size(); } void sort(int column, Qt::SortOrder order = Qt::AscendingOrder) override; - void setFilterString(const QString &string); - void msgsReceived(const QHash *new_msgs = nullptr); - void sortMessages(); + void setFilterStrings(const QMap &filters); + void msgsReceived(const QHash *new_msgs, bool has_new_ids); + void fetchData(); void suppress(); void clearSuppress(); void reset(); - QList msgs; + void forceResetModel(); + void dbcModified(); + std::vector msgs; QSet> suppressed_bytes; private: - QString filter_str; + void sortMessages(std::vector &new_msgs); + bool matchMessage(const MessageId &id, const CanData &data, const QMap &filters); + + QMap filter_str; + QSet dbc_address; int sort_column = 0; Qt::SortOrder sort_order = Qt::AscendingOrder; }; @@ -43,6 +62,33 @@ class MessageView : public QTreeView { void drawBranches(QPainter *painter, const QRect &rect, const QModelIndex &index) const override {} void dataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight, const QVector &roles = QVector()) override; void updateBytesSectionSize(); + void headerContextMenuEvent(const QPoint &pos); +}; + +class MessageViewHeader : public QHeaderView { + // https://stackoverflow.com/a/44346317 + + Q_OBJECT +public: + MessageViewHeader(QWidget *parent, MessageListModel *model); + void showEvent(QShowEvent *e) override; + void updateHeaderPositions(); + + void updateGeometries() override; + QSize sizeHint() const override; + +public slots: + void clearFilters(); + +signals: + void filtersUpdated(const QMap &filters); + +private: + void updateFilters(); + + QMap editors; + QMap> values; + MessageListModel *model; }; class MessagesWidget : public QWidget { @@ -56,15 +102,19 @@ class MessagesWidget : public QWidget { void updateSuppressedButtons(); void reset(); +public slots: + void dbcModified(); + signals: void msgSelectionChanged(const MessageId &message_id); protected: MessageView *view; + MessageViewHeader *header; std::optional current_msg_id; - QLineEdit *filter; QCheckBox *multiple_lines_bytes; MessageListModel *model; QPushButton *suppress_add; QPushButton *suppress_clear; + QLabel *num_msg_label; }; diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index f7593249889efd..d0cada680a4afe 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -38,6 +38,7 @@ void Settings::save() { s.setValue("log_livestream", log_livestream); s.setValue("log_path", log_path); s.setValue("drag_direction", drag_direction); + s.setValue("suppress_defined_signals", suppress_defined_signals); } void Settings::load() { @@ -61,6 +62,7 @@ void Settings::load() { log_livestream = s.value("log_livestream", true).toBool(); log_path = s.value("log_path").toString(); drag_direction = (Settings::DragDirection)s.value("drag_direction", 0).toInt(); + suppress_defined_signals = s.value("suppress_defined_signals", false).toBool(); if (log_path.isEmpty()) { log_path = QStandardPaths::writableLocation(QStandardPaths::HomeLocation) + "/cabana_live_stream/"; } diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index 11548ef5ccc462..b8a3797f86ae9d 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -36,6 +36,7 @@ class Settings : public QObject { int sparkline_range = 15; // 15 seconds bool multiple_lines_bytes = true; bool log_livestream = true; + bool suppress_defined_signals = false; QString log_path; QString last_dir; QString last_route_dir; diff --git a/tools/cabana/signalview.cc b/tools/cabana/signalview.cc index 32b0a3940882f6..124f9f6b9aa5cf 100644 --- a/tools/cabana/signalview.cc +++ b/tools/cabana/signalview.cc @@ -590,48 +590,40 @@ void SignalView::handleSignalUpdated(const cabana::Signal *sig) { } void SignalView::updateState(const QHash *msgs) { - if (model->rowCount() == 0 || (msgs && !msgs->contains(model->msg_id))) return; - const auto &last_msg = can->lastMessage(model->msg_id); + if (model->rowCount() == 0 || (msgs && !msgs->contains(model->msg_id)) || last_msg.dat.size() == 0) return; + for (auto item : model->root->children) { double value = get_raw_value((uint8_t *)last_msg.dat.constData(), last_msg.dat.size(), *item->sig); - item->sig_val = QString::number(value, 'f', item->sig->precision); - // Show unit - if (!item->sig->unit.isEmpty()) { - item->sig_val += " " + item->sig->unit; - } - // Show enum string - for (auto &[val, desc] : item->sig->val_desc) { - if (std::abs(value - val.toInt()) < 1e-6) { - item->sig_val = desc; - } - } + item->sig_val = item->sig->formatValue(value); max_value_width = std::max(max_value_width, fontMetrics().width(item->sig_val)); } - // update visible sparkline - QSize size(tree->columnWidth(1) - delegate->button_size.width(), delegate->button_size.height()); - int min_max_width = std::min(size.width() - 10, QFontMetrics(delegate->minmax_font).width("-000.00") + 5); - int value_width = std::min(max_value_width, size.width() * 0.35); - size -= {value_width + min_max_width, style()->pixelMetric(QStyle::PM_FocusFrameVMargin) * 2}; - QModelIndex top = tree->indexAt(QPoint(0, 0)); - QModelIndex bottom = tree->indexAt(tree->viewport()->rect().bottomLeft()); - int start_row = top.parent().isValid() ? top.parent().row() + 1 : top.row(); - int end_row = model->rowCount() - 1; - if (bottom.isValid()) { - end_row = bottom.parent().isValid() ? bottom.parent().row() : bottom.row(); - } - QFutureSynchronizer synchronizer; - for (int i = start_row; i <= end_row; ++i) { - auto item = model->getItem(model->index(i, 1)); - auto &s = item->sparkline; - if (s.last_ts != last_msg.ts || s.size() != size || s.time_range != settings.sparkline_range) { - synchronizer.addFuture(QtConcurrent::run( - &s, &Sparkline::update, model->msg_id, item->sig, last_msg.ts, settings.sparkline_range, size)); + if (top.isValid()) { + // update visible sparkline + int first_visible_row = top.parent().isValid() ? top.parent().row() + 1 : top.row(); + int last_visible_row = model->rowCount() - 1; + QModelIndex bottom = tree->indexAt(tree->viewport()->rect().bottomLeft()); + if (bottom.isValid()) { + last_visible_row = bottom.parent().isValid() ? bottom.parent().row() : bottom.row(); + } + + QSize size(tree->columnWidth(1) - delegate->button_size.width(), delegate->button_size.height()); + int min_max_width = std::min(size.width() - 10, QFontMetrics(delegate->minmax_font).width("-000.00") + 5); + int value_width = std::min(max_value_width, size.width() * 0.35); + size -= {value_width + min_max_width, style()->pixelMetric(QStyle::PM_FocusFrameVMargin) * 2}; + + QFutureSynchronizer synchronizer; + for (int i = first_visible_row; i <= last_visible_row; ++i) { + auto item = model->getItem(model->index(i, 1)); + auto &s = item->sparkline; + if (s.last_ts != last_msg.ts || s.size() != size || s.time_range != settings.sparkline_range) { + synchronizer.addFuture(QtConcurrent::run( + &s, &Sparkline::update, model->msg_id, item->sig, last_msg.ts, settings.sparkline_range, size)); + } } } - synchronizer.waitForFinished(); for (int i = 0; i < model->rowCount(); ++i) { emit model->dataChanged(model->index(i, 1), model->index(i, 1), {Qt::DisplayRole}); diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc index 88c56dce432bdd..701ecbb258088b 100644 --- a/tools/cabana/streams/abstractstream.cc +++ b/tools/cabana/streams/abstractstream.cc @@ -1,4 +1,5 @@ #include "tools/cabana/streams/abstractstream.h" + #include AbstractStream *can = nullptr; @@ -11,6 +12,7 @@ AbstractStream::AbstractStream(QObject *parent) : QObject(parent) { void AbstractStream::updateMessages(QHash *messages) { auto prev_src_size = sources.size(); + auto prev_msg_size = last_msgs.size(); for (auto it = messages->begin(); it != messages->end(); ++it) { const auto &id = it.key(); last_msgs[id] = it.value(); @@ -20,13 +22,14 @@ void AbstractStream::updateMessages(QHash *messages) { emit sourcesUpdated(sources); } emit updated(); - emit msgsReceived(messages); + emit msgsReceived(messages, prev_msg_size != last_msgs.size()); delete messages; processing = false; } void AbstractStream::updateEvent(const MessageId &id, double sec, const uint8_t *data, uint8_t size) { - all_msgs[id].compute((const char*)data, size, sec, getSpeed()); + QList mask = settings.suppress_defined_signals ? dbc()->mask(id) : QList(); + all_msgs[id].compute((const char *)data, size, sec, getSpeed(), mask); if (!new_msgs->contains(id)) { new_msgs->insert(id, {}); } @@ -48,6 +51,12 @@ bool AbstractStream::postEvents() { return false; } +const std::vector &AbstractStream::events(const MessageId &id) const { + static std::vector empty_events; + auto it = events_.find(id); + return it != events_.end() ? it->second : empty_events; +} + const CanData &AbstractStream::lastMessage(const MessageId &id) { static CanData empty_data = {}; auto it = last_msgs.find(id); @@ -66,10 +75,11 @@ void AbstractStream::updateLastMsgsTo(double sec) { auto it = std::lower_bound(ev.crbegin(), ev.crend(), last_ts, [](auto e, uint64_t ts) { return e->mono_time > ts; }); + QList mask = settings.suppress_defined_signals ? dbc()->mask(id) : QList(); if (it != ev.crend()) { double ts = (*it)->mono_time / 1e9 - routeStartTime(); auto &m = all_msgs[id]; - m.compute((const char *)(*it)->dat, (*it)->size, ts, getSpeed()); + m.compute((const char *)(*it)->dat, (*it)->size, ts, getSpeed(), mask); m.count = std::distance(it, ev.crend()); m.freq = m.count / std::max(1.0, ts); } @@ -78,22 +88,27 @@ void AbstractStream::updateLastMsgsTo(double sec) { // use a timer to prevent recursive calls QTimer::singleShot(0, [this]() { emit updated(); - emit msgsReceived(&last_msgs); + emit msgsReceived(&last_msgs, true); }); } -void AbstractStream::parseEvents(std::unordered_map> &msgs, - std::vector::const_iterator first, std::vector::const_iterator last) { +void AbstractStream::mergeEvents(std::vector::const_iterator first, std::vector::const_iterator last) { size_t memory_size = 0; + size_t events_cnt = 0; for (auto it = first; it != last; ++it) { if ((*it)->which == cereal::Event::Which::CAN) { for (const auto &c : (*it)->event.getCan()) { memory_size += sizeof(CanEvent) + sizeof(uint8_t) * c.getDat().size(); + ++events_cnt; } } } + if (memory_size == 0) return; char *ptr = memory_blocks.emplace_back(new char[memory_size]).get(); + std::unordered_map> new_events_map; + std::vector new_events; + new_events.reserve(events_cnt); for (auto it = first; it != last; ++it) { if ((*it)->which == cereal::Event::Which::CAN) { uint64_t ts = (*it)->mono_time; @@ -106,31 +121,28 @@ void AbstractStream::parseEvents(std::unordered_mapsize = dat.size(); memcpy(e->dat, (uint8_t *)dat.begin(), e->size); - msgs[{.source = e->src, .address = e->address}].push_back(e); - all_events_.push_back(e); + new_events_map[{.source = e->src, .address = e->address}].push_back(e); + new_events.push_back(e); ptr += sizeof(CanEvent) + sizeof(uint8_t) * e->size; } } } -} - -void AbstractStream::mergeEvents(std::vector::const_iterator first, std::vector::const_iterator last, bool append) { - if (first == last) return; - if (append) { - parseEvents(events_, first, last); - } else { - std::unordered_map> new_events; - parseEvents(new_events, first, last); - for (auto &[id, new_e] : new_events) { - auto &e = events_[id]; - auto it = std::upper_bound(e.cbegin(), e.cend(), new_e.front(), [](const CanEvent *l, const CanEvent *r) { - return l->mono_time < r->mono_time; - }); - e.insert(it, new_e.cbegin(), new_e.cend()); - } + bool append = new_events.front()->mono_time > lastest_event_ts; + for (auto &[id, new_e] : new_events_map) { + auto &e = events_[id]; + auto pos = append ? e.end() : std::upper_bound(e.cbegin(), e.cend(), new_e.front(), [](const CanEvent *l, const CanEvent *r) { + return l->mono_time < r->mono_time; + }); + e.insert(pos, new_e.cbegin(), new_e.cend()); } - total_sec = (all_events_.back()->mono_time - all_events_.front()->mono_time) / 1e9; + + auto pos = append ? all_events_.end() : std::upper_bound(all_events_.begin(), all_events_.end(), new_events.front(), [](auto l, auto r) { + return l->mono_time < r->mono_time; + }); + all_events_.insert(pos, new_events.cbegin(), new_events.cend()); + + lastest_event_ts = all_events_.back()->mono_time; emit eventsMerged(); } @@ -150,10 +162,11 @@ static inline QColor blend(const QColor &a, const QColor &b) { return QColor((a.red() + b.red()) / 2, (a.green() + b.green()) / 2, (a.blue() + b.blue()) / 2, (a.alpha() + b.alpha()) / 2); } -void CanData::compute(const char *can_data, const int size, double current_sec, double playback_speed, uint32_t in_freq) { +void CanData::compute(const char *can_data, const int size, double current_sec, double playback_speed, const QList &mask, uint32_t in_freq) { ts = current_sec; ++count; - freq = in_freq == 0 ? count / std::max(1.0, current_sec) : in_freq; + const double sec_to_first_event = current_sec - (can->allEvents().front()->mono_time / 1e9 - can->routeStartTime()); + freq = in_freq == 0 ? count / std::max(1.0, sec_to_first_event) : in_freq; if (dat.size() != size) { dat.resize(size); bit_change_counts.resize(size); @@ -168,8 +181,9 @@ void CanData::compute(const char *can_data, const int size, double current_sec, const QColor &greyish_blue = !lighter ? GREYISH_BLUE : GREYISH_BLUE_LIGHTER; for (int i = 0; i < size; ++i) { - const uint8_t last = dat[i]; - const uint8_t cur = can_data[i]; + const uint8_t mask_byte = (i < mask.size()) ? (~mask[i]) : 0xff; + const uint8_t last = dat[i] & mask_byte; + const uint8_t cur = can_data[i] & mask_byte; const int delta = cur - last; if (last != cur) { diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h index 644aa133d49b80..9b317b3ed6729b 100644 --- a/tools/cabana/streams/abstractstream.h +++ b/tools/cabana/streams/abstractstream.h @@ -13,7 +13,7 @@ #include "tools/replay/replay.h" struct CanData { - void compute(const char *dat, const int size, double current_sec, double playback_speed, uint32_t in_freq = 0); + void compute(const char *dat, const int size, double current_sec, double playback_speed, const QList &mask, uint32_t in_freq = 0); double ts = 0.; uint32_t count = 0; @@ -41,13 +41,12 @@ class AbstractStream : public QObject { AbstractStream(QObject *parent); virtual ~AbstractStream() {}; inline bool liveStreaming() const { return route() == nullptr; } - inline double lastEventSecond() const { return lastEventMonoTime() / 1e9 - routeStartTime(); } virtual void seekTo(double ts) {} virtual QString routeName() const = 0; virtual QString carFingerprint() const { return ""; } virtual double routeStartTime() const { return 0; } virtual double currentSec() const = 0; - double totalSeconds() const { return total_sec; } + virtual double totalSeconds() const { return lastEventMonoTime() / 1e9 - routeStartTime(); } const CanData &lastMessage(const MessageId &id); virtual VisionStreamType visionStreamType() const { return VISION_STREAM_ROAD; } virtual const Route *route() const { return nullptr; } @@ -55,8 +54,8 @@ class AbstractStream : public QObject { virtual double getSpeed() { return 1; } virtual bool isPaused() const { return false; } virtual void pause(bool pause) {} - const std::deque &allEvents() const { return all_events_; } - const std::deque &events(const MessageId &id) const { return events_.at(id); } + const std::vector &allEvents() const { return all_events_; } + const std::vector &events(const MessageId &id) const; virtual const std::vector> getTimeline() { return {}; } signals: @@ -66,7 +65,7 @@ class AbstractStream : public QObject { void streamStarted(); void eventsMerged(); void updated(); - void msgsReceived(const QHash *); + void msgsReceived(const QHash *new_msgs, bool has_new_ids); void sourcesUpdated(const SourceSet &s); public: @@ -74,20 +73,19 @@ class AbstractStream : public QObject { SourceSet sources; protected: - void mergeEvents(std::vector::const_iterator first, std::vector::const_iterator last, bool append); + void mergeEvents(std::vector::const_iterator first, std::vector::const_iterator last); bool postEvents(); - uint64_t lastEventMonoTime() const { return all_events_.empty() ? 0 : all_events_.back()->mono_time; } + uint64_t lastEventMonoTime() const { return lastest_event_ts; } void updateEvent(const MessageId &id, double sec, const uint8_t *data, uint8_t size); void updateMessages(QHash *); - void parseEvents(std::unordered_map> &msgs, std::vector::const_iterator first, std::vector::const_iterator last); void updateLastMsgsTo(double sec); - double total_sec = 0; + uint64_t lastest_event_ts = 0; std::atomic processing = false; std::unique_ptr> new_msgs; QHash all_msgs; - std::unordered_map> events_; - std::deque all_events_; + std::unordered_map> events_; + std::vector all_events_; std::deque> memory_blocks; }; diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc index 466537e9b862c7..65468600896ce0 100644 --- a/tools/cabana/streams/livestream.cc +++ b/tools/cabana/streams/livestream.cc @@ -52,7 +52,7 @@ void LiveStream::timerEvent(QTimerEvent *event) { { // merge events received from live stream thread. std::lock_guard lk(lock); - mergeEvents(receivedEvents.cbegin(), receivedEvents.cend(), true); + mergeEvents(receivedEvents.cbegin(), receivedEvents.cend()); receivedEvents.clear(); receivedMessages.clear(); } @@ -67,7 +67,6 @@ void LiveStream::timerEvent(QTimerEvent *event) { void LiveStream::updateEvents() { static double prev_speed = 1.0; - static uint64_t prev_newest_event_ts = all_events_.back()->mono_time; if (first_update_ts == 0) { first_update_ts = nanos_since_boot(); @@ -82,16 +81,13 @@ void LiveStream::updateEvents() { return; } - uint64_t last_event_ts = all_events_.back()->mono_time; - bool at_the_end = current_event_ts == prev_newest_event_ts; - if (!at_the_end) { - last_event_ts = first_event_ts + (nanos_since_boot() - first_update_ts) * speed_; - } - + uint64_t last_ts = post_last_event && speed_ == 1.0 + ? all_events_.back()->mono_time + : first_event_ts + (nanos_since_boot() - first_update_ts) * speed_; auto first = std::upper_bound(all_events_.cbegin(), all_events_.cend(), current_event_ts, [](uint64_t ts, auto e) { return ts < e->mono_time; }); - auto last = std::upper_bound(first, all_events_.cend(), last_event_ts, [](uint64_t ts, auto e) { + auto last = std::upper_bound(first, all_events_.cend(), last_ts, [](uint64_t ts, auto e) { return ts < e->mono_time; }); @@ -102,14 +98,13 @@ void LiveStream::updateEvents() { current_event_ts = e->mono_time; } postEvents(); - prev_newest_event_ts = all_events_.back()->mono_time; } void LiveStream::seekTo(double sec) { sec = std::max(0.0, sec); first_update_ts = nanos_since_boot(); - first_event_ts = std::min(sec * 1e9 + begin_event_ts, lastEventMonoTime()); - current_event_ts = first_event_ts; + current_event_ts = first_event_ts = std::min(sec * 1e9 + begin_event_ts, lastEventMonoTime()); + post_last_event = (first_event_ts == lastEventMonoTime()); emit seekedTo((current_event_ts - begin_event_ts) / 1e9); } diff --git a/tools/cabana/streams/livestream.h b/tools/cabana/streams/livestream.h index 197ac64e3e1e30..87ba7a01336cd0 100644 --- a/tools/cabana/streams/livestream.h +++ b/tools/cabana/streams/livestream.h @@ -50,6 +50,7 @@ class LiveStream : public AbstractStream { uint64_t current_event_ts = 0; uint64_t first_event_ts = 0; uint64_t first_update_ts = 0; + bool post_last_event = true; double speed_ = 1; bool paused_ = false; }; diff --git a/tools/cabana/streams/pandastream.cc b/tools/cabana/streams/pandastream.cc index 6eab40a652b1e7..5b3bf890e8b4d8 100644 --- a/tools/cabana/streams/pandastream.cc +++ b/tools/cabana/streams/pandastream.cc @@ -1,9 +1,12 @@ #include "tools/cabana/streams/pandastream.h" -#include +#include #include +#include #include +#include "selfdrive/ui/qt/util.h" + PandaStream::PandaStream(QObject *parent, PandaStreamConfig config_) : config(config_), LiveStream(parent) { if (config.serial.isEmpty()) { auto serials = Panda::list(); @@ -97,16 +100,105 @@ OpenPandaWidget::OpenPandaWidget(AbstractStream **stream) : AbstractOpenStreamWi main_layout->addStretch(1); QFormLayout *form_layout = new QFormLayout(); - form_layout->addRow(tr("Serial"), serial_edit = new QLineEdit(this)); - serial_edit->setPlaceholderText(tr("Leave empty to use default serial")); + QHBoxLayout *serial_layout = new QHBoxLayout(); + serial_edit = new QComboBox(); + serial_edit->setFixedWidth(300); + serial_layout->addWidget(serial_edit); + + QPushButton *refresh = new QPushButton(tr("Refresh")); + refresh->setFixedWidth(100); + serial_layout->addWidget(refresh); + form_layout->addRow(tr("Serial"), serial_layout); main_layout->addLayout(form_layout); + + config_layout = new QFormLayout(); + main_layout->addLayout(config_layout); + main_layout->addStretch(1); + + QObject::connect(refresh, &QPushButton::clicked, this, &OpenPandaWidget::refreshSerials); + QObject::connect(serial_edit, &QComboBox::currentTextChanged, this, &OpenPandaWidget::buildConfigForm); + + // Populate serials + refreshSerials(); + buildConfigForm(); +} + +void OpenPandaWidget::refreshSerials() { + serial_edit->clear(); + for (auto serial : Panda::list()) { + serial_edit->addItem(QString::fromStdString(serial)); + } +} + +void OpenPandaWidget::buildConfigForm() { + clearLayout(config_layout); + QString serial = serial_edit->currentText(); + + bool has_fd = false; + bool has_panda = !serial.isEmpty(); + + if (has_panda) { + try { + Panda panda = Panda(serial.toStdString()); + has_fd = (panda.hw_type == cereal::PandaState::PandaType::RED_PANDA) || (panda.hw_type == cereal::PandaState::PandaType::RED_PANDA_V2); + } catch (const std::exception& e) { + has_panda = false; + } + } + + if (has_panda) { + config.serial = serial; + config.bus_config.resize(3); + for (int i = 0; i < config.bus_config.size(); i++) { + QHBoxLayout *bus_layout = new QHBoxLayout; + + // CAN Speed + bus_layout->addWidget(new QLabel(tr("CAN Speed (kbps):"))); + QComboBox *can_speed = new QComboBox; + for (int j = 0; j < std::size(speeds); j++) { + can_speed->addItem(QString::number(speeds[j])); + + if (data_speeds[j] == config.bus_config[i].can_speed_kbps) { + can_speed->setCurrentIndex(j); + } + } + QObject::connect(can_speed, qOverload(&QComboBox::currentIndexChanged), [=](int index) {config.bus_config[i].can_speed_kbps = speeds[index];}); + bus_layout->addWidget(can_speed); + + // CAN-FD Speed + if (has_fd) { + QCheckBox *enable_fd = new QCheckBox("CAN-FD"); + bus_layout->addWidget(enable_fd); + bus_layout->addWidget(new QLabel(tr("Data Speed (kbps):"))); + QComboBox *data_speed = new QComboBox; + for (int j = 0; j < std::size(data_speeds); j++) { + data_speed->addItem(QString::number(data_speeds[j])); + + if (data_speeds[j] == config.bus_config[i].data_speed_kbps) { + data_speed->setCurrentIndex(j); + } + } + + data_speed->setEnabled(false); + bus_layout->addWidget(data_speed); + + QObject::connect(data_speed, qOverload(&QComboBox::currentIndexChanged), [=](int index) {config.bus_config[i].data_speed_kbps = data_speeds[index];}); + QObject::connect(enable_fd, &QCheckBox::stateChanged, data_speed, &QComboBox::setEnabled); + QObject::connect(enable_fd, &QCheckBox::stateChanged, [=](int state) {config.bus_config[i].can_fd = (bool)state;}); + } + + config_layout->addRow(tr("Bus %1:").arg(i), bus_layout); + } + } else { + config.serial = ""; + config_layout->addWidget(new QLabel(tr("No panda found"))); + } } bool OpenPandaWidget::open() { try { - PandaStreamConfig config = {.serial = serial_edit->text()}; *stream = new PandaStream(qApp, config); } catch (std::exception &e) { QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to connect to panda: '%1'").arg(e.what())); diff --git a/tools/cabana/streams/pandastream.h b/tools/cabana/streams/pandastream.h index df7ccded516497..f726c5cfb67172 100644 --- a/tools/cabana/streams/pandastream.h +++ b/tools/cabana/streams/pandastream.h @@ -1,8 +1,15 @@ #pragma once +#include +#include +#include + #include "tools/cabana/streams/livestream.h" #include "selfdrive/boardd/panda.h" +const uint32_t speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U}; +const uint32_t data_speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U, 2000U, 5000U}; + struct BusConfig { int can_speed_kbps = 500; int data_speed_kbps = 2000; @@ -40,5 +47,10 @@ class OpenPandaWidget : public AbstractOpenStreamWidget { QString title() override { return tr("&Panda"); } private: - QLineEdit *serial_edit; + void refreshSerials(); + void buildConfigForm(); + + QComboBox *serial_edit; + QFormLayout *config_layout; + PandaStreamConfig config = {}; }; diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc index 68e46c77da54e3..46103764beba75 100644 --- a/tools/cabana/streams/replaystream.cc +++ b/tools/cabana/streams/replaystream.cc @@ -25,10 +25,9 @@ static bool event_filter(const Event *e, void *opaque) { void ReplayStream::mergeSegments() { for (auto &[n, seg] : replay->segments()) { if (seg && seg->isLoaded() && !processed_segments.count(n)) { - const auto &events = seg->log->events; - bool append = processed_segments.empty() || *processed_segments.rbegin() < n; processed_segments.insert(n); - mergeEvents(events.cbegin(), events.cend(), append); + const auto &events = seg->log->events; + mergeEvents(events.cbegin(), events.cend()); } } } diff --git a/tools/cabana/streams/replaystream.h b/tools/cabana/streams/replaystream.h index 5b8e018fc9e7bd..aefc763d20f53f 100644 --- a/tools/cabana/streams/replaystream.h +++ b/tools/cabana/streams/replaystream.h @@ -13,6 +13,7 @@ class ReplayStream : public AbstractStream { void seekTo(double ts) override { replay->seekTo(std::max(double(0), ts), false); }; inline QString routeName() const override { return replay->route()->name(); } inline QString carFingerprint() const override { return replay->carFingerprint().c_str(); } + double totalSeconds() const override { return replay->totalSeconds(); } inline VisionStreamType visionStreamType() const override { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } inline double routeStartTime() const override { return replay->routeStartTime() / (double)1e9; } inline double currentSec() const override { return replay->currentSeconds(); } diff --git a/tools/cabana/tools/findsignal.cc b/tools/cabana/tools/findsignal.cc new file mode 100644 index 00000000000000..bc969356d4df77 --- /dev/null +++ b/tools/cabana/tools/findsignal.cc @@ -0,0 +1,273 @@ +#include "tools/cabana/tools/findsignal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +// FindSignalModel + +QVariant FindSignalModel::headerData(int section, Qt::Orientation orientation, int role) const { + static QString titles[] = {"Id", "Start Bit, size", "(time, value)"}; + if (role != Qt::DisplayRole) return {}; + return orientation == Qt::Horizontal ? titles[section] : QString::number(section + 1); +} + +QVariant FindSignalModel::data(const QModelIndex &index, int role) const { + if (role == Qt::DisplayRole) { + const auto &s = filtered_signals[index.row()]; + switch (index.column()) { + case 0: return s.id.toString(); + case 1: return QString("%1, %2").arg(s.sig.start_bit).arg(s.sig.size); + case 2: return s.values.join(" "); + } + } + return {}; +} + +void FindSignalModel::search(std::function cmp) { + beginResetModel(); + + std::mutex lock; + const auto prev_sigs = !histories.isEmpty() ? histories.back() : initial_signals; + filtered_signals.clear(); + filtered_signals.reserve(prev_sigs.size()); + QtConcurrent::blockingMap(prev_sigs, [&](auto &s) { + const auto &events = can->events(s.id); + auto first = std::upper_bound(events.cbegin(), events.cend(), s.mono_time, [](uint64_t ts, auto &e) { return ts < e->mono_time; }); + auto last = events.cend(); + if (last_time < std::numeric_limits::max()) { + last = std::upper_bound(events.cbegin(), events.cend(), last_time, [](uint64_t ts, auto &e) { return ts < e->mono_time; }); + } + + auto it = std::find_if(first, last, [&](const CanEvent *e) { return cmp(get_raw_value(e->dat, e->size, s.sig)); }); + if (it != last) { + auto values = s.values; + values += QString("(%1, %2)").arg((*it)->mono_time / 1e9 - can->routeStartTime(), 0, 'f', 2).arg(get_raw_value((*it)->dat, (*it)->size, s.sig)); + std::lock_guard lk(lock); + filtered_signals.push_back({.id = s.id, .mono_time = (*it)->mono_time, .sig = s.sig, .values = values}); + } + }); + histories.push_back(filtered_signals); + + endResetModel(); +} + +void FindSignalModel::undo() { + if (!histories.isEmpty()) { + beginResetModel(); + histories.pop_back(); + filtered_signals.clear(); + if (!histories.isEmpty()) filtered_signals = histories.back(); + endResetModel(); + } +} + +void FindSignalModel::reset() { + beginResetModel(); + histories.clear(); + filtered_signals.clear(); + initial_signals.clear(); + endResetModel(); +} + +// FindSignalDlg +FindSignalDlg::FindSignalDlg(QWidget *parent) : QDialog(parent, Qt::WindowFlags() | Qt::Window) { + setWindowTitle(tr("Find Signal")); + setAttribute(Qt::WA_DeleteOnClose); + QVBoxLayout *main_layout = new QVBoxLayout(this); + + // Messages group + message_group = new QGroupBox(tr("Messages"), this); + QFormLayout *message_layout = new QFormLayout(message_group); + message_layout->addRow(tr("Bus"), bus_edit = new QLineEdit()); + bus_edit->setPlaceholderText(tr("comma-seperated values. Leave blank for all")); + message_layout->addRow(tr("Address"), address_edit = new QLineEdit()); + address_edit->setPlaceholderText(tr("comma-seperated hex values. Leave blank for all")); + QHBoxLayout *hlayout = new QHBoxLayout(); + hlayout->addWidget(first_time_edit = new QLineEdit("0")); + hlayout->addWidget(new QLabel("-")); + hlayout->addWidget(last_time_edit = new QLineEdit("MAX")); + hlayout->addWidget(new QLabel("seconds")); + hlayout->addStretch(0); + message_layout->addRow(tr("Time"), hlayout); + + // Signal group + properties_group = new QGroupBox(tr("Signal")); + QFormLayout *property_layout = new QFormLayout(properties_group); + property_layout->setFieldGrowthPolicy(QFormLayout::FieldsStayAtSizeHint); + + hlayout = new QHBoxLayout(); + hlayout->addWidget(min_size = new QSpinBox); + hlayout->addWidget(new QLabel("-")); + hlayout->addWidget(max_size = new QSpinBox); + hlayout->addWidget(litter_endian = new QCheckBox(tr("Little endian"))); + hlayout->addWidget(is_signed = new QCheckBox(tr("Signed"))); + hlayout->addStretch(0); + min_size->setRange(1, 64); + max_size->setRange(1, 64); + min_size->setValue(8); + max_size->setValue(8); + litter_endian->setChecked(true); + property_layout->addRow(tr("Size"), hlayout); + property_layout->addRow(tr("Factor"), factor_edit = new QLineEdit("1.0")); + property_layout->addRow(tr("Offset"), offset_edit = new QLineEdit("0.0")); + + // find group + QGroupBox *find_group = new QGroupBox(tr("Find signal"), this); + QVBoxLayout *vlayout = new QVBoxLayout(find_group); + hlayout = new QHBoxLayout(); + hlayout->addWidget(new QLabel(tr("Value"))); + hlayout->addWidget(compare_cb = new QComboBox(this)); + hlayout->addWidget(value1 = new QLineEdit); + hlayout->addWidget(to_label = new QLabel("-")); + hlayout->addWidget(value2 = new QLineEdit); + hlayout->addWidget(undo_btn = new QPushButton(tr("Undo prev find"), this)); + hlayout->addWidget(search_btn = new QPushButton(tr("Find"))); + hlayout->addWidget(reset_btn = new QPushButton(tr("Reset"), this)); + vlayout->addLayout(hlayout); + + compare_cb->addItems({"=", ">", ">=", "!=", "<", "<=", "between"}); + value1->setFocus(Qt::OtherFocusReason); + value2->setVisible(false); + to_label->setVisible(false); + undo_btn->setEnabled(false); + reset_btn->setEnabled(false); + + auto double_validator = new QDoubleValidator(this); + double_validator->setLocale(QLocale::C); + for (auto edit : {value1, value2, factor_edit, offset_edit, first_time_edit, last_time_edit}) { + edit->setValidator(double_validator); + } + + vlayout->addWidget(view = new QTableView(this)); + view->setContextMenuPolicy(Qt::CustomContextMenu); + view->horizontalHeader()->setStretchLastSection(true); + view->horizontalHeader()->setSelectionMode(QAbstractItemView::NoSelection); + view->setSelectionBehavior(QAbstractItemView::SelectRows); + view->setModel(model = new FindSignalModel(this)); + + hlayout = new QHBoxLayout(); + hlayout->addWidget(message_group); + hlayout->addWidget(properties_group); + main_layout->addLayout(hlayout); + main_layout->addWidget(find_group); + main_layout->addWidget(stats_label = new QLabel()); + + setMinimumSize({700, 650}); + QObject::connect(search_btn, &QPushButton::clicked, this, &FindSignalDlg::search); + QObject::connect(undo_btn, &QPushButton::clicked, model, &FindSignalModel::undo); + QObject::connect(model, &QAbstractItemModel::modelReset, this, &FindSignalDlg::modelReset); + QObject::connect(reset_btn, &QPushButton::clicked, model, &FindSignalModel::reset); + QObject::connect(view, &QTableView::customContextMenuRequested, this, &FindSignalDlg::customMenuRequested); + QObject::connect(view, &QTableView::doubleClicked, [this](const QModelIndex &index) { + if (index.isValid()) emit openMessage(model->filtered_signals[index.row()].id); + }); + QObject::connect(compare_cb, qOverload(&QComboBox::currentIndexChanged), [=](int index) { + to_label->setVisible(index == compare_cb->count() - 1); + value2->setVisible(index == compare_cb->count() - 1); + }); +} + +void FindSignalDlg::search() { + if (model->histories.isEmpty()) { + setInitialSignals(); + } + auto v1 = value1->text().toDouble(); + auto v2 = value2->text().toDouble(); + std::function cmp = nullptr; + switch (compare_cb->currentIndex()) { + case 0: cmp = [v1](double v) { return v == v1;}; break; + case 1: cmp = [v1](double v) { return v > v1;};break; + case 2: cmp = [v1](double v) { return v >= v1;};break; + case 3: cmp = [v1](double v) { return v != v1;}; break; + case 4: cmp = [v1](double v) { return v < v1;}; break; + case 5: cmp = [v1](double v) { return v <= v1;}; break; + case 6: cmp = [v1, v2](double v) { return v >= v1 && v <= v2;}; break; + } + properties_group->setEnabled(false); + message_group->setEnabled(false); + search_btn->setEnabled(false); + stats_label->setVisible(false); + search_btn->setText("Finding ...."); + QTimer::singleShot(0, [=]() { model->search(cmp); }); +} + +void FindSignalDlg::setInitialSignals() { + QSet buses; + for (auto bus : bus_edit->text().trimmed().split(",")) { + bus = bus.trimmed(); + if (!bus.isEmpty()) buses.insert(bus.toUShort()); + } + + QSet addresses; + for (auto addr : address_edit->text().trimmed().split(",")) { + addr = addr.trimmed(); + if (!addr.isEmpty()) addresses.insert(addr.toULong(nullptr, 16)); + } + + cabana::Signal sig{}; + sig.is_little_endian = litter_endian->isChecked(); + sig.is_signed = is_signed->isChecked(); + sig.factor = factor_edit->text().toDouble(); + sig.offset = offset_edit->text().toDouble(); + + auto [first_sec, last_sec] = std::minmax(first_time_edit->text().toDouble(), last_time_edit->text().toDouble()); + uint64_t first_time = (can->routeStartTime() + first_sec) * 1e9; + model->last_time = std::numeric_limits::max(); + if (last_sec > 0) { + model->last_time = (can->routeStartTime() + last_sec) * 1e9; + } + model->initial_signals.clear(); + + for (auto it = can->last_msgs.cbegin(); it != can->last_msgs.cend(); ++it) { + if (buses.isEmpty() || buses.contains(it.key().source) && (addresses.isEmpty() || addresses.contains(it.key().address))) { + const auto &events = can->events(it.key()); + auto e = std::lower_bound(events.cbegin(), events.cend(), first_time, [](auto e, uint64_t ts) { return e->mono_time < ts; }); + if (e != events.cend()) { + const int total_size = it.value().dat.size() * 8; + for (int size = min_size->value(); size <= max_size->value(); ++size) { + for (int start = 0; start <= total_size - size; ++start) { + FindSignalModel::SearchSignal s{.id = it.key(), .mono_time = first_time, .sig = sig}; + updateSigSizeParamsFromRange(s.sig, start, size); + s.value = get_raw_value((*e)->dat, (*e)->size, s.sig); + model->initial_signals.push_back(s); + } + } + } + } + } +} + +void FindSignalDlg::modelReset() { + properties_group->setEnabled(model->histories.isEmpty()); + message_group->setEnabled(model->histories.isEmpty()); + search_btn->setText(model->histories.isEmpty() ? tr("Find") : tr("Find Next")); + reset_btn->setEnabled(!model->histories.isEmpty()); + undo_btn->setEnabled(model->histories.size() > 1); + search_btn->setEnabled(model->rowCount() > 0 || model->histories.isEmpty()); + stats_label->setVisible(true); + stats_label->setText(tr("%1 matches. right click on an item to create signal. double click to open message").arg(model->filtered_signals.size())); +} + +void FindSignalDlg::customMenuRequested(const QPoint &pos) { + if (auto index = view->indexAt(pos); index.isValid()) { + QMenu menu(this); + menu.addAction(tr("Create Signal")); + if (menu.exec(view->mapToGlobal(pos))) { + auto &s = model->filtered_signals[index.row()]; + auto msg = dbc()->msg(s.id); + if (!msg) { + UndoStack::push(new EditMsgCommand(s.id, dbc()->newMsgName(s.id), can->lastMessage(s.id).dat.size())); + msg = dbc()->msg(s.id); + } + s.sig.name = dbc()->newSignalName(s.id); + UndoStack::push(new AddSigCommand(s.id, s.sig)); + emit openMessage(s.id); + } + } +} diff --git a/tools/cabana/tools/findsignal.h b/tools/cabana/tools/findsignal.h new file mode 100644 index 00000000000000..f046bdf7e35ae5 --- /dev/null +++ b/tools/cabana/tools/findsignal.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include + +#include "tools/cabana/commands.h" +#include "tools/cabana/settings.h" + +class FindSignalModel : public QAbstractTableModel { +public: + struct SearchSignal { + MessageId id = {}; + uint64_t mono_time = 0; + cabana::Signal sig = {}; + double value = 0.; + QStringList values; + }; + + FindSignalModel(QObject *parent) : QAbstractTableModel(parent) {} + 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 columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; } + int rowCount(const QModelIndex &parent = QModelIndex()) const override { return std::min(filtered_signals.size(), 300); } + void search(std::function cmp); + void reset(); + void undo(); + + QList filtered_signals; + QList initial_signals; + QList> histories; + uint64_t last_time = std::numeric_limits::max(); +}; + +class FindSignalDlg : public QDialog { + Q_OBJECT +public: + FindSignalDlg(QWidget *parent); + +signals: + void openMessage(const MessageId &id); + +private: + void search(); + void modelReset(); + void setInitialSignals(); + void customMenuRequested(const QPoint &pos); + + QLineEdit *value1, *value2, *factor_edit, *offset_edit; + QLineEdit *bus_edit, *address_edit, *first_time_edit, *last_time_edit; + QComboBox *compare_cb; + QSpinBox *min_size, *max_size; + QCheckBox *litter_endian, *is_signed; + QPushButton *search_btn, *reset_btn, *undo_btn; + QGroupBox *properties_group, *message_group; + QTableView *view; + QLabel *to_label, *stats_label; + FindSignalModel *model; +}; diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc index 7bae2a7bb43be8..d95d6ac5258ace 100644 --- a/tools/cabana/util.cc +++ b/tools/cabana/util.cc @@ -131,6 +131,29 @@ void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem & painter->setPen(old_pen); } +// TabBar + +int TabBar::addTab(const QString &text) { + int index = QTabBar::addTab(text); + QToolButton *btn = new ToolButton("x", tr("Close Tab")); + int width = style()->pixelMetric(QStyle::PM_TabCloseIndicatorWidth, nullptr, btn); + int height = style()->pixelMetric(QStyle::PM_TabCloseIndicatorHeight, nullptr, btn); + btn->setFixedSize({width, height}); + setTabButton(index, QTabBar::RightSide, btn); + QObject::connect(btn, &QToolButton::clicked, this, &TabBar::closeTabClicked); + return index; +} + +void TabBar::closeTabClicked() { + QObject *object = sender(); + for (int i = 0; i < count(); ++i) { + if (tabButton(i, QTabBar::RightSide) == object) { + emit tabCloseRequested(i); + break; + } + } +} + QColor getColor(const cabana::Signal *sig) { float h = 19 * (float)sig->lsb / 64.0; h = fmod(h, 1.0); @@ -191,7 +214,7 @@ void setTheme(int theme) { new_palette.setColor(QPalette::Disabled, QPalette::ButtonText, QColor("#777777")); new_palette.setColor(QPalette::Disabled, QPalette::WindowText, QColor("#777777")); new_palette.setColor(QPalette::Disabled, QPalette::Text, QColor("#777777"));; - new_palette.setColor(QPalette::Light, QColor("#3c3f41")); + new_palette.setColor(QPalette::Light, QColor("#777777")); new_palette.setColor(QPalette::Dark, QColor("#353535")); } else { new_palette = style->standardPalette(); diff --git a/tools/cabana/util.h b/tools/cabana/util.h index 5a3d129a76d18b..dcf1ac0cbf2c3b 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -122,4 +122,15 @@ class ToolButton : public QToolButton { int theme; }; +class TabBar : public QTabBar { + Q_OBJECT + +public: + TabBar(QWidget *parent) : QTabBar(parent) {} + int addTab(const QString &text); + +private: + void closeTabClicked(); +}; + int num_decimals(double num); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 53a88972c8b41a..846a83c24cb1e6 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -113,21 +113,25 @@ QWidget *VideoWidget::createCameraWidget() { l->addLayout(slider_layout); QObject::connect(slider, &QSlider::sliderReleased, [this]() { can->seekTo(slider->value() / 1000.0); }); QObject::connect(slider, &QSlider::valueChanged, [=](int value) { time_label->setText(utils::formatSeconds(value / 1000)); }); + QObject::connect(slider, &Slider::updateMaximumTime, this, &VideoWidget::setMaximumTime); QObject::connect(cam_widget, &CameraWidget::clicked, []() { can->pause(!can->isPaused()); }); QObject::connect(can, &AbstractStream::updated, this, &VideoWidget::updateState); - QObject::connect(can, &AbstractStream::eventsMerged, [this]() { - end_time_label->setText(utils::formatSeconds(can->totalSeconds())); - slider->setRange(0, can->totalSeconds() * 1000); - }); + QObject::connect(can, &AbstractStream::streamStarted, [this]() { setMaximumTime(can->totalSeconds()); }); return w; } +void VideoWidget::setMaximumTime(double sec) { + maximum_time = sec; + end_time_label->setText(utils::formatSeconds(sec)); + slider->setRange(0, sec * 1000); +} + void VideoWidget::rangeChanged(double min, double max, bool is_zoomed) { if (can->liveStreaming()) return; if (!is_zoomed) { min = 0; - max = can->totalSeconds(); + max = maximum_time; } end_time_label->setText(utils::formatSeconds(max)); slider->setRange(min * 1000, max * 1000); @@ -180,10 +184,15 @@ void Slider::streamStarted() { void Slider::loadThumbnails() { const auto &segments = can->route()->segments(); + double max_time = 0; for (auto it = segments.rbegin(); it != segments.rend() && !abort_load_thumbnail; ++it) { LogReader log; std::string qlog = it->second.qlog.toStdString(); if (!qlog.empty() && log.load(qlog, &abort_load_thumbnail, {cereal::Event::Which::THUMBNAIL, cereal::Event::Which::CONTROLS_STATE}, true, 0, 3)) { + if (max_time == 0 && !log.events.empty()) { + max_time = (*(log.events.rbegin()))->mono_time / 1e9 - can->routeStartTime(); + emit updateMaximumTime(max_time); + } for (auto ev = log.events.cbegin(); ev != log.events.cend() && !abort_load_thumbnail; ++ev) { if ((*ev)->which == cereal::Event::Which::THUMBNAIL) { auto thumb = (*ev)->event.getThumbnail(); @@ -253,7 +262,8 @@ void Slider::mousePressEvent(QMouseEvent *e) { void Slider::mouseMoveEvent(QMouseEvent *e) { QPixmap thumb; AlertInfo alert; - double seconds = (minimum() + e->pos().x() * ((maximum() - minimum()) / (double)width())) / 1000.0; + int pos = std::clamp(e->pos().x(), 0, width()); + double seconds = (minimum() + pos * ((maximum() - minimum()) / (double)width())) / 1000.0; { std::lock_guard lk(thumbnail_lock); uint64_t mono_time = (seconds + can->routeStartTime()) * 1e9; @@ -264,7 +274,7 @@ void Slider::mouseMoveEvent(QMouseEvent *e) { alert = alert_it->second; } } - int x = std::clamp(e->pos().x() - thumb.width() / 2, THUMBNAIL_MARGIN, rect().right() - thumb.width() - THUMBNAIL_MARGIN); + int x = std::clamp(pos - thumb.width() / 2, THUMBNAIL_MARGIN, rect().right() - thumb.width() - THUMBNAIL_MARGIN); int y = -thumb.height(); thumbnail_label.showPixmap(mapToParent({x, y}), utils::formatSeconds(seconds), thumb, alert); QSlider::mouseMoveEvent(e); diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index 960053f9a35ac1..c2ec7599199f5f 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -37,6 +37,9 @@ class Slider : public QSlider { Slider(QWidget *parent); ~Slider(); +signals: + void updateMaximumTime(double); + private: void mousePressEvent(QMouseEvent *e) override; void mouseMoveEvent(QMouseEvent *e) override; @@ -46,6 +49,7 @@ class Slider : public QSlider { void streamStarted(); void loadThumbnails(); + double max_sec = 0; int slider_x = -1; std::vector> timeline; std::mutex thumbnail_lock; @@ -64,6 +68,7 @@ class VideoWidget : public QFrame { public: VideoWidget(QWidget *parnet = nullptr); void rangeChanged(double min, double max, bool is_zommed); + void setMaximumTime(double sec); protected: void updateState(); @@ -71,6 +76,7 @@ class VideoWidget : public QFrame { QWidget *createCameraWidget(); CameraWidget *cam_widget; + double maximum_time = 0; QLabel *end_time_label; QLabel *time_label; QHBoxLayout *slider_layout; diff --git a/tools/mac_setup.sh b/tools/mac_setup.sh index 67ca098c00f1c2..6e6538c5c53f9f 100755 --- a/tools/mac_setup.sh +++ b/tools/mac_setup.sh @@ -52,6 +52,7 @@ brew "zeromq" brew "protobuf" brew "protobuf-c" brew "swig" +brew "gcc@12" cask "gcc-arm-embedded" EOS diff --git a/tools/plotjuggler/layouts/can-states.xml b/tools/plotjuggler/layouts/can-states.xml new file mode 100644 index 00000000000000..8c761517e617b8 --- /dev/null +++ b/tools/plotjuggler/layouts/can-states.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/scripts/setup_ssh_keys.py b/tools/scripts/setup_ssh_keys.py index b6d448673306da..8f03303b59037a 100755 --- a/tools/scripts/setup_ssh_keys.py +++ b/tools/scripts/setup_ssh_keys.py @@ -14,8 +14,10 @@ keys = requests.get(f"https://github.com/{username}.keys", timeout=10) if keys.status_code == 200: - Params().put("GithubSshKeys", keys.text) - Params().put("GithubUsername", username) + params = Params() + params.put_bool("SshEnabled", True) + params.put("GithubSshKeys", keys.text) + params.put("GithubUsername", username) print("Setup ssh keys successfully") else: print("Error getting public keys from github")