diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 7cc45d23b9..a4f7c68573 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 diff --git a/Jenkinsfile b/Jenkinsfile index 3b16b3f112..1841c68bba 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -81,6 +81,7 @@ pipeline { parallel { + /* stage('simulator') { agent { dockerfile { @@ -108,6 +109,7 @@ pipeline { } } } + */ stage('build') { agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } @@ -152,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"], diff --git a/SConstruct b/SConstruct index b148a9116a..31aa6ecced 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 dc780f858c..e1805f65ee 160000 --- a/body +++ b/body @@ -1 +1 @@ -Subproject commit dc780f858c1ef641471d09b72569e199e3e10acb +Subproject commit e1805f65ee75fab4454c21eda8b42b49d4bdc48f diff --git a/cereal b/cereal index f0471b7165..0565db5918 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f0471b71651788b3a6455b0431093daf941a0980 +Subproject commit 0565db5918cab74f85a75f298dc79b2fc5602a8e diff --git a/common/gpio.cc b/common/gpio.cc index 9f5c211a4b..aa65c0bd9d 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 8f6532bc79..35ceecfdc3 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,7 +104,7 @@ 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}, @@ -134,6 +134,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}, diff --git a/docs/CARS.md b/docs/CARS.md index 0d4c53ac01..390d2bf349 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. -# 223 Supported Cars +# 224 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -32,7 +32,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Genesis|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|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|| @@ -57,7 +58,7 @@ A supported vehicle is one that just works when you install a comma three. All s |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 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|| diff --git a/laika_repo b/laika_repo index 73bf110ae0..2e5c7b8a85 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d +Subproject commit 2e5c7b8a85cbaebda7fa715fe53d5e6ecbd62b0a diff --git a/panda b/panda index ac3ea96e27..9cf6c5a158 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ac3ea96e277142c226af22288fc1e3d87679712b +Subproject commit 9cf6c5a1581d21985cfaa6d0206d7b620ec9a239 diff --git a/release/check-submodules.sh b/release/check-submodules.sh index 182042e6b4..bc85a43c57 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 3d07924a91..547bb58144 100644 --- a/release/files_common +++ b/release/files_common @@ -320,6 +320,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 @@ -436,6 +442,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 @@ -573,17 +580,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 cae701eccc..3623fe5b16 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 39be41aa65..79a1a1e272 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/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1f1249194d..bc454aa54e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -48,8 +48,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; @@ -225,9 +225,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 +540,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 d4d53aa230..600a46c809 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -10,11 +10,11 @@ #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) { + handle = std::make_unique(serial); } hw_type = get_hw_type(); @@ -39,8 +39,20 @@ 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(); + + for (auto s : PandaSpiHandle::list()) { + if (std::find(serials.begin(), serials.end(), s) == serials.end()) { + serials.push_back(s); + } + } + + return serials; } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 156efe7ef3..69df2e2b66 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -48,13 +48,13 @@ private: 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(); diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc index e73cb69318..120d2f67d5 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 bd262dfa0e..b669d77e7f 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -21,6 +21,7 @@ public: 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 +31,6 @@ public: 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,6 +48,7 @@ public: 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[]); }; @@ -69,6 +68,7 @@ private: 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); diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 971756002b..f61d9ee1a6 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -7,7 +7,7 @@ import subprocess 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 @@ from system.swaglog import cloudlog 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 717b6ce820..bcc446b050 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -1,9 +1,12 @@ +#include #include #include #include #include #include +#include +#include #include "common/util.h" #include "common/timing.h" @@ -27,41 +30,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 +123,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 +134,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 +145,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 +184,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 +213,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 +240,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; } } diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 61e7a3d55d..88e8c72153 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -173,17 +173,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 2f058165ac..c8504d6fb1 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -58,9 +58,9 @@ class CarInterface(CarInterfaceBase): 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/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 8c391bb276..06da87bf34 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,28 +1,13 @@ -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 import apply_std_steer_angle_limits +from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg from selfdrive.car.ford.values import CANBUS, 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 +15,7 @@ class CarController: 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 +31,47 @@ class CarController: ### 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) - - 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)) + apply_curvature = 0. + self.apply_curvature_last = apply_curvature + can_sends.append(create_lka_msg(self.packer)) + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) ### 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 2276b1208a..215900fef6 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -118,7 +118,7 @@ class CarState(CarStateBase): ("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 373ce096c6..9ddde80f87 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -4,64 +4,58 @@ from selfdrive.car.ford.values import CANBUS HUDControl = car.CarControl.HUDControl -def create_lka_command(packer, angle_deg: float, curvature: float): +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_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 +101,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 +141,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 f3d77bc05a..7c6a8ecbe4 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.dashcamOnly = True - ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] - # Angle-based steering - ret.steerControlType = CarParams.SteerControlType.angle - ret.steerActuatorDelay = 0.4 + # These cars are dashcam only until the port is finished + ret.dashcamOnly = True + + ret.radarOffCan = 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/values.py b/selfdrive/car/ford/values.py index 0e6ef464b3..b93be61448 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,18 +1,14 @@ -from collections import defaultdict, namedtuple +from collections import defaultdict from dataclasses import dataclass from enum import Enum from typing import Dict, List, Union from cereal import car -from selfdrive.car import dbc_dict +from selfdrive.car 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: @@ -25,12 +21,13 @@ 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 - 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]) + # 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]) def __init__(self, CP): pass @@ -43,9 +40,11 @@ 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" class RADAR: @@ -53,7 +52,7 @@ class RADAR: DELPHI_MRR = 'FORD_CADS' -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", None)) @dataclass @@ -63,12 +62,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 +89,30 @@ FW_QUERY_CONFIG = FwQueryConfig( ) 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 +188,26 @@ FW_VERSIONS = { (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/honda/carstate.py b/selfdrive/car/honda/carstate.py index a37667fd3a..16880d1b1f 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,7 +274,7 @@ class CarState(CarStateBase): 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 990238ae5d..da921ccda7 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -231,11 +231,11 @@ class CarInterface(CarInterfaceBase): 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/tests/test_honda.py b/selfdrive/car/honda/tests/test_honda.py new file mode 100755 index 0000000000..7a8c86fb0a --- /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 2906b23bda..07eed0cfc2 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -4,9 +4,10 @@ from typing import Dict, List, Optional, Union 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,19 +142,46 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { 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), ], ) @@ -1067,6 +1094,8 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ FW_VERSIONS = { 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 @@ DBC = { 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 @@ STEER_THRESHOLD = { 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/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py index a52027f448..6ecfa03796 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 c4fda42b63..c0cf23b50d 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -145,7 +145,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { 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.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), @@ -225,7 +225,10 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { ], # Genesis - CAR.GENESIS_GV60_EV_1ST_GEN: HyundaiCarInfo("Genesis GV60 2023", "All", harness=Harness.hyundai_k), + CAR.GENESIS_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), @@ -448,11 +451,13 @@ FW_VERSIONS = { 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: { @@ -764,19 +769,23 @@ FW_VERSIONS = { }, 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: { @@ -1320,25 +1329,26 @@ FW_VERSIONS = { (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', ], @@ -1356,6 +1366,7 @@ FW_VERSIONS = { 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: { @@ -1542,6 +1553,7 @@ FW_VERSIONS = { 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 ', diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index ac0521cc68..d1b73afcf6 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -18,6 +18,7 @@ from selfdrive.car.body.values import CAR as COMMA non_tested_cars = [ FORD.ESCAPE_MK4, FORD.FOCUS_MK4, + FORD.MAVERICK_MK1, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU, @@ -42,6 +43,7 @@ routes = [ 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 +74,7 @@ routes = [ 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), diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index b6f78c4b47..4627b9a570 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -245,7 +245,7 @@ class TestCarModelBase(unittest.TestCase): # 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 3a9f92c046..0a49d39914 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 diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index eb1a04cee6..397b29525d 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/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index f627e517be..bdb43c542a 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -568,6 +568,7 @@ FW_VERSIONS = { (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', @@ -586,6 +587,7 @@ FW_VERSIONS = { ], (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', @@ -596,6 +598,7 @@ FW_VERSIONS = { (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', @@ -604,6 +607,7 @@ FW_VERSIONS = { 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', @@ -706,10 +710,12 @@ FW_VERSIONS = { 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', @@ -1079,6 +1085,7 @@ FW_VERSIONS = { 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 1d92cd4fb2..21d8fe2d66 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -99,7 +99,8 @@ class Controls: 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 @@ -132,9 +133,6 @@ class Controls: 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 @@ class Controls: 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") @@ -608,6 +606,7 @@ class Controls: 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: @@ -757,7 +756,6 @@ class Controls: 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/radard.py b/selfdrive/controls/radard.py index db87604e98..34f0f274fe 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -35,7 +35,7 @@ class KalmanParams(): 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 @@ class RadarD(): 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/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py index 1ea133cc19..a18390fef3 100755 --- a/selfdrive/debug/internal/fuzz_fw_fingerprint.py +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -28,7 +28,7 @@ if __name__ == "__main__": 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/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 8c4dbc55ee..8952405b8e 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -126,6 +126,7 @@ if __name__ == "__main__": 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/SConscript b/selfdrive/locationd/SConscript index 4b7fba19b6..7024196efd 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -7,6 +7,11 @@ 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) + + # 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) env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp new file mode 100644 index 0000000000..149134fbb5 --- /dev/null +++ b/selfdrive/locationd/generated/glonass.cpp @@ -0,0 +1,201 @@ +// 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 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; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_4_t::_read() { + m_tau_n = m__io->read_bits_int_be(22); + m_delta_tau_n = m__io->read_bits_int_be(5); + 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() { +} + +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_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; + + 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 = m__io->read_bits_int_be(24); + m_x_speedup = m__io->read_bits_int_be(5); + m_x = m__io->read_bits_int_be(27); +} + +glonass_t::string_1_t::~string_1_t() { + _clean_up(); +} + +void glonass_t::string_1_t::_clean_up() { +} + +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; + + 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 = m__io->read_bits_int_be(24); + m_y_speedup = m__io->read_bits_int_be(5); + m_y = m__io->read_bits_int_be(27); +} + +glonass_t::string_2_t::~string_2_t() { + _clean_up(); +} + +void glonass_t::string_2_t::_clean_up() { +} + +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; + + 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 = m__io->read_bits_int_be(11); + 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 = m__io->read_bits_int_be(24); + m_z_speedup = m__io->read_bits_int_be(5); + m_z = m__io->read_bits_int_be(27); +} + +glonass_t::string_3_t::~string_3_t() { + _clean_up(); +} + +void glonass_t::string_3_t::_clean_up() { +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h new file mode 100644 index 0000000000..48bdabfe96 --- /dev/null +++ b/selfdrive/locationd/generated/glonass.h @@ -0,0 +1,232 @@ +#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_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: + uint64_t m_tau_n; + uint64_t m_delta_tau_n; + 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: + uint64_t tau_n() const { return m_tau_n; } + uint64_t delta_tau_n() const { return m_delta_tau_n; } + 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_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: + uint64_t m_not_used; + uint64_t m_p1; + uint64_t m_t_k; + uint64_t m_x_vel; + uint64_t m_x_speedup; + uint64_t m_x; + 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; } + uint64_t x_vel() const { return m_x_vel; } + uint64_t x_speedup() const { return m_x_speedup; } + uint64_t x() const { return m_x; } + 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: + uint64_t m_b_n; + bool m_p2; + uint64_t m_t_b; + uint64_t m_not_used; + uint64_t m_y_vel; + uint64_t m_y_speedup; + uint64_t m_y; + 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; } + uint64_t y_vel() const { return m_y_vel; } + uint64_t y_speedup() const { return m_y_speedup; } + uint64_t y() const { return m_y; } + 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 m_p3; + uint64_t m_gamma_n; + bool m_not_used; + uint64_t m_p; + bool m_l_n; + uint64_t m_z_vel; + uint64_t m_z_speedup; + uint64_t m_z; + glonass_t* m__root; + glonass_t* m__parent; + + public: + bool p3() const { return m_p3; } + uint64_t gamma_n() const { return m_gamma_n; } + bool not_used() const { return m_not_used; } + uint64_t p() const { return m_p; } + bool l_n() const { return m_l_n; } + uint64_t z_vel() const { return m_z_vel; } + uint64_t z_speedup() const { return m_z_speedup; } + uint64_t z() const { return m_z; } + 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 9b020735bb..8e1cb85b95 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 293e2e4a05..9dfc5031f5 100644 --- a/selfdrive/locationd/generated/gps.h +++ b/selfdrive/locationd/generated/gps.h @@ -273,7 +273,7 @@ public: ~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 @@ public: 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 5e743e1ee7..34fe1e52ca 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 0000000000..95ca78c16f --- /dev/null +++ b/selfdrive/locationd/glonass.ksy @@ -0,0 +1,107 @@ +# http://gauss.gge.unb.ca/GLONASS.ICD.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 + _: 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 + type: b24 + - id: x_speedup + type: b5 + - id: x + type: b27 + 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 + type: b24 + - id: y_speedup + type: b5 + - id: y + type: b27 + string_3: + seq: + - id: p3 + type: b1 + - id: gamma_n + type: b11 + - id: not_used + type: b1 + - id: p + type: b2 + - id: l_n + type: b1 + - id: z_vel + type: b24 + - id: z_speedup + type: b5 + - id: z + type: b27 + string_4: + seq: + - id: tau_n + type: b22 + - id: delta_tau_n + type: b5 + - 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 + string_non_immediate: + seq: + - id: data_1 + type: b64 + - id: data_2 + type: b8 \ No newline at end of file diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch new file mode 100644 index 0000000000..2f99e93637 --- /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 6a48fe62c..149134fbb 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 6f5cde316b..893ad1b25b 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/models/lane_kf.py b/selfdrive/locationd/models/lane_kf.py new file mode 100755 index 0000000000..4d38fa8e09 --- /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/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 6059eab68f..d89f521228 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -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,8 +76,9 @@ 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 @@ -254,6 +272,18 @@ class TestLaikad(unittest.TestCase): # 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/torqued.py b/selfdrive/locationd/torqued.py index 588bca1578..fcc068d34e 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -22,15 +22,15 @@ FIT_POINTS_TOTAL_QLOG = 600 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 @@ class TorqueEstimator: 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 @@ class TorqueEstimator: '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 @@ class TorqueEstimator: 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 @@ class TorqueEstimator: 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 @@ class TorqueEstimator: 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 6431e9d48b..8bd9985170 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -170,9 +170,18 @@ kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { kaitai::kstream stream(subframe_data); gps_t subframe(&stream); int subframe_id = subframe.how()->subframe_id(); + int sv_id = msg->sv_id(); + uint64_t tow_counter = subframe.how()->tow_count(); - if (subframe_id == 1) gps_subframes[msg->sv_id()].clear(); - gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + bool clear_buffer = subframe_id == 1; + if (gps_sat_tow_count.count(sv_id) != 0) { + int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id]; + clear_buffer |= counter_diff != 1 && counter_diff != -100798; + } + if (clear_buffer) gps_subframes[sv_id].clear(); + + gps_subframes[sv_id][subframe_id] = subframe_data; + gps_sat_tow_count[sv_id] = tow_counter; } if (gps_subframes[msg->sv_id()].size() == 5) { diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 542e72816b..919e9963f1 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -103,6 +103,7 @@ class UbloxMsgParser { inline bool valid_so_far(); std::unordered_map> gps_subframes; + std::unordered_map gps_sat_tow_count; size_t bytes_in_parse_buf = 0; uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 369c529626..865966d6c5 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -20,7 +20,7 @@ from selfdrive.manager.process_config import managed_processes 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 dabfbe4ee0..ce18073d9b 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -290,10 +290,11 @@ class DaemonProcess(ManagerProcess): 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/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 6d4df0423a..889ab3d279 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -4,17 +4,17 @@ import signal 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 @@ class TestManager(unittest.TestCase): 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 82338e456b..7bbc1b3477 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" + 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" - # 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_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 8805b3dce8..2daf01ea14 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/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 1c65051665..3fa5e927a2 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -11,7 +11,9 @@ from struct import unpack_from, calcsize, pack 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 main() -> NoReturn: 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/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 22e021dcaf..fa4fa9540d 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 22ec099285..606dcad9c4 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -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()) @@ -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 a173cf5205..51de0f0013 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -18a70665bdb6b6aee4a224e826417049415e8290 +844c84eae269e74b246b039d2b33fd44e98a5b68 \ 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 c58909bf7f..569090f606 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -29,6 +29,7 @@ source_segments = [ ("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 @@ segments = [ ("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/ui/SConscript b/selfdrive/ui/SConscript index 669c214746..0f28f5ccc3 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/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index bde8628dc4..63b87149d4 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/util.cc b/selfdrive/ui/qt/util.cc index 59903e3376..4f52310649 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 61a27a8669..3188f3f9b9 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 16aad13437..4a7874b160 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 ecb44557c9..2606df5efe 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 8226dd59f9..16f35dffaa 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 bda64c53ab..f50d2d9b3f 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 105d6f77f0..445de72fc3 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_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 31202e45f2..792a918770 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 0379e926c4..e8e2ab6bd3 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 c62d737481..4df963167a 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/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 30e2810ec4..7ee3738057 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 8e2ad7bd91..92b3bde413 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; @@ -1172,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; + // 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}, @@ -1189,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/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index ab51059d9a..83fcb8f7a9 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 6c2ef1c7bc..f03c531b20 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 selfdrive.manager.process_config import managed_processes 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 @@ class TestCamerad(unittest.TestCase): 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 @@ class TestCamerad(unittest.TestCase): 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/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore new file mode 100644 index 0000000000..ac06c0cf85 --- /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 0000000000..61f2720db4 --- /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 0000000000..0b03b4db9e --- /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 8e22d5ee67..2e1d47b166 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 718e2e50af..fbf3c03d36 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,21 @@ 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 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 b98c75d452..010fe0fc88 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -6,11 +6,12 @@ #include #include -#include "tools/cabana/canmessages.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()); @@ -33,10 +34,20 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); } +QSize BinaryView::minimumSizeHint() const { + return {horizontalHeader()->minimumSectionSize() * 9 + VERTICAL_HEADER_WIDTH + 2, 0}; +} + 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); } } @@ -177,7 +188,9 @@ void BinaryViewModel::setMessage(const QString &message_id) { 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); @@ -193,6 +206,7 @@ void BinaryViewModel::updateState() { 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) { @@ -201,7 +215,7 @@ 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) { + 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 +226,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 +248,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 e936efc658..55de71572e 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -60,6 +60,7 @@ public: void highlight(const Signal *sig); QSet getOverlappingSignals() const; inline void updateState() { model->updateState(); } + QSize minimumSizeHint() const override; signals: void signalClicked(const Signal *sig); diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 4f037ba595..d9b6b5948b 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,43 @@ 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.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(); - } - return ret; + MainWindow w; + w.show(); + return app.exec(); } diff --git a/tools/cabana/canmessages.cc b/tools/cabana/canmessages.cc deleted file mode 100644 index 6e19eb440f..0000000000 --- a/tools/cabana/canmessages.cc +++ /dev/null @@ -1,101 +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::pause(bool pause) { - replay->pause(pause); - emit (pause ? paused() : resume()); -} - -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 ea43933565..0000000000 --- a/tools/cabana/canmessages.h +++ /dev/null @@ -1,81 +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(); } - void pause(bool pause); - inline const std::vector> getTimeline() { return replay->getTimeline(); } - -signals: - void paused(); - void resume(); - 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 952ed2d83f..3ecdbb08bd 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -2,39 +2,64 @@ #include #include -#include +#include +#include #include #include +#include #include #include #include #include #include +#include "selfdrive/ui/qt/util.h" + // ChartsWidget 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(bootstrapPixmap("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"}); + toolbar->addWidget(new QLabel(tr("Columns:"))); + 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); + + toolbar->addWidget(zoom_range_lb = new QLabel()); + reset_zoom_btn = toolbar->addAction(bootstrapPixmap("arrow-counterclockwise"), ""); reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); - remove_all_btn = toolbar->addAction("✖"); + remove_all_btn = toolbar->addAction(bootstrapPixmap("x"), ""); remove_all_btn->setToolTip(tr("Remove all charts")); dock_btn = toolbar->addAction(""); main_layout->addWidget(toolbar); // charts 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 = new QGridLayout); + charts_main_layout->addStretch(0); QScrollArea *charts_scroll = new QScrollArea(this); charts_scroll->setWidgetResizable(true); @@ -42,21 +67,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; - use_dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > - QApplication::style()->standardPalette().color(QPalette::Background).value(); - updateToolBar(); - 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; @@ -65,27 +98,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) { @@ -103,42 +124,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'))); + zoom_range_lb->setText(is_zoomed ? tr("Zooming: %1 - %2").arg(zoomed_range.first, 0, 'f', 2).arg(zoomed_range.second, 0, 'f', 2) : ""); + title_label->setText(tr("Charts: %1").arg(charts.size())); + dock_btn->setIcon(bootstrapPixmap(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); + } } ChartView *ChartsWidget::findChart(const QString &id, const Signal *sig) { @@ -147,27 +188,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]() { align_charts_timer->start(100); }); - 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); } @@ -175,11 +219,46 @@ 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 = column_count; + for (; n > 1; --n) { + if ((n * (CHART_MIN_WIDTH + charts_layout->spacing())) < rect().width()) break; + } + 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(); } @@ -221,9 +300,10 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { chart->addAxis(axis_y, Qt::AlignLeft); 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(bootstrapPixmap("x")); remove_btn->setAutoRaise(true); remove_btn->setToolTip(tr("Remove Chart")); close_btn_proxy = new QGraphicsProxyWidget(chart); @@ -231,7 +311,7 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { close_btn_proxy->setZValue(chart->zValue() + 11); QToolButton *manage_btn = new QToolButton(); - manage_btn->setText("🔧"); + manage_btn->setIcon(bootstrapPixmap("gear")); manage_btn->setAutoRaise(true); manage_btn->setToolTip(tr("Manage series")); manage_btn_proxy = new QGraphicsProxyWidget(chart); @@ -240,19 +320,21 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { 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; @@ -267,7 +349,12 @@ void ChartView::setPlotAreaLeftPosition(int pos) { void ChartView::addSeries(const QString &msg_id, const Signal *sig) { QLineSeries *series = new QLineSeries(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 chart()->addSeries(series); series->attachAxis(axis_x); series->attachAxis(axis_y); @@ -275,7 +362,6 @@ 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); } @@ -310,7 +396,6 @@ void ChartView::signalUpdated(const Signal *sig) { updateTitle(); // TODO: don't update series if only name changed. updateSeries(sig); - updateAxisY(); } } @@ -331,6 +416,17 @@ void ChartView::msgRemoved(uint32_t address) { } } +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)); + } + } + } +} + void ChartView::manageSeries() { SeriesSelector dlg(this); for (auto &s : sigs) { @@ -343,14 +439,7 @@ 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(); @@ -374,41 +463,28 @@ 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(); } + scene()->invalidate({}, QGraphicsScene::ForegroundLayer); } -void ChartView::updateSeries(const Signal *sig) { - auto events = can->events(); - if (!events || sigs.isEmpty()) return; - +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()) { @@ -416,14 +492,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(); } } } @@ -434,18 +511,11 @@ 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(); } } @@ -495,6 +565,24 @@ void ChartView::leaveEvent(QEvent *event) { 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()) { @@ -510,7 +598,7 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) { emit zoomIn(min, max); } event->accept(); - } else if (event->button() == Qt::RightButton) { + } else if (!can->liveStreaming() && event->button() == Qt::RightButton) { emit zoomReset(); event->accept(); } else { @@ -550,8 +638,38 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { QChartView::mouseMoveEvent(ev); } +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)); diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 819432920b..f134136323 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -1,18 +1,19 @@ #pragma once #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; @@ -22,11 +23,11 @@ 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; @@ -36,9 +37,8 @@ public: uint32_t address = 0; const Signal *sig = nullptr; QLineSeries *series = nullptr; - double min_y = 0; - double max_y = 0; QVector vals; + uint64_t last_value_mono_time = 0; }; signals: @@ -58,13 +58,15 @@ private slots: private: QList::iterator removeSeries(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); @@ -74,8 +76,9 @@ private: QPointF track_pt; 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"; }; class ChartsWidget : public QWidget { @@ -86,41 +89,51 @@ public: 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 *zoom_range_lb; + 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; QTimer *align_charts_timer; - QVBoxLayout *charts_layout; + 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; + QComboBox *columns_cb; + int column_count = 1; + const int CHART_MIN_WIDTH = 300; }; class SeriesSelector : public QDialog { diff --git a/tools/cabana/commands.h b/tools/cabana/commands.h index 7ea1f66653..e46223d630 100644 --- a/tools/cabana/commands.h +++ b/tools/cabana/commands.h @@ -2,7 +2,6 @@ #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" class EditMsgCommand : public QUndoCommand { diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index c7675121bb..5db6737be8 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -28,9 +28,7 @@ public: static std::pair parseId(const QString &id); inline static std::vector allDBCNames() { return get_dbc_names(); } - inline std::map &allMsgs() { return msgs; } inline QString name() const { return dbc ? dbc->name.c_str() : ""; } - void updateMsg(const QString &id, const QString &name, uint32_t size); void removeMsg(const QString &id); inline const std::map &messages() const { return msgs; } diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 06377616da..233bc493df 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -8,19 +8,17 @@ #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 +28,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); + toolbar->setIconSize({16, 16}); toolbar->addWidget(new QLabel("time:")); time_label = new QLabel(this); time_label->setStyleSheet("font-weight:bold"); @@ -45,24 +40,18 @@ 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(bootstrapPixmap("pencil"), "", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); + remove_msg_act = toolbar->addAction(bootstrapPixmap("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); @@ -85,9 +74,9 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart tab_widget = new QTabWidget(this); tab_widget->setTabPosition(QTabWidget::South); - tab_widget->addTab(scroll, "&Msg"); + tab_widget->addTab(scroll, bootstrapPixmap("file-earmark-ruled"), "&Msg"); history_log = new LogsWidget(this); - tab_widget->addTab(history_log, "&Logs"); + tab_widget->addTab(history_log, bootstrapPixmap("stopwatch"), "&Logs"); main_layout->addWidget(tab_widget); stacked_layout = new QStackedLayout(this); @@ -98,7 +87,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart QObject::connect(binary_view, &BinaryView::resizeSignal, this, &DetailWidget::resizeSignal); QObject::connect(binary_view, &BinaryView::addSignal, this, &DetailWidget::addSignal); QObject::connect(tab_widget, &QTabWidget::currentChanged, [this]() { updateState(); }); - QObject::connect(can, &CANMessages::msgsReceived, this, &DetailWidget::updateState); + QObject::connect(can, &AbstractStream::msgsReceived, this, &DetailWidget::updateState); QObject::connect(dbc(), &DBCManager::DBCFileChanged, [this]() { dbcMsgChanged(); }); QObject::connect(tabbar, &QTabBar::customContextMenuRequested, this, &DetailWidget::showTabBarContextMenu); QObject::connect(tabbar, &QTabBar::currentChanged, [this](int index) { @@ -174,8 +163,14 @@ void DetailWidget::dbcMsgChanged(int show_form_idx) { 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(); @@ -184,10 +179,10 @@ void DetailWidget::dbcMsgChanged(int show_form_idx) { 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(bootstrapPixmap(msg ? "exclamation-triangle" : "info-circle")); + } warning_widget->setVisible(!warnings.isEmpty()); setUpdatesEnabled(true); } diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 18c58ab6bf..c59a6fca50 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -31,6 +31,7 @@ public: DetailWidget(ChartsWidget *charts, QWidget *parent); void setMessage(const QString &message_id); void dbcMsgChanged(int show_form_idx = -1); + QSize minimumSizeHint() const override { return binary_view->minimumSizeHint(); } QUndoStack *undo_stack = nullptr; private: @@ -46,7 +47,7 @@ private: 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; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 485a21cc1b..ccc2c5533d 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -1,6 +1,5 @@ #include "tools/cabana/historylog.h" -#include #include #include #include @@ -8,16 +7,15 @@ // 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 HexColors::toVariantList(m.colors); } return {}; } @@ -36,21 +34,21 @@ void HistoryLogModel::refresh() { beginResetModel(); last_fetch_time = 0; 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 +59,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; @@ -81,6 +84,7 @@ void HistoryLogModel::updateState() { if ((has_more_data = !new_msgs.empty())) { beginInsertRows({}, 0, new_msgs.size() - 1); messages.insert(messages.begin(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); + updateColors(); endInsertRows(); } last_fetch_time = current_time; @@ -93,11 +97,29 @@ void HistoryLogModel::fetchMore(const QModelIndex &parent) { if ((has_more_data = !new_msgs.empty())) { beginInsertRows({}, messages.size(), messages.size() + new_msgs.size() - 1); messages.insert(messages.end(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); + if (!dynamic_mode) { + updateColors(); + } endInsertRows(); } } } +void HistoryLogModel::updateColors() { + if (!display_signals_mode || sigs.empty()) { + const auto freq = can->lastMessage(msg_id).freq; + if (dynamic_mode) { + for (auto it = messages.rbegin(); it != messages.rend(); ++it) { + it->colors = hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + } + } else { + for (auto it = messages.begin(); it != messages.end(); ++it) { + it->colors = hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + } + } + } +} + template std::deque HistoryLogModel::fetchData(InputIt first, InputIt last, uint64_t min_time) { std::deque msgs; @@ -114,7 +136,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,16 +147,16 @@ 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(); if (dynamic_mode) { - auto it = std::lower_bound(events->rbegin(), events->rend(), from_time, [=](auto &e, uint64_t ts) { - return e->mono_time > ts; + auto it = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { + return e->mono_time < ts; }); - if (it != events->rend()) ++it; return fetchData(it, events->rend(), min_time); } else { assert(min_time == 0); @@ -148,120 +170,102 @@ std::deque HistoryLogModel::fetchData(uint64_t from_ti // 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); - QHBoxLayout *h = new QHBoxLayout(); - signals_cb = new QComboBox(this); - signals_cb->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Preferred); - h->addWidget(signals_cb); - comp_box = new QComboBox(); + 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); + + 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(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) { + 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()); - } } diff --git a/tools/cabana/historylog.h b/tools/cabana/historylog.h index f20f51637a..83b5f623ea 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,23 @@ public: 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; } - void setDynamicMode(int state); - void segmentsMerged(); + int columnCount(const QModelIndex &parent = QModelIndex()) const override { + return display_signals_mode && !sigs.empty() ? sigs.size() + 1 : 2; + } + void updateColors(); void refresh(); +public slots: + void setDisplayType(int type); + void setDynamicMode(int state); + void segmentsMerged(); + +public: struct Message { uint64_t mono_time = 0; QVector sig_values; - QString data; + QByteArray data; + QVector colors; }; template @@ -46,6 +54,7 @@ public: std::deque fetchData(uint64_t from_time, uint64_t min_time = 0); QString msg_id; + HexColors hex_colors; bool has_more_data = true; const int batch_size = 50; int filter_sig_idx = -1; @@ -54,13 +63,8 @@ public: 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,7 +73,8 @@ class LogsWidget : public QWidget { public: LogsWidget(QWidget *parent); void setMessage(const QString &message_id); - void updateState(); + void updateState() {if (dynamic_mode->isChecked()) model->updateState(); } + void showEvent(QShowEvent *event) override { if (dynamic_mode->isChecked()) model->refresh(); } signals: void openChart(const QString &msg_id, const Signal *sig); @@ -78,13 +83,10 @@ private slots: void setFilter(); private: - void doubleClicked(const QModelIndex &index); - void showEvent(QShowEvent *event) override; - - 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 9df5894b90..d674ec9059 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -24,17 +24,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"); @@ -58,23 +59,34 @@ MainWindow::MainWindow() : QMainWindow() { 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(detail_widget->undo_stack, &QUndoStack::cleanChanged, [this](bool clean) { setWindowModified(!clean); }); } 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); file_menu->addAction(tr("Load DBC From Clipboard"), this, &MainWindow::loadDBCFromClipboard); + 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(); - 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")); @@ -116,19 +128,31 @@ void MainWindow::createDockWindows() { 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); } @@ -168,34 +192,61 @@ 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())); + 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::openRecentFile() { + if (auto action = qobject_cast(sender())) { + remindSaveChanges(); + loadFile(action->data().toString()); + } +} + +void MainWindow::loadDBCFromName(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() { + 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()) { @@ -205,19 +256,31 @@ void MainWindow::loadDBCFromFingerprint() { 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()); + detail_widget->undo_stack->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 +289,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 (!detail_widget->undo_stack->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; + } + } + detail_widget->undo_stack->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 +345,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 +360,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); } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index f8b5f92349..4e65fa01cf 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "tools/cabana/chartswidget.h" @@ -22,11 +23,14 @@ public: void showStatusMessage(const QString &msg, int timeout = 0) { statusBar()->showMessage(msg, timeout); } public slots: + void newFile(); + void openFile(); + void openRecentFile(); void loadDBCFromName(const QString &name); void loadDBCFromFingerprint(); - void loadDBCFromFile(); void loadDBCFromClipboard(); - void saveDBCToFile(); + void save(); + void saveAs(); void saveDBCToClipboard(); signals: @@ -34,6 +38,11 @@ signals: void updateProgressBar(uint64_t cur, uint64_t total, bool success); protected: + void remindSaveChanges(); + void saveFile(const QString &fn); + void loadFile(const QString &fn); + void setCurrentFile(const QString &fn); + void updateRecentFileActions(); void createActions(); void createDockWindows(); QComboBox *createDBCSelector(); @@ -45,14 +54,19 @@ protected: void setOption(); void findSimilarBits(); - 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 fd0bc68514..69c6d298c0 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,9 +1,10 @@ #include "tools/cabana/messageswidget.h" #include -#include #include #include +#include +#include #include "tools/cabana/dbcmanager.h" @@ -21,9 +22,9 @@ 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); @@ -31,11 +32,12 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget->setColumnWidth(2, 80); table_widget->horizontalHeader()->setStretchLastSection(true); table_widget->verticalHeader()->hide(); + main_layout->addWidget(table_widget); // 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); @@ -65,9 +67,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; @@ -75,8 +78,8 @@ 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) { + return HexColors::toVariantList(can_data.colors); } return {}; } @@ -85,7 +88,24 @@ void MessageListModel::setFilterString(const QString &string) { filter_str = string; msgs.clear(); for (auto it = can->can_msgs.begin(); it != can->can_msgs.end(); ++it) { + bool found = false; + + // Search by message id or name if (it.key().contains(filter_str, Qt::CaseInsensitive) || msgName(it.key()).contains(filter_str, Qt::CaseInsensitive)) { + found = true; + } + + // Search by signal name + const DBCMsg *msg = dbc()->msg(it.key()); + if (msg != nullptr) { + for (auto &signal: msg->getSignals()) { + if (QString::fromStdString(signal->name).contains(filter_str, Qt::CaseInsensitive)) { + found = true; + } + } + } + + if (found) { msgs.push_back(it.key()); } } @@ -96,22 +116,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 = std::tuple{can->lastMessage(l).src, can->lastMessage(l).address, l}; + auto rr = std::tuple{can->lastMessage(r).src, can->lastMessage(r).address, 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(); diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 62aa23c02e..69fdd26170 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -1,9 +1,11 @@ #pragma once #include +#include #include +#include -#include "tools/cabana/canmessages.h" +#include "tools/cabana/streams/abstractstream.h" class MessageListModel : public QAbstractTableModel { Q_OBJECT @@ -32,6 +34,8 @@ 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); } signals: void msgSelectionChanged(const QString &message_id); diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 5e7f833317..78969c76c3 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -15,23 +15,31 @@ 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); } 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(); } // SettingsDlg @@ -46,23 +54,17 @@ 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); - - 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); + 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); 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 +76,8 @@ 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_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 d231a3a53a..56c6a992ae 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -14,11 +14,16 @@ public: 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 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 +36,8 @@ public: SettingsDlg(QWidget *parent); void save(); QSpinBox *fps; - QSpinBox *cached_segment; + QSpinBox *cached_minutes; QSpinBox *chart_height; - QSpinBox *max_chart_x_range; }; extern Settings settings; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index 3845e72be1..112c2807f9 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -6,53 +6,63 @@ #include #include +#include "selfdrive/ui/qt/util.h" + // SignalForm SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { - QFormLayout *form_layout = new QFormLayout(this); + auto double_validator = new QDoubleValidator(this); + double_validator->setLocale(QLocale::C); // Match locale of QString::toDouble() instead of system + + QVBoxLayout *main_layout = new QVBoxLayout(this); + QFormLayout *form_layout = new QFormLayout(); + main_layout->addLayout(form_layout); name = new QLineEdit(); name->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name)); form_layout->addRow(tr("Name"), name); + QHBoxLayout *hl = new QHBoxLayout(this); size = new QSpinBox(); size->setMinimum(1); - form_layout->addRow(tr("Size"), size); - + hl->addWidget(size); endianness = new QComboBox(); - endianness->addItems({"Little", "Big"}); - form_layout->addRow(tr("Endianness"), endianness); - - form_layout->addRow(tr("lsb"), lsb = new QLabel()); - form_layout->addRow(tr("msb"), msb = new QLabel()); - + endianness->addItems({"Little Endianness", "Big Endianness"}); + hl->addWidget(endianness); sign = new QComboBox(); sign->addItems({"Signed", "Unsigned"}); - form_layout->addRow(tr("sign"), sign); - - auto double_validator = new QDoubleValidator(this); - - factor = new QLineEdit(); - factor->setValidator(double_validator); - form_layout->addRow(tr("Factor"), factor); + hl->addWidget(sign); + form_layout->addRow(tr("Size"), hl); offset = new QLineEdit(); offset->setValidator(double_validator); form_layout->addRow(tr("Offset"), offset); + factor = new QLineEdit(); + factor->setValidator(double_validator); + form_layout->addRow(tr("Factor"), factor); + + expand_btn = new QToolButton(this); + expand_btn->setText(tr("more...")); + main_layout->addWidget(expand_btn, 0, Qt::AlignRight); // TODO: parse the following parameters in opendbc + QWidget *extra_container = new QWidget(this); + QFormLayout *extra_layout = new QFormLayout(extra_container); unit = new QLineEdit(); - form_layout->addRow(tr("Unit"), unit); + extra_layout->addRow(tr("Unit"), unit); comment = new QLineEdit(); - form_layout->addRow(tr("Comment"), comment); + extra_layout->addRow(tr("Comment"), comment); min_val = new QLineEdit(); min_val->setValidator(double_validator); - form_layout->addRow(tr("Minimum value"), min_val); + extra_layout->addRow(tr("Minimum value"), min_val); max_val = new QLineEdit(); max_val->setValidator(double_validator); - form_layout->addRow(tr("Maximum value"), max_val); + extra_layout->addRow(tr("Maximum value"), max_val); val_desc = new QLineEdit(); - form_layout->addRow(tr("Value descriptions"), val_desc); + extra_layout->addRow(tr("Value descriptions"), val_desc); + + main_layout->addWidget(extra_container); + extra_container->setVisible(false); QObject::connect(name, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); QObject::connect(factor, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); @@ -60,6 +70,10 @@ SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { QObject::connect(size, &QSpinBox::editingFinished, this, &SignalForm::changed); QObject::connect(sign, SIGNAL(activated(int)), SIGNAL(changed())); QObject::connect(endianness, SIGNAL(activated(int)), SIGNAL(changed())); + QObject::connect(expand_btn, &QToolButton::clicked, [=]() { + extra_container->setVisible(!extra_container->isVisible()); + expand_btn->setText(extra_container->isVisible() ? tr("less...") : tr("more...")); + }); } void SignalForm::textBoxEditingFinished() { @@ -77,6 +91,8 @@ SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(pa main_layout->setContentsMargins(0, 0, 0, 0); main_layout->setSpacing(0); + bg_color = QColor(getColor(form_idx)); + // title bar auto title_bar = new QWidget(this); title_bar->setFixedHeight(32); @@ -94,13 +110,13 @@ SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(pa title_layout->addWidget(title); plot_btn = new QToolButton(this); - plot_btn->setText("📈"); + plot_btn->setIcon(bootstrapPixmap("graph-up")); 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->setIcon(bootstrapPixmap("x")); remove_btn->setToolTip(tr("Remove signal")); title_layout->addWidget(remove_btn); main_layout->addWidget(title_bar); @@ -130,7 +146,7 @@ void SignalEdit::setSignal(const QString &message_id, const Signal *signal) { updateForm(msg_id == message_id && form->isVisible()); msg_id = message_id; color_label->setText(QString::number(form_idx + 1)); - color_label->setStyleSheet(QString("color:black; background-color:%2").arg(getColor(form_idx))); + color_label->setStyleSheet(QString("color:black; background-color:%2").arg(bg_color.name())); title->setText(sig->name.c_str()); show(); } @@ -180,8 +196,6 @@ void SignalEdit::updateForm(bool visible) { 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->setVisible(visible); @@ -189,8 +203,9 @@ void SignalEdit::updateForm(bool 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))); + auto text_color = sig == s ? "white" : "black"; + auto _bg_color = sig == s ? bg_color.darker(125) : bg_color; // 4/5x brightness + color_label->setStyleSheet(QString("color:%1; background-color:%2").arg(text_color).arg(_bg_color.name())); } void SignalEdit::enterEvent(QEvent *event) { diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index d7b9084e7f..90df3fd3f4 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -7,8 +7,8 @@ #include #include "selfdrive/ui/qt/widgets/controls.h" -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" class SignalForm : public QWidget { Q_OBJECT @@ -17,9 +17,9 @@ public: void textBoxEditingFinished(); QLineEdit *name, *unit, *comment, *val_desc, *offset, *factor, *min_val, *max_val; - QLabel *lsb, *msb; QSpinBox *size; QComboBox *sign, *endianness; + QToolButton *expand_btn; signals: void changed(); @@ -54,5 +54,6 @@ protected: QLabel *color_label; QLabel *icon; int form_idx = 0; + QColor bg_color; QToolButton *plot_btn; }; diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc new file mode 100644 index 0000000000..b188c41826 --- /dev/null +++ b/tools/cabana/streams/abstractstream.cc @@ -0,0 +1,60 @@ +#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; + QObject::connect(this, &AbstractStream::received, this, &AbstractStream::process, Qt::QueuedConnection); +} + +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 std::unique_ptr new_msgs = std::make_unique>(); + static QHash hex_colors; + static double prev_update_ts = 0; + + if (event->which == cereal::Event::Which::CAN) { + double current_sec = currentSec(); + 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.src = c.getSrc(); + data.address = c.getAddress(); + 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; + } + data.colors = hex_colors[id].compute(data.dat, data.ts, data.freq); + } + + 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; +} diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h new file mode 100644 index 0000000000..9a0187da5e --- /dev/null +++ b/tools/cabana/streams/abstractstream.h @@ -0,0 +1,69 @@ +#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.; + uint8_t src = 0; + uint32_t address = 0; + uint32_t count = 0; + uint32_t freq = 0; + QByteArray dat; + QVector colors; +}; + +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) { return can_msgs[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: + QMap can_msgs; + +protected: + void process(QHash *); + bool updateEvent(const Event *event); + + bool is_live_streaming = false; + std::atomic counters_begin_sec = 0; + std::atomic processing = false; + QHash counters; +}; + +// 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 0000000000..3edd2a7758 --- /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 0000000000..ba20032b67 --- /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 0000000000..fd58f7a409 --- /dev/null +++ b/tools/cabana/streams/replaystream.cc @@ -0,0 +1,54 @@ +#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::seekTo(double ts) { + replay->seekTo(std::max(double(0), ts), false); + counters_begin_sec = 0; + emit updated(); +} + +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 0000000000..1688915212 --- /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; + 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 8a05e3e236..63d01b152d 100644 --- a/tools/cabana/tools/findsimilarbits.cc +++ b/tools/cabana/tools/findsimilarbits.cc @@ -7,8 +7,8 @@ #include #include -#include "tools/cabana/canmessages.h" #include "tools/cabana/dbcmanager.h" +#include "tools/cabana/streams/abstractstream.h" FindSimilarBitsDlg::FindSimilarBitsDlg(QWidget *parent) : QDialog(parent, Qt::WindowFlags() | Qt::Window) { setWindowTitle(tr("Find similar bits")); diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc new file mode 100644 index 0000000000..0684d3c59a --- /dev/null +++ b/tools/cabana/util.cc @@ -0,0 +1,99 @@ +#include "tools/cabana/util.h" + +#include +#include + +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); +} + +const QVector &HexColors::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; + return colors; +} + +void HexColors::clear() { + prev_dat.clear(); + last_change_t.clear(); + colors.clear(); +} + +QList HexColors::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 { + QList colors = index.data(Qt::UserRole).toList(); + if (colors.empty()) { + QStyledItemDelegate::paint(painter, option, index); + return; + } + 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 + 1, m, m, m); + + 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++; + } +} diff --git a/tools/cabana/util.h b/tools/cabana/util.h new file mode 100644 index 0000000000..146410d7c6 --- /dev/null +++ b/tools/cabana/util.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include +#include +#include + +class HexColors { +public: + const QVector &compute(const QByteArray &dat, double ts, uint32_t freq); + static QList toVariantList(const QVector &colors); + void clear(); + +private: + const int periodic_threshold = 10; + const int start_alpha = 128; + const float fade_time = 2.0; + QByteArray prev_dat; + QVector last_change_t; + QVector colors; +}; + +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)]; +} diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index ed4354ce65..445a21c755 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -3,38 +3,70 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include #include +#include "selfdrive/ui/qt/util.h" + 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); + QVBoxLayout *frame_layout = new QVBoxLayout(frame); + if (!can->liveStreaming()) { + frame_layout->addWidget(createCameraWidget()); + } - cam_widget = new CameraWidget("camerad", can->visionStreamType(), false, this); - cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); - main_layout->addWidget(cam_widget); + // btn controls + QHBoxLayout *control_layout = new QHBoxLayout(); + 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); }); + control_layout->addWidget(btn); + group->addButton(btn); + if (speed == 1.0) btn->setChecked(true); + } + 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 - QHBoxLayout *slider_layout = new QHBoxLayout(); - QLabel *time_label = new QLabel("00:00"); + slider_layout = new QHBoxLayout(); + time_label = new ElidedLabel("00:00"); + time_label->setToolTip(tr("Click to set current time")); slider_layout->addWidget(time_label); slider = new Slider(this); @@ -43,40 +75,47 @@ VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) { end_time_label = new QLabel(this); slider_layout->addWidget(end_time_label); - main_layout->addLayout(slider_layout); + l->addLayout(slider_layout); - // btn controls - QHBoxLayout *control_layout = new QHBoxLayout(); - play_btn = new QPushButton("⏸"); - play_btn->setStyleSheet("font-weight:bold; height:16px"); - control_layout->addWidget(play_btn); - - QButtonGroup *group = new QButtonGroup(this); - group->setExclusive(true); - for (float speed : {0.1, 0.5, 1., 2.}) { - QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); - btn->setCheckable(true); - QObject::connect(btn, &QPushButton::clicked, [=]() { can->setSpeed(speed); }); - control_layout->addWidget(btn); - group->addButton(btn); - if (speed == 1.0) btn->setChecked(true); - } - main_layout->addLayout(control_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, []() { can->pause(!can->isPaused()); }); - QObject::connect(play_btn, &QPushButton::clicked, []() { can->pause(!can->isPaused()); }); - QObject::connect(can, &CANMessages::updated, this, &VideoWidget::updateState); - QObject::connect(can, &CANMessages::paused, [this]() { play_btn->setText("▶"); }); - QObject::connect(can, &CANMessages::resume, [this]() { play_btn->setText("⏸"); }); - QObject::connect(can, &CANMessages::streamStarted, [this]() { + 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::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(); @@ -90,6 +129,11 @@ void VideoWidget::updateState() { slider->setValue(can->currentSec() * 1000); } +void VideoWidget::updatePlayBtnState() { + play_btn->setIcon(bootstrapPixmap(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); @@ -101,7 +145,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 ec8bc4bec4..51197eedd6 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 @@ private: QSize thumbnail_size = {}; }; -class VideoWidget : public QFrame { +class VideoWidget : public QWidget { Q_OBJECT public: @@ -44,9 +47,14 @@ public: protected: void updateState(); + 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 5aff0ee3d7..01f44df0ce 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 bd204e7ae7..a2e130342c 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 + 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 - 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')}") + 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() +stats = defaultdict(int) # type: ignore +keys = ['success', 'failed', 'ublox_fail', 'laikad_fail', 'proc_crash', 'checker_crash'] - tcnt = 0 - while True: - if not checker_thread.is_alive(): - # assume this only happens when the signal got matched - return True - - # 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 - - print(f"Time elapsed: {tcnt}[s]", end = "\r") - time.sleep(1) - tcnt += 1 +def print_report(): + print("\nFuzzy testing report summary:") + for k in keys: + print(f" {k}: {stats[k]}") -def main(): - if len(sys.argv) < 2: - print(f"usage: {sys.argv[0]} [-c]") - ip_addr = sys.argv[1] +def update_stats(matched, log, info): + if matched: + stats['success'] += 1 + return - continuous_mode = False - if len(sys.argv) == 3 and sys.argv[2] == '-c': - print("Continuous Mode!") - continuous_mode = True + 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 + +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) - while True: - # spoof random location - spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},100", duration) - start_time = time.monotonic() + try: + while True: + # spoof random location + spoof_proc = run_lime_gps(rinex_file, f"{lat},{lon},{alt}", timeout) - # 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 + # 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 - end_time = time.monotonic() - spoof_proc.terminate() + 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() - # -1 to count process startup - print(f"Time to get Signal: {round(end_time - start_time - 1, 4)}") + if con is not None and not con.closed: + con.root.exposed_kill_procs() + con.close() + + 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 0000000000..4f62e60db0 --- /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 a649a105c3..0000000000 --- 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 0000000000..b35c66d02d --- /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 a6aca1c404..da0f64eaca 100755 --- a/tools/gpstest/simulate_gps_signal.py +++ b/tools/gpstest/simulate_gps_signal.py @@ -16,7 +16,7 @@ cache_dir = '/tmp/gpstest/' 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)