diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 7cc45d23b9a594..d5be221303553f 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -61,7 +61,7 @@ jobs: cp .pylintrc $STRIPPED_DIR cp mypy.ini $STRIPPED_DIR cd $STRIPPED_DIR - ${{ env.RUN }} "SKIP=test_translations pre-commit run --all" + ${{ env.RUN }} "pre-commit run --all" build_all: name: build all @@ -219,6 +219,7 @@ jobs: $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ selfdrive/locationd/test/_test_locationd_lib.py && \ + ./selfdrive/locationd/test/test_glonass_runner && \ $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST system/hardware/tici && \ diff --git a/Jenkinsfile b/Jenkinsfile index 37621dde5c19f4..1841c68bbab780 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -81,6 +81,7 @@ pipeline { parallel { + /* stage('simulator') { agent { dockerfile { @@ -90,7 +91,8 @@ pipeline { } } steps { - sh "git config --global --add safe.directory ${WORKSPACE}" + sh "git config --global --add safe.directory '*'" + sh "git submodule update --init --recursive" sh "git lfs pull" lock(resource: "", label: "simulator", inversePrecedence: true, quantity: 1) { sh "${WORKSPACE}/tools/sim/build_container.sh" @@ -107,6 +109,7 @@ pipeline { } } } + */ stage('build') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } @@ -151,7 +154,7 @@ pipeline { stage('camerad-ar') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } steps { - phone_steps("tici-ar0321", [ + phone_steps("tici-ar0231", [ ["build", "cd selfdrive/manager && ./build.py"], ["test camerad", "python system/camerad/test/test_camerad.py"], ["test exposure", "python system/camerad/test/test_exposure.py"], @@ -189,7 +192,7 @@ pipeline { steps { phone_steps("tici-common", [ ["build", "cd selfdrive/manager && ./build.py"], - ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"], + ["model replay", "cd selfdrive/test/process_replay && NO_NAV=1 ./model_replay.py"], ]) } } diff --git a/RELEASES.md b/RELEASES.md index 97c0e4b8da2646..aaca12f02436ab 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -7,7 +7,9 @@ Version 0.9.1 (2022-12-XX) * Genesis GV60 2023 support thanks to sunnyhaibin! * Hyundai Tucson 2022-23 support * Kia Niro Hybrid 2023 support thanks to sunnyhaibin! +* Kia Sorento 2022-23 support thanks to sunnyhaibin! * Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! +* Volkswagen Crafter and MAN TGE 2017-23 support thanks to jyoung8607! Version 0.9.0 (2022-11-21) ======================== diff --git a/SConstruct b/SConstruct index b148a9116a7c4a..31aa6ecceddb49 100644 --- a/SConstruct +++ b/SConstruct @@ -282,7 +282,7 @@ Export('envCython') # Qt build environment qt_env = env.Clone() -qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus"] +qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus", "Xml"] qt_libs = [] if arch == "Darwin": @@ -311,6 +311,7 @@ else: qt_libs = [f"Qt5{m}" for m in qt_modules] if arch == "larch64": qt_libs += ["GLESv2", "wayland-client"] + qt_env.PrependENVPath('PATH', Dir("#third_party/qt5/larch64/bin/").abspath) elif arch != "Darwin": qt_libs += ["GL"] @@ -387,10 +388,10 @@ rednose_config = { if arch != "larch64": rednose_config['to_build'].update({ 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, [], rednose_deps), + 'lane': ('#selfdrive/locationd/models/lane_kf.py', True, [], rednose_deps), 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []), 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []), 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []), - 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, [], rednose_deps), }) Export('rednose_config') diff --git a/body b/body index dc780f858c1ef6..e1805f65ee75fa 160000 --- a/body +++ b/body @@ -1 +1 @@ -Subproject commit dc780f858c1ef641471d09b72569e199e3e10acb +Subproject commit e1805f65ee75fab4454c21eda8b42b49d4bdc48f diff --git a/cereal b/cereal index f200875ca300d3..bdbac40160a550 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f200875ca300d3a7b9293c4effcc9456e359e505 +Subproject commit bdbac40160a550c88e481a3bf700a1016901bd9b diff --git a/common/SConscript b/common/SConscript index 8aee6f42a7f3f4..5d6170611fc8f9 100644 --- a/common/SConscript +++ b/common/SConscript @@ -10,11 +10,13 @@ common_libs = [ 'statlog.cc', 'swaglog.cc', 'util.cc', - 'gpio.cc', 'i2c.cc', 'watchdog.cc', ] +if arch != "Darwin": + common_libs.append('gpio.cc') + _common = fxn('common', common_libs, LIBS="json11") files = [ diff --git a/common/clutil.cc b/common/clutil.cc index 9d3447d8074727..3cfc8a8c8c047a 100644 --- a/common/clutil.cc +++ b/common/clutil.cc @@ -5,6 +5,7 @@ #include #include "common/util.h" +#include "common/swaglog.h" namespace { // helper functions @@ -31,14 +32,14 @@ void cl_print_info(cl_platform_id platform, cl_device_id device) { case CL_DEVICE_TYPE_ACCELERATOR: type_str = "CL_DEVICE_TYPE_ACCELERATOR"; break; } - std::cout << "vendor: " << get_platform_info(platform, CL_PLATFORM_VENDOR) << std::endl - << "platform version: " << get_platform_info(platform, CL_PLATFORM_VERSION) << std::endl - << "profile: " << get_platform_info(platform, CL_PLATFORM_PROFILE) << std::endl - << "extensions: " << get_platform_info(platform, CL_PLATFORM_EXTENSIONS) << std::endl - << "name :" << get_device_info(device, CL_DEVICE_NAME) << std::endl - << "device version :" << get_device_info(device, CL_DEVICE_VERSION) << std::endl - << "max work group size :" << work_group_size << std::endl - << "type = " << device_type << " = " << type_str << std::endl; + LOGD("vendor: %s", get_platform_info(platform, CL_PLATFORM_VENDOR).c_str()); + LOGD("platform version: %s", get_platform_info(platform, CL_PLATFORM_VERSION).c_str()); + LOGD("profile: %s", get_platform_info(platform, CL_PLATFORM_PROFILE).c_str()); + LOGD("extensions: %s", get_platform_info(platform, CL_PLATFORM_EXTENSIONS).c_str()); + LOGD("name: %s", get_device_info(device, CL_DEVICE_NAME).c_str()); + LOGD("device version: %s", get_device_info(device, CL_DEVICE_VERSION).c_str()); + LOGD("max work group size: %d", work_group_size); + LOGD("type = %d = ", device_type, type_str); } void cl_print_build_errors(cl_program program, cl_device_id device) { @@ -49,7 +50,7 @@ void cl_print_build_errors(cl_program program, cl_device_id device) { std::string log(log_size, '\0'); clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL); - std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; + LOGE("build failed; status=%d, log: %s", status, log.c_str()); } } // namespace @@ -61,14 +62,15 @@ cl_device_id cl_get_device_id(cl_device_type device_type) { CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL)); for (size_t i = 0; i < num_platforms; ++i) { - std::cout << "platform[" << i << "] CL_PLATFORM_NAME: " << get_platform_info(platform_ids[i], CL_PLATFORM_NAME) << std::endl; + LOGD("platform[%d] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str()); + // Get first device if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) { cl_print_info(platform_ids[i], device_id); return device_id; } } - std::cout << "No valid openCL platform found" << std::endl; + LOGE("No valid openCL platform found"); assert(0); return nullptr; } diff --git a/common/gpio.cc b/common/gpio.cc index 9f5c211a4b5617..aa65c0bd9d0014 100644 --- a/common/gpio.cc +++ b/common/gpio.cc @@ -1,5 +1,20 @@ #include "common/gpio.h" +#ifdef __APPLE__ +int gpio_init(int pin_nr, bool output) { + return 0; +} + +int gpio_set(int pin_nr, bool high) { + return 0; +} + +int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { + return 0; +} + +#else + #include #include @@ -63,3 +78,5 @@ int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int p close(fd); return rq.fd; } + +#endif diff --git a/common/params.cc b/common/params.cc index 8f6532bc790f9d..db5e5e700d9d12 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,13 +104,14 @@ std::unordered_map keys = { {"DisablePowerDown", PERSISTENT}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, - {"ExperimentalLongitudinalEnabled", PERSISTENT}, // WARNING: THIS MAY DISABLE AEB + {"ExperimentalLongitudinalEnabled", PERSISTENT}, {"DisableUpdates", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT}, {"DongleId", PERSISTENT}, {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, @@ -134,6 +135,7 @@ std::unordered_map keys = { {"IsRhdDetected", PERSISTENT}, {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"IsTestedBranch", CLEAR_ON_MANAGER_START}, + {"IsReleaseBranch", CLEAR_ON_MANAGER_START}, {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"LaikadEphemeris", PERSISTENT | DONT_LOG}, @@ -169,7 +171,7 @@ std::unordered_map keys = { {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, diff --git a/docs/CARS.md b/docs/CARS.md index afcac0f637a13a..4f14d990080b84 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,240 +4,250 @@ 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. -# 223 Supported Cars +# 233 Supported Cars -|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| -|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| -|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| -|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| -|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| -|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-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 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| -|Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|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|Stock|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|Stock|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|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-22|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-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 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-21|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 2021-22[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D| -|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|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-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson Hybrid 2022[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[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| -|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| -|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|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 Hybrid 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 F| -|Kia|Niro Hybrid 2022|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 H| +|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|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|| +|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| +|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| +|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| +|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| +|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|| +|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|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| +|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| +|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| +|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|Stock|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|Stock|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|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-22|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-21|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 2021-22[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| +|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| +|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|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[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| +|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| +|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| +|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| +|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 Hybrid 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 F|| +|Kia|Niro Hybrid 2022|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 H|| |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|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 Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| -|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|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 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|Lexus Safety System+|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-22|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 2017-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| -|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-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| -|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| -|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| -|Ram|1500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,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|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|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-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2021-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|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|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| -|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|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 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| +|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| +|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| +|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| +|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| +|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| +|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| +|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 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|Lexus Safety System+|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-22|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 2017-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-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| +|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| +|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| +|Ram|1500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram|| +|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,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|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|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-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| +|Toyota|Camry Hybrid 2021-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 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| +|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| 1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
2By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace stock ACC. NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
-3Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
+3Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
-5Requires a red panda, additional harness box, additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.
+5Requires a red panda for this CAN FD car. All the hardware needed is sold in the CAN FD kit.
6openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
7Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
8Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
diff --git a/docs/assets/icon-youtube.svg b/docs/assets/icon-youtube.svg new file mode 100644 index 00000000000000..4e2c9fdfa91083 --- /dev/null +++ b/docs/assets/icon-youtube.svg @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/laika_repo b/laika_repo index 5eb0c3c2596dd1..b30afe44d28307 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 5eb0c3c2596dd12a232b83bdb057a716810e89cf +Subproject commit b30afe44d283072b6ee0ea17f290cca5d002baf7 diff --git a/panda b/panda index 345147fe2bc3a0..e7f36a2992b353 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 345147fe2bc3a06c44709426f9fcd298588b9fe4 +Subproject commit e7f36a2992b353d7d66fbb697d166d4de8718c99 diff --git a/poetry.lock b/poetry.lock index 9ab86329d756df..8edfbb868c1cf6 100644 --- a/poetry.lock +++ b/poetry.lock @@ -80,6 +80,14 @@ develop = ["imgaug (>=0.4.0)", "pytest"] imgaug = ["imgaug (>=0.4.0)"] tests = ["pytest"] +[[package]] +name = "antlr4-python3-runtime" +version = "4.9.3" +description = "ANTLR 4.9.3 runtime for Python 3.7" +category = "dev" +optional = false +python-versions = "*" + [[package]] name = "anyio" version = "3.6.2" @@ -2431,6 +2439,18 @@ rsa = ["cryptography (>=3.0.0)"] signals = ["blinker (>=1.4.0)"] signedtoken = ["cryptography (>=3.0.0)", "pyjwt (>=2.0.0,<3)"] +[[package]] +name = "omegaconf" +version = "2.3.0" +description = "A flexible configuration library" +category = "dev" +optional = false +python-versions = ">=3.6" + +[package.dependencies] +antlr4-python3-runtime = ">=4.9.0,<4.10.0" +PyYAML = ">=5.1.0" + [[package]] name = "onnx" version = "1.12.0" @@ -4444,7 +4464,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "2ab9d6aee6cc2c7bb5ba9763a54779ef9535cb752ec0038736a442992a04d459" +content-hash = "3c2597d2199a29ef79dd4d5fbc5e2350d86c82a3fe6716d13b93ebc268053eb9" [metadata.files] adal = [ @@ -4557,6 +4577,9 @@ albumentations = [ {file = "albumentations-1.3.0-py3-none-any.whl", hash = "sha256:294165d87d03bc8323e484927f0a5c1a3c64b0e7b9c32a979582a6c93c363bdf"}, {file = "albumentations-1.3.0.tar.gz", hash = "sha256:be1af36832c8893314f2a5550e8ac19801e04770734c1b70fa3c996b41f37bed"}, ] +antlr4-python3-runtime = [ + {file = "antlr4-python3-runtime-4.9.3.tar.gz", hash = "sha256:f224469b4168294902bb1efa80a8bf7855f24c99aef99cbefc1bcd3cce77881b"}, +] anyio = [ {file = "anyio-3.6.2-py3-none-any.whl", hash = "sha256:fbbe32bd270d2a2ef3ed1c5d45041250284e31fc0a4df4a5a6071842051a51e3"}, {file = "anyio-3.6.2.tar.gz", hash = "sha256:25ea0d673ae30af41a0c442f81cf3b38c7e79fdc7b60335a4c14e05eb0947421"}, @@ -6426,6 +6449,10 @@ oauthlib = [ {file = "oauthlib-3.2.2-py3-none-any.whl", hash = "sha256:8139f29aac13e25d502680e9e19963e83f16838d48a0d71c287fe40e7067fbca"}, {file = "oauthlib-3.2.2.tar.gz", hash = "sha256:9859c40929662bec5d64f34d01c99e093149682a3f38915dc0655d5a633dd918"}, ] +omegaconf = [ + {file = "omegaconf-2.3.0-py3-none-any.whl", hash = "sha256:7b4df175cdb08ba400f45cae3bdcae7ba8365db4d165fc65fd04b050ab63b46b"}, + {file = "omegaconf-2.3.0.tar.gz", hash = "sha256:d5d4b6d29955cc50ad50c46dc269bcd92c6e00f5f90d23ab5fee7bfca4ba4cc7"}, +] onnx = [ {file = "onnx-1.12.0-cp310-cp310-macosx_10_12_x86_64.whl", hash = "sha256:bdbd2578424c70836f4d0f9dda16c21868ddb07cc8192f9e8a176908b43d694b"}, {file = "onnx-1.12.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:213e73610173f6b2e99f99a4b0636f80b379c417312079d603806e48ada4ca8b"}, diff --git a/pyproject.toml b/pyproject.toml index af9fcc6e6e6a12..ed3a821e29cdc9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -171,6 +171,7 @@ torchvision = { url = "https://download.pytorch.org/whl/cu113/torchvision-0.12.0 triton = "^1.1.1" Werkzeug = "^2.1.2" zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" } +omegaconf = "^2.3.0" [build-system] diff --git a/release/check-submodules.sh b/release/check-submodules.sh index 182042e6b4918d..bc85a43c57b5d3 100755 --- a/release/check-submodules.sh +++ b/release/check-submodules.sh @@ -1,7 +1,7 @@ #!/bin/bash while read hash submodule ref; do - git -C $submodule fetch --depth 100 origin master + git -C $submodule fetch --depth 200 origin master git -C $submodule branch -r --contains $hash | grep "origin/master" if [ "$?" -eq 0 ]; then echo "$submodule ok" diff --git a/release/files_common b/release/files_common index d0afa30dea2f7d..5b310d5c8089a6 100644 --- a/release/files_common +++ b/release/files_common @@ -225,10 +225,7 @@ selfdrive/locationd/SConscript selfdrive/locationd/ubloxd.cc selfdrive/locationd/ublox_msg.cc selfdrive/locationd/ublox_msg.h -selfdrive/locationd/generated/ubx.cpp -selfdrive/locationd/generated/ubx.h -selfdrive/locationd/generated/gps.cpp -selfdrive/locationd/generated/gps.h +selfdrive/locationd/generated/* selfdrive/locationd/laikad.py selfdrive/locationd/locationd.h @@ -280,7 +277,6 @@ selfdrive/loggerd/deleter.py selfdrive/loggerd/xattr_cache.py selfdrive/sensord/SConscript -selfdrive/sensord/libdiag.h selfdrive/sensord/sensors_qcom2.cc selfdrive/sensord/sensors/*.cc selfdrive/sensord/sensors/*.h @@ -321,6 +317,12 @@ selfdrive/ui/qt/widgets/*.cc selfdrive/ui/qt/widgets/*.h selfdrive/ui/qt/maps/*.cc selfdrive/ui/qt/maps/*.h +selfdrive/ui/qt/setup/*.cc +selfdrive/ui/qt/setup/*.h + +selfdrive/ui/installer/*.cc +selfdrive/ui/installer/*.h +selfdrive/ui/installer/*.cc system/camerad/SConscript system/camerad/main.cc @@ -437,6 +439,7 @@ third_party/acados/larch64/** third_party/acados/include/** third_party/acados/acados_template/** +third_party/bootstrap/** third_party/qt5/larch64/bin/** scripts/update_now.sh @@ -574,17 +577,13 @@ opendbc/tesla_can.dbc opendbc/tesla_radar.dbc opendbc/tesla_powertrain.dbc -tinygrad_repo/openpilot/compile.py tinygrad_repo/accel/opencl/* +tinygrad_repo/tinygrad/llops/ops_opencl.py +tinygrad_repo/openpilot/compile.py tinygrad_repo/extra/onnx.py tinygrad_repo/extra/thneed.py tinygrad_repo/extra/utils.py tinygrad_repo/tinygrad/llops/ops_gpu.py -tinygrad_repo/tinygrad/llops/ops_opencl.py -tinygrad_repo/tinygrad/helpers.py -tinygrad_repo/tinygrad/mlops.py -tinygrad_repo/tinygrad/ops.py -tinygrad_repo/tinygrad/shapetracker.py -tinygrad_repo/tinygrad/tensor.py -tinygrad_repo/tinygrad/nn/__init__.py -tinygrad_repo/tinygrad/nn/optim.py +tinygrad_repo/tinygrad/shape/* +tinygrad_repo/tinygrad/nn/* +tinygrad_repo/tinygrad/*.py diff --git a/scripts/cell.sh b/scripts/cell.sh index cae701eccc1ee2..3623fe5b16659d 100755 --- a/scripts/cell.sh +++ b/scripts/cell.sh @@ -1,5 +1,3 @@ #!/usr/bin/bash - -nmcli connection modify --temporary lte ipv4.route-metric 1 -nmcli connection modify --temporary lte ipv6.route-metric 1 -nmcli con up lte +nmcli connection modify --temporary lte ipv4.route-metric 1 ipv6.route-metric 1 +nmcli con up --wait --ask lte diff --git a/selfdrive/assets/assets.qrc b/selfdrive/assets/assets.qrc index 39be41aa65d56c..79a1a1e272971c 100644 --- a/selfdrive/assets/assets.qrc +++ b/selfdrive/assets/assets.qrc @@ -1,5 +1,6 @@ + ../../third_party/bootstrap/bootstrap-icons.svg img_continue_triangle.svg img_circled_check.svg img_circled_slash.svg diff --git a/selfdrive/assets/navigation/default_marker.svg b/selfdrive/assets/navigation/default_marker.svg new file mode 100644 index 00000000000000..43d5290a96f56f --- /dev/null +++ b/selfdrive/assets/navigation/default_marker.svg @@ -0,0 +1,5 @@ + + + + + diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1f1249194db6b8..0473d3488cae2c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -48,8 +49,8 @@ #define MAX_IR_POWER 0.5f #define MIN_IR_POWER 0.0f -#define CUTOFF_IL 200 -#define SATURATE_IL 1600 +#define CUTOFF_IL 400 +#define SATURATE_IL 1000 #define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') using namespace std::chrono_literals; @@ -105,6 +106,8 @@ void sync_time(Panda *panda, SyncTimeDir dir) { bool safety_setter_thread(std::vector pandas) { LOGD("Starting safety setter thread"); + Params p; + // there should be at least one panda connected if (pandas.size() == 0) { return false; @@ -116,25 +119,20 @@ bool safety_setter_thread(std::vector pandas) { pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); } - Params p = Params(); - - // wait for VIN to be read + // wait for FW query at OBD port to finish while (true) { if (do_exit || !check_all_connected(pandas) || !ignition) { return false; } - std::string value_vin = p.get("CarVin"); - if (value_vin.size() > 0) { - // sanity check VIN format - assert(value_vin.size() == 17); - LOGW("got CarVin %s", value_vin.c_str()); + if (p.getBool("FirmwareObdQueryDone")) { + LOGW("finished FW query at OBD port"); break; } util::sleep_for(20); } - // set to ELM327 for ECU knockouts + // set to ELM327 to finish fingerprinting and for potential ECU knockouts for (Panda *panda : pandas) { panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); } @@ -225,9 +223,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { //Dont send if older than 1 second if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { for (const auto& panda : pandas) { - LOGT("sending sendcan to panda: %s", (panda->hw_serial).c_str()); + LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->hw_serial).c_str()); + LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); } } } @@ -540,9 +538,8 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { if (sm.updated("driverCameraState")) { auto event = sm["driverCameraState"]; int cur_integ_lines = event.getDriverCameraState().getIntegLines(); - float cur_gain = event.getDriverCameraState().getGain(); - cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); + cur_integ_lines = integ_lines_filter.update(cur_integ_lines); last_front_frame_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 3e447c830eeed4..647a0d9c781c3a 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -10,11 +10,13 @@ #include "common/util.h" Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // TODO: support SPI here one day... - if (serial.find("spi") != std::string::npos) { - handle = std::make_unique(serial); - } else { + // try USB first, then SPI + try { handle = std::make_unique(serial); + } catch (std::exception &e) { +#ifndef __APPLE__ + handle = std::make_unique(serial); +#endif } hw_type = get_hw_type(); @@ -26,6 +28,8 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { (hw_type == cereal::PandaState::PandaType::DOS) || (hw_type == cereal::PandaState::PandaType::TRES); + can_reset_communications(); + return; } @@ -37,8 +41,22 @@ bool Panda::comms_healthy() { return handle->comms_healthy; } +std::string Panda::hw_serial() { + return handle->hw_serial; +} + std::vector Panda::list() { - return PandaUsbHandle::list(); + std::vector serials = PandaUsbHandle::list(); + +#ifndef __APPLE__ + for (auto s : PandaSpiHandle::list()) { + if (std::find(serials.begin(), serials.end(), s) == serials.end()) { + serials.push_back(s); + } + } +#endif + + return serials; } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { @@ -174,10 +192,6 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data int32_t pos = 0; uint8_t send_buf[2 * USB_TX_SOFT_LIMIT]; - uint32_t magic = CAN_TRANSACTION_MAGIC; - memcpy(&send_buf[0], &magic, sizeof(uint32_t)); - pos += sizeof(uint32_t); - for (auto cmsg : can_data_list) { // check if the message is intended for this panda uint8_t bus = cmsg.getSrc(); @@ -194,20 +208,25 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; header.data_len_code = data_len_code; header.bus = bus - bus_offset; + header.checksum = 0; memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header)); - pos += sizeof(can_header); - memcpy(&send_buf[pos], (uint8_t *)can_data.begin(), can_data.size()); - pos += can_data.size(); + memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size()); + uint32_t msg_size = sizeof(can_header) + can_data.size(); + + // set checksum + ((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size); + + pos += msg_size; if (pos >= USB_TX_SOFT_LIMIT) { write_func(send_buf, pos); - pos = sizeof(uint32_t); + pos = 0; } } // send remaining packets - if (pos > sizeof(uint32_t)) write_func(send_buf, pos); + if (pos > 0) write_func(send_buf, pos); } void Panda::can_send(capnp::List::Reader can_data_list) { @@ -217,36 +236,38 @@ void Panda::can_send(capnp::List::Reader can_data_list) { } bool Panda::can_receive(std::vector& out_vec) { - uint8_t data[RECV_SIZE]; - int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE); + // Check if enough space left in buffer to store RECV_SIZE data + assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); + + int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); if (!comms_healthy()) { return false; } if (recv == RECV_SIZE) { LOGW("Panda receive buffer full"); } + receive_buffer_size += recv; - return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec); + return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); } -bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) { - if (size < sizeof(uint32_t)) { - return true; - } +void Panda::can_reset_communications() { + handle->control_write(0xc0, 0, 0); +} - uint32_t magic; - memcpy(&magic, &data[0], sizeof(uint32_t)); - if (magic != CAN_TRANSACTION_MAGIC) { - LOGE("CAN recv: buffer didn't start with magic"); - handle->comms_healthy = false; - return false; - } +bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec) { + int pos = 0; - int pos = sizeof(uint32_t); - while (pos < size) { + while (pos <= size - sizeof(can_header)) { can_header header; memcpy(&header, &data[pos], sizeof(can_header)); + const uint8_t data_len = dlc_to_len[header.data_len_code]; + if (pos + sizeof(can_header) + data_len > size) { + // we don't have all the data for this message yet + break; + } + can_frame &canData = out_vec.emplace_back(); canData.busTime = 0; canData.address = header.addr; @@ -258,10 +279,28 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &o canData.src += CAN_RETURNED_BUS_OFFSET; } - const uint8_t data_len = dlc_to_len[header.data_len_code]; + if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) { + LOGE("Panda CAN checksum failed"); + size = 0; + return false; + } + canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); pos += sizeof(can_header) + data_len; } + + // move the overflowing data to the beginning of the buffer for the next round + memmove(data, &data[pos], size - pos); + size -= pos; + return true; } + +uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) { + uint8_t checksum = 0U; + for (uint32_t i = 0U; i < len; i++) { + checksum ^= data[i]; + } + return checksum; +} diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 75fec57a3e7a88..69df2e2b66a305 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -30,6 +30,7 @@ struct __attribute__((packed)) can_header { uint8_t returned : 1; uint8_t extended : 1; uint32_t addr : 29; + uint8_t checksum : 8; }; struct can_frame { @@ -47,13 +48,13 @@ class Panda { public: Panda(std::string serial="", uint32_t bus_offset=0); - std::string hw_serial; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool has_rtc = false; const uint32_t bus_offset; bool connected(); bool comms_healthy(); + std::string hw_serial(); // Static functions static std::vector list(); @@ -80,11 +81,16 @@ class Panda { void set_canfd_non_iso(uint16_t bus, bool non_iso); void can_send(capnp::List::Reader can_data_list); bool can_receive(std::vector& out_vec); + void can_reset_communications(); protected: // for unit tests + uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64]; + uint32_t receive_buffer_size = 0; + Panda(uint32_t bus_offset) : bus_offset(bus_offset) {} void pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func); - bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec); + bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); + uint8_t calculate_checksum(uint8_t *data, uint32_t len); }; diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc index e73cb69318e155..120d2f67d59b84 100644 --- a/selfdrive/boardd/panda_comms.cc +++ b/selfdrive/boardd/panda_comms.cc @@ -44,7 +44,7 @@ PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); if (ret < 0) { goto fail; } - auto hw_serial = std::string((char *)desc_serial, ret); + hw_serial = std::string((char *)desc_serial, ret); if (serial.empty() || serial == hw_serial) { break; } diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index bd262dfa0ecdd9..506b96b3722519 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -5,7 +5,9 @@ #include #include +#ifndef __APPLE__ #include +#endif #include @@ -21,6 +23,7 @@ class PandaCommsHandle { virtual ~PandaCommsHandle() {}; virtual void cleanup() = 0; + std::string hw_serial; std::atomic connected = true; std::atomic comms_healthy = true; static std::vector list(); @@ -30,9 +33,6 @@ class PandaCommsHandle { virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; - -protected: - std::recursive_mutex hw_lock; }; class PandaUsbHandle : public PandaCommsHandle { @@ -50,9 +50,11 @@ class PandaUsbHandle : public PandaCommsHandle { private: libusb_context *ctx = NULL; libusb_device_handle *dev_handle = NULL; + std::recursive_mutex hw_lock; void handle_usb_issue(int err, const char func[]); }; +#ifndef __APPLE__ class PandaSpiHandle : public PandaCommsHandle { public: PandaSpiHandle(std::string serial); @@ -69,9 +71,11 @@ class PandaSpiHandle : public PandaCommsHandle { int spi_fd = -1; uint8_t tx_buf[SPI_BUF_SIZE]; uint8_t rx_buf[SPI_BUF_SIZE]; + inline static std::recursive_mutex hw_lock; int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack); int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len); int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); }; +#endif diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 971756002bc49d..f61d9ee1a68e60 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -7,7 +7,7 @@ from typing import List, NoReturn from functools import cmp_to_key -from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU +from panda import Panda, PandaDFU from common.basedir import BASEDIR from common.params import Params from system.hardware import HARDWARE @@ -15,10 +15,8 @@ def get_expected_signature(panda: Panda) -> bytes: - fn = DEFAULT_H7_FW_FN if (panda.get_mcu_type() == MCU_TYPE_H7) else DEFAULT_FW_FN - try: - return Panda.get_signature_from_firmware(fn) + return Panda.get_signature_from_firmware(panda.get_mcu_type().config.app_path) except Exception: cloudlog.exception("Error computing expected signature") return b"" diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 717b6ce820b7b3..c9d4b2ea0e4a40 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -1,9 +1,13 @@ +#ifndef __APPLE__ +#include #include #include #include #include #include +#include +#include #include "common/util.h" #include "common/timing.h" @@ -27,41 +31,77 @@ struct __attribute__((packed)) spi_header { const int SPI_MAX_RETRIES = 5; const int SPI_ACK_TIMEOUT = 50; // milliseconds +const std::string SPI_DEVICE = "/dev/spidev0.0"; + +class LockEx { +public: + LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) { + m.lock(); + flock(fd, LOCK_EX); + }; + + ~LockEx() { + m.unlock(); + flock(fd, LOCK_UN); + } + +private: + int fd; + std::recursive_mutex &m; +}; PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { - LOGD("opening SPI panda: %s", serial.c_str()); + int ret; + const int uid_len = 12; + uint8_t uid[uid_len] = {0}; - int err; uint32_t spi_mode = SPI_MODE_0; uint32_t spi_speed = 30000000; uint8_t spi_bits_per_word = 8; - spi_fd = open(serial.c_str(), O_RDWR); + spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); if (spi_fd < 0) { - LOGE("failed opening SPI device %d", err); + LOGE("failed opening SPI device %d", spi_fd); goto fail; } // SPI settings - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); - if (err < 0) { - LOGE("failed setting SPI mode %d", err); + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode); + if (ret < 0) { + LOGE("failed setting SPI mode %d", ret); goto fail; } - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (err < 0) { + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + if (ret < 0) { LOGE("failed setting SPI speed"); goto fail; } - err = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); - if (err < 0) { + ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word); + if (ret < 0) { LOGE("failed setting SPI bits per word"); goto fail; } + // get hw UID/serial + ret = control_read(0xc3, 0, 0, uid, uid_len); + if (ret == uid_len) { + std::stringstream stream; + for (int i = 0; i < uid_len; i++) { + stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); + } + hw_serial = stream.str(); + } else { + LOGD("failed to get serial %d", ret); + goto fail; + } + + if (!serial.empty() && (serial != hw_serial)) { + goto fail; + } + return; fail: @@ -84,6 +124,7 @@ void PandaSpiHandle::cleanup() { int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); ControlPacket_t packet = { .request = request, .param1 = param1, @@ -94,6 +135,7 @@ int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t par } int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); ControlPacket_t packet = { .request = request, .param1 = param1, @@ -104,15 +146,15 @@ int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t para } int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); return bulk_transfer(endpoint, data, length, NULL, 0); } int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + LockEx lock(spi_fd, hw_lock); return bulk_transfer(endpoint, NULL, 0, data, length); } int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) { - std::lock_guard lk(hw_lock); - const int xfer_size = 0x40 * 15; int ret = 0; @@ -143,7 +185,12 @@ int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t t } std::vector PandaSpiHandle::list() { - // TODO: list all pandas available over SPI + try { + PandaSpiHandle sh(""); + return {sh.hw_serial}; + } catch (std::exception &e) { + // no panda on SPI + } return {}; } @@ -167,7 +214,6 @@ int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint1 int ret; int count = SPI_MAX_RETRIES; - std::lock_guard lk(hw_lock); do { // TODO: handle error ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); @@ -195,7 +241,7 @@ int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { // handle timeout if (millis_since_boot() - start_millis > SPI_ACK_TIMEOUT) { - LOGE("SPI: timed out waiting for ACK"); + LOGD("SPI: timed out waiting for ACK"); return -1; } } @@ -294,3 +340,4 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx transfer_fail: return ret; } +#endif diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index e9bbcb4586b45b..b8ebbd88a36cf8 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -51,7 +51,7 @@ def test_loopback(self): cp.safetyConfigs = [safety_config]*num_pandas params = Params() - params.put("CarVin", b"0"*17) + params.put_bool("FirmwareObdQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index cc3b4bce9a6eda..3b78eb7bd2bbbf 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -16,7 +16,8 @@ int random_int(int min, int max) { struct PandaTest : public Panda { PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type); void test_can_send(); - void test_can_recv(); + void test_can_recv(uint32_t chunk_size = 0); + void test_chunked_can_recv(); std::map test_data; int can_list_size = 0; @@ -58,11 +59,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState void PandaTest::test_can_send() { std::vector unpacked_data; this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { - uint32_t magic; - memcpy(&magic, &chunk[0], sizeof(uint32_t)); - - REQUIRE(magic == CAN_TRANSACTION_MAGIC); - unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]); + unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]); }); REQUIRE(unpacked_data.size() == total_pakets_size); @@ -77,16 +74,30 @@ void PandaTest::test_can_send() { REQUIRE(header.addr == cnt); REQUIRE(test_data.find(data_len) != test_data.end()); const std::string &dat = test_data[data_len]; - REQUIRE(memcmp(dat.data(), &unpacked_data[pos + 5], dat.size()) == 0); + REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0); ++cnt; } REQUIRE(cnt == can_list_size); } -void PandaTest::test_can_recv() { +void PandaTest::test_can_recv(uint32_t rx_chunk_size) { std::vector frames; - this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) { - this->unpack_can_buffer(data, size, frames); + this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) { + if (rx_chunk_size == 0) { + REQUIRE(this->unpack_can_buffer(data, size, frames)); + } else { + this->receive_buffer_size = 0; + uint32_t pos = 0; + + while(pos < size) { + uint32_t chunk_size = std::min(rx_chunk_size, size - pos); + memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size); + this->receive_buffer_size += chunk_size; + pos += chunk_size; + + REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames)); + } + } }); REQUIRE(frames.size() == can_list_size); @@ -109,6 +120,9 @@ TEST_CASE("send/recv CAN 2.0 packets") { SECTION("can_receive") { test.test_can_recv(); } + SECTION("chunked_can_receive") { + test.test_can_recv(0x40); + } } TEST_CASE("send/recv CAN FD packets") { @@ -122,4 +136,7 @@ TEST_CASE("send/recv CAN FD packets") { SECTION("can_receive") { test.test_can_recv(); } + SECTION("chunked_can_receive") { + test.test_can_recv(0x40); + } } diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md index 2fce0f9036176e..1c80ea3d937b91 100644 --- a/selfdrive/car/CARS_template.md +++ b/selfdrive/car/CARS_template.md @@ -1,5 +1,6 @@ {% set footnote_tag = '[{}](#footnotes)' -%} {% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' -%} +{% set video_icon = '' -%} @@ -12,7 +13,7 @@ A supported vehicle is one that just works when you install a comma three. All s |{{Column | map(attribute='value') | join('|')}}| |---|---|---|{% 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, footnote_tag)}}|{% endfor %} +|{% for column in Column %}{{car_info.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %} {% endfor %} diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index bc5b36e2ee3c18..a90c4cfb876059 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -24,7 +24,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.wheelSpeedFactor = SPEED_FROM_RPM ret.centerToFront = ret.wheelbase * 0.44 - ret.radarOffCan = True + ret.radarUnavailable = True ret.openpilotLongitudinalControl = True ret.steerControlType = car.CarParams.SteerControlType.angle diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 61e7a3d55dffdf..4ccce979d3cfa5 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -99,7 +99,7 @@ def fingerprint(logcan, sendcan, num_pandas): else: cloudlog.warning("Getting VIN & FW versions") vin_rx_addr, vin = get_vin(logcan, sendcan, bus) - ecu_rx_addrs = get_present_ecus(logcan, sendcan) + ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) cached = False @@ -113,7 +113,10 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("Malformed VIN", vin=vin, error=True) vin = VIN_UNKNOWN cloudlog.warning("VIN %s", vin) - Params().put("CarVin", vin) + + params = Params() + params.put("CarVin", vin) + params.put_bool("FirmwareObdQueryDone", True) finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 @@ -173,17 +176,15 @@ def fingerprint(logcan, sendcan, num_pandas): return car_fingerprint, finger, vin, car_fw, source, exact_match -def get_car(logcan, sendcan, num_pandas=1): +def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" - experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled") - CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 2f058165ac96e0..da086105e34593 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config -from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags +from selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase @@ -12,7 +12,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD - ret.radarOffCan = DBC[candidate]['radar'] is None + ret.radarUnavailable = True # DBC[candidate]['radar'] is None ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 @@ -58,9 +58,9 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.mass = 2493. + STD_CARGO_KG CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.minSteerSpeed = 14.5 - for fw in car_fw: - if fw.ecu == 'eps' and fw.fwVersion.startswith((b"68312176", b"68273275")): - ret.minSteerSpeed = 0. + # Older EPS FW allow steer to zero + if any(fw.ecu == 'eps' and fw.fwVersion[:4] <= b"6831" for fw in car_fw): + ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: ret.steerActuatorDelay = 0.2 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 348e3c3632dfb4..0ab8c10b44b17f 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -45,12 +45,13 @@ def _address_to_track(address): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) + self.CP = CP self.rcp = _create_radar_can_parser(CP.carFingerprint) self.updated_messages = set() self.trigger_msg = LAST_MSG def update(self, can_strings): - if self.rcp is None: + if self.rcp is None or self.CP.radarUnavailable: return super().update(None) vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py index 03313e2ff6781e..bc03619d0d4932 100755 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -29,7 +29,7 @@ def get_all_car_info() -> List[CarInfo]: all_car_info: List[CarInfo] = [] footnotes = get_all_footnotes() for model, car_info in get_interface_attr("CAR_INFO", combine_brands=True).items(): - CP = interfaces[model][0].get_params(model, fingerprint=gen_empty_fingerprint(), car_fw=[car.CarParams.CarFw(ecu="unknown")]) + CP = interfaces[model][0].get_params(model, fingerprint=gen_empty_fingerprint(), car_fw=[car.CarParams.CarFw(ecu="unknown")], experimental_long=False) if CP.dashcamOnly or car_info is None: continue diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 03e9e721b58a76..c0fb4420dfaf81 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -21,6 +21,7 @@ class Column(Enum): STEERING_TORQUE = "Steering Torque" AUTO_RESUME = "Resume from stop" HARNESS = "Harness" + VIDEO = "Video" class Star(Enum): @@ -125,20 +126,11 @@ class CarInfo: harness: Enum = Harness.none def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): - # TODO: set all the min steer speeds in carParams and remove this - if self.min_steer_speed is not None: - assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams" - else: - self.min_steer_speed = CP.minSteerSpeed - - # TODO: set all the min enable speeds in carParams correctly and remove this - if self.min_enable_speed is None: - self.min_enable_speed = CP.minEnableSpeed - self.car_name = CP.carName self.car_fingerprint = CP.carFingerprint self.make, self.model, self.years = split_name(self.name) + # longitudinal column op_long = "Stock" if CP.openpilotLongitudinalControl and not CP.enableDsu: op_long = "openpilot" @@ -149,6 +141,23 @@ def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): else: self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL) + # min steer & enable speed columns + # TODO: set all the min steer speeds in carParams and remove this + if self.min_steer_speed is not None: + assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams" + else: + self.min_steer_speed = CP.minSteerSpeed + + # TODO: set all the min enable speeds in carParams correctly and remove this + if self.min_enable_speed is None: + self.min_enable_speed = CP.minEnableSpeed + + # harness column + harness_col = self.harness.value + if self.harness is not Harness.none: + model_years = self.model + (' ' + self.years if self.years else '') + harness_col = f'{harness_col}' + self.row = { Column.MAKE: self.make, Column.MODEL: self.model, @@ -158,7 +167,8 @@ def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph", Column.STEERING_TORQUE: Star.EMPTY, Column.AUTO_RESUME: Star.FULL if CP.autoResumeSng else Star.EMPTY, - Column.HARNESS: self.harness.value, + Column.HARNESS: harness_col, + Column.VIDEO: self.video_link if self.video_link is not None else "", # replaced with an image and link from template in get_column } # Set steering torque star from max lateral acceleration @@ -194,6 +204,13 @@ def get_detail_sentence(self, CP): if self.row[Column.STEERING_TORQUE] != Star.FULL: sentence_builder += " This car may not be able to take tight turns on its own." + # experimental mode + exp_link = "Experimental mode" + if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable: + sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}." + else: + sentence_builder += f" {exp_link}, with traffic light and stop sign handling, is not currently available for this car, but may be added in a future software update." + return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc) else: @@ -202,12 +219,14 @@ def get_detail_sentence(self, CP): else: raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}") - def get_column(self, column: Column, star_icon: str, footnote_tag: str) -> str: + def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str: item: Union[str, Star] = self.row[column] if isinstance(item, Star): item = star_icon.format(item.value) elif column == Column.MODEL and len(self.years): item += f" {self.years}" + elif column == Column.VIDEO and len(item) > 0: + item = video_icon.format(item) footnotes = get_footnotes(self.footnotes, column) if len(footnotes): diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 9f6ace2b5f14e0..e5d550fac8e89e 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -87,5 +87,5 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que for addr, subaddr, bus in ecu_addrs: msg = f" 0x{hex(addr)}" if subaddr is not None: - msg += f" (sub-address: 0x{hex(subaddr)})" + msg += f" (sub-address: {hex(subaddr)})" print(msg) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 8c391bb276440e..00fb461223d07d 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,28 +1,14 @@ -import math from cereal import car -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from opendbc.can.packer import CANPacker -from selfdrive.car.ford import fordcan -from selfdrive.car.ford.values import CANBUS, CarControllerParams +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, \ + create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): - # rate limit - steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) - rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN - max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) - - # absolute limit (LatCtlPath_An_Actl) - apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - apply_path_angle = clip(apply_path_angle, -0.5, 0.5235) - apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO - - return apply_angle - - class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -30,7 +16,7 @@ def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) self.frame = 0 - self.apply_angle_last = 0 + self.apply_curvature_last = 0 self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False @@ -46,66 +32,69 @@ def update(self, CC, CS): ### acc buttons ### if CC.cruiseControl.cancel: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main)) elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True)) - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True)) + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main)) # if stock lane centering isn't off, send a button press to toggle it off # the stock system checks for steering pressed, and eventually disengages cruise control elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0: - can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True)) - + can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### - if CC.latActive: - lca_rq = 1 - apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) - else: - lca_rq = 0 - apply_angle = 0. - # send steering commands at 20Hz if (self.frame % CarControllerParams.STEER_STEP) == 0: - # use LatCtlPath_An_Actl to actuate steering - path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - - # set slower ramp type when small steering angle change - # 0=Slow, 1=Medium, 2=Fast, 3=Immediately - steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg) - if steer_change < 2.0: - ramp_type = 0 - elif steer_change < 4.0: - ramp_type = 1 - elif steer_change < 6.0: - ramp_type = 2 + 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) else: - ramp_type = 3 - precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) + apply_curvature = 0. - self.apply_angle_last = apply_angle - can_sends.append(fordcan.create_lka_command(self.packer, 0, 0)) - can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, - 0, path_angle, 0, 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 + mode = 1 if CC.latActive else 0 + counter = self.frame // CarControllerParams.STEER_STEP + can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter)) + else: + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + + ### longitudinal control ### + # send acc command at 50Hz + if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + 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)) ### 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 if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + 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 if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: - can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = self.apply_angle_last + new_actuators.curvature = self.apply_curvature_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 2276b1208ad309..215900fef6c859 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -118,7 +118,7 @@ def get_can_parser(CP): ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver - ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons + ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthrough the remaining buttons ("WiprFront_D_Stat", "Steering_Data_FD1"), ("LghtAmb_D_Sns", "Steering_Data_FD1"), ("AccButtnGapDecPress", "Steering_Data_FD1"), diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 373ce096c6609d..594d50f59ff30b 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -4,64 +4,120 @@ HUDControl = car.CarControl.HUDControl -def create_lka_command(packer, angle_deg: float, curvature: float): +def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + 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 + return 0xFF - (checksum & 0xFF) + + +def create_lka_msg(packer): """ - Creates a CAN message for the Ford LKAS Command. + Creates an empty CAN message for the Ford LKA Command. This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout. Frequency is 20Hz. """ - values = { - "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] - "LkaActvStats_D2_Req": 0, # action [0|7] - "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees - "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick - "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter - "LdwActvStats_D_Req": 0, # LDW status [0|7] - "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength - } - return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values) + return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) -def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): +def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float): """ Creates a CAN message for the Ford TJA/LCA Command. - This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam - assist and highway driving. It is not subject to the PSCM lockout. + This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway + driving. It is not subject to the PSCM lockout. - Ford lane centering command uses a third order polynomial to describe the road centerline. The - polynomial is defined by the following coefficients: - c0: lateral offset between the vehicle and the centerline - c1: heading angle between the vehicle and the centerline - c2: curvature of the centerline + Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined + by the following coefficients: + c0: lateral offset between the vehicle and the centerline (positive is right) + c1: heading angle between the vehicle and the centerline (positive is right) + c2: curvature of the centerline (positive is left) c3: rate of change of curvature of the centerline - As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and - speed, the steering angle cannot be easily controlled. + As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering + angle cannot be easily controlled. - The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. - This can be done using tools such as Forscan. + The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done + using tools such as Forscan. Frequency is 20Hz. """ values = { - "LatCtlRng_L_Max": 0, # Unknown [0|126] meter - "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] - "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] - "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] - "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter - "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians - "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 - "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, + # 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + # Makes no difference with curvature control + "LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + # The stock system always uses comfortable + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter } return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) -def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): +def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + """ + Create a CAN message for the new Ford Lane Centering command. + + This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has + additional signals for a counter and checksum. + + Frequency is 20Hz. + """ + + values = { + "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, + # 3=SafeRampOut, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians + "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter + "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 + "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] + "LatCtlPath_No_Cnt": counter, # [0|15] + "LatCtlPath_No_Cs": 0, # [0|255] + } + + # calculate checksum + dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2] + values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) + + 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): + """ + Creates a CAN message for the Ford ACC Command. + + This command can be used to enable ACC, to set the ACC gas/brake/decel values + and to disable ACC. + + Frequency is 50Hz. + """ + + 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 + "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + } + return packer.make_can_msg("ACCDATA", 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. @@ -107,16 +163,15 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo values = { **stock_values, - "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 + "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_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): +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. + 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. @@ -148,7 +203,8 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, sto return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) -def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus: int = CANBUS.camera): +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. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f3d77bc05a7916..8ce30e5d68721c 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -2,39 +2,52 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, get_safety_config -from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter +from selfdrive.car.ford.values import CAR, Ecu from selfdrive.car.interfaces import CarInterfaceBase -CarParams = car.CarParams +TransmissionType = car.CarParams.TransmissionType +GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): 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 - ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] - # Angle-based steering - ret.steerControlType = CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.4 + ret.radarUnavailable = True + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 - if candidate == CAR.ESCAPE_MK4: + if candidate == CAR.BRONCO_SPORT_MK1: + ret.wheelbase = 2.67 + ret.steerRatio = 17.7 + ret.mass = 1625 + STD_CARGO_KG + + elif candidate == CAR.ESCAPE_MK4: ret.wheelbase = 2.71 - ret.steerRatio = 14.3 # Copied from Focus + ret.steerRatio = 16.7 ret.mass = 1750 + STD_CARGO_KG elif candidate == CAR.EXPLORER_MK6: ret.wheelbase = 3.025 - ret.steerRatio = 16.8 # learned + ret.steerRatio = 16.8 ret.mass = 2050 + STD_CARGO_KG elif candidate == CAR.FOCUS_MK4: ret.wheelbase = 2.7 - ret.steerRatio = 13.8 # learned + ret.steerRatio = 15.0 ret.mass = 1350 + STD_CARGO_KG + elif candidate == CAR.MAVERICK_MK1: + ret.wheelbase = 3.076 + ret.steerRatio = 17.0 + ret.mass = 1650 + STD_CARGO_KG + else: raise ValueError(f"Unsupported car: {candidate}") diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index c942703002ff29..ee4efb311db53f 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -47,7 +47,7 @@ def __init__(self, CP): self.updated_messages = set() self.track_id = 0 self.radar = DBC[CP.carFingerprint]['radar'] - if self.radar is None: + if self.radar is None or CP.radarUnavailable: self.rcp = None elif self.radar == RADAR.DELPHI_ESR: self.rcp = _create_delphi_esr_radar_can_parser() diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 0e6ef464b3fd4b..3723fe1d4a62d7 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,23 +1,21 @@ -from collections import defaultdict, namedtuple +from collections import defaultdict from dataclasses import dataclass from enum import Enum -from typing import Dict, List, Union +from typing import Dict, List, Set, Union from cereal import car -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu -TransmissionType = car.CarParams.TransmissionType -GearShifter = car.CarState.GearShifter - -AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) 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 @@ -25,12 +23,16 @@ class CarControllerParams: # Message: Steering_Data_FD1, but send twice as fast BUTTONS_STEP = 10 / 2 - LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians - # TODO: remove this once we understand how the EPS calculates the steering angle better - STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Nm + CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 + STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold, Nm + + # Curvature rate limits + # TODO: unify field names used by curvature and angle control cars + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.005, 0.00056, 0.0002]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.008, 0.00089, 0.00032]) - RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + ACCEL_MAX = 2.0 # m/s^s max acceleration + ACCEL_MIN = -3.5 # m/s^s max deceleration def __init__(self, CP): pass @@ -43,9 +45,14 @@ class CANBUS: class CAR: + BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN" ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" FOCUS_MK4 = "FORD FOCUS 4TH GEN" + MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" + + +CANFD_CARS: Set[str] = set() class RADAR: @@ -63,12 +70,14 @@ class FordCarInfo(CarInfo): 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-21"), FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), ], CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-22"), CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), + CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022", "Co-Pilot360 Assist"), } FW_QUERY_CONFIG = FwQueryConfig( @@ -88,6 +97,30 @@ class FordCarInfo(CarInfo): ) FW_VERSIONS = { + CAR.BRONCO_SPORT_MK1: { + (Ecu.eps, 0x730, None): [ + b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'M1PA-14C204-GF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'N1PA-14C204-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'LX6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ1P-14G395-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.ESCAPE_MK4: { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -163,4 +196,26 @@ class FordCarInfo(CarInfo): (Ecu.shiftByWire, 0x732, None): [ ], }, + CAR.MAVERICK_MK1: { + (Ecu.eps, 0x730, None): [ + b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'NZ6A-14C204-AAA\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NZ6A-14C204-PA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'NZ6A-14C204-ZA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'NZ6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, } diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index c7e4d4eb301949..a1695733fab2c9 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -62,6 +62,7 @@ class Request: @dataclass class FwQueryConfig: requests: List[Request] + # TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus # Overrides and removes from essential ecus for specific models and ecus (exact matching) non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) # Ecus added for data collection, not to be fingerprinted on diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index f4d92ab960fa0d..0db260abbd9f04 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -146,12 +146,16 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): return True, set() -def get_present_ecus(logcan, sendcan): +def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]: queries = list() parallel_queries = list() responses = set() for brand, r in REQUESTS: + # Skip query if no panda available + if r.bus > num_pandas * 4 - 1: + continue + for brand_versions in VERSIONS[brand].values(): for ecu_type, addr, sub_addr in brand_versions: # Only query ecus in whitelist if whitelist is not empty @@ -171,7 +175,7 @@ def get_present_ecus(logcan, sendcan): queries.insert(0, parallel_queries) - ecu_responses: Set[Tuple[int, Optional[int], int]] = set() + ecu_responses = set() for query in queries: ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) return ecu_responses diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b4a79d10a6d518..56f6ff5a312a69 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -50,9 +50,14 @@ def update(self, CC, CS): # Steering (Active: 50Hz, inactive: 10Hz) # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked - init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera + init_lka_counter = not self.sent_lka_steering_cmd + # Also send at 50Hz until we're in sync with camera so counters align when relay closes, preventing a fault + # openpilot can subtly drift, so this is activated throughout a drive to stay synced + out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.camera_lka_steering_cmd_counter + 1) % 4 + sync_steer = (init_lka_counter or out_of_sync) and self.CP.networkLocation == NetworkLocation.fwdCamera + steer_step = self.params.INACTIVE_STEER_STEP - if CC.latActive or init_lka_counter: + if CC.latActive or sync_steer: steer_step = self.params.STEER_STEP # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the @@ -63,7 +68,7 @@ def update(self, CC, CS): elif (self.frame - self.last_steer_frame) >= steer_step: # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus if init_lka_counter: - self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 + self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) @@ -109,7 +114,7 @@ def update(self, CC, CS): # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) - if not self.CP.radarOffCan: + if not self.CP.radarUnavailable: tt = self.frame * DT_CTRL time_and_headlights_step = 10 if self.frame % time_and_headlights_step == 0: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index de0fd2eed6ea1d..8ea8ea8874f2f8 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -18,6 +18,7 @@ def __init__(self, CP): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] self.loopback_lka_steering_cmd_updated = False + self.pt_lka_steering_cmd_counter = 0 self.camera_lka_steering_cmd_counter = 0 self.buttons_counter = 0 @@ -33,6 +34,7 @@ def update(self, pt_cp, cam_cp, loopback_cp): # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.CP.networkLocation == NetworkLocation.fwdCamera: + self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( @@ -180,6 +182,15 @@ def get_can_parser(CP): ("ECMAcceleratorPos", 80), ] + # Used to read back last counter sent to PT by camera + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + checks += [ + ("ASCMLKASteeringCmd", 0), + ] + if CP.transmissionType == TransmissionType.direct: signals.append(("RegenPaddle", "EBCMRegenPaddle")) checks.append(("EBCMRegenPaddle", 50)) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 195df36a7fea24..3c2a12ef863f4d 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -63,29 +63,29 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = True ret.networkLocation = NetworkLocation.fwdCamera - ret.radarOffCan = True # no radar + ret.radarUnavailable = True # no radar ret.pcmCruise = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM ret.minEnableSpeed = 5 * CV.KPH_TO_MS + # Tuning for experimental long + ret.longitudinalTuning.kpV = [2.0, 1.5] + ret.longitudinalTuning.kiV = [0.72] + ret.stopAccel = -2.0 + ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 + ret.longitudinalActuatorDelayUpperBound = 0.5 + if experimental_long: ret.pcmCruise = False ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG - # Tuning - ret.longitudinalTuning.kpV = [2.0, 1.5] - ret.longitudinalTuning.kiV = [0.72] - ret.stopAccel = -2.0 - ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling - ret.vEgoStopping = 0.25 - ret.vEgoStarting = 0.25 - ret.longitudinalActuatorDelayUpperBound = 0.5 - else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway - ret.radarOffCan = False + ret.radarUnavailable = False ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 6904e6f899f914..b86a85f9153ea3 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -3,7 +3,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser -from selfdrive.car.gm.values import DBC, CAR, CanBus +from selfdrive.car.gm.values import DBC, CanBus from selfdrive.car.interfaces import RadarInterfaceBase RADAR_HEADER_MSG = 1120 @@ -16,9 +16,6 @@ def create_radar_can_parser(car_fingerprint): - if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): - return None - # C1A-ARS3-A by Continental radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) signals = list(zip(['FLRRNumValidTargets', @@ -34,11 +31,12 @@ def create_radar_can_parser(car_fingerprint): return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE) + class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.rcp = create_radar_can_parser(CP.carFingerprint) + self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index ece128c253d718..e633ec5f62dfcb 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -75,7 +75,7 @@ class CAR: class Footnote(Enum): OBD_II = CarFootnote( - 'Requires a community built ASCM harness. ' + + 'Requires a community built ASCM harness. ' + 'NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).', Column.MODEL) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index a37667fd3a0dae..16880d1b1f9c3a 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,7 +274,7 @@ def update(self, cp, cp_cam, cp_body): ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models - if self.CP.carFingerprint in (CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE): + if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE): if ret.brake > 0.1: ret.brakePressed = True diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 990238ae5d8a3b..98243d81dd3a61 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -34,7 +34,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] - ret.radarOffCan = True + ret.radarUnavailable = True if candidate not in HONDA_BOSCH_RADARLESS: # Disable the radar and let openpilot control longitudinal @@ -231,11 +231,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - elif candidate in (CAR.PILOT, CAR.PASSPORT): - ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight - ret.wheelbase = 2.82 + elif candidate == CAR.PILOT: + ret.mass = 4278. * CV.LB_TO_KG + STD_CARGO_KG # average weight + ret.wheelbase = 2.86 ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 17.25 # as spec + ret.steerRatio = 16.0 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 629ab01d4cfe92..660be4c449d065 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -21,7 +21,7 @@ def __init__(self, CP): self.track_id = 0 self.radar_fault = False self.radar_wrong_config = False - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.radar_ts = CP.radarTimeStep self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar diff --git a/selfdrive/car/honda/tests/test_honda.py b/selfdrive/car/honda/tests/test_honda.py new file mode 100755 index 00000000000000..7a8c86fb0a254f --- /dev/null +++ b/selfdrive/car/honda/tests/test_honda.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import re +import unittest + +from selfdrive.car.honda.values import FW_VERSIONS + +HONDA_FW_VERSION_RE = br"\d{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" + + +class TestHondaFingerprint(unittest.TestCase): + def test_fw_version_format(self): + # Asserts all FW versions follow an expected format + for fw_by_ecu in FW_VERSIONS.values(): + for fws in fw_by_ecu.values(): + for fw in fws: + self.assertTrue(re.match(HONDA_FW_VERSION_RE, fw) is not None, fw) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 2906b23bda9d39..07eed0cfc22efe 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -4,9 +4,10 @@ from cereal import car 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.fw_query_definitions import FwQueryConfig, Request, StdQueries +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -91,7 +92,6 @@ class CAR: ACURA_RDX = "ACURA RDX 2018" ACURA_RDX_3G = "ACURA RDX 2020" PILOT = "HONDA PILOT 2017" - PASSPORT = "HONDA PASSPORT 2021" RIDGELINE = "HONDA RIDGELINE 2017" INSIGHT = "HONDA INSIGHT 2019" HONDA_E = "HONDA E 2020" @@ -142,20 +142,47 @@ def init_make(self, CP: car.CarParams): CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.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), + ], CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), } +HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xF112) +HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xF112) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ + # Currently used to fingerprint + Request( + [StdQueries.UDS_VERSION_REQUEST], + [StdQueries.UDS_VERSION_RESPONSE], + bus=1, + ), + + # Data collection requests: + # Log extra identifiers for current ECUs + Request( + [HONDA_VERSION_REQUEST], + [HONDA_VERSION_RESPONSE], + bus=1, + ), + # Query Nidec PT bus from camera for data collection Request( [StdQueries.UDS_VERSION_REQUEST], [StdQueries.UDS_VERSION_RESPONSE], + bus=0, ), ], + extra_ecus=[ + # The only other ECU on PT bus accessible by camera on radarless Civic + (Ecu.unknown, 0x18DAB3F1, None), + ], ) FW_VERSIONS = { @@ -1067,6 +1094,8 @@ def init_make(self, CP: car.CarParams): b'28101-5EZ-A060\x00\x00', b'28101-5EZ-A100\x00\x00', b'28101-5EZ-A210\x00\x00', + b'28101-5EZ-A600\x00\x00', + b'28101-5EZ-A430\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-RLV-4060\x00\x00', @@ -1078,6 +1107,9 @@ def init_make(self, CP: car.CarParams): b'37805-RLV-C520\x00\x00', b'37805-RLV-C530\x00\x00', b'37805-RLV-C910\x00\x00', + b'37805-RLV-B220\x00\x00', + b'37805-RLV-B210\x00\x00', + b'37805-RLV-L160\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', @@ -1110,6 +1142,7 @@ def init_make(self, CP: car.CarParams): b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', + b'36161-TGS-A030\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TG7-A020\x00\x00', @@ -1147,6 +1180,9 @@ def init_make(self, CP: car.CarParams): b'78109-TGS-AP20\x00\x00', b'78109-TGT-AJ20\x00\x00', b'78109-TGT-AK30\x00\x00', + b'78109-TGS-AT20\x00\x00', + b'78109-TGS-AX20\x00\x00', + b'78109-TGS-AJ20\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A130\x00\x00', @@ -1163,42 +1199,6 @@ def init_make(self, CP: car.CarParams): b'57114-TGT-A530\x00\x00', ], }, - CAR.PASSPORT: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-RLV-B220\x00\x00', - b'37805-RLV-B210\x00\x00', - b'37805-RLV-L160\x00\x00', - ], - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TGS-A230\x00\x00', - ], - (Ecu.fwdRadar, 0x18dab0f1, None): [ - b'36161-TGS-A030\x00\x00', - b'36161-TGS-A130\x00\x00', - ], - (Ecu.gateway, 0x18daeff1, None): [ - b'38897-TG7-A040\x00\x00', - b'38897-TG7-A030\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TGS-A010\x00\x00', - ], - (Ecu.shiftByWire, 0x18da0bf1, None): [ - b'54008-TG7-A530\x00\x00', - ], - (Ecu.transmission, 0x18da1ef1, None): [ - b'28101-5EZ-A600\x00\x00', - b'28101-5EZ-A430\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TGS-AT20\x00\x00', - b'78109-TGS-AX20\x00\x00', - b'78109-TGS-AJ20\x00\x00', - ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TGS-A530\x00\x00', - ], - }, CAR.ACURA_RDX: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-TX5-A220\x00\x00', @@ -1460,6 +1460,12 @@ def init_make(self, CP: car.CarParams): b'78108-T21-A230\x00\x00', b'78108-T22-A020\x00\x00', ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T20-A070\x00\x00', + b'36161-T20-A080\x00\x00', + b'36161-T20-A060\x00\x00', + b'36161-T47-A070\x00\x00', + ], (Ecu.vsa, 0x18DA28F1, None): [ b'57114-T20-AB40\x00\x00', b'57114-T43-JB30\x00\x00', @@ -1499,7 +1505,6 @@ def init_make(self, CP: car.CarParams): CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.PASSPORT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), @@ -1514,7 +1519,7 @@ def init_make(self, CP: car.CarParams): HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, - CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE} + CAR.PILOT, CAR.RIDGELINE} HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022} HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index f759bc2ba0c587..06efa54605f619 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -88,7 +88,7 @@ def update(self, cp, cp_cam): 50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"]) ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"] ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"] - ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0 # cruise state @@ -158,6 +158,9 @@ def update(self, cp, cp_cam): def update_canfd(self, cp, cp_cam): ret = car.CarState.new_message() + self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 + speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR): if self.CP.carFingerprint in EV_CAR: ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255. @@ -199,14 +202,18 @@ def update_canfd(self, cp, cp_cam): ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0 ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0 - ret.cruiseState.available = True - self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 - if not self.CP.openpilotLongitudinalControl: - speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + # cruise state + # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement + ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0 + if self.CP.openpilotLongitudinalControl: + # These are not used for engage/disengage since openpilot keeps track of state using the buttons + ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1 + ret.cruiseState.standstill = False + else: cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp - ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor - ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) + ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 + ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"]) cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" @@ -439,6 +446,7 @@ def get_can_parser_canfd(CP): ("DriverBraking", "TCS"), ("ACCEnable", "TCS"), + ("ACC_REQ", "TCS"), ("COUNTER", cruise_btn_msg), ("CRUISE_BUTTONS", cruise_btn_msg), diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 43c9642342b0a6..af7239571ccc5e 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -72,8 +72,6 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove else: a_raw = accel a_val = clip(accel, accel_last - jn, accel_last + jn) - if stopping: - a_raw = 0 values = { "ACCMode": 0 if not enabled else (2 if gas_override else 1), diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 545e4fd20b47b2..1b45c47a559cc0 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "hyundai" - ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is @@ -188,10 +188,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only ret.wheelbase = 2.756 ret.steerRatio = 13.6 - elif candidate == CAR.KIA_SORENTO_PHEV_4TH_GEN: - ret.mass = 4095.8 * CV.LB_TO_KG + STD_CARGO_KG # weight from EX and above trims, average of FWD and AWD versions (EX, X-Line EX AWD, SX, SX Pestige, X-Line SX Prestige AWD) + elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN): ret.wheelbase = 2.81 - ret.steerRatio = 13.27 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sorento-phev/2022/specifications + ret.steerRatio = 13.5 # average of the platforms + if candidate == CAR.KIA_SORENTO_4TH_GEN: + ret.mass = 3957 * CV.LB_TO_KG + STD_CARGO_KG + else: + ret.mass = 4537 * CV.LB_TO_KG + STD_CARGO_KG elif candidate == CAR.KIA_NIRO_HEV_2ND_GEN: ret.mass = 3247.0 * CV.LB_TO_KG + STD_CARGO_KG # weight from EX and above trims ret.wheelbase = 2.72 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 0d22611fb577ce..4ecca542b5169c 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -37,7 +37,7 @@ def __init__(self, CP): self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 self.track_id = 0 - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) def update(self, can_strings): diff --git a/selfdrive/car/hyundai/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py index a52027f4486dfa..6ecfa03796cd03 100755 --- a/selfdrive/car/hyundai/tests/test_hyundai.py +++ b/selfdrive/car/hyundai/tests/test_hyundai.py @@ -2,24 +2,19 @@ import unittest from cereal import car -from selfdrive.car.car_helpers import get_interface_attr -from selfdrive.car.fw_versions import FW_QUERY_CONFIGS -from selfdrive.car.hyundai.values import CANFD_CAR +from selfdrive.car.hyundai.values import CANFD_CAR, FW_QUERY_CONFIG, FW_VERSIONS Ecu = car.CarParams.Ecu - ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} -VERSIONS = get_interface_attr("FW_VERSIONS", ignore_none=True) class TestHyundaiFingerprint(unittest.TestCase): def test_auxiliary_request_ecu_whitelist(self): # Asserts only auxiliary Ecus can exist in database for CAN-FD cars - config = FW_QUERY_CONFIGS['hyundai'] - whitelisted_ecus = {ecu for r in config.requests for ecu in r.whitelist_ecus if r.bus > 3} + whitelisted_ecus = {ecu for r in FW_QUERY_CONFIG.requests for ecu in r.whitelist_ecus if r.bus > 3} for car_model in CANFD_CAR: - ecus = {fw[0] for fw in VERSIONS['hyundai'][car_model].keys()} + ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()} ecus_not_in_whitelist = ecus - whitelisted_ecus ecu_strings = ", ".join([f'Ecu.{ECU_NAME[ecu]}' for ecu in ecus_not_in_whitelist]) self.assertEqual(len(ecus_not_in_whitelist), 0, f'{car_model}: Car model has ECUs not in auxiliary request whitelists: {ecu_strings}') diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index cf6a9f48eab12d..54bcc59e3dba3f 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -35,7 +35,7 @@ def __init__(self, CP): # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.IONIQ, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -108,6 +108,7 @@ class CAR: KIA_SELTOS = "KIA SELTOS 2021" KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN" KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN" KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN" KIA_STINGER = "KIA STINGER GT2 2018" @@ -125,9 +126,10 @@ class CAR: class Footnote(Enum): + # footnotes which mention "red panda" will be replaced with the CAN FD panda kit on the shop page CANFD = CarFootnote( - "Requires a red panda, additional harness box, " + - "additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.", + "Requires a red panda for this CAN FD car. " + + "All the hardware needed is sold in the CAN FD kit.", Column.MODEL, shop_footnote=True) @@ -146,9 +148,12 @@ def init_make(self, CP: car.CarParams): HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e), HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e), ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), + CAR.ELANTRA_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.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages + 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), + ], 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), @@ -163,7 +168,7 @@ def init_make(self, CP: car.CarParams): CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", "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-22", "All", "https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), + CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", "https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e), CAR.TUCSON: [ HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), @@ -196,7 +201,10 @@ def init_make(self, CP: car.CarParams): HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), HyundaiCarInfo("Kia Niro EV 2022", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), ], - 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), + 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), + ], CAR.KIA_NIRO_HEV_2021: [ HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), @@ -216,7 +224,8 @@ def init_make(self, CP: car.CarParams): HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", "https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c), HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e), ], - CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), + CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2022-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), @@ -228,11 +237,14 @@ def init_make(self, CP: car.CarParams): ], # Genesis - CAR.GENESIS_GV60_EV_1ST_GEN: HyundaiCarInfo("Genesis GV60 2023", "All", harness=Harness.hyundai_k), + 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), + ], 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 2017-19", "All", harness=Harness.hyundai_h), + 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), } @@ -364,6 +376,13 @@ class Buttons: ) FW_VERSIONS = { + CAR.HYUNDAI_GENESIS: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS 1.1 -150210', + b'\xf1\x00DH LKAS 1.4 -140110', + b'\xf1\x00DH LKAS 1.5 -140425', + ], + }, CAR.IONIQ: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', @@ -451,11 +470,13 @@ class Buttons: b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7501 4AEEC104', b'\xf1\x00AE MDPS C 1.00 1.03 56310/G7300 4AEEC103', b'\xf1\x00AE MDPS C 1.00 1.03 56310G7300\x00 4AEEC103', + b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7301 4AEEC104', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', b'\xf1\x00AEE MFC AT USA LHD 1.00 1.00 95740-G2400 180222', b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G2300 170703', + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G2400 180222', ], }, CAR.IONIQ_HEV_2022: { @@ -518,6 +539,8 @@ class Buttons: b'HM6M1_0a0_G20', b'HM6M2_0a0_BD0', b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', + b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M00', + b'\xf1\x81HM6M1_0a0_G20', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware @@ -535,6 +558,7 @@ class Buttons: b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101', b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC102', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', @@ -544,6 +568,7 @@ class Buttons: b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.03 99211-L0000 210603', b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.05 99211-L1000 201109', b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.06 99211-L1000 210325', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.07 99211-L1000 211223', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', @@ -554,6 +579,7 @@ class Buttons: b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', 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\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE', b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', @@ -605,6 +631,7 @@ class Buttons: b'\xf1\x87SALFBA7460044GJ2gx\x87\x88Vf\x86hx\x88\x87\x88wwwwgw\x86wd?\xfa\xff\x86U_\xff\xaf\x1f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SAMFBA8105254GJ2wx\x87\x88Vf\x86hx\x88\x87\x88wwwwwwww\x86O\xfa\xff\x99\x88\x7f\xffZG\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', b'\xf1\x87SANFB45889451GC7wx\x87\x88gw\x87x\x88\x88x\x88\x87wxw\x87wxw\x87\x8f\xfc\xffeU\x8f\xff+Q\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSA\xb9\x13\xf9p', ], }, CAR.SONATA_LF: { @@ -761,19 +788,23 @@ class Buttons: }, CAR.SANTA_FE_HEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + b'\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', + b'\xf1\x00TM MDPS R 1.00 1.05 57700-CL000 4TSHP105', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TMH MFC AT EUR LHD 1.00 1.06 99211-S1500 220727', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16UA3I\x94\xac\x8f', b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x87391312MTC1', + b'\xf1\x87391312MTE0', ], }, CAR.SANTA_FE_PHEV_2022: { @@ -800,6 +831,7 @@ class Buttons: b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 ', b'\xf1\x00CK__ SCC F_CUP 1.00 1.03 96400-J5100 ', b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5000 ', + b'\xf1\x00CK__ SCC F_CUP 1.00 1.02 96400-J5100 ', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81606DE051\x00\x00\x00\x00\x00\x00\x00\x00', @@ -807,6 +839,7 @@ class Buttons: b'\xf1\x82CKJN3TMSDE0B\x00\x00\x00\x00', b'\xf1\x82CKKN3TMD_H0A\x00\x00\x00\x00', b'\xe0\x19\xff\xe7\xe7g\x01\xa2\x00\x0f\x00\x9e\x00\x06\x00\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\x00\x00\x0f\x0e\x0f\x0f\x0e\r\x00\x00\x7f\x02.\xff\x00\x00~p\x00\x00\x00\x00u\xff\xf9\xff\x00\x00\x00\x00V\t\xd5\x01\xc0\x00\x00\x00\x007\xfb\xfc\x0b\x8d\x00', + b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5200 4C2CL104', @@ -814,6 +847,7 @@ class Buttons: b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104', b'\xf1\x00CK MDPS R 1.00 1.06 57700-J5420 4C4VL106', b'\xf1\x00CK MDPS R 1.00 1.07 57700-J5220 4C2VL107', + b'\xf1\x00CK MDPS R 1.00 1.06 57700-J5220 4C2VL106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822', @@ -821,6 +855,7 @@ class Buttons: b'\xf1\x00CK MFC AT EUR LHD 1.00 1.03 95740-J5000 170822', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SCK0T33NB2\xb3\xee\xba\xdc', b'\xf1\x87VCJLE17622572DK0vd6D\x99\x98y\x97vwVffUfvfC%CuT&Dx\x87o\xff{\x1c\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x89E21\x00\x00\x00\x00\x00\x00\x00\xf1\x82SCK0T33NB0', @@ -1012,6 +1047,25 @@ class Buttons: b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.GENESIS_G80: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DH__ SCC F-CUP 1.00 1.01 96400-B1120 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.03 95895-B1500 180713', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.02 95895-B1500 170810', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.01 95895-B1500 161014', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.GENESIS_G90: { (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7'], (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 '], @@ -1186,22 +1240,27 @@ class Buttons: (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91", b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', + b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL2&[\xc3\x01', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', + b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', + b'\xf1\x00DEP MFC AT USA LHD 1.00 1.05 99211-G5000 190826', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ', + b'\xf1\x00DEhe SCC F-CUP 1.00 1.02 99110-G5100 ', ], }, CAR.KIA_NIRO_HEV_2021: { @@ -1313,25 +1372,26 @@ class Buttons: (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', + b'\xf1\x00CN7_ SCC FNCUP 1.00 1.01 99110-AA000 ', b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106', b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', - b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06', + b'\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AB000 220426', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', - b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01', ], @@ -1349,6 +1409,7 @@ class Buttons: b'\xf1\x81HM6M2_0a0_FF0', b'\xf1\x82CNCVD0AMFCXCSFFB', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_G80', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_HC0', ], }, CAR.ELANTRA_HEV_2021: { @@ -1463,6 +1524,7 @@ class Buttons: b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.05 99210-CV000 211027', b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328', b'\xf1\x00CV1 MFC AT EUR RHD 1.00 1.00 99210-CV100 220630', + b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630', ], }, CAR.IONIQ_5: { @@ -1474,11 +1536,13 @@ class Buttons: b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007', ], }, CAR.TUCSON_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9240 14T', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', @@ -1523,19 +1587,30 @@ class Buttons: 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', ], (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 ', ], }, CAR.GENESIS_GV60_EV_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215', + b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU000 211215', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ', ], }, + CAR.KIA_SORENTO_4TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.05 99210-R5000 210623', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00MQ4_ SCC FHCUP 1.00 1.06 99110-P2000 ', + ], + }, CAR.KIA_NIRO_HEV_2ND_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.08 99211-AT000 220531', @@ -1561,10 +1636,10 @@ class Buttons: "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022}, } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_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_NIRO_HEV_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} # 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} +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } @@ -1629,5 +1704,6 @@ class Buttons: CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), + CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ad1fd22b906921..7192f5252c83cd 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -88,13 +88,14 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @classmethod - def get_params(cls, candidate: str, fingerprint: Optional[Dict[int, Dict[int, int]]] = None, car_fw: Optional[List[car.CarParams.CarFw]] = None, experimental_long: bool = False): - if fingerprint is None: - fingerprint = gen_empty_fingerprint() - - if car_fw is None: - car_fw = list() + def get_non_essential_params(cls, candidate: str): + """ + Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints. + """ + return cls.get_params(candidate, gen_empty_fingerprint(), list(), False) + @classmethod + def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool): ret = CarInterfaceBase.get_std_params(candidate) ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fdd2439ff9a612..65444ff6e075d5 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 386e8590893e7b..2e769cf662300f 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -19,7 +19,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle - ret.radarOffCan = True + ret.radarUnavailable = True if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 6371b56be849cf..e9af828e2b5576 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -39,7 +39,7 @@ class NissanCarInfo(CarInfo): CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), - CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"), + CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22", 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), diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 22468801eca7f9..f94333baabd70b 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "subaru" - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.autoResumeSng = False diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index eaab25647d378d..44ffa17074a58d 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -18,6 +18,7 @@ non_tested_cars = [ FORD.ESCAPE_MK4, FORD.FOCUS_MK4, + FORD.MAVERICK_MK1, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU, @@ -25,6 +26,7 @@ HYUNDAI.GENESIS_G90, HYUNDAI.KIA_OPTIMA_H, HONDA.ODYSSEY_CHN, + VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter ] CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,)) @@ -42,6 +44,7 @@ CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500), 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("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6), #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), @@ -72,7 +75,7 @@ CarTestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL), CarTestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.INSIGHT), CarTestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.PILOT), - CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PASSPORT), + CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PILOT), # Passport CarTestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.CIVIC_BOSCH), CarTestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), CarTestRoute("54fd8451b3974762|2021-04-01--14-50-10", HONDA.RIDGELINE), @@ -100,6 +103,7 @@ CarTestRoute("db68bbe12250812c|2022-12-05--00-54-12", HYUNDAI.TUCSON_4TH_GEN), # 2023 CarTestRoute("36e10531feea61a4|2022-07-25--13-37-42", HYUNDAI.TUCSON_HYBRID_4TH_GEN), CarTestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), + CarTestRoute("1d0d000db3370fd0|2023-01-04--22-28-42", HYUNDAI.KIA_SORENTO_4TH_GEN, segment=5), CarTestRoute("628935d7d3e5f4f7|2022-11-30--01-12-46", HYUNDAI.KIA_SORENTO_PHEV_4TH_GEN), CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.PALISADE), CarTestRoute("05a8f0197fdac372|2022-10-19--14-14-09", HYUNDAI.IONIQ_5), # HDA2 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 48d85584b3d51f..24c995a2d08d8d 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -25,7 +25,7 @@ def test_car_interfaces(self, car_name): car_fw = [] - car_params = CarInterface.get_params(car_name, fingerprints, car_fw) + car_params = CarInterface.get_params(car_name, fingerprints, car_fw, experimental_long=False) car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -35,6 +35,12 @@ def test_car_interfaces(self, car_name): self.assertGreater(car_params.centerToFront, 0) self.assertGreater(car_params.maxLateralAccel, 0) + # Longitudinal sanity checks + self.assertEqual(len(car_params.longitudinalTuning.kpV), len(car_params.longitudinalTuning.kpBP)) + self.assertEqual(len(car_params.longitudinalTuning.kiV), len(car_params.longitudinalTuning.kiBP)) + self.assertEqual(len(car_params.longitudinalTuning.deadzoneV), len(car_params.longitudinalTuning.deadzoneBP)) + + # Lateral sanity checks if car_params.steerControlType != car.CarParams.SteerControlType.angle: tune = car_params.lateralTuning if tune.which() == 'pid': @@ -70,7 +76,7 @@ def test_car_interfaces(self, car_name): # Run radar interface once radar_interface.update([]) - if not car_params.radarOffCan and radar_interface.rcp is not None and \ + if not car_params.radarUnavailable and radar_interface.rcp is not None and \ hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): radar_interface._update([radar_interface.trigger_msg]) diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index 2e43103852d748..e9f23145cd109a 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -67,7 +67,7 @@ def test_blacklisted_ecus(self): blacklisted_addrs = (0x7c4, 0x7d0) # includes A/C ecu and an unknown ecu for car_model, ecus in FW_VERSIONS.items(): with self.subTest(car_model=car_model): - CP = interfaces[car_model][0].get_params(car_model) + CP = interfaces[car_model][0].get_non_essential_params(car_model) if CP.carName == 'subaru': for ecu in ecus.keys(): self.assertNotIn(ecu[1], blacklisted_addrs, f'{car_model}: Blacklisted ecu: (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])})') diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py index a82c20ce012ef3..3efdd7404e3afd 100755 --- a/selfdrive/car/tests/test_lateral_limits.py +++ b/selfdrive/car/tests/test_lateral_limits.py @@ -40,7 +40,7 @@ class TestLateralLimits(unittest.TestCase): @classmethod def setUpClass(cls): CarInterface, _, _ = interfaces[cls.car_model] - CP = CarInterface.get_params(cls.car_model) + CP = CarInterface.get_non_essential_params(cls.car_model) if CP.dashcamOnly: raise unittest.SkipTest("Platform is behind dashcamOnly") diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 641c109316bedd..4627b9a570c61e 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -238,14 +238,14 @@ def test_panda_safety_carstate(self): # TODO: check rest of panda's carstate (steering, ACC main on, etc.) checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() - if self.CP.carName not in ("hyundai", "volkswagen", "body"): + if self.CP.carName not in ("hyundai", "body"): # TODO: fix standstill mismatches for other makes checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving() # TODO: remove this exception once this mismatch is resolved brake_pressed = CS.brakePressed if CS.brakePressed and not self.safety.get_brake_pressed_prev(): - if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05: + if self.CP.carFingerprint in (HONDA.PILOT, HONDA.RIDGELINE) and CS.brake > 0.05: brake_pressed = False checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev() diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index bb24818fc28219..e3dbb657c8d9b6 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -12,9 +12,11 @@ TESLA AP1 MODEL S: [.nan, 2.5, .nan] TESLA AP2 MODEL S: [.nan, 2.5, .nan] # Guess +FORD BRONCO SPORT 1ST GEN: [.nan, 1.5, .nan] FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] +FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan] ### # No steering wheel @@ -36,6 +38,7 @@ KIA SPORTAGE HYBRID 5TH GEN: [2.5, 2.5, 0.1] GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1] KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1] GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] +KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1] KIA NIRO HYBRID 2ND GEN: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index eb1a04cee61f23..397b29525dd4d2 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -22,7 +22,6 @@ HONDA FIT 2018: [1.5719981427109775, 0.5712761407108976, 0.110773383324281] HONDA HRV 2019: [2.0661212805710205, 0.7521343418694775, 0.17760375789242094] HONDA INSIGHT 2019: [1.5201671214069354, 0.5660229120683284, 0.25808042580281876] HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] -HONDA PASSPORT 2021: [1.5305538930036766, 0.7956063674638759, 0.19599407381531284] HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index aeb2e6f280177a..56057dfae01d3b 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -59,6 +59,7 @@ SKODA SCALA 1ST GEN: SKODA SUPERB 3RD GEN SKODA KODIAQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAROQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAMIQ 1ST GEN: SKODA SUPERB 3RD GEN +VOLKSWAGEN CRAFTER 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-ROC 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-CROSS 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN TOURAN 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6dbdd4b5d9f7d7..222cf70613a5fa 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -143,7 +143,7 @@ def update(self, CC, CS): # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True - if self.frame % 100 == 0 or send_ui: + if self.frame % 20 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 64a0bed0b20248..6c2b8659820ee5 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -125,6 +125,7 @@ class ToyotaCarInfo(CarInfo): ], CAR.COROLLAH_TSS2: [ ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), + ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), ToyotaCarInfo("Lexus UX Hybrid 2019-22"), ], @@ -230,7 +231,7 @@ class ToyotaCarInfo(CarInfo): # FIXME: On some models, abs can sometimes be missing Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS], # On some models, the engine can show on two different addresses - Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.LEXUS_IS], + Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.LEXUS_IS, CAR.LEXUS_RC], } ) @@ -765,6 +766,7 @@ class ToyotaCarInfo(CarInfo): b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02312U5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -831,6 +833,7 @@ class ToyotaCarInfo(CarInfo): b'\x01896630ZJ1000\x00\x00\x00\x00', b'\x01896630ZU8000\x00\x00\x00\x00', b'\x01896637621000\x00\x00\x00\x00', + b'\x01896637623000\x00\x00\x00\x00', b'\x01896637624000\x00\x00\x00\x00', b'\x01896637626000\x00\x00\x00\x00', b'\x01896637639000\x00\x00\x00\x00', @@ -839,6 +842,7 @@ class ToyotaCarInfo(CarInfo): b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZK8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -1737,22 +1741,29 @@ class ToyotaCarInfo(CarInfo): ], }, CAR.LEXUS_RC: { + (Ecu.engine, 0x700, None): [ + b'\x01896632478200\x00\x00\x00\x00', + ], (Ecu.engine, 0x7e0, None): [ b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ + b'F152624150\x00\x00\x00\x00\x00\x00', b'F152624221\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ + b'881512407000\x00\x00\x00\x00', b'881512409100\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B24081\x00\x00\x00\x00\x00\x00', + b'8965B24320\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F2401200\x00\x00\x00\x00', b'8646F2402200\x00\x00\x00\x00', ], }, diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 7ea9aa871bb102..64d124688043a5 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -44,7 +44,7 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgo < 0.1 + ret.standstill = ret.vEgoRaw == 0 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. @@ -160,7 +160,7 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): # vEgo obtained from Bremse_1 vehicle speed rather than Bremse_3 wheel speeds because Bremse_3 isn't present on NSF ret.vEgoRaw = pt_cp.vl["Bremse_1"]["Geschwindigkeit_neu__Bremse_1_"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.vEgo < 0.1 + ret.standstill = ret.vEgoRaw == 0 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index da0ce25afa62ad..1e620b4f73eb4c 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -23,7 +23,7 @@ def __init__(self, CP, CarController, CarState): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" - ret.radarOffCan = True + ret.radarUnavailable = True use_off_car_defaults = len(fingerprint[0]) == 0 # Pick sensible carParams during offline doc generation/CI jobs @@ -107,6 +107,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.mass = 2011 + STD_CARGO_KG ret.wheelbase = 2.98 + elif candidate == CAR.CRAFTER_MK2: + ret.mass = 2100 + STD_CARGO_KG + ret.wheelbase = 3.64 # SWB, LWB is 4.49, TBD how to detect difference + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + elif candidate == CAR.GOLF_MK7: ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index d1f6bfb02c441e..9f6dee26893cec 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -111,6 +111,7 @@ class CANBUS: class CAR: ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport + CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN" # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants @@ -122,7 +123,7 @@ class CAR: TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California - TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW VW T-Roc and variants + TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW T-Roc and variants AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants @@ -184,6 +185,13 @@ def init_make(self, CP: car.CarParams): VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), VWCarInfo("Volkswagen Teramont X 2021-22"), ], + CAR.CRAFTER_MK2: [ + VWCarInfo("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), + ], CAR.GOLF_MK7: [ VWCarInfo("Volkswagen e-Golf 2014-20"), VWCarInfo("Volkswagen Golf 2015-20"), @@ -352,6 +360,23 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0907572P \xf1\x890682', ], }, + CAR.CRAFTER_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906056EK\xf1\x896391', + ], + # Only current upstreamed vehicle has a manual transmission + #(Ecu.transmission, 0x7e1, None): [ + #], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BG\xf1\x890703\xf1\x82\x0e16120016130012051G1313052900', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + ], + }, CAR.GOLF_MK7: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906016A \xf1\x897697', @@ -373,6 +398,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704L906056CR\xf1\x895813', b'\xf1\x8704L906056HE\xf1\x893758', b'\xf1\x8704L906056HN\xf1\x896590', + b'\xf1\x8704L906056HT\xf1\x896591', b'\xf1\x870EA906016A \xf1\x898343', b'\xf1\x870EA906016E \xf1\x894219', b'\xf1\x870EA906016F \xf1\x894238', @@ -405,6 +431,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870CW300041H \xf1\x891010', b'\xf1\x870CW300042F \xf1\x891604', b'\xf1\x870CW300043B \xf1\x891601', + b'\xf1\x870CW300043E \xf1\x891603', b'\xf1\x870CW300044S \xf1\x894530', b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300045 \xf1\x894531', @@ -442,6 +469,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120053114317121C111C9113', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2252229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142405A2252229333463100', b'\xf1\x875Q0959655C \xf1\x890361\xf1\x82\x111413001112120004110415121610169112', @@ -565,6 +593,7 @@ def init_make(self, CP: car.CarParams): (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703N906026E \xf1\x892114', b'\xf1\x8704E906023AH\xf1\x893379', + b'\xf1\x8704L906026DP\xf1\x891538', b'\xf1\x8704L906026ET\xf1\x891990', b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', @@ -583,6 +612,7 @@ def init_make(self, CP: car.CarParams): ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', + b'\xf1\x873Q0959655AF\xf1\x890195\xf1\x82\r56140056130012026612120211', b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111', b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', @@ -593,6 +623,7 @@ def init_make(self, CP: car.CarParams): (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00711A1', + b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514B0060703', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', @@ -601,6 +632,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020600', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572A \xf1\x890126', b'\xf1\x873Q0907572A \xf1\x890130', b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572C \xf1\x890195', @@ -703,10 +735,12 @@ def init_make(self, CP: car.CarParams): b'\xf1\x875N0906259 \xf1\x890002', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115B \xf1\x890005', + b'\xf1\x8783A907115F \xf1\x890002', b'\xf1\x8783A907115G \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158DT\xf1\x893698', + b'\xf1\x8709G927158FM\xf1\x893757', b'\xf1\x8709G927158GC\xf1\x893821', b'\xf1\x8709G927158GD\xf1\x893820', b'\xf1\x870D9300043 \xf1\x895202', @@ -933,25 +967,33 @@ def init_make(self, CP: car.CarParams): b'\xf1\x8704L906021EL\xf1\x897542', b'\xf1\x8704L906026BP\xf1\x891198', b'\xf1\x8704L906026BP\xf1\x897608', + b'\xf1\x8704L906056CR\xf1\x892797', b'\xf1\x8705E906018AS\xf1\x899596', + b'\xf1\x878V0906264H \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041D \xf1\x891004', + b'\xf1\x870CW300041G \xf1\x891003', b'\xf1\x870CW300050J \xf1\x891908', b'\xf1\x870D9300042M \xf1\x895016', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AC\xf1\x890189\xf1\x82\r11110011110011021511110200', + b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11110011110011021511110200', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r12110012120012021612110200', + b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1312001313001305171311052900', b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\0161312001313001305171311052900', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521N01342A1', b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511N01805A0', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521N01309A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521N05808A1', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101', b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572K \xf1\x890402\xf1\x82\x0101', b'\xf1\x875Q0907572P \xf1\x890682', ], }, @@ -1023,6 +1065,7 @@ def init_make(self, CP: car.CarParams): }, CAR.SKODA_OCTAVIA_MK3: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704C906025L \xf1\x896198', b'\xf1\x8704E906016ER\xf1\x895823', b'\xf1\x8704E906027HD\xf1\x893742', b'\xf1\x8704E906027MH\xf1\x894786', @@ -1034,6 +1077,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870CW300041L \xf1\x891601', b'\xf1\x870CW300041N \xf1\x891605', b'\xf1\x870CW300043B \xf1\x891601', + b'\xf1\x870CW300043P \xf1\x891605', b'\xf1\x870D9300041C \xf1\x894936', b'\xf1\x870D9300041J \xf1\x894902', b'\xf1\x870D9300041P \xf1\x894507', @@ -1043,6 +1087,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x873Q0959655AQ\xf1\x890200\xf1\x82\r11120011100010312212113100', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100', + b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\x0e3221003221002105755331052100', b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100', b'\xf1\x875QD959655 \xf1\x890388\xf1\x82\x111101000011110006110411111111119111', ], @@ -1069,6 +1114,7 @@ def init_make(self, CP: car.CarParams): b'\xf1\x870CW300050 \xf1\x891709', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1211110411110411--04040404131111112H14', b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\022111104111104112104040404111111112H14', ], (Ecu.eps, 0x712, None): [ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fea19adbdbbfea..fff6bcf576c638 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,7 +17,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature -from selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI @@ -99,11 +99,12 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): get_one_can(self.can_sock) num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], num_pandas) + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") and not is_release_branch() + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) else: self.CI, self.CP = CI, CI.CP - self.joystick_mode = self.params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) + self.joystick_mode = self.params.get_bool("JoystickDebugMode") or self.CP.notCar # set alternative experiences from parameters self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") @@ -132,9 +133,6 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] - if is_release_branch(): - self.CP.experimentalLongitudinalAvailable = False - # Write CarParams for radard cp_bytes = self.CP.to_bytes() self.params.put("CarParams", cp_bytes) @@ -142,7 +140,7 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): put_nonblocking("CarParamsPersistent", cp_bytes) # cleanup old params - if not self.CP.experimentalLongitudinalAvailable: + if not self.CP.experimentalLongitudinalAvailable or is_release_branch(): self.params.remove("ExperimentalLongitudinalEnabled") if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") @@ -175,6 +173,7 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): self.cruise_mismatch_counter = 0 self.can_rcv_timeout_counter = 0 self.last_blinker_frame = 0 + self.last_steering_pressed_frame = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] @@ -569,9 +568,11 @@ def state_control(self, CS): CC = car.CarControl.new_message() CC.enabled = self.enabled + # Check which actuators can be enabled + standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl actuators = CC.actuators @@ -606,6 +607,7 @@ def state_control(self, CS): actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) + actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -622,29 +624,34 @@ def state_control(self, CS): lac_log.output = actuators.steer lac_log.saturated = abs(actuators.steer) >= 0.9 + if CS.steeringPressed: + self.last_steering_pressed_frame = self.sm.frame + recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 + # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: - undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 - turning = abs(lac_log.desiredLateralAccel) > 1.0 - good_speed = CS.vEgo > 5 - max_torque = abs(self.last_actuators.steer) > 0.99 - if undershooting and turning and good_speed and max_torque: - self.events.add(EventName.steerSaturated) - elif lac_log.active and lac_log.saturated: - dpath_points = lat_plan.dPathPoints - if len(dpath_points): - # Check if we deviated from the path - # TODO use desired vs actual curvature - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - steering_value = actuators.steeringAngleDeg - else: - steering_value = actuators.steer + if lac_log.active and not recent_steer_pressed: + if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: + undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 + turning = abs(lac_log.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.last_actuators.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + lac_log.active and self.events.add(EventName.steerSaturated) + elif lac_log.saturated: + dpath_points = lat_plan.dPathPoints + if len(dpath_points): + # Check if we deviated from the path + # TODO use desired vs actual curvature + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + steering_value = actuators.steeringAngleDeg + else: + steering_value = actuators.steer - left_deviation = steering_value > 0 and dpath_points[0] < -0.20 - right_deviation = steering_value < 0 and dpath_points[0] > 0.20 + left_deviation = steering_value > 0 and dpath_points[0] < -0.20 + right_deviation = steering_value < 0 and dpath_points[0] > 0.20 - if left_deviation or right_deviation: - self.events.add(EventName.steerSaturated) + if left_deviation or right_deviation: + self.events.add(EventName.steerSaturated) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: @@ -755,7 +762,6 @@ def publish_logs(self, CS, start_time, CC, lac_log): controlsState.alertType = current_alert.alert_type controlsState.alertSound = current_alert.audible_alert - controlsState.canMonoTimes = list(CS.canMonoTimes) controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index cefbc1078c183a..d38959c560406c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -3,7 +3,7 @@ from common.numpy_fast import clip from common.realtime import DT_CTRL -MIN_STEER_SPEED = 0.3 +MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s class LatControl(ABC): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index d692f80b4ba48b..9ed140d38e2b07 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,7 +1,7 @@ import math from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees @@ -14,7 +14,7 @@ def __init__(self, CP, CI): def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) else: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 2bc3cef76bf7b2..dca82c67293b03 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,7 +5,7 @@ from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl class LatControlINDI(LatControl): @@ -82,7 +82,7 @@ def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_ rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: indi_log.active = False self.steer_filter.x = 0.0 output_steer = 0 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6bd678073e70aa..6696d2e30408c0 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,7 +1,7 @@ import math from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.pid import PIDController @@ -28,7 +28,7 @@ def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_ pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = error - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: output_steer = 0.0 pid_log.active = False self.pid.reset() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d10d39d94549cf..2f560943798eba 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -2,7 +2,7 @@ from cereal import log from common.numpy_fast import interp -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -39,7 +39,7 @@ def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: output_torque = 0.0 pid_log.active = False else: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 545a4c43ff9cc2..e8095813f29550 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -30,10 +30,8 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, long_control_state = LongCtrlState.off else: - if long_control_state == LongCtrlState.off: + if long_control_state in (LongCtrlState.off, LongCtrlState.pid): long_control_state = LongCtrlState.pid - - elif long_control_state == LongCtrlState.pid: if stopping_condition: long_control_state = LongCtrlState.stopping diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index dc27fd27a9ad93..c0179512328d0d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -306,7 +306,7 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.max_a = max_a - def update(self, carstate, radarstate, v_cruise, x, v, a, j): + def update(self, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0f63183237497..0febfbafd9330d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,7 +15,6 @@ from system.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s -AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] @@ -111,9 +110,7 @@ def update(self, sm): self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego if force_slow_decel: - # if required so, force a smooth deceleration - accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) - accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) + v_cruise = 0.0 # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) @@ -122,7 +119,7 @@ def update(self, sm): self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index f15ab2fa5662bf..993774f719f5b0 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -20,7 +20,7 @@ class TestLatControl(unittest.TestCase): @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) def test_saturation(self, car_name, controller): CarInterface, CarController, CarState = interfaces[car_name] - CP = CarInterface.get_params(car_name) + CP = CarInterface.get_non_essential_params(car_name) CI = CarInterface(CP, CarController, CarState) VM = VehicleModel(CP) diff --git a/selfdrive/controls/lib/tests/test_vehicle_model.py b/selfdrive/controls/lib/tests/test_vehicle_model.py index 3e08cb0aa08d30..03d97a7e3f8462 100755 --- a/selfdrive/controls/lib/tests/test_vehicle_model.py +++ b/selfdrive/controls/lib/tests/test_vehicle_model.py @@ -12,7 +12,7 @@ class TestVehicleModel(unittest.TestCase): def setUp(self): - CP = CarInterface.get_params(CAR.CIVIC) + CP = CarInterface.get_non_essential_params(CAR.CIVIC) self.VM = VehicleModel(CP) def test_round_trip_yaw_rate(self): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index db87604e987989..34f0f274fe7b6b 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -35,7 +35,7 @@ def __init__(self, dt): self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] -def laplacian_cdf(x, mu, b): +def laplacian_pdf(x, mu, b): b = max(b, 1e-4) return math.exp(-abs(x-mu)/b) @@ -45,9 +45,9 @@ def match_vision_to_cluster(v_ego, lead, clusters): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) + prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -165,7 +165,6 @@ def update(self, sm, rr): dat.valid = sm.all_checks() and len(rr.errors) == 0 radarState = dat.radarState radarState.mdMonoTime = sm.logMonoTime['modelV2'] - radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py index 8f263a98e75d8e..d5f468f214522c 100755 --- a/selfdrive/controls/tests/test_state_machine.py +++ b/selfdrive/controls/tests/test_state_machine.py @@ -31,7 +31,7 @@ class TestStateMachine(unittest.TestCase): def setUp(self): CarInterface, CarController, CarState = interfaces["mock"] - CP = CarInterface.get_params("mock") + CP = CarInterface.get_non_essential_params("mock") CI = CarInterface(CP, CarController, CarState) self.controlsd = Controls(CI=CI) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index b40c8e304c8336..71c7b34be594c3 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -51,7 +51,7 @@ def cycle_alerts(duration=200, is_metric=False): cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] CS = car.CarState.new_message() - CP = CarInterface.get_params("HONDA CIVIC 2016") + CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016") sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState'] + cameras) diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index ac7e7102d05d31..07ce5ebddb0179 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -52,6 +52,10 @@ class ConfigValues(NamedTuple): b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2019 SANTA FE + b"TM__ SCC F-CUP 1.00 1.00 99110-S1210\x19\x01%\x168 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": diff --git a/selfdrive/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py index 1ea133cc198bad..a18390fef3976a 100755 --- a/selfdrive/debug/internal/fuzz_fw_fingerprint.py +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -28,7 +28,7 @@ for candidate, fws in FWS.items(): fw_dict = {} for (tp, addr, subaddr), fw_list in fws.items(): - fw_dict[(addr, subaddr)] = random.choice(fw_list) + fw_dict[(addr, subaddr)] = [random.choice(fw_list)] matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate) diff --git a/selfdrive/debug/internal/test_paramsd.py b/selfdrive/debug/internal/test_paramsd.py index 3d8e422c352fcb..81524496f71a82 100755 --- a/selfdrive/debug/internal/test_paramsd.py +++ b/selfdrive/debug/internal/test_paramsd.py @@ -20,7 +20,7 @@ DT = 0.01 -CP = CarInterface.get_params(CAR.CIVIC) +CP = CarInterface.get_non_essential_params(CAR.CIVIC) VM = VehicleModel(CP) x, y = 0, 0 # m, m diff --git a/selfdrive/debug/print_docs_diff.py b/selfdrive/debug/print_docs_diff.py index b80428645873d3..103dd52dd83e43 100755 --- a/selfdrive/debug/print_docs_diff.py +++ b/selfdrive/debug/print_docs_diff.py @@ -9,6 +9,7 @@ FOOTNOTE_TAG = "{}" STAR_ICON = '' +VIDEO_ICON = '' COLUMNS = "|" + "|".join([column.value for column in Column]) + "|" COLUMN_HEADER = "|---|---|---|{}|".format("|".join([":---:"] * (len(Column) - 3))) ARROW_SYMBOL = "➡️" @@ -39,8 +40,8 @@ def match_cars(base_cars, new_cars): def build_column_diff(base_car, new_car): row_builder = [] for column in Column: - base_column = base_car.get_column(column, STAR_ICON, FOOTNOTE_TAG) - new_column = new_car.get_column(column, STAR_ICON, FOOTNOTE_TAG) + base_column = base_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + new_column = new_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) if base_column != new_column: row_builder.append(f"{base_column} {ARROW_SYMBOL} {new_column}") @@ -74,11 +75,11 @@ def print_car_info_diff(path): # Removals for car_info in car_removals: - changes["removals"].append(format_row([car_info.get_column(column, STAR_ICON, FOOTNOTE_TAG) for column in Column])) + changes["removals"].append(format_row([car_info.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) # Additions for car_info in car_additions: - changes["additions"].append(format_row([car_info.get_column(column, STAR_ICON, FOOTNOTE_TAG) for column in Column])) + changes["additions"].append(format_row([car_info.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) for new_car, base_car in car_changes: # Column changes diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 8c4dbc55ee8d30..8952405b8eb500 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -126,6 +126,7 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) # type: ignore except (NegativeResponseError, MessageTimeoutError): print("Security access failed!") + print("Open the hood and retry (disables the \"diagnostic firewall\" on newer vehicles)") quit() try: diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 5268902785092c..86a228a6ff3ea0 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -3,3 +3,4 @@ ubloxd_test params_learner paramsd locationd +test/test_glonass_runner diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 4b7fba19b6c933..61a0ed7f424894 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -7,8 +7,14 @@ if GetOption('kaitai'): cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) + glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd) -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) + # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910 + patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES') + env.Depends(patch, glonass) + +glonass_obj = env.Object('generated/glonass.cpp') +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] @@ -20,3 +26,6 @@ lenv.Depends(locationd, libkf) if File("liblocationd.cc").exists(): liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) lenv.Depends(liblocationd, libkf) + +if GetOption('test'): + env.Program("test/test_glonass_runner", ['test/test_glonass_runner.cc', 'test/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp new file mode 100644 index 00000000000000..cd0f96ab68da91 --- /dev/null +++ b/selfdrive/locationd/generated/glonass.cpp @@ -0,0 +1,353 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "glonass.h" + +glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::_read() { + m_idle_chip = m__io->read_bits_int_be(1); + m_string_number = m__io->read_bits_int_be(4); + //m__io->align_to_byte(); + switch (string_number()) { + case 4: { + m_data = new string_4_t(m__io, this, m__root); + break; + } + case 1: { + m_data = new string_1_t(m__io, this, m__root); + break; + } + case 3: { + m_data = new string_3_t(m__io, this, m__root); + break; + } + case 5: { + m_data = new string_5_t(m__io, this, m__root); + break; + } + case 2: { + m_data = new string_2_t(m__io, this, m__root); + break; + } + default: { + m_data = new string_non_immediate_t(m__io, this, m__root); + break; + } + } + m_hamming_code = m__io->read_bits_int_be(8); + m_pad_1 = m__io->read_bits_int_be(11); + m_superframe_number = m__io->read_bits_int_be(16); + m_pad_2 = m__io->read_bits_int_be(8); + m_frame_number = m__io->read_bits_int_be(8); +} + +glonass_t::~glonass_t() { + _clean_up(); +} + +void glonass_t::_clean_up() { + if (m_data) { + delete m_data; m_data = 0; + } +} + +glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_tau_n = false; + f_delta_tau_n = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_4_t::_read() { + m_tau_n_sign = m__io->read_bits_int_be(1); + m_tau_n_value = m__io->read_bits_int_be(21); + m_delta_tau_n_sign = m__io->read_bits_int_be(1); + m_delta_tau_n_value = m__io->read_bits_int_be(4); + m_e_n = m__io->read_bits_int_be(5); + m_not_used_1 = m__io->read_bits_int_be(14); + m_p4 = m__io->read_bits_int_be(1); + m_f_t = m__io->read_bits_int_be(4); + m_not_used_2 = m__io->read_bits_int_be(3); + m_n_t = m__io->read_bits_int_be(11); + m_n = m__io->read_bits_int_be(5); + m_m = m__io->read_bits_int_be(2); +} + +glonass_t::string_4_t::~string_4_t() { + _clean_up(); +} + +void glonass_t::string_4_t::_clean_up() { +} + +int32_t glonass_t::string_4_t::tau_n() { + if (f_tau_n) + return m_tau_n; + m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value())); + f_tau_n = true; + return m_tau_n; +} + +int32_t glonass_t::string_4_t::delta_tau_n() { + if (f_delta_tau_n) + return m_delta_tau_n; + m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value())); + f_delta_tau_n = true; + return m_delta_tau_n; +} + +glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_non_immediate_t::_read() { + m_data_1 = m__io->read_bits_int_be(64); + m_data_2 = m__io->read_bits_int_be(8); +} + +glonass_t::string_non_immediate_t::~string_non_immediate_t() { + _clean_up(); +} + +void glonass_t::string_non_immediate_t::_clean_up() { +} + +glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_5_t::_read() { + m_n_a = m__io->read_bits_int_be(11); + m_tau_c = m__io->read_bits_int_be(32); + m_not_used = m__io->read_bits_int_be(1); + m_n_4 = m__io->read_bits_int_be(5); + m_tau_gps = m__io->read_bits_int_be(22); + m_l_n = m__io->read_bits_int_be(1); +} + +glonass_t::string_5_t::~string_5_t() { + _clean_up(); +} + +void glonass_t::string_5_t::_clean_up() { +} + +glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_x_vel = false; + f_x_accel = false; + f_x = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_1_t::_read() { + m_not_used = m__io->read_bits_int_be(2); + m_p1 = m__io->read_bits_int_be(2); + m_t_k = m__io->read_bits_int_be(12); + m_x_vel_sign = m__io->read_bits_int_be(1); + m_x_vel_value = m__io->read_bits_int_be(23); + m_x_accel_sign = m__io->read_bits_int_be(1); + m_x_accel_value = m__io->read_bits_int_be(4); + m_x_sign = m__io->read_bits_int_be(1); + m_x_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_1_t::~string_1_t() { + _clean_up(); +} + +void glonass_t::string_1_t::_clean_up() { +} + +int32_t glonass_t::string_1_t::x_vel() { + if (f_x_vel) + return m_x_vel; + m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value())); + f_x_vel = true; + return m_x_vel; +} + +int32_t glonass_t::string_1_t::x_accel() { + if (f_x_accel) + return m_x_accel; + m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value())); + f_x_accel = true; + return m_x_accel; +} + +int32_t glonass_t::string_1_t::x() { + if (f_x) + return m_x; + m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value())); + f_x = true; + return m_x; +} + +glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_y_vel = false; + f_y_accel = false; + f_y = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_2_t::_read() { + m_b_n = m__io->read_bits_int_be(3); + m_p2 = m__io->read_bits_int_be(1); + m_t_b = m__io->read_bits_int_be(7); + m_not_used = m__io->read_bits_int_be(5); + m_y_vel_sign = m__io->read_bits_int_be(1); + m_y_vel_value = m__io->read_bits_int_be(23); + m_y_accel_sign = m__io->read_bits_int_be(1); + m_y_accel_value = m__io->read_bits_int_be(4); + m_y_sign = m__io->read_bits_int_be(1); + m_y_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_2_t::~string_2_t() { + _clean_up(); +} + +void glonass_t::string_2_t::_clean_up() { +} + +int32_t glonass_t::string_2_t::y_vel() { + if (f_y_vel) + return m_y_vel; + m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value())); + f_y_vel = true; + return m_y_vel; +} + +int32_t glonass_t::string_2_t::y_accel() { + if (f_y_accel) + return m_y_accel; + m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value())); + f_y_accel = true; + return m_y_accel; +} + +int32_t glonass_t::string_2_t::y() { + if (f_y) + return m_y; + m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value())); + f_y = true; + return m_y; +} + +glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_gamma_n = false; + f_z_vel = false; + f_z_accel = false; + f_z = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_3_t::_read() { + m_p3 = m__io->read_bits_int_be(1); + m_gamma_n_sign = m__io->read_bits_int_be(1); + m_gamma_n_value = m__io->read_bits_int_be(10); + m_not_used = m__io->read_bits_int_be(1); + m_p = m__io->read_bits_int_be(2); + m_l_n = m__io->read_bits_int_be(1); + m_z_vel_sign = m__io->read_bits_int_be(1); + m_z_vel_value = m__io->read_bits_int_be(23); + m_z_accel_sign = m__io->read_bits_int_be(1); + m_z_accel_value = m__io->read_bits_int_be(4); + m_z_sign = m__io->read_bits_int_be(1); + m_z_value = m__io->read_bits_int_be(26); +} + +glonass_t::string_3_t::~string_3_t() { + _clean_up(); +} + +void glonass_t::string_3_t::_clean_up() { +} + +int32_t glonass_t::string_3_t::gamma_n() { + if (f_gamma_n) + return m_gamma_n; + m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value())); + f_gamma_n = true; + return m_gamma_n; +} + +int32_t glonass_t::string_3_t::z_vel() { + if (f_z_vel) + return m_z_vel; + m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value())); + f_z_vel = true; + return m_z_vel; +} + +int32_t glonass_t::string_3_t::z_accel() { + if (f_z_accel) + return m_z_accel; + m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value())); + f_z_accel = true; + return m_z_accel; +} + +int32_t glonass_t::string_3_t::z() { + if (f_z) + return m_z; + m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value())); + f_z = true; + return m_z; +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h new file mode 100644 index 00000000000000..19867ba22b220e --- /dev/null +++ b/selfdrive/locationd/generated/glonass.h @@ -0,0 +1,375 @@ +#ifndef GLONASS_H_ +#define GLONASS_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class glonass_t : public kaitai::kstruct { + +public: + class string_4_t; + class string_non_immediate_t; + class string_5_t; + class string_1_t; + class string_2_t; + class string_3_t; + + glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~glonass_t(); + + class string_4_t : public kaitai::kstruct { + + public: + + string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_4_t(); + + private: + bool f_tau_n; + int32_t m_tau_n; + + public: + int32_t tau_n(); + + private: + bool f_delta_tau_n; + int32_t m_delta_tau_n; + + public: + int32_t delta_tau_n(); + + private: + bool m_tau_n_sign; + uint64_t m_tau_n_value; + bool m_delta_tau_n_sign; + uint64_t m_delta_tau_n_value; + uint64_t m_e_n; + uint64_t m_not_used_1; + bool m_p4; + uint64_t m_f_t; + uint64_t m_not_used_2; + uint64_t m_n_t; + uint64_t m_n; + uint64_t m_m; + glonass_t* m__root; + glonass_t* m__parent; + + public: + bool tau_n_sign() const { return m_tau_n_sign; } + uint64_t tau_n_value() const { return m_tau_n_value; } + bool delta_tau_n_sign() const { return m_delta_tau_n_sign; } + uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; } + uint64_t e_n() const { return m_e_n; } + uint64_t not_used_1() const { return m_not_used_1; } + bool p4() const { return m_p4; } + uint64_t f_t() const { return m_f_t; } + uint64_t not_used_2() const { return m_not_used_2; } + uint64_t n_t() const { return m_n_t; } + uint64_t n() const { return m_n; } + uint64_t m() const { return m_m; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_non_immediate_t : public kaitai::kstruct { + + public: + + string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_non_immediate_t(); + + private: + uint64_t m_data_1; + uint64_t m_data_2; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t data_1() const { return m_data_1; } + uint64_t data_2() const { return m_data_2; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_5_t : public kaitai::kstruct { + + public: + + string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_5_t(); + + private: + uint64_t m_n_a; + uint64_t m_tau_c; + bool m_not_used; + uint64_t m_n_4; + uint64_t m_tau_gps; + bool m_l_n; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t n_a() const { return m_n_a; } + uint64_t tau_c() const { return m_tau_c; } + bool not_used() const { return m_not_used; } + uint64_t n_4() const { return m_n_4; } + uint64_t tau_gps() const { return m_tau_gps; } + bool l_n() const { return m_l_n; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_1_t : public kaitai::kstruct { + + public: + + string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_1_t(); + + private: + bool f_x_vel; + int32_t m_x_vel; + + public: + int32_t x_vel(); + + private: + bool f_x_accel; + int32_t m_x_accel; + + public: + int32_t x_accel(); + + private: + bool f_x; + int32_t m_x; + + public: + int32_t x(); + + private: + uint64_t m_not_used; + uint64_t m_p1; + uint64_t m_t_k; + bool m_x_vel_sign; + uint64_t m_x_vel_value; + bool m_x_accel_sign; + uint64_t m_x_accel_value; + bool m_x_sign; + uint64_t m_x_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t not_used() const { return m_not_used; } + uint64_t p1() const { return m_p1; } + uint64_t t_k() const { return m_t_k; } + bool x_vel_sign() const { return m_x_vel_sign; } + uint64_t x_vel_value() const { return m_x_vel_value; } + bool x_accel_sign() const { return m_x_accel_sign; } + uint64_t x_accel_value() const { return m_x_accel_value; } + bool x_sign() const { return m_x_sign; } + uint64_t x_value() const { return m_x_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_2_t : public kaitai::kstruct { + + public: + + string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_2_t(); + + private: + bool f_y_vel; + int32_t m_y_vel; + + public: + int32_t y_vel(); + + private: + bool f_y_accel; + int32_t m_y_accel; + + public: + int32_t y_accel(); + + private: + bool f_y; + int32_t m_y; + + public: + int32_t y(); + + private: + uint64_t m_b_n; + bool m_p2; + uint64_t m_t_b; + uint64_t m_not_used; + bool m_y_vel_sign; + uint64_t m_y_vel_value; + bool m_y_accel_sign; + uint64_t m_y_accel_value; + bool m_y_sign; + uint64_t m_y_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t b_n() const { return m_b_n; } + bool p2() const { return m_p2; } + uint64_t t_b() const { return m_t_b; } + uint64_t not_used() const { return m_not_used; } + bool y_vel_sign() const { return m_y_vel_sign; } + uint64_t y_vel_value() const { return m_y_vel_value; } + bool y_accel_sign() const { return m_y_accel_sign; } + uint64_t y_accel_value() const { return m_y_accel_value; } + bool y_sign() const { return m_y_sign; } + uint64_t y_value() const { return m_y_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + + class string_3_t : public kaitai::kstruct { + + public: + + string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_3_t(); + + private: + bool f_gamma_n; + int32_t m_gamma_n; + + public: + int32_t gamma_n(); + + private: + bool f_z_vel; + int32_t m_z_vel; + + public: + int32_t z_vel(); + + private: + bool f_z_accel; + int32_t m_z_accel; + + public: + int32_t z_accel(); + + private: + bool f_z; + int32_t m_z; + + public: + int32_t z(); + + private: + bool m_p3; + bool m_gamma_n_sign; + uint64_t m_gamma_n_value; + bool m_not_used; + uint64_t m_p; + bool m_l_n; + bool m_z_vel_sign; + uint64_t m_z_vel_value; + bool m_z_accel_sign; + uint64_t m_z_accel_value; + bool m_z_sign; + uint64_t m_z_value; + glonass_t* m__root; + glonass_t* m__parent; + + public: + bool p3() const { return m_p3; } + bool gamma_n_sign() const { return m_gamma_n_sign; } + uint64_t gamma_n_value() const { return m_gamma_n_value; } + bool not_used() const { return m_not_used; } + uint64_t p() const { return m_p; } + bool l_n() const { return m_l_n; } + bool z_vel_sign() const { return m_z_vel_sign; } + uint64_t z_vel_value() const { return m_z_vel_value; } + bool z_accel_sign() const { return m_z_accel_sign; } + uint64_t z_accel_value() const { return m_z_accel_value; } + bool z_sign() const { return m_z_sign; } + uint64_t z_value() const { return m_z_value; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + +private: + bool m_idle_chip; + uint64_t m_string_number; + kaitai::kstruct* m_data; + uint64_t m_hamming_code; + uint64_t m_pad_1; + uint64_t m_superframe_number; + uint64_t m_pad_2; + uint64_t m_frame_number; + glonass_t* m__root; + kaitai::kstruct* m__parent; + +public: + bool idle_chip() const { return m_idle_chip; } + uint64_t string_number() const { return m_string_number; } + kaitai::kstruct* data() const { return m_data; } + uint64_t hamming_code() const { return m_hamming_code; } + uint64_t pad_1() const { return m_pad_1; } + uint64_t superframe_number() const { return m_superframe_number; } + uint64_t pad_2() const { return m_pad_2; } + uint64_t frame_number() const { return m_frame_number; } + glonass_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // GLONASS_H_ diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp index 9b020735bb0c6b..8e1cb85b956085 100644 --- a/selfdrive/locationd/generated/gps.cpp +++ b/selfdrive/locationd/generated/gps.cpp @@ -274,9 +274,9 @@ gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : } void gps_t::tlm_t::_read() { - m_magic = m__io->read_bytes(1); - if (!(magic() == std::string("\x8B", 1))) { - throw kaitai::validation_not_equal_error(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0")); + m_preamble = m__io->read_bytes(1); + if (!(preamble() == std::string("\x8B", 1))) { + throw kaitai::validation_not_equal_error(std::string("\x8B", 1), preamble(), _io(), std::string("/types/tlm/seq/0")); } m_tlm = m__io->read_bits_int_be(14); m_integrity_status = m__io->read_bits_int_be(1); diff --git a/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h index 293e2e4a05974a..9dfc5031f505c9 100644 --- a/selfdrive/locationd/generated/gps.h +++ b/selfdrive/locationd/generated/gps.h @@ -273,7 +273,7 @@ class gps_t : public kaitai::kstruct { ~tlm_t(); private: - std::string m_magic; + std::string m_preamble; uint64_t m_tlm; bool m_integrity_status; bool m_reserved; @@ -281,7 +281,7 @@ class gps_t : public kaitai::kstruct { gps_t* m__parent; public: - std::string magic() const { return m_magic; } + std::string preamble() const { return m_preamble; } uint64_t tlm() const { return m_tlm; } bool integrity_status() const { return m_integrity_status; } bool reserved() const { return m_reserved; } diff --git a/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp index 5e743e1ee7e95d..34fe1e52caa059 100644 --- a/selfdrive/locationd/generated/ubx.cpp +++ b/selfdrive/locationd/generated/ubx.cpp @@ -89,13 +89,10 @@ void ubx_t::rxm_rawx_t::_read() { m_num_meas = m__io->read_u1(); m_rec_stat = m__io->read_u1(); m_reserved1 = m__io->read_bytes(3); - int l_measurements = num_meas(); m__raw_measurements = new std::vector(); - m__raw_measurements->reserve(l_measurements); m__io__raw_measurements = new std::vector(); - m__io__raw_measurements->reserve(l_measurements); m_measurements = new std::vector(); - m_measurements->reserve(l_measurements); + const int l_measurements = num_meas(); for (int i = 0; i < l_measurements; i++) { m__raw_measurements->push_back(m__io->read_bytes(32)); kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); @@ -184,9 +181,8 @@ void ubx_t::rxm_sfrbx_t::_read() { m_reserved2 = m__io->read_bytes(1); m_version = m__io->read_u1(); m_reserved3 = m__io->read_bytes(1); - int l_body = num_words(); m_body = new std::vector(); - m_body->reserve(l_body); + const int l_body = num_words(); for (int i = 0; i < l_body; i++) { m_body->push_back(m__io->read_u4le()); } diff --git a/selfdrive/locationd/glonass.ksy b/selfdrive/locationd/glonass.ksy new file mode 100644 index 00000000000000..be99f6e497ab37 --- /dev/null +++ b/selfdrive/locationd/glonass.ksy @@ -0,0 +1,176 @@ +# http://gauss.gge.unb.ca/GLONASS.ICD.pdf +# some variables are misprinted but good in the old doc +# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf +meta: + id: glonass + endian: be + bit-endian: be +seq: + - id: idle_chip + type: b1 + - id: string_number + type: b4 + - id: data + type: + switch-on: string_number + cases: + 1: string_1 + 2: string_2 + 3: string_3 + 4: string_4 + 5: string_5 + _: string_non_immediate + - id: hamming_code + type: b8 + - id: pad_1 + type: b11 + - id: superframe_number + type: b16 + - id: pad_2 + type: b8 + - id: frame_number + type: b8 + +types: + string_1: + seq: + - id: not_used + type: b2 + - id: p1 + type: b2 + - id: t_k + type: b12 + - id: x_vel_sign + type: b1 + - id: x_vel_value + type: b23 + - id: x_accel_sign + type: b1 + - id: x_accel_value + type: b4 + - id: x_sign + type: b1 + - id: x_value + type: b26 + instances: + x_vel: + value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value' + x_accel: + value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value' + x: + value: 'x_sign ? (x_value * (-1)) : x_value' + string_2: + seq: + - id: b_n + type: b3 + - id: p2 + type: b1 + - id: t_b + type: b7 + - id: not_used + type: b5 + - id: y_vel_sign + type: b1 + - id: y_vel_value + type: b23 + - id: y_accel_sign + type: b1 + - id: y_accel_value + type: b4 + - id: y_sign + type: b1 + - id: y_value + type: b26 + instances: + y_vel: + value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value' + y_accel: + value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value' + y: + value: 'y_sign ? (y_value * (-1)) : y_value' + string_3: + seq: + - id: p3 + type: b1 + - id: gamma_n_sign + type: b1 + - id: gamma_n_value + type: b10 + - id: not_used + type: b1 + - id: p + type: b2 + - id: l_n + type: b1 + - id: z_vel_sign + type: b1 + - id: z_vel_value + type: b23 + - id: z_accel_sign + type: b1 + - id: z_accel_value + type: b4 + - id: z_sign + type: b1 + - id: z_value + type: b26 + instances: + gamma_n: + value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value' + z_vel: + value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value' + z_accel: + value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value' + z: + value: 'z_sign ? (z_value * (-1)) : z_value' + string_4: + seq: + - id: tau_n_sign + type: b1 + - id: tau_n_value + type: b21 + - id: delta_tau_n_sign + type: b1 + - id: delta_tau_n_value + type: b4 + - id: e_n + type: b5 + - id: not_used_1 + type: b14 + - id: p4 + type: b1 + - id: f_t + type: b4 + - id: not_used_2 + type: b3 + - id: n_t + type: b11 + - id: n + type: b5 + - id: m + type: b2 + instances: + tau_n: + value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value' + delta_tau_n: + value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value' + string_5: + seq: + - id: n_a + type: b11 + - id: tau_c + type: b32 + - id: not_used + type: b1 + - id: n_4 + type: b5 + - id: tau_gps + type: b22 + - id: l_n + type: b1 + string_non_immediate: + seq: + - id: data_1 + type: b64 + - id: data_2 + type: b8 diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch new file mode 100644 index 00000000000000..fa34a8ef15784b --- /dev/null +++ b/selfdrive/locationd/glonass_fix.patch @@ -0,0 +1,13 @@ +diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp +index 5b17bc327..b5c6aa610 100644 +--- a/selfdrive/locationd/generated/glonass.cpp ++++ b/selfdrive/locationd/generated/glonass.cpp +@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass + void glonass_t::_read() { + m_idle_chip = m__io->read_bits_int_be(1); + m_string_number = m__io->read_bits_int_be(4); +- m__io->align_to_byte(); ++ //m__io->align_to_byte(); + switch (string_number()) { + case 4: { + m_data = new string_4_t(m__io, this, m__root); diff --git a/selfdrive/locationd/gps.ksy b/selfdrive/locationd/gps.ksy index 6f5cde316b8b27..893ad1b25bee5e 100644 --- a/selfdrive/locationd/gps.ksy +++ b/selfdrive/locationd/gps.ksy @@ -19,7 +19,7 @@ seq: types: tlm: seq: - - id: magic + - id: preamble contents: [0x8b] - id: tlm type: b14 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 1f13c2b69aa447..e85cd0ad42cfcc 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -3,6 +3,7 @@ import math import os import time +import shutil from collections import defaultdict from concurrent.futures import Future, ProcessPoolExecutor from datetime import datetime @@ -16,7 +17,7 @@ from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -29,17 +30,17 @@ MAX_TIME_GAP = 10 EPHEMERIS_CACHE = 'LaikadEphemeris' DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/" -CACHE_VERSION = 0.1 +CACHE_VERSION = 0.2 POS_FIX_RESIDUAL_THRESHOLD = 100.0 class Laikad: - def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False, - valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), + def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False, + valid_ephem_types=(EphemerisType.NAV,), save_ephemeris=False, use_qcom=False): """ valid_const: GNSS constellation which can be used - auto_fetch_orbits: If true fetch orbits from internet when needed + auto_fetch_navs: If true fetch navs from internet when needed auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere. valid_ephem_types: Valid ephemeris types to be used by AstroDog save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. @@ -47,11 +48,11 @@ def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_ self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER) self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom) - self.auto_fetch_orbits = auto_fetch_orbits + self.auto_fetch_navs = auto_fetch_navs self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None - self.last_fetch_orbits_t = None + self.last_fetch_navs_t = None self.got_first_gnss_msg = False self.last_cached_t = None self.save_ephemeris = save_ephemeris @@ -74,29 +75,30 @@ def load_cache(self): try: cache = json.loads(cache, object_hook=deserialize_hook) - self.astro_dog.add_orbits(cache['orbits']) - self.astro_dog.add_navs(cache['nav']) - self.last_fetch_orbits_t = cache['last_fetch_orbits_t'] + if cache['version'] == CACHE_VERSION: + self.astro_dog.add_navs(cache['navs']) + self.last_fetch_navs_t = cache['last_fetch_navs_t'] + else: + cache['navs'] = {} except json.decoder.JSONDecodeError: cloudlog.exception("Error parsing cache") - timestamp = self.last_fetch_orbits_t.as_datetime() if self.last_fetch_orbits_t is not None else 'Nan' + timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan' cloudlog.debug( - f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and orbits ({sum([len(v) for v in cache['orbits']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['orbits'].keys())} {list(cache['nav'].keys())} " + - f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.orbit_fetched_times._ranges]}") + f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " + + f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}") def cache_ephemeris(self, t: GPSTime): if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): put_nonblocking(EPHEMERIS_CACHE, json.dumps( - {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav}, + {'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs}, cls=CacheSerializer)) cloudlog.debug("Cache saved") self.last_cached_t = t def get_lsq_fix(self, t, measurements): if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: - min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 5 + min_measurements = 7 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 6 position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) - if len(position_solution) < 3: return None position_estimate = position_solution[:3] @@ -137,17 +139,22 @@ def is_ephemeris(self, gnss_msg): if self.use_qcom: return gnss_msg.which() == 'drSvPoly' else: - return gnss_msg.which() == 'ephemeris' + return gnss_msg.which() in ('ephemeris', 'glonassEphemeris') def read_ephemeris(self, gnss_msg): - # TODO this only works on GLONASS if self.use_qcom: # TODO this is not robust to gps week rollover if self.gps_week is None: return ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) else: - ephem = convert_ublox_ephem(gnss_msg.ephemeris) + if gnss_msg.which() == 'ephemeris': + ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris) + elif gnss_msg.which() == 'glonassEphemeris': + ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris) + else: + cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") + return self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) @@ -155,8 +162,13 @@ def process_report(self, new_meas, t): # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] processed_measurements = process_measurements(new_meas, self.astro_dog) - - instant_fix = self.get_lsq_fix(t, processed_measurements) + if self.last_fix_pos is not None: + corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog) + instant_fix = self.get_lsq_fix(t, corrected_measurements) + #instant_fix = self.get_lsq_fix(t, processed_measurements) + else: + corrected_measurements = [] + instant_fix = self.get_lsq_fix(t, processed_measurements) if instant_fix is None: return None else: @@ -164,8 +176,6 @@ def process_report(self, new_meas, t): self.last_fix_t = t self.last_fix_pos = position_estimate self.lat_fix_pos_std = position_std - - corrected_measurements = correct_measurements(processed_measurements, position_estimate, self.astro_dog) if (t*1e9) % 10 == 0: cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements @@ -185,8 +195,8 @@ def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if week > 0: self.got_first_gnss_msg = True latest_msg_t = GPSTime(week, tow) - if self.auto_fetch_orbits: - self.fetch_orbits(latest_msg_t, block) + if self.auto_fetch_navs: + self.fetch_navs(latest_msg_t, block) output = self.process_report(new_meas, t) if output is None: @@ -254,9 +264,9 @@ def init_gnss_localizer(self, est_pos): p_initial_diag[GStates.ECEF_POS] = 1000 ** 2 self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) - def fetch_orbits(self, t: GPSTime, block): - # Download new orbits if 1 hour of orbits data left - if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or abs(t - self.last_fetch_orbits_t) > SECS_IN_MIN): + def fetch_navs(self, t: GPSTime, block): + # Download new navs if 1 hour of navs data left + if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN): astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir ret = None @@ -271,22 +281,22 @@ def fetch_orbits(self, t: GPSTime, block): if ret is not None: if ret[0] is None: - self.last_fetch_orbits_t = ret[2] + self.last_fetch_navs_t = ret[2] else: - self.astro_dog.orbits, self.astro_dog.orbit_fetched_times, self.last_fetch_orbits_t = ret + self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret self.cache_ephemeris(t=t) def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir): astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir) - cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}") start_time = time.monotonic() try: - astro_dog.get_orbit_data(t, only_predictions=True) - cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s") - cloudlog.debug(f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}" + + astro_dog.get_navs(t) + cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s") + cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" + f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}") - return astro_dog.orbits, astro_dog.orbit_fetched_times, t + return astro_dog.navs, astro_dog.navs_fetched_times, t except (DownloadFailed, RuntimeError, ValueError, IOError) as e: cloudlog.warning(f"No orbit data found or parsing failure: {e}") return None, None, t @@ -385,7 +395,15 @@ def process_msg(laikad, gnss_msg, mono_time, block=False): return laikad.process_gnss_msg(gnss_msg, mono_time, block=block) +def clear_tmp_cache(): + if os.path.exists(DOWNLOADS_CACHE_FOLDER): + shutil.rmtree(DOWNLOADS_CACHE_FOLDER) + os.mkdir(DOWNLOADS_CACHE_FOLDER) + + def main(sm=None, pm=None, qc=None): + #clear_tmp_cache() + use_qcom = not Params().get_bool("UbloxAvailable", block=True) if use_qcom or (qc is not None and qc): raw_gnss_socket = "qcomGnss" @@ -397,9 +415,14 @@ def main(sm=None, pm=None, qc=None): if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) + # disable until set as main gps source, to better analyze startup time + use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ + replay = "REPLAY" in os.environ - use_internet = "LAIKAD_NO_INTERNET" not in os.environ - laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom) + if replay or "CI" in os.environ: + use_internet = True + + laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: sm.update() @@ -409,17 +432,15 @@ def main(sm=None, pm=None, qc=None): msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay) if msg is None: + # TODO: beautify this, locationd needs a valid message msg = messaging.new_message("gnssMeasurements") - msg.valid = False - pm.send('gnssMeasurements', msg) 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_orbits: - laikad.fetch_orbits(t, block=replay) - + if laikad.auto_fetch_navs: + laikad.fetch_navs(t, block=replay) if __name__ == "__main__": main() diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index da57fb7ff44c97..32fec7724ad0f7 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -3,8 +3,8 @@ extern "C" { typedef Localizer* Localizer_t; - Localizer *localizer_init() { - return new Localizer(); + Localizer *localizer_init(bool has_ublox) { + return new Localizer(has_ublox); } void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 442adcd34743b3..8941b50248fc22 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -30,10 +30,11 @@ const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s const float GPS_MUL_FACTOR = 10.0; const float GPS_POS_STD_THRESHOLD = 50.0; const float GPS_VEL_STD_THRESHOLD = 5.0; -const float GPS_POS_ERROR_RESET_THRESHOLD = 200.0; -const float GPS_POS_STD_RESET_THRESHOLD = 5.0; +const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; -const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 5.0; +const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; +const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -84,6 +85,8 @@ Localizer::Localizer() { this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); } +Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; } + void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd predicted_state = this->kf->get_x(); MatrixXdr predicted_cov = this->kf->get_P(); @@ -227,7 +230,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData } double sensor_time = 1e-9 * log.getTimestamp(); - + // sensor time and log time should be close if (std::abs(current_time - sensor_time) > 0.1) { LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); @@ -308,7 +311,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R } if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) { - this->gps_valid = false; + //this->gps_valid = false; this->determine_gps_mode(current_time); return; } @@ -316,7 +319,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R double sensor_time = current_time - sensor_time_offset; // Process message - this->gps_valid = true; + //this->gps_valid = true; this->gps_mode = true; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -351,18 +354,17 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); } + this->last_gps_msg = sensor_time; this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { - this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid(); - if (!this->gps_valid) { + if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { this->determine_gps_mode(current_time); return; } - this->gps_mode = true; double sensor_time = log.getMeasTime() * 1e-9; if (ublox_available) @@ -406,29 +408,42 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { - this->gps_valid = false; this->determine_gps_mode(current_time); return; } + // prevent jumping gnss measurements (covered lots, standstill...) + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; + orientation_reset &= !this->standstill; + if (orientation_reset) { + this->orientation_reset_count++; + } + else { + this->orientation_reset_count = 0; + } + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { // always reset on first gps message and if the location is off but the accuracy is high LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } else if (ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD && orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD) { + } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + this->orientation_reset_count = 0; } - this->last_gps_msg = current_time; + this->gps_mode = true; + this->last_gps_msg = sensor_time; this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { this->car_speed = std::abs(log.getVEgo()); - if (log.getStandstill()) { + this->standstill = log.getStandstill(); + if (this->standstill) { this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); } @@ -437,7 +452,7 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - + if (!this->is_timestamp_valid(current_time)) { this->observation_timings_invalid = true; return; @@ -604,7 +619,7 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build } bool Localizer::is_gps_ok() { - return this->gps_valid; + return (this->kf->get_filter_time() - this->last_gps_msg) < 1.0; } bool Localizer::critical_services_valid(std::map critical_services) { @@ -616,7 +631,7 @@ bool Localizer::critical_services_valid(std::map critical_s return true; } -bool Localizer::is_timestamp_valid(double current_time) { +bool Localizer::is_timestamp_valid(double current_time) { double filter_time = this->kf->get_filter_time(); if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { LOGE("Observation timestamp is older than the max rewind threshold of the filter"); @@ -643,19 +658,18 @@ void Localizer::determine_gps_mode(double current_time) { int Localizer::locationd_thread() { ublox_available = Params().getBool("UbloxAvailable", true); - const char* gps_location_socket; if (ublox_available) { gps_location_socket = "gpsLocationExternal"; } else { gps_location_socket = "gpsLocation"; } - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; - PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; bool filterInitialized = false; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 7a282be9d72673..a292a3c936cefc 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -24,6 +24,7 @@ class Localizer { public: Localizer(); + Localizer(bool has_ublox); int locationd_thread(); @@ -77,9 +78,10 @@ class Localizer { double reset_tracker = 0.0; bool device_fell = false; bool gps_mode = false; - bool gps_valid = false; double last_gps_msg = 0; bool ublox_available = true; bool observation_timings_invalid = false; - std::map observation_values_invalid; + std::map observation_values_invalid; + bool standstill = true; + int32_t orientation_reset_count = 0; }; diff --git a/selfdrive/locationd/models/lane_kf.py b/selfdrive/locationd/models/lane_kf.py new file mode 100755 index 00000000000000..4d38fa8e090866 --- /dev/null +++ b/selfdrive/locationd/models/lane_kf.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import sys +import numpy as np +import sympy as sp + +from selfdrive.locationd.models.constants import ObservationKind +from rednose.helpers.ekf_sym import gen_code, EKF_sym + + +class LaneKalman(): + name = 'lane' + + @staticmethod + def generate_code(generated_dir): + # make functions and jacobians with sympy + # state variables + dim = 6 + state = sp.MatrixSymbol('state', dim, 1) + + dd = sp.Symbol('dd') # WARNING: NOT TIME + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim, 1))) + state_dot[:3,0] = sp.Matrix(state[3:6,0]) + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = sp.Matrix(state) + dd*state_dot + + # + # Observation functions + # + h_lane_sym = sp.Matrix(state[:3,0]) + obs_eqs = [[h_lane_sym, ObservationKind.LANE_PT, None]] + gen_code(generated_dir, LaneKalman.name, f_sym, dd, state, obs_eqs, dim, dim) + + def __init__(self, generated_dir, pt_std=5): + # state + # left and right lane centers in ecef + # WARNING: this is not a temporal model + # the 'time' in this kalman filter is + # the distance traveled by the vehicle, + # which should approximately be the + # distance along the lane path + # a more logical parametrization + # states 0-2 are ecef coordinates distance d + # states 3-5 is the 3d "velocity" of the + # lane in ecef (m/m). + x_initial = np.array([0,0,0, + 0,0,0]) + + # state covariance + P_initial = np.diag([1e16, 1e16, 1e16, + 1**2, 1**2, 1**2]) + + # process noise + Q = np.diag([0.1**2, 0.1**2, 0.1**2, + 0.1**2, 0.1**2, 0.1*2]) + + self.dim_state = len(x_initial) + + # init filter + self.filter = EKF_sym(generated_dir, self.name, Q, x_initial, P_initial, x_initial.shape[0], P_initial.shape[0]) + self.obs_noise = {ObservationKind.LANE_PT: np.diag([pt_std**2]*3)} + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + data = np.atleast_2d(data) + return self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i,:,:] = obs_noise + return R + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + LaneKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 9bd4ed08372138..7e30b1e3a7d767 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -14,8 +14,9 @@ MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) +ROLL_STD_MAX = math.radians(1.5) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 @@ -36,10 +37,8 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No self.yaw_rate = 0.0 self.yaw_rate_std = 0.0 self.roll = 0.0 - self.steering_pressed = False self.steering_angle = 0.0 - - self.valid = True + self.roll_valid = False def handle_log(self, t, which, msg): if which == 'liveLocationKalman': @@ -48,8 +47,8 @@ def handle_log(self, t, which, msg): localizer_roll = msg.orientationNED.value[0] localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] - roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX - if roll_valid: + self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + if self.roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? roll_std = 2 * localizer_roll_std @@ -88,10 +87,9 @@ def handle_log(self, t, which, msg): elif which == 'carState': self.steering_angle = msg.steeringAngleDeg - self.steering_pressed = msg.steeringPressed self.speed = msg.vEgo - in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed + in_linear_region = abs(self.steering_angle) < 45 self.active = self.speed > 1 and in_linear_region if self.active: @@ -158,6 +156,7 @@ def main(sm=None, pm=None): learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) angle_offset_average = params['angleOffsetAverageDeg'] angle_offset = angle_offset_average + roll = 0.0 while True: sm.update() @@ -177,6 +176,8 @@ def main(sm=None, pm=None): angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) + roll = clip(float(x[States.ROAD_ROLL]), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA) + roll_std = float(P[States.ROAD_ROLL]) # Account for the opposite signs of the yaw rates sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) @@ -187,12 +188,14 @@ def main(sm=None, pm=None): liveParameters.sensorValid = sensors_valid liveParameters.steerRatio = float(x[States.STEER_RATIO]) liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) - liveParameters.roll = float(x[States.ROAD_ROLL]) + liveParameters.roll = roll liveParameters.angleOffsetAverageDeg = angle_offset_average liveParameters.angleOffsetDeg = angle_offset liveParameters.valid = all(( abs(liveParameters.angleOffsetAverageDeg) < 10.0, abs(liveParameters.angleOffsetDeg) < 10.0, + abs(liveParameters.roll) < ROLL_MAX, + roll_std < ROLL_STD_MAX, 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) diff --git a/selfdrive/locationd/test/_test_locationd_lib.py b/selfdrive/locationd/test/_test_locationd_lib.py index c4a053bbc60bdd..819bb1570ea686 100755 --- a/selfdrive/locationd/test/_test_locationd_lib.py +++ b/selfdrive/locationd/test/_test_locationd_lib.py @@ -19,7 +19,7 @@ class TestLocationdLib(unittest.TestCase): def setUp(self): header = '''typedef ...* Localizer_t; -Localizer_t localizer_init(); +Localizer_t localizer_init(bool has_ublox); void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' @@ -27,7 +27,7 @@ def setUp(self): self.ffi.cdef(header) self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) - self.localizer = self.lib.localizer_init() + self.localizer = self.lib.localizer_init(True) # default to ublox self.buff_size = 2048 self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') diff --git a/selfdrive/locationd/test/test_glonass_kaitai.cc b/selfdrive/locationd/test/test_glonass_kaitai.cc new file mode 100644 index 00000000000000..22f5202a3d03dd --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "selfdrive/locationd/generated/glonass.h" + +typedef std::vector> string_data; + +#define IDLE_CHIP_IDX 0 +#define STRING_NUMBER_IDX 1 +// string data 1-5 +#define HC_IDX 0 +#define PAD1_IDX 1 +#define SUPERFRAME_IDX 2 +#define PAD2_IDX 3 +#define FRAME_IDX 4 + +// Indexes for string number 1 +#define ST1_NU_IDX 2 +#define ST1_P1_IDX 3 +#define ST1_T_K_IDX 4 +#define ST1_X_VEL_S_IDX 5 +#define ST1_X_VEL_V_IDX 6 +#define ST1_X_ACCEL_S_IDX 7 +#define ST1_X_ACCEL_V_IDX 8 +#define ST1_X_S_IDX 9 +#define ST1_X_V_IDX 10 +#define ST1_HC_OFF 11 + +// Indexes for string number 2 +#define ST2_BN_IDX 2 +#define ST2_P2_IDX 3 +#define ST2_TB_IDX 4 +#define ST2_NU_IDX 5 +#define ST2_Y_VEL_S_IDX 6 +#define ST2_Y_VEL_V_IDX 7 +#define ST2_Y_ACCEL_S_IDX 8 +#define ST2_Y_ACCEL_V_IDX 9 +#define ST2_Y_S_IDX 10 +#define ST2_Y_V_IDX 11 +#define ST2_HC_OFF 12 + +// Indexes for string number 3 +#define ST3_P3_IDX 2 +#define ST3_GAMMA_N_S_IDX 3 +#define ST3_GAMMA_N_V_IDX 4 +#define ST3_NU_1_IDX 5 +#define ST3_P_IDX 6 +#define ST3_L_N_IDX 7 +#define ST3_Z_VEL_S_IDX 8 +#define ST3_Z_VEL_V_IDX 9 +#define ST3_Z_ACCEL_S_IDX 10 +#define ST3_Z_ACCEL_V_IDX 11 +#define ST3_Z_S_IDX 12 +#define ST3_Z_V_IDX 13 +#define ST3_HC_OFF 14 + +// Indexes for string number 4 +#define ST4_TAU_N_S_IDX 2 +#define ST4_TAU_N_V_IDX 3 +#define ST4_DELTA_TAU_N_S_IDX 4 +#define ST4_DELTA_TAU_N_V_IDX 5 +#define ST4_E_N_IDX 6 +#define ST4_NU_1_IDX 7 +#define ST4_P4_IDX 8 +#define ST4_F_T_IDX 9 +#define ST4_NU_2_IDX 10 +#define ST4_N_T_IDX 11 +#define ST4_N_IDX 12 +#define ST4_M_IDX 13 +#define ST4_HC_OFF 14 + +// Indexes for string number 5 +#define ST5_N_A_IDX 2 +#define ST5_TAU_C_IDX 3 +#define ST5_NU_IDX 4 +#define ST5_N_4_IDX 5 +#define ST5_TAU_GPS_IDX 6 +#define ST5_L_N_IDX 7 +#define ST5_HC_OFF 8 + +// Indexes for non immediate +#define ST6_DATA_1_IDX 2 +#define ST6_DATA_2_IDX 3 +#define ST6_HC_OFF 4 + + +std::string generate_inp_data(string_data& data) { + std::string inp_data = ""; + for (auto& [b, v] : data) { + std::string tmp = std::bitset<64>(v).to_string(); + inp_data += tmp.substr(64-b, b); + } + assert(inp_data.size() == 128); + + std::string string_data; + string_data.reserve(16); + for (int i = 0; i < 128; i+=8) { + std::string substr = inp_data.substr(i, 8); + string_data.push_back( (uint8_t)std::stoi(substr.c_str(), 0, 2)); + } + + return string_data; +} + +string_data generate_string_data(uint8_t string_number) { + + srand((unsigned)time(0)); + string_data data; // + data.push_back({1, 0}); // idle chip + data.push_back({4, string_number}); // string number + + if (string_number == 1) { + data.push_back({2, 3}); // not_used + data.push_back({2, 1}); // p1 + data.push_back({12, 113}); // t_k + data.push_back({1, rand() & 1}); // x_vel_sign + data.push_back({23, 7122}); // x_vel_value + data.push_back({1, rand() & 1}); // x_accel_sign + data.push_back({4, 3}); // x_accel_value + data.push_back({1, rand() & 1}); // x_sign + data.push_back({26, 33554431}); // x_value + } else if (string_number == 2) { + data.push_back({3, 3}); // b_n + data.push_back({1, 1}); // p2 + data.push_back({7, 123}); // t_b + data.push_back({5, 31}); // not_used + data.push_back({1, rand() & 1}); // y_vel_sign + data.push_back({23, 7422}); // y_vel_value + data.push_back({1, rand() & 1}); // y_accel_sign + data.push_back({4, 3}); // y_accel_value + data.push_back({1, rand() & 1}); // y_sign + data.push_back({26, 67108863}); // y_value + } else if (string_number == 3) { + data.push_back({1, 0}); // p3 + data.push_back({1, 1}); // gamma_n_sign + data.push_back({10, 123}); // gamma_n_value + data.push_back({1, 0}); // not_used + data.push_back({2, 2}); // p + data.push_back({1, 1}); // l_n + data.push_back({1, rand() & 1}); // z_vel_sign + data.push_back({23, 1337}); // z_vel_value + data.push_back({1, rand() & 1}); // z_accel_sign + data.push_back({4, 9}); // z_accel_value + data.push_back({1, rand() & 1}); // z_sign + data.push_back({26, 100023}); // z_value + } else if (string_number == 4) { + data.push_back({1, rand() & 1}); // tau_n_sign + data.push_back({21, 197152}); // tau_n_value + data.push_back({1, rand() & 1}); // delta_tau_n_sign + data.push_back({4, 4}); // delta_tau_n_value + data.push_back({5, 0}); // e_n + data.push_back({14, 2}); // not_used_1 + data.push_back({1, 1}); // p4 + data.push_back({4, 9}); // f_t + data.push_back({3, 3}); // not_used_2 + data.push_back({11, 2047}); // n_t + data.push_back({5, 2}); // n + data.push_back({2, 1}); // m + } else if (string_number == 5) { + data.push_back({11, 2047}); // n_a + data.push_back({32, 4294767295}); // tau_c + data.push_back({1, 0}); // not_used_1 + data.push_back({5, 2}); // n_4 + data.push_back({22, 4114304}); // tau_gps + data.push_back({1, 0}); // l_n + } else { // non-immediate data is not parsed + data.push_back({64, rand()}); // data_1 + data.push_back({8, 6}); // data_2 + } + + data.push_back({8, rand() & 0xFF}); // hamming code + data.push_back({11, rand() & 0x7FF}); // pad + data.push_back({16, rand() & 0xFFFF}); // superframe + data.push_back({8, rand() & 0xFF}); // pad + data.push_back({8, rand() & 0xFF}); // frame + return data; +} + +TEST_CASE("parse_string_number_1"){ + string_data data = generate_string_data(1); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str1(inp_data); + glonass_t str1_data(&str1); + glonass_t::string_1_t* s1 = static_cast(str1_data.data()); + + REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); + REQUIRE(s1->p1() == data[ST1_P1_IDX].second); + REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); + + int mul = s1->x_vel_sign() ? (-1) : 1; + REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); + mul = s1->x_accel_sign() ? (-1) : 1; + REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); + mul = s1->x_sign() ? (-1) : 1; + REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_2"){ + string_data data = generate_string_data(2); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str2(inp_data); + glonass_t str2_data(&str2); + glonass_t::string_2_t* s2 = static_cast(str2_data.data()); + + REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); + REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); + REQUIRE(s2->p2() == data[ST2_P2_IDX].second); + REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); + int mul = s2->y_vel_sign() ? (-1) : 1; + REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); + mul = s2->y_accel_sign() ? (-1) : 1; + REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); + mul = s2->y_sign() ? (-1) : 1; + REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_3"){ + string_data data = generate_string_data(3); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str3(inp_data); + glonass_t str3_data(&str3); + glonass_t::string_3_t* s3 = static_cast(str3_data.data()); + + REQUIRE(s3->p3() == data[ST3_P3_IDX].second); + int mul = s3->gamma_n_sign() ? (-1) : 1; + REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); + REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); + REQUIRE(s3->p() == data[ST3_P_IDX].second); + REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); + mul = s3->z_vel_sign() ? (-1) : 1; + REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); + mul = s3->z_accel_sign() ? (-1) : 1; + REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); + mul = s3->z_sign() ? (-1) : 1; + REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_4"){ + string_data data = generate_string_data(4); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str4(inp_data); + glonass_t str4_data(&str4); + glonass_t::string_4_t* s4 = static_cast(str4_data.data()); + + int mul = s4->tau_n_sign() ? (-1) : 1; + REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); + mul = s4->delta_tau_n_sign() ? (-1) : 1; + REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); + REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); + REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); + REQUIRE(s4->p4() == data[ST4_P4_IDX].second); + REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); + REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); + REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); + REQUIRE(s4->n() == data[ST4_N_IDX].second); + REQUIRE(s4->m() == data[ST4_M_IDX].second); +} + +TEST_CASE("parse_string_number_5"){ + string_data data = generate_string_data(5); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str5(inp_data); + glonass_t str5_data(&str5); + glonass_t::string_5_t* s5 = static_cast(str5_data.data()); + + REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); + REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); + REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); + REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); + REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); + REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); +} + +TEST_CASE("parse_string_number_NI"){ + string_data data = generate_string_data((rand() % 10) + 6); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); + + kaitai::kstream strni(inp_data); + glonass_t strni_data(&strni); + glonass_t::string_non_immediate_t* sni = static_cast(strni_data.data()); + + REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); + REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); +} diff --git a/selfdrive/locationd/test/test_glonass_runner.cc b/selfdrive/locationd/test/test_glonass_runner.cc new file mode 100644 index 00000000000000..62bf7476a18996 --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 198ecfe93574b6..d89f5212285383 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -4,7 +4,7 @@ from collections import defaultdict from datetime import datetime from unittest import mock -from unittest.mock import Mock, patch +from unittest.mock import patch from common.params import Params from laika.constants import SECS_IN_DAY @@ -22,7 +22,24 @@ def get_log(segs=range(0)): logs = [] for i in segs: logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i))) - return [m for m in logs if m.which() == 'ubloxGnss'] + + all_logs = [m for m in logs if m.which() == 'ubloxGnss'] + low_gnss = [] + for m in all_logs: + if m.ubloxGnss.which() != 'measurementReport': + continue + + MAX_MEAS = 7 + if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS: + mb = m.as_builder() + mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS + mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS] + mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000 + low_gnss.append(mb.as_reader()) + else: + low_gnss.append(m) + + return all_logs, low_gnss def verify_messages(lr, laikad, return_one_success=False): @@ -59,55 +76,56 @@ class TestLaikad(unittest.TestCase): @classmethod def setUpClass(cls): - logs = get_log(range(1)) + logs, low_gnss = get_log(range(1)) cls.logs = logs + cls.low_gnss = low_gnss first_gps_time = get_first_gps_time(logs) cls.first_gps_time = first_gps_time def setUp(self): Params().remove(EPHEMERIS_CACHE) - def test_fetch_orbits_non_blocking(self): + def test_fetch_navs_non_blocking(self): gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) laikad = Laikad() - laikad.fetch_orbits(gpstime, block=False) + laikad.fetch_navs(gpstime, block=False) laikad.orbit_fetch_future.result(30) # Get results and save orbits to laikad: - laikad.fetch_orbits(gpstime, block=False) + laikad.fetch_navs(gpstime, block=False) - ephem = laikad.astro_dog.orbits['G01'][0] + ephem = laikad.astro_dog.navs['G01'][0] self.assertIsNotNone(ephem) - laikad.fetch_orbits(gpstime+2*SECS_IN_DAY, block=False) + laikad.fetch_navs(gpstime+2*SECS_IN_DAY, block=False) laikad.orbit_fetch_future.result(30) # Get results and save orbits to laikad: - laikad.fetch_orbits(gpstime + 2 * SECS_IN_DAY, block=False) + laikad.fetch_navs(gpstime + 2 * SECS_IN_DAY, block=False) - ephem2 = laikad.astro_dog.orbits['G01'][0] + ephem2 = laikad.astro_dog.navs['G01'][0] self.assertIsNotNone(ephem) self.assertNotEqual(ephem, ephem2) - def test_fetch_orbits_with_wrong_clocks(self): + def test_fetch_navs_with_wrong_clocks(self): laikad = Laikad() - def check_has_orbits(): - self.assertGreater(len(laikad.astro_dog.orbits), 0) - ephem = laikad.astro_dog.orbits['G01'][0] + def check_has_navs(): + self.assertGreater(len(laikad.astro_dog.navs), 0) + ephem = laikad.astro_dog.navs['G01'][0] self.assertIsNotNone(ephem) real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1)) wrong_future_clock_time = real_current_time + SECS_IN_DAY - laikad.fetch_orbits(wrong_future_clock_time, block=True) - check_has_orbits() - self.assertEqual(laikad.last_fetch_orbits_t, wrong_future_clock_time) + laikad.fetch_navs(wrong_future_clock_time, block=True) + check_has_navs() + self.assertEqual(laikad.last_fetch_navs_t, wrong_future_clock_time) # Test fetching orbits with earlier time - assert real_current_time < laikad.last_fetch_orbits_t + assert real_current_time < laikad.last_fetch_navs_t laikad.astro_dog.orbits = {} - laikad.fetch_orbits(real_current_time, block=True) - check_has_orbits() - self.assertEqual(laikad.last_fetch_orbits_t, real_current_time) + laikad.fetch_navs(real_current_time, block=True) + check_has_navs() + self.assertEqual(laikad.last_fetch_navs_t, real_current_time) def test_ephemeris_source_in_msg(self): data_mock = defaultdict(str) @@ -115,22 +133,22 @@ def test_ephemeris_source_in_msg(self): gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC laikad = Laikad() - laikad.fetch_orbits(gpstime, block=True) - meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + laikad.fetch_navs(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0]) msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) # Verify gps satellite returns same source - meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0]) msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) # Test nasa source by using older date gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) laikad = Laikad() - laikad.fetch_orbits(gpstime, block=True) - meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0]) + laikad.fetch_navs(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0]) msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) # Test nav source type ephem = GPSEphemeris(data_mock, gpstime) @@ -142,7 +160,7 @@ def test_laika_online(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 555 + correct_msgs_expected = 554 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) @@ -161,9 +179,8 @@ def test_kf_becomes_valid(self): def test_laika_online_nav_only(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) # Disable fetch_orbits to test NAV only - laikad.fetch_orbits = Mock() correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 559 + correct_msgs_expected = 554 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) @@ -171,45 +188,45 @@ def test_laika_online_nav_only(self): def test_laika_offline(self, downloader_mock): downloader_mock.side_effect = DownloadFailed("Mock download failed") laikad = Laikad(auto_update=False) - laikad.fetch_orbits(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True) + laikad.fetch_navs(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True) @mock.patch('laika.downloader.download_and_cache_file') def test_download_failed_russian_source(self, downloader_mock): downloader_mock.side_effect = DownloadFailed laikad = Laikad(auto_update=False) correct_msgs = verify_messages(self.logs, laikad) - self.assertEqual(16, len(correct_msgs)) - self.assertEqual(16, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + self.assertEqual(0, len(correct_msgs)) + self.assertEqual(0, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) def test_laika_get_orbits(self): laikad = Laikad(auto_update=False) # Pretend process has loaded the orbits on startup by using the time of the first gps message. - laikad.fetch_orbits(self.first_gps_time, block=True) - self.dict_has_values(laikad.astro_dog.orbits) + laikad.fetch_navs(self.first_gps_time, block=True) + self.dict_has_values(laikad.astro_dog.navs) @unittest.skip("Use to debug live data") - def test_laika_get_orbits_now(self): + def test_laika_get_navs_now(self): laikad = Laikad(auto_update=False) - laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) + laikad.fetch_navs(GPSTime.from_datetime(datetime.utcnow()), block=True) prn = "G01" - self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) + self.assertGreater(len(laikad.astro_dog.navs[prn]), 0) prn = "R01" - self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) - print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + self.assertGreater(len(laikad.astro_dog.navs[prn]), 0) + print(min(laikad.astro_dog.navs[prn], key=lambda e: e.epoch).epoch.as_datetime()) - def test_get_orbits_in_process(self): + def test_get_navs_in_process(self): laikad = Laikad(auto_update=False) - has_orbits = False + has_navs = False for m in self.logs: laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False) if laikad.orbit_fetch_future is not None: laikad.orbit_fetch_future.result() - vals = laikad.astro_dog.orbits.values() - has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0 - if has_orbits: + vals = laikad.astro_dog.navs.values() + has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0 + if has_navs: break - self.assertTrue(has_orbits) - self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) + self.assertTrue(has_navs) + self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0) self.assertEqual(None, laikad.orbit_fetch_future) def test_cache(self): @@ -228,17 +245,16 @@ def wait_for_cache(): wait_for_cache() Params().remove(EPHEMERIS_CACHE) - laikad.astro_dog.get_navs(self.first_gps_time) - laikad.fetch_orbits(self.first_gps_time, block=True) + #laikad.astro_dog.get_navs(self.first_gps_time) + laikad.fetch_navs(self.first_gps_time, block=True) # Wait for cache to save wait_for_cache() # Check both nav and orbits separate laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True) - # Verify orbits and nav are loaded from cache - self.dict_has_values(laikad.astro_dog.orbits) - self.dict_has_values(laikad.astro_dog.nav) + # Verify navs are loaded from cache + self.dict_has_values(laikad.astro_dog.navs) # Verify cache is working for only nav by running a segment msg = verify_messages(self.logs, laikad, return_one_success=True) self.assertIsNotNone(msg) @@ -246,7 +262,7 @@ def wait_for_cache(): with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() - laikad.fetch_orbits(self.first_gps_time, block=False) + laikad.fetch_navs(self.first_gps_time, block=False) mock_method.assert_not_called() # Verify cache is working for only orbits by running a segment @@ -256,6 +272,18 @@ def wait_for_cache(): # Verify orbit data is not downloaded mock_method.assert_not_called() + def test_low_gnss_meas(self): + cnt = 0 + laikad = Laikad() + for m in self.low_gnss: + msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) + if msg is None: + continue + gm = msg.gnssMeasurements + if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid: + cnt += 1 + self.assertEqual(cnt, 554) + def dict_has_values(self, dct): self.assertGreater(len(dct), 0) self.assertGreater(min([len(v) for v in dct.values()]), 0) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 7569530211dbd2..9f643e2b8ffb62 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -8,6 +8,7 @@ import cereal.messaging as messaging from cereal.services import service_list from common.params import Params +from common.transformations.coordinates import ecef2geodetic from selfdrive.manager.process_config import managed_processes @@ -45,16 +46,25 @@ def get_fake_msg(self, name, t): except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) + if name == "gpsLocationExternal": msg.gpsLocationExternal.flags = 1 msg.gpsLocationExternal.verticalAccuracy = 1.0 msg.gpsLocationExternal.speedAccuracy = 1.0 msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] - msg.gpsLocationExternal.latitude = self.lat - msg.gpsLocationExternal.longitude = self.lon + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 - msg.gpsLocationExternal.altitude = self.alt + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] @@ -67,9 +77,11 @@ def test_params_gps(self): # first reset params Params().remove('LastGPSPosition') - self.lat = 30 + (random.random() * 10.0) - self.lon = -70 + (random.random() * 10.0) - self.alt = 5 + (random.random() * 10.0) + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + self.fake_duration = 90 # secs # get fake messages at the correct frequency, listed in services.py fake_msgs = [] @@ -83,7 +95,6 @@ def test_params_gps(self): time.sleep(1) # wait for async params write lastGPS = json.loads(Params().get('LastGPSPosition')) - self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py index 7aa588d43e4255..cd4ce0de04a9ce 100755 --- a/selfdrive/locationd/test/test_ublox_processing.py +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -1,5 +1,5 @@ import unittest - +import time import numpy as np from laika import AstroDog @@ -8,7 +8,8 @@ from laika.opt import calc_pos_fix from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader - +from selfdrive.test.helpers import with_processes +import cereal.messaging as messaging def get_gnss_measurements(log_reader): gnss_measurements = [] @@ -21,6 +22,12 @@ def get_gnss_measurements(log_reader): gnss_measurements.append(read_raw_ublox(report)) return gnss_measurements +def get_ublox_raw(log_reader): + ublox_raw = [] + for msg in log_reader: + if msg.which() == "ubloxRaw": + ublox_raw.append(msg) + return ublox_raw class TestUbloxProcessing(unittest.TestCase): NUM_TEST_PROCESS_MEAS = 10 @@ -30,6 +37,10 @@ def setUpClass(cls): lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) cls.gnss_measurements = get_gnss_measurements(lr) + # test gps ephemeris continuity check (drive has ephemeris issues with cutover data) + lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14)) + cls.ublox_raw = get_ublox_raw(lr) + def test_read_ublox_raw(self): count_gps = 0 count_glonass = 0 @@ -76,6 +87,29 @@ def test_get_fix(self): self.assertEqual(count_processed_measurements, 69) self.assertEqual(count_corrected_measurements, 69) + @with_processes(['ubloxd']) + def test_ublox_gps_cutover(self): + time.sleep(2) + ugs = messaging.sub_sock("ubloxGnss", timeout=0.1) + ur_pm = messaging.PubMaster(['ubloxRaw']) + + def replay_segment(): + rcv_msgs = [] + for msg in self.ublox_raw: + ur_pm.send(msg.which(), msg.as_builder()) + time.sleep(0.01) + rcv_msgs += messaging.drain_sock(ugs) + + time.sleep(0.1) + rcv_msgs += messaging.drain_sock(ugs) + return rcv_msgs + + # replay twice to enforce cutover data on rewind + rcv_msgs = replay_segment() + rcv_msgs += replay_segment() + + ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs) + self.assertEqual(ephems_cnt, 15) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 588bca15788cf6..fcc068d34e5623 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -22,15 +22,15 @@ MIN_VEL = 15 # m/s FRICTION_FACTOR = 1.5 # ~85% of data coverage FACTOR_SANITY = 0.3 +FACTOR_SANITY_QLOG = 0.5 FRICTION_SANITY = 0.5 +FRICTION_SANITY_QLOG = 0.8 STEER_MIN_THRESHOLD = 0.02 MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 LAT_ACC_THRESHOLD = 1 STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) -MAX_RESETS = 5.0 -MAX_INVALID_THRESHOLD = 10 MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches @@ -100,10 +100,15 @@ def __init__(self, CP, decimated=False): self.min_bucket_points = MIN_BUCKET_POINTS / 10 self.min_points_total = MIN_POINTS_TOTAL_QLOG self.fit_points = FIT_POINTS_TOTAL_QLOG + self.factor_sanity = FACTOR_SANITY_QLOG + self.friction_sanity = FRICTION_SANITY_QLOG + else: self.min_bucket_points = MIN_BUCKET_POINTS self.min_points_total = MIN_POINTS_TOTAL self.fit_points = FIT_POINTS_TOTAL + self.factor_sanity = FACTOR_SANITY + self.friction_sanity = FRICTION_SANITY self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 @@ -123,10 +128,10 @@ def __init__(self, CP, decimated=False): 'points': [] } self.decay = MIN_FILTER_DECAY - self.min_lataccel_factor = (1.0 - FACTOR_SANITY) * self.offline_latAccelFactor - self.max_lataccel_factor = (1.0 + FACTOR_SANITY) * self.offline_latAccelFactor - self.min_friction = (1.0 - FRICTION_SANITY) * self.offline_friction - self.max_friction = (1.0 + FRICTION_SANITY) * self.offline_friction + self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor + self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor + self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction + self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction # try to restore cached params params = Params() @@ -165,7 +170,6 @@ def get_restore_key(self, CP, version): def reset(self): self.resets += 1.0 - self.invalid_values_tracker = 0.0 self.decay = MIN_FILTER_DECAY self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total) @@ -190,12 +194,6 @@ def update_params(self, params): self.filtered_params[param].update(value) self.filtered_params[param].update_alpha(self.decay) - def is_sane(self, latAccelFactor, latAccelOffset, friction): - if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, friction]]): - return False - return (self.max_friction >= friction >= self.min_friction) and\ - (self.max_lataccel_factor >= latAccelFactor >= self.min_lataccel_factor) - def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) @@ -225,23 +223,20 @@ def get_msg(self, valid=True, with_points=False): liveTorqueParameters.useParams = self.use_params if self.filtered_points.is_valid(): - latAccelFactor, latAccelOffset, friction_coeff = self.estimate_params() + latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) - liveTorqueParameters.frictionCoefficientRaw = float(friction_coeff) + liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) - if self.is_sane(latAccelFactor, latAccelOffset, friction_coeff): - liveTorqueParameters.liveValid = True - self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff}) - self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5) - else: - cloudlog.exception("Live torque parameters are outside acceptable bounds.") + if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]]): + cloudlog.exception("Live torque parameters are invalid.") liveTorqueParameters.liveValid = False - self.invalid_values_tracker += 1.0 - # Reset when ~10 invalid over 5 secs - if self.invalid_values_tracker > MAX_INVALID_THRESHOLD: - # Do not reset the filter as it may cause a drastic jump, just reset points - self.reset() + self.reset() + else: + liveTorqueParameters.liveValid = True + latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor) + frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction) + self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff}) else: liveTorqueParameters.liveValid = False diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index b45dffae339d05..3ff768ba067e10 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -65,6 +65,21 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } +inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) { + // convert time to year (conversion from A3.1.3) + int J = 0; + if (1 <= Nt && Nt <= 366) { + J = 1; + } else if (367 <= Nt && Nt <= 731) { + J = 2; + } else if (732 <= Nt && Nt <= 1096) { + J = 3; + } else if (1097 <= Nt && Nt <= 1461) { + J = 4; + } + uint16_t year = 1996 + 4*(N4 -1) + (J - 1); + return year; +} bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); @@ -103,9 +118,9 @@ std::pair> UbloxMsgParser::gen_msg() { switch (ubx_message.msg_type()) { case 0x0107: return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - case 0x0213: + case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - case 0x0215: + case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; case 0x0a09: return {"ubloxGnss", gen_mon_hw(static_cast(body))}; @@ -147,115 +162,248 @@ kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { return capnp::messageToFlatArray(msg_builder); } - -kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { +kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes + // We will first need to separate the data from the padding and parity auto body = *msg->body(); + assert(body.size() == 10); + + std::string subframe_data; + subframe_data.reserve(30); + for (uint32_t word : body) { + word = word >> 6; // TODO: Verify parity + subframe_data.push_back(word >> 16); + subframe_data.push_back(word >> 8); + subframe_data.push_back(word >> 0); + } - if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) { - // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes - // We will first need to separate the data from the padding and parity - assert(body.size() == 10); + // Collect subframes in map and parse when we have all the parts + { + kaitai::kstream stream(subframe_data); + gps_t subframe(&stream); - std::string subframe_data; - subframe_data.reserve(30); - for (uint32_t word : body) { - word = word >> 6; // TODO: Verify parity - subframe_data.push_back(word >> 16); - subframe_data.push_back(word >> 8); - subframe_data.push_back(word >> 0); + int subframe_id = subframe.how()->subframe_id(); + if (subframe_id > 3) { + // dont parse almanac subframes + return kj::Array(); } + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + } - // Collect subframes in map and parse when we have all the parts + // publish if subframes 1-3 have been collected + if (gps_subframes[msg->sv_id()].size() == 3) { + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); + eph.setSvId(msg->sv_id()); + + int iode_s2 = 0; + int iode_s3 = 0; + int iodc_lsb = 0; + + // Subframe 1 { - kaitai::kstream stream(subframe_data); + kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); gps_t subframe(&stream); - int subframe_id = subframe.how()->subframe_id(); + gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); + + eph.setGpsWeek(subframe_1->week_no()); + eph.setTgd(subframe_1->t_gd() * pow(2, -31)); + eph.setToc(subframe_1->t_oc() * pow(2, 4)); + eph.setAf2(subframe_1->af_2() * pow(2, -55)); + eph.setAf1(subframe_1->af_1() * pow(2, -43)); + eph.setAf0(subframe_1->af_0() * pow(2, -31)); + eph.setSvHealth(subframe_1->sv_health()); + iodc_lsb = subframe_1->iodc_lsb(); + } - if (subframe_id == 1) gps_subframes[msg->sv_id()].clear(); - gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + // Subframe 2 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); + gps_t subframe(&stream); + gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); + + eph.setCrs(subframe_2->c_rs() * pow(2, -5)); + eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); + eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); + eph.setCuc(subframe_2->c_uc() * pow(2, -29)); + eph.setEcc(subframe_2->e() * pow(2, -33)); + eph.setCus(subframe_2->c_us() * pow(2, -29)); + eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); + eph.setToe(subframe_2->t_oe() * pow(2, 4)); + iode_s2 = subframe_2->iode(); } - if (gps_subframes[msg->sv_id()].size() == 5) { - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(msg->sv_id()); - - // Subframe 1 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); - gps_t subframe(&stream); - gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - - eph.setGpsWeek(subframe_1->week_no()); - eph.setTgd(subframe_1->t_gd() * pow(2, -31)); - eph.setToc(subframe_1->t_oc() * pow(2, 4)); - eph.setAf2(subframe_1->af_2() * pow(2, -55)); - eph.setAf1(subframe_1->af_1() * pow(2, -43)); - eph.setAf0(subframe_1->af_0() * pow(2, -31)); - } - - // Subframe 2 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); - gps_t subframe(&stream); - gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); - - eph.setCrs(subframe_2->c_rs() * pow(2, -5)); - eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); - eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); - eph.setCuc(subframe_2->c_uc() * pow(2, -29)); - eph.setEcc(subframe_2->e() * pow(2, -33)); - eph.setCus(subframe_2->c_us() * pow(2, -29)); - eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); - eph.setToe(subframe_2->t_oe() * pow(2, 4)); - } - - // Subframe 3 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); - gps_t subframe(&stream); - gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); - - eph.setCic(subframe_3->c_ic() * pow(2, -29)); - eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); - eph.setCis(subframe_3->c_is() * pow(2, -29)); - eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); - eph.setCrc(subframe_3->c_rc() * pow(2, -5)); - eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); - eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); - eph.setIode(subframe_3->iode()); - eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); - } - - // Subframe 4 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); - gps_t subframe(&stream); - gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); - - // This is page 18, why is the page id 56? - if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { - auto iono = static_cast(subframe_4->body()); - double a0 = iono->a0() * pow(2, -30); - double a1 = iono->a1() * pow(2, -27); - double a2 = iono->a2() * pow(2, -24); - double a3 = iono->a3() * pow(2, -24); - eph.setIonoAlpha({a0, a1, a2, a3}); - - double b0 = iono->b0() * pow(2, 11); - double b1 = iono->b1() * pow(2, 14); - double b2 = iono->b2() * pow(2, 16); - double b3 = iono->b3() * pow(2, 16); - eph.setIonoBeta({b0, b1, b2, b3}); - } - } - - return capnp::messageToFlatArray(msg_builder); + // Subframe 3 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); + gps_t subframe(&stream); + gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); + + eph.setCic(subframe_3->c_ic() * pow(2, -29)); + eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); + eph.setCis(subframe_3->c_is() * pow(2, -29)); + eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); + eph.setCrc(subframe_3->c_rc() * pow(2, -5)); + eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); + eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); + eph.setIode(subframe_3->iode()); + eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); + iode_s3 = subframe_3->iode(); } + + gps_subframes[msg->sv_id()].clear(); + if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { + // data set cutover, reject ephemeris + return kj::Array(); + } + return capnp::messageToFlatArray(msg_builder); } return kj::Array(); } +kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + if (msg->sv_id() == 255) { + // data can be decoded before identifying the SV number, in this case 255 + // is returned, which means "unknown" (ublox p32) + return kj::Array(); + } + + auto body = *msg->body(); + assert(body.size() == 4); + { + std::string string_data; + string_data.reserve(16); + for (uint32_t word : body) { + for (int i = 3; i >= 0; i--) + string_data.push_back(word >> 8*i); + } + + kaitai::kstream stream(string_data); + glonass_t gl_string(&stream); + + int string_number = gl_string.string_number(); + if (string_number > 5 || gl_string.idle_chip()) { + // dont parse non immediate data, idle_chip == 0 + return kj::Array(); + } + + // immediate data is the same within one superframe + if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) { + glonass_strings[msg->sv_id()].clear(); + glonass_superframes[msg->sv_id()] = gl_string.superframe_number(); + } + glonass_strings[msg->sv_id()][string_number] = string_data; + } + + // publish if strings 1-5 have been collected + if (glonass_strings[msg->sv_id()].size() != 5) { + return kj::Array(); + } + + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris(); + eph.setSvId(msg->sv_id()); + uint16_t current_day = 0; + + // string number 1 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][1]); + glonass_t gl_stream(&stream); + glonass_t::string_1_t* data = static_cast(gl_stream.data()); + + eph.setP1(data->p1()); + eph.setTk(data->t_k()); + eph.setXVel(data->x_vel() * pow(2, -20)); + eph.setXAccel(data->x_accel() * pow(2, -30)); + eph.setX(data->x() * pow(2, -11)); + } + + // string number 2 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][2]); + glonass_t gl_stream(&stream); + glonass_t::string_2_t* data = static_cast(gl_stream.data()); + + eph.setSvHealth(data->b_n()>>2); // MSB indicates health + eph.setP2(data->p2()); + eph.setTb(data->t_b()); + eph.setYVel(data->y_vel() * pow(2, -20)); + eph.setYAccel(data->y_accel() * pow(2, -30)); + eph.setY(data->y() * pow(2, -11)); + } + + // string number 3 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][3]); + glonass_t gl_stream(&stream); + glonass_t::string_3_t* data = static_cast(gl_stream.data()); + + eph.setP3(data->p3()); + eph.setGammaN(data->gamma_n() * pow(2, -40)); + eph.setSvHealth(eph.getSvHealth() | data->l_n()); + eph.setZVel(data->z_vel() * pow(2, -20)); + eph.setZAccel(data->z_accel() * pow(2, -30)); + eph.setZ(data->z() * pow(2, -11)); + } + + // string number 4 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][4]); + glonass_t gl_stream(&stream); + glonass_t::string_4_t* data = static_cast(gl_stream.data()); + + current_day = data->n_t(); + eph.setTauN(data->tau_n() * pow(2, -30)); + eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30)); + eph.setAge(data->e_n()); + eph.setP4(data->p4()); + eph.setSvURA(glonass_URA_lookup.at(data->f_t())); + if (msg->sv_id() != data->n()) { + LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n()) + } + eph.setSvType(data->m()); + } + + // string number 5 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][5]); + glonass_t gl_stream(&stream); + glonass_t::string_5_t* data = static_cast(gl_stream.data()); + + // string5 parsing is only needed to get the year, this can be removed and + // the year can be fetched later in laika (note rollovers and leap year) + uint8_t n_4 = data->n_4(); + uint16_t year = get_glonass_year(n_4, current_day); + + uint16_t last_leap_year = 1996 + 4*(n_4-1); + uint16_t days_till_this_year = (year - last_leap_year)*365; + if (days_till_this_year != 0) { + days_till_this_year++; + } + + eph.setYear(year); + eph.setDayInYear(current_day - days_till_this_year); + eph.setHour((eph.getTk()>>7) & 0x1F); + eph.setMinute((eph.getTk()>>1) & 0x3F); + eph.setSecond((eph.getTk() & 0x1) * 30); + } + + glonass_strings[msg->sv_id()].clear(); + return capnp::messageToFlatArray(msg_builder); +} + + +kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + switch (msg->gnss_id()) { + case ubx_t::gnss_type_t::GNSS_TYPE_GPS: + return parse_gps_ephemeris(msg); + case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS: + return parse_glonass_ephemeris(msg); + default: + return kj::Array(); + } +} + kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { MessageBuilder msg_builder; auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 542e72816ba7b4..6988f20b7402e5 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -10,6 +10,7 @@ #include "cereal/messaging/messaging.h" #include "common/util.h" #include "selfdrive/locationd/generated/gps.h" +#include "selfdrive/locationd/generated/glonass.h" #include "selfdrive/locationd/generated/ubx.h" using namespace std::string_literals; @@ -101,11 +102,22 @@ class UbloxMsgParser { inline bool valid_cheksum(); inline bool valid(); inline bool valid_so_far(); + inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt); + + kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); + kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); std::unordered_map> gps_subframes; size_t bytes_in_parse_buf = 0; uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; -}; + // user range accuracy in meters + const std::unordered_map glonass_URA_lookup = + {{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7}, + { 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32}, + {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}}; + std::unordered_map> glonass_strings; + std::unordered_map glonass_superframes; +}; diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 92706c53ecf209..3b961bce6e1324 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -13,6 +13,8 @@ if arch == "Darwin": # fix OpenCL del libs[libs.index('OpenCL')] env['FRAMEWORKS'] = ['OpenCL'] + # exclude v4l + del src[src.index('encoder/v4l_encoder.cc')] logger_lib = env.Library('logger', src) libs.insert(0, logger_lib) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 9beb3c3bf16b00..e09cdfaa9e8427 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -24,15 +24,25 @@ void logger_rotate(LoggerdState *s) { } void rotate_if_needed(LoggerdState *s) { - if (s->ready_to_rotate == s->max_waiting) { - logger_rotate(s); - } + // all encoders ready, trigger rotation + bool all_ready = s->ready_to_rotate == s->max_waiting; + // fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions + bool timed_out = false; double tms = millis_since_boot(); - if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 && - (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE && - !LOGGERD_TEST) { - LOGW("no camera packet seen. auto rotating"); + double seg_length_secs = (tms - s->last_rotate_tms) / 1000.; + if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) { + // TODO: might be nice to put these reasons in the sentinel + if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) { + timed_out = true; + LOGE("no camera packets seen. auto rotating"); + } else if (seg_length_secs > SEGMENT_LENGTH*1.2) { + timed_out = true; + LOGE("segment too long. auto rotating"); + } + } + + if (all_ready || timed_out) { logger_rotate(s); } } diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index c8a7d415398b04..5b69e3dca7401e 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -15,7 +15,7 @@ MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 2395 +TOTAL_SCONS_NODES = 2460 MAX_BUILD_PROGRESS = 100 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 369c529626cc74..865966d6c5215d 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -20,7 +20,7 @@ from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from system.swaglog import cloudlog, add_file_handler from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ - terms_version, training_version, is_tested_branch + terms_version, training_version, is_tested_branch, is_release_branch @@ -76,6 +76,7 @@ def manager_init() -> None: params.put("GitBranch", get_short_branch(default="")) params.put("GitRemote", get_origin(default="")) params.put_bool("IsTestedBranch", is_tested_branch()) + params.put_bool("IsReleaseBranch", is_release_branch()) # set dongle id reg_res = register(show_spinner=True) diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index dabfbe4ee0b1f6..ce18073d9bf9ed 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -290,10 +290,11 @@ def stop(self, retry=True, block=True) -> None: def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, - not_run: Optional[List[str]]=None) -> None: + not_run: Optional[List[str]]=None) -> List[ManagerProcess]: if not_run is None: not_run = [] + running = [] for p in procs: # Conditions that make a process run run = any(( @@ -311,7 +312,10 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None if run: p.start() + running.append(p) else: p.stop(block=False) p.check_watchdog(started) + + return running diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index c03e9954975505..8fc4d94e552755 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -17,6 +17,11 @@ def logging(started, params, CP: car.CarParams) -> bool: run = (not CP.notCar) or not params.get_bool("DisableLogging") return started and run +def ublox(started, params, CP: car.CarParams) -> bool: + use_ublox = os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') + params.put_bool("UbloxAvailable", use_ublox) + return started and use_ublox + procs = [ # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview), @@ -35,7 +40,6 @@ def logging(started, params, CP: car.CarParams) -> bool: NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False), NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), - NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), @@ -50,7 +54,8 @@ def logging(started, params, CP: car.CarParams) -> bool: PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), - PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI), + NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox), + PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 6d4df0423a2d83..889ab3d2796eab 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -4,17 +4,17 @@ import time import unittest +from cereal import car from common.params import Params import selfdrive.manager.manager as manager -from selfdrive.manager.process import DaemonProcess +from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes from system.hardware import HARDWARE os.environ['FAKEUPLOAD'] = "1" MAX_STARTUP_TIME = 3 -ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['pandad', ])] - +BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] class TestManager(unittest.TestCase): def setUp(self): @@ -47,24 +47,25 @@ def test_clean_exit(self): HARDWARE.set_power_save(False) manager.manager_init() manager.manager_prepare() - for p in ALL_PROCESSES: - managed_processes[p].start() + + CP = car.CarParams.new_message() + procs = ensure_running(managed_processes.values(), True, Params(), CP, not_run=BLACKLIST_PROCS) time.sleep(10) - for p in reversed(ALL_PROCESSES): - with self.subTest(proc=p): - state = managed_processes[p].get_process_state_msg() - self.assertTrue(state.running, f"{p} not running") - exit_code = managed_processes[p].stop(retry=False) + for p in procs: + with self.subTest(proc=p.name): + state = p.get_process_state_msg() + self.assertTrue(state.running, f"{p.name} not running") + exit_code = p.stop(retry=False) - self.assertTrue(exit_code is not None, f"{p} failed to exit") + self.assertTrue(exit_code is not None, f"{p.name} failed to exit") # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] - if managed_processes[p].sigkill: + if p.sigkill: exit_codes = [-signal.SIGKILL] - self.assertIn(exit_code, exit_codes, f"{p} died with {exit_code}") + self.assertIn(exit_code, exit_codes, f"{p.name} died with {exit_code}") if __name__ == "__main__": diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 82338e456b3156..7bbc1b34776cde 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -70,30 +70,14 @@ lenv.Program('_dmonitoringmodeld', [ if use_thneed and arch == "larch64" or GetOption('pc_thneed'): fn = File("models/supercombo").abspath - if GetOption('pc_thneed'): - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && GPU=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" - else: - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 MATMUL=1 PYOPENCL_NO_CACHE=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" - - # is there a better way then listing all of tinygrad? - lenv.Command(fn + ".thneed", [fn + ".onnx", - "#tinygrad_repo/openpilot/compile.py", - "#tinygrad_repo/accel/opencl/conv.cl", - "#tinygrad_repo/accel/opencl/matmul.cl", - "#tinygrad_repo/accel/opencl/ops_opencl.py", - "#tinygrad_repo/accel/opencl/preprocessing.py", - "#tinygrad_repo/extra/onnx.py", - "#tinygrad_repo/extra/thneed.py", - "#tinygrad_repo/extra/utils.py", - "#tinygrad_repo/tinygrad/llops/ops_gpu.py", - "#tinygrad_repo/tinygrad/llops/ops_opencl.py", - "#tinygrad_repo/tinygrad/helpers.py", - "#tinygrad_repo/tinygrad/mlops.py", - "#tinygrad_repo/tinygrad/ops.py", - "#tinygrad_repo/tinygrad/shapetracker.py", - "#tinygrad_repo/tinygrad/tensor.py", - "#tinygrad_repo/tinygrad/nn/__init__.py" - ], cmd) + tinygrad_opts = ["NATIVE_EXPLOG=1", "VALIDHACKS=1", "OPTWG=1", "IMAGE=2", "GPU=1", "CLCACHE=0"] + if not GetOption('pc_thneed'): + # use FLOAT16 on device for speed + don't cache the CL kernels for space + tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"] + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + + tinygrad_files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.startswith("tinygrad_repo/")], []) + lenv.Command(fn + ".thneed", [fn + ".onnx"] + tinygrad_files, cmd) llenv = lenv.Clone() if GetOption('pc_thneed'): @@ -116,4 +100,4 @@ llenv.Program('_modeld', [ lenv.Program('_navmodeld', [ "navmodeld.cc", "models/nav.cc", - ]+common_model, LIBS=libs + transformations) \ No newline at end of file + ]+common_model, LIBS=libs + transformations) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 8805b3dce8d5cc..2daf01ea142c6e 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:db746e3753de84367595fedd089c2bd41b06bd401ea28e085663533d0e63d74b -size 45962473 +oid sha256:3d8f2f9d53908e7a5496412956548dab1aa08ed0b5fb9d1c685f275369f07a57 +size 45962515 diff --git a/selfdrive/sensord/libdiag.h b/selfdrive/sensord/libdiag.h deleted file mode 100644 index 03a59464edc5c1..00000000000000 --- a/selfdrive/sensord/libdiag.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define DIAG_MAX_RX_PKT_SIZ 4096 - -bool Diag_LSM_Init(uint8_t* pIEnv); -bool Diag_LSM_DeInit(void); - -// DCI - -#define DIAG_CON_APSS 0x001 -#define DIAG_CON_MPSS 0x002 -#define DIAG_CON_LPASS 0x004 -#define DIAG_CON_WCNSS 0x008 - -enum { - DIAG_DCI_NO_ERROR = 1001, -} diag_dci_error_type; - -int diag_register_dci_client(int*, uint16_t*, int, void*); -int diag_log_stream_config(int client_id, int set_mask, uint16_t log_codes_array[], int num_codes); -int diag_register_dci_stream(void (*func_ptr_logs)(unsigned char *ptr, int len), void (*func_ptr_events)(unsigned char *ptr, int len)); -int diag_release_dci_client(int*); - -int diag_send_dci_async_req(int client_id, unsigned char buf[], int bytes, unsigned char *rsp_ptr, int rsp_len, - void (*func_ptr)(unsigned char *ptr, int len, void *data_ptr), void *data_ptr); - - -#ifdef __cplusplus -} -#endif diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index f56af1c7051057..9d0a62bd3b780c 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -123,7 +123,7 @@ def reset_device(self) -> bool: init_baudrate(self) # clear configuration - self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x00\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\x5b") + self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7") # clear flash memory (almanac backup) self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0") @@ -303,8 +303,7 @@ def main(): pigeon, pm = create_pigeon() init_baudrate(pigeon) - r = initialize_pigeon(pigeon) - Params().put_bool("UbloxAvailable", r) + initialize_pigeon(pigeon) # start receiving data run_receiving(pigeon, pm) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 1c650516651e47..3fa5e927a278c1 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -11,7 +11,9 @@ from cereal import log import cereal.messaging as messaging +from common.gpio import gpio_init, gpio_set from laika.gps_time import GPSTime +from system.hardware.tici.pins import GPIO from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, @@ -182,6 +184,7 @@ def main() -> NoReturn: def cleanup(sig, frame): cloudlog.warning(f"caught sig {sig}, disabling quectel gps") + gpio_set(GPIO.UBLOX_PWR_EN, False) teardown_quectel(diag) cloudlog.warning("quectel cleanup done") sys.exit(0) @@ -190,6 +193,8 @@ def cleanup(sig, frame): setup_quectel(diag) cloudlog.warning("quectel setup done") + gpio_init(GPIO.UBLOX_PWR_EN, True) + gpio_set(GPIO.UBLOX_PWR_EN, True) pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 65935f89792d3e..00ddfe627eb90f 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -19,6 +19,7 @@ def __init__(self, title, duration, **kwargs): self.ensure_start = kwargs.get("ensure_start", False) self.enabled = kwargs.get("enabled", True) self.e2e = kwargs.get("e2e", False) + self.force_decel = kwargs.get("force_decel", False) self.duration = duration self.title = title @@ -32,6 +33,7 @@ def evaluate(self): only_lead2=self.only_lead2, only_radar=self.only_radar, e2e=self.e2e, + force_decel=self.force_decel, ) valid = True @@ -60,6 +62,10 @@ def evaluate(self): if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: print('LongitudinalPlanner not starting!') valid = False + if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04: + print('Not stopping with force decel') + valid = False + print("maneuver end", valid) return valid, np.array(logs) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 8e150d800c554d..bd0556aa076eb9 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -15,7 +15,7 @@ class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - enabled=True, only_lead2=False, only_radar=False, e2e=False): + enabled=True, only_lead2=False, only_radar=False, e2e=False, force_decel=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -39,6 +39,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, self.only_lead2 = only_lead2 self.only_radar = only_radar self.e2e = e2e + self.force_decel = force_decel self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate @@ -48,7 +49,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, from selfdrive.car.honda.values import CAR from selfdrive.car.honda.interface import CarInterface - self.planner = LongitudinalPlanner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed) + self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed) @property def current_time(self): @@ -111,6 +112,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off control.controlsState.vCruise = float(v_cruise * 3.6) control.controlsState.experimentalMode = self.e2e + control.controlsState.forceDecel = self.force_decel car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 686b35e4569ac1..bc477ca9fee569 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 +import itertools import os -from parameterized import parameterized +from parameterized import parameterized_class import unittest from common.params import Params @@ -9,8 +10,8 @@ # TODO: make new FCW tests -def create_maneuvers(e2e): - return [ +def create_maneuvers(kwargs): + maneuvers = [ Maneuver( 'approach stopped car at 25m/s, initial distance: 120m', duration=20., @@ -19,7 +20,7 @@ def create_maneuvers(e2e): initial_distance_lead=120., speed_lead_values=[30., 0.], breakpoints=[0., 1.], - e2e=e2e, + **kwargs, ), Maneuver( 'approach stopped car at 20m/s, initial distance 90m', @@ -29,7 +30,7 @@ def create_maneuvers(e2e): initial_distance_lead=90., speed_lead_values=[20., 0.], breakpoints=[0., 1.], - e2e=e2e, + **kwargs, ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', @@ -39,7 +40,7 @@ def create_maneuvers(e2e): initial_distance_lead=35., speed_lead_values=[20., 20., 0.], breakpoints=[0., 15., 35.0], - e2e=e2e, + **kwargs, ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', @@ -49,7 +50,7 @@ def create_maneuvers(e2e): initial_distance_lead=35., speed_lead_values=[20., 20., 0.], breakpoints=[0., 15., 25.0], - e2e=e2e, + **kwargs, ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', @@ -59,7 +60,7 @@ def create_maneuvers(e2e): initial_distance_lead=35., speed_lead_values=[20., 20., 0.], breakpoints=[0., 15., 21.66], - e2e=e2e, + **kwargs, ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', @@ -71,7 +72,7 @@ def create_maneuvers(e2e): prob_lead_values=[0., 1., 1.], cruise_values=[20., 20., 20.], breakpoints=[2., 2.01, 8.8], - e2e=e2e, + **kwargs, ), Maneuver( "approach stopped car at 20m/s, with prob_lead_values", @@ -83,7 +84,7 @@ def create_maneuvers(e2e): prob_lead_values=[0.0, 0., 1.], cruise_values=[20., 20., 20.], breakpoints=[0.0, 2., 2.01], - e2e=e2e, + **kwargs, ), Maneuver( "approach slower cut-in car at 20m/s", @@ -94,7 +95,7 @@ def create_maneuvers(e2e): speed_lead_values=[15., 15.], breakpoints=[1., 11.], only_lead2=True, - e2e=e2e, + **kwargs, ), Maneuver( "stay stopped behind radar override lead", @@ -106,7 +107,7 @@ def create_maneuvers(e2e): prob_lead_values=[0., 0.], breakpoints=[1., 11.], only_radar=True, - e2e=e2e, + **kwargs, ), Maneuver( "NaN recovery", @@ -117,10 +118,20 @@ def create_maneuvers(e2e): speed_lead_values=[0., 0., 0.0], breakpoints=[1., 1.01, 11.], cruise_values=[float("nan"), 15., 15.], - e2e=e2e, + **kwargs, ), - # controls relies on planner commanding to move for stock-ACC resume spamming Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + **kwargs, + ), + ] + if not kwargs['force_decel']: + # controls relies on planner commanding to move for stock-ACC resume spamming + maneuvers.append(Maneuver( "resume from a stop", duration=20., initial_speed=0., @@ -129,20 +140,16 @@ def create_maneuvers(e2e): speed_lead_values=[0., 0., 2.], breakpoints=[1., 10., 15.], ensure_start=True, - e2e=e2e, - ), - Maneuver( - 'cruising at 25 m/s while disabled', - duration=20., - initial_speed=25., - lead_relevancy=False, - enabled=False, - e2e=e2e, - ), - ] + **kwargs, + )) + return maneuvers +@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2)) class LongitudinalControl(unittest.TestCase): + e2e: bool + force_decel: bool + @classmethod def setUpClass(cls): os.environ['SIMULATION'] = "1" @@ -154,11 +161,12 @@ def setUpClass(cls): params.put_bool("Passive", bool(os.getenv("PASSIVE"))) params.put_bool("OpenpilotEnabledToggle", True) - @parameterized.expand([(man,) for e2e in [True, False] for man in create_maneuvers(e2e)]) - def test_maneuver(self, maneuver): - print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') - valid, _ = maneuver.evaluate() - self.assertTrue(valid, msg=maneuver.title) + def test_maneuver(self): + for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}): + with self.subTest(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel): + print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') + valid, _ = maneuver.evaluate() + self.assertTrue(valid) if __name__ == "__main__": diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 324801feada9bb..68cbb28aaf4671 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -223,7 +223,7 @@ def model_replay(lr, frs): try: expected_msgs = 2*MAX_FRAMES if not NO_NAV: - expected_msgs += NAV_FRAMES*2 + expected_msgs += NAV_FRAMES*3 cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs] ignore = [ diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 22e021dcaf16a8..fa4fa9540d7829 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -db587bfef2317c5a3471632ac47381457e1be853 \ No newline at end of file +2a255ecea3665a3d295210195fd8ef0791ab49a0 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index c5465fc0255488..b531cb3430e989 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -180,7 +180,7 @@ def fingerprint(msgs, fsm, can_sock, fingerprint): def get_car_params(msgs, fsm, can_sock, fingerprint): if fingerprint: CarInterface, _, _ = interfaces[fingerprint] - CP = CarInterface.get_params(fingerprint) + CP = CarInterface.get_non_essential_params(fingerprint) else: can = FakeSocket(wait=False) sendcan = FakeSocket(wait=False) @@ -188,7 +188,7 @@ def get_car_params(msgs, fsm, can_sock, fingerprint): canmsgs = [msg for msg in msgs if msg.which() == 'can'] for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) - _, CP = get_car(can, sendcan) + _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) Params().put("CarParams", CP.to_bytes()) @@ -204,7 +204,7 @@ def controlsd_rcv_callback(msg, CP, cfg, fsm): def radar_rcv_callback(msg, CP, cfg, fsm): if msg.which() != "can": return [], False - elif CP.radarOffCan: + elif CP.radarUnavailable: return ["radarState", "liveTracks"], True radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474], @@ -448,6 +448,9 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None): if CP.openpilotLongitudinalControl: params.put_bool("ExperimentalLongitudinalEnabled", True) + # controlsd process configuration assume all routes are out of dashcam + params.put_bool("DashcamOverride", True) + def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] @@ -554,7 +557,7 @@ def cpp_replay_process(cfg, lr, fingerprint=None): managed_processes[cfg.proc_name].start() try: - with Timeout(TIMEOUT): + with Timeout(TIMEOUT, error_msg=f"timed out testing process {repr(cfg.proc_name)}"): while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()): time.sleep(0) @@ -568,7 +571,7 @@ def cpp_replay_process(cfg, lr, fingerprint=None): resp_sockets = cfg.pub_sub[msg.which()] if cfg.should_recv_callback is None else cfg.should_recv_callback(msg) for s in resp_sockets: - response = messaging.recv_one(sockets[s]) + response = messaging.recv_one_retry(sockets[s]) if response is None: print(f"Warning, no response received {i}") diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 603d2b2d96e24a..68675e700763e6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -557ffbf5a1c9cafba1ff8d6f3e2642df98b2d6e6 \ No newline at end of file +2beed04e654cdf84fac5842869f38c8cd55e9867 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index c58909bf7fa6fc..569090f606c2cf 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -29,6 +29,7 @@ ("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 + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1 ("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 @@ -52,6 +53,7 @@ ("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"), + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), ("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"), diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index 9e06b986621864..b7d586a83bac53 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -56,7 +56,7 @@ cd $SOURCE_DIR rm -f .git/index.lock git reset --hard -git fetch --verbose origin $GIT_COMMIT +git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; git reset --hard $GIT_COMMIT git checkout $GIT_COMMIT @@ -69,6 +69,7 @@ git lfs pull (ulimit -n 65535 && git lfs prune) echo "git checkout done, t=$SECONDS" +du -hs $SOURCE_DIR $SOURCE_DIR/.git rsync -a --delete $SOURCE_DIR $TEST_DIR diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 669c214746714a..0f28f5ccc3c904 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,4 +1,5 @@ import os +import json Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') @@ -41,6 +42,7 @@ assets_src = "#selfdrive/assets/assets.qrc" qt_env.Command(assets, assets_src, f"rcc $SOURCES -o $TARGET") qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, "#selfdrive/assets/assets.o"])) asset_obj = qt_env.Object("assets", assets) +Export('asset_obj') # build soundd qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) @@ -66,11 +68,18 @@ if GetOption('test'): # build translation files -translation_sources = Glob("#selfdrive/ui/translations/*.ts", strings=True) +with open(File("translations/languages.json").abspath) as f: + languages = json.loads(f.read()) +translation_sources = [f"#selfdrive/ui/translations/{l}.ts" for l in languages.values()] translation_targets = [src.replace(".ts", ".qm") for src in translation_sources] -lrelease = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease' -qt_env.Command(translation_targets, translation_sources, f"{lrelease} $SOURCES") - +lrelease_bin = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease' + +lupdate = qt_env.Command(translation_sources, qt_src, "selfdrive/ui/update_translations.py") +lrelease = qt_env.Command(translation_targets, translation_sources, f"{lrelease_bin} $SOURCES") +qt_env.Depends(lrelease, lupdate) +qt_env.NoClean(translation_sources) +qt_env.Precious(translation_sources) +qt_env.NoCache(lupdate) # setup and factory resetter if GetOption('extras'): diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index b5519daccfc4b8..dd30b5fefff01e 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -47,7 +47,7 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), map_eta->setVisible(false); auto last_gps_position = coordinate_from_param("LastGPSPosition"); - if (last_gps_position) { + if (last_gps_position.has_value()) { last_position = *last_gps_position; } @@ -82,6 +82,7 @@ void MapWindow::initLayers() { m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee")); m_map->setPaintProperty("navLayer", "line-width", 7.5); m_map->setLayoutProperty("navLayer", "line-cap", "round"); + m_map->addAnnotationIcon("default_marker", QImage("../assets/navigation/default_marker.svg")); } if (!m_map->layerExists("carPosLayer")) { qDebug() << "Initializing carPosLayer"; @@ -124,7 +125,8 @@ void MapWindow::updateState(const UIState &s) { } } - if (sm.updated("gnssMeasurements")) { + // TODO should check a valid/status flag + if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){ auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); auto laikad_pos = laikad_location.getPositionECEF(); auto laikad_pos_ecef = laikad_pos.getValue(); @@ -220,7 +222,6 @@ void MapWindow::updateState(const UIState &s) { emit instructionsChanged(i); } } else { - m_map->setPitch(MIN_PITCH); clearRoute(); } } @@ -237,6 +238,7 @@ void MapWindow::updateState(const UIState &s) { m_map->setLayoutProperty("navLayer", "visibility", "visible"); route_rcv_frame = sm.rcv_frame("navRoute"); + updateDestinationMarker(); } } @@ -274,6 +276,7 @@ void MapWindow::clearRoute() { if (!m_map.isNull()) { m_map->setLayoutProperty("navLayer", "visibility", "none"); m_map->setPitch(MIN_PITCH); + updateDestinationMarker(); } map_instructions->hideIfNoError(); @@ -361,6 +364,19 @@ void MapWindow::offroadTransition(bool offroad) { last_bearing = {}; } +void MapWindow::updateDestinationMarker() { + if (marker_id != -1) { + m_map->removeAnnotation(marker_id); + marker_id = -1; + } + + auto nav_dest = coordinate_from_param("NavDestination"); + if (nav_dest.has_value()) { + auto ano = QMapbox::SymbolAnnotation {*nav_dest, "default_marker"}; + marker_id = m_map->addAnnotation(QVariant::fromValue(ano)); + } +} + MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) { is_rhd = Params().getBool("IsRhdDetected"); QHBoxLayout *main_layout = new QHBoxLayout(this); diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index c3d5e92530f89a..0d8b93a5f4d1cc 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -79,6 +79,7 @@ class MapWindow : public QOpenGLWidget { QMapboxGLSettings m_settings; QScopedPointer m_map; + QMapbox::AnnotationID marker_id = -1; void initLayers(); @@ -111,6 +112,7 @@ class MapWindow : public QOpenGLWidget { MapETA* map_eta; void clearRoute(); + void updateDestinationMarker(); uint64_t route_rcv_frame = 0; private slots: diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index bde8628dc434ba..63b87149d41977 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -132,16 +132,17 @@ void TogglesPanel::updateToggles() { .arg(tr("New Driving Visualization")) .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.")); + const bool is_release = params.getBool("IsReleaseBranch"); auto cp_bytes = params.get("CarParamsPersistent"); if (!cp_bytes.empty()) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); - if (!CP.getExperimentalLongitudinalAvailable()) { + if (!CP.getExperimentalLongitudinalAvailable() || is_release) { params.remove("ExperimentalLongitudinalEnabled"); } - op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); const bool op_long = CP.getOpenpilotLongitudinalControl() && !CP.getExperimentalLongitudinalAvailable(); const bool exp_long_enabled = CP.getExperimentalLongitudinalAvailable() && params.getBool("ExperimentalLongitudinalEnabled"); @@ -154,9 +155,18 @@ void TogglesPanel::updateToggles() { e2e_toggle->setEnabled(false); params.remove("ExperimentalMode"); - const QString no_long = tr("Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control."); - const QString exp_long = tr("Enable experimental longitudinal control to allow experimental mode."); - e2e_toggle->setDescription("" + (CP.getExperimentalLongitudinalAvailable() ? exp_long : no_long) + "

" + e2e_description); + const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control."); + + QString long_desc = unavailable + " " + \ + tr("openpilot longitudinal control may come in a future update."); + if (CP.getExperimentalLongitudinalAvailable()) { + if (is_release) { + long_desc = unavailable + " " + tr("An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches."); + } else { + long_desc = tr("Enable experimental longitudinal control to allow Experimental mode."); + } + } + e2e_toggle->setDescription("" + long_desc + "

" + e2e_description); } e2e_toggle->refresh(); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 1b4b880fbf15d8..7a4b4240c3a9ef 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -175,11 +175,62 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } +ExperimentalButton::ExperimentalButton(QWidget *parent) : QPushButton(parent) { + setVisible(false); + setFixedSize(btn_size, btn_size); + setCheckable(true); + + params = Params(); + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); + + QObject::connect(this, &QPushButton::toggled, [=](bool checked) { + params.putBool("ExperimentalMode", checked); + }); +} + +void ExperimentalButton::updateState(const UIState &s) { + const SubMaster &sm = *(s.sm); + + // button is "visible" if engageable or enabled + const auto cs = sm["controlsState"].getControlsState(); + setVisible(cs.getEngageable() || cs.getEnabled()); + + // button is "checked" if experimental mode is enabled + setChecked(sm["controlsState"].getControlsState().getExperimentalMode()); + + // disable button when experimental mode is not available, or has not been confirmed for the first time + const auto cp = sm["carParams"].getCarParams(); + const bool experimental_mode_available = cp.getExperimentalLongitudinalAvailable() ? params.getBool("ExperimentalLongitudinalEnabled") : cp.getOpenpilotLongitudinalControl(); + setEnabled(params.getBool("ExperimentalModeConfirmed") && experimental_mode_available); +} + +void ExperimentalButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + QPoint center(btn_size / 2, btn_size / 2); + QPixmap img = isChecked() ? experimental_img : engage_img; + + p.setOpacity(1.0); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 166)); + p.drawEllipse(center, btn_size / 2, btn_size / 2); + p.setOpacity(isDown() ? 0.8 : 1.0); + p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, img); +} + + AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { pm = std::make_unique>({"uiDebug"}); - engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); - experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size - 5, img_size - 5}); + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(bdr_s); + main_layout->setSpacing(0); + + experimental_btn = new ExperimentalButton(this); + main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); } @@ -227,9 +278,11 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); setProperty("status", s.status); - // update engageability and DM icons at 2Hz + // update engageability/experimental mode button + experimental_btn->updateState(s); + + // update DM icons at 2Hz if (sm.frame % (UI_FREQ / 2) == 0) { - setProperty("engageable", cs.getEngageable() || cs.getEnabled()); setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } @@ -382,16 +435,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { configFont(p, "Inter", 66, "Regular"); drawText(p, rect().center().x(), 290, speedUnit, 200); - // engage-ability icon - if (engageable) { - SubMaster &sm = *(uiState()->sm); - drawIcon(p, rect().right() - radius / 2 - bdr_s * 2, radius / 2 + int(bdr_s * 1.5), - sm["controlsState"].getControlsState().getExperimentalMode() ? experimental_img : engage_img, blackColor(166), 1.0); - } - // dm icon if (!hideDM) { - int dm_icon_x = rightHandDM ? rect().right() - radius / 2 - (bdr_s * 2) : radius / 2 + (bdr_s * 2); + int dm_icon_x = rightHandDM ? rect().right() - btn_size / 2 - (bdr_s * 2) : btn_size / 2 + (bdr_s * 2); drawIcon(p, dm_icon_x, rect().bottom() - footer_h / 2, dm_img, blackColor(70), dmActive ? 1.0 : 0.2); } @@ -414,7 +460,7 @@ void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QB p.setOpacity(1.0); // bg dictates opacity of ellipse p.setPen(Qt::NoPen); p.setBrush(bg); - p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); + 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); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3dbb05b674a33f..73c2e03795ba76 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -1,11 +1,16 @@ #pragma once +#include #include #include #include "common/util.h" -#include "selfdrive/ui/qt/widgets/cameraview.h" #include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + + +const int btn_size = 192; +const int img_size = (btn_size / 4) * 3; // ***** onroad widgets ***** @@ -24,6 +29,21 @@ class OnroadAlerts : public QWidget { Alert alert = {}; }; +class ExperimentalButton : public QPushButton { + Q_OBJECT + +public: + explicit ExperimentalButton(QWidget *parent = 0); + void updateState(const UIState &s); + +private: + void paintEvent(QPaintEvent *event) override; + + Params params; + QPixmap engage_img; + QPixmap experimental_img; +}; + // container window for the NVG UI class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -36,7 +56,6 @@ class AnnotatedCameraWidget : public CameraWidget { Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit); Q_PROPERTY(bool is_metric MEMBER is_metric); - Q_PROPERTY(bool engageable MEMBER engageable); Q_PROPERTY(bool dmActive MEMBER dmActive); Q_PROPERTY(bool hideDM MEMBER hideDM); Q_PROPERTY(bool rightHandDM MEMBER rightHandDM); @@ -50,18 +69,14 @@ class AnnotatedCameraWidget : public CameraWidget { void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - QPixmap engage_img; - QPixmap experimental_img; + ExperimentalButton *experimental_btn; QPixmap dm_img; - const int radius = 192; - const int img_size = (radius / 2) * 1.5; float speed; QString speedUnit; float setSpeed; float speedLimit; bool is_cruise_set = false; bool is_metric = false; - bool engageable = false; bool dmActive = false; bool hideDM = false; bool rightHandDM = false; diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 59903e33760fb0..4f523106492bab 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -2,11 +2,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include "common/params.h" #include "common/swaglog.h" @@ -218,3 +221,37 @@ QColor interpColor(float xv, std::vector xp, std::vector fp) { ); } } + +static QHash load_bootstrap_icons() { + QHash icons; + + QFile f(":/bootstrap-icons.svg"); + if (f.open(QIODevice::ReadOnly | QIODevice::Text)) { + QDomDocument xml; + xml.setContent(&f); + QDomNode n = xml.documentElement().firstChild(); + while (!n.isNull()) { + QDomElement e = n.toElement(); + if (!e.isNull() && e.hasAttribute("id")) { + QString svg_str; + QTextStream stream(&svg_str); + n.save(stream, 0); + svg_str.replace("", ""); + icons[e.attribute("id")] = svg_str.toUtf8(); + } + n = n.nextSibling(); + } + } + return icons; +} + +QPixmap bootstrapPixmap(const QString &id) { + static QHash icons = load_bootstrap_icons(); + + QPixmap pixmap; + if (auto it = icons.find(id); it != icons.end()) { + pixmap.loadFromData(it.value(), "svg"); + } + return pixmap; +} diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 61a27a86698f1c..3188f3f9b91799 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -22,6 +22,7 @@ void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, co void initApp(int argc, char *argv[]); QWidget* topWidget (QWidget* widget); QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); +QPixmap bootstrapPixmap(const QString &id); QRect getTextRect(QPainter &p, int flags, const QString &text); void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 16aad134376091..4a7874b160ee8f 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -358,7 +358,7 @@ void CameraWidget::vipcThread() { while (!QThread::currentThread()->isInterruptionRequested()) { if (!vipc_client || cur_stream != requested_stream_type) { clearFrames(); - qDebug() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; + qDebug().nospace() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; cur_stream = requested_stream_type; vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); } diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index ecb44557c9ee24..2606df5efee871 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1044,11 +1044,19 @@ location set Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten zur Straßengewandten Weitwinkelkamera, um manche Kurven besser zu zeigen. Außerdem wird das Experimenteller Modus logo oben rechts angezeigt. - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar, da es den eingebauten adaptiven Tempomaten des Autos benutzt. + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt. - Enable experimental longitudinal control to allow experimental mode. + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. Aktiviere den experimentellen Openpilot Tempomaten für experimentelle Funktionen. diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 8226dd59f9d6c1..16f35dffaa17c8 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1017,14 +1017,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. openpilotはこの車の場合、車に内蔵されているACCを標準で利用します。この機能を有効にすることで実験段階のopenpilotによるアクセル制御を利用できます。実験モードと合わせて利用することをお勧めします。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - この車のACCがアクセル制御を行うため、実験モードを利用することができません。 - - - Enable experimental longitudinal control to allow experimental mode. - 実験段階のopenpilotによるアクセル制御を有効にしてください。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilotは標準ではゆっくりとくつろげる運転を提供します。この実験モードを有効にすると、以下のくつろげる段階ではない開発中の機能を利用する事ができます。 @@ -1045,6 +1037,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 新しい運転画面では、低速時に広角カメラの映像を表示することで、曲がる際の道路の視覚を向上します。実験段階を表すマークが右上に表示されます。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + この車のACCがアクセル制御を行うため実験モードを利用することができません。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 実験段階のopenpilotによるアクセル制御を有効にしてください。 + Updater diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index bda64c53aba5d4..f50d2d9b3f9130 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -501,7 +501,7 @@ location set Become a comma prime member at connect.comma.ai - connect.comma.ai에서 comma prime에 가입합니다 + connect.comma.ai 접속 comma prime 가입 PRIME FEATURES: @@ -1017,17 +1017,9 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 이 차량은 openpilot 롱컨트롤 대신 차량의 내장 ACC로 기본 설정됩니다. openpilot 롱컨트롤을 사용하려면 이 옵션을 활성화하세요. 실험적 openpilot 롱컨트롤을 사용하는 경우 실험적 모드를 활성화 하세요. - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 차량의 기본 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량에서는 실험적 모드를 사용할수 없습니다. - - - Enable experimental longitudinal control to allow experimental mode. - 실험적 롱컨트롤을 사용하려면 실험적 모드를 활성화 하세요. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: - openpilot은 기본적으로 <b>안정적 모드</b>로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 <b>알파 수준 기능</b>을 활성화 합니다. 실험 모드의 특징은 아래에 나열되어 있습니다 + openpilot은 기본적으로 <b>안정적 모드</b>로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 <b>알파 수준 기능</b>을 활성화 합니다. 실험적 모드의 특징은 아래에 나열되어 있습니다 🌮 End-to-End Longitudinal Control 🌮 @@ -1043,7 +1035,23 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - 주행 시각화는 저속에서 도로를 향하는 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드 로고도 우측상단에 표시됩니다. + 주행 시각화는 저속에서 도로를 향하는 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드의 로고도 우측상단에 표시됩니다. + + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 차량에 장착된 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량은 실험적 모드를 사용할 수 없습니다. + + + openpilot longitudinal control may come in a future update. + 오픈파일럿 롱컨트롤은 향후 업데이트에서 제공될 수 있습니다. + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + 오픈파일럿 롱컨트롤의 실험 버전은 실험적 모드와 함께 릴리즈 되지 않은 브랜치에서 테스트할 수 있습니다. + + + Enable experimental longitudinal control to allow Experimental mode. + 실험적 롱컨트롤을 사용하려면 실험적 모드를 활성화 하세요. diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 105d6f77f02bee..445de72fc35c07 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -132,19 +132,19 @@ Driver Camera - Câmera voltada para o Motorista + Câmera do Motorista PREVIEW - PREVISUAL + VER Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) - Pré-visualizar a câmera voltada para o motorista para garantir que monitor tem uma boa visibilidade (veículo precisa estar desligado) + Pré-visualizar a câmera voltada para o motorista para garantir que o monitoramento do sistema tenha uma boa visibilidade (veículo precisa estar desligado) Reset Calibration - Resetar Calibragem + Reinicializar Calibragem RESET @@ -200,7 +200,7 @@ openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. - o openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. o openpilot está continuamente calibrando, resetar raramente é necessário. + O openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. O openpilot está continuamente calibrando, resetar raramente é necessário. Your device is pointed %1° %2 and %3° %4. @@ -388,8 +388,8 @@ Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai - Obtenha instruções passo a passo exibidas e muito mais com -uma assinatura prime Inscreva-se agora: https://connect.comma.ai + Obtenha instruções passo a passo exibidas e muito mais com +uma assinatura prime. Inscreva-se agora: https://connect.comma.ai No home @@ -502,7 +502,7 @@ trabalho definido Become a comma prime member at connect.comma.ai - Torne-se um membro comma prime em connect.comma.ai + Seja um membro comma prime em connect.comma.ai PRIME FEATURES: @@ -514,11 +514,11 @@ trabalho definido 1 year of storage - 1 ano de armazenamento + 1 ano na nuvem Developer perks - Benefícios para desenvolvedor + Benefícios para devs @@ -835,7 +835,7 @@ trabalho definido Current Version - Versao Atual + Versão Atual Download @@ -863,11 +863,11 @@ trabalho definido UNINSTALL - DESINSTAL + REMOVER Uninstall %1 - Desintalar o %1 + Desinstalar o %1 Are you sure you want to uninstall? @@ -987,7 +987,7 @@ trabalho definido Disengage on Accelerator Pedal - Desacionar Com Pedal Do Acelerador + Desacionar com Pedal do Acelerador When enabled, pressing the accelerator pedal will disengage openpilot. @@ -995,7 +995,7 @@ trabalho definido Show ETA in 24h Format - Mostrar ETA em formato 24h + Mostrar ETA em Formato 24h Use 24h format instead of am/pm @@ -1021,14 +1021,6 @@ trabalho definido On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. Neste carro o penpilot por padrão utiliza o ACC nativo do veículo ao invés de controlar longitudinalmente. Ative isto para mudar para o controle longitudinal do openpilot. Ativar o Modo Experimental é recomendado quando em uso do controle longitudinal experimental do openpilot. - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - O Modo Experimental está atualmente indisponível para este carro, já que o ACC original do carro é usado para controle longitudinal. - - - Enable experimental longitudinal control to allow experimental mode. - Ative o controle longitudinal experimental para permitir o modo experimental. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot por padrão funciona em <b>modo chill</b>. modo Experimental ativa <b>recursos de nível-alfa</b> que não estão prontos para o modo chill. Recursos experimentais estão listados abaixo: @@ -1049,6 +1041,22 @@ trabalho definido The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. A visualização da direção fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será exibido no canto superior direito. + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + O modo Experimental está atualmente indisponível para este carro já que o ACC original do carro é usado para controle longitudinal. + + + openpilot longitudinal control may come in a future update. + O controle longitudinal openpilot pode vir em uma atualização futura. + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Uma versão experimental do controle longitudinal openpilot pode ser testada, juntamente com o modo Experimental, em branches de desenvolvimento. + + + Enable experimental longitudinal control to allow Experimental mode. + Ative o controle longitudinal experimental para permitir o modo Experimental. + Updater diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 0de0ba5f9af4fa..ce394ecb97bc2e 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -238,6 +238,14 @@ Disengage to Power Off ยกเลิกระบบช่วยขับเพื่อปิดเครื่อง + + Reset + รีเซ็ต + + + Review + ทบทวน + DriveStats @@ -273,6 +281,17 @@ กำลังเปิดกล้อง + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + คุณกำลังใช้โหมดทดลอง + + + CHILL MODE ON + คุณกำลังใช้โหมดชิล + + InputDialog @@ -463,6 +482,17 @@ location set จดจำ connect.comma.ai โดยการเพิ่มไปยังหน้าจอโฮม เพื่อใช้งานเหมือนเป็นแอปพลิเคชัน + + ParamControl + + Enable + เปิดใช้งาน + + + Cancel + ยกเลิก + + PrimeAdWidget @@ -585,13 +615,6 @@ location set ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ - - RichTextDialog - - Ok - ตกลง - - SettingsWindow @@ -850,6 +873,10 @@ location set Select a branch เลือก Branch + + Uninstall + ถอนการติดตั้ง + SshControl @@ -974,29 +1001,57 @@ location set Show map on left side when in split screen view. แสดงแผนที่ด้านซ้ายของหน้าจอเมื่ออยู่ในโหมดแบ่งหน้าจอ - - 🌮 End-to-end longitudinal (extremely alpha) 🌮 - 🌮 ควบคุมเร่ง/เบรคแบบ End-to-end (อยู่ขั้นพัฒนา) 🌮 - Experimental openpilot Longitudinal Control ทดลองใช้ระบบควบคุมการเร่ง/เบรคโดย openpilot - <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> - <b>คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นทดลอง และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด</b> + Experimental Mode + โหมดทดลอง - Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. - ให้ openpilot ควบคุมการเร่ง/เบรคแบบ end-to-end โดย openpilot จะขับอย่างที่มนุษย์คิด ระบบยังอยู่ในขั้นทดลอง + WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). + คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นพัฒนา และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด - openpilot longitudinal control is not currently available for this car. - ขณะนี้ยังไม่มีระบบควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. + โดยปกติสำหรับรถคันนี้ openpilot จะควบคุมการเร่ง/เบรคด้วยระบบ ACC จากโรงงาน แทนการควยคุมโดย openpilot เปิดสวิตซ์นี้เพื่อให้ openpilot ควบคุมการเร่ง/เบรค แนะนำให้เปิดโหมดทดลองเมื่อต้องการให้ openpilot ควบคุมการเร่ง/เบรค ซึ่งอยู่ในขั้นพัฒนา - Enable experimental longitudinal control to enable this. - เปิดใช้งานระบบควบคุมการเร่ง/เบรคขั้นทดลอง เพื่อเปิดใช้งานสิ่งนี้ + openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: + โดยปกติ openpilot จะขับใน<b>โหมดชิล</b> เปิดโหมดทดลองเพื่อใช้<b>ความสามารถในขั้นพัฒนา</b> ซึ่งยังไม่พร้อมสำหรับโหมดชิล ความสามารถในขั้นพัฒนามีดังนี้: + + + 🌮 End-to-End Longitudinal Control 🌮 + 🌮 ควบคุมเร่ง/เบรคแบบ End-to-End 🌮 + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. + ให้ openpilot ควบคุมการเร่ง/เบรค โดย openpilot จะขับอย่างที่มนุษย์คิด รวมถึงการหยุดที่ไฟแดง และป้ายหยุดรถ เนื่องจาก openpilot จะกำหนดความเร็วในการขับด้วยตัวเอง การตั้งความเร็วจะเป็นเพียงการกำหนดความเร็วสูงสูดเท่านั้น ความสามารถนี้ยังอยู่ในขั้นพัฒนา อาจเกิดข้อผิดพลาดขึ้นได้ + + + New Driving Visualization + การแสดงภาพการขับขี่แบบใหม่ + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย + + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + ขณะนี้โหมดทดลองไม่สามารถใช้งานได้ในรถคันนี้ เนื่องจากเปิดใช้ระบบควบคุมการเร่ง/เบรคของรถที่ติดตั้งจากโรงงานอยู่ + + + openpilot longitudinal control may come in a future update. + ระบบควบคุมการเร่ง/เบรคโดย openpilot อาจมาในการอัปเดตในอนาคต + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + เวอร์ชันทดลองของระบบควบคุมการเร่ง/เบรคโดย openpilot สามารถทดสอบได้พร้อมกับโหมดการทดลอง บน branch ที่กำลังพัฒนา + + + Enable experimental longitudinal control to allow Experimental mode. + เปิดระบบควบคุมการเร่ง/เบรคขั้นพัฒนาโดย openpilot เพื่อเปิดใช้งานโหมดทดลอง @@ -1052,5 +1107,9 @@ location set Forget Wi-Fi Network "%1"? เลิกใช้เครือข่าย Wi-Fi "%1"? + + Forget + เลิกใช้ + diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 31202e45f2ad63..792a9187708b89 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1015,14 +1015,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 针对此车辆，openpilot默认使用车辆自带的ACC，而非openpilot的纵向控制。启用此选项将切换到openpilot纵向控制。当使用试验性的openpilot纵向控制时，建议同时启用试验模式。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 由于此车辆使用自带的ACC纵向控制，当前无法使用试验模式。 - - - Enable experimental longitudinal control to allow experimental mode. - 启用试验性的纵向控制，以便允许使用试验模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 默认 <b>轻松模式</b>驾驶车辆。试验模式启用一些轻松模式之外的 <b>试验性功能</b>。试验性功能包括： @@ -1043,6 +1035,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 当低速行驶时，驾驶视角将切换到前向广角摄像头，便于更完整地显示转向路径。右上角将显示试验模式图标。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 由于此车辆使用自带的ACC纵向控制，当前无法使用试验模式。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 启用试验性的纵向控制，以便允许使用试验模式。 + Updater diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 0379e926c412bb..e8e2ab6bd3a7ff 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1017,14 +1017,6 @@ location set On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. 在本車輛中，openpilot預設將使用原車內建的ACC系統，而非openpilot縱向控制。開啟此開關來啟用openpilot縱向控制，使用此選項時建議一併啟用實驗模式。 - - Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. - 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 - - - Enable experimental longitudinal control to allow experimental mode. - 啟用實驗性縱向控制以使用實驗模式。 - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: openpilot 預設以 <b>輕鬆模式</b> 駕駛。 實驗模式啟用了尚未準備好進入輕鬆模式的 <b>alpha 級功能</b>。實驗功能如下： @@ -1045,6 +1037,22 @@ location set The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 低速行駛時，將會切換成路側廣角鏡頭，以完整顯示轉彎路徑，右上角將出現實驗模式圖案。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 + + + openpilot longitudinal control may come in a future update. + + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + + + + Enable experimental longitudinal control to allow Experimental mode. + 啟用實驗性縱向控制以使用實驗模式。 + Updater diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c62d7374816aa2..4df963167a9dbb 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -163,7 +163,7 @@ static void update_state(UIState *s) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); } if (sm.updated("wideRoadCameraState")) { - float scale = (sm["wideRoadCameraState"].getWideRoadCameraState().getSensor() == cereal::FrameData::ImageSensor::AR0321) ? 6.0f : 1.0f; + float scale = (sm["wideRoadCameraState"].getWideRoadCameraState().getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 9da2a05a11a8de..2cb7d1c13b6872 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -423,6 +423,9 @@ def main() -> None: wait_helper = WaitTimeHelper() wait_helper.only_check_for_update = True + # invalidate old finalized update + set_consistent_flag(False) + # Run the update loop while True: wait_helper.ready_event.clear() diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 30e2810ec4b243..7ee37380572702 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -167,7 +167,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setExposureValPercent(perc); if (c->camera_id == CAMERA_ID_AR0231) { - framed.setSensor(cereal::FrameData::ImageSensor::AR0321); + framed.setSensor(cereal::FrameData::ImageSensor::AR0231); } else if (c->camera_id == CAMERA_ID_OX03C10) { framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); } diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 091b0d91d9a699..92b3bde41330c6 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -60,15 +60,15 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { const float DC_GAIN_AR0231 = 2.5; const float DC_GAIN_OX03C10 = 7.32; -const float DC_GAIN_ON_GREY_AR0231= 0.2; +const float DC_GAIN_ON_GREY_AR0231 = 0.2; const float DC_GAIN_OFF_GREY_AR0231 = 0.3; -const float DC_GAIN_ON_GREY_OX03C10= 0.25; -const float DC_GAIN_OFF_GREY_OX03C10 = 0.35; +const float DC_GAIN_ON_GREY_OX03C10 = 0.9; +const float DC_GAIN_OFF_GREY_OX03C10 = 1.0; const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; -const int DC_GAIN_MIN_WEIGHT_OX03C10 = 16; -const int DC_GAIN_MAX_WEIGHT_OX03C10 = 32; +const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; const float TARGET_GREY_FACTOR_AR0231 = 1.0; const float TARGET_GREY_FACTOR_OX03C10 = 0.02; @@ -104,8 +104,8 @@ const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x11; // 2.5x const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; -const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.05; -const float ANALOG_GAIN_COST_HIGH_OX03C10 = 0.8; +const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; +const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4; const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms @@ -1041,6 +1041,30 @@ void CameraState::handle_camera_event(void *evdat) { } } +void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { + float score = 1e6; + if (camera_id == CAMERA_ID_AR0231) { + // Cost of ev diff + score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; + // Cost of absolute gain + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + // Cost of changing gain + score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; + } else if (camera_id == CAMERA_ID_OX03C10) { + score = std::abs(desired_ev - (exp_t * exp_gain)); + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; + } + + if (score < best_ev_score) { + new_exp_t = exp_t; + new_exp_g = exp_g_idx; + best_ev_score = score; + } +} + void CameraState::set_camera_exposure(float grey_frac) { if (!enabled) return; const float dt = 0.05; @@ -1066,9 +1090,9 @@ void CameraState::set_camera_exposure(float grey_frac) { float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); - float best_ev_score = 1e6; - int new_g = 0; - int new_t = 0; + best_ev_score = 1e6; + new_exp_g = 0; + new_exp_t = 0; // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes @@ -1095,8 +1119,8 @@ void CameraState::set_camera_exposure(float grey_frac) { gain_idx = std::stoi(gain_bytes); exposure_time = std::stoi(time_bytes); - new_g = gain_idx; - new_t = exposure_time; + new_exp_g = gain_idx; + new_exp_t = exposure_time; enable_dc_gain = false; } else { // Simple brute force optimizer to choose sensor parameters @@ -1112,23 +1136,7 @@ void CameraState::set_camera_exposure(float grey_frac) { continue; } - // Compute error to desired ev - float score = std::abs(desired_ev - (t * gain)) * 10; - - // Going below recommended gain needs lower penalty to not overexpose - float m = g > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(g - (int)analog_gain_rec_idx) * m; - - // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); - - // Small penalty on changing gain - score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (g - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(g - gain_idx) * (score + 1.0) / 10.0; - - if (score < best_ev_score) { - new_t = t; - new_g = g; - best_ev_score = score; - } + update_exposure_score(desired_ev, t, g, gain); } } @@ -1137,9 +1145,9 @@ void CameraState::set_camera_exposure(float grey_frac) { measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = sensor_analog_gains[new_g]; - gain_idx = new_g; - exposure_time = new_t; + analog_gain_frac = sensor_analog_gains[new_exp_g]; + gain_idx = new_exp_g; + exposure_time = new_exp_t; dc_gain_enabled = enable_dc_gain; float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); @@ -1156,7 +1164,7 @@ void CameraState::set_camera_exposure(float grey_frac) { // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); if (camera_id == CAMERA_ID_AR0231) { - uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; + uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; struct i2c_random_wr_payload exp_reg_array[] = { {0x3366, analog_gain_reg}, {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, @@ -1164,15 +1172,16 @@ void CameraState::set_camera_exposure(float grey_frac) { }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); } else if (camera_id == CAMERA_ID_OX03C10) { - // t_HCG + t_LCG + t_VS on LPD, t_SPD on SPD - uint32_t hcg_time = std::max((dc_gain_weight * exposure_time / dc_gain_max_weight), 0); - uint32_t lcg_time = std::max(((dc_gain_max_weight - dc_gain_weight) * exposure_time / dc_gain_max_weight), 0); - // uint32_t spd_time = std::max(hcg_time / 16, (uint32_t)exposure_time_min); - uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 128, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); - uint32_t spd_time = vs_time; - - uint32_t real_gain = ox03c10_analog_gains_reg[new_g]; + // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = exposure_time; + uint32_t lcg_time = hcg_time; + uint32_t spd_time = exposure_time_max + VS_TIME_MAX_OX03C10; + uint32_t 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}, @@ -1181,7 +1190,7 @@ void CameraState::set_camera_exposure(float grey_frac) { {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, {0x3588, min_gain>>8}, {0x3589, min_gain&0xFF}, - {0x3548, min_gain>>8}, {0x3549, 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/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 9f0c3743f1d7af..9e0109ab20b5ad 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -47,6 +47,9 @@ class CameraState { float cur_ev[3]; float min_ev, max_ev; + float best_ev_score; + int new_exp_g; + int new_exp_t; float measured_grey_fraction; float target_grey_fraction; @@ -58,6 +61,7 @@ class CameraState { int camera_num; void handle_camera_event(void *evdat); + void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); void set_camera_exposure(float grey_frac); void sensors_start(); diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index ab51059d9ac852..83fcb8f7a9adfc 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -126,7 +126,7 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3219, 0x08}, {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure - {0x350a, 0x04}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + {0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain @@ -711,11 +711,11 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x4221, 0x03}, // this is changed from 1 -> 3 // DCG exposure coarse - {0x3501, 0x01}, {0x3502, 0xc8}, + // {0x3501, 0x01}, {0x3502, 0xc8}, // SPD exposure coarse - {0x3541, 0x01}, {0x3542, 0xc8}, + // {0x3541, 0x01}, {0x3542, 0xc8}, // VS exposure coarse - {0x35c1, 0x00}, {0x35c2, 0x01}, + // {0x35c1, 0x00}, {0x35c2, 0x01}, // crc reference {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py index 6c2ef1c7bce95b..f03c531b20e7a1 100755 --- a/system/camerad/test/test_camerad.py +++ b/system/camerad/test/test_camerad.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import time import unittest +import numpy as np from collections import defaultdict import cereal.messaging as messaging @@ -10,8 +11,10 @@ from system.hardware import TICI TEST_TIMESPAN = 30 -LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0321: 0.5, # ARs use synced pulses for frame starts +LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 0.5, # ARs use synced pulses for frame starts log.FrameData.ImageSensor.ox03c10: 1.0} # OXs react to out-of-sync at next frame +FRAME_DELTA_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 1.0, + log.FrameData.ImageSensor.ox03c10: 1.0} CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') @@ -36,10 +39,16 @@ def setUpClass(cls): managed_processes['camerad'].stop() cls.log_by_frame_id = defaultdict(list) + cls.sensor_type = None for cam, msgs in cls.logs.items(): + if cls.sensor_type is None: + cls.sensor_type = getattr(msgs[0], msgs[0].which()).sensor.raw expected_frames = service_list[cam].frequency * TEST_TIMESPAN assert expected_frames*0.95 < len(msgs) < expected_frames*1.05, f"unexpected frame count {cam}: {expected_frames=}, got {len(msgs)}" + dts = np.abs(np.diff([getattr(m, m.which()).timestampSof/1e6 for m in msgs]) - 1000/service_list[cam].frequency) + assert (dts < FRAME_DELTA_TOLERANCE[cls.sensor_type]).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}" + for m in msgs: cls.log_by_frame_id[getattr(m, m.which()).frameId].append(m) @@ -64,17 +73,16 @@ def test_frame_skips(self): assert len(skips) == 0, f"Found frame skips, missing cameras for the following frames: {skips}" def test_frame_sync(self): - sensor_type = [getattr(msgs[0], msgs[0].which()).sensor for frame_id, msgs in self.log_by_frame_id.items()][0].raw frame_times = {frame_id: [getattr(m, m.which()).timestampSof for m in msgs] for frame_id, msgs in self.log_by_frame_id.items()} diffs = {frame_id: (max(ts) - min(ts))/1e6 for frame_id, ts in frame_times.items()} def get_desc(fid, diff): cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]] return (diff, cam_times) - laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[sensor_type]} + laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]} def in_tol(diff): - return 50 - LAG_FRAME_TOLERANCE[sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[sensor_type] + return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type] if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames): print("TODO: handle camera out of sync") else: diff --git a/system/camerad/test/test_exposure.py b/system/camerad/test/test_exposure.py index 8cce7e7ffa592b..201b205c4f7fd9 100755 --- a/system/camerad/test/test_exposure.py +++ b/system/camerad/test/test_exposure.py @@ -6,16 +6,13 @@ from selfdrive.test.helpers import with_processes from system.camerad.snapshot.snapshot import get_snapshots -from system.hardware import TICI - TEST_TIME = 45 REPEAT = 5 class TestCamerad(unittest.TestCase): @classmethod def setUpClass(cls): - if not TICI: - raise unittest.SkipTest + pass def _numpy_rgb2gray(self, im): ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) @@ -37,13 +34,11 @@ def test_camera_operation(self): start = time.time() while time.time() - start < TEST_TIME and passed < REPEAT: rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") + wpic, _ = get_snapshots(frame="wideRoadCameraState") res = self._is_exposure_okay(rpic) res = res and self._is_exposure_okay(dpic) - - if TICI: - wpic, _ = get_snapshots(frame="wideRoadCameraState") - res = res and self._is_exposure_okay(wpic) + res = res and self._is_exposure_okay(wpic) if passed > 0 and not res: passed = -passed # fails test if any failure after first sus diff --git a/system/micd.py b/system/micd.py index a56140e3b96cf1..97ba0c262ed387 100755 --- a/system/micd.py +++ b/system/micd.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import sounddevice as sd import numpy as np from cereal import messaging @@ -84,11 +83,11 @@ def callback(self, indata, frames, time, status): self.measurements = self.measurements[FFT_SAMPLES:] - def micd_thread(self, device=None): - if device is None: - device = "sysdefault" + def micd_thread(self): + # sounddevice must be imported after forking processes + import sounddevice as sd # pylint: disable=import-outside-toplevel - with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") while True: self.update() diff --git a/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore new file mode 100644 index 00000000000000..ac06c0cf85b3ae --- /dev/null +++ b/third_party/bootstrap/.gitignore @@ -0,0 +1 @@ +/icons/ diff --git a/third_party/bootstrap/bootstrap-icons.svg b/third_party/bootstrap/bootstrap-icons.svg new file mode 100644 index 00000000000000..61f2720db4dbf1 --- /dev/null +++ b/third_party/bootstrap/bootstrap-icons.svg @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/third_party/bootstrap/pull.sh b/third_party/bootstrap/pull.sh new file mode 100755 index 00000000000000..0b03b4db9eff3e --- /dev/null +++ b/third_party/bootstrap/pull.sh @@ -0,0 +1,14 @@ +#!/bin/bash +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +cd $DIR + +if [ ! -d icons/ ]; then + git clone https://github.com/twbs/icons/ +fi + +cd icons +git fetch --all +git checkout d5aa187483a1b0b186f87adcfa8576350d970d98 +cp bootstrap-icons.svg ../ diff --git a/tinygrad_repo b/tinygrad_repo index 8e22d5ee675277..2e1d47b16625ff 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 8e22d5ee675277181e1eff644dde9e844fc40fce +Subproject commit 2e1d47b16625ff343516287cdd9e4bcb26f5c4ef diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 718e2e50afe590..0c9ad14973fa73 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -1,6 +1,6 @@ import os Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', 'replay_lib', - 'cereal', 'transformations', 'widgets', 'opendbc') + 'cereal', 'transformations', 'widgets', 'opendbc', 'asset_obj') base_frameworks = qt_env['FRAMEWORKS'] base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', @@ -8,18 +8,24 @@ base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', if arch == "Darwin": base_frameworks.append('OpenCL') + base_frameworks.append('QtCharts') else: base_libs.append('OpenCL') + base_libs.append('Qt5Charts') + +qt_libs = ['qt_util'] + base_libs -qt_libs = ['qt_util', 'Qt5Charts'] + base_libs cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs cabana_env = qt_env.Clone() prev_moc_path = cabana_env['QT_MOCHPREFIX'] cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_' -cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc', 'dbcmanager.cc', - 'canmessages.cc', 'commands.cc', 'messageswidget.cc', 'settings.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) -cabana_env.Program('_cabana', ['cabana.cc', cabana_lib], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) +cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc', 'dbcmanager.cc', + 'commands.cc', 'messageswidget.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) +cabana_env.Program('_cabana', ['cabana.cc', cabana_lib, asset_obj], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) + +if arch == "Darwin": + cabana_env.Execute('install_name_tool -change opendbc/can/libdbc.dylib @loader_path/../../opendbc/can/libdbc.dylib ./_cabana') if GetOption('test'): cabana_env.Program('tests/_test_cabana', ['tests/test_runner.cc', 'tests/test_cabana.cc', cabana_lib], LIBS=[cabana_libs]) diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index b98c75d452ed05..4d7b3148c64a1d 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -4,13 +4,16 @@ #include #include #include +#include #include -#include "tools/cabana/canmessages.h" +#include "tools/cabana/commands.h" +#include "tools/cabana/streams/abstractstream.h" // BinaryView const int CELL_HEIGHT = 36; +const int VERTICAL_HEADER_WIDTH = 30; inline int get_bit_index(const QModelIndex &index, bool little_endian) { return index.row() * 8 + (little_endian ? 7 - index.column() : index.column()); @@ -29,14 +32,27 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) { setFrameShape(QFrame::NoFrame); setShowGrid(false); setMouseTracking(true); - setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &BinaryView::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &BinaryView::refresh); +} + +QSize BinaryView::minimumSizeHint() const { + return {(horizontalHeader()->minimumSectionSize() + 1) * 9 + VERTICAL_HEADER_WIDTH, + CELL_HEIGHT * std::min(model->rowCount(), 10)}; } void BinaryView::highlight(const Signal *sig) { if (sig != hovered_sig) { + for (int i = 0; i < model->items.size(); ++i) { + auto &item_sigs = model->items[i].sigs; + if ((sig && item_sigs.contains(sig)) || (hovered_sig && item_sigs.contains(hovered_sig))) { + auto index = model->index(i / model->columnCount(), i % model->columnCount()); + emit model->dataChanged(index, index, {Qt::DisplayRole}); + } + } hovered_sig = sig; - model->dataChanged(model->index(0, 0), model->index(model->rowCount() - 1, model->columnCount() - 1)); emit signalHovered(hovered_sig); } } @@ -113,13 +129,20 @@ void BinaryView::leaveEvent(QEvent *event) { } void BinaryView::setMessage(const QString &message_id) { - model->setMessage(message_id); + model->msg_id = message_id; + verticalScrollBar()->setValue(0); + refresh(); +} + +void BinaryView::refresh() { + if (model->msg_id.isEmpty()) return; + clearSelection(); anchor_index = QModelIndex(); resize_sig = nullptr; hovered_sig = nullptr; highlightPosition(QCursor::pos()); - updateState(); + model->refresh(); } QSet BinaryView::getOverlappingSignals() const { @@ -144,9 +167,8 @@ std::tuple BinaryView::getSelection(QModelIndex index) { // BinaryViewModel -void BinaryViewModel::setMessage(const QString &message_id) { +void BinaryViewModel::refresh() { beginResetModel(); - msg_id = message_id; items.clear(); if ((dbc_msg = dbc()->msg(msg_id))) { row_count = dbc_msg->size; @@ -173,11 +195,14 @@ void BinaryViewModel::setMessage(const QString &message_id) { items.resize(row_count * column_count); } endResetModel(); + updateState(); } void BinaryViewModel::updateState() { auto prev_items = items; - const auto &binary = can->lastMessage(msg_id).dat; + const auto &last_msg = can->lastMessage(msg_id); + const auto &binary = last_msg.dat; + // data size may changed. if (binary.size() > row_count) { beginInsertRows({}, row_count, binary.size() - 1); @@ -187,12 +212,13 @@ void BinaryViewModel::updateState() { } char hex[3] = {'\0'}; for (int i = 0; i < binary.size(); ++i) { - for (int j = 0; j < column_count - 1; ++j) { + for (int j = 0; j < 8; ++j) { items[i * column_count + j].val = ((binary[i] >> (7 - j)) & 1) != 0 ? '1' : '0'; } hex[0] = toHex(binary[i] >> 4); hex[1] = toHex(binary[i] & 0xf); items[i * column_count + 8].val = hex; + items[i * column_count + 8].bg_color = last_msg.colors[i]; } for (int i = binary.size(); i < row_count; ++i) { for (int j = 0; j < column_count; ++j) { @@ -200,8 +226,8 @@ void BinaryViewModel::updateState() { } } - for (int i = 0; i < row_count * column_count; ++i) { - if (i >= prev_items.size() || prev_items[i].val != items[i].val) { + for (int i = 0; i < items.size(); ++i) { + if (i >= prev_items.size() || prev_items[i].val != items[i].val || prev_items[i].bg_color != items[i].bg_color) { auto idx = index(i / column_count, i % column_count); emit dataChanged(idx, idx); } @@ -212,7 +238,7 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i if (orientation == Qt::Vertical) { switch (role) { case Qt::DisplayRole: return section; - case Qt::SizeHintRole: return QSize(30, 0); + case Qt::SizeHintRole: return QSize(VERTICAL_HEADER_WIDTH, 0); case Qt::TextAlignmentRole: return Qt::AlignCenter; } } @@ -234,12 +260,14 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op if (index.column() == 8) { painter->setFont(hex_font); + painter->fillRect(option.rect, item->bg_color); } else if (option.state & QStyle::State_Selected) { painter->fillRect(option.rect, selection_color); painter->setPen(option.palette.color(QPalette::BrightText)); } else if (!item->sigs.isEmpty() && (!bin_view->selectionModel()->hasSelection() || !item->sigs.contains(bin_view->resize_sig))) { - painter->fillRect(option.rect, item->bg_color); - painter->setPen(item->sigs.contains(bin_view->hovered_sig) ? option.palette.color(QPalette::BrightText) : Qt::black); + bool sig_hovered = item->sigs.contains(bin_view->hovered_sig); + painter->fillRect(option.rect, sig_hovered ? item->bg_color.darker(125) : item->bg_color); // 4/5x brightness + painter->setPen(sig_hovered ? option.palette.color(QPalette::BrightText) : Qt::black); } painter->drawText(option.rect, Qt::AlignCenter, item->val); diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index e936efc65830af..f697ea28cb9d3c 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -21,7 +21,7 @@ class BinaryItemDelegate : public QStyledItemDelegate { class BinaryViewModel : public QAbstractTableModel { public: BinaryViewModel(QObject *parent) : QAbstractTableModel(parent) {} - void setMessage(const QString &message_id); + void refresh(); void updateState(); QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const { return {}; } @@ -39,12 +39,11 @@ class BinaryViewModel : public QAbstractTableModel { QColor bg_color = QApplication::style()->standardPalette().color(QPalette::Base); bool is_msb = false; bool is_lsb = false; - QString val = "0"; + QString val = "-"; QList sigs; }; std::vector items; -private: QString msg_id; const DBCMsg *dbc_msg = nullptr; int row_count = 0; @@ -60,6 +59,7 @@ class BinaryView : public QTableView { void highlight(const Signal *sig); QSet getOverlappingSignals() const; inline void updateState() { model->updateState(); } + QSize minimumSizeHint() const override; signals: void signalClicked(const Signal *sig); @@ -68,6 +68,7 @@ class BinaryView : public QTableView { void resizeSignal(const Signal *sig, int from, int size); private: + void refresh(); std::tuple getSelection(QModelIndex index); void setSelection(const QRect &rect, QItemSelectionModel::SelectionFlags flags) override; void mousePressEvent(QMouseEvent *event) override; diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 4f037ba595c54e..e34e2d02050ae9 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -4,12 +4,15 @@ #include "common/prefix.h" #include "selfdrive/ui/qt/util.h" #include "tools/cabana/mainwin.h" +#include "tools/cabana/streams/livestream.h" +#include "tools/cabana/streams/replaystream.h" int main(int argc, char *argv[]) { QCoreApplication::setApplicationName("Cabana"); QCoreApplication::setAttribute(Qt::AA_ShareOpenGLContexts); initApp(argc, argv); QApplication app(argc, argv); + app.setApplicationDisplayName("Cabana"); QCommandLineParser cmd_parser; cmd_parser.addHelpOption(); @@ -17,29 +20,50 @@ int main(int argc, char *argv[]) { cmd_parser.addOption({"demo", "use a demo route instead of providing your own"}); cmd_parser.addOption({"qcam", "load qcamera"}); cmd_parser.addOption({"ecam", "load wide road camera"}); + cmd_parser.addOption({"stream", "read can messages from live streaming"}); + cmd_parser.addOption({"zmq", "the ip address on which to receive zmq messages", "zmq"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); + cmd_parser.addOption({"no-vipc", "do not output video"}); + cmd_parser.addOption({"dbc", "dbc file to open", "dbc"}); cmd_parser.process(app); const QStringList args = cmd_parser.positionalArguments(); - if (args.empty() && !cmd_parser.isSet("demo")) { + if (args.empty() && !cmd_parser.isSet("demo") && !cmd_parser.isSet("stream")) { cmd_parser.showHelp(); } + std::unique_ptr op_prefix; + std::unique_ptr stream; - const QString route = args.empty() ? DEMO_ROUTE : args.first(); - uint32_t replay_flags = REPLAY_FLAG_NONE; - if (cmd_parser.isSet("ecam")) { - replay_flags |= REPLAY_FLAG_ECAM; - } else if (cmd_parser.isSet("qcam")) { - replay_flags |= REPLAY_FLAG_QCAMERA; + if (cmd_parser.isSet("stream")) { + stream.reset(new LiveStream(&app, cmd_parser.value("zmq"))); + } else { + // TODO: Remove when OpenpilotPrefix supports ZMQ +#ifndef __APPLE__ + op_prefix.reset(new OpenpilotPrefix()); +#endif + const QString route = args.empty() ? DEMO_ROUTE : args.first(); + uint32_t replay_flags = REPLAY_FLAG_NONE; + if (cmd_parser.isSet("ecam")) { + replay_flags |= REPLAY_FLAG_ECAM; + } else if (cmd_parser.isSet("qcam")) { + replay_flags |= REPLAY_FLAG_QCAMERA; + } else if (cmd_parser.isSet("no-vipc")) { + replay_flags |= REPLAY_FLAG_NO_VIPC; + } + auto replay_stream = new ReplayStream(&app); + stream.reset(replay_stream); + if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { + return 0; + } } - OpenpilotPrefix op_prefix; - CANMessages p(&app); - int ret = 0; - if (p.loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { - MainWindow w; - w.show(); - ret = app.exec(); + MainWindow w; + + // Load DBC + if (cmd_parser.isSet("dbc")) { + w.loadFile(cmd_parser.value("dbc")); } - return ret; + + w.show(); + return app.exec(); } diff --git a/tools/cabana/canmessages.cc b/tools/cabana/canmessages.cc deleted file mode 100644 index ad3ee80e3d89cc..00000000000000 --- a/tools/cabana/canmessages.cc +++ /dev/null @@ -1,96 +0,0 @@ -#include "tools/cabana/canmessages.h" -#include "tools/cabana/dbcmanager.h" - -CANMessages *can = nullptr; - -CANMessages::CANMessages(QObject *parent) : QObject(parent) { - can = this; - QObject::connect(this, &CANMessages::received, this, &CANMessages::process, Qt::QueuedConnection); - QObject::connect(&settings, &Settings::changed, this, &CANMessages::settingChanged); -} - -CANMessages::~CANMessages() { - replay->stop(); -} - -static bool event_filter(const Event *e, void *opaque) { - CANMessages *c = (CANMessages *)opaque; - return c->eventFilter(e); -} - -bool CANMessages::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { - replay = new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this); - replay->setSegmentCacheLimit(settings.cached_segment_limit); - replay->installEventFilter(event_filter, this); - QObject::connect(replay, &Replay::seekedTo, this, &CANMessages::seekedTo); - QObject::connect(replay, &Replay::segmentsMerged, this, &CANMessages::eventsMerged); - QObject::connect(replay, &Replay::streamStarted, this, &CANMessages::streamStarted); - if (replay->load()) { - const auto &segments = replay->route()->segments(); - if (std::none_of(segments.begin(), segments.end(), [](auto &s) { return s.second.rlog.length() > 0; })) { - qWarning() << "no rlogs in route" << route; - return false; - } - replay->start(); - return true; - } - return false; -} - -void CANMessages::process(QHash *messages) { - for (auto it = messages->begin(); it != messages->end(); ++it) { - can_msgs[it.key()] = it.value(); - } - emit updated(); - emit msgsReceived(messages); - delete messages; - processing = false; -} - -bool CANMessages::eventFilter(const Event *event) { - static std::unique_ptr new_msgs = std::make_unique>(); - static double prev_update_ts = 0; - - if (event->which == cereal::Event::Which::CAN) { - double current_sec = replay->currentSeconds(); - if (counters_begin_sec == 0 || counters_begin_sec >= current_sec) { - new_msgs->clear(); - counters.clear(); - counters_begin_sec = current_sec; - } - - auto can_events = event->event.getCan(); - for (const auto &c : can_events) { - QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); - CanData &data = (*new_msgs)[id]; - data.ts = current_sec; - data.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); - data.count = ++counters[id]; - if (double delta = (current_sec - counters_begin_sec); delta > 0) { - data.freq = data.count / delta; - } - } - - double ts = millis_since_boot(); - if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing && !new_msgs->isEmpty()) { - // delay posting CAN message if UI thread is busy - processing = true; - prev_update_ts = ts; - // use pointer to avoid data copy in queued connection. - emit received(new_msgs.release()); - new_msgs.reset(new QHash); - new_msgs->reserve(100); - } - } - return true; -} - -void CANMessages::seekTo(double ts) { - replay->seekTo(std::max(double(0), ts), false); - counters_begin_sec = 0; - emit updated(); -} - -void CANMessages::settingChanged() { - replay->setSegmentCacheLimit(settings.cached_segment_limit); -} diff --git a/tools/cabana/canmessages.h b/tools/cabana/canmessages.h deleted file mode 100644 index 4dac4fe9df4dd6..00000000000000 --- a/tools/cabana/canmessages.h +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "opendbc/can/common_dbc.h" -#include "tools/cabana/settings.h" -#include "tools/replay/replay.h" - -struct CanData { - double ts = 0.; - uint32_t count = 0; - uint32_t freq = 0; - QByteArray dat; -}; - -class CANMessages : public QObject { - Q_OBJECT - -public: - CANMessages(QObject *parent); - ~CANMessages(); - bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); - void seekTo(double ts); - bool eventFilter(const Event *event); - - inline QString routeName() const { return replay->route()->name(); } - inline QString carFingerprint() const { return replay->carFingerprint().c_str(); } - inline VisionStreamType visionStreamType() const { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } - inline double totalSeconds() const { return replay->totalSeconds(); } - inline double routeStartTime() const { return replay->routeStartTime() / (double)1e9; } - inline double currentSec() const { return replay->currentSeconds(); } - inline const CanData &lastMessage(const QString &id) { return can_msgs[id]; } - - inline const Route* route() const { return replay->route(); } - inline const std::vector *events() const { return replay->events(); } - inline void setSpeed(float speed) { replay->setSpeed(speed); } - inline bool isPaused() const { return replay->isPaused(); } - inline void pause(bool pause) { replay->pause(pause); } - inline const std::vector> getTimeline() { return replay->getTimeline(); } - -signals: - void seekedTo(double sec); - void streamStarted(); - void eventsMerged(); - void updated(); - void msgsReceived(const QHash *); - void received(QHash *); - -public: - QMap can_msgs; - -protected: - void process(QHash *); - void settingChanged(); - - Replay *replay = nullptr; - std::atomic counters_begin_sec = 0; - std::atomic processing = false; - QHash counters; -}; - -inline QString toHex(const QByteArray &dat) { - return dat.toHex(' ').toUpper(); -} -inline char toHex(uint value) { - return "0123456789ABCDEF"[value & 0xF]; -} - -inline const QString &getColor(int i) { - // TODO: add more colors - static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; - return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)]; -} - -// A global pointer referring to the unique CANMessages object -extern CANMessages *can; diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index cf01533aaa9fcd..e49d28db445568 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -2,9 +2,12 @@ #include #include -#include +#include +#include #include #include +#include +#include #include #include #include @@ -15,26 +18,49 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); // toolbar QToolBar *toolbar = new QToolBar(tr("Charts"), this); - title_label = new QLabel(); - title_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - toolbar->addWidget(title_label); - show_all_values_btn = toolbar->addAction(""); - toolbar->addWidget(range_label = new QLabel()); - reset_zoom_btn = toolbar->addAction("⟲"); + toolbar->setIconSize({16, 16}); + + QAction *new_plot_btn = toolbar->addAction(utils::icon("file-plus"), ""); + new_plot_btn->setToolTip(tr("New Plot")); + toolbar->addWidget(title_label = new QLabel()); + title_label->setContentsMargins(0, 0, 12, 0); + columns_cb = new QComboBox(this); + columns_cb->addItems({"1", "2", "3", "4"}); + columns_lb_action = toolbar->addWidget(new QLabel(tr("Columns:"))); + columns_cb_action = toolbar->addWidget(columns_cb); + + QLabel *stretch_label = new QLabel(this); + stretch_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + toolbar->addWidget(stretch_label); + + toolbar->addWidget(new QLabel(tr("Range:"))); + toolbar->addWidget(range_lb = new QLabel(this)); + range_slider = new QSlider(Qt::Horizontal, this); + range_slider->setToolTip(tr("Set the chart range")); + range_slider->setRange(1, settings.max_cached_minutes * 60); + range_slider->setSingleStep(1); + range_slider->setPageStep(60); // 1 min + toolbar->addWidget(range_slider); + + reset_zoom_btn = toolbar->addAction(utils::icon("zoom-out"), ""); reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); - remove_all_btn = toolbar->addAction("✖"); + remove_all_btn = toolbar->addAction(utils::icon("x"), ""); remove_all_btn->setToolTip(tr("Remove all charts")); dock_btn = toolbar->addAction(""); main_layout->addWidget(toolbar); // charts + charts_layout = new QGridLayout(); + charts_layout->setSpacing(10); + QWidget *charts_container = new QWidget(this); - charts_layout = new QVBoxLayout(charts_container); - charts_layout->addStretch(); + QVBoxLayout *charts_main_layout = new QVBoxLayout(charts_container); + charts_main_layout->setContentsMargins(0, 0, 0, 0); + charts_main_layout->addLayout(charts_layout); + charts_main_layout->addStretch(0); QScrollArea *charts_scroll = new QScrollArea(this); charts_scroll->setWidgetResizable(true); @@ -42,17 +68,29 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); main_layout->addWidget(charts_scroll); - max_chart_range = settings.max_chart_x_range; + align_charts_timer = new QTimer(this); + align_charts_timer->setSingleShot(true); + align_charts_timer->callOnTimeout(this, &ChartsWidget::alignCharts); + + // init settings use_dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > QApplication::style()->standardPalette().color(QPalette::Background).value(); + column_count = std::clamp(settings.chart_column_count, 1, columns_cb->count()); + max_chart_range = std::clamp(settings.chart_range, 1, settings.max_cached_minutes * 60); + display_range = {0, max_chart_range}; + columns_cb->setCurrentIndex(column_count - 1); + range_slider->setValue(max_chart_range); updateToolBar(); QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &ChartsWidget::removeAll); - QObject::connect(can, &CANMessages::eventsMerged, this, &ChartsWidget::eventsMerged); - QObject::connect(can, &CANMessages::updated, this, &ChartsWidget::updateState); - QObject::connect(show_all_values_btn, &QAction::triggered, this, &ChartsWidget::showAllData); + QObject::connect(can, &AbstractStream::eventsMerged, this, &ChartsWidget::eventsMerged); + QObject::connect(can, &AbstractStream::updated, this, &ChartsWidget::updateState); + QObject::connect(range_slider, &QSlider::valueChanged, this, &ChartsWidget::setMaxChartRange); + QObject::connect(new_plot_btn, &QAction::triggered, this, &ChartsWidget::newChart); QObject::connect(remove_all_btn, &QAction::triggered, this, &ChartsWidget::removeAll); QObject::connect(reset_zoom_btn, &QAction::triggered, this, &ChartsWidget::zoomReset); + QObject::connect(columns_cb, SIGNAL(activated(int)), SLOT(setColumnCount(int))); + QObject::connect(&settings, &Settings::changed, this, &ChartsWidget::settingChanged); QObject::connect(dock_btn, &QAction::triggered, [this]() { emit dock(!docking); docking = !docking; @@ -61,27 +99,15 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { } void ChartsWidget::eventsMerged() { - if (auto events = can->events(); events && !events->empty()) { - event_range.first = (events->front()->mono_time / (double)1e9) - can->routeStartTime(); - event_range.second = (events->back()->mono_time / (double)1e9) - can->routeStartTime(); - updateState(); - } -} - -void ChartsWidget::updateDisplayRange() { - auto prev_range = display_range; - double current_sec = can->currentSec(); - if (current_sec < display_range.first || current_sec >= (display_range.second - 5)) { - // reached the end, or seeked to a timestamp out of range. - display_range.first = current_sec - 5; - } - display_range.first = std::floor(std::max(display_range.first, event_range.first) * 10.0) / 10.0; - display_range.second = std::floor(std::min(display_range.first + max_chart_range, event_range.second) * 10.0) / 10.0; - if (prev_range != display_range) { + { + assert(!can->liveStreaming()); QFutureSynchronizer future_synchronizer; - for (auto c : charts) - future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::setEventsRange, display_range)); + const auto events = can->events(); + for (auto c : charts) { + future_synchronizer.addFuture(QtConcurrent::run(c, &ChartView::updateSeries, nullptr, events, true)); + } } + updateState(); } void ChartsWidget::zoomIn(double min, double max) { @@ -99,42 +125,62 @@ void ChartsWidget::zoomReset() { void ChartsWidget::updateState() { if (charts.isEmpty()) return; + if (can->liveStreaming()) { + // appends incoming events to the end of series + const auto events = can->events(); + for (auto c : charts) { + c->updateSeries(nullptr, events, false); + } + } + + const double cur_sec = can->currentSec(); if (!is_zoomed) { - updateDisplayRange(); - } else if (can->currentSec() < zoomed_range.first || can->currentSec() >= zoomed_range.second) { + double pos = (cur_sec - display_range.first) / max_chart_range; + if (pos < 0 || pos > 0.8) { + const double min_event_sec = (can->events()->front()->mono_time / (double)1e9) - can->routeStartTime(); + display_range.first = std::floor(std::max(min_event_sec, cur_sec - max_chart_range * 0.2)); + } + display_range.second = std::floor(display_range.first + max_chart_range); + } else if (cur_sec < zoomed_range.first || cur_sec >= zoomed_range.second) { + // loop in zoommed range can->seekTo(zoomed_range.first); } - const auto &range = is_zoomed ? zoomed_range : display_range; setUpdatesEnabled(false); + const auto &range = is_zoomed ? zoomed_range : display_range; for (auto c : charts) { - c->setDisplayRange(range.first, range.second); - c->scene()->invalidate({}, QGraphicsScene::ForegroundLayer); + c->updatePlot(cur_sec, range.first, range.second); } setUpdatesEnabled(true); } -void ChartsWidget::showAllData() { - bool switch_to_show_all = max_chart_range == settings.max_chart_x_range; - max_chart_range = switch_to_show_all ? settings.cached_segment_limit * 60 - : settings.max_chart_x_range; - max_chart_range = std::min(max_chart_range, (uint32_t)can->totalSeconds()); +void ChartsWidget::setMaxChartRange(int value) { + max_chart_range = settings.chart_range = value; + double current_sec = can->currentSec(); + const double min_event_sec = (can->events()->front()->mono_time / (double)1e9) - can->routeStartTime(); + // keep current_sec's pos + double pos = (current_sec - display_range.first) / (display_range.second - display_range.first); + display_range.first = std::floor(std::max(min_event_sec, current_sec - max_chart_range * (1.0 - pos))); + display_range.second = std::floor(display_range.first + max_chart_range); updateToolBar(); updateState(); } void ChartsWidget::updateToolBar() { - int min_range = std::min(settings.max_chart_x_range, (int)can->totalSeconds()); - bool displaying_all = max_chart_range != min_range; - show_all_values_btn->setText(tr("%1 minutes").arg(max_chart_range / 60)); - show_all_values_btn->setToolTip(tr("Click to display %1 data").arg(displaying_all ? tr("%1 minutes").arg(min_range / 60) : tr("ALL cached"))); - show_all_values_btn->setVisible(!is_zoomed); + range_lb->setText(QString(" %1:%2 ").arg(max_chart_range / 60, 2, 10, QLatin1Char('0')).arg(max_chart_range % 60, 2, 10, QLatin1Char('0'))); + title_label->setText(tr("Charts: %1").arg(charts.size())); + dock_btn->setIcon(utils::icon(docking ? "arrow-up-right" : "arrow-down-left")); + dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts")); remove_all_btn->setEnabled(!charts.isEmpty()); reset_zoom_btn->setEnabled(is_zoomed); - range_label->setText(is_zoomed ? tr("%1 - %2").arg(zoomed_range.first, 0, 'f', 2).arg(zoomed_range.second, 0, 'f', 2) : ""); - title_label->setText(charts.size() > 0 ? tr("Charts (%1)").arg(charts.size()) : tr("Charts")); - dock_btn->setText(docking ? "⬈" : "⬋"); - dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts")); +} + +void ChartsWidget::settingChanged() { + range_slider->setRange(1, settings.max_cached_minutes * 60); + for (auto c : charts) { + c->setFixedHeight(settings.chart_height); + c->setSeriesType(settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter); + } } ChartView *ChartsWidget::findChart(const QString &id, const Signal *sig) { @@ -143,27 +189,30 @@ ChartView *ChartsWidget::findChart(const QString &id, const Signal *sig) { return nullptr; } +ChartView *ChartsWidget::createChart() { + auto chart = new ChartView(this); + chart->setFixedHeight(settings.chart_height); + chart->setMinimumWidth(CHART_MIN_WIDTH); + chart->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + chart->chart()->setTheme(use_dark_theme ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); + QObject::connect(chart, &ChartView::remove, [=]() { removeChart(chart); }); + QObject::connect(chart, &ChartView::zoomIn, this, &ChartsWidget::zoomIn); + QObject::connect(chart, &ChartView::zoomReset, this, &ChartsWidget::zoomReset); + QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::seriesChanged); + QObject::connect(chart, &ChartView::seriesAdded, this, &ChartsWidget::seriesChanged); + QObject::connect(chart, &ChartView::axisYUpdated, [this]() { align_charts_timer->start(100); }); + charts.push_back(chart); + updateLayout(); + return chart; +} + void ChartsWidget::showChart(const QString &id, const Signal *sig, bool show, bool merge) { setUpdatesEnabled(false); ChartView *chart = findChart(id, sig); if (show && !chart) { - chart = merge && charts.size() > 0 ? charts.back() : nullptr; - if (!chart) { - chart = new ChartView(this); - chart->chart()->setTheme(use_dark_theme ? QChart::QChart::ChartThemeDark : QChart::ChartThemeLight); - chart->setEventsRange(display_range); - auto range = is_zoomed ? zoomed_range : display_range; - chart->setDisplayRange(range.first, range.second); - QObject::connect(chart, &ChartView::remove, [=]() { removeChart(chart); }); - QObject::connect(chart, &ChartView::zoomIn, this, &ChartsWidget::zoomIn); - QObject::connect(chart, &ChartView::zoomReset, this, &ChartsWidget::zoomReset); - QObject::connect(chart, &ChartView::seriesRemoved, this, &ChartsWidget::seriesChanged); - QObject::connect(chart, &ChartView::seriesAdded, this, &ChartsWidget::seriesChanged); - QObject::connect(chart, &ChartView::axisYUpdated, this, &ChartsWidget::alignCharts); - charts_layout->insertWidget(0, chart); - charts.push_back(chart); - } + chart = merge && charts.size() > 0 ? charts.back() : createChart(); chart->addSeries(id, sig); + updateState(); } else if (!show && chart) { chart->removeSeries(id, sig); } @@ -171,11 +220,52 @@ void ChartsWidget::showChart(const QString &id, const Signal *sig, bool show, bo setUpdatesEnabled(true); } +void ChartsWidget::setColumnCount(int n) { + n = std::clamp(n + 1, 1, columns_cb->count()); + if (column_count != n) { + column_count = settings.chart_column_count = n; + updateLayout(); + } +} + +void ChartsWidget::updateLayout() { + int n = columns_cb->count(); + for (; n > 1; --n) { + if ((n * CHART_MIN_WIDTH + (n - 1) * charts_layout->spacing()) < charts_layout->geometry().width()) break; + } + + bool show_column_cb = n > 1; + columns_lb_action->setVisible(show_column_cb); + columns_cb_action->setVisible(show_column_cb); + + n = std::min(column_count, n); + for (int i = 0; i < charts.size(); ++i) { + charts_layout->addWidget(charts[charts.size() - i - 1], i / n, i % n); + } +} + +void ChartsWidget::resizeEvent(QResizeEvent *event) { + QWidget::resizeEvent(event); + updateLayout(); +} + +void ChartsWidget::newChart() { + SeriesSelector dlg(this); + if (dlg.exec() == QDialog::Accepted) { + QList series_list = dlg.series(); + if (!series_list.isEmpty()) { + auto c = createChart(); + c->addSeries(series_list); + } + } +} + void ChartsWidget::removeChart(ChartView *chart) { charts.removeOne(chart); chart->deleteLater(); updateToolBar(); alignCharts(); + updateLayout(); emit seriesChanged(); } @@ -209,17 +299,21 @@ bool ChartsWidget::eventFilter(QObject *obj, QEvent *event) { // ChartView ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { + series_type = settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter; + QChart *chart = new QChart(); chart->setBackgroundRoundness(0); axis_x = new QValueAxis(this); axis_y = new QValueAxis(this); chart->addAxis(axis_x, Qt::AlignBottom); chart->addAxis(axis_y, Qt::AlignLeft); + chart->legend()->layout()->setContentsMargins(0, 0, 40, 0); chart->legend()->setShowToolTips(true); chart->layout()->setContentsMargins(0, 0, 0, 0); + chart->setMargins({20, 11, 11, 11}); QToolButton *remove_btn = new QToolButton(); - remove_btn->setText("X"); + remove_btn->setIcon(utils::icon("x")); remove_btn->setAutoRaise(true); remove_btn->setToolTip(tr("Remove Chart")); close_btn_proxy = new QGraphicsProxyWidget(chart); @@ -227,28 +321,40 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { close_btn_proxy->setZValue(chart->zValue() + 11); QToolButton *manage_btn = new QToolButton(); - manage_btn->setText("🔧"); + manage_btn->setToolButtonStyle(Qt::ToolButtonIconOnly); + manage_btn->setIcon(utils::icon("gear")); manage_btn->setAutoRaise(true); - manage_btn->setToolTip(tr("Manage series")); + QMenu *menu = new QMenu(this); + line_series_action = menu->addAction(tr("Line"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeLine); }); + line_series_action->setCheckable(true); + line_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeLine); + scatter_series_action = menu->addAction(tr("Scatter"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeScatter); }); + scatter_series_action->setCheckable(true); + scatter_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeScatter); + menu->addSeparator(); + menu->addAction(tr("Manage series"), this, &ChartView::manageSeries); + manage_btn->setMenu(menu); + manage_btn->setPopupMode(QToolButton::InstantPopup); manage_btn_proxy = new QGraphicsProxyWidget(chart); manage_btn_proxy->setWidget(manage_btn); manage_btn_proxy->setZValue(chart->zValue() + 11); setChart(chart); setRenderHint(QPainter::Antialiasing); - setRubberBand(QChartView::HorizontalRubberBand); - updateFromSettings(); + // TODO: enable zoomIn/seekTo in live streaming mode. + setRubberBand(can->liveStreaming() ? QChartView::NoRubberBand : QChartView::HorizontalRubberBand); QObject::connect(dbc(), &DBCManager::signalRemoved, this, &ChartView::signalRemoved); QObject::connect(dbc(), &DBCManager::signalUpdated, this, &ChartView::signalUpdated); QObject::connect(dbc(), &DBCManager::msgRemoved, this, &ChartView::msgRemoved); QObject::connect(dbc(), &DBCManager::msgUpdated, this, &ChartView::msgUpdated); - QObject::connect(&settings, &Settings::changed, this, &ChartView::updateFromSettings); QObject::connect(remove_btn, &QToolButton::clicked, this, &ChartView::remove); - QObject::connect(manage_btn, &QToolButton::clicked, this, &ChartView::manageSeries); } qreal ChartView::getYAsixLabelWidth() const { + if (axis_y->max() <= axis_y->min() || axis_y->tickCount() <= 1) { + return 0; + } QFontMetrics fm(axis_y->labelsFont()); int n = qMax(int(-qFloor(std::log10((axis_y->max() - axis_y->min()) / (axis_y->tickCount() - 1)))), 0) + 1; return qMax(fm.width(QString::number(axis_y->min(), 'f', n)), fm.width(QString::number(axis_y->max(), 'f', n))) + 20; @@ -262,8 +368,7 @@ void ChartView::setPlotAreaLeftPosition(int pos) { } void ChartView::addSeries(const QString &msg_id, const Signal *sig) { - QLineSeries *series = new QLineSeries(this); - series->setUseOpenGL(true); + QXYSeries *series = createSeries(series_type); chart()->addSeries(series); series->attachAxis(axis_x); series->attachAxis(axis_y); @@ -271,14 +376,13 @@ void ChartView::addSeries(const QString &msg_id, const Signal *sig) { sigs.push_back({.msg_id = msg_id, .address = address, .source = source, .sig = sig, .series = series}); updateTitle(); updateSeries(sig); - updateAxisY(); emit seriesAdded(msg_id, sig); } void ChartView::removeSeries(const QString &msg_id, const Signal *sig) { auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); if (it != sigs.end()) { - it = removeSeries(it); + it = removeItem(it); } } @@ -286,7 +390,7 @@ bool ChartView::hasSeries(const QString &msg_id, const Signal *sig) const { return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); } -QList::iterator ChartView::removeSeries(const QList::iterator &it) { +QList::iterator ChartView::removeItem(const QList::iterator &it) { chart()->removeSeries(it->series); it->series->deleteLater(); QString msg_id = it->msg_id; @@ -306,13 +410,12 @@ void ChartView::signalUpdated(const Signal *sig) { updateTitle(); // TODO: don't update series if only name changed. updateSeries(sig); - updateAxisY(); } } void ChartView::signalRemoved(const Signal *sig) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->sig == sig) ? removeSeries(it) : ++it; + it = (it->sig == sig) ? removeItem(it) : ++it; } } @@ -323,7 +426,18 @@ void ChartView::msgUpdated(uint32_t address) { void ChartView::msgRemoved(uint32_t address) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->address == address) ? removeSeries(it) : ++it; + it = (it->address == address) ? removeItem(it) : ++it; + } +} + +void ChartView::addSeries(const QList &series_list) { + for (auto &s : series_list) { + if (auto m = dbc()->msg(s[0])) { + auto it = m->sigs.find(s[2]); + if (it != m->sigs.end() && !hasSeries(s[0], &(it->second))) { + addSeries(s[0], &(it->second)); + } + } } } @@ -339,19 +453,12 @@ void ChartView::manageSeries() { if (series_list.isEmpty()) { emit remove(); } else { - for (auto &s : series_list) { - if (auto m = dbc()->msg(s[0])) { - auto it = m->sigs.find(s[2]); - if (it != m->sigs.end() && !hasSeries(s[0], &(it->second))) { - addSeries(s[0], &(it->second)); - } - } - } + addSeries(series_list); for (auto it = sigs.begin(); it != sigs.end(); /**/) { bool exists = std::any_of(series_list.cbegin(), series_list.cend(), [&](auto &s) { return s[0] == it->msg_id && s[2] == it->sig->name.c_str(); }); - it = exists ? ++it : removeSeries(it); + it = exists ? ++it : removeItem(it); } } } @@ -370,41 +477,59 @@ void ChartView::updateTitle() { } } -void ChartView::updateFromSettings() { - setFixedHeight(settings.chart_height); -} - -void ChartView::setEventsRange(const std::pair &range) { - if (range != events_range) { - events_range = range; - updateSeries(); - } -} - -void ChartView::setDisplayRange(double min, double max) { +void ChartView::updatePlot(double cur, double min, double max) { + cur_sec = cur; if (min != axis_x->min() || max != axis_x->max()) { axis_x->setRange(min, max); updateAxisY(); + + // Show points when zoomed in enough + for (auto &s : sigs) { + auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + auto end = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; }); + + int num_points = std::max(end - begin, 1); + int pixels_per_point = width() / num_points; + + if (series_type == QAbstractSeries::SeriesTypeScatter) { + ((QScatterSeries *)s.series)->setMarkerSize(std::clamp(pixels_per_point / 3, 1, 8)); + } else { + s.series->setPointsVisible(pixels_per_point > 20); + + // TODO: On MacOS QChartWidget doesn't work with the OpenGL settings that CameraWidget needs. +#ifndef __APPLE + // OpenGL mode lacks certain features (such as showing points), only use when drawing many points + bool use_opengl = pixels_per_point < 1; + s.series->setUseOpenGL(use_opengl); + + // Qt doesn't properly apply device pixel ratio in OpenGL mode + QApplication *application = static_cast(QApplication::instance()); + float scale = use_opengl ? application->devicePixelRatio() : 1.0; + + QPen pen = s.series->pen(); + pen.setWidth(2.0 * scale); + s.series->setPen(pen); +#endif + } + } } -} -void ChartView::updateSeries(const Signal *sig) { - auto events = can->events(); - if (!events || sigs.isEmpty()) return; + scene()->invalidate({}, QGraphicsScene::ForegroundLayer); +} +void ChartView::updateSeries(const Signal *sig, const std::vector *events, bool clear) { + events = events ? events : can->events(); for (auto &s : sigs) { if (!sig || s.sig == sig) { - s.vals.clear(); - s.vals.reserve((events_range.second - events_range.first) * 1000); // [n]seconds * 1000hz - s.min_y = std::numeric_limits::max(); - s.max_y = std::numeric_limits::lowest(); - + if (clear) { + s.vals.clear(); + s.vals.reserve(settings.max_cached_minutes * 60 * 100); // [n]seconds * 100hz + s.last_value_mono_time = 0; + } double route_start_time = can->routeStartTime(); - Event begin_event(cereal::Event::Which::INIT_DATA, (route_start_time + events_range.first) * 1e9); - auto begin = std::lower_bound(events->begin(), events->end(), &begin_event, Event::lessThan()); - double end_ns = (route_start_time + events_range.second) * 1e9; - - for (auto it = begin; it != events->end() && (*it)->mono_time <= end_ns; ++it) { + Event begin_event(cereal::Event::Which::INIT_DATA, s.last_value_mono_time); + auto begin = std::upper_bound(events->begin(), events->end(), &begin_event, Event::lessThan()); + for (auto it = begin; it != events->end(); ++it) { if ((*it)->which == cereal::Event::Which::CAN) { for (const auto &c : (*it)->event.getCan()) { if (s.source == c.getSrc() && s.address == c.getAddress()) { @@ -412,14 +537,15 @@ void ChartView::updateSeries(const Signal *sig) { double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *s.sig); double ts = ((*it)->mono_time / (double)1e9) - route_start_time; // seconds s.vals.push_back({ts, value}); - - if (value < s.min_y) s.min_y = value; - if (value > s.max_y) s.max_y = value; } } } } + if (events->size()) { + s.last_value_mono_time = events->back()->mono_time; + } s.series->replace(s.vals); + updateAxisY(); } } } @@ -430,30 +556,23 @@ void ChartView::updateAxisY() { double min_y = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); - if (events_range == std::pair{axis_x->min(), axis_x->max()}) { - for (auto &s : sigs) { - if (s.min_y < min_y) min_y = s.min_y; - if (s.max_y > max_y) max_y = s.max_y; - } - } else { - for (auto &s : sigs) { - auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); - for (auto it = begin; it != s.vals.end() && it->x() <= axis_x->max(); ++it) { - if (it->y() < min_y) min_y = it->y(); - if (it->y() > max_y) max_y = it->y(); - } + for (auto &s : sigs) { + auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + for (auto it = begin; it != s.vals.end() && it->x() <= axis_x->max(); ++it) { + if (it->y() < min_y) min_y = it->y(); + if (it->y() > max_y) max_y = it->y(); } } if (min_y == std::numeric_limits::max()) min_y = 0; if (max_y == std::numeric_limits::lowest()) max_y = 0; - if (max_y == min_y) { - axis_y->setRange(min_y - 1, max_y + 1); + if (std::abs(max_y - min_y) < 1e-3) { + applyNiceNumbers(min_y - 1, max_y + 1); } else { double range = max_y - min_y; applyNiceNumbers(min_y - range * 0.05, max_y + range * 0.05); } - QTimer::singleShot(0, this, &ChartView::axisYUpdated); + emit axisYUpdated(); } void ChartView::applyNiceNumbers(qreal min, qreal max) { @@ -465,6 +584,7 @@ void ChartView::applyNiceNumbers(qreal min, qreal max) { tick_count = int(max - min) + 1; axis_y->setRange(min * step, max * step); axis_y->setTickCount(tick_count); + axis_y->setLabelFormat("%.1f"); } // nice numbers can be expressed as form of 1*10^n, 2* 10^n or 5*10^n @@ -486,27 +606,52 @@ qreal ChartView::niceNumber(qreal x, bool ceiling) { } void ChartView::leaveEvent(QEvent *event) { - track_pt = {0, 0}; + track_pts.clear(); scene()->update(); QChartView::leaveEvent(event); } +void ChartView::mousePressEvent(QMouseEvent *event) { + if (event->button() == Qt::LeftButton && !chart()->plotArea().contains(event->pos()) && + !manage_btn_proxy->widget()->underMouse() && !close_btn_proxy->widget()->underMouse()) { + QMimeData *mimeData = new QMimeData; + mimeData->setData(mime_type, QByteArray::number((qulonglong)this)); + QDrag *drag = new QDrag(this); + drag->setMimeData(mimeData); + drag->setPixmap(grab()); + drag->setHotSpot(event->pos()); + Qt::DropAction dropAction = drag->exec(Qt::CopyAction | Qt::MoveAction, Qt::MoveAction); + if (dropAction == Qt::MoveAction) { + return; + } + } else { + QChartView::mousePressEvent(event); + } +} + void ChartView::mouseReleaseEvent(QMouseEvent *event) { auto rubber = findChild(); if (event->button() == Qt::LeftButton && rubber && rubber->isVisible()) { rubber->hide(); QRectF rect = rubber->geometry().normalized(); - double min = std::floor(chart()->mapToValue(rect.topLeft()).x() * 10.0) / 10.0; - double max = std::floor(chart()->mapToValue(rect.bottomRight()).x() * 10.0) / 10.0; + double min = chart()->mapToValue(rect.topLeft()).x(); + double max = chart()->mapToValue(rect.bottomRight()).x(); + + // Prevent zooming/seeking past the end of the route + min = std::clamp(min, can->routeStartTime(), can->routeStartTime() + can->totalSeconds()); + max = std::clamp(max, can->routeStartTime(), can->routeStartTime() + can->totalSeconds()); + + double min_rounded = std::floor(min * 10.0) / 10.0; + double max_rounded = std::floor(max * 10.0) / 10.0; if (rubber->width() <= 0) { // no rubber dragged, seek to mouse position can->seekTo(min); - } else if ((max - min) >= 0.5) { + } else if ((max_rounded - min_rounded) >= 0.5) { // zoom in if selected range is greater than 0.5s - emit zoomIn(min, max); + emit zoomIn(min_rounded, max_rounded); } event->accept(); - } else if (event->button() == Qt::RightButton) { + } else if (!can->liveStreaming() && event->button() == Qt::RightButton) { emit zoomReset(); event->accept(); } else { @@ -518,45 +663,127 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { auto rubber = findChild(); bool is_zooming = rubber && rubber->isVisible(); const auto plot_area = chart()->plotArea(); - track_pt = {0, 0}; + track_pts.clear(); if (!is_zooming && plot_area.contains(ev->pos())) { + track_pts.resize(sigs.size()); QStringList text_list; const double sec = chart()->mapToValue(ev->pos()).x(); - for (auto &s : sigs) { + for (int i = 0; i < sigs.size(); ++i) { 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(sigs[i].vals.rbegin(), sigs[i].vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); + if (it != sigs[i].vals.rend() && it->x() >= axis_x->min()) { value = QString::number(it->y()); - auto value_pos = chart()->mapToPosition(*it); - if (value_pos.x() > track_pt.x()) track_pt = value_pos; + track_pts[i] = chart()->mapToPosition(*it); } - text_list.push_back(QString(" %1 : %2 ").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value)); + text_list.push_back(QString("■ %2: %3").arg(sigs[i].series->color().name(), sigs[i].sig->name.c_str(), value)); } - if (track_pt.x() == 0) track_pt = ev->pos(); - QString text = QString("
 Time: %1  
%2
") - .arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3) - .arg(text_list.join("
")); - QPoint pt((int)track_pt.x() + 20, plot_area.top() - 20); - QToolTip::showText(mapToGlobal(pt), text, this, plot_area.toRect()); + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + auto pt = (max == track_pts.end()) ? ev->pos() : *max; + text_list.push_front(QString::number(chart()->mapToValue(pt).x(), 'f', 3)); + QPointF tooltip_pt(pt.x() + 12, plot_area.top() - 20); + QToolTip::showText(mapToGlobal(tooltip_pt.toPoint()), pt.isNull() ? "" : text_list.join("
"), this, plot_area.toRect()); scene()->update(); } else { QToolTip::hideText(); } + QChartView::mouseMoveEvent(ev); + if (is_zooming) { + QRect rubber_rect = rubber->geometry(); + rubber_rect.setLeft(std::max(rubber_rect.left(), (int)plot_area.left())); + rubber_rect.setRight(std::min(rubber_rect.right(), (int)plot_area.right())); + if (rubber_rect != rubber->geometry()) { + rubber->setGeometry(rubber_rect); + } + } +} + +void ChartView::dragMoveEvent(QDragMoveEvent *event) { + if (event->mimeData()->hasFormat(mime_type)) { + event->setDropAction(event->source() == this ? Qt::MoveAction : Qt::CopyAction); + event->accept(); + } else { + event->ignore(); + } +} + +void ChartView::dropEvent(QDropEvent *event) { + if (event->mimeData()->hasFormat(mime_type)) { + if (event->source() == this) { + event->setDropAction(Qt::MoveAction); + event->accept(); + } else { + ChartView *source_chart = (ChartView *)event->source(); + QList series; + for (auto &s : source_chart->sigs) { + series.push_back({s.msg_id, msgName(s.msg_id), QString::fromStdString(s.sig->name)}); + } + addSeries(series); + emit source_chart->remove(); + event->acceptProposedAction(); + } + } else { + event->ignore(); + } } void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { - qreal x = chart()->mapToPosition(QPointF{can->currentSec(), 0}).x(); + 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}); - if (!track_pt.isNull()) { + + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + if (max != track_pts.end() && !max->isNull()) { painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); - painter->drawLine(QPointF{track_pt.x(), y1}, QPointF{track_pt.x(), y2}); - painter->setBrush(Qt::darkGray); - painter->drawEllipse(track_pt, 5, 5); + painter->drawLine(QPointF{max->x(), y1}, QPointF{max->x(), y2}); + painter->setPen(Qt::NoPen); + for (int i = 0; i < track_pts.size(); ++i) { + if (!track_pts[i].isNull() && i < sigs.size()) { + painter->setBrush(sigs[i].series->color().darker(125)); + painter->drawEllipse(track_pts[i], 5.5, 5.5); + } + } + } +} + +QXYSeries *ChartView::createSeries(QAbstractSeries::SeriesType type) { + QXYSeries *series = nullptr; + if (type == QAbstractSeries::SeriesTypeLine) { + series = new QLineSeries(this); + } else { + series = new QScatterSeries(this); + } + // TODO: Due to a bug in CameraWidget the camera frames + // are drawn instead of the graphs on MacOS. Re-enable OpenGL when fixed +#ifndef __APPLE__ + series->setUseOpenGL(true); +#endif + return series; +} + +void ChartView::setSeriesType(QAbstractSeries::SeriesType type) { + if (type != series_type) { + series_type = type; + line_series_action->setChecked(type == QAbstractSeries::SeriesTypeLine); + scatter_series_action->setChecked(type == QAbstractSeries::SeriesTypeScatter); + + for (auto &s : sigs) { + chart()->removeSeries(s.series); + s.series->deleteLater(); + } + for (auto &s : sigs) { + auto series = createSeries(series_type); + chart()->addSeries(series); + series->attachAxis(axis_x); + series->attachAxis(axis_y); + series->replace(s.vals); + s.series = series; + } + updateTitle(); } } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 7a8325382dd84b..9b2afd45a99808 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -1,17 +1,20 @@ #pragma once #include -#include +#include +#include #include #include #include -#include +#include +#include #include #include +#include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" using namespace QtCharts; @@ -21,23 +24,23 @@ class ChartView : public QChartView { public: ChartView(QWidget *parent = nullptr); void addSeries(const QString &msg_id, const Signal *sig); + void addSeries(const QList &series_list); void removeSeries(const QString &msg_id, const Signal *sig); bool hasSeries(const QString &msg_id, const Signal *sig) const; - void updateSeries(const Signal *sig = nullptr); - void setEventsRange(const std::pair &range); - void setDisplayRange(double min, double max); + void updateSeries(const Signal *sig = nullptr, const std::vector *events = nullptr, bool clear = true); + void updatePlot(double cur, double min, double max); void setPlotAreaLeftPosition(int pos); qreal getYAsixLabelWidth() const; + void setSeriesType(QAbstractSeries::SeriesType type); struct SigItem { QString msg_id; uint8_t source = 0; uint32_t address = 0; const Signal *sig = nullptr; - QLineSeries *series = nullptr; - double min_y = 0; - double max_y = 0; + QXYSeries *series = nullptr; QVector vals; + uint64_t last_value_mono_time = 0; }; signals: @@ -56,25 +59,32 @@ private slots: void manageSeries(); private: - QList::iterator removeSeries(const QList::iterator &it); + QList::iterator removeItem(const QList::iterator &it); + void mousePressEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void mouseMoveEvent(QMouseEvent *ev) override; + void dragMoveEvent(QDragMoveEvent *event) override; + void dropEvent(QDropEvent *event) override; void leaveEvent(QEvent *event) override; void resizeEvent(QResizeEvent *event) override; void updateAxisY(); void updateTitle(); - void updateFromSettings(); void drawForeground(QPainter *painter, const QRectF &rect) override; void applyNiceNumbers(qreal min, qreal max); qreal niceNumber(qreal x, bool ceiling); + QXYSeries *createSeries(QAbstractSeries::SeriesType type); QValueAxis *axis_x; QValueAxis *axis_y; - QPointF track_pt; + QVector track_pts; QGraphicsProxyWidget *close_btn_proxy; QGraphicsProxyWidget *manage_btn_proxy; - std::pair events_range = {0, 0}; QList sigs; + double cur_sec = 0; + const QString mime_type = "application/x-cabanachartview"; + QAbstractSeries::SeriesType series_type = QAbstractSeries::SeriesTypeLine; + QAction *line_series_action; + QAction *scatter_series_action; }; class ChartsWidget : public QWidget { @@ -85,40 +95,52 @@ class ChartsWidget : public QWidget { void showChart(const QString &id, const Signal *sig, bool show, bool merge); inline bool hasSignal(const QString &id, const Signal *sig) { return findChart(id, sig) != nullptr; } +public slots: + void setColumnCount(int n); + signals: void dock(bool floating); void rangeChanged(double min, double max, bool is_zommed); void seriesChanged(); private: + void resizeEvent(QResizeEvent *event) override; void alignCharts(); + void newChart(); + ChartView * createChart(); void removeChart(ChartView *chart); void eventsMerged(); void updateState(); - void updateDisplayRange(); void zoomIn(double min, double max); void zoomReset(); void updateToolBar(); void removeAll(); - void showAllData(); + void setMaxChartRange(int value); + void updateLayout(); + void settingChanged(); bool eventFilter(QObject *obj, QEvent *event) override; ChartView *findChart(const QString &id, const Signal *sig); QLabel *title_label; - QLabel *range_label; + QLabel *range_lb; + QSlider *range_slider; bool docking = true; - QAction *show_all_values_btn; QAction *dock_btn; QAction *reset_zoom_btn; QAction *remove_all_btn; - QVBoxLayout *charts_layout; + QTimer *align_charts_timer; + QGridLayout *charts_layout; QList charts; uint32_t max_chart_range = 0; bool is_zoomed = false; - std::pair event_range; std::pair display_range; std::pair zoomed_range; bool use_dark_theme = false; + QAction *columns_lb_action; + QAction *columns_cb_action; + QComboBox *columns_cb; + int column_count = 1; + const int CHART_MIN_WIDTH = 300; }; class SeriesSelector : public QDialog { diff --git a/tools/cabana/commands.cc b/tools/cabana/commands.cc index b3f5cb1c66c509..e4bf999062c2f9 100644 --- a/tools/cabana/commands.cc +++ b/tools/cabana/commands.cc @@ -1,3 +1,5 @@ +#include + #include "tools/cabana/commands.h" // EditMsgCommand @@ -73,3 +75,12 @@ EditSignalCommand::EditSignalCommand(const QString &id, const Signal *sig, const void EditSignalCommand::undo() { dbc()->updateSignal(id, new_signal.name.c_str(), old_signal); } void EditSignalCommand::redo() { dbc()->updateSignal(id, old_signal.name.c_str(), new_signal); } + +namespace UndoStack { + +QUndoStack *instance() { + static QUndoStack *undo_stack = new QUndoStack(qApp); + return undo_stack; +} + +} // namespace UndoStack diff --git a/tools/cabana/commands.h b/tools/cabana/commands.h index 7ea1f666533bea..c07a00b7603198 100644 --- a/tools/cabana/commands.h +++ b/tools/cabana/commands.h @@ -1,8 +1,8 @@ #pragma once #include +#include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" class EditMsgCommand : public QUndoCommand { @@ -61,3 +61,8 @@ class EditSignalCommand : public QUndoCommand { Signal old_signal = {}; Signal new_signal = {}; }; + +namespace UndoStack { + QUndoStack *instance(); + inline void push(QUndoCommand *cmd) { instance()->push(cmd); } +}; diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index 01bdff17a1c3c1..83f5fdb74a6156 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -68,7 +68,7 @@ void DBCManager::addSignal(const QString &id, const Signal &sig) { if (auto m = const_cast(msg(id))) { auto &s = m->sigs[sig.name.c_str()]; s = sig; - emit signalAdded(&s); + emit signalAdded(parseId(id).second, &s); } } @@ -110,6 +110,7 @@ DBCManager *dbc() { std::vector DBCMsg::getSignals() const { std::vector ret; + ret.reserve(sigs.size()); for (auto &[_, sig] : sigs) ret.push_back(&sig); std::sort(ret.begin(), ret.end(), [](auto l, auto r) { return l->start_bit < r->start_bit; }); return ret; diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index c7675121bbd9ba..03bd16f2a5902e 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -8,7 +8,9 @@ struct DBCMsg { QString name; uint32_t size; + // signal must be saved as value in map to make undo stack work properly. std::map sigs; + // return vector, sort by start_bits std::vector getSignals() const; }; @@ -28,9 +30,7 @@ class DBCManager : public QObject { static std::pair parseId(const QString &id); inline static std::vector allDBCNames() { return get_dbc_names(); } - inline std::map &allMsgs() { return msgs; } inline QString name() const { return dbc ? dbc->name.c_str() : ""; } - void updateMsg(const QString &id, const QString &name, uint32_t size); void removeMsg(const QString &id); inline const std::map &messages() const { return msgs; } @@ -41,7 +41,7 @@ class DBCManager : public QObject { } signals: - void signalAdded(const Signal *sig); + void signalAdded(uint32_t address, const Signal *sig); void signalRemoved(const Signal *sig); void signalUpdated(const Signal *sig); void msgUpdated(uint32_t address); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 06377616da2a4b..3af0fa9fc37f6f 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -4,23 +4,18 @@ #include #include #include -#include -#include +#include -#include "selfdrive/ui/qt/util.h" -#include "tools/cabana/canmessages.h" #include "tools/cabana/commands.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" // DetailWidget DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { - undo_stack = new QUndoStack(this); - setMinimumWidth(500); QWidget *main_widget = new QWidget(this); QVBoxLayout *main_layout = new QVBoxLayout(main_widget); main_layout->setContentsMargins(0, 0, 0, 0); - main_layout->setSpacing(0); // tabbar tabbar = new QTabBar(this); @@ -30,12 +25,9 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart tabbar->setContextMenuPolicy(Qt::CustomContextMenu); main_layout->addWidget(tabbar); - QFrame *title_frame = new QFrame(this); - QVBoxLayout *frame_layout = new QVBoxLayout(title_frame); - title_frame->setFrameShape(QFrame::StyledPanel); - // message title - toolbar = new QToolBar(this); + QToolBar *toolbar = new QToolBar(this); + toolbar->setIconSize({16, 16}); toolbar->addWidget(new QLabel("time:")); time_label = new QLabel(this); time_label->setStyleSheet("font-weight:bold"); @@ -45,61 +37,49 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart name_label->setAlignment(Qt::AlignCenter); name_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); toolbar->addWidget(name_label); - toolbar->addAction("🖍", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); - remove_msg_act = toolbar->addAction("X", this, &DetailWidget::removeMsg); + toolbar->addAction(utils::icon("pencil"), "", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); + remove_msg_act = toolbar->addAction(utils::icon("x-lg"), "", this, &DetailWidget::removeMsg); remove_msg_act->setToolTip(tr("Remove Message")); - toolbar->setVisible(false); - frame_layout->addWidget(toolbar); + main_layout->addWidget(toolbar); // warning warning_widget = new QWidget(this); QHBoxLayout *warning_hlayout = new QHBoxLayout(warning_widget); - warning_hlayout->setContentsMargins(0, 0, 0, 0); - QLabel *warning_icon = new QLabel(this); - warning_icon->setPixmap(style()->standardPixmap(QStyle::SP_MessageBoxWarning).scaledToWidth(24, Qt::SmoothTransformation)); - warning_hlayout->addWidget(warning_icon, 0, Qt::AlignTop); - warning_label = new QLabel(this); - warning_hlayout->addWidget(warning_label, 1, Qt::AlignLeft); + warning_hlayout->addWidget(warning_icon = new QLabel(this), 0, Qt::AlignTop); + warning_hlayout->addWidget(warning_label = new QLabel(this), 1, Qt::AlignLeft); warning_widget->hide(); - frame_layout->addWidget(warning_widget); - main_layout->addWidget(title_frame); + main_layout->addWidget(warning_widget); // msg widget - QWidget *msg_widget = new QWidget(this); - QVBoxLayout *msg_layout = new QVBoxLayout(msg_widget); - msg_layout->setContentsMargins(0, 0, 0, 0); - // binary view - binary_view = new BinaryView(this); - msg_layout->addWidget(binary_view); - // signals - signals_layout = new QVBoxLayout(); - signals_layout->setSpacing(0); - msg_layout->addLayout(signals_layout); - msg_layout->addStretch(0); - - scroll = new QScrollArea(this); - scroll->setFrameShape(QFrame::NoFrame); - scroll->setWidget(msg_widget); - scroll->setWidgetResizable(true); - scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + splitter = new QSplitter(Qt::Vertical, this); + splitter->setAutoFillBackground(true); + splitter->addWidget(binary_view = new BinaryView(this)); + splitter->addWidget(signal_view = new SignalView(charts, this)); + binary_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); + signal_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + splitter->setStretchFactor(0, 0); + splitter->setStretchFactor(1, 1); tab_widget = new QTabWidget(this); tab_widget->setTabPosition(QTabWidget::South); - tab_widget->addTab(scroll, "&Msg"); - history_log = new LogsWidget(this); - tab_widget->addTab(history_log, "&Logs"); + tab_widget->addTab(splitter, utils::icon("file-earmark-ruled"), "&Msg"); + tab_widget->addTab(history_log = new LogsWidget(this), utils::icon("stopwatch"), "&Logs"); main_layout->addWidget(tab_widget); stacked_layout = new QStackedLayout(this); stacked_layout->addWidget(new WelcomeWidget(this)); stacked_layout->addWidget(main_widget); - QObject::connect(binary_view, &BinaryView::signalClicked, this, &DetailWidget::showForm); - QObject::connect(binary_view, &BinaryView::resizeSignal, this, &DetailWidget::resizeSignal); - QObject::connect(binary_view, &BinaryView::addSignal, this, &DetailWidget::addSignal); + QObject::connect(binary_view, &BinaryView::resizeSignal, signal_view->model, &SignalModel::resizeSignal); + QObject::connect(binary_view, &BinaryView::addSignal, signal_view->model, &SignalModel::addSignal); + QObject::connect(binary_view, &BinaryView::signalHovered, signal_view, &SignalView::signalHovered); + QObject::connect(binary_view, &BinaryView::signalClicked, signal_view, &SignalView::expandSignal); + QObject::connect(signal_view, &SignalView::showChart, charts, &ChartsWidget::showChart); + QObject::connect(signal_view, &SignalView::highlight, binary_view, &BinaryView::highlight); QObject::connect(tab_widget, &QTabWidget::currentChanged, [this]() { updateState(); }); - QObject::connect(can, &CANMessages::msgsReceived, this, &DetailWidget::updateState); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, [this]() { dbcMsgChanged(); }); + QObject::connect(can, &AbstractStream::msgsReceived, this, &DetailWidget::updateState); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &DetailWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &DetailWidget::refresh); QObject::connect(tabbar, &QTabBar::customContextMenuRequested, this, &DetailWidget::showTabBarContextMenu); QObject::connect(tabbar, &QTabBar::currentChanged, [this](int index) { if (index != -1 && tabbar->tabText(index) != msg_id) { @@ -107,14 +87,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart } }); QObject::connect(tabbar, &QTabBar::tabCloseRequested, tabbar, &QTabBar::removeTab); - QObject::connect(charts, &ChartsWidget::seriesChanged, this, &DetailWidget::updateChartState); - QObject::connect(history_log, &LogsWidget::openChart, [this](const QString &id, const Signal *sig) { - this->charts->showChart(id, sig, true, false); - }); - QObject::connect(undo_stack, &QUndoStack::indexChanged, [this]() { - if (undo_stack->count() > 0) - dbcMsgChanged(); - }); + QObject::connect(charts, &ChartsWidget::seriesChanged, signal_view, &SignalView::updateChartState); } void DetailWidget::showTabBarContextMenu(const QPoint &pt) { @@ -139,60 +112,47 @@ void DetailWidget::setMessage(const QString &message_id) { index = tabbar->addTab(message_id); tabbar->setTabToolTip(index, msgName(message_id)); } - tabbar->setCurrentIndex(index); - dbcMsgChanged(); - scroll->verticalScrollBar()->setValue(0); - stacked_layout->setCurrentIndex(1); -} - -void DetailWidget::dbcMsgChanged(int show_form_idx) { - if (msg_id.isEmpty()) return; setUpdatesEnabled(false); + signal_view->setMessage(msg_id); binary_view->setMessage(msg_id); history_log->setMessage(msg_id); - int i = 0; + stacked_layout->setCurrentIndex(1); + tabbar->setCurrentIndex(index); + refresh(); + splitter->setSizes({1, 2}); + + setUpdatesEnabled(true); +} + +void DetailWidget::refresh() { + if (msg_id.isEmpty()) return; + QStringList warnings; const DBCMsg *msg = dbc()->msg(msg_id); if (msg) { - for (auto sig : msg->getSignals()) { - SignalEdit *form = i < signal_list.size() ? signal_list[i] : nullptr; - if (!form) { - form = new SignalEdit(i, this); - QObject::connect(form, &SignalEdit::remove, this, &DetailWidget::removeSignal); - QObject::connect(form, &SignalEdit::save, this, &DetailWidget::saveSignal); - QObject::connect(form, &SignalEdit::showFormClicked, this, &DetailWidget::showForm); - QObject::connect(form, &SignalEdit::highlight, binary_view, &BinaryView::highlight); - QObject::connect(binary_view, &BinaryView::signalHovered, form, &SignalEdit::signalHovered); - QObject::connect(form, &SignalEdit::showChart, charts, &ChartsWidget::showChart); - signals_layout->addWidget(form); - signal_list.push_back(form); - } - form->setSignal(msg_id, sig); - form->setChartOpened(charts->hasSignal(msg_id, sig)); - ++i; - } - if (msg->size != can->lastMessage(msg_id).dat.size()) + 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()) { + warnings.push_back(tr("%1 has overlapping bits.").arg(s->name.c_str())); + } + } else { + warnings.push_back(tr("Drag-Select in binary view to create new signal.")); } - for (/**/; i < signal_list.size(); ++i) - signal_list[i]->hide(); - - toolbar->setVisible(!msg_id.isEmpty()); remove_msg_act->setEnabled(msg != nullptr); name_label->setText(msgName(msg_id)); - for (auto s : binary_view->getOverlappingSignals()) - warnings.push_back(tr("%1 has overlapping bits.").arg(s->name.c_str())); - - warning_label->setText(warnings.join('\n')); + if (!warnings.isEmpty()) { + warning_label->setText(warnings.join('\n')); + warning_icon->setPixmap(utils::icon(msg ? "exclamation-triangle" : "info-circle")); + } warning_widget->setVisible(!warnings.isEmpty()); - setUpdatesEnabled(true); } -void DetailWidget::updateState(const QHash * msgs) { +void DetailWidget::updateState(const QHash *msgs) { time_label->setText(QString::number(can->currentSec(), 'f', 3)); if (msg_id.isEmpty() || (msgs && !msgs->contains(msg_id))) return; @@ -203,86 +163,18 @@ void DetailWidget::updateState(const QHash * msgs) { history_log->updateState(); } -void DetailWidget::showForm(const Signal *sig) { - setUpdatesEnabled(false); - for (auto f : signal_list) { - f->updateForm(f->sig == sig && !f->form->isVisible()); - if (f->sig == sig && f->form->isVisible()) { - QTimer::singleShot(0, [=]() { scroll->ensureWidgetVisible(f); }); - } - } - setUpdatesEnabled(true); -} - -void DetailWidget::updateChartState() { - for (auto f : signal_list) - f->setChartOpened(charts->hasSignal(f->msg_id, f->sig)); -} - void DetailWidget::editMsg() { QString id = msg_id; auto msg = dbc()->msg(id); int size = msg ? msg->size : can->lastMessage(id).dat.size(); EditMessageDialog dlg(id, msgName(id), size, this); if (dlg.exec()) { - undo_stack->push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); + UndoStack::push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); } } void DetailWidget::removeMsg() { - undo_stack->push(new RemoveMsgCommand(msg_id)); -} - -void DetailWidget::addSignal(int start_bit, int size, bool little_endian) { - auto msg = dbc()->msg(msg_id); - if (!msg) { - for (int i = 1; /**/; ++i) { - QString name = QString("NEW_MSG_%1").arg(i); - auto it = std::find_if(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; }); - if (it == dbc()->messages().end()) { - undo_stack->push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); - msg = dbc()->msg(msg_id); - break; - } - } - } - Signal sig = {.is_little_endian = little_endian, .factor = 1}; - for (int i = 1; /**/; ++i) { - sig.name = "NEW_SIGNAL_" + std::to_string(i); - if (msg->sigs.count(sig.name.c_str()) == 0) break; - } - updateSigSizeParamsFromRange(sig, start_bit, size); - undo_stack->push(new AddSigCommand(msg_id, sig)); -} - -void DetailWidget::resizeSignal(const Signal *sig, int start_bit, int size) { - Signal s = *sig; - updateSigSizeParamsFromRange(s, start_bit, size); - saveSignal(sig, s); -} - -void DetailWidget::saveSignal(const Signal *sig, const Signal &new_sig) { - auto msg = dbc()->msg(msg_id); - if (new_sig.name != sig->name) { - auto it = msg->sigs.find(new_sig.name.c_str()); - if (it != msg->sigs.end()) { - QString warning_str = tr("There is already a signal with the same name '%1'").arg(new_sig.name.c_str()); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - } - auto [start, end] = getSignalRange(&new_sig); - if (start < 0 || end >= msg->size * 8) { - QString warning_str = tr("Signal size [%1] exceed limit").arg(new_sig.size); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - - undo_stack->push(new EditSignalCommand(msg_id, sig, new_sig)); -} - -void DetailWidget::removeSignal(const Signal *sig) { - undo_stack->push(new RemoveSigCommand(msg_id, sig)); + UndoStack::push(new RemoveMsgCommand(msg_id)); } // EditMessageDialog @@ -293,7 +185,7 @@ EditMessageDialog::EditMessageDialog(const QString &msg_id, const QString &title form_layout->addRow("ID", new QLabel(msg_id)); name_edit = new QLineEdit(title, this); - name_edit->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name_edit)); + name_edit->setValidator(new NameValidator(name_edit)); form_layout->addRow(tr("Name"), name_edit); size_spin = new QSpinBox(this); diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 18c58ab6bfe5c2..3a3f3adf0e2261 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -1,10 +1,9 @@ #pragma once +#include #include -#include #include #include -#include #include "tools/cabana/binaryview.h" #include "tools/cabana/chartswidget.h" @@ -30,33 +29,25 @@ class DetailWidget : public QWidget { public: DetailWidget(ChartsWidget *charts, QWidget *parent); void setMessage(const QString &message_id); - void dbcMsgChanged(int show_form_idx = -1); - QUndoStack *undo_stack = nullptr; + void refresh(); + QSize minimumSizeHint() const override { return binary_view->minimumSizeHint(); } private: - void showForm(const Signal *sig); - void updateChartState(); void showTabBarContextMenu(const QPoint &pt); - void addSignal(int start_bit, int size, bool little_endian); - void resizeSignal(const Signal *sig, int from, int to); - void saveSignal(const Signal *sig, const Signal &new_sig); - void removeSignal(const Signal *sig); void editMsg(); void removeMsg(); void updateState(const QHash * msgs = nullptr); QString msg_id; - QLabel *name_label, *time_label, *warning_label; + QLabel *name_label, *time_label, *warning_icon, *warning_label; QWidget *warning_widget; - QVBoxLayout *signals_layout; QTabBar *tabbar; QTabWidget *tab_widget; - QToolBar *toolbar; QAction *remove_msg_act; LogsWidget *history_log; BinaryView *binary_view; - QScrollArea *scroll; + SignalView *signal_view; ChartsWidget *charts; + QSplitter *splitter; QStackedLayout *stacked_layout; - QList signal_list; }; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 485a21cc1bc50f..95bd94e76be92b 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -1,56 +1,55 @@ #include "tools/cabana/historylog.h" -#include #include #include #include +#include "tools/cabana/commands.h" + // HistoryLogModel QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { + const bool show_signals = display_signals_mode && sigs.size() > 0; + const auto &m = messages[index.row()]; if (role == Qt::DisplayRole) { - const auto &m = messages[index.row()]; if (index.column() == 0) { return QString::number((m.mono_time / (double)1e9) - can->routeStartTime(), 'f', 2); } - return !sigs.empty() ? QString::number(m.sig_values[index.column() - 1]) : m.data; - } else if (role == Qt::FontRole && index.column() == 1 && sigs.empty()) { - return QFontDatabase::systemFont(QFontDatabase::FixedFont); - } else if (role == Qt::ToolTipRole && index.column() > 0 && !sigs.empty()) { - return tr("double click to open the chart"); + return show_signals ? QString::number(m.sig_values[index.column() - 1]) : toHex(m.data); + } else if (role == Qt::UserRole && index.column() == 1 && !show_signals) { + return ChangeTracker::toVariantList(m.colors); } return {}; } void HistoryLogModel::setMessage(const QString &message_id) { msg_id = message_id; - sigs.clear(); - if (auto dbc_msg = dbc()->msg(msg_id)) { - sigs = dbc_msg->getSignals(); - } - filter_cmp = nullptr; - refresh(); } void HistoryLogModel::refresh() { beginResetModel(); + sigs.clear(); + if (auto dbc_msg = dbc()->msg(msg_id)) { + sigs = dbc_msg->getSignals(); + } last_fetch_time = 0; + has_more_data = true; messages.clear(); + hex_colors.clear(); updateState(); endResetModel(); } QVariant HistoryLogModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal) { + const bool show_signals = display_signals_mode && !sigs.empty(); if (role == Qt::DisplayRole || role == Qt::ToolTipRole) { if (section == 0) { return "Time"; } - return !sigs.empty() ? QString::fromStdString(sigs[section - 1]->name).replace('_', ' ') : "Data"; - } else if (role == Qt::BackgroundRole && section > 0 && !sigs.empty()) { + return show_signals ? QString::fromStdString(sigs[section - 1]->name).replace('_', ' ') : "Data"; + } else if (role == Qt::BackgroundRole && section > 0 && show_signals) { return QBrush(QColor(getColor(section - 1))); - } else if (role == Qt::ForegroundRole && section > 0 && !sigs.empty()) { - return QBrush(Qt::black); } } return {}; @@ -61,6 +60,11 @@ void HistoryLogModel::setDynamicMode(int state) { refresh(); } +void HistoryLogModel::setDisplayType(int type) { + display_signals_mode = type == 0; + refresh(); +} + void HistoryLogModel::segmentsMerged() { if (!dynamic_mode) { has_more_data = true; @@ -71,12 +75,11 @@ void HistoryLogModel::setFilter(int sig_idx, const QString &value, std::function filter_sig_idx = sig_idx; filter_value = value.toDouble(); filter_cmp = value.isEmpty() ? nullptr : cmp; - refresh(); } void HistoryLogModel::updateState() { if (!msg_id.isEmpty()) { - uint64_t current_time = (can->currentSec() + can->routeStartTime()) * 1e9; + uint64_t current_time = (can->lastMessage(msg_id).ts + can->routeStartTime()) * 1e9 + 1; auto new_msgs = dynamic_mode ? fetchData(current_time, last_fetch_time) : fetchData(0); if ((has_more_data = !new_msgs.empty())) { beginInsertRows({}, 0, new_msgs.size() - 1); @@ -106,7 +109,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I for (auto it = first; it != last && (*it)->mono_time > min_time; ++it) { if ((*it)->which == cereal::Event::Which::CAN) { for (const auto &c : (*it)->event.getCan()) { - if (src == c.getSrc() && address == c.getAddress()) { + if (address == c.getAddress() && src == c.getSrc()) { const auto dat = c.getDat(); for (int i = 0; i < sigs.size(); ++i) { values[i] = get_raw_value((uint8_t *)dat.begin(), dat.size(), *(sigs[i])); @@ -114,7 +117,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I if (!filter_cmp || filter_cmp(values[filter_sig_idx], filter_value)) { auto &m = msgs.emplace_back(); m.mono_time = (*it)->mono_time; - m.data = toHex(QByteArray((char *)dat.begin(), dat.size())); + m.data = QByteArray((char *)dat.begin(), dat.size()); m.sig_values = values; if (msgs.size() >= batch_size && min_time == 0) return msgs; @@ -125,143 +128,149 @@ std::deque HistoryLogModel::fetchData(InputIt first, I } return msgs; } + template std::deque HistoryLogModel::fetchData<>(std::vector::iterator first, std::vector::iterator last, uint64_t min_time); template std::deque HistoryLogModel::fetchData<>(std::vector::reverse_iterator first, std::vector::reverse_iterator last, uint64_t min_time); std::deque HistoryLogModel::fetchData(uint64_t from_time, uint64_t min_time) { auto events = can->events(); + const auto freq = can->lastMessage(msg_id).freq; + const bool update_colors = !display_signals_mode || sigs.empty(); + if (dynamic_mode) { - auto it = std::lower_bound(events->rbegin(), events->rend(), from_time, [=](auto &e, uint64_t ts) { - return e->mono_time > ts; - }); - if (it != events->rend()) ++it; - return fetchData(it, events->rend(), min_time); + auto first = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { return e->mono_time < ts; }); + auto msgs = fetchData(first, events->rend(), min_time); + if (update_colors && (min_time > 0 || messages.empty())) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } else { assert(min_time == 0); - auto it = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { - return ts < e->mono_time; - }); - return fetchData(it, events->end(), 0); + auto first = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { return ts < e->mono_time; }); + auto msgs = fetchData(first, events->end(), 0); + if (update_colors) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } } // HeaderView QSize HeaderView::sectionSizeFromContents(int logicalIndex) const { - int default_size = qMax(100, rect().width() / model()->columnCount()); - const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString(); - const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text); - QSize size = rect.size() + QSize{10, 6}; - return {qMax(size.width(), default_size), size.height()}; + static QSize time_col_size = fontMetrics().boundingRect({0, 0, 200, 200}, defaultAlignment(), "000000.000").size() + QSize(10, 6); + if (logicalIndex == 0) { + return time_col_size; + } else { + int default_size = qMax(100, (rect().width() - time_col_size.width()) / (model()->columnCount() - 1)); + const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString(); + const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text); + QSize size = rect.size() + QSize{10, 6}; + return QSize{qMax(size.width(), default_size), size.height()}; + } } void HeaderView::paintSection(QPainter *painter, const QRect &rect, int logicalIndex) const { auto bg_role = model()->headerData(logicalIndex, Qt::Horizontal, Qt::BackgroundRole); if (bg_role.isValid()) { - QPen pen(model()->headerData(logicalIndex, Qt::Horizontal, Qt::ForegroundRole).value(), 1); - painter->setPen(pen); painter->fillRect(rect, bg_role.value()); } QString text = model()->headerData(logicalIndex, Qt::Horizontal, Qt::DisplayRole).toString(); painter->drawText(rect.adjusted(5, 3, -5, -3), defaultAlignment(), text); } -// HistoryLog - -HistoryLog::HistoryLog(QWidget *parent) : QTableView(parent) { - setHorizontalHeader(new HeaderView(Qt::Horizontal, this)); - horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap); - horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); - verticalHeader()->setVisible(false); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); -} - // LogsWidget LogsWidget::LogsWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + + QWidget *toolbar = new QWidget(this); + toolbar->setAutoFillBackground(true); + QHBoxLayout *h = new QHBoxLayout(toolbar); + + filters_widget = new QWidget(this); + QHBoxLayout *filter_layout = new QHBoxLayout(filters_widget); + filter_layout->setContentsMargins(0, 0, 0, 0); + filter_layout->addWidget(display_type_cb = new QComboBox(this)); + filter_layout->addWidget(signals_cb = new QComboBox(this)); + filter_layout->addWidget(comp_box = new QComboBox(this)); + filter_layout->addWidget(value_edit = new QLineEdit(this)); + h->addWidget(filters_widget); + h->addStretch(0); + h->addWidget(dynamic_mode = new QCheckBox(tr("Dynamic")), 0, Qt::AlignRight); - QHBoxLayout *h = new QHBoxLayout(); - signals_cb = new QComboBox(this); - signals_cb->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); - h->addWidget(signals_cb); - comp_box = new QComboBox(); + display_type_cb->addItems({"Signal Value", "Hex Value"}); comp_box->addItems({">", "=", "!=", "<"}); - h->addWidget(comp_box); - value_edit = new QLineEdit(this); value_edit->setClearButtonEnabled(true); value_edit->setValidator(new QDoubleValidator(-500000, 500000, 6, this)); - h->addWidget(value_edit); - dynamic_mode = new QCheckBox(tr("Dynamic")); - h->addWidget(dynamic_mode, 0, Qt::AlignRight); - main_layout->addLayout(h); + dynamic_mode->setChecked(true); + dynamic_mode->setEnabled(!can->liveStreaming()); - model = new HistoryLogModel(this); - logs = new HistoryLog(this); - logs->setModel(model); - main_layout->addWidget(logs); + main_layout->addWidget(toolbar); + QFrame *line = new QFrame(this); + line->setFrameStyle(QFrame::HLine | QFrame::Sunken); + main_layout->addWidget(line);; - QObject::connect(logs, &QTableView::doubleClicked, this, &LogsWidget::doubleClicked); + main_layout->addWidget(logs = new QTableView(this)); + logs->setModel(model = new HistoryLogModel(this)); + logs->setItemDelegateForColumn(1, new MessageBytesDelegate(this)); + logs->setHorizontalHeader(new HeaderView(Qt::Horizontal, this)); + logs->horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap); + logs->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); + logs->verticalHeader()->setVisible(false); + logs->setFrameShape(QFrame::NoFrame); + + QObject::connect(display_type_cb, SIGNAL(activated(int)), model, SLOT(setDisplayType(int))); + QObject::connect(dynamic_mode, &QCheckBox::stateChanged, model, &HistoryLogModel::setDynamicMode); QObject::connect(signals_cb, SIGNAL(activated(int)), this, SLOT(setFilter())); QObject::connect(comp_box, SIGNAL(activated(int)), this, SLOT(setFilter())); QObject::connect(value_edit, &QLineEdit::textChanged, this, &LogsWidget::setFilter); - QObject::connect(dynamic_mode, &QCheckBox::stateChanged, model, &HistoryLogModel::setDynamicMode); - QObject::connect(can, &CANMessages::seekedTo, model, &HistoryLogModel::refresh); - QObject::connect(can, &CANMessages::eventsMerged, model, &HistoryLogModel::segmentsMerged); + QObject::connect(can, &AbstractStream::seekedTo, model, &HistoryLogModel::refresh); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &LogsWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &LogsWidget::refresh); + QObject::connect(can, &AbstractStream::eventsMerged, model, &HistoryLogModel::segmentsMerged); } void LogsWidget::setMessage(const QString &message_id) { model->setMessage(message_id); - cur_filter_text = ""; - value_edit->setText(""); - signals_cb->clear(); - comp_box->setCurrentIndex(0); - bool has_signals = model->sigs.size() > 0; - if (has_signals) { + refresh(); +} + +void LogsWidget::refresh() { + if (model->msg_id.isEmpty()) return; + + model->setFilter(0, "", nullptr); + model->refresh(); + bool has_signal = model->sigs.size(); + if (has_signal) { + signals_cb->clear(); for (auto s : model->sigs) { signals_cb->addItem(s->name.c_str()); } } - comp_box->setVisible(has_signals); - value_edit->setVisible(has_signals); - signals_cb->setVisible(has_signals); + value_edit->clear(); + comp_box->setCurrentIndex(0); + filters_widget->setVisible(has_signal); } -static bool not_equal(double l, double r) { return l != r; } - void LogsWidget::setFilter() { - if (cur_filter_text.isEmpty() && value_edit->text().isEmpty()) { - return; - } + if (value_edit->text().isEmpty() && !value_edit->isModified()) return; - std::function cmp; + std::function cmp = nullptr; switch (comp_box->currentIndex()) { case 0: cmp = std::greater{}; break; case 1: cmp = std::equal_to{}; break; - case 2: cmp = not_equal; break; + case 2: cmp = [](double l, double r) { return l != r; }; break; // not equal case 3: cmp = std::less{}; break; } model->setFilter(signals_cb->currentIndex(), value_edit->text(), cmp); - cur_filter_text = value_edit->text(); -} - -void LogsWidget::showEvent(QShowEvent *event) { - if (dynamic_mode->isChecked()) { - model->refresh(); - } -} - -void LogsWidget::updateState() { - if (dynamic_mode->isChecked()) { - model->updateState(); - } -} - -void LogsWidget::doubleClicked(const QModelIndex &index) { - if (index.isValid()) { - if (model->sigs.size() > 0 && index.column() > 0) { - emit openChart(model->msg_id, model->sigs[index.column()-1]); - } - can->seekTo(model->messages[index.row()].mono_time / (double)1e9 - can->routeStartTime()); - } + model->refresh(); } diff --git a/tools/cabana/historylog.h b/tools/cabana/historylog.h index f20f51637a3184..2458fc1c316279 100644 --- a/tools/cabana/historylog.h +++ b/tools/cabana/historylog.h @@ -7,8 +7,8 @@ #include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" class HeaderView : public QHeaderView { public: @@ -30,15 +30,22 @@ class HistoryLogModel : public QAbstractTableModel { void fetchMore(const QModelIndex &parent) override; inline bool canFetchMore(const QModelIndex &parent) const override { return has_more_data; } int rowCount(const QModelIndex &parent = QModelIndex()) const override { return messages.size(); } - int columnCount(const QModelIndex &parent = QModelIndex()) const override { return std::max(1ul, sigs.size()) + 1; } + int columnCount(const QModelIndex &parent = QModelIndex()) const override { + return display_signals_mode && !sigs.empty() ? sigs.size() + 1 : 2; + } + void refresh(); + +public slots: + void setDisplayType(int type); void setDynamicMode(int state); void segmentsMerged(); - void refresh(); +public: struct Message { uint64_t mono_time = 0; QVector sig_values; - QString data; + QByteArray data; + QVector colors; }; template @@ -46,6 +53,7 @@ class HistoryLogModel : public QAbstractTableModel { std::deque fetchData(uint64_t from_time, uint64_t min_time = 0); QString msg_id; + ChangeTracker hex_colors; bool has_more_data = true; const int batch_size = 50; int filter_sig_idx = -1; @@ -54,13 +62,8 @@ class HistoryLogModel : public QAbstractTableModel { std::function filter_cmp = nullptr; std::deque messages; std::vector sigs; - bool dynamic_mode = false; -}; - -class HistoryLog : public QTableView { -public: - HistoryLog(QWidget *parent); - int sizeHintForColumn(int column) const override { return -1; }; + bool dynamic_mode = true; + bool display_signals_mode = true; }; class LogsWidget : public QWidget { @@ -69,22 +72,19 @@ class LogsWidget : public QWidget { public: LogsWidget(QWidget *parent); void setMessage(const QString &message_id); - void updateState(); - -signals: - void openChart(const QString &msg_id, const Signal *sig); + void updateState() {if (dynamic_mode->isChecked()) model->updateState(); } + void showEvent(QShowEvent *event) override { if (dynamic_mode->isChecked()) model->refresh(); } private slots: void setFilter(); private: - void doubleClicked(const QModelIndex &index); - void showEvent(QShowEvent *event) override; + void refresh(); - HistoryLog *logs; + QTableView *logs; HistoryLogModel *model; QCheckBox *dynamic_mode; - QComboBox *signals_cb, *comp_box; + QComboBox *signals_cb, *comp_box, *display_type_cb; QLineEdit *value_edit; - QString cur_filter_text; + QWidget *filters_widget; }; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 43aa3300292a04..94cf9840f70a4e 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -2,7 +2,6 @@ #include #include -#include #include #include #include @@ -15,6 +14,8 @@ #include #include +#include "tools/cabana/commands.h" + static MainWindow *main_win = nullptr; void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; @@ -24,17 +25,18 @@ void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const MainWindow::MainWindow() : QMainWindow() { createDockWindows(); detail_widget = new DetailWidget(charts_widget, this); - detail_widget->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Preferred); setCentralWidget(detail_widget); createActions(); createStatusBar(); createShortcuts(); + // restore states restoreGeometry(settings.geometry); if (isMaximized()) { setGeometry(QApplication::desktop()->availableGeometry(this)); } restoreState(settings.window_state); + messages_widget->restoreHeaderState(settings.message_header_state); qRegisterMetaType("uint64_t"); qRegisterMetaType("ReplyMsgType"); @@ -53,40 +55,61 @@ MainWindow::MainWindow() : QMainWindow() { fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); } - QObject::connect(dbc_combo, SIGNAL(activated(const QString &)), SLOT(loadDBCFromName(const QString &))); QObject::connect(this, &MainWindow::showMessage, statusBar(), &QStatusBar::showMessage); QObject::connect(this, &MainWindow::updateProgressBar, this, &MainWindow::updateDownloadProgress); QObject::connect(messages_widget, &MessagesWidget::msgSelectionChanged, detail_widget, &DetailWidget::setMessage); QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); - QObject::connect(charts_widget, &ChartsWidget::rangeChanged, video_widget, &VideoWidget::rangeChanged); - QObject::connect(can, &CANMessages::streamStarted, this, &MainWindow::loadDBCFromFingerprint); + QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::loadDBCFromFingerprint); QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); - QObject::connect(detail_widget->undo_stack, &QUndoStack::indexChanged, [this](int index) { - setWindowTitle(tr("%1%2 - Cabana").arg(index > 0 ? "* " : "").arg(dbc()->name())); - }); + QObject::connect(UndoStack::instance(), &QUndoStack::cleanChanged, this, &MainWindow::undoStackCleanChanged); } void MainWindow::createActions() { QMenu *file_menu = menuBar()->addMenu(tr("&File")); - file_menu->addAction(tr("Open DBC File..."), this, &MainWindow::loadDBCFromFile); + 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); + + open_recent_menu = file_menu->addMenu(tr("Open &Recent")); + for (int i = 0; i < MAX_RECENT_FILES; ++i) { + recent_files_acts[i] = new QAction(this); + recent_files_acts[i]->setVisible(false); + QObject::connect(recent_files_acts[i], &QAction::triggered, this, &MainWindow::openRecentFile); + open_recent_menu->addAction(recent_files_acts[i]); + } + updateRecentFileActions(); + + file_menu->addSeparator(); + QMenu *load_opendbc_menu = file_menu->addMenu(tr("Load DBC from commaai/opendbc")); + // load_opendbc_menu->setStyleSheet("QMenu { menu-scrollable: true; }"); + auto dbc_names = dbc()->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); + } + file_menu->addAction(tr("Load DBC From Clipboard"), this, &MainWindow::loadDBCFromClipboard); + file_menu->addSeparator(); - file_menu->addAction(tr("Save DBC As..."), this, &MainWindow::saveDBCToFile); + file_menu->addAction(tr("Save DBC..."), this, &MainWindow::save)->setShortcuts(QKeySequence::Save); + file_menu->addAction(tr("Save DBC As..."), this, &MainWindow::saveAs)->setShortcuts(QKeySequence::SaveAs); file_menu->addAction(tr("Copy DBC To Clipboard"), this, &MainWindow::saveDBCToClipboard); file_menu->addSeparator(); - file_menu->addAction(tr("Settings..."), this, &MainWindow::setOption); + file_menu->addAction(tr("Settings..."), this, &MainWindow::setOption)->setShortcuts(QKeySequence::Preferences); + + file_menu->addSeparator(); + file_menu->addAction(tr("E&xit"), qApp, &QApplication::closeAllWindows)->setShortcuts(QKeySequence::Quit); QMenu *edit_menu = menuBar()->addMenu(tr("&Edit")); - auto undo_act = detail_widget->undo_stack->createUndoAction(this, tr("&Undo")); + auto undo_act = UndoStack::instance()->createUndoAction(this, tr("&Undo")); undo_act->setShortcuts(QKeySequence::Undo); edit_menu->addAction(undo_act); - auto redo_act = detail_widget->undo_stack->createRedoAction(this, tr("&Rndo")); + auto redo_act = UndoStack::instance()->createRedoAction(this, tr("&Rndo")); redo_act->setShortcuts(QKeySequence::Redo); edit_menu->addAction(redo_act); edit_menu->addSeparator(); QMenu *commands_menu = edit_menu->addMenu(tr("Command &List")); - auto undo_view = new QUndoView(detail_widget->undo_stack); + auto undo_view = new QUndoView(UndoStack::instance()); undo_view->setWindowTitle(tr("Command List")); QWidgetAction *commands_act = new QWidgetAction(this); commands_act->setDefaultWidget(undo_view); @@ -101,54 +124,43 @@ void MainWindow::createActions() { void MainWindow::createDockWindows() { // left panel - QWidget *messages_container = new QWidget(this); - QVBoxLayout *messages_layout = new QVBoxLayout(messages_container); - dbc_combo = createDBCSelector(); - messages_layout->addWidget(dbc_combo); messages_widget = new MessagesWidget(this); - messages_layout->addWidget(messages_widget); - QDockWidget *dock = new QDockWidget(tr("MESSAGES"), this); dock->setObjectName("MessagesPanel"); dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea | Qt::TopDockWidgetArea | Qt::BottomDockWidgetArea); dock->setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - dock->setWidget(messages_container); + dock->setWidget(messages_widget); addDockWidget(Qt::LeftDockWidgetArea, dock); // right panel - QWidget *right_container = new QWidget(this); - r_layout = new QVBoxLayout(right_container); charts_widget = new ChartsWidget(this); + QWidget *charts_container = new QWidget(this); + charts_layout = new QVBoxLayout(charts_container); + charts_layout->setContentsMargins(0, 0, 0, 0); + charts_layout->addWidget(charts_widget); + + // splitter between video and charts + video_splitter = new QSplitter(Qt::Vertical, this); video_widget = new VideoWidget(this); - r_layout->addWidget(video_widget, 0, Qt::AlignTop); - r_layout->addWidget(charts_widget, 1); - r_layout->addStretch(0); + video_splitter->addWidget(video_widget); + QObject::connect(charts_widget, &ChartsWidget::rangeChanged, video_widget, &VideoWidget::rangeChanged); + + video_splitter->addWidget(charts_container); + video_splitter->setStretchFactor(1, 1); + video_splitter->restoreState(settings.video_splitter_state); + if (can->liveStreaming() || video_splitter->sizes()[0] == 0) { + // display video at minimum size. + video_splitter->setSizes({1, 1}); + } video_dock = new QDockWidget(can->routeName(), this); video_dock->setObjectName(tr("VideoPanel")); video_dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea); video_dock->setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - video_dock->setWidget(right_container); + video_dock->setWidget(video_splitter); addDockWidget(Qt::RightDockWidgetArea, video_dock); } -QComboBox *MainWindow::createDBCSelector() { - QComboBox *c = new QComboBox(this); - c->setEditable(true); - c->lineEdit()->setPlaceholderText(tr("Select from an existing DBC file")); - c->setInsertPolicy(QComboBox::NoInsert); - c->completer()->setCompletionMode(QCompleter::PopupCompletion); - c->completer()->setFilterMode(Qt::MatchContains); - - auto dbc_names = dbc()->allDBCNames(); - std::sort(dbc_names.begin(), dbc_names.end()); - for (const auto &name : dbc_names) { - c->addItem(QString::fromStdString(name)); - } - c->setCurrentIndex(-1); - return c; -} - void MainWindow::createStatusBar() { progress_bar = new QProgressBar(); progress_bar->setRange(0, 100); @@ -164,60 +176,113 @@ void MainWindow::createShortcuts() { // TODO: add more shortcuts here. } +void MainWindow::undoStackCleanChanged(bool clean) { + setWindowModified(!clean); +} + void MainWindow::DBCFileChanged() { - detail_widget->undo_stack->clear(); - int index = dbc_combo->findText(QFileInfo(dbc()->name()).baseName()); - dbc_combo->setCurrentIndex(index); - setWindowTitle(tr("%1 - Cabana").arg(dbc()->name())); + UndoStack::instance()->clear(); + setWindowFilePath(QString("%1").arg(dbc()->name())); } -void MainWindow::loadDBCFromName(const QString &name) { - if (name != dbc()->name()) { - dbc()->open(name); +void MainWindow::newFile() { + remindSaveChanges(); + dbc()->open("untitled.dbc", ""); +} + +void MainWindow::openFile() { + remindSaveChanges(); + QString fn = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); + if (!fn.isEmpty()) { + loadFile(fn); } } -void MainWindow::loadDBCFromFile() { - QString file_name = QFileDialog::getOpenFileName(this, tr("Open File"), settings.last_dir, "DBC (*.dbc)"); - if (!file_name.isEmpty()) { - settings.last_dir = QFileInfo(file_name).absolutePath(); - QFile file(file_name); +void MainWindow::loadFile(const QString &fn) { + if (!fn.isEmpty()) { + QFile file(fn); if (file.open(QIODevice::ReadOnly)) { - auto dbc_name = QFileInfo(file_name).baseName(); + auto dbc_name = QFileInfo(fn).baseName(); dbc()->open(dbc_name, file.readAll()); + setCurrentFile(fn); + statusBar()->showMessage(tr("DBC File %1 loaded").arg(fn), 2000); } } } +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) { + if (name != dbc()->name()) { + remindSaveChanges(); + dbc()->open(name); + } +} + void MainWindow::loadDBCFromClipboard() { + remindSaveChanges(); QString dbc_str = QGuiApplication::clipboard()->text(); - dbc()->open("From Clipboard", dbc_str); - QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!")); + dbc()->open("from_clipboard.dbc", dbc_str); + if (dbc()->messages().size() > 0) { + QMessageBox::information(this, tr("Load From Clipboard"), tr("DBC Successfully Loaded!")); + } else { + QMessageBox::warning(this, tr("Load From Clipboard"), tr("Failed to parse dbc from clipboard!\nMake sure that you paste the text with correct format.")); + } } void MainWindow::loadDBCFromFingerprint() { + // Don't overwrite already loaded DBC + if (!dbc()->name().isEmpty()) { + return; + } + + remindSaveChanges(); auto fingerprint = can->carFingerprint(); video_dock->setWindowTitle(tr("ROUTE: %1 FINGERPINT: %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) { - loadDBCFromName(dbc_name.toString()); + loadDBCFromOpendbc(dbc_name.toString()); return; } } - dbc()->open("New_DBC", ""); + newFile(); } -void MainWindow::saveDBCToFile() { - QString file_name = QFileDialog::getSaveFileName(this, tr("Save File"), - QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); - if (!file_name.isEmpty()) { - settings.last_dir = QFileInfo(file_name).absolutePath(); - QFile file(file_name); - if (file.open(QIODevice::WriteOnly)) { - file.write(dbc()->generateDBC().toUtf8()); - detail_widget->undo_stack->clear(); - } +void MainWindow::save() { + if (current_file.isEmpty()) { + saveAs(); + } else { + saveFile(current_file); + } +} + +void MainWindow::saveFile(const QString &fn) { + QFile file(fn); + if (file.open(QIODevice::WriteOnly)) { + file.write(dbc()->generateDBC().toUtf8()); + UndoStack::instance()->setClean(); + setCurrentFile(fn); + statusBar()->showMessage(tr("File saved"), 2000); + } +} + +void MainWindow::saveAs() { + QString fn = QFileDialog::getSaveFileName(this, tr("Save File"), QDir::cleanPath(settings.last_dir + "/untitled.dbc"), tr("DBC (*.dbc)")); + if (!fn.isEmpty()) { + saveFile(fn); } } @@ -226,6 +291,49 @@ void MainWindow::saveDBCToClipboard() { QMessageBox::information(this, tr("Copy To Clipboard"), tr("DBC Successfully copied!")); } +void MainWindow::setCurrentFile(const QString &fn) { + current_file = fn; + setWindowFilePath(QString("%1").arg(fn)); + settings.recent_files.removeAll(fn); + settings.recent_files.prepend(fn); + while (settings.recent_files.size() > MAX_RECENT_FILES) { + settings.recent_files.removeLast(); + } + settings.last_dir = QFileInfo(fn).absolutePath(); + updateRecentFileActions(); +} + +void MainWindow::updateRecentFileActions() { + int num_recent_files = std::min(settings.recent_files.size(), MAX_RECENT_FILES); + + for (int i = 0; i < num_recent_files; ++i) { + QString text = tr("&%1 %2").arg(i + 1).arg(QFileInfo(settings.recent_files[i]).fileName()); + recent_files_acts[i]->setText(text); + recent_files_acts[i]->setData(settings.recent_files[i]); + recent_files_acts[i]->setVisible(true); + } + for (int i = num_recent_files; i < MAX_RECENT_FILES; ++i) { + recent_files_acts[i]->setVisible(false); + } + open_recent_menu->setEnabled(num_recent_files > 0); +} + +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)); + if (ret == QMessageBox::Ok) { + save(); + } else { + discard_changes = true; + } + } + UndoStack::instance()->clear(); + current_file = ""; +} + void MainWindow::updateDownloadProgress(uint64_t cur, uint64_t total, bool success) { if (success && cur < total) { progress_bar->setValue((cur / (double)total) * 100); @@ -239,13 +347,13 @@ void MainWindow::updateDownloadProgress(uint64_t cur, uint64_t total, bool succe void MainWindow::dockCharts(bool dock) { if (dock && floating_window) { floating_window->removeEventFilter(charts_widget); - r_layout->insertWidget(2, charts_widget, 1); + charts_layout->insertWidget(0, charts_widget, 1); floating_window->deleteLater(); floating_window = nullptr; } else if (!dock && !floating_window) { floating_window = new QWidget(this); floating_window->setWindowFlags(Qt::Window); - floating_window->setWindowTitle("Charts - Cabana"); + floating_window->setWindowTitle("Charts"); floating_window->setLayout(new QVBoxLayout()); floating_window->layout()->addWidget(charts_widget); floating_window->installEventFilter(charts_widget); @@ -254,22 +362,19 @@ void MainWindow::dockCharts(bool dock) { } void MainWindow::closeEvent(QCloseEvent *event) { - if (detail_widget->undo_stack->index() > 0) { - auto ret = QMessageBox::question(this, tr("Unsaved Changes"), - tr("Are you sure you want to exit without saving?\nAny unsaved changes will be lost."), - QMessageBox::Yes | QMessageBox::No); - if (ret == QMessageBox::No) { - event->ignore(); - return; - } - } + remindSaveChanges(); main_win = nullptr; if (floating_window) floating_window->deleteLater(); + // save states settings.geometry = saveGeometry(); settings.window_state = saveState(); + if (!can->liveStreaming()) { + settings.video_splitter_state = video_splitter->saveState(); + } + settings.message_header_state = messages_widget->saveHeaderState(); settings.save(); QWidget::closeEvent(event); } @@ -280,6 +385,7 @@ void MainWindow::setOption() { } void MainWindow::findSimilarBits() { - FindSimilarBitsDlg dlg(this); - dlg.exec(); + FindSimilarBitsDlg *dlg = new FindSimilarBitsDlg(this); + QObject::connect(dlg, &FindSimilarBitsDlg::openMessage, messages_widget, &MessagesWidget::selectMessage); + dlg->show(); } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index f8b5f923493987..c26f6973c072e4 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -1,10 +1,10 @@ #pragma once -#include #include #include #include #include +#include #include #include "tools/cabana/chartswidget.h" @@ -20,13 +20,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); public slots: - void loadDBCFromName(const QString &name); + void newFile(); + void openFile(); + void openRecentFile(); + void openOpendbcFile(); + void loadDBCFromOpendbc(const QString &name); void loadDBCFromFingerprint(); - void loadDBCFromFile(); void loadDBCFromClipboard(); - void saveDBCToFile(); + void save(); + void saveAs(); void saveDBCToClipboard(); signals: @@ -34,9 +39,12 @@ public slots: void updateProgressBar(uint64_t cur, uint64_t total, bool success); protected: + void remindSaveChanges(); + void saveFile(const QString &fn); + void setCurrentFile(const QString &fn); + void updateRecentFileActions(); void createActions(); void createDockWindows(); - QComboBox *createDBCSelector(); void createStatusBar(); void createShortcuts(); void closeEvent(QCloseEvent *event) override; @@ -44,15 +52,20 @@ public slots: void updateDownloadProgress(uint64_t cur, uint64_t total, bool success); void setOption(); void findSimilarBits(); + void undoStackCleanChanged(bool clean); - VideoWidget *video_widget; + VideoWidget *video_widget = nullptr; QDockWidget *video_dock; MessagesWidget *messages_widget; DetailWidget *detail_widget; ChartsWidget *charts_widget; QWidget *floating_window = nullptr; - QVBoxLayout *r_layout; + QVBoxLayout *charts_layout; QProgressBar *progress_bar; QJsonDocument fingerprint_to_dbc; - QComboBox *dbc_combo; + QSplitter *video_splitter;; + QString current_file = ""; + enum { MAX_RECENT_FILES = 15 }; + QAction *recent_files_acts[MAX_RECENT_FILES] = {}; + QMenu *open_recent_menu = nullptr; }; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index dbf87a3da21ac5..9d0fc23e4dcdc9 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,15 +1,17 @@ #include "tools/cabana/messageswidget.h" +#include #include -#include +#include #include +#include +#include #include #include "tools/cabana/dbcmanager.h" MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); // message filter QLineEdit *filter = new QLineEdit(this); @@ -21,24 +23,30 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget = new QTableView(this); model = new MessageListModel(this); table_widget->setModel(model); + table_widget->setItemDelegateForColumn(4, new MessageBytesDelegate(table_widget)); table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); table_widget->setSelectionMode(QAbstractItemView::SingleSelection); - table_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); table_widget->setSortingEnabled(true); table_widget->sortByColumn(0, Qt::AscendingOrder); - table_widget->setColumnWidth(0, 250); - table_widget->setColumnWidth(1, 80); - table_widget->setColumnWidth(2, 80); table_widget->horizontalHeader()->setStretchLastSection(true); table_widget->verticalHeader()->hide(); main_layout->addWidget(table_widget); + // suppress + QHBoxLayout *suppress_layout = new QHBoxLayout(); + suppress_add = new QPushButton("Suppress Highlighted"); + suppress_clear = new QPushButton(); + suppress_layout->addWidget(suppress_add); + suppress_layout->addWidget(suppress_clear); + main_layout->addLayout(suppress_layout); + // signals/slots QObject::connect(filter, &QLineEdit::textChanged, model, &MessageListModel::setFilterString); - QObject::connect(can, &CANMessages::msgsReceived, model, &MessageListModel::msgsReceived); + QObject::connect(can, &AbstractStream::msgsReceived, model, &MessageListModel::msgsReceived); 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(model, &MessageListModel::modelReset, [this]() { selectMessage(current_msg_id); }); QObject::connect(table_widget->selectionModel(), &QItemSelectionModel::currentChanged, [=](const QModelIndex ¤t, const QModelIndex &previous) { if (current.isValid() && current.row() < model->msgs.size()) { if (model->msgs[current.row()] != current_msg_id) { @@ -47,12 +55,35 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { } } }); - QObject::connect(model, &MessageListModel::modelReset, [this]() { - if (int row = model->msgs.indexOf(current_msg_id); row != -1) - table_widget->selectionModel()->setCurrentIndex(model->index(row, 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); + QObject::connect(suppress_add, &QPushButton::clicked, [=]() { + model->suppress(); + updateSuppressedButtons(); + }); + QObject::connect(suppress_clear, &QPushButton::clicked, [=]() { + model->clearSuppress(); + updateSuppressedButtons(); }); + + updateSuppressedButtons(); +} + +void MessagesWidget::selectMessage(const QString &msg_id) { + if (int row = model->msgs.indexOf(msg_id); row != -1) { + table_widget->selectionModel()->setCurrentIndex(model->index(row, 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); + } +} + +void MessagesWidget::updateSuppressedButtons() { + if (model->suppressed_bytes.empty()) { + suppress_clear->setEnabled(false); + suppress_clear->setText("Clear Suppressed"); + } else { + suppress_clear->setEnabled(true); + suppress_clear->setText(QString("Clear Suppressed (%1)").arg(model->suppressed_bytes.size())); + } } + // MessageListModel QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, int role) const { @@ -62,9 +93,10 @@ QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, } QVariant MessageListModel::data(const QModelIndex &index, int role) const { + const auto &id = msgs[index.row()]; + auto &can_data = can->lastMessage(id); + if (role == Qt::DisplayRole) { - const auto &id = msgs[index.row()]; - auto &can_data = can->lastMessage(id); switch (index.column()) { case 0: return msgName(id); case 1: return id; @@ -72,19 +104,41 @@ QVariant MessageListModel::data(const QModelIndex &index, int role) const { case 3: return can_data.count; case 4: return toHex(can_data.dat); } - } else if (role == Qt::FontRole && index.column() == columnCount() - 1) { - return QFontDatabase::systemFont(QFontDatabase::FixedFont); + } else if (role == Qt::UserRole && index.column() == 4) { + QList colors; + colors.reserve(can_data.dat.size()); + for (int i = 0; i < can_data.dat.size(); i++){ + if (suppressed_bytes.contains({id, i})) { + colors.append(QColor(255, 255, 255, 0)); + } else { + colors.append(i < can_data.colors.size() ? can_data.colors[i] : QColor(255, 255, 255, 0)); + } + } + return colors; + } return {}; } void MessageListModel::setFilterString(const QString &string) { + auto contains = [](const QString &id, const QString &txt) { + auto cs = Qt::CaseInsensitive; + if (id.contains(txt, cs) || msgName(id).contains(txt, cs)) return true; + // Search by signal name + if (const auto msg = dbc()->msg(id)) { + for (auto &signal : msg->getSignals()) { + if (QString::fromStdString(signal->name).contains(txt, cs)) return true; + } + } + return false; + }; + filter_str = string; - bool search_id = filter_str.contains(':'); msgs.clear(); for (auto it = can->can_msgs.begin(); it != can->can_msgs.end(); ++it) { - if ((search_id ? it.key() : msgName(it.key())).contains(filter_str, Qt::CaseInsensitive)) + if (filter_str.isEmpty() || contains(it.key(), filter_str)) { msgs.push_back(it.key()); + } } sortMessages(); } @@ -93,22 +147,27 @@ void MessageListModel::sortMessages() { beginResetModel(); if (sort_column == 0) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - bool ret = std::pair{msgName(l), l} < std::pair{msgName(r), r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + 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) { - return sort_order == Qt::AscendingOrder ? l < r : l > r; + auto ll = DBCManager::parseId(l); + auto rr = DBCManager::parseId(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) { - bool ret = std::pair{can->lastMessage(l).freq, l} < std::pair{can->lastMessage(r).freq, r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + 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 == 3) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &r) { - bool ret = std::pair{can->lastMessage(l).count, l} < std::pair{can->lastMessage(r).count, r}; - return sort_order == Qt::AscendingOrder ? ret : !ret; + 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(); @@ -138,3 +197,21 @@ void MessageListModel::sort(int column, Qt::SortOrder order) { sortMessages(); } } + +void MessageListModel::suppress() { + const double cur_ts = can->currentSec(); + + for (auto &id : msgs) { + auto &can_data = can->lastMessage(id); + for (int i = 0; i < can_data.dat.size(); i++) { + const double dt = cur_ts - can_data.last_change_t[i]; + if (dt < 2.0) { + suppressed_bytes.insert({id, i}); + } + } + } +} + +void MessageListModel::clearSuppress() { + suppressed_bytes.clear(); +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 3a42bed4beebd5..1927faa6463610 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -1,9 +1,12 @@ #pragma once #include +#include +#include +#include #include -#include "tools/cabana/canmessages.h" +#include "tools/cabana/streams/abstractstream.h" class MessageListModel : public QAbstractTableModel { Q_OBJECT @@ -18,7 +21,10 @@ Q_OBJECT void setFilterString(const QString &string); void msgsReceived(const QHash *new_msgs = nullptr); void sortMessages(); + void suppress(); + void clearSuppress(); QStringList msgs; + QSet> suppressed_bytes; private: QString filter_str; @@ -31,6 +37,11 @@ class MessagesWidget : public QWidget { public: MessagesWidget(QWidget *parent); + void selectMessage(const QString &message_id); + QByteArray saveHeaderState() const { return table_widget->horizontalHeader()->saveState(); } + bool restoreHeaderState(const QByteArray &state) const { return table_widget->horizontalHeader()->restoreState(state); } + void updateSuppressedButtons(); + signals: void msgSelectionChanged(const QString &message_id); @@ -38,4 +49,7 @@ class MessagesWidget : public QWidget { QTableView *table_widget; QString current_msg_id; MessageListModel *model; + QPushButton *suppress_add; + QPushButton *suppress_clear; + }; diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 5e7f833317523a..22c7a941ab2bef 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -15,23 +15,33 @@ Settings::Settings() { void Settings::save() { QSettings s("settings", QSettings::IniFormat); s.setValue("fps", fps); - s.setValue("cached_segment", cached_segment_limit); + s.setValue("max_cached_minutes", max_cached_minutes); s.setValue("chart_height", chart_height); - s.setValue("max_chart_x_range", max_chart_x_range); + s.setValue("chart_range", chart_range); + s.setValue("chart_column_count", chart_column_count); s.setValue("last_dir", last_dir); s.setValue("window_state", window_state); s.setValue("geometry", geometry); + s.setValue("video_splitter_state", video_splitter_state); + s.setValue("recent_files", recent_files); + s.setValue("message_header_state", message_header_state); + s.setValue("chart_series_type", chart_series_type); } void Settings::load() { QSettings s("settings", QSettings::IniFormat); fps = s.value("fps", 10).toInt(); - cached_segment_limit = s.value("cached_segment", 3).toInt(); + max_cached_minutes = s.value("max_cached_minutes", 5).toInt(); chart_height = s.value("chart_height", 200).toInt(); - max_chart_x_range = s.value("max_chart_x_range", 3 * 60).toInt(); + chart_range = s.value("chart_range", 3 * 60).toInt(); + chart_column_count = s.value("chart_column_count", 1).toInt(); last_dir = s.value("last_dir", QDir::homePath()).toString(); window_state = s.value("window_state").toByteArray(); geometry = s.value("geometry").toByteArray(); + video_splitter_state = s.value("video_splitter_state").toByteArray(); + recent_files = s.value("recent_files").toStringList(); + message_header_state = s.value("message_header_state").toByteArray(); + chart_series_type = s.value("chart_series_type", 0).toInt(); } // SettingsDlg @@ -46,23 +56,22 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { fps->setValue(settings.fps); form_layout->addRow("FPS", fps); - cached_segment = new QSpinBox(this); - cached_segment->setRange(3, 60); - cached_segment->setSingleStep(1); - cached_segment->setValue(settings.cached_segment_limit); - form_layout->addRow(tr("Cached segments limit"), cached_segment); + cached_minutes = new QSpinBox(this); + cached_minutes->setRange(5, 60); + cached_minutes->setSingleStep(1); + cached_minutes->setValue(settings.max_cached_minutes); + form_layout->addRow(tr("Max Cached Minutes"), cached_minutes); - max_chart_x_range = new QSpinBox(this); - max_chart_x_range->setRange(1, 60); - max_chart_x_range->setSingleStep(1); - max_chart_x_range->setValue(settings.max_chart_x_range / 60); - form_layout->addRow(tr("Chart range (minutes)"), max_chart_x_range); + chart_series_type = new QComboBox(this); + chart_series_type->addItems({tr("Line"), tr("Scatter")}); + chart_series_type->setCurrentIndex(settings.chart_series_type); + form_layout->addRow(tr("Chart Default Series Type"), chart_series_type); chart_height = new QSpinBox(this); chart_height->setRange(100, 500); chart_height->setSingleStep(10); chart_height->setValue(settings.chart_height); - form_layout->addRow(tr("Chart height"), chart_height); + form_layout->addRow(tr("Chart Height"), chart_height); auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); form_layout->addRow(buttonBox); @@ -74,9 +83,9 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { void SettingsDlg::save() { settings.fps = fps->value(); - settings.cached_segment_limit = cached_segment->value(); + settings.max_cached_minutes = cached_minutes->value(); + settings.chart_series_type = chart_series_type->currentIndex(); settings.chart_height = chart_height->value(); - settings.max_chart_x_range = max_chart_x_range->value() * 60; settings.save(); accept(); emit settings.changed(); diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index d231a3a53aaede..7abad81c2922a7 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -14,11 +14,17 @@ class Settings : public QObject { void load(); int fps = 10; - int cached_segment_limit = 3; + int max_cached_minutes = 5; int chart_height = 200; - int max_chart_x_range = 3 * 60; // 3 minutes + int chart_column_count = 1; + int chart_range = 3 * 60; // e minutes + int chart_series_type = 0; QString last_dir; - QByteArray window_state, geometry; + QByteArray geometry; + QByteArray video_splitter_state; + QByteArray window_state; + QStringList recent_files; + QByteArray message_header_state; signals: void changed(); @@ -31,9 +37,9 @@ class SettingsDlg : public QDialog { SettingsDlg(QWidget *parent); void save(); QSpinBox *fps; - QSpinBox *cached_segment; + QSpinBox *cached_minutes; QSpinBox *chart_height; - QSpinBox *max_chart_x_range; + QComboBox *chart_series_type; }; extern Settings settings; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index b2491d5b1f7437..7f31293c41b647 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -1,157 +1,192 @@ #include "tools/cabana/signaledit.h" -#include -#include #include #include +#include +#include +#include +#include #include -// SignalForm +#include "tools/cabana/commands.h" -SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { - QFormLayout *form_layout = new QFormLayout(this); +// SignalModel - name = new QLineEdit(); - name->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name)); - form_layout->addRow(tr("Name"), name); +SignalModel::SignalModel(QObject *parent) : root(new Item), QAbstractItemModel(parent) { + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &SignalModel::refresh); + QObject::connect(dbc(), &DBCManager::msgUpdated, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::msgRemoved, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, this, &SignalModel::handleSignalAdded); + QObject::connect(dbc(), &DBCManager::signalUpdated, this, &SignalModel::handleSignalUpdated); + QObject::connect(dbc(), &DBCManager::signalRemoved, this, &SignalModel::handleSignalRemoved); + QObject::connect(can, &AbstractStream::msgsReceived, this, &SignalModel::updateState); +} - size = new QSpinBox(); - size->setMinimum(1); - form_layout->addRow(tr("Size"), size); +void SignalModel::insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig) { + Item *item = new Item{.sig = sig, .parent = parent_item, .title = sig->name.c_str(), .type = Item::Sig}; + parent_item->children.insert(pos, item); + QString titles[]{"Name", "Size", "Little Endian", "Signed", "Offset", "Factor", "Extra Info", "Unit", "Comment", "Minimum", "Maximum", "Description"}; + for (int i = 0; i < std::size(titles); ++i) { + item->children.push_back(new Item{.sig = sig, .parent = item, .title = titles[i], .type = (Item::Type)(i + Item::Name)}); + } +} - endianness = new QComboBox(); - endianness->addItems({"Little", "Big"}); - form_layout->addRow(tr("Endianness"), endianness); +void SignalModel::setMessage(const QString &id) { + msg_id = id; + filter_str = ""; + refresh(); + updateState(nullptr); +} - form_layout->addRow(tr("lsb"), lsb = new QLabel()); - form_layout->addRow(tr("msb"), msb = new QLabel()); +void SignalModel::setFilter(const QString &txt) { + filter_str = txt; + refresh(); +} - sign = new QComboBox(); - sign->addItems({"Signed", "Unsigned"}); - form_layout->addRow(tr("sign"), sign); +void SignalModel::refresh() { + beginResetModel(); + root.reset(new SignalModel::Item); + if (auto msg = dbc()->msg(msg_id)) { + for (auto &s : msg->getSignals()) { + if (filter_str.isEmpty() || QString::fromStdString(s->name).contains(filter_str, Qt::CaseInsensitive)) { + insertItem(root.get(), root->children.size(), s); + } + } + } + endResetModel(); +} - auto double_validator = new QDoubleValidator(this); +void SignalModel::updateState(const QHash *msgs) { + if (!msgs || (msgs->contains(msg_id))) { + auto &dat = can->lastMessage(msg_id).dat; + int row = 0; + for (auto item : root->children) { + double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *item->sig); + item->sig_val = QString::number(value); + emit dataChanged(index(row, 1), index(row, 1), {Qt::DisplayRole}); + ++row; + } + } +} - factor = new QLineEdit(); - factor->setValidator(double_validator); - form_layout->addRow(tr("Factor"), factor); +int SignalModel::rowCount(const QModelIndex &parent) const { + if (parent.column() > 0) return 0; - offset = new QLineEdit(); - offset->setValidator(double_validator); - form_layout->addRow(tr("Offset"), offset); + auto parent_item = getItem(parent); + int row_count = parent_item->children.size(); + if (parent_item->type == Item::Sig && !parent_item->extra_expanded) { + row_count -= (Item::Desc - Item::ExtraInfo); + } + return row_count; +} - // TODO: parse the following parameters in opendbc - unit = new QLineEdit(); - form_layout->addRow(tr("Unit"), unit); - comment = new QLineEdit(); - form_layout->addRow(tr("Comment"), comment); - min_val = new QLineEdit(); - min_val->setValidator(double_validator); - form_layout->addRow(tr("Minimum value"), min_val); - max_val = new QLineEdit(); - max_val->setValidator(double_validator); - form_layout->addRow(tr("Maximum value"), max_val); - val_desc = new QLineEdit(); - form_layout->addRow(tr("Value descriptions"), val_desc); +Qt::ItemFlags SignalModel::flags(const QModelIndex &index) const { + if (!index.isValid()) return Qt::NoItemFlags; - QObject::connect(name, &QLineEdit::textEdited, this, &SignalForm::changed); - QObject::connect(factor, &QLineEdit::textEdited, this, &SignalForm::changed); - QObject::connect(offset, &QLineEdit::textEdited, this, &SignalForm::changed); - QObject::connect(sign, SIGNAL(activated(int)), SIGNAL(changed())); - QObject::connect(endianness, SIGNAL(activated(int)), SIGNAL(changed())); - QObject::connect(size, SIGNAL(valueChanged(int)), SIGNAL(changed())); + auto item = getItem(index); + Qt::ItemFlags flags = Qt::ItemIsSelectable | Qt::ItemIsEnabled; + if (index.column() == 1 && item->type != Item::Sig && item->type != Item::ExtraInfo) { + flags |= (item->type == Item::Endian || item->type == Item::Signed) ? Qt::ItemIsUserCheckable : Qt::ItemIsEditable; + } + return flags; } -// SignalEdit - -SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); - main_layout->setSpacing(0); +int SignalModel::signalRow(const Signal *sig) const { + auto &children = root->children; + for (int i = 0; i < children.size(); ++i) { + if (children[i]->sig == sig) return i; + } + return -1; +} - // title bar - auto title_bar = new QWidget(this); - title_bar->setFixedHeight(32); - QHBoxLayout *title_layout = new QHBoxLayout(title_bar); - title_layout->setContentsMargins(0, 0, 0, 0); - title_bar->setStyleSheet("QToolButton {width:15px;height:15px;font-size:15px}"); - color_label = new QLabel(this); - color_label->setFixedWidth(25); - color_label->setContentsMargins(5, 0, 0, 0); - title_layout->addWidget(color_label); - icon = new QLabel(this); - title_layout->addWidget(icon); - title = new ElidedLabel(this); - title->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - title_layout->addWidget(title); - - plot_btn = new QToolButton(this); - plot_btn->setText("📈"); - plot_btn->setCheckable(true); - plot_btn->setAutoRaise(true); - title_layout->addWidget(plot_btn); - auto remove_btn = new QToolButton(this); - remove_btn->setAutoRaise(true); - remove_btn->setText("x"); - remove_btn->setToolTip(tr("Remove signal")); - title_layout->addWidget(remove_btn); - main_layout->addWidget(title_bar); +QModelIndex SignalModel::index(int row, int column, const QModelIndex &parent) const { + if (!hasIndex(row, column, parent)) return {}; + return createIndex(row, column, getItem(parent)->children[row]); +} - // signal form - form = new SignalForm(this); - form->setVisible(false); - main_layout->addWidget(form); +QModelIndex SignalModel::parent(const QModelIndex &index) const { + if (!index.isValid()) return {}; + Item *parent_item = getItem(index)->parent; + return parent_item == root.get() ? QModelIndex() : createIndex(parent_item->row(), 0, parent_item); +} - // bottom line - QFrame *hline = new QFrame(); - hline->setFrameShape(QFrame::HLine); - hline->setFrameShadow(QFrame::Sunken); - main_layout->addWidget(hline); +QVariant SignalModel::data(const QModelIndex &index, int role) const { + if (index.isValid()) { + const Item *item = getItem(index); + if (role == Qt::DisplayRole || role == Qt::EditRole) { + if (index.column() == 0) { + return item->type == Item::Sig ? QString::fromStdString(item->sig->name) : item->title; + } else { + switch (item->type) { + case Item::Sig: return item->sig_val; + case Item::Name: return QString::fromStdString(item->sig->name); + case Item::Size: return item->sig->size; + case Item::Offset: return QString::number(item->sig->offset, 'f', 6); + case Item::Factor: return QString::number(item->sig->factor, 'f', 6); + default: break; + } + } + } else if (role == Qt::CheckStateRole && index.column() == 1) { + if (item->type == Item::Endian) return item->sig->is_little_endian ? Qt::Checked : Qt::Unchecked; + if (item->type == Item::Signed) return item->sig->is_signed ? Qt::Checked : Qt::Unchecked; + } else if (role == Qt::DecorationRole && index.column() == 0 && item->type == Item::ExtraInfo) { + return utils::icon(item->parent->extra_expanded ? "chevron-compact-down" : "chevron-compact-up"); + } + } + return {}; +} - save_timer = new QTimer(this); - save_timer->setInterval(300); - save_timer->setSingleShot(true); - save_timer->callOnTimeout(this, &SignalEdit::saveSignal); +bool SignalModel::setData(const QModelIndex &index, const QVariant &value, int role) { + if (role != Qt::EditRole && role != Qt::CheckStateRole) return false; - QObject::connect(title, &ElidedLabel::clicked, [this]() { emit showFormClicked(sig); }); - QObject::connect(plot_btn, &QToolButton::clicked, [this](bool checked) { - emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); - }); - QObject::connect(remove_btn, &QToolButton::clicked, [this]() { emit remove(sig); }); - QObject::connect(form, &SignalForm::changed, [this]() { save_timer->start(); }); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + Item *item = getItem(index); + Signal s = *item->sig; + switch (item->type) { + case Item::Name: s.name = value.toString().toStdString(); break; + case Item::Size: s.size = value.toInt(); break; + case Item::Endian: s.is_little_endian = value.toBool(); break; + case Item::Signed: s.is_signed = value.toBool(); break; + case Item::Offset: s.offset = value.toDouble(); break; + case Item::Factor: s.factor = value.toDouble(); break; + default: return false; + } + bool ret = saveSignal(item->sig, s); + emit dataChanged(index, index, {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + return ret; } -void SignalEdit::setSignal(const QString &message_id, const Signal *signal) { - sig = signal; - updateForm(msg_id == message_id && form->isVisible()); - msg_id = message_id; - color_label->setText(QString::number(form_idx + 1)); - color_label->setStyleSheet(QString("color:black; background-color:%2").arg(getColor(form_idx))); - title->setText(sig->name.c_str()); - show(); +void SignalModel::showExtraInfo(const QModelIndex &index) { + auto item = getItem(index); + if (item->type == Item::ExtraInfo) { + if (!item->parent->extra_expanded) { + item->parent->extra_expanded = true; + beginInsertRows(index.parent(), 7, 13); + endInsertRows(); + } else { + item->parent->extra_expanded = false; + beginRemoveRows(index.parent(), 7, 13); + endRemoveRows(); + } + } } -void SignalEdit::saveSignal() { - if (!sig || !form->changed_by_user) return; +bool SignalModel::saveSignal(const Signal *origin_s, Signal &s) { + auto msg = dbc()->msg(msg_id); + if (s.name != origin_s->name && msg->sigs.count(s.name.c_str()) != 0) { + QString text = tr("There is already a signal with the same name '%1'").arg(s.name.c_str()); + QMessageBox::warning(nullptr, tr("Failed to save signal"), text); + return false; + } - Signal s = *sig; - s.name = form->name->text().toStdString(); - s.size = form->size->text().toInt(); - s.offset = form->offset->text().toDouble(); - s.factor = form->factor->text().toDouble(); - s.is_signed = form->sign->currentIndex() == 0; - bool little_endian = form->endianness->currentIndex() == 0; - if (little_endian != s.is_little_endian) { + if (s.is_little_endian != origin_s->is_little_endian) { int start = std::floor(s.start_bit / 8); - if (little_endian) { + if (s.is_little_endian) { int end = std::floor((s.start_bit - s.size + 1) / 8); s.start_bit = start == end ? s.start_bit - s.size + 1 : bigEndianStartBitsIndex(s.start_bit); } else { int end = std::floor((s.start_bit + s.size - 1) / 8); s.start_bit = start == end ? s.start_bit + s.size - 1 : bigEndianBitIndex(s.start_bit); } - s.is_little_endian = little_endian; } if (s.is_little_endian) { s.lsb = s.start_bit; @@ -160,45 +195,254 @@ void SignalEdit::saveSignal() { s.lsb = bigEndianStartBitsIndex(bigEndianBitIndex(s.start_bit) + s.size - 1); s.msb = s.start_bit; } - if (s != *sig) - emit save(this->sig, s); + + UndoStack::push(new EditSignalCommand(msg_id, origin_s, s)); + return true; } -void SignalEdit::setChartOpened(bool opened) { - plot_btn->setToolTip(opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened chart")); - plot_btn->setChecked(opened); +void SignalModel::addSignal(int start_bit, int size, bool little_endian) { + auto msg = dbc()->msg(msg_id); + for (int i = 1; !msg; ++i) { + QString name = QString("NEW_MSG_%1").arg(i); + if (std::none_of(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; })) { + UndoStack::push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); + msg = dbc()->msg(msg_id); + } + } + + Signal sig = {.is_little_endian = little_endian, .factor = 1}; + for (int i = 1; /**/; ++i) { + sig.name = "NEW_SIGNAL_" + std::to_string(i); + if (msg->sigs.count(sig.name.c_str()) == 0) break; + } + updateSigSizeParamsFromRange(sig, start_bit, size); + UndoStack::push(new AddSigCommand(msg_id, sig)); +} + +void SignalModel::resizeSignal(const Signal *sig, int start_bit, int size) { + Signal s = *sig; + updateSigSizeParamsFromRange(s, start_bit, size); + saveSignal(sig, s); } -void SignalEdit::updateForm(bool visible) { - if (visible && sig) { - form->changed_by_user = false; - if (form->name->text() != sig->name.c_str()) { - form->name->setText(sig->name.c_str()); +void SignalModel::removeSignal(const Signal *sig) { + UndoStack::push(new RemoveSigCommand(msg_id, sig)); +} + +void SignalModel::handleMsgChanged(uint32_t address) { + if (address == DBCManager::parseId(msg_id).second) { + refresh(); + } +} + +void SignalModel::handleSignalAdded(uint32_t address, const Signal *sig) { + if (address == DBCManager::parseId(msg_id).second) { + int i = 0; + for (; i < root->children.size(); ++i) { + if (sig->start_bit < root->children[i]->sig->start_bit) break; } - form->endianness->setCurrentIndex(sig->is_little_endian ? 0 : 1); - form->sign->setCurrentIndex(sig->is_signed ? 0 : 1); - form->factor->setText(QString::number(sig->factor)); - form->offset->setText(QString::number(sig->offset)); - form->msb->setText(QString::number(sig->msb)); - form->lsb->setText(QString::number(sig->lsb)); - form->size->setValue(sig->size); - form->changed_by_user = true; + beginInsertRows({}, i, i); + insertItem(root.get(), i, sig); + endInsertRows(); } - form->setVisible(visible); - icon->setText(visible ? "▼ " : "> "); } -void SignalEdit::signalHovered(const Signal *s) { - auto color = sig == s ? "white" : "black"; - color_label->setStyleSheet(QString("color:%1; background-color:%2").arg(color).arg(getColor(form_idx))); +void SignalModel::handleSignalUpdated(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + emit dataChanged(index(row, 0), index(row, 1), {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + } } -void SignalEdit::enterEvent(QEvent *event) { - emit highlight(sig); - QWidget::enterEvent(event); +void SignalModel::handleSignalRemoved(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + beginRemoveRows({}, row, row); + delete root->children.takeAt(row); + endRemoveRows(); + } +} + +// SignalItemDelegate + +SignalItemDelegate::SignalItemDelegate(QObject *parent) { + name_validator = new NameValidator(this); + double_validator = new QDoubleValidator(this); + double_validator->setLocale(QLocale::C); // Match locale of QString::toDouble() instead of system +} + +void SignalItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item && !index.parent().isValid() && index.column() == 0) { + painter->save(); + painter->setRenderHint(QPainter::Antialiasing); + if (option.state & QStyle::State_Selected) { + painter->fillRect(option.rect, option.palette.highlight()); + } + + // color label + auto bg_color = QColor(getColor(item->row())); + QRect rc{option.rect.left() + 3, option.rect.top(), 22, option.rect.height()}; + painter->setPen(Qt::NoPen); + painter->setBrush(item->highlight ? bg_color.darker(125) : bg_color); + painter->drawRoundedRect(rc.adjusted(0, 2, 0, -2), 5, 5); + painter->setPen(item->highlight ? Qt::white : Qt::black); + painter->drawText(rc, Qt::AlignCenter, QString::number(item->row() + 1)); + + // signal name + QFont font; + font.setBold(true); + painter->setFont(font); + painter->setPen((option.state & QStyle::State_Selected ? option.palette.highlightedText() : option.palette.text()).color()); + painter->drawText(option.rect.adjusted(rc.width() + 9, 0, 0, 0), option.displayAlignment, index.data(Qt::DisplayRole).toString()); + painter->restore(); + } else { + QStyledItemDelegate::paint(painter, option, index); + } +} + +QWidget *SignalItemDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item->type == SignalModel::Item::Name || item->type == SignalModel::Item::Offset || item->type == SignalModel::Item::Factor) { + QLineEdit *e = new QLineEdit(parent); + e->setFrame(false); + e->setValidator(index.row() == 0 ? name_validator : double_validator); + return e; + } else if (item->type == SignalModel::Item::Size) { + QSpinBox *spin = new QSpinBox(parent); + spin->setFrame(false); + spin->setRange(1, 64); + return spin; + } + return QStyledItemDelegate::createEditor(parent, option, index); +} + +// SignalView + +SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { + // title bar + QWidget *title_bar = new QWidget(this); + title_bar->setAutoFillBackground(true); + QHBoxLayout *hl = new QHBoxLayout(title_bar); + hl->addWidget(signal_count_lb = new QLabel()); + filter_edit = new QLineEdit(this); + filter_edit->setClearButtonEnabled(true); + filter_edit->setPlaceholderText(tr("filter signals by name")); + hl->addWidget(filter_edit); + hl->addStretch(1); + auto collapse_btn = new QToolButton(); + collapse_btn->setIcon(utils::icon("dash-square")); + collapse_btn->setIconSize({12, 12}); + collapse_btn->setAutoRaise(true); + collapse_btn->setToolTip(tr("Collapse All")); + hl->addWidget(collapse_btn); + + // tree view + tree = new QTreeView(this); + tree->setModel(model = new SignalModel(this)); + tree->setItemDelegate(new SignalItemDelegate(this)); + tree->setFrameShape(QFrame::NoFrame); + tree->setHeaderHidden(true); + tree->setMouseTracking(true); + tree->setExpandsOnDoubleClick(false); + tree->header()->setSectionResizeMode(QHeaderView::Stretch); + tree->setMinimumHeight(300); + tree->setStyleSheet("QSpinBox{background-color:white;border:none;} QLineEdit{background-color:white;}"); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + main_layout->addWidget(title_bar); + main_layout->addWidget(tree); + + QObject::connect(filter_edit, &QLineEdit::textEdited, model, &SignalModel::setFilter); + QObject::connect(collapse_btn, &QPushButton::clicked, tree, &QTreeView::collapseAll); + QObject::connect(tree, &QAbstractItemView::clicked, this, &SignalView::rowClicked); + QObject::connect(tree, &QTreeView::viewportEntered, [this]() { emit highlight(nullptr); }); + QObject::connect(tree, &QTreeView::entered, [this](const QModelIndex &index) { emit highlight(model->getItem(index)->sig); }); + QObject::connect(model, &QAbstractItemModel::modelReset, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsInserted, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsRemoved, this, &SignalView::rowsChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, [this](uint32_t address, const Signal *sig) { expandSignal(sig); }); +} + +void SignalView::setMessage(const QString &id) { + msg_id = id; + filter_edit->clear(); + model->setMessage(id); +} + +void SignalView::rowsChanged() { + auto create_btn = [](const QString &id, const QString &tooltip) { + auto btn = new QToolButton(); + btn->setIcon(utils::icon(id)); + btn->setToolTip(tooltip); + btn->setAutoRaise(true); + return btn; + }; + + signal_count_lb->setText(tr("Signals: %1").arg(model->rowCount())); + + for (int i = 0; i < model->rowCount(); ++i) { + auto index = model->index(i, 1); + if (!tree->indexWidget(index)) { + QWidget *w = new QWidget(this); + QHBoxLayout *h = new QHBoxLayout(w); + h->setContentsMargins(0, 2, 0, 2); + h->addStretch(1); + + auto remove_btn = create_btn("x", tr("Remove signal")); + auto plot_btn = create_btn("graph-up", ""); + plot_btn->setCheckable(true); + h->addWidget(plot_btn); + h->addWidget(remove_btn); + + tree->setIndexWidget(index, w); + auto sig = model->getItem(index)->sig; + QObject::connect(remove_btn, &QToolButton::clicked, [=]() { model->removeSignal(sig); }); + QObject::connect(plot_btn, &QToolButton::clicked, [=](bool checked) { + emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); + }); + } + } + updateChartState(); } -void SignalEdit::leaveEvent(QEvent *event) { - emit highlight(nullptr); - QWidget::leaveEvent(event); +void SignalView::rowClicked(const QModelIndex &index) { + auto item = model->getItem(index); + if (item->type == SignalModel::Item::Sig) { + auto sig_index = model->index(index.row(), 0, index.parent()); + tree->setExpanded(sig_index, !tree->isExpanded(sig_index)); + } else if (item->type == SignalModel::Item::ExtraInfo) { + model->showExtraInfo(index); + } +} + +void SignalView::expandSignal(const Signal *sig) { + if (int row = model->signalRow(sig); row != -1) { + auto idx = model->index(row, 0); + bool expand = !tree->isExpanded(idx); + tree->setExpanded(idx, expand); + tree->scrollTo(idx, QAbstractItemView::PositionAtTop); + if (expand) tree->setCurrentIndex(idx); + } +} + +void SignalView::updateChartState() { + int i = 0; + for (auto item : model->root->children) { + auto plot_btn = tree->indexWidget(model->index(i, 1))->findChildren()[0]; + bool chart_opened = charts->hasSignal(msg_id, item->sig); + plot_btn->setChecked(chart_opened); + plot_btn->setToolTip(chart_opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened plot")); + ++i; + } +} + +void SignalView::signalHovered(const Signal *sig) { + auto &children = model->root->children; + for (int i = 0; i < children.size(); ++i) { + bool highlight = children[i]->sig == sig; + if (std::exchange(children[i]->highlight, highlight) != highlight) { + emit model->dataChanged(model->index(i, 0), model->index(i, 0)); + } + } } diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index eec943226eeb58..ae88a5b13a8448 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -1,59 +1,97 @@ #pragma once -#include +#include #include #include -#include -#include -#include +#include +#include -#include "selfdrive/ui/qt/widgets/controls.h" -#include "tools/cabana/canmessages.h" +#include "tools/cabana/chartswidget.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" -class SignalForm : public QWidget { +class SignalModel : public QAbstractItemModel { Q_OBJECT public: - SignalForm(QWidget *parent); - QLineEdit *name, *unit, *comment, *val_desc, *offset, *factor, *min_val, *max_val; - QLabel *lsb, *msb; - QSpinBox *size; - QComboBox *sign, *endianness; - bool changed_by_user = false; - - signals: - void changed(); + struct Item { + enum Type {Root, Sig, Name, Size, Endian, Signed, Offset, Factor, ExtraInfo, Unit, Comment, Min, Max, Desc }; + ~Item() { qDeleteAll(children); } + inline int row() { return parent->children.indexOf(this); } + + Type type = Type::Root; + Item *parent = nullptr; + QList children; + + const Signal *sig = nullptr; + QString title; + bool highlight = false; + bool extra_expanded = false; + QString sig_val = "-"; + }; + + SignalModel(QObject *parent); + int rowCount(const QModelIndex &parent = QModelIndex()) const override; + int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 2; } + QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; + QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; + QModelIndex parent(const QModelIndex &index) const override; + Qt::ItemFlags flags(const QModelIndex &index) const override; + bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; + void setMessage(const QString &id); + void setFilter(const QString &txt); + void addSignal(int start_bit, int size, bool little_endian); + bool saveSignal(const Signal *origin_s, Signal &s); + void resizeSignal(const Signal *sig, int start_bit, int size); + void removeSignal(const Signal *sig); + inline Item *getItem(const QModelIndex &index) const { return index.isValid() ? (Item *)index.internalPointer() : root.get(); } + int signalRow(const Signal *sig) const; + void showExtraInfo(const QModelIndex &index); + +private: + void insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig); + void handleSignalAdded(uint32_t address, const Signal *sig); + void handleSignalUpdated(const Signal *sig); + void handleSignalRemoved(const Signal *sig); + void handleMsgChanged(uint32_t address); + void refresh(); + void updateState(const QHash *msgs); + + QString msg_id; + QString filter_str; + std::unique_ptr root; + friend class SignalView; }; -class SignalEdit : public QWidget { +class SignalItemDelegate : public QStyledItemDelegate { +public: + SignalItemDelegate(QObject *parent); + void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QValidator *name_validator, *double_validator; +}; + +class SignalView : public QWidget { Q_OBJECT public: - SignalEdit(int index, QWidget *parent = nullptr); - void setSignal(const QString &msg_id, const Signal *sig); - void setChartOpened(bool opened); + SignalView(ChartsWidget *charts, QWidget *parent); + void setMessage(const QString &id); void signalHovered(const Signal *sig); - void updateForm(bool show); - const Signal *sig = nullptr; - SignalForm *form = nullptr; - QString msg_id; + void updateChartState(); + void expandSignal(const Signal *sig); + void rowClicked(const QModelIndex &index); + SignalModel *model = nullptr; signals: void highlight(const Signal *sig); void showChart(const QString &name, const Signal *sig, bool show, bool merge); - void remove(const Signal *sig); - void save(const Signal *sig, const Signal &new_sig); - void showFormClicked(const Signal *sig); - -protected: - void enterEvent(QEvent *event) override; - void leaveEvent(QEvent *event) override; - void saveSignal(); - - ElidedLabel *title; - QLabel *color_label; - QLabel *icon; - int form_idx = 0; - QToolButton *plot_btn; - QTimer *save_timer; + +private: + void rowsChanged(); + + QString msg_id; + QTreeView *tree; + QLineEdit *filter_edit; + ChartsWidget *charts; + QLabel *signal_count_lb; }; diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc new file mode 100644 index 00000000000000..8a77e0eb395d3a --- /dev/null +++ b/tools/cabana/streams/abstractstream.cc @@ -0,0 +1,94 @@ +#include "tools/cabana/streams/abstractstream.h" + +AbstractStream *can = nullptr; + +AbstractStream::AbstractStream(QObject *parent, bool is_live_streaming) : is_live_streaming(is_live_streaming), QObject(parent) { + can = this; + new_msgs = std::make_unique>(); + QObject::connect(this, &AbstractStream::received, this, &AbstractStream::process, Qt::QueuedConnection); + QObject::connect(this, &AbstractStream::seekedTo, this, &AbstractStream::updateLastMsgsTo); +} + +void AbstractStream::process(QHash *messages) { + for (auto it = messages->begin(); it != messages->end(); ++it) { + can_msgs[it.key()] = it.value(); + } + emit updated(); + emit msgsReceived(messages); + delete messages; + processing = false; +} + +bool AbstractStream::updateEvent(const Event *event) { + static double prev_update_ts = 0; + + if (event->which == cereal::Event::Which::CAN) { + double current_sec = event->mono_time / 1e9 - routeStartTime(); + for (const auto &c : event->event.getCan()) { + QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); + CanData &data = (*new_msgs)[id]; + data.ts = current_sec; + data.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); + data.count = ++counters[id]; + data.freq = data.count / std::max(1.0, current_sec); + change_trackers[id].compute(data.dat, data.ts, data.freq); + data.colors = change_trackers[id].colors; + data.last_change_t = change_trackers[id].last_change_t; + } + + double ts = millis_since_boot(); + if ((ts - prev_update_ts) > (1000.0 / settings.fps) && !processing && !new_msgs->isEmpty()) { + // delay posting CAN message if UI thread is busy + processing = true; + prev_update_ts = ts; + // use pointer to avoid data copy in queued connection. + emit received(new_msgs.release()); + new_msgs.reset(new QHash); + new_msgs->reserve(100); + } + } + return true; +} + +const CanData &AbstractStream::lastMessage(const QString &id) { + static CanData empty_data; + auto it = can_msgs.find(id); + return it != can_msgs.end() ? it.value() : empty_data; +} + +void AbstractStream::updateLastMsgsTo(double sec) { + QHash, CanData> last_msgs; // Much faster than QHash + last_msgs.reserve(can_msgs.size()); + double route_start_time = routeStartTime(); + uint64_t last_ts = (sec + route_start_time) * 1e9; + auto last = std::upper_bound(events()->rbegin(), events()->rend(), last_ts, [](uint64_t ts, auto &e) { return e->mono_time < ts; }); + for (auto it = last; it != events()->rend(); ++it) { + if ((*it)->which == cereal::Event::Which::CAN) { + for (const auto &c : (*it)->event.getCan()) { + auto &m = last_msgs[{c.getSrc(), c.getAddress()}]; + if (++m.count == 1) { + m.ts = ((*it)->mono_time / 1e9) - route_start_time; + m.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); + m.colors = QVector(m.dat.size(), QColor(0, 0, 0, 0)); + m.last_change_t = QVector(m.dat.size(), m.ts); + } else { + m.freq = m.count / std::max(1.0, m.ts); + } + } + } + } + + // it is thread safe to update data here. + // updateEvent will not be called before replayStream::seekedTo return. + new_msgs->clear(); + change_trackers.clear(); + counters.clear(); + can_msgs.clear(); + for (auto it = last_msgs.cbegin(); it != last_msgs.cend(); ++it) { + QString msg_id = QString("%1:%2").arg(it.key().first).arg(it.key().second, 1, 16); + can_msgs[msg_id] = it.value(); + counters[msg_id] = it.value().count; + } + emit updated(); + emit msgsReceived(&can_msgs); +} diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h new file mode 100644 index 00000000000000..0dbb3d96a6474a --- /dev/null +++ b/tools/cabana/streams/abstractstream.h @@ -0,0 +1,70 @@ +#pragma once + +#include + +#include +#include + +#include "tools/cabana/settings.h" +#include "tools/cabana/util.h" +#include "tools/replay/replay.h" + +struct CanData { + double ts = 0.; + uint32_t count = 0; + uint32_t freq = 0; + QByteArray dat; + QVector colors; + QVector last_change_t; +}; + +class AbstractStream : public QObject { + Q_OBJECT + +public: + AbstractStream(QObject *parent, bool is_live_streaming); + virtual ~AbstractStream() {}; + inline bool liveStreaming() const { return is_live_streaming; } + virtual void seekTo(double ts) {} + virtual QString routeName() const = 0; + virtual QString carFingerprint() const { return ""; } + virtual double totalSeconds() const { return 0; } + virtual double routeStartTime() const { return 0; } + virtual double currentSec() const = 0; + virtual QDateTime currentDateTime() const { return {}; } + virtual const CanData &lastMessage(const QString &id); + virtual VisionStreamType visionStreamType() const { return VISION_STREAM_ROAD; } + virtual const Route *route() const { return nullptr; } + virtual const std::vector *events() const = 0; + virtual void setSpeed(float speed) {} + virtual bool isPaused() const { return false; } + virtual void pause(bool pause) {} + virtual const std::vector> getTimeline() { return {}; } + +signals: + void paused(); + void resume(); + void seekedTo(double sec); + void streamStarted(); + void eventsMerged(); + void updated(); + void msgsReceived(const QHash *); + void received(QHash *); + +public: + QHash can_msgs; + +protected: + void process(QHash *); + bool updateEvent(const Event *event); + void updateLastMsgsTo(double sec); + + bool is_live_streaming = false; + std::atomic processing = false; + QHash counters; + std::unique_ptr> new_msgs; + QHash change_trackers; +}; + +// A global pointer referring to the unique AbstractStream object +extern AbstractStream *can; diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc new file mode 100644 index 00000000000000..3edd2a77586e37 --- /dev/null +++ b/tools/cabana/streams/livestream.cc @@ -0,0 +1,103 @@ +#include "tools/cabana/streams/livestream.h" + +LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) { + timer = new QTimer(this); + timer->callOnTimeout(this, &LiveStream::removeExpiredEvents); + timer->start(3 * 1000); + + stream_thread = new QThread(this); + QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); }); + QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater); + stream_thread->start(); +} + +LiveStream::~LiveStream() { + stream_thread->requestInterruption(); + stream_thread->quit(); + stream_thread->wait(); + for (Event *e : can_events) ::delete e; + for (auto m : messages) delete m; +} + +void LiveStream::streamThread() { + if (!zmq_address.isEmpty()) { + setenv("ZMQ", "1", 1); + } + std::unique_ptr context(Context::create()); + std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString(); + std::unique_ptr sock(SubSocket::create(context.get(), "can", address)); + assert(sock != NULL); + sock->setTimeout(50); + // run as fast as messages come in + while (!QThread::currentThread()->isInterruptionRequested()) { + Message *msg = sock->receive(true); + if (!msg) { + QThread::msleep(50); + continue; + } + AlignedBuffer *buf = messages.emplace_back(new AlignedBuffer()); + Event *evt = ::new Event(buf->align(msg)); + delete msg; + + handleEvent(evt); + // TODO: write stream to log file to replay it with cabana --data_dir flag. + } +} + +void LiveStream::handleEvent(Event *evt) { + std::lock_guard lk(lock); + can_events.push_back(evt); + + current_ts = evt->mono_time; + if (start_ts == 0 || current_ts < start_ts) { + if (current_ts < start_ts) { + qDebug() << "stream is looping back to old time stamp"; + } + start_ts = current_ts.load(); + emit streamStarted(); + } + + if (!pause_) { + if (speed_ < 1 && last_update_ts > 0) { + auto it = std::upper_bound(can_events.begin(), can_events.end(), last_update_event_ts, [](uint64_t ts, auto &e) { + return ts < e->mono_time; + }); + + if (it != can_events.end() && + (nanos_since_boot() - last_update_ts) < ((*it)->mono_time - last_update_event_ts) / speed_) { + return; + } + evt = (*it); + } + updateEvent(evt); + last_update_event_ts = evt->mono_time; + last_update_ts = nanos_since_boot(); + } +} + +void LiveStream::removeExpiredEvents() { + std::lock_guard lk(lock); + if (can_events.size() > 0) { + const uint64_t max_ns = settings.max_cached_minutes * 60 * 1e9; + const uint64_t last_ns = can_events.back()->mono_time; + while (!can_events.empty() && (last_ns - can_events.front()->mono_time) > max_ns) { + ::delete can_events.front(); + delete messages.front(); + can_events.pop_front(); + messages.pop_front(); + } + } +} + +const std::vector *LiveStream::events() const { + events_vector.clear(); + std::lock_guard lk(lock); + events_vector.reserve(can_events.size()); + std::copy(can_events.begin(), can_events.end(), std::back_inserter(events_vector)); + return &events_vector; +} + +void LiveStream::pause(bool pause) { + pause_ = pause; + emit paused(); +} diff --git a/tools/cabana/streams/livestream.h b/tools/cabana/streams/livestream.h new file mode 100644 index 00000000000000..ba20032b677bd9 --- /dev/null +++ b/tools/cabana/streams/livestream.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include "tools/cabana/streams/abstractstream.h" + +class LiveStream : public AbstractStream { + Q_OBJECT + +public: + LiveStream(QObject *parent, QString address = {}); + virtual ~LiveStream(); + inline QString routeName() const override { + return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address); + } + inline double routeStartTime() const override { return start_ts / (double)1e9; } + inline double currentSec() const override { return (current_ts - start_ts) / (double)1e9; } + void setSpeed(float speed) override { speed_ = std::min(1.0, speed); } + bool isPaused() const override { return pause_; } + void pause(bool pause) override; + const std::vector *events() const override; + +protected: + virtual void handleEvent(Event *evt); + virtual void streamThread(); + virtual void removeExpiredEvents(); + + mutable std::mutex lock; + mutable std::vector events_vector; + std::deque can_events; + std::deque messages; + std::atomic start_ts = 0; + std::atomic current_ts = 0; + std::atomic speed_ = 1; + std::atomic pause_ = false; + uint64_t last_update_event_ts = 0; + uint64_t last_update_ts = 0; + + const QString zmq_address; + QThread *stream_thread; + QTimer *timer; +}; diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc new file mode 100644 index 00000000000000..72c4a13048c9b5 --- /dev/null +++ b/tools/cabana/streams/replaystream.cc @@ -0,0 +1,48 @@ +#include "tools/cabana/streams/replaystream.h" + +#include "tools/cabana/dbcmanager.h" + +ReplayStream::ReplayStream(QObject *parent) : AbstractStream(parent, false) { + QObject::connect(&settings, &Settings::changed, [this]() { + if (replay) replay->setSegmentCacheLimit(settings.max_cached_minutes); + }); +} + +ReplayStream::~ReplayStream() { + if (replay) replay->stop(); +} + +static bool event_filter(const Event *e, void *opaque) { + return ((ReplayStream *)opaque)->eventFilter(e); +} + +bool ReplayStream::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { + replay = new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this); + replay->setSegmentCacheLimit(settings.max_cached_minutes); + replay->installEventFilter(event_filter, this); + QObject::connect(replay, &Replay::seekedTo, this, &AbstractStream::seekedTo); + QObject::connect(replay, &Replay::segmentsMerged, this, &AbstractStream::eventsMerged); + QObject::connect(replay, &Replay::streamStarted, this, &AbstractStream::streamStarted); + if (replay->load()) { + const auto &segments = replay->route()->segments(); + if (std::none_of(segments.begin(), segments.end(), [](auto &s) { return s.second.rlog.length() > 0; })) { + qWarning() << "no rlogs in route" << route; + return false; + } + replay->start(); + return true; + } + return false; +} + +bool ReplayStream::eventFilter(const Event *event) { + if (event->which == cereal::Event::Which::CAN) { + updateEvent(event); + } + return true; +} + +void ReplayStream::pause(bool pause) { + replay->pause(pause); + emit(pause ? paused() : resume()); +} diff --git a/tools/cabana/streams/replaystream.h b/tools/cabana/streams/replaystream.h new file mode 100644 index 00000000000000..a9a74e33b56b93 --- /dev/null +++ b/tools/cabana/streams/replaystream.h @@ -0,0 +1,32 @@ +#pragma once + +#include "opendbc/can/common_dbc.h" +#include "tools/cabana/streams/abstractstream.h" +#include "tools/cabana/settings.h" + +class ReplayStream : public AbstractStream { + Q_OBJECT + +public: + ReplayStream(QObject *parent); + ~ReplayStream(); + bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); + bool eventFilter(const Event *event); + 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(); } + inline VisionStreamType visionStreamType() const override { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } + inline double totalSeconds() const override { return replay->totalSeconds(); } + inline double routeStartTime() const override { return replay->routeStartTime() / (double)1e9; } + inline double currentSec() const override { return replay->currentSeconds(); } + inline QDateTime currentDateTime() const override { return replay->currentDateTime(); } + inline const Route *route() const override { return replay->route(); } + inline const std::vector *events() const override { return replay->events(); } + inline void setSpeed(float speed) override { replay->setSpeed(speed); } + inline bool isPaused() const override { return replay->isPaused(); } + void pause(bool pause) override; + inline const std::vector> getTimeline() override { return replay->getTimeline(); } + +private: + Replay *replay = nullptr; +}; diff --git a/tools/cabana/tools/findsimilarbits.cc b/tools/cabana/tools/findsimilarbits.cc index 5fa5bcf7b82cd6..63d01b152d8f86 100644 --- a/tools/cabana/tools/findsimilarbits.cc +++ b/tools/cabana/tools/findsimilarbits.cc @@ -7,11 +7,13 @@ #include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" -FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent) { +FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent, Qt::WindowFlags() | Qt::Window) { setWindowTitle(tr("Find similar bits")); + setAttribute(Qt::WA_DeleteOnClose); + QVBoxLayout *main_layout = new QVBoxLayout(this); QHBoxLayout *form_layout = new QHBoxLayout(); @@ -25,14 +27,28 @@ FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent) { } bus_combo->model()->sort(0); bus_combo->setCurrentIndex(0); + + msg_cb = new QComboBox(this); + for (auto &[address, msg] : dbc()->messages()) { + msg_cb->addItem(msg.name, address); + } + msg_cb->model()->sort(0); + msg_cb->setCurrentIndex(0); + + byte_idx_sb = new QSpinBox(this); + byte_idx_sb->setRange(0, 63); + + bit_idx_sb = new QSpinBox(this); + bit_idx_sb->setRange(0, 7); + form_layout->addWidget(new QLabel("Bus")); form_layout->addWidget(bus_combo); + form_layout->addWidget(msg_cb); + form_layout->addWidget(new QLabel("Byte Index")); + form_layout->addWidget(byte_idx_sb); + form_layout->addWidget(new QLabel("Bit Index")); + form_layout->addWidget(bit_idx_sb); - bit_combo = new QComboBox(this); - bit_combo->addItems({"0", "1"}); - bit_combo->setCurrentIndex(1); - form_layout->addWidget(new QLabel("Bit")); - form_layout->addWidget(bit_combo); min_msgs = new QLineEdit(this); min_msgs->setValidator(new QIntValidator(this)); @@ -45,21 +61,29 @@ FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent) { main_layout->addLayout(form_layout); table = new QTableWidget(this); + table->setSelectionBehavior(QAbstractItemView::SelectRows); + table->setSelectionMode(QAbstractItemView::SingleSelection); table->setEditTriggers(QAbstractItemView::NoEditTriggers); table->horizontalHeader()->setStretchLastSection(true); main_layout->addWidget(table); setMinimumSize({700, 500}); QObject::connect(search_btn, &QPushButton::clicked, this, &FindSimilarBitsDlg::find); + QObject::connect(table, &QTableWidget::doubleClicked, [this](const QModelIndex &index) { + if (index.isValid()) { + emit openMessage(bus_combo->currentText() + ":" + table->item(index.row(), 0)->text()); + } + }); } void FindSimilarBitsDlg::find() { search_btn->setEnabled(false); table->clear(); - auto msg_mismatched = calcBits(bus_combo->currentText().toUInt(), bit_combo->currentIndex(), min_msgs->text().toInt()); + uint32_t selected_address = msg_cb->currentData().toUInt(); + auto msg_mismatched = calcBits(bus_combo->currentText().toUInt(), selected_address, byte_idx_sb->value(), bit_idx_sb->value(), min_msgs->text().toInt()); table->setRowCount(msg_mismatched.size()); table->setColumnCount(6); - table->setHorizontalHeaderLabels({"address", "byte idx", "bit idx", "mismatches", "total", "perc%"}); + table->setHorizontalHeaderLabels({"address", "byte idx", "bit idx", "mismatches", "total msgs", "% mismatched"}); for (int i = 0; i < msg_mismatched.size(); ++i) { auto &m = msg_mismatched[i]; table->setItem(i, 0, new QTableWidgetItem(QString("%1").arg(m.address, 1, 16))); @@ -72,18 +96,24 @@ void FindSimilarBitsDlg::find() { search_btn->setEnabled(true); } -QList FindSimilarBitsDlg::calcBits(uint8_t bus, int bit_to_find, int min_msgs_cnt) { +QList FindSimilarBitsDlg::calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt) { QHash> mismatches; QHash msg_count; auto events = can->events(); + int bit_to_find = -1; for (auto e : *events) { if (e->which == cereal::Event::Which::CAN) { for (const auto &c : e->event.getCan()) { if (c.getSrc() == bus) { + const auto dat = c.getDat(); uint32_t address = c.getAddress(); + if (address == selected_address && dat.size() > byte_idx) { + bit_to_find = ((dat[byte_idx] >> (7 - bit_idx)) & 1) != 0; + } ++msg_count[address]; + if (bit_to_find == -1) continue; + auto &mismatched = mismatches[address]; - const auto dat = c.getDat(); if (mismatched.size() < dat.size() * 8) { mismatched.resize(dat.size() * 8); } @@ -104,12 +134,12 @@ QList FindSimilarBitsDlg::calcBits(uint8_ if (auto cnt = msg_count[it.key()]; cnt > min_msgs_cnt) { auto &mismatched = it.value(); for (int i = 0; i < mismatched.size(); ++i) { - if (uint32_t perc = (mismatched[i] / (double)cnt) * 100; perc < 50) { + if (float perc = (mismatched[i] / (double)cnt) * 100; perc < 50) { result.push_back({it.key(), (uint32_t)i / 8, (uint32_t)i % 8, mismatched[i], cnt, perc}); } } } } - std::sort(result.begin(), result.end(), [](auto &l, auto &r) { return l.perc > r.perc; }); + std::sort(result.begin(), result.end(), [](auto &l, auto &r) { return l.perc < r.perc; }); return result; } diff --git a/tools/cabana/tools/findsimilarbits.h b/tools/cabana/tools/findsimilarbits.h index 79db4a1c69a081..30d78f0dea119c 100644 --- a/tools/cabana/tools/findsimilarbits.h +++ b/tools/cabana/tools/findsimilarbits.h @@ -3,21 +3,29 @@ #include #include #include +#include #include class FindSimilarBitsDlg : public QDialog { + Q_OBJECT + public: FindSimilarBitsDlg(QWidget *parent); +signals: + void openMessage(const QString &msg_id); + private: struct mismatched_struct { - uint32_t address, byte_idx, bit_idx, mismatches, total, perc; + uint32_t address, byte_idx, bit_idx, mismatches, total; + float perc; }; - QList calcBits(uint8_t bus, int bit_to_find, int min_msgs_cnt); + QList calcBits(uint8_t bus, uint32_t selected_address, int byte_idx, int bit_idx, int min_msgs_cnt); void find(); QTableWidget *table; - QComboBox *bus_combo, *bit_combo; + QComboBox *bus_combo, *msg_cb; + QSpinBox *byte_idx_sb, *bit_idx_sb; QPushButton *search_btn; QLineEdit *min_msgs; }; diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc new file mode 100644 index 00000000000000..f9d19dfda3ab01 --- /dev/null +++ b/tools/cabana/util.cc @@ -0,0 +1,118 @@ +#include "tools/cabana/util.h" + +#include +#include +#include + +#include "selfdrive/ui/qt/util.h" + +static QColor blend(QColor a, 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 ChangeTracker::compute(const QByteArray &dat, double ts, uint32_t freq) { + if (prev_dat.size() != dat.size()) { + colors.resize(dat.size()); + last_change_t.resize(dat.size()); + std::fill(colors.begin(), colors.end(), QColor(0, 0, 0, 0)); + std::fill(last_change_t.begin(), last_change_t.end(), ts); + } else { + for (int i = 0; i < dat.size(); ++i) { + const uint8_t last = prev_dat[i]; + const uint8_t cur = dat[i]; + + if (last != cur) { + double delta_t = ts - last_change_t[i]; + if (delta_t * freq > periodic_threshold) { + // Last change was while ago, choose color based on delta up or down + if (cur > last) { + colors[i] = QColor(0, 187, 255, start_alpha); // Cyan + } else { + colors[i] = QColor(255, 0, 0, start_alpha); // Red + } + } else { + // Periodic changes + colors[i] = blend(colors[i], QColor(102, 86, 169, start_alpha / 2)); // Greyish/Blue + } + + last_change_t[i] = ts; + } else { + // Fade out + float alpha_delta = 1.0 / (freq + 1) / fade_time; + colors[i].setAlphaF(std::max(0.0, colors[i].alphaF() - alpha_delta)); + } + } + } + + prev_dat = dat; +} + +void ChangeTracker::clear() { + prev_dat.clear(); + last_change_t.clear(); + colors.clear(); +} + +QList ChangeTracker::toVariantList(const QVector &colors) { + QList ret; + ret.reserve(colors.size()); + for (auto &c : colors) ret.append(c); + return ret; +} + +// MessageBytesDelegate + +MessageBytesDelegate::MessageBytesDelegate(QObject *parent) : QStyledItemDelegate(parent) { + fixed_font = QFontDatabase::systemFont(QFontDatabase::FixedFont); +} + +void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + QStyleOptionViewItemV4 opt = option; + initStyleOption(&opt, index); + + if ((option.state & QStyle::State_Selected) && (option.state & QStyle::State_Active)) { + painter->setPen(option.palette.color(QPalette::HighlightedText)); + } else { + painter->setPen(option.palette.color(QPalette::Text)); + } + + painter->setFont(fixed_font); + QRect space = painter->boundingRect(opt.rect, opt.displayAlignment, " "); + QRect pos = painter->boundingRect(opt.rect, opt.displayAlignment, "00"); + pos.moveLeft(pos.x() + space.width()); + + int m = space.width() / 2; + const QMargins margins(m, m, m, m); + + QList colors = index.data(Qt::UserRole).toList(); + int i = 0; + for (auto &byte : opt.text.split(" ")) { + if (i < colors.size()) { + painter->fillRect(pos.marginsAdded(margins), colors[i].value()); + } + painter->drawText(pos, opt.displayAlignment, byte); + pos.moveLeft(pos.right() + space.width()); + i++; + } +} + +NameValidator::NameValidator(QObject *parent) : QRegExpValidator(QRegExp("^(\\w+)"), parent) { } + +QValidator::State NameValidator::validate(QString &input, int &pos) const { + input.replace(' ', '_'); + return QRegExpValidator::validate(input, pos); +} + +namespace utils { +QPixmap icon(const QString &id) { + static bool dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > + QApplication::style()->standardPalette().color(QPalette::Background).value(); + QPixmap pm = bootstrapPixmap(id); + if (dark_theme) { + QPainter p(&pm); + p.setCompositionMode(QPainter::CompositionMode_SourceIn); + p.fillRect(pm.rect(), Qt::lightGray); + } + return pm; +} +} // namespace utils diff --git a/tools/cabana/util.h b/tools/cabana/util.h new file mode 100644 index 00000000000000..b76652f4b00b80 --- /dev/null +++ b/tools/cabana/util.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +class ChangeTracker { +public: + void compute(const QByteArray &dat, double ts, uint32_t freq); + static QList toVariantList(const QVector &colors); + void clear(); + + QVector last_change_t; + QVector colors; + +private: + const int periodic_threshold = 10; + const int start_alpha = 128; + const float fade_time = 2.0; + QByteArray prev_dat; +}; + +class MessageBytesDelegate : public QStyledItemDelegate { + Q_OBJECT +public: + MessageBytesDelegate(QObject *parent); + void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QFont fixed_font; +}; + +inline QString toHex(const QByteArray &dat) { return dat.toHex(' ').toUpper(); } +inline char toHex(uint value) { return "0123456789ABCDEF"[value & 0xF]; } +inline const QString &getColor(int i) { + // TODO: add more colors + static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; + return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)]; +} + +class NameValidator : public QRegExpValidator { + Q_OBJECT + +public: + NameValidator(QObject *parent=nullptr); + QValidator::State validate(QString &input, int &pos) const override; +}; + +namespace utils { +QPixmap icon(const QString &id); +} diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 7e40ba2adbad72..d8d89a708cec87 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -3,11 +3,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -17,43 +17,28 @@ inline QString formatTime(int seconds) { return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss"); } -VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) { - setFrameShape(QFrame::StyledPanel); - setFrameShadow(QFrame::Sunken); - QHBoxLayout *containter_layout = new QHBoxLayout(this); - QVBoxLayout *main_layout = new QVBoxLayout(); - main_layout->setContentsMargins(0, 0, 0, 0); +VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + QFrame *frame = new QFrame(this); + frame->setFrameShape(QFrame::StyledPanel); + frame->setFrameShadow(QFrame::Sunken); + main_layout->addWidget(frame); - containter_layout->addStretch(1); - containter_layout->addLayout(main_layout); - containter_layout->addStretch(1); - - cam_widget = new CameraWidget("camerad", can->visionStreamType(), false, this); - cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); - main_layout->addWidget(cam_widget); - - // slider controls - QHBoxLayout *slider_layout = new QHBoxLayout(); - QLabel *time_label = new QLabel("00:00"); - slider_layout->addWidget(time_label); - - slider = new Slider(this); - slider->setSingleStep(0); - slider_layout->addWidget(slider); - - end_time_label = new QLabel(this); - slider_layout->addWidget(end_time_label); - main_layout->addLayout(slider_layout); + QVBoxLayout *frame_layout = new QVBoxLayout(frame); + if (!can->liveStreaming()) { + frame_layout->addWidget(createCameraWidget()); + } // btn controls QHBoxLayout *control_layout = new QHBoxLayout(); - play_btn = new QPushButton("⏸"); - play_btn->setStyleSheet("font-weight:bold; height:16px"); + play_btn = new QPushButton(); control_layout->addWidget(play_btn); QButtonGroup *group = new QButtonGroup(this); group->setExclusive(true); for (float speed : {0.1, 0.5, 1., 2.}) { + if (can->liveStreaming() && speed > 1) continue; + QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); btn->setCheckable(true); QObject::connect(btn, &QPushButton::clicked, [=]() { can->setSpeed(speed); }); @@ -61,25 +46,74 @@ VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) { group->addButton(btn); if (speed == 1.0) btn->setChecked(true); } - main_layout->addLayout(control_layout); + frame_layout->addLayout(control_layout); + + QObject::connect(play_btn, &QPushButton::clicked, []() { can->pause(!can->isPaused()); }); + QObject::connect(can, &AbstractStream::paused, this, &VideoWidget::updatePlayBtnState); + QObject::connect(can, &AbstractStream::resume, this, &VideoWidget::updatePlayBtnState); + updatePlayBtnState(); +} + +QWidget *VideoWidget::createCameraWidget() { + QWidget *w = new QWidget(this); + QVBoxLayout *l = new QVBoxLayout(w); + l->setContentsMargins(0, 0, 0, 0); + cam_widget = new CameraWidget("camerad", can->visionStreamType(), false); + l->addWidget(cam_widget); + + // slider controls + slider_layout = new QHBoxLayout(); + time_label = new ElidedLabel("00:00"); + time_label->setToolTip(tr("Click to set current time")); + slider_layout->addWidget(time_label); - QObject::connect(can, &CANMessages::updated, this, &VideoWidget::updateState); + slider = new Slider(this); + slider->setSingleStep(0); + slider_layout->addWidget(slider); + + end_time_label = new QLabel(this); + slider_layout->addWidget(end_time_label); + l->addLayout(slider_layout); + + cam_widget->setMinimumHeight(100); + cam_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum); + + QObject::connect(time_label, &ElidedLabel::clicked, this, &VideoWidget::timeLabelClicked); QObject::connect(slider, &QSlider::sliderReleased, [this]() { can->seekTo(slider->value() / 1000.0); }); QObject::connect(slider, &QSlider::valueChanged, [=](int value) { time_label->setText(formatTime(value / 1000)); }); - QObject::connect(cam_widget, &CameraWidget::clicked, [this]() { pause(!can->isPaused()); }); - QObject::connect(play_btn, &QPushButton::clicked, [=]() { pause(!can->isPaused()); }); - QObject::connect(can, &CANMessages::streamStarted, [this]() { + QObject::connect(cam_widget, &CameraWidget::clicked, []() { can->pause(!can->isPaused()); }); + QObject::connect(can, &AbstractStream::updated, this, &VideoWidget::updateState); + QObject::connect(can, &AbstractStream::streamStarted, [this]() { end_time_label->setText(formatTime(can->totalSeconds())); slider->setRange(0, can->totalSeconds() * 1000); }); + return w; } -void VideoWidget::pause(bool pause) { - play_btn->setText(!pause ? "⏸" : "▶"); - can->pause(pause); +void VideoWidget::timeLabelClicked() { + auto time_edit = new QTimeEdit(this); + auto init_date_time = can->currentDateTime(); + time_edit->setDateTime(init_date_time); + time_edit->setDisplayFormat("hh:mm:ss"); + time_label->setVisible(false); + slider_layout->insertWidget(0, time_edit); + QTimer::singleShot(0, [=]() { time_edit->setFocus(); }); + + QObject::connect(time_edit, &QTimeEdit::editingFinished, [=]() { + if (time_edit->dateTime() != init_date_time) { + int seconds = can->route()->datetime().secsTo(time_edit->dateTime()); + can->seekTo(seconds); + } + time_edit->setVisible(false); + time_label->setVisible(true); + time_edit->deleteLater(); + }); } void VideoWidget::rangeChanged(double min, double max, bool is_zoomed) { + if (can->liveStreaming()) return; + if (!is_zoomed) { min = 0; max = can->totalSeconds(); @@ -93,6 +127,11 @@ void VideoWidget::updateState() { slider->setValue(can->currentSec() * 1000); } +void VideoWidget::updatePlayBtnState() { + play_btn->setIcon(utils::icon(can->isPaused() ? "play" : "pause")); + play_btn->setToolTip(can->isPaused() ? tr("Play") : tr("Pause")); +} + // Slider Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { QTimer *timer = new QTimer(this); @@ -104,7 +143,12 @@ Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { setMouseTracking(true); QObject::connect(can, SIGNAL(streamStarted()), timer, SLOT(start())); - QObject::connect(can, &CANMessages::streamStarted, this, &Slider::streamStarted); + QObject::connect(can, &AbstractStream::streamStarted, this, &Slider::streamStarted); +} + +Slider::~Slider() { + abort_load_thumbnail = true; + thumnail_future.waitForFinished(); } void Slider::streamStarted() { diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index 86cdc6f114a7ca..51197eedd64508 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -3,19 +3,22 @@ #include #include +#include #include #include #include #include #include "selfdrive/ui/qt/widgets/cameraview.h" -#include "tools/cabana/canmessages.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/streams/abstractstream.h" class Slider : public QSlider { Q_OBJECT public: Slider(QWidget *parent); + ~Slider(); private: void mousePressEvent(QMouseEvent *e) override; @@ -35,7 +38,7 @@ class Slider : public QSlider { QSize thumbnail_size = {}; }; -class VideoWidget : public QFrame { +class VideoWidget : public QWidget { Q_OBJECT public: @@ -44,10 +47,14 @@ class VideoWidget : public QFrame { protected: void updateState(); - void pause(bool pause); + void updatePlayBtnState(); + void timeLabelClicked(); + QWidget *createCameraWidget(); CameraWidget *cam_widget; QLabel *end_time_label; + ElidedLabel *time_label; + QHBoxLayout *slider_layout; QPushButton *play_btn; Slider *slider; }; diff --git a/tools/gpstest/README.md b/tools/gpstest/README.md index 5aff0ee3d7ced1..01f44df0ce4538 100644 --- a/tools/gpstest/README.md +++ b/tools/gpstest/README.md @@ -3,12 +3,9 @@ Testing the GPS receiver using GPS spoofing. At the moment only static location relpay is supported. # Usage -``` -# on host, start gps signal simulation -./run_static_lime.py -``` +on C3 run `rpc_server.py`, on host PC run `fuzzy_testing.py` -`run_static_lime.py` downloads the latest ephemeris file from +`simulate_gps_signal.py` downloads the latest ephemeris file from https://cddis.nasa.gov/archive/gnss/data/daily/20xx/brdc/. diff --git a/tools/gpstest/fuzzy_testing.py b/tools/gpstest/fuzzy_testing.py index bd204e7ae7be17..a2e130342c0890 100755 --- a/tools/gpstest/fuzzy_testing.py +++ b/tools/gpstest/fuzzy_testing.py @@ -1,147 +1,115 @@ #!/usr/bin/env python3 -import sys -import time -import random -import datetime as dt -import subprocess as sp +import argparse import multiprocessing -import threading -from typing import Tuple, Any +import rpyc # pylint: disable=import-error +from collections import defaultdict -from laika.downloader import download_nav -from laika.gps_time import GPSTime -from laika.helpers import ConstellationId +from helper import download_rinex, exec_LimeGPS_bin +from helper import get_random_coords, get_continuous_coords -cache_dir = '/tmp/gpstest/' +#------------------------------------------------------------------------------ +# this script is supposed to run on HOST PC +# limeSDR is unreliable via c3 USB +#------------------------------------------------------------------------------ -def download_rinex(): - # TODO: check if there is a better way to get the full brdc file for LimeGPS - gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) - utc_time = dt.datetime.utcnow() - dt.timedelta(1) - gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) - return download_nav(gps_time, cache_dir, ConstellationId.GPS) - - -def exec_LimeGPS_bin(rinex_file: str, location: str, duration: int): - # this functions should never return, cause return means, timeout is - # reached or it crashed - try: - cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", location] - sp.check_output(cmd, timeout=duration) - except sp.TimeoutExpired: - print("LimeGPS timeout reached!") - except Exception as e: - print(f"LimeGPS crashed: {str(e)}") - - -def run_lime_gps(rinex_file: str, location: str, duration: int): - print(f"LimeGPS {location} {duration}") - +def run_lime_gps(rinex_file: str, location: str, timeout: int): + # needs to run longer than the checker + timeout += 10 + print(f"LimeGPS {location} {timeout}") p = multiprocessing.Process(target=exec_LimeGPS_bin, - args=(rinex_file, location, duration)) + args=(rinex_file, location, timeout)) p.start() return p +con = None +def run_remote_checker(lat, lon, alt, duration, ip_addr): + global con + try: + con = rpyc.connect(ip_addr, 18861) + con._config['sync_request_timeout'] = duration+20 + except ConnectionRefusedError: + print("could not run remote checker is 'rpc_server.py' running???") + return False, None, None -def get_random_coords(lat, lon) -> Tuple[int, int]: - # jump around the world - # max values, lat: -90 to 90, lon: -180 to 180 - - lat_add = random.random()*20 + 10 - lon_add = random.random()*20 + 20 - - lat = ((lat + lat_add + 90) % 180) - 90 - lon = ((lon + lon_add + 180) % 360) - 180 - return round(lat, 5), round(lon, 5) - -def get_continuous_coords(lat, lon) -> Tuple[int, int]: - # continuously move around the world - - lat_add = random.random()*0.01 - lon_add = random.random()*0.01 - - lat = ((lat + lat_add + 90) % 180) - 90 - lon = ((lon + lon_add + 180) % 360) - 180 - return round(lat, 5), round(lon, 5) - -rc_p: Any = None -def exec_remote_checker(lat, lon, duration, ip_addr): - global rc_p - # TODO: good enough for testing - remote_cmd = "export PYTHONPATH=/data/pythonpath && " - remote_cmd += "cd /data/openpilot && " - remote_cmd += f"timeout {duration} /usr/local/pyenv/shims/python tools/gpstest/remote_checker.py " - remote_cmd += f"{lat} {lon}" - - ssh_cmd = ["ssh", "-i", "/home/batman/openpilot/xx/phone/key/id_rsa", - f"comma@{ip_addr}"] - ssh_cmd += [remote_cmd] - - rc_p = sp.Popen(ssh_cmd, stdout=sp.PIPE) - rc_p.wait() - rc_output = rc_p.stdout.read() - print(f"Checker Result: {rc_output.strip().decode('utf-8')}") + matched, log, info = con.root.exposed_run_checker(lat, lon, alt, + timeout=duration, + use_laikad=True) + con.close() # TODO: might wanna fetch more logs here + con = None + print(f"Remote Checker: {log} {info}") + return matched, log, info -def run_remote_checker(spoof_proc, lat, lon, duration, ip_addr) -> bool: - checker_thread = threading.Thread(target=exec_remote_checker, - args=(lat, lon, duration, ip_addr)) - checker_thread.start() - tcnt = 0 - while True: - if not checker_thread.is_alive(): - # assume this only happens when the signal got matched - return True +stats = defaultdict(int) # type: ignore +keys = ['success', 'failed', 'ublox_fail', 'laikad_fail', 'proc_crash', 'checker_crash'] - # the spoofing process has a timeout, kill checker if reached - if not spoof_proc.is_alive(): - rc_p.kill() - # spoofing process died, assume timeout - print("Spoofing process timeout") - return False +def print_report(): + print("\nFuzzy testing report summary:") + for k in keys: + print(f" {k}: {stats[k]}") - print(f"Time elapsed: {tcnt}[s]", end = "\r") - time.sleep(1) - tcnt += 1 +def update_stats(matched, log, info): + if matched: + stats['success'] += 1 + return -def main(): - if len(sys.argv) < 2: - print(f"usage: {sys.argv[0]} [-c]") - ip_addr = sys.argv[1] + stats['failed'] += 1 + if log == "PROC CRASH": + stats['proc_crash'] += 1 + if log == "CHECKER CRASHED": + stats['checker_crash'] += 1 + if log == "TIMEOUT": + if "LAIKAD" in info: + stats['laikad_fail'] += 1 + else: # "UBLOX" in info + stats['ublox_fail'] += 1 - continuous_mode = False - if len(sys.argv) == 3 and sys.argv[2] == '-c': - print("Continuous Mode!") - continuous_mode = True +def main(ip_addr, continuous_mode, timeout, pos): rinex_file = download_rinex() - duration = 60*3 # max runtime in seconds - lat, lon = get_random_coords(47.2020, 15.7403) + lat, lon, alt = pos + if lat == 0 and lon == 0 and alt == 0: + lat, lon, alt = get_random_coords(47.2020, 15.7403) + + try: + while True: + # spoof random location + spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},{alt}", timeout) - while True: - # spoof random location - spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},100", duration) - start_time = time.monotonic() + # remote checker execs blocking + matched, log, info = run_remote_checker(lat, lon, alt, timeout, ip_addr) + update_stats(matched, log, info) + spoof_proc.terminate() + spoof_proc = None - # remote checker runs blocking - if not run_remote_checker(spoof_proc, lat, lon, duration, ip_addr): - # location could not be matched by ublox module - pass + if continuous_mode: + lat, lon, alt = get_continuous_coords(lat, lon, alt) + else: + lat, lon, alt = get_random_coords(lat, lon) + except KeyboardInterrupt: + if spoof_proc is not None: + spoof_proc.terminate() - end_time = time.monotonic() - spoof_proc.terminate() + if con is not None and not con.closed: + con.root.exposed_kill_procs() + con.close() - # -1 to count process startup - print(f"Time to get Signal: {round(end_time - start_time - 1, 4)}") + print_report() - if continuous_mode: - lat, lon = get_continuous_coords(lat, lon) - else: - lat, lon = get_random_coords(lat, lon) if __name__ == "__main__": - main() + parser = argparse.ArgumentParser(description="Fuzzy test GPS stack with random locations.") + parser.add_argument("ip_addr", type=str) + parser.add_argument("-c", "--contin", type=bool, nargs='?', default=False, help='Continous location change') + parser.add_argument("-t", "--timeout", type=int, nargs='?', default=180, help='Timeout to get location') + + # for replaying a location + parser.add_argument("lat", type=float, nargs='?', default=0) + parser.add_argument("lon", type=float, nargs='?', default=0) + parser.add_argument("alt", type=float, nargs='?', default=0) + args = parser.parse_args() + main(args.ip_addr, args.contin, args.timeout, (args.lat, args.lon, args.alt)) diff --git a/tools/gpstest/helper.py b/tools/gpstest/helper.py new file mode 100644 index 00000000000000..4f62e60db021c9 --- /dev/null +++ b/tools/gpstest/helper.py @@ -0,0 +1,53 @@ +import random +import datetime as dt +import subprocess as sp +from typing import Tuple + +from laika.downloader import download_nav +from laika.gps_time import GPSTime +from laika.helpers import ConstellationId + + +def download_rinex(): + # TODO: check if there is a better way to get the full brdc file for LimeGPS + gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) + utc_time = dt.datetime.utcnow() - dt.timedelta(1) + gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) + return download_nav(gps_time, '/tmp/gpstest/', ConstellationId.GPS) + + +def exec_LimeGPS_bin(rinex_file: str, location: str, duration: int): + # this functions should never return, cause return means, timeout is + # reached or it crashed + try: + cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", location] + sp.check_output(cmd, timeout=duration) + except sp.TimeoutExpired: + print("LimeGPS timeout reached!") + except Exception as e: + print(f"LimeGPS crashed: {str(e)}") + + +def get_random_coords(lat, lon) -> Tuple[float, float, int]: + # jump around the world + # max values, lat: -90 to 90, lon: -180 to 180 + + lat_add = random.random()*20 + 10 + lon_add = random.random()*20 + 20 + alt = random.randint(-10**3, 4*10**3) + + lat = ((lat + lat_add + 90) % 180) - 90 + lon = ((lon + lon_add + 180) % 360) - 180 + return round(lat, 5), round(lon, 5), alt + + +def get_continuous_coords(lat, lon, alt) -> Tuple[float, float, int]: + # continuously move around the world + lat_add = random.random()*0.01 + lon_add = random.random()*0.01 + alt_add = random.randint(-100, 100) + + lat = ((lat + lat_add + 90) % 180) - 90 + lon = ((lon + lon_add + 180) % 360) - 180 + alt += alt_add + return round(lat, 5), round(lon, 5), alt diff --git a/tools/gpstest/remote_checker.py b/tools/gpstest/remote_checker.py deleted file mode 100644 index a649a105c341c1..00000000000000 --- a/tools/gpstest/remote_checker.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python3 -import sys -import time -from typing import List - -from common.params import Params -import cereal.messaging as messaging -from selfdrive.manager.process_config import managed_processes - -DELTA = 0.001 -# assume running openpilot for now -procs: List[str] = []#"ubloxd", "pigeond"] - - -def main(): - if len(sys.argv) != 4: - print("args: ") - return - - quectel_mod = Params().get_bool("UbloxAvailable") - sol_lat = float(sys.argv[2]) - sol_lon = float(sys.argv[3]) - - for p in procs: - managed_processes[p].start() - time.sleep(0.5) # give time to startup - - socket = 'gpsLocation' if quectel_mod else 'gpsLocationExternal' - gps_sock = messaging.sub_sock(socket, timeout=0.1) - - # analyze until the location changed - while True: - events = messaging.drain_sock(gps_sock) - for e in events: - loc = e.gpsLocation if quectel_mod else e.gpsLocationExternal - lat = loc.latitude - lon = loc.longitude - - if abs(lat - sol_lat) < DELTA and abs(lon - sol_lon) < DELTA: - print("MATCH") - return - - time.sleep(0.1) - - for p in procs: - if not managed_processes[p].proc.is_alive(): - print(f"ERROR: '{p}' died") - return - - -if __name__ == "__main__": - main() - for p in procs: - managed_processes[p].stop() diff --git a/tools/gpstest/rpc_server.py b/tools/gpstest/rpc_server.py new file mode 100644 index 00000000000000..b35c66d02dac11 --- /dev/null +++ b/tools/gpstest/rpc_server.py @@ -0,0 +1,185 @@ +import os +import time +import shutil +from datetime import datetime +from collections import defaultdict + +import rpyc # pylint: disable=import-error +from rpyc.utils.server import ThreadedServer # pylint: disable=import-error + +#from common.params import Params +import cereal.messaging as messaging +from selfdrive.manager.process_config import managed_processes +from laika.lib.coordinates import ecef2geodetic + +DELTA = 0.001 +ALT_DELTA = 30 +MATCH_NUM = 10 +REPORT_STATS = 10 + +EPHEM_CACHE = "/data/params/d/LaikadEphemeris" +DOWNLOAD_CACHE = "/tmp/comma_download_cache" + +SERVER_LOG_FILE = "/tmp/fuzzy_server.log" +server_log = open(SERVER_LOG_FILE, "w+") + +def slog(msg): + server_log.write(f"{datetime.now().strftime('%H:%M:%S.%f')} | {msg}\n") + server_log.flush() + +def handle_laikad(msg): + if not hasattr(msg, 'correctedMeasurements'): + return None + + num_corr = len(msg.correctedMeasurements) + pos_ecef = msg.positionECEF.value + pos_geo = [] + if len(pos_ecef) > 0: + pos_geo = ecef2geodetic(pos_ecef) + + pos_std = msg.positionECEF.std + pos_valid = msg.positionECEF.valid + + slog(f"{num_corr} {pos_geo} {pos_ecef} {pos_std} {pos_valid}") + return pos_geo, (num_corr, pos_geo, list(pos_ecef), list(msg.positionECEF.std)) + +hw_msgs = 0 +ephem_msgs: dict = defaultdict(int) +def handle_ublox(msg): + global hw_msgs + + d = msg.to_dict() + + if 'hwStatus2' in d: + hw_msgs += 1 + + if 'ephemeris' in d: + ephem_msgs[msg.ephemeris.svId] += 1 + + num_meas = None + if 'measurementReport' in d: + num_meas = msg.measurementReport.numMeas + + return [hw_msgs, ephem_msgs, num_meas] + + +def start_procs(procs): + for p in procs: + managed_processes[p].start() + time.sleep(1) + +def kill_procs(procs, no_retry=False): + for p in procs: + managed_processes[p].stop() + time.sleep(1) + + if not no_retry: + for p in procs: + mp = managed_processes[p].proc + if mp is not None and mp.is_alive(): + managed_processes[p].stop() + time.sleep(3) + +def check_alive_procs(procs): + for p in procs: + mp = managed_processes[p].proc + if mp is None or not mp.is_alive(): + return False, p + return True, None + + +class RemoteCheckerService(rpyc.Service): + def on_connect(self, conn): + pass + + def on_disconnect(self, conn): + #kill_procs(self.procs, no_retry=False) + # this execution is delayed, it will kill the next run of laikad + # TODO: add polling to wait for everything is killed + pass + + def run_checker(self, slat, slon, salt, sockets, procs, timeout): + global hw_msgs, ephem_msgs + hw_msgs = 0 + ephem_msgs = defaultdict(int) + + slog(f"Run test: {slat} {slon} {salt}") + + # quectel_mod = Params().get_bool("UbloxAvailable") + + match_cnt = 0 + msg_cnt = 0 + stats_laikad = [] + stats_ublox = [] + + self.procs = procs + start_procs(procs) + sm = messaging.SubMaster(sockets) + + start_time = time.monotonic() + while True: + sm.update() + + if sm.updated['ubloxGnss']: + stats_ublox.append(handle_ublox(sm['ubloxGnss'])) + + if sm.updated['gnssMeasurements']: + pos_geo, stats = handle_laikad(sm['gnssMeasurements']) + if pos_geo is None or len(pos_geo) == 0: + continue + + match = all(abs(g-s) < DELTA for g,s in zip(pos_geo[:2], [slat, slon])) + match &= abs(pos_geo[2] - salt) < ALT_DELTA + if match: + match_cnt += 1 + if match_cnt >= MATCH_NUM: + return True, "MATCH", f"After: {round(time.monotonic() - start_time, 4)}" + + # keep some stats for error reporting + stats_laikad.append(stats) + + if (msg_cnt % 10) == 0: + a, p = check_alive_procs(procs) + if not a: + return False, "PROC CRASH", f"{p}" + msg_cnt += 1 + + if (time.monotonic() - start_time) > timeout: + h = f"LAIKAD: {stats_laikad[-REPORT_STATS:]}" + if len(h) == 0: + h = f"UBLOX: {stats_ublox[-REPORT_STATS:]}" + return False, "TIMEOUT", h + + + def exposed_run_checker(self, slat, slon, salt, timeout=180, use_laikad=True): + try: + procs = [] + sockets = [] + + if use_laikad: + procs.append("laikad") # pigeond, ubloxd # might wanna keep them running + sockets += ['ubloxGnss', 'gnssMeasurements'] + + if os.path.exists(EPHEM_CACHE): + os.remove(EPHEM_CACHE) + shutil.rmtree(DOWNLOAD_CACHE, ignore_errors=True) + + ret = self.run_checker(slat, slon, salt, sockets, procs, timeout) + kill_procs(procs) + return ret + + except Exception as e: + # always make sure processes get killed + kill_procs(procs) + return False, "CHECKER CRASHED", f"{str(e)}" + + + def exposed_kill_procs(self): + kill_procs(self.procs, no_retry=True) + + +if __name__ == "__main__": + print(f"Sever Log written to: {SERVER_LOG_FILE}") + t = ThreadedServer(RemoteCheckerService, port=18861) + t.start() + diff --git a/tools/gpstest/simulate_gps_signal.py b/tools/gpstest/simulate_gps_signal.py index a6aca1c4041181..da0f64eacad0bb 100755 --- a/tools/gpstest/simulate_gps_signal.py +++ b/tools/gpstest/simulate_gps_signal.py @@ -16,7 +16,7 @@ def download_rinex(): # TODO: check if there is a better way to get the full brdc file for LimeGPS gps_time = GPSTime.from_datetime(dt.datetime.utcnow()) - utc_time = dt.datetime.utcnow() - dt.timedelta(1) + utc_time = dt.datetime.utcnow()# - dt.timedelta(1) gps_time = GPSTime.from_datetime(dt.datetime(utc_time.year, utc_time.month, utc_time.day)) return download_nav(gps_time, cache_dir, ConstellationId.GPS) @@ -36,11 +36,15 @@ def get_random_coords(lat, lon) -> Tuple[int, int]: # jump around the world return get_coords(lat, lon, 20, 20, 10, 20) -def run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout): +def run_limeSDR_loop(lat, lon, alt, contin_sim, rinex_file, timeout): while True: try: - print(f"starting LimeGPS, Location: {lat},{lon}") - cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", f"{lat},{lon},100"] + # TODO: add starttime setting and altitude + # -t 2023/01/15,00:00:00 -T 2023/01/15,00:00:00 + # this needs to match the date of the navigation file + print(f"starting LimeGPS, Location: {lat} {lon} {alt}") + cmd = ["LimeGPS/LimeGPS", "-e", rinex_file, "-l", f"{lat},{lon},{alt}"] + print(f"CMD: {cmd}") sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) except KeyboardInterrupt: print("stopping LimeGPS") @@ -71,7 +75,7 @@ def run_hackRF_loop(lat, lon, rinex_file, timeout): try: print(f"starting gps-sdr-sim, Location: {lat},{lon}") # create 30second file and replay with hackrf endless - cmd = ["gps-sdr-sim/gps-sdr-sim", "-e", rinex_file, "-l", f"{lat},{lon},100", "-d", "30"] + cmd = ["gps-sdr-sim/gps-sdr-sim", "-e", rinex_file, "-l", f"{lat},{lon},-200", "-d", "30"] sp.check_output(cmd, stderr=sp.PIPE, timeout=timeout) # created in current working directory except Exception: @@ -90,7 +94,7 @@ def run_hackRF_loop(lat, lon, rinex_file, timeout): print(f"hackrf_transfer crashed:{str(e)}") -def main(lat, lon, jump_sim, contin_sim, hackrf_mode): +def main(lat, lon, alt, jump_sim, contin_sim, hackrf_mode): if hackrf_mode: if not os.path.exists('hackrf'): @@ -130,17 +134,18 @@ def main(lat, lon, jump_sim, contin_sim, hackrf_mode): if jump_sim: timeout = 30 - if not hackrf_mode: - run_limeSDR_loop(lat, lon, contin_sim, rinex_file, timeout) - else: + if hackrf_mode: run_hackRF_loop(lat, lon, rinex_file, timeout) + else: + run_limeSDR_loop(lat, lon, alt, contin_sim, rinex_file, timeout) if __name__ == "__main__": parser = argparse.ArgumentParser(description="Simulate static [or random jumping] GPS signal.") parser.add_argument("lat", type=float, nargs='?', default=0) parser.add_argument("lon", type=float, nargs='?', default=0) + parser.add_argument("alt", type=float, nargs='?', default=0) parser.add_argument("--jump", action="store_true", help="signal that jumps around the world") parser.add_argument("--contin", action="store_true", help="continuously/slowly moving around the world") parser.add_argument("--hackrf", action="store_true", help="hackrf mode (DEFAULT: LimeSDR)") args = parser.parse_args() - main(args.lat, args.lon, args.jump, args.contin, args.hackrf) + main(args.lat, args.lon, args.alt, args.jump, args.contin, args.hackrf) diff --git a/tools/mac_setup.sh b/tools/mac_setup.sh index e85292da037c79..dd2041e0cb857b 100755 --- a/tools/mac_setup.sh +++ b/tools/mac_setup.sh @@ -52,14 +52,9 @@ brew "zeromq" brew "protobuf" brew "protobuf-c" brew "swig" +cask "gcc-arm-embedded" EOS -# Install gcc-arm-embedded 10.3-2021.10. 11.x is broken on M1 Macs with Xcode 13.3~ -brew uninstall gcc-arm-embedded || true -curl -L https://github.com/Homebrew/homebrew-cask/raw/d407663b8017a0a062c7fc0b929faf2e16abd1ff/Casks/gcc-arm-embedded.rb > /tmp/gcc-arm-embedded.rb -brew install --cask /tmp/gcc-arm-embedded.rb -rm /tmp/gcc-arm-embedded.rb - echo "[ ] finished brew install t=$SECONDS" BREW_PREFIX=$(brew --prefix) diff --git a/tools/replay/camera.cc b/tools/replay/camera.cc index 72b385dca865f5..66898c9244b9de 100644 --- a/tools/replay/camera.cc +++ b/tools/replay/camera.cc @@ -56,7 +56,7 @@ void CameraServer::cameraThread(Camera &cam) { .timestamp_eof = eidx.getTimestampEof(), }; yuv->set_frame_id(eidx.getFrameId()); - vipc_server_->send(yuv, &extra, false); + vipc_server_->send(yuv, &extra); } else { rError("camera[%d] failed to get frame: %lu", cam.type, eidx.getSegmentId()); } diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc index 77357a28737f3d..5ad702590c6302 100644 --- a/tools/replay/consoleui.cc +++ b/tools/replay/consoleui.cc @@ -50,14 +50,6 @@ void add_str(WINDOW *w, const char *str, Color color = Color::Default, bool bold if (color != Color::Default) wattroff(w, COLOR_PAIR(color)); } -std::string format_seconds(int s) { - int total_minutes = s / 60; - int seconds = s % 60; - int hours = total_minutes / 60; - int minutes = total_minutes % 60; - return util::string_format("%02d:%02d:%02d", hours, minutes, seconds); -} - } // namespace ConsoleUI::ConsoleUI(Replay *replay, QObject *parent) : replay(replay), sm({"carState", "liveParameters"}), QObject(parent) { @@ -177,8 +169,8 @@ void ConsoleUI::updateStatus() { } auto [status_str, status_color] = status_text[status]; write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); - std::string suffix = " / " + format_seconds(replay->totalSeconds()); - write_item(0, 25, "TIME: ", format_seconds(replay->currentSeconds()), suffix, true); + std::string current_segment = " - " + std::to_string((int)(replay->currentSeconds() / 60)); + write_item(0, 25, "TIME: ", replay->currentDateTime().toString("ddd MMMM dd hh:mm:ss").toStdString(), current_segment, true); auto p = sm["liveParameters"].getLiveParameters(); write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 998c93b9381394..178b116a873b66 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -25,6 +25,13 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s s.push_back(it.name); } } + + if (!allow_list.empty()) { + // the following events are needed for replay to work properly. + allow_list.insert(cereal::Event::Which::INIT_DATA); + allow_list.insert(cereal::Event::Which::CAR_PARAMS); + } + qDebug() << "services " << s; qDebug() << "loading route " << route; @@ -107,9 +114,9 @@ void Replay::seekTo(double seconds, bool relative) { rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; + emit seekedTo(seconds); return isSegmentMerged(seg); }); - emit seekedTo(seconds); queueSegment(); } diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 2c68443df0eeed..6788a97d0320cc 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -64,6 +64,7 @@ class Replay : public QObject { inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } inline const Route* route() const { return route_.get(); } inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; } + inline QDateTime currentDateTime() const { return route_->datetime().addSecs(currentSeconds()); } inline uint64_t routeStartTime() const { return route_start_ts_; } inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } inline int totalSeconds() const { return segments_.size() * 60; } diff --git a/tools/replay/route.cc b/tools/replay/route.cc index f0d6ec5a128d37..619aeb3f5f355b 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -19,18 +19,19 @@ Route::Route(const QString &route, const QString &data_dir) : data_dir_(data_dir } RouteIdentifier Route::parseRoute(const QString &str) { - QRegExp rx(R"(^([a-z0-9]{16})([|_/])(\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2})(?:(--|/)(\d*))?$)"); + QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2})(?:(--|/)(\d*))?$)"); if (rx.indexIn(str) == -1) return {}; const QStringList list = rx.capturedTexts(); - return {list[1], list[3], list[5].toInt(), list[1] + "|" + list[3]}; + return {.dongle_id = list[1], .timestamp = list[3], .segment_id = list[5].toInt(), .str = list[1] + "|" + list[3]}; } bool Route::load() { - if (route_.str.isEmpty()) { + if (route_.str.isEmpty() || (data_dir_.isEmpty() && route_.dongle_id.isEmpty())) { rInfo("invalid route format"); return false; } + date_time_ = QDateTime::fromString(route_.timestamp, "yyyy-MM-dd--HH-mm-ss"); return data_dir_.isEmpty() ? loadFromServer() : loadFromLocal(); } diff --git a/tools/replay/route.h b/tools/replay/route.h index 6b78ebad870e7f..86adf6a14ddb2f 100644 --- a/tools/replay/route.h +++ b/tools/replay/route.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "tools/replay/framereader.h" @@ -27,6 +28,7 @@ class Route { Route(const QString &route, const QString &data_dir = {}); bool load(); inline const QString &name() const { return route_.str; } + inline const QDateTime datetime() const { return date_time_; } inline const QString &dir() const { return data_dir_; } inline const RouteIdentifier &identifier() const { return route_; } inline const std::map &segments() const { return segments_; } @@ -41,6 +43,7 @@ class Route { RouteIdentifier route_ = {}; QString data_dir_; std::map segments_; + QDateTime date_time_; }; class Segment : public QObject {