diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index a4f7c68573..d5be221303 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -219,6 +219,7 @@ jobs: $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ selfdrive/locationd/test/_test_locationd_lib.py && \ + ./selfdrive/locationd/test/test_glonass_runner && \ $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST system/hardware/tici && \ diff --git a/RELEASES.md b/RELEASES.md index 1b5ef8f7a7..9103fbc4b5 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -6,8 +6,10 @@ Version 0.9.1 (2022-12-XX) * Chevrolet Bolt EV 2022-23 support thanks to JasonJShuler! * Genesis GV60 2023 support thanks to sunnyhaibin! * Hyundai Tucson 2022-23 support +* Kia K5 Hybrid 2020 support thanks to sunnyhaibin! * Kia Sorento 2022-23 support thanks to sunnyhaibin! * Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! +* Volkswagen Crafter and MAN TGE 2017-23 support thanks to jyoung8607! Version 0.9.0 (2022-11-21) ======================== diff --git a/cereal b/cereal index 29d571c841..79f76b153e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 29d571c84142ce75e7113085f62acf6285faf6b6 +Subproject commit 79f76b153e288852abc789c881baf9d5250acc3e diff --git a/common/SConscript b/common/SConscript index 8aee6f42a7..5d6170611f 100644 --- a/common/SConscript +++ b/common/SConscript @@ -10,11 +10,13 @@ common_libs = [ 'statlog.cc', 'swaglog.cc', 'util.cc', - 'gpio.cc', 'i2c.cc', 'watchdog.cc', ] +if arch != "Darwin": + common_libs.append('gpio.cc') + _common = fxn('common', common_libs, LIBS="json11") files = [ diff --git a/common/params.cc b/common/params.cc index f35fc8e12e..2435cc3e5b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -111,6 +111,7 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, @@ -170,7 +171,7 @@ std::unordered_map keys = { {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, diff --git a/docs/CARS.md b/docs/CARS.md index 390d2bf349..45873c4924 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. -# 224 Supported Cars +# 233 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -30,7 +30,8 @@ A supported vehicle is one that just works when you install a comma three. All s |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| |Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| +|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| +|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| |Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| @@ -99,6 +100,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| |Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| +|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| @@ -106,6 +108,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| |Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| |Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| +|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| |Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| |Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| |Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| @@ -135,6 +138,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B|| @@ -181,6 +186,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| +|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| |Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| @@ -209,6 +215,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| @@ -217,6 +225,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| diff --git a/docs/overview.rst b/docs/overview.rst index cda51ba3d4..cc4c582155 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -77,3 +77,4 @@ tools Simulator tools/ssh/README.md Webcam + tools/cabana/README.md diff --git a/laika_repo b/laika_repo index 2e5c7b8a85..b30afe44d2 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 2e5c7b8a85cbaebda7fa715fe53d5e6ecbd62b0a +Subproject commit b30afe44d283072b6ee0ea17f290cca5d002baf7 diff --git a/opendbc b/opendbc index e8ce453227..77996759d9 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit e8ce453227ece42bdb99b898c98876665373d7ef +Subproject commit 77996759d95b110e08b747e04636e7d3f680207a diff --git a/release/files_common b/release/files_common index 547bb58144..5b310d5c80 100644 --- a/release/files_common +++ b/release/files_common @@ -225,10 +225,7 @@ selfdrive/locationd/SConscript selfdrive/locationd/ubloxd.cc selfdrive/locationd/ublox_msg.cc selfdrive/locationd/ublox_msg.h -selfdrive/locationd/generated/ubx.cpp -selfdrive/locationd/generated/ubx.h -selfdrive/locationd/generated/gps.cpp -selfdrive/locationd/generated/gps.h +selfdrive/locationd/generated/* selfdrive/locationd/laikad.py selfdrive/locationd/locationd.h diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index bc454aa54e..0473d3488c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -105,6 +106,8 @@ void sync_time(Panda *panda, SyncTimeDir dir) { bool safety_setter_thread(std::vector pandas) { LOGD("Starting safety setter thread"); + Params p; + // there should be at least one panda connected if (pandas.size() == 0) { return false; @@ -116,25 +119,20 @@ bool safety_setter_thread(std::vector pandas) { pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); } - Params p = Params(); - - // wait for VIN to be read + // wait for FW query at OBD port to finish while (true) { if (do_exit || !check_all_connected(pandas) || !ignition) { return false; } - std::string value_vin = p.get("CarVin"); - if (value_vin.size() > 0) { - // sanity check VIN format - assert(value_vin.size() == 17); - LOGW("got CarVin %s", value_vin.c_str()); + if (p.getBool("FirmwareObdQueryDone")) { + LOGW("finished FW query at OBD port"); break; } util::sleep_for(20); } - // set to ELM327 for ECU knockouts + // set to ELM327 to finish fingerprinting and for potential ECU knockouts for (Panda *panda : pandas) { panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 600a46c809..647a0d9c78 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -14,7 +14,9 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { try { handle = std::make_unique(serial); } catch (std::exception &e) { +#ifndef __APPLE__ handle = std::make_unique(serial); +#endif } hw_type = get_hw_type(); @@ -46,11 +48,13 @@ std::string Panda::hw_serial() { std::vector Panda::list() { std::vector serials = PandaUsbHandle::list(); +#ifndef __APPLE__ for (auto s : PandaSpiHandle::list()) { if (std::find(serials.begin(), serials.end(), s) == serials.end()) { serials.push_back(s); } } +#endif return serials; } @@ -232,6 +236,9 @@ void Panda::can_send(capnp::List::Reader can_data_list) { } bool Panda::can_receive(std::vector& out_vec) { + // Check if enough space left in buffer to store RECV_SIZE data + assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); + int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); if (!comms_healthy()) { return false; @@ -274,6 +281,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector #include +#ifndef __APPLE__ #include +#endif #include @@ -52,6 +54,7 @@ private: void handle_usb_issue(int err, const char func[]); }; +#ifndef __APPLE__ class PandaSpiHandle : public PandaCommsHandle { public: PandaSpiHandle(std::string serial); @@ -75,3 +78,4 @@ private: int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); }; +#endif diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index bcc446b050..c9d4b2ea0e 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -1,3 +1,4 @@ +#ifndef __APPLE__ #include #include #include @@ -339,3 +340,4 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx transfer_fail: return ret; } +#endif diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index e9bbcb4586..b8ebbd88a3 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -51,7 +51,7 @@ class TestBoardd(unittest.TestCase): cp.safetyConfigs = [safety_config]*num_pandas params = Params() - params.put("CarVin", b"0"*17) + params.put_bool("FirmwareObdQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index bc5b36e2ee..a90c4cfb87 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -24,7 +24,7 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeedFactor = SPEED_FROM_RPM ret.centerToFront = ret.wheelbase * 0.44 - ret.radarOffCan = True + ret.radarUnavailable = True ret.openpilotLongitudinalControl = True ret.steerControlType = car.CarParams.SteerControlType.angle diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 88e8c72153..4ccce979d3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -99,7 +99,7 @@ def fingerprint(logcan, sendcan, num_pandas): else: cloudlog.warning("Getting VIN & FW versions") vin_rx_addr, vin = get_vin(logcan, sendcan, bus) - ecu_rx_addrs = get_present_ecus(logcan, sendcan) + ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) cached = False @@ -113,7 +113,10 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("Malformed VIN", vin=vin, error=True) vin = VIN_UNKNOWN cloudlog.warning("VIN %s", vin) - Params().put("CarVin", vin) + + params = Params() + params.put("CarVin", vin) + params.put_bool("FirmwareObdQueryDone", True) finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index c8504d6fb1..da086105e3 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config -from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags +from selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD - ret.radarOffCan = DBC[candidate]['radar'] is None + ret.radarUnavailable = True # DBC[candidate]['radar'] is None ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 348e3c3632..0ab8c10b44 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -45,12 +45,13 @@ def _address_to_track(address): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) + self.CP = CP self.rcp = _create_radar_can_parser(CP.carFingerprint) self.updated_messages = set() self.trigger_msg = LAST_MSG def update(self, can_strings): - if self.rcp is None: + if self.rcp is None or self.CP.radarUnavailable: return super().update(None) vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 9f6ace2b5f..e5d550fac8 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -87,5 +87,5 @@ if __name__ == "__main__": for addr, subaddr, bus in ecu_addrs: msg = f" 0x{hex(addr)}" if subaddr is not None: - msg += f" (sub-address: 0x{hex(subaddr)})" + msg += f" (sub-address: {hex(subaddr)})" print(msg) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 06da87bf34..00fb461223 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -2,8 +2,9 @@ from cereal import car from common.numpy_fast import clip from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_angle_limits -from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg -from selfdrive.car.ford.values import CANBUS, CarControllerParams +from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \ + create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg +from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -53,7 +54,29 @@ class CarController: 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.)) + + if self.CP.carFingerprint in CANFD_CARS: + # TODO: extended mode + mode = 1 if CC.latActive else 0 + counter = self.frame // CarControllerParams.STEER_STEP + can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter)) + else: + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.)) + + ### longitudinal control ### + # send acc command at 50Hz + if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + precharge_brake = accel < -0.1 + if accel > -0.5: + gas = accel + decel = False + else: + gas = -5.0 + decel = True + + can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 215900fef6..f97225e9f2 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -14,7 +14,7 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) if CP.transmissionType == TransmissionType.automatic: - self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] + self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -51,7 +51,7 @@ class CarState(CarStateBase): # gear if self.CP.transmissionType == TransmissionType.automatic: - gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None) + gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]) ret.gearShifter = self.parse_gear_shifter(gear) elif self.CP.transmissionType == TransmissionType.manual: ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 @@ -171,7 +171,7 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: signals += [ - ("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position + ("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position ] checks += [ ("Gear_Shift_by_Wire_FD1", 10), diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 9ddde80f87..594d50f59f 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -4,6 +4,15 @@ from selfdrive.car.ford.values import CANBUS HUDControl = car.CarControl.HUDControl +def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray): + checksum = mode + counter + checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature + checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate + checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle + checksum += (dat[4] & 0x3) + dat[5] # path offset + return 0xFF - (checksum & 0xFF) + + def create_lka_msg(packer): """ Creates an empty CAN message for the Ford LKA Command. @@ -16,7 +25,8 @@ def create_lka_msg(packer): return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) -def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: 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. @@ -55,6 +65,58 @@ def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) +def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float, + curvature_rate: float, counter: int): + """ + Create a CAN message for the new Ford Lane Centering command. + + This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has + additional signals for a counter and checksum. + + Frequency is 20Hz. + """ + + values = { + "LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode, + # 3=SafeRampOut, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians + "LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter + "LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2 + "HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1] + "LatCtlPath_No_Cnt": counter, # [0|15] + "LatCtlPath_No_Cs": 0, # [0|255] + } + + # calculate checksum + dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2] + values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat) + + return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values) + + +def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool): + """ + Creates a CAN message for the Ford ACC Command. + + This command can be used to enable ACC, to set the ACC gas/brake/decel values + and to disable ACC. + + Frequency is 50Hz. + """ + + values = { + "AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2 + "Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes + "AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2 + "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active + } + return packer.make_can_msg("ACCDATA", CANBUS.main, values) + + def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): """ Creates a CAN message for the Ford IPC IPMA/LKAS status. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 7c6a8ecbe4..8ce30e5d68 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -18,7 +18,7 @@ class CarInterface(CarInterfaceBase): # These cars are dashcam only until the port is finished ret.dashcamOnly = True - ret.radarOffCan = True + ret.radarUnavailable = True ret.steerControlType = car.CarParams.SteerControlType.angle ret.steerActuatorDelay = 0.2 ret.steerLimitTimer = 1.0 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index c942703002..ee4efb311d 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -47,7 +47,7 @@ class RadarInterface(RadarInterfaceBase): self.updated_messages = set() self.track_id = 0 self.radar = DBC[CP.carFingerprint]['radar'] - if self.radar is None: + if self.radar is None or CP.radarUnavailable: self.rcp = None elif self.radar == RADAR.DELPHI_ESR: self.rcp = _create_delphi_esr_radar_can_parser() diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index b93be61448..526b74b16c 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,7 +1,7 @@ from collections import defaultdict from dataclasses import dataclass from enum import Enum -from typing import Dict, List, Union +from typing import Dict, List, Set, Union from cereal import car from selfdrive.car import AngleRateLimit, dbc_dict @@ -14,6 +14,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: # Messages: Lane_Assist_Data1, LateralMotionControl STEER_STEP = 5 + # Message: ACCDATA + ACC_CONTROL_STEP = 2 # Message: IPMA_Data LKAS_UI_STEP = 100 # Message: ACCDATA_3 @@ -29,6 +31,9 @@ class CarControllerParams: 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]) + ACCEL_MAX = 2.0 # m/s^s max acceleration + ACCEL_MIN = -3.5 # m/s^s max deceleration + def __init__(self, CP): pass @@ -47,12 +52,15 @@ class CAR: MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" +CANFD_CARS: Set[str] = set() + + class RADAR: DELPHI_ESR = 'ford_fusion_2018_adas' DELPHI_MRR = 'FORD_CADS' -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", None)) +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) @dataclass @@ -67,7 +75,10 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { FordCarInfo("Ford Escape 2020-21"), FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"), ], - CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-22"), + CAR.EXPLORER_MK6: [ + FordCarInfo("Ford Explorer 2020-22"), + FordCarInfo("Lincoln Aviator 2021", "Co-Pilot360 Plus"), + ], CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022", "Co-Pilot360 Assist"), } @@ -157,13 +168,16 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x706, None): [ b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7E0, None): [ b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.shiftByWire, 0x732, None): [ + b'L1MP-14C561-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MP-14G395-JB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index f4d92ab960..0db260abbd 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -146,12 +146,16 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): return True, set() -def get_present_ecus(logcan, sendcan): +def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]: queries = list() parallel_queries = list() responses = set() for brand, r in REQUESTS: + # Skip query if no panda available + if r.bus > num_pandas * 4 - 1: + continue + for brand_versions in VERSIONS[brand].values(): for ecu_type, addr, sub_addr in brand_versions: # Only query ecus in whitelist if whitelist is not empty @@ -171,7 +175,7 @@ def get_present_ecus(logcan, sendcan): queries.insert(0, parallel_queries) - ecu_responses: Set[Tuple[int, Optional[int], int]] = set() + ecu_responses = set() for query in queries: ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) return ecu_responses diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b4a79d10a6..56f6ff5a31 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -50,9 +50,14 @@ class CarController: # Steering (Active: 50Hz, inactive: 10Hz) # Attempt to sync with camera on startup at 50Hz, first few msgs are blocked - init_lka_counter = not self.sent_lka_steering_cmd and self.CP.networkLocation == NetworkLocation.fwdCamera + init_lka_counter = not self.sent_lka_steering_cmd + # Also send at 50Hz until we're in sync with camera so counters align when relay closes, preventing a fault + # openpilot can subtly drift, so this is activated throughout a drive to stay synced + out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.camera_lka_steering_cmd_counter + 1) % 4 + sync_steer = (init_lka_counter or out_of_sync) and self.CP.networkLocation == NetworkLocation.fwdCamera + steer_step = self.params.INACTIVE_STEER_STEP - if CC.latActive or init_lka_counter: + if CC.latActive or sync_steer: steer_step = self.params.STEER_STEP # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the @@ -63,7 +68,7 @@ class CarController: elif (self.frame - self.last_steer_frame) >= steer_step: # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus if init_lka_counter: - self.lka_steering_cmd_counter = CS.camera_lka_steering_cmd_counter + 1 + self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1 if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) @@ -109,7 +114,7 @@ class CarController: # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) - if not self.CP.radarOffCan: + if not self.CP.radarUnavailable: tt = self.frame * DT_CTRL time_and_headlights_step = 10 if self.frame % time_and_headlights_step == 0: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index de0fd2eed6..8ea8ea8874 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -18,6 +18,7 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] self.loopback_lka_steering_cmd_updated = False + self.pt_lka_steering_cmd_counter = 0 self.camera_lka_steering_cmd_counter = 0 self.buttons_counter = 0 @@ -33,6 +34,7 @@ class CarState(CarStateBase): # Variables used for avoiding LKAS faults self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0 if self.CP.networkLocation == NetworkLocation.fwdCamera: + self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] self.camera_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] ret.wheelSpeeds = self.get_wheel_speeds( @@ -180,6 +182,15 @@ class CarState(CarStateBase): ("ECMAcceleratorPos", 80), ] + # Used to read back last counter sent to PT by camera + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + checks += [ + ("ASCMLKASteeringCmd", 0), + ] + if CP.transmissionType == TransmissionType.direct: signals.append(("RegenPaddle", "EBCMRegenPaddle")) checks.append(("EBCMRegenPaddle", 50)) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 856dcbaae5..3c2a12ef86 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -63,7 +63,7 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = True ret.networkLocation = NetworkLocation.fwdCamera - ret.radarOffCan = True # no radar + ret.radarUnavailable = True # no radar ret.pcmCruise = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM ret.minEnableSpeed = 5 * CV.KPH_TO_MS @@ -85,7 +85,7 @@ class CarInterface(CarInterfaceBase): else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway - ret.radarOffCan = False + ret.radarUnavailable = False ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 5cb211812d..b86a85f915 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -36,7 +36,7 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.rcp = None if CP.radarOffCan else create_radar_can_parser(CP.carFingerprint) + self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index da921ccda7..98243d81dd 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase): if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] - ret.radarOffCan = True + ret.radarUnavailable = True if candidate not in HONDA_BOSCH_RADARLESS: # Disable the radar and let openpilot control longitudinal diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 629ab01d4c..660be4c449 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -21,7 +21,7 @@ class RadarInterface(RadarInterfaceBase): self.track_id = 0 self.radar_fault = False self.radar_wrong_config = False - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.radar_ts = CP.radarTimeStep self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 07eed0cfc2..3d1c006aaa 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -1440,6 +1440,7 @@ FW_VERSIONS = { (Ecu.eps, 0x18DA30F1, None): [ b'39990-T39-A130\x00\x00', b'39990-T43-J020\x00\x00', + b'39990-T24-T120\x00\x00', ], (Ecu.gateway, 0x18DAEFF1, None): [ b'38897-T20-A020\x00\x00', @@ -1447,11 +1448,13 @@ FW_VERSIONS = { b'38897-T21-A010\x00\x00', b'38897-T20-A210\x00\x00', b'38897-T20-A310\x00\x00', + b'38897-T24-Z120\x00\x00', ], (Ecu.srs, 0x18DA53F1, None): [ b'77959-T20-A970\x00\x00', b'77959-T47-A940\x00\x00', b'77959-T47-A950\x00\x00', + b'77959-T20-M820\x00\x00', ], (Ecu.combinationMeter, 0x18DA60F1, None): [ b'78108-T21-A220\x00\x00', @@ -1459,22 +1462,26 @@ FW_VERSIONS = { b'78108-T23-A110\x00\x00', b'78108-T21-A230\x00\x00', b'78108-T22-A020\x00\x00', + b'78108-T21-MB10\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', + b'36161-T24-T070\x00\x00', ], (Ecu.vsa, 0x18DA28F1, None): [ b'57114-T20-AB40\x00\x00', b'57114-T43-JB30\x00\x00', + b'57114-T24-TB30\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-65D-A020\x00\x00', b'28101-65D-A120\x00\x00', b'28101-65H-A020\x00\x00', b'28101-65H-A120\x00\x00', + b'28101-65J-N010\x00\x00', ], (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-64L-A540\x00\x00', @@ -1482,6 +1489,7 @@ FW_VERSIONS = { b'37805-64S-A720\x00\x00', b'37805-64A-A540\x00\x00', b'37805-64A-A620\x00\x00', + b'37805-64D-P510\x00\x00', ], }, } diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b81c5e3f7d..6f043fddbb 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -181,7 +181,8 @@ class CarController: if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, - CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, + CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020): can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) # 5 Hz ACC options diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index c2ffffbf22..858f3d0876 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -21,7 +21,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, - CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022): + CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6d6d9833df..c708e2b78e 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "hyundai" - ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is @@ -166,7 +166,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.65 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - elif candidate == CAR.KIA_K5_2021: + elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020): ret.mass = 3228. * CV.LB_TO_KG ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 0d22611fb5..4ecca542b5 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -37,7 +37,7 @@ class RadarInterface(RadarInterfaceBase): self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 self.track_id = 0 - self.radar_off_can = CP.radarOffCan + self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) def update(self, can_strings): diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index c0cf23b50d..4c8e924c49 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -35,7 +35,7 @@ class CarControllerParams: # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.IONIQ, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -96,6 +96,7 @@ class CAR: # Kia KIA_FORTE = "KIA FORTE E 2018 & GT 2021" KIA_K5_2021 = "KIA K5 2021" + KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020" KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" @@ -147,7 +148,10 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { ], CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), - CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages + CAR.HYUNDAI_GENESIS: [ + HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages + HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + ], CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h), # TODO: confirm 2020-21 harness CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c), @@ -189,13 +193,17 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { # Kia CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a), + CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness=Harness.hyundai_a), CAR.KIA_NIRO_EV: [ HyundaiCarInfo("Kia Niro EV 2019", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), HyundaiCarInfo("Kia Niro EV 2020", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), HyundaiCarInfo("Kia Niro EV 2021", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), HyundaiCarInfo("Kia Niro EV 2022", "All", "https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), ], - CAR.KIA_NIRO_PHEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + CAR.KIA_NIRO_PHEV: [ + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness=Harness.hyundai_d), + ], CAR.KIA_NIRO_HEV_2021: [ HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), @@ -232,7 +240,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f), CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f), CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l), - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2017-19", "All", harness=Harness.hyundai_h), + CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h), CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), } @@ -364,6 +372,13 @@ FW_QUERY_CONFIG = FwQueryConfig( ) FW_VERSIONS = { + CAR.HYUNDAI_GENESIS: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS 1.1 -150210', + b'\xf1\x00DH LKAS 1.4 -140110', + b'\xf1\x00DH LKAS 1.5 -140425', + ], + }, CAR.IONIQ: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', @@ -401,17 +416,18 @@ FW_VERSIONS = { }, CAR.IONIQ_PHEV: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', + b'\xf1\x00AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', + b'\xf1\x00AEhe SCC F-CUP 1.00 1.02 99110-G2100 ', ], (Ecu.eps, 0x7d4, None): [ - b'\xf1\000AE MDPS C 1.00 1.01 56310/G2510 4APHC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2510 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2560 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310G2510\x00 4APHC101', ], (Ecu.fwdCamera, 0x7c4, None): [ - b'\xf1\000AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', b'\xf1\x00AEP MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2700 201027', ], @@ -424,6 +440,7 @@ FW_VERSIONS = { b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00', b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt', b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00', + b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL0\x00\x00\x00\x00', ], }, CAR.IONIQ_EV_2020: { @@ -669,7 +686,7 @@ FW_VERSIONS = { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650', b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650', - b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650', + b'\xf1\x00TM ESC \r 104\x19\x07\x08 58910-S2650', b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600', b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600', b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600', @@ -691,6 +708,7 @@ FW_VERSIONS = { b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', b'\xf1\x87LBJSGA7082574HG0\x87www\x98\x88\x88\x88\x99\xaa\xb9\x9afw\x86gx\x99\xa7\x89co\xf8\xffvU_\xffR\xaf\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\xa6\xe0\x91', b'\xf1\x87LBKSGA0458404HG0vfvg\x87www\x89\x99\xa8\x99y\xaa\xa7\x9ax\x88\xa7\x88t_\xf9\xff\x86w\x8f\xff\x15x\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\x00\x00\x00', b'\xf1\x87LDJUEA6010814HG1\x87w\x87x\x86gvw\x88\x88\x98\x88gw\x86wx\x88\x97\x88\x85o\xf8\xff\x86f_\xff\xd37\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', @@ -1028,6 +1046,25 @@ FW_VERSIONS = { b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.GENESIS_G80: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DH__ SCC F-CUP 1.00 1.01 96400-B1120 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.03 95895-B1500 180713', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.02 95895-B1500 170810', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.01 95895-B1500 161014', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c', + b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.GENESIS_G90: { (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7'], (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 '], @@ -1116,6 +1153,23 @@ FW_VERSIONS = { b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', ], }, + CAR.KIA_K5_HEV_2020: { + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00DLhe SCC FHCUP 1.00 1.02 99110-L7000 ', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L7000 4DLHC102', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00DL3HMFC AT KOR LHD 1.00 1.02 99210-L2000 200309', + ], + (Ecu.engine, 0x7E0, None): [ + b'\xf1\x87391162JLA0', + ], + (Ecu.transmission, 0x7E1, None): [ + b'\xf1\x00PSBG2323 E08\x00\x00\x00\x00\x00\x00\x00TDL2H20KA2\xe3\xc6cz', + ], + }, CAR.KONA_EV: { (Ecu.abs, 0x7D1, None): [ b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', @@ -1202,22 +1256,27 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91", b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', + b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL2&[\xc3\x01', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', + b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', + b'\xf1\x00DEP MFC AT USA LHD 1.00 1.05 99211-G5000 190826', ], (Ecu.fwdRadar, 0x7D0, None): [ b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ', + b'\xf1\x00DEhe SCC F-CUP 1.00 1.02 99110-G5100 ', ], }, CAR.KIA_NIRO_HEV_2021: { @@ -1493,6 +1552,7 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007', ], }, CAR.TUCSON_4TH_GEN: { @@ -1570,7 +1630,7 @@ FW_VERSIONS = { } CHECKSUM = { - "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022], + "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.KIA_K5_HEV_2020], "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], } @@ -1578,10 +1638,10 @@ FEATURES = { # which message has the gear "use_cluster_gears": {CAR.ELANTRA, CAR.KONA}, "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, - "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022}, + "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020}, # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022}, + "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020}, } CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN} @@ -1592,7 +1652,7 @@ CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, C # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # these cars use a different gas signal +HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020} # these cars use a different gas signal EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} # these cars require a special panda safety mode due to missing counters and checksums in the messages @@ -1617,6 +1677,7 @@ DBC = { CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7192f5252c..e75067da7e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -401,15 +401,15 @@ class CarStateBase(ABC): return GearShifter.unknown d: Dict[str, car.CarState.GearShifter] = { - 'P': GearShifter.park, 'PARK': GearShifter.park, - 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, - 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, - 'E': GearShifter.eco, 'ECO': GearShifter.eco, - 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, - 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, - 'S': GearShifter.sport, 'SPORT': GearShifter.sport, - 'L': GearShifter.low, 'LOW': GearShifter.low, - 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, + 'P': GearShifter.park, 'PARK': GearShifter.park, + 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, + 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, + 'E': GearShifter.eco, 'ECO': GearShifter.eco, + 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, + 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, + 'S': GearShifter.sport, 'SPORT': GearShifter.sport, + 'L': GearShifter.low, 'LOW': GearShifter.low, + 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, } return d.get(gear.upper(), GearShifter.unknown) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fdd2439ff9..65444ff6e0 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 386e859089..2e769cf662 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -19,7 +19,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle - ret.radarOffCan = True + ret.radarUnavailable = True if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 22468801ec..f94333baab 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "subaru" - ret.radarOffCan = True + ret.radarUnavailable = True ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.autoResumeSng = False diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index d1b73afcf6..ce38eeb0de 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -26,6 +26,7 @@ non_tested_cars = [ HYUNDAI.GENESIS_G90, HYUNDAI.KIA_OPTIMA_H, HONDA.ODYSSEY_CHN, + VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter ] CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,)) @@ -122,6 +123,7 @@ routes = [ CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6), # HDA2 CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6), # HDA1 CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), + CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020), CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV), CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV), CarTestRoute("34a875f29f69841a|2021-07-29--13-02-09", HYUNDAI.KIA_NIRO_HEV_2021), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index deb0454b9c..24c995a2d0 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -76,7 +76,7 @@ class TestCarInterfaces(unittest.TestCase): # Run radar interface once radar_interface.update([]) - if not car_params.radarOffCan and radar_interface.rcp is not None and \ + if not car_params.radarUnavailable and radar_interface.rcp is not None and \ hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): radar_interface._update([radar_interface.trigger_msg]) diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 397b29525d..7da508a156 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -25,7 +25,7 @@ HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819] -HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171] +HYUNDAI GENESIS 2015-2016: [2.7807965280270794, 2.325, 0.0984484465421171] HYUNDAI IONIQ 5 2022: [3.172929, 2.713050, 0.096019] HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index aeb2e6f280..1ac43efc39 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -37,6 +37,7 @@ HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 +KIA K5 HYBRID 2020: KIA K5 2021 KIA STINGER 2022: KIA STINGER GT2 2018 GENESIS G90 2017: GENESIS G70 2018 GENESIS G80 2017: GENESIS G70 2018 @@ -59,6 +60,7 @@ SKODA SCALA 1ST GEN: SKODA SUPERB 3RD GEN SKODA KODIAQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAROQ 1ST GEN: SKODA SUPERB 3RD GEN SKODA KAMIQ 1ST GEN: SKODA SUPERB 3RD GEN +VOLKSWAGEN CRAFTER 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-ROC 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN T-CROSS 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN VOLKSWAGEN TOURAN 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6dbdd4b5d9..222cf70613 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -143,7 +143,7 @@ class CarController: # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True - if self.frame % 100 == 0 or send_ui: + if self.frame % 20 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index c413028a25..490c94db75 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -125,6 +125,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { ], CAR.COROLLAH_TSS2: [ ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), + ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), ToyotaCarInfo("Lexus UX Hybrid 2019-22"), ], @@ -841,11 +842,13 @@ FW_VERSIONS = { b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZK8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZZ0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -858,6 +861,7 @@ FW_VERSIONS = { b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B12451\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'8965B16101\x00\x00\x00\x00\x00\x00', b'8965B76012\x00\x00\x00\x00\x00\x00', b'8965B76050\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', @@ -905,6 +909,7 @@ FW_VERSIONS = { b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1601200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b"\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00", b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index da0ce25afa..1e620b4f73 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -23,7 +23,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" - ret.radarOffCan = True + ret.radarUnavailable = True use_off_car_defaults = len(fingerprint[0]) == 0 # Pick sensible carParams during offline doc generation/CI jobs @@ -107,6 +107,11 @@ class CarInterface(CarInterfaceBase): ret.mass = 2011 + STD_CARGO_KG ret.wheelbase = 2.98 + elif candidate == CAR.CRAFTER_MK2: + ret.mass = 2100 + STD_CARGO_KG + ret.wheelbase = 3.64 # SWB, LWB is 4.49, TBD how to detect difference + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + elif candidate == CAR.GOLF_MK7: ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index bdb43c542a..9f6dee2689 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -111,6 +111,7 @@ class CANBUS: class CAR: ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport + CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN" # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants @@ -122,7 +123,7 @@ class CAR: TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California - TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW VW T-Roc and variants + TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW T-Roc and variants AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants @@ -184,6 +185,13 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), VWCarInfo("Volkswagen Teramont X 2021-22"), ], + CAR.CRAFTER_MK2: [ + VWCarInfo("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), + VWCarInfo("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), + ], CAR.GOLF_MK7: [ VWCarInfo("Volkswagen e-Golf 2014-20"), VWCarInfo("Volkswagen Golf 2015-20"), @@ -352,6 +360,23 @@ FW_VERSIONS = { b'\xf1\x875Q0907572P \xf1\x890682', ], }, + CAR.CRAFTER_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906056EK\xf1\x896391', + ], + # Only current upstreamed vehicle has a manual transmission + #(Ecu.transmission, 0x7e1, None): [ + #], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BG\xf1\x890703\xf1\x82\x0e16120016130012051G1313052900', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + ], + }, CAR.GOLF_MK7: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906016A \xf1\x897697', @@ -942,16 +967,19 @@ FW_VERSIONS = { b'\xf1\x8704L906021EL\xf1\x897542', b'\xf1\x8704L906026BP\xf1\x891198', b'\xf1\x8704L906026BP\xf1\x897608', + b'\xf1\x8704L906056CR\xf1\x892797', b'\xf1\x8705E906018AS\xf1\x899596', b'\xf1\x878V0906264H \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041D \xf1\x891004', b'\xf1\x870CW300041G \xf1\x891003', b'\xf1\x870CW300050J \xf1\x891908', b'\xf1\x870D9300042M \xf1\x895016', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AC\xf1\x890189\xf1\x82\r11110011110011021511110200', + b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11110011110011021511110200', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r12110012120012021612110200', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1312001313001305171311052900', b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\0161312001313001305171311052900', @@ -965,6 +993,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101', b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572K \xf1\x890402\xf1\x82\x0101', b'\xf1\x875Q0907572P \xf1\x890682', ], }, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21d8fe2d66..fff6bcf576 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -173,6 +173,7 @@ class Controls: self.cruise_mismatch_counter = 0 self.can_rcv_timeout_counter = 0 self.last_blinker_frame = 0 + self.last_steering_pressed_frame = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] @@ -623,29 +624,34 @@ class Controls: lac_log.output = actuators.steer lac_log.saturated = abs(actuators.steer) >= 0.9 + if CS.steeringPressed: + self.last_steering_pressed_frame = self.sm.frame + recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 + # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: - undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 - turning = abs(lac_log.desiredLateralAccel) > 1.0 - good_speed = CS.vEgo > 5 - max_torque = abs(self.last_actuators.steer) > 0.99 - if undershooting and turning and good_speed and max_torque: - self.events.add(EventName.steerSaturated) - elif lac_log.active and lac_log.saturated: - dpath_points = lat_plan.dPathPoints - if len(dpath_points): - # Check if we deviated from the path - # TODO use desired vs actual curvature - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - steering_value = actuators.steeringAngleDeg - else: - steering_value = actuators.steer + if lac_log.active and not recent_steer_pressed: + if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: + undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 + turning = abs(lac_log.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.last_actuators.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + lac_log.active and self.events.add(EventName.steerSaturated) + elif lac_log.saturated: + dpath_points = lat_plan.dPathPoints + if len(dpath_points): + # Check if we deviated from the path + # TODO use desired vs actual curvature + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + steering_value = actuators.steeringAngleDeg + else: + steering_value = actuators.steer - left_deviation = steering_value > 0 and dpath_points[0] < -0.20 - right_deviation = steering_value < 0 and dpath_points[0] > 0.20 + left_deviation = steering_value > 0 and dpath_points[0] < -0.20 + right_deviation = steering_value < 0 and dpath_points[0] > 0.20 - if left_deviation or right_deviation: - self.events.add(EventName.steerSaturated) + if left_deviation or right_deviation: + self.events.add(EventName.steerSaturated) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index ac7e7102d0..07ce5ebddb 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -52,6 +52,10 @@ SUPPORTED_FW_VERSIONS = { b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2019 SANTA FE + b"TM__ SCC F-CUP 1.00 1.00 99110-S1210\x19\x01%\x168 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 5268902785..86a228a6ff 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -3,3 +3,4 @@ ubloxd_test params_learner paramsd locationd +test/test_glonass_runner diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 7024196efd..61a0ed7f42 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -13,7 +13,8 @@ if GetOption('kaitai'): 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) +glonass_obj = env.Object('generated/glonass.cpp') +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] @@ -25,3 +26,6 @@ lenv.Depends(locationd, libkf) if File("liblocationd.cc").exists(): liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) lenv.Depends(liblocationd, libkf) + +if GetOption('test'): + env.Program("test/test_glonass_runner", ['test/test_glonass_runner.cc', 'test/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp index 149134fbb5..cd0f96ab68 100644 --- a/selfdrive/locationd/generated/glonass.cpp +++ b/selfdrive/locationd/generated/glonass.cpp @@ -31,6 +31,10 @@ void glonass_t::_read() { m_data = new string_3_t(m__io, this, m__root); break; } + case 5: { + m_data = new string_5_t(m__io, this, m__root); + break; + } case 2: { m_data = new string_2_t(m__io, this, m__root); break; @@ -60,6 +64,8 @@ void glonass_t::_clean_up() { glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; + f_tau_n = false; + f_delta_tau_n = false; try { _read(); @@ -70,8 +76,10 @@ glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, } 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_tau_n_sign = m__io->read_bits_int_be(1); + m_tau_n_value = m__io->read_bits_int_be(21); + m_delta_tau_n_sign = m__io->read_bits_int_be(1); + m_delta_tau_n_value = m__io->read_bits_int_be(4); m_e_n = m__io->read_bits_int_be(5); m_not_used_1 = m__io->read_bits_int_be(14); m_p4 = m__io->read_bits_int_be(1); @@ -89,6 +97,22 @@ glonass_t::string_4_t::~string_4_t() { void glonass_t::string_4_t::_clean_up() { } +int32_t glonass_t::string_4_t::tau_n() { + if (f_tau_n) + return m_tau_n; + m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value())); + f_tau_n = true; + return m_tau_n; +} + +int32_t glonass_t::string_4_t::delta_tau_n() { + if (f_delta_tau_n) + return m_delta_tau_n; + m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value())); + f_delta_tau_n = true; + return m_delta_tau_n; +} + glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; @@ -113,9 +137,40 @@ glonass_t::string_non_immediate_t::~string_non_immediate_t() { void glonass_t::string_non_immediate_t::_clean_up() { } +glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void glonass_t::string_5_t::_read() { + m_n_a = m__io->read_bits_int_be(11); + m_tau_c = m__io->read_bits_int_be(32); + m_not_used = m__io->read_bits_int_be(1); + m_n_4 = m__io->read_bits_int_be(5); + m_tau_gps = m__io->read_bits_int_be(22); + m_l_n = m__io->read_bits_int_be(1); +} + +glonass_t::string_5_t::~string_5_t() { + _clean_up(); +} + +void glonass_t::string_5_t::_clean_up() { +} + glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; + f_x_vel = false; + f_x_accel = false; + f_x = false; try { _read(); @@ -129,9 +184,12 @@ 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); + m_x_vel_sign = m__io->read_bits_int_be(1); + m_x_vel_value = m__io->read_bits_int_be(23); + m_x_accel_sign = m__io->read_bits_int_be(1); + m_x_accel_value = m__io->read_bits_int_be(4); + m_x_sign = m__io->read_bits_int_be(1); + m_x_value = m__io->read_bits_int_be(26); } glonass_t::string_1_t::~string_1_t() { @@ -141,9 +199,36 @@ glonass_t::string_1_t::~string_1_t() { void glonass_t::string_1_t::_clean_up() { } +int32_t glonass_t::string_1_t::x_vel() { + if (f_x_vel) + return m_x_vel; + m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value())); + f_x_vel = true; + return m_x_vel; +} + +int32_t glonass_t::string_1_t::x_accel() { + if (f_x_accel) + return m_x_accel; + m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value())); + f_x_accel = true; + return m_x_accel; +} + +int32_t glonass_t::string_1_t::x() { + if (f_x) + return m_x; + m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value())); + f_x = true; + return m_x; +} + glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; + f_y_vel = false; + f_y_accel = false; + f_y = false; try { _read(); @@ -158,9 +243,12 @@ void glonass_t::string_2_t::_read() { 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); + m_y_vel_sign = m__io->read_bits_int_be(1); + m_y_vel_value = m__io->read_bits_int_be(23); + m_y_accel_sign = m__io->read_bits_int_be(1); + m_y_accel_value = m__io->read_bits_int_be(4); + m_y_sign = m__io->read_bits_int_be(1); + m_y_value = m__io->read_bits_int_be(26); } glonass_t::string_2_t::~string_2_t() { @@ -170,9 +258,37 @@ glonass_t::string_2_t::~string_2_t() { void glonass_t::string_2_t::_clean_up() { } +int32_t glonass_t::string_2_t::y_vel() { + if (f_y_vel) + return m_y_vel; + m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value())); + f_y_vel = true; + return m_y_vel; +} + +int32_t glonass_t::string_2_t::y_accel() { + if (f_y_accel) + return m_y_accel; + m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value())); + f_y_accel = true; + return m_y_accel; +} + +int32_t glonass_t::string_2_t::y() { + if (f_y) + return m_y; + m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value())); + f_y = true; + return m_y; +} + glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; + f_gamma_n = false; + f_z_vel = false; + f_z_accel = false; + f_z = false; try { _read(); @@ -184,13 +300,17 @@ glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, 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_gamma_n_sign = m__io->read_bits_int_be(1); + m_gamma_n_value = m__io->read_bits_int_be(10); m_not_used = m__io->read_bits_int_be(1); m_p = m__io->read_bits_int_be(2); m_l_n = m__io->read_bits_int_be(1); - m_z_vel = 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); + m_z_vel_sign = m__io->read_bits_int_be(1); + m_z_vel_value = m__io->read_bits_int_be(23); + m_z_accel_sign = m__io->read_bits_int_be(1); + m_z_accel_value = m__io->read_bits_int_be(4); + m_z_sign = m__io->read_bits_int_be(1); + m_z_value = m__io->read_bits_int_be(26); } glonass_t::string_3_t::~string_3_t() { @@ -199,3 +319,35 @@ glonass_t::string_3_t::~string_3_t() { void glonass_t::string_3_t::_clean_up() { } + +int32_t glonass_t::string_3_t::gamma_n() { + if (f_gamma_n) + return m_gamma_n; + m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value())); + f_gamma_n = true; + return m_gamma_n; +} + +int32_t glonass_t::string_3_t::z_vel() { + if (f_z_vel) + return m_z_vel; + m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value())); + f_z_vel = true; + return m_z_vel; +} + +int32_t glonass_t::string_3_t::z_accel() { + if (f_z_accel) + return m_z_accel; + m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value())); + f_z_accel = true; + return m_z_accel; +} + +int32_t glonass_t::string_3_t::z() { + if (f_z) + return m_z; + m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value())); + f_z = true; + return m_z; +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h index 48bdabfe96..19867ba22b 100644 --- a/selfdrive/locationd/generated/glonass.h +++ b/selfdrive/locationd/generated/glonass.h @@ -15,6 +15,7 @@ class glonass_t : public kaitai::kstruct { public: class string_4_t; class string_non_immediate_t; + class string_5_t; class string_1_t; class string_2_t; class string_3_t; @@ -42,8 +43,24 @@ public: ~string_4_t(); private: - uint64_t m_tau_n; - uint64_t m_delta_tau_n; + bool f_tau_n; + int32_t m_tau_n; + + public: + int32_t tau_n(); + + private: + bool f_delta_tau_n; + int32_t m_delta_tau_n; + + public: + int32_t delta_tau_n(); + + private: + bool m_tau_n_sign; + uint64_t m_tau_n_value; + bool m_delta_tau_n_sign; + uint64_t m_delta_tau_n_value; uint64_t m_e_n; uint64_t m_not_used_1; bool m_p4; @@ -56,8 +73,10 @@ public: 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; } + bool tau_n_sign() const { return m_tau_n_sign; } + uint64_t tau_n_value() const { return m_tau_n_value; } + bool delta_tau_n_sign() const { return m_delta_tau_n_sign; } + uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; } uint64_t e_n() const { return m_e_n; } uint64_t not_used_1() const { return m_not_used_1; } bool p4() const { return m_p4; } @@ -96,6 +115,40 @@ public: glonass_t* _parent() const { return m__parent; } }; + class string_5_t : public kaitai::kstruct { + + public: + + string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~string_5_t(); + + private: + uint64_t m_n_a; + uint64_t m_tau_c; + bool m_not_used; + uint64_t m_n_4; + uint64_t m_tau_gps; + bool m_l_n; + glonass_t* m__root; + glonass_t* m__parent; + + public: + uint64_t n_a() const { return m_n_a; } + uint64_t tau_c() const { return m_tau_c; } + bool not_used() const { return m_not_used; } + uint64_t n_4() const { return m_n_4; } + uint64_t tau_gps() const { return m_tau_gps; } + bool l_n() const { return m_l_n; } + glonass_t* _root() const { return m__root; } + glonass_t* _parent() const { return m__parent; } + }; + class string_1_t : public kaitai::kstruct { public: @@ -109,13 +162,37 @@ public: public: ~string_1_t(); + private: + bool f_x_vel; + int32_t m_x_vel; + + public: + int32_t x_vel(); + + private: + bool f_x_accel; + int32_t m_x_accel; + + public: + int32_t x_accel(); + + private: + bool f_x; + int32_t m_x; + + public: + int32_t x(); + private: uint64_t m_not_used; uint64_t m_p1; uint64_t m_t_k; - uint64_t m_x_vel; - uint64_t m_x_speedup; - uint64_t m_x; + bool m_x_vel_sign; + uint64_t m_x_vel_value; + bool m_x_accel_sign; + uint64_t m_x_accel_value; + bool m_x_sign; + uint64_t m_x_value; glonass_t* m__root; glonass_t* m__parent; @@ -123,9 +200,12 @@ 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; } + bool x_vel_sign() const { return m_x_vel_sign; } + uint64_t x_vel_value() const { return m_x_vel_value; } + bool x_accel_sign() const { return m_x_accel_sign; } + uint64_t x_accel_value() const { return m_x_accel_value; } + bool x_sign() const { return m_x_sign; } + uint64_t x_value() const { return m_x_value; } glonass_t* _root() const { return m__root; } glonass_t* _parent() const { return m__parent; } }; @@ -143,14 +223,38 @@ public: public: ~string_2_t(); + private: + bool f_y_vel; + int32_t m_y_vel; + + public: + int32_t y_vel(); + + private: + bool f_y_accel; + int32_t m_y_accel; + + public: + int32_t y_accel(); + + private: + bool f_y; + int32_t m_y; + + public: + int32_t y(); + private: uint64_t m_b_n; bool m_p2; uint64_t m_t_b; uint64_t m_not_used; - uint64_t m_y_vel; - uint64_t m_y_speedup; - uint64_t m_y; + bool m_y_vel_sign; + uint64_t m_y_vel_value; + bool m_y_accel_sign; + uint64_t m_y_accel_value; + bool m_y_sign; + uint64_t m_y_value; glonass_t* m__root; glonass_t* m__parent; @@ -159,9 +263,12 @@ public: 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; } + bool y_vel_sign() const { return m_y_vel_sign; } + uint64_t y_vel_value() const { return m_y_vel_value; } + bool y_accel_sign() const { return m_y_accel_sign; } + uint64_t y_accel_value() const { return m_y_accel_value; } + bool y_sign() const { return m_y_sign; } + uint64_t y_value() const { return m_y_value; } glonass_t* _root() const { return m__root; } glonass_t* _parent() const { return m__parent; } }; @@ -179,27 +286,63 @@ public: public: ~string_3_t(); + private: + bool f_gamma_n; + int32_t m_gamma_n; + + public: + int32_t gamma_n(); + + private: + bool f_z_vel; + int32_t m_z_vel; + + public: + int32_t z_vel(); + + private: + bool f_z_accel; + int32_t m_z_accel; + + public: + int32_t z_accel(); + + private: + bool f_z; + int32_t m_z; + + public: + int32_t z(); + private: bool m_p3; - uint64_t m_gamma_n; + bool m_gamma_n_sign; + uint64_t m_gamma_n_value; 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; + bool m_z_vel_sign; + uint64_t m_z_vel_value; + bool m_z_accel_sign; + uint64_t m_z_accel_value; + bool m_z_sign; + uint64_t m_z_value; glonass_t* m__root; glonass_t* m__parent; public: bool p3() const { return m_p3; } - uint64_t gamma_n() const { return m_gamma_n; } + bool gamma_n_sign() const { return m_gamma_n_sign; } + uint64_t gamma_n_value() const { return m_gamma_n_value; } bool not_used() const { return m_not_used; } uint64_t p() const { return m_p; } bool l_n() const { return m_l_n; } - 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; } + bool z_vel_sign() const { return m_z_vel_sign; } + uint64_t z_vel_value() const { return m_z_vel_value; } + bool z_accel_sign() const { return m_z_accel_sign; } + uint64_t z_accel_value() const { return m_z_accel_value; } + bool z_sign() const { return m_z_sign; } + uint64_t z_value() const { return m_z_value; } glonass_t* _root() const { return m__root; } glonass_t* _parent() const { return m__parent; } }; diff --git a/selfdrive/locationd/glonass.ksy b/selfdrive/locationd/glonass.ksy index 95ca78c16f..be99f6e497 100644 --- a/selfdrive/locationd/glonass.ksy +++ b/selfdrive/locationd/glonass.ksy @@ -1,4 +1,6 @@ # http://gauss.gge.unb.ca/GLONASS.ICD.pdf +# some variables are misprinted but good in the old doc +# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf meta: id: glonass endian: be @@ -16,6 +18,7 @@ seq: 2: string_2 3: string_3 4: string_4 + 5: string_5 _: string_non_immediate - id: hamming_code type: b8 @@ -37,12 +40,25 @@ types: type: b2 - id: t_k type: b12 - - id: x_vel - type: b24 - - id: x_speedup - type: b5 - - id: x - type: b27 + - id: x_vel_sign + type: b1 + - id: x_vel_value + type: b23 + - id: x_accel_sign + type: b1 + - id: x_accel_value + type: b4 + - id: x_sign + type: b1 + - id: x_value + type: b26 + instances: + x_vel: + value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value' + x_accel: + value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value' + x: + value: 'x_sign ? (x_value * (-1)) : x_value' string_2: seq: - id: b_n @@ -53,36 +69,70 @@ types: type: b7 - id: not_used type: b5 - - id: y_vel - type: b24 - - id: y_speedup - type: b5 - - id: y - type: b27 + - id: y_vel_sign + type: b1 + - id: y_vel_value + type: b23 + - id: y_accel_sign + type: b1 + - id: y_accel_value + type: b4 + - id: y_sign + type: b1 + - id: y_value + type: b26 + instances: + y_vel: + value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value' + y_accel: + value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value' + y: + value: 'y_sign ? (y_value * (-1)) : y_value' string_3: seq: - id: p3 type: b1 - - id: gamma_n - type: b11 + - id: gamma_n_sign + type: b1 + - id: gamma_n_value + type: b10 - id: not_used type: b1 - id: p type: b2 - id: l_n type: b1 - - id: z_vel - type: b24 - - id: z_speedup - type: b5 - - id: z - type: b27 + - id: z_vel_sign + type: b1 + - id: z_vel_value + type: b23 + - id: z_accel_sign + type: b1 + - id: z_accel_value + type: b4 + - id: z_sign + type: b1 + - id: z_value + type: b26 + instances: + gamma_n: + value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value' + z_vel: + value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value' + z_accel: + value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value' + z: + value: 'z_sign ? (z_value * (-1)) : z_value' string_4: seq: - - id: tau_n - type: b22 - - id: delta_tau_n - type: b5 + - id: tau_n_sign + type: b1 + - id: tau_n_value + type: b21 + - id: delta_tau_n_sign + type: b1 + - id: delta_tau_n_value + type: b4 - id: e_n type: b5 - id: not_used_1 @@ -99,9 +149,28 @@ types: type: b5 - id: m type: b2 + instances: + tau_n: + value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value' + delta_tau_n: + value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value' + string_5: + seq: + - id: n_a + type: b11 + - id: tau_c + type: b32 + - id: not_used + type: b1 + - id: n_4 + type: b5 + - id: tau_gps + type: b22 + - id: l_n + type: b1 string_non_immediate: seq: - id: data_1 type: b64 - id: data_2 - type: b8 \ No newline at end of file + type: b8 diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch index 2f99e93637..fa34a8ef15 100644 --- a/selfdrive/locationd/glonass_fix.patch +++ b/selfdrive/locationd/glonass_fix.patch @@ -1,5 +1,5 @@ diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp -index 6a48fe62c..149134fbb 100644 +index 5b17bc327..b5c6aa610 100644 --- a/selfdrive/locationd/generated/glonass.cpp +++ b/selfdrive/locationd/generated/glonass.cpp @@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index b8d86c63fd..e85cd0ad42 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -17,7 +17,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -139,17 +139,22 @@ class Laikad: if self.use_qcom: return gnss_msg.which() == 'drSvPoly' else: - return gnss_msg.which() == 'ephemeris' + return gnss_msg.which() in ('ephemeris', 'glonassEphemeris') def read_ephemeris(self, gnss_msg): - # TODO this only works on GLONASS if self.use_qcom: # TODO this is not robust to gps week rollover if self.gps_week is None: return ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) else: - ephem = convert_ublox_ephem(gnss_msg.ephemeris) + if gnss_msg.which() == 'ephemeris': + ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris) + elif gnss_msg.which() == 'glonassEphemeris': + ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris) + else: + cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") + return self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) @@ -410,8 +415,13 @@ def main(sm=None, pm=None, qc=None): if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) + # disable until set as main gps source, to better analyze startup time + use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ + replay = "REPLAY" in os.environ - use_internet = "LAIKAD_NO_INTERNET" not in os.environ + if replay or "CI" in os.environ: + use_internet = True + laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 7e7c2b091f..7e30b1e3a7 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -14,8 +14,9 @@ from system.swaglog import cloudlog MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) +ROLL_STD_MAX = math.radians(1.5) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 @@ -37,8 +38,7 @@ class ParamsLearner: self.yaw_rate_std = 0.0 self.roll = 0.0 self.steering_angle = 0.0 - - self.valid = True + self.roll_valid = False def handle_log(self, t, which, msg): if which == 'liveLocationKalman': @@ -47,8 +47,8 @@ class ParamsLearner: localizer_roll = msg.orientationNED.value[0] localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] - roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX - if roll_valid: + self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + if self.roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? roll_std = 2 * localizer_roll_std @@ -156,6 +156,7 @@ def main(sm=None, pm=None): learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) angle_offset_average = params['angleOffsetAverageDeg'] angle_offset = angle_offset_average + roll = 0.0 while True: sm.update() @@ -175,6 +176,8 @@ def main(sm=None, pm=None): angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) + roll = clip(float(x[States.ROAD_ROLL]), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA) + roll_std = float(P[States.ROAD_ROLL]) # Account for the opposite signs of the yaw rates sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) @@ -185,12 +188,14 @@ def main(sm=None, pm=None): liveParameters.sensorValid = sensors_valid liveParameters.steerRatio = float(x[States.STEER_RATIO]) liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) - liveParameters.roll = float(x[States.ROAD_ROLL]) + liveParameters.roll = roll liveParameters.angleOffsetAverageDeg = angle_offset_average liveParameters.angleOffsetDeg = angle_offset liveParameters.valid = all(( abs(liveParameters.angleOffsetAverageDeg) < 10.0, abs(liveParameters.angleOffsetDeg) < 10.0, + abs(liveParameters.roll) < ROLL_MAX, + roll_std < ROLL_STD_MAX, 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) diff --git a/selfdrive/locationd/test/test_glonass_kaitai.cc b/selfdrive/locationd/test/test_glonass_kaitai.cc new file mode 100644 index 0000000000..22f5202a3d --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "selfdrive/locationd/generated/glonass.h" + +typedef std::vector> string_data; + +#define IDLE_CHIP_IDX 0 +#define STRING_NUMBER_IDX 1 +// string data 1-5 +#define HC_IDX 0 +#define PAD1_IDX 1 +#define SUPERFRAME_IDX 2 +#define PAD2_IDX 3 +#define FRAME_IDX 4 + +// Indexes for string number 1 +#define ST1_NU_IDX 2 +#define ST1_P1_IDX 3 +#define ST1_T_K_IDX 4 +#define ST1_X_VEL_S_IDX 5 +#define ST1_X_VEL_V_IDX 6 +#define ST1_X_ACCEL_S_IDX 7 +#define ST1_X_ACCEL_V_IDX 8 +#define ST1_X_S_IDX 9 +#define ST1_X_V_IDX 10 +#define ST1_HC_OFF 11 + +// Indexes for string number 2 +#define ST2_BN_IDX 2 +#define ST2_P2_IDX 3 +#define ST2_TB_IDX 4 +#define ST2_NU_IDX 5 +#define ST2_Y_VEL_S_IDX 6 +#define ST2_Y_VEL_V_IDX 7 +#define ST2_Y_ACCEL_S_IDX 8 +#define ST2_Y_ACCEL_V_IDX 9 +#define ST2_Y_S_IDX 10 +#define ST2_Y_V_IDX 11 +#define ST2_HC_OFF 12 + +// Indexes for string number 3 +#define ST3_P3_IDX 2 +#define ST3_GAMMA_N_S_IDX 3 +#define ST3_GAMMA_N_V_IDX 4 +#define ST3_NU_1_IDX 5 +#define ST3_P_IDX 6 +#define ST3_L_N_IDX 7 +#define ST3_Z_VEL_S_IDX 8 +#define ST3_Z_VEL_V_IDX 9 +#define ST3_Z_ACCEL_S_IDX 10 +#define ST3_Z_ACCEL_V_IDX 11 +#define ST3_Z_S_IDX 12 +#define ST3_Z_V_IDX 13 +#define ST3_HC_OFF 14 + +// Indexes for string number 4 +#define ST4_TAU_N_S_IDX 2 +#define ST4_TAU_N_V_IDX 3 +#define ST4_DELTA_TAU_N_S_IDX 4 +#define ST4_DELTA_TAU_N_V_IDX 5 +#define ST4_E_N_IDX 6 +#define ST4_NU_1_IDX 7 +#define ST4_P4_IDX 8 +#define ST4_F_T_IDX 9 +#define ST4_NU_2_IDX 10 +#define ST4_N_T_IDX 11 +#define ST4_N_IDX 12 +#define ST4_M_IDX 13 +#define ST4_HC_OFF 14 + +// Indexes for string number 5 +#define ST5_N_A_IDX 2 +#define ST5_TAU_C_IDX 3 +#define ST5_NU_IDX 4 +#define ST5_N_4_IDX 5 +#define ST5_TAU_GPS_IDX 6 +#define ST5_L_N_IDX 7 +#define ST5_HC_OFF 8 + +// Indexes for non immediate +#define ST6_DATA_1_IDX 2 +#define ST6_DATA_2_IDX 3 +#define ST6_HC_OFF 4 + + +std::string generate_inp_data(string_data& data) { + std::string inp_data = ""; + for (auto& [b, v] : data) { + std::string tmp = std::bitset<64>(v).to_string(); + inp_data += tmp.substr(64-b, b); + } + assert(inp_data.size() == 128); + + std::string string_data; + string_data.reserve(16); + for (int i = 0; i < 128; i+=8) { + std::string substr = inp_data.substr(i, 8); + string_data.push_back( (uint8_t)std::stoi(substr.c_str(), 0, 2)); + } + + return string_data; +} + +string_data generate_string_data(uint8_t string_number) { + + srand((unsigned)time(0)); + string_data data; // + data.push_back({1, 0}); // idle chip + data.push_back({4, string_number}); // string number + + if (string_number == 1) { + data.push_back({2, 3}); // not_used + data.push_back({2, 1}); // p1 + data.push_back({12, 113}); // t_k + data.push_back({1, rand() & 1}); // x_vel_sign + data.push_back({23, 7122}); // x_vel_value + data.push_back({1, rand() & 1}); // x_accel_sign + data.push_back({4, 3}); // x_accel_value + data.push_back({1, rand() & 1}); // x_sign + data.push_back({26, 33554431}); // x_value + } else if (string_number == 2) { + data.push_back({3, 3}); // b_n + data.push_back({1, 1}); // p2 + data.push_back({7, 123}); // t_b + data.push_back({5, 31}); // not_used + data.push_back({1, rand() & 1}); // y_vel_sign + data.push_back({23, 7422}); // y_vel_value + data.push_back({1, rand() & 1}); // y_accel_sign + data.push_back({4, 3}); // y_accel_value + data.push_back({1, rand() & 1}); // y_sign + data.push_back({26, 67108863}); // y_value + } else if (string_number == 3) { + data.push_back({1, 0}); // p3 + data.push_back({1, 1}); // gamma_n_sign + data.push_back({10, 123}); // gamma_n_value + data.push_back({1, 0}); // not_used + data.push_back({2, 2}); // p + data.push_back({1, 1}); // l_n + data.push_back({1, rand() & 1}); // z_vel_sign + data.push_back({23, 1337}); // z_vel_value + data.push_back({1, rand() & 1}); // z_accel_sign + data.push_back({4, 9}); // z_accel_value + data.push_back({1, rand() & 1}); // z_sign + data.push_back({26, 100023}); // z_value + } else if (string_number == 4) { + data.push_back({1, rand() & 1}); // tau_n_sign + data.push_back({21, 197152}); // tau_n_value + data.push_back({1, rand() & 1}); // delta_tau_n_sign + data.push_back({4, 4}); // delta_tau_n_value + data.push_back({5, 0}); // e_n + data.push_back({14, 2}); // not_used_1 + data.push_back({1, 1}); // p4 + data.push_back({4, 9}); // f_t + data.push_back({3, 3}); // not_used_2 + data.push_back({11, 2047}); // n_t + data.push_back({5, 2}); // n + data.push_back({2, 1}); // m + } else if (string_number == 5) { + data.push_back({11, 2047}); // n_a + data.push_back({32, 4294767295}); // tau_c + data.push_back({1, 0}); // not_used_1 + data.push_back({5, 2}); // n_4 + data.push_back({22, 4114304}); // tau_gps + data.push_back({1, 0}); // l_n + } else { // non-immediate data is not parsed + data.push_back({64, rand()}); // data_1 + data.push_back({8, 6}); // data_2 + } + + data.push_back({8, rand() & 0xFF}); // hamming code + data.push_back({11, rand() & 0x7FF}); // pad + data.push_back({16, rand() & 0xFFFF}); // superframe + data.push_back({8, rand() & 0xFF}); // pad + data.push_back({8, rand() & 0xFF}); // frame + return data; +} + +TEST_CASE("parse_string_number_1"){ + string_data data = generate_string_data(1); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str1(inp_data); + glonass_t str1_data(&str1); + glonass_t::string_1_t* s1 = static_cast(str1_data.data()); + + REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); + REQUIRE(s1->p1() == data[ST1_P1_IDX].second); + REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); + + int mul = s1->x_vel_sign() ? (-1) : 1; + REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); + mul = s1->x_accel_sign() ? (-1) : 1; + REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); + mul = s1->x_sign() ? (-1) : 1; + REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_2"){ + string_data data = generate_string_data(2); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str2(inp_data); + glonass_t str2_data(&str2); + glonass_t::string_2_t* s2 = static_cast(str2_data.data()); + + REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); + REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); + REQUIRE(s2->p2() == data[ST2_P2_IDX].second); + REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); + int mul = s2->y_vel_sign() ? (-1) : 1; + REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); + mul = s2->y_accel_sign() ? (-1) : 1; + REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); + mul = s2->y_sign() ? (-1) : 1; + REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_3"){ + string_data data = generate_string_data(3); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str3(inp_data); + glonass_t str3_data(&str3); + glonass_t::string_3_t* s3 = static_cast(str3_data.data()); + + REQUIRE(s3->p3() == data[ST3_P3_IDX].second); + int mul = s3->gamma_n_sign() ? (-1) : 1; + REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); + REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); + REQUIRE(s3->p() == data[ST3_P_IDX].second); + REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); + mul = s3->z_vel_sign() ? (-1) : 1; + REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); + mul = s3->z_accel_sign() ? (-1) : 1; + REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); + mul = s3->z_sign() ? (-1) : 1; + REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_4"){ + string_data data = generate_string_data(4); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str4(inp_data); + glonass_t str4_data(&str4); + glonass_t::string_4_t* s4 = static_cast(str4_data.data()); + + int mul = s4->tau_n_sign() ? (-1) : 1; + REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); + mul = s4->delta_tau_n_sign() ? (-1) : 1; + REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); + REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); + REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); + REQUIRE(s4->p4() == data[ST4_P4_IDX].second); + REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); + REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); + REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); + REQUIRE(s4->n() == data[ST4_N_IDX].second); + REQUIRE(s4->m() == data[ST4_M_IDX].second); +} + +TEST_CASE("parse_string_number_5"){ + string_data data = generate_string_data(5); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str5(inp_data); + glonass_t str5_data(&str5); + glonass_t::string_5_t* s5 = static_cast(str5_data.data()); + + REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); + REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); + REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); + REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); + REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); + REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); +} + +TEST_CASE("parse_string_number_NI"){ + string_data data = generate_string_data((rand() % 10) + 6); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); + + kaitai::kstream strni(inp_data); + glonass_t strni_data(&strni); + glonass_t::string_non_immediate_t* sni = static_cast(strni_data.data()); + + REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); + REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); +} diff --git a/selfdrive/locationd/test/test_glonass_runner.cc b/selfdrive/locationd/test/test_glonass_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py index 7aa588d43e..cd4ce0de04 100755 --- a/selfdrive/locationd/test/test_ublox_processing.py +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -1,5 +1,5 @@ import unittest - +import time import numpy as np from laika import AstroDog @@ -8,7 +8,8 @@ from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ from laika.opt import calc_pos_fix from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader - +from selfdrive.test.helpers import with_processes +import cereal.messaging as messaging def get_gnss_measurements(log_reader): gnss_measurements = [] @@ -21,6 +22,12 @@ def get_gnss_measurements(log_reader): gnss_measurements.append(read_raw_ublox(report)) return gnss_measurements +def get_ublox_raw(log_reader): + ublox_raw = [] + for msg in log_reader: + if msg.which() == "ubloxRaw": + ublox_raw.append(msg) + return ublox_raw class TestUbloxProcessing(unittest.TestCase): NUM_TEST_PROCESS_MEAS = 10 @@ -30,6 +37,10 @@ class TestUbloxProcessing(unittest.TestCase): lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) cls.gnss_measurements = get_gnss_measurements(lr) + # test gps ephemeris continuity check (drive has ephemeris issues with cutover data) + lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14)) + cls.ublox_raw = get_ublox_raw(lr) + def test_read_ublox_raw(self): count_gps = 0 count_glonass = 0 @@ -76,6 +87,29 @@ class TestUbloxProcessing(unittest.TestCase): self.assertEqual(count_processed_measurements, 69) self.assertEqual(count_corrected_measurements, 69) + @with_processes(['ubloxd']) + def test_ublox_gps_cutover(self): + time.sleep(2) + ugs = messaging.sub_sock("ubloxGnss", timeout=0.1) + ur_pm = messaging.PubMaster(['ubloxRaw']) + + def replay_segment(): + rcv_msgs = [] + for msg in self.ublox_raw: + ur_pm.send(msg.which(), msg.as_builder()) + time.sleep(0.01) + rcv_msgs += messaging.drain_sock(ugs) + + time.sleep(0.1) + rcv_msgs += messaging.drain_sock(ugs) + return rcv_msgs + + # replay twice to enforce cutover data on rewind + rcv_msgs = replay_segment() + rcv_msgs += replay_segment() + + ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs) + self.assertEqual(ephems_cnt, 15) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 8bd9985170..3ff768ba06 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -65,6 +65,21 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } +inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) { + // convert time to year (conversion from A3.1.3) + int J = 0; + if (1 <= Nt && Nt <= 366) { + J = 1; + } else if (367 <= Nt && Nt <= 731) { + J = 2; + } else if (732 <= Nt && Nt <= 1096) { + J = 3; + } else if (1097 <= Nt && Nt <= 1461) { + J = 4; + } + uint16_t year = 1996 + 4*(N4 -1) + (J - 1); + return year; +} bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); @@ -103,9 +118,9 @@ std::pair> UbloxMsgParser::gen_msg() { switch (ubx_message.msg_type()) { case 0x0107: return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - case 0x0213: + case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - case 0x0215: + case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; case 0x0a09: return {"ubloxGnss", gen_mon_hw(static_cast(body))}; @@ -147,125 +162,248 @@ kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { return capnp::messageToFlatArray(msg_builder); } - -kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { +kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes + // We will first need to separate the data from the padding and parity auto body = *msg->body(); + assert(body.size() == 10); - if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) { - // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes - // We will first need to separate the data from the padding and parity - assert(body.size() == 10); + std::string subframe_data; + subframe_data.reserve(30); + for (uint32_t word : body) { + word = word >> 6; // TODO: Verify parity + subframe_data.push_back(word >> 16); + subframe_data.push_back(word >> 8); + subframe_data.push_back(word >> 0); + } - std::string subframe_data; - subframe_data.reserve(30); - for (uint32_t word : body) { - word = word >> 6; // TODO: Verify parity - subframe_data.push_back(word >> 16); - subframe_data.push_back(word >> 8); - subframe_data.push_back(word >> 0); + // Collect subframes in map and parse when we have all the parts + { + kaitai::kstream stream(subframe_data); + gps_t subframe(&stream); + + int subframe_id = subframe.how()->subframe_id(); + if (subframe_id > 3) { + // dont parse almanac subframes + return kj::Array(); } + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + } - // Collect subframes in map and parse when we have all the parts + // publish if subframes 1-3 have been collected + if (gps_subframes[msg->sv_id()].size() == 3) { + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); + eph.setSvId(msg->sv_id()); + + int iode_s2 = 0; + int iode_s3 = 0; + int iodc_lsb = 0; + + // Subframe 1 { - kaitai::kstream stream(subframe_data); + kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); gps_t subframe(&stream); - int subframe_id = subframe.how()->subframe_id(); - int sv_id = msg->sv_id(); - uint64_t tow_counter = subframe.how()->tow_count(); + gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - 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; + eph.setGpsWeek(subframe_1->week_no()); + eph.setTgd(subframe_1->t_gd() * pow(2, -31)); + eph.setToc(subframe_1->t_oc() * pow(2, 4)); + eph.setAf2(subframe_1->af_2() * pow(2, -55)); + eph.setAf1(subframe_1->af_1() * pow(2, -43)); + eph.setAf0(subframe_1->af_0() * pow(2, -31)); + eph.setSvHealth(subframe_1->sv_health()); + iodc_lsb = subframe_1->iodc_lsb(); } - if (gps_subframes[msg->sv_id()].size() == 5) { - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(msg->sv_id()); + // Subframe 2 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); + gps_t subframe(&stream); + gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); - // Subframe 1 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); - gps_t subframe(&stream); - gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - - eph.setGpsWeek(subframe_1->week_no()); - eph.setTgd(subframe_1->t_gd() * pow(2, -31)); - eph.setToc(subframe_1->t_oc() * pow(2, 4)); - eph.setAf2(subframe_1->af_2() * pow(2, -55)); - eph.setAf1(subframe_1->af_1() * pow(2, -43)); - eph.setAf0(subframe_1->af_0() * pow(2, -31)); - eph.setSvHealth(subframe_1->sv_health()); - } - - // Subframe 2 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); - gps_t subframe(&stream); - gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); - - eph.setCrs(subframe_2->c_rs() * pow(2, -5)); - eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); - eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); - eph.setCuc(subframe_2->c_uc() * pow(2, -29)); - eph.setEcc(subframe_2->e() * pow(2, -33)); - eph.setCus(subframe_2->c_us() * pow(2, -29)); - eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); - eph.setToe(subframe_2->t_oe() * pow(2, 4)); - } - - // Subframe 3 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); - gps_t subframe(&stream); - gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); - - eph.setCic(subframe_3->c_ic() * pow(2, -29)); - eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); - eph.setCis(subframe_3->c_is() * pow(2, -29)); - eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); - eph.setCrc(subframe_3->c_rc() * pow(2, -5)); - eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); - eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); - eph.setIode(subframe_3->iode()); - eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); - } - - // Subframe 4 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); - gps_t subframe(&stream); - gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); - - // This is page 18, why is the page id 56? - if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { - auto iono = static_cast(subframe_4->body()); - double a0 = iono->a0() * pow(2, -30); - double a1 = iono->a1() * pow(2, -27); - double a2 = iono->a2() * pow(2, -24); - double a3 = iono->a3() * pow(2, -24); - eph.setIonoAlpha({a0, a1, a2, a3}); - - double b0 = iono->b0() * pow(2, 11); - double b1 = iono->b1() * pow(2, 14); - double b2 = iono->b2() * pow(2, 16); - double b3 = iono->b3() * pow(2, 16); - eph.setIonoBeta({b0, b1, b2, b3}); - } - } - - return capnp::messageToFlatArray(msg_builder); + eph.setCrs(subframe_2->c_rs() * pow(2, -5)); + eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); + eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); + eph.setCuc(subframe_2->c_uc() * pow(2, -29)); + eph.setEcc(subframe_2->e() * pow(2, -33)); + eph.setCus(subframe_2->c_us() * pow(2, -29)); + eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); + eph.setToe(subframe_2->t_oe() * pow(2, 4)); + iode_s2 = subframe_2->iode(); } + + // Subframe 3 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); + gps_t subframe(&stream); + gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); + + eph.setCic(subframe_3->c_ic() * pow(2, -29)); + eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); + eph.setCis(subframe_3->c_is() * pow(2, -29)); + eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); + eph.setCrc(subframe_3->c_rc() * pow(2, -5)); + eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); + eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); + eph.setIode(subframe_3->iode()); + eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); + iode_s3 = subframe_3->iode(); + } + + gps_subframes[msg->sv_id()].clear(); + if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { + // data set cutover, reject ephemeris + return kj::Array(); + } + return capnp::messageToFlatArray(msg_builder); } return kj::Array(); } +kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) { + if (msg->sv_id() == 255) { + // data can be decoded before identifying the SV number, in this case 255 + // is returned, which means "unknown" (ublox p32) + return kj::Array(); + } + + auto body = *msg->body(); + assert(body.size() == 4); + { + std::string string_data; + string_data.reserve(16); + for (uint32_t word : body) { + for (int i = 3; i >= 0; i--) + string_data.push_back(word >> 8*i); + } + + kaitai::kstream stream(string_data); + glonass_t gl_string(&stream); + + int string_number = gl_string.string_number(); + if (string_number > 5 || gl_string.idle_chip()) { + // dont parse non immediate data, idle_chip == 0 + return kj::Array(); + } + + // immediate data is the same within one superframe + if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) { + glonass_strings[msg->sv_id()].clear(); + glonass_superframes[msg->sv_id()] = gl_string.superframe_number(); + } + glonass_strings[msg->sv_id()][string_number] = string_data; + } + + // publish if strings 1-5 have been collected + if (glonass_strings[msg->sv_id()].size() != 5) { + return kj::Array(); + } + + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris(); + eph.setSvId(msg->sv_id()); + uint16_t current_day = 0; + + // string number 1 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][1]); + glonass_t gl_stream(&stream); + glonass_t::string_1_t* data = static_cast(gl_stream.data()); + + eph.setP1(data->p1()); + eph.setTk(data->t_k()); + eph.setXVel(data->x_vel() * pow(2, -20)); + eph.setXAccel(data->x_accel() * pow(2, -30)); + eph.setX(data->x() * pow(2, -11)); + } + + // string number 2 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][2]); + glonass_t gl_stream(&stream); + glonass_t::string_2_t* data = static_cast(gl_stream.data()); + + eph.setSvHealth(data->b_n()>>2); // MSB indicates health + eph.setP2(data->p2()); + eph.setTb(data->t_b()); + eph.setYVel(data->y_vel() * pow(2, -20)); + eph.setYAccel(data->y_accel() * pow(2, -30)); + eph.setY(data->y() * pow(2, -11)); + } + + // string number 3 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][3]); + glonass_t gl_stream(&stream); + glonass_t::string_3_t* data = static_cast(gl_stream.data()); + + eph.setP3(data->p3()); + eph.setGammaN(data->gamma_n() * pow(2, -40)); + eph.setSvHealth(eph.getSvHealth() | data->l_n()); + eph.setZVel(data->z_vel() * pow(2, -20)); + eph.setZAccel(data->z_accel() * pow(2, -30)); + eph.setZ(data->z() * pow(2, -11)); + } + + // string number 4 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][4]); + glonass_t gl_stream(&stream); + glonass_t::string_4_t* data = static_cast(gl_stream.data()); + + current_day = data->n_t(); + eph.setTauN(data->tau_n() * pow(2, -30)); + eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30)); + eph.setAge(data->e_n()); + eph.setP4(data->p4()); + eph.setSvURA(glonass_URA_lookup.at(data->f_t())); + if (msg->sv_id() != data->n()) { + LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n()) + } + eph.setSvType(data->m()); + } + + // string number 5 + { + kaitai::kstream stream(glonass_strings[msg->sv_id()][5]); + glonass_t gl_stream(&stream); + glonass_t::string_5_t* data = static_cast(gl_stream.data()); + + // string5 parsing is only needed to get the year, this can be removed and + // the year can be fetched later in laika (note rollovers and leap year) + uint8_t n_4 = data->n_4(); + uint16_t year = get_glonass_year(n_4, current_day); + + uint16_t last_leap_year = 1996 + 4*(n_4-1); + uint16_t days_till_this_year = (year - last_leap_year)*365; + if (days_till_this_year != 0) { + days_till_this_year++; + } + + eph.setYear(year); + eph.setDayInYear(current_day - days_till_this_year); + eph.setHour((eph.getTk()>>7) & 0x1F); + eph.setMinute((eph.getTk()>>1) & 0x3F); + eph.setSecond((eph.getTk() & 0x1) * 30); + } + + glonass_strings[msg->sv_id()].clear(); + return capnp::messageToFlatArray(msg_builder); +} + + +kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + switch (msg->gnss_id()) { + case ubx_t::gnss_type_t::GNSS_TYPE_GPS: + return parse_gps_ephemeris(msg); + case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS: + return parse_glonass_ephemeris(msg); + default: + return kj::Array(); + } +} + kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { MessageBuilder msg_builder; auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 919e9963f1..6988f20b74 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -10,6 +10,7 @@ #include "cereal/messaging/messaging.h" #include "common/util.h" #include "selfdrive/locationd/generated/gps.h" +#include "selfdrive/locationd/generated/glonass.h" #include "selfdrive/locationd/generated/ubx.h" using namespace std::string_literals; @@ -101,12 +102,22 @@ class UbloxMsgParser { inline bool valid_cheksum(); inline bool valid(); inline bool valid_so_far(); + inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt); + + kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); + kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); std::unordered_map> gps_subframes; - 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]; -}; + // user range accuracy in meters + const std::unordered_map glonass_URA_lookup = + {{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7}, + { 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32}, + {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}}; + std::unordered_map> glonass_strings; + std::unordered_map glonass_superframes; +}; diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 92706c53ec..3b961bce6e 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -13,6 +13,8 @@ if arch == "Darwin": # fix OpenCL del libs[libs.index('OpenCL')] env['FRAMEWORKS'] = ['OpenCL'] + # exclude v4l + del src[src.index('encoder/v4l_encoder.cc')] logger_lib = env.Library('logger', src) libs.insert(0, logger_lib) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 9beb3c3bf1..e09cdfaa9e 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -24,15 +24,25 @@ void logger_rotate(LoggerdState *s) { } void rotate_if_needed(LoggerdState *s) { - if (s->ready_to_rotate == s->max_waiting) { - logger_rotate(s); + // all encoders ready, trigger rotation + bool all_ready = s->ready_to_rotate == s->max_waiting; + + // fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions + bool timed_out = false; + double tms = millis_since_boot(); + double seg_length_secs = (tms - s->last_rotate_tms) / 1000.; + if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) { + // TODO: might be nice to put these reasons in the sentinel + if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) { + timed_out = true; + LOGE("no camera packets seen. auto rotating"); + } else if (seg_length_secs > SEGMENT_LENGTH*1.2) { + timed_out = true; + LOGE("segment too long. auto rotating"); + } } - double tms = millis_since_boot(); - if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 && - (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE && - !LOGGERD_TEST) { - LOGW("no camera packet seen. auto rotating"); + if (all_ready || timed_out) { logger_rotate(s); } } diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index c8a7d41539..5b69e3dca7 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -15,7 +15,7 @@ from system.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 2395 +TOTAL_SCONS_NODES = 2460 MAX_BUILD_PROGRESS = 100 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 81e3bdb3df..7af911ab2a 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -226,7 +226,11 @@ class RouteEngine: remaining = 1.0 - along_geometry / max(step['distance'], 1) total_distance = step['distance'] * remaining total_time = step['duration'] * remaining - total_time_typical = step['duration_typical'] * remaining + + if step['duration_typical'] is None: + total_time_typical = total_time + else: + total_time_typical = step['duration_typical'] * remaining # Add up totals for future steps for i in range(self.step_idx + 1, len(self.route)): diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index bb53dd1bd4..9d0a62bd3b 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -123,7 +123,7 @@ class TTYPigeon(): init_baudrate(self) # clear configuration - self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x00\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\x5b") + self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7") # clear flash memory (almanac backup) self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0") diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 606dcad9c4..b531cb3430 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -204,7 +204,7 @@ def controlsd_rcv_callback(msg, CP, cfg, fsm): def radar_rcv_callback(msg, CP, cfg, fsm): if msg.which() != "can": return [], False - elif CP.radarOffCan: + elif CP.radarUnavailable: return ["radarState", "liveTracks"], True radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 51de0f0013..2c66c7075c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -844c84eae269e74b246b039d2b33fd44e98a5b68 \ No newline at end of file +7c2a98f4a94b6dded6a90d8913ab85a63069c186 \ No newline at end of file diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 1b4b880fbf..7a4b4240c3 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -175,11 +175,62 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } +ExperimentalButton::ExperimentalButton(QWidget *parent) : QPushButton(parent) { + setVisible(false); + setFixedSize(btn_size, btn_size); + setCheckable(true); + + params = Params(); + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); + + QObject::connect(this, &QPushButton::toggled, [=](bool checked) { + params.putBool("ExperimentalMode", checked); + }); +} + +void ExperimentalButton::updateState(const UIState &s) { + const SubMaster &sm = *(s.sm); + + // button is "visible" if engageable or enabled + const auto cs = sm["controlsState"].getControlsState(); + setVisible(cs.getEngageable() || cs.getEnabled()); + + // button is "checked" if experimental mode is enabled + setChecked(sm["controlsState"].getControlsState().getExperimentalMode()); + + // disable button when experimental mode is not available, or has not been confirmed for the first time + const auto cp = sm["carParams"].getCarParams(); + const bool experimental_mode_available = cp.getExperimentalLongitudinalAvailable() ? params.getBool("ExperimentalLongitudinalEnabled") : cp.getOpenpilotLongitudinalControl(); + setEnabled(params.getBool("ExperimentalModeConfirmed") && experimental_mode_available); +} + +void ExperimentalButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + QPoint center(btn_size / 2, btn_size / 2); + QPixmap img = isChecked() ? experimental_img : engage_img; + + p.setOpacity(1.0); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 166)); + p.drawEllipse(center, btn_size / 2, btn_size / 2); + p.setOpacity(isDown() ? 0.8 : 1.0); + p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, img); +} + + AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { pm = std::make_unique>({"uiDebug"}); - engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); - experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size - 5, img_size - 5}); + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(bdr_s); + main_layout->setSpacing(0); + + experimental_btn = new ExperimentalButton(this); + main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); } @@ -227,9 +278,11 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); setProperty("status", s.status); - // update engageability and DM icons at 2Hz + // update engageability/experimental mode button + experimental_btn->updateState(s); + + // update DM icons at 2Hz if (sm.frame % (UI_FREQ / 2) == 0) { - setProperty("engageable", cs.getEngageable() || cs.getEnabled()); setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } @@ -382,16 +435,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { configFont(p, "Inter", 66, "Regular"); drawText(p, rect().center().x(), 290, speedUnit, 200); - // engage-ability icon - if (engageable) { - SubMaster &sm = *(uiState()->sm); - drawIcon(p, rect().right() - radius / 2 - bdr_s * 2, radius / 2 + int(bdr_s * 1.5), - sm["controlsState"].getControlsState().getExperimentalMode() ? experimental_img : engage_img, blackColor(166), 1.0); - } - // dm icon if (!hideDM) { - int dm_icon_x = rightHandDM ? rect().right() - radius / 2 - (bdr_s * 2) : radius / 2 + (bdr_s * 2); + int dm_icon_x = rightHandDM ? rect().right() - btn_size / 2 - (bdr_s * 2) : btn_size / 2 + (bdr_s * 2); drawIcon(p, dm_icon_x, rect().bottom() - footer_h / 2, dm_img, blackColor(70), dmActive ? 1.0 : 0.2); } @@ -414,7 +460,7 @@ void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QB p.setOpacity(1.0); // bg dictates opacity of ellipse p.setPen(Qt::NoPen); p.setBrush(bg); - p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); + p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); p.setOpacity(opacity); p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3dbb05b674..73c2e03795 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -1,11 +1,16 @@ #pragma once +#include #include #include #include "common/util.h" -#include "selfdrive/ui/qt/widgets/cameraview.h" #include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + + +const int btn_size = 192; +const int img_size = (btn_size / 4) * 3; // ***** onroad widgets ***** @@ -24,6 +29,21 @@ private: Alert alert = {}; }; +class ExperimentalButton : public QPushButton { + Q_OBJECT + +public: + explicit ExperimentalButton(QWidget *parent = 0); + void updateState(const UIState &s); + +private: + void paintEvent(QPaintEvent *event) override; + + Params params; + QPixmap engage_img; + QPixmap experimental_img; +}; + // container window for the NVG UI class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -36,7 +56,6 @@ class AnnotatedCameraWidget : public CameraWidget { Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit); Q_PROPERTY(bool is_metric MEMBER is_metric); - Q_PROPERTY(bool engageable MEMBER engageable); Q_PROPERTY(bool dmActive MEMBER dmActive); Q_PROPERTY(bool hideDM MEMBER hideDM); Q_PROPERTY(bool rightHandDM MEMBER rightHandDM); @@ -50,18 +69,14 @@ private: void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - QPixmap engage_img; - QPixmap experimental_img; + ExperimentalButton *experimental_btn; QPixmap dm_img; - const int radius = 192; - const int img_size = (radius / 2) * 1.5; float speed; QString speedUnit; float setSpeed; float speedLimit; bool is_cruise_set = false; bool is_metric = false; - bool engageable = false; bool dmActive = false; bool hideDM = false; bool rightHandDM = false; diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 0de0ba5f9a..ce394ecb97 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -238,6 +238,14 @@ Disengage to Power Off ยกเลิกระบบช่วยขับเพื่อปิดเครื่อง + + Reset + รีเซ็ต + + + Review + ทบทวน + DriveStats @@ -273,6 +281,17 @@ กำลังเปิดกล้อง + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + คุณกำลังใช้โหมดทดลอง + + + CHILL MODE ON + คุณกำลังใช้โหมดชิล + + InputDialog @@ -463,6 +482,17 @@ location set จดจำ connect.comma.ai โดยการเพิ่มไปยังหน้าจอโฮม เพื่อใช้งานเหมือนเป็นแอปพลิเคชัน + + ParamControl + + Enable + เปิดใช้งาน + + + Cancel + ยกเลิก + + PrimeAdWidget @@ -585,13 +615,6 @@ location set ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ - - RichTextDialog - - Ok - ตกลง - - SettingsWindow @@ -850,6 +873,10 @@ location set Select a branch เลือก Branch + + Uninstall + ถอนการติดตั้ง + SshControl @@ -974,29 +1001,57 @@ location set Show map on left side when in split screen view. แสดงแผนที่ด้านซ้ายของหน้าจอเมื่ออยู่ในโหมดแบ่งหน้าจอ - - 🌮 End-to-end longitudinal (extremely alpha) 🌮 - 🌮 ควบคุมเร่ง/เบรคแบบ End-to-end (อยู่ขั้นพัฒนา) 🌮 - Experimental openpilot Longitudinal Control ทดลองใช้ระบบควบคุมการเร่ง/เบรคโดย openpilot - <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> - <b>คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นทดลอง และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด</b> + Experimental Mode + โหมดทดลอง - Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. - ให้ openpilot ควบคุมการเร่ง/เบรคแบบ end-to-end โดย openpilot จะขับอย่างที่มนุษย์คิด ระบบยังอยู่ในขั้นทดลอง + WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). + คำเตือน: การควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ยังอยู่ในขั้นพัฒนา และระบบเบรคฉุกเฉินอัตโนมัติ (AEB) จะถูกปิด - openpilot longitudinal control is not currently available for this car. - ขณะนี้ยังไม่มีระบบควบคุมการเร่ง/เบรคโดย openpilot สำหรับรถคันนี้ + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. + โดยปกติสำหรับรถคันนี้ openpilot จะควบคุมการเร่ง/เบรคด้วยระบบ ACC จากโรงงาน แทนการควยคุมโดย openpilot เปิดสวิตซ์นี้เพื่อให้ openpilot ควบคุมการเร่ง/เบรค แนะนำให้เปิดโหมดทดลองเมื่อต้องการให้ openpilot ควบคุมการเร่ง/เบรค ซึ่งอยู่ในขั้นพัฒนา - Enable experimental longitudinal control to enable this. - เปิดใช้งานระบบควบคุมการเร่ง/เบรคขั้นทดลอง เพื่อเปิดใช้งานสิ่งนี้ + openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: + โดยปกติ openpilot จะขับใน<b>โหมดชิล</b> เปิดโหมดทดลองเพื่อใช้<b>ความสามารถในขั้นพัฒนา</b> ซึ่งยังไม่พร้อมสำหรับโหมดชิล ความสามารถในขั้นพัฒนามีดังนี้: + + + 🌮 End-to-End Longitudinal Control 🌮 + 🌮 ควบคุมเร่ง/เบรคแบบ End-to-End 🌮 + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. + ให้ openpilot ควบคุมการเร่ง/เบรค โดย openpilot จะขับอย่างที่มนุษย์คิด รวมถึงการหยุดที่ไฟแดง และป้ายหยุดรถ เนื่องจาก openpilot จะกำหนดความเร็วในการขับด้วยตัวเอง การตั้งความเร็วจะเป็นเพียงการกำหนดความเร็วสูงสูดเท่านั้น ความสามารถนี้ยังอยู่ในขั้นพัฒนา อาจเกิดข้อผิดพลาดขึ้นได้ + + + New Driving Visualization + การแสดงภาพการขับขี่แบบใหม่ + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย + + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + ขณะนี้โหมดทดลองไม่สามารถใช้งานได้ในรถคันนี้ เนื่องจากเปิดใช้ระบบควบคุมการเร่ง/เบรคของรถที่ติดตั้งจากโรงงานอยู่ + + + openpilot longitudinal control may come in a future update. + ระบบควบคุมการเร่ง/เบรคโดย openpilot อาจมาในการอัปเดตในอนาคต + + + An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + เวอร์ชันทดลองของระบบควบคุมการเร่ง/เบรคโดย openpilot สามารถทดสอบได้พร้อมกับโหมดการทดลอง บน branch ที่กำลังพัฒนา + + + Enable experimental longitudinal control to allow Experimental mode. + เปิดระบบควบคุมการเร่ง/เบรคขั้นพัฒนาโดย openpilot เพื่อเปิดใช้งานโหมดทดลอง @@ -1052,5 +1107,9 @@ location set Forget Wi-Fi Network "%1"? เลิกใช้เครือข่าย Wi-Fi "%1"? + + Forget + เลิกใช้ + diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 9da2a05a11..2cb7d1c13b 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -423,6 +423,9 @@ def main() -> None: wait_helper = WaitTimeHelper() wait_helper.only_check_for_update = True + # invalidate old finalized update + set_consistent_flag(False) + # Run the update loop while True: wait_helper.ready_event.clear() diff --git a/system/micd.py b/system/micd.py index a56140e3b9..97ba0c262e 100755 --- a/system/micd.py +++ b/system/micd.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import sounddevice as sd import numpy as np from cereal import messaging @@ -84,11 +83,11 @@ class Mic: self.measurements = self.measurements[FFT_SAMPLES:] - def micd_thread(self, device=None): - if device is None: - device = "sysdefault" + def micd_thread(self): + # sounddevice must be imported after forking processes + import sounddevice as sd # pylint: disable=import-outside-toplevel - with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") while True: self.update() diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index fbf3c03d36..a9922ba9be 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -21,9 +21,12 @@ 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', '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) + 'commands.cc', 'messageswidget.cc', 'route.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) cabana_env.Program('_cabana', ['cabana.cc', cabana_lib, asset_obj], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) +if arch == "Darwin": + cabana_env.Execute('install_name_tool -change opendbc/can/libdbc.dylib @loader_path/../../opendbc/can/libdbc.dylib ./_cabana') + if GetOption('test'): cabana_env.Program('tests/_test_cabana', ['tests/test_runner.cc', 'tests/test_cabana.cc', cabana_lib], LIBS=[cabana_libs]) diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index 010fe0fc88..4d7b3148c6 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -4,8 +4,10 @@ #include #include #include +#include #include +#include "tools/cabana/commands.h" #include "tools/cabana/streams/abstractstream.h" // BinaryView @@ -30,12 +32,15 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) { setFrameShape(QFrame::NoFrame); setShowGrid(false); setMouseTracking(true); - setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &BinaryView::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &BinaryView::refresh); } QSize BinaryView::minimumSizeHint() const { - return {horizontalHeader()->minimumSectionSize() * 9 + VERTICAL_HEADER_WIDTH + 2, 0}; + return {(horizontalHeader()->minimumSectionSize() + 1) * 9 + VERTICAL_HEADER_WIDTH, + CELL_HEIGHT * std::min(model->rowCount(), 10)}; } void BinaryView::highlight(const Signal *sig) { @@ -124,13 +129,20 @@ void BinaryView::leaveEvent(QEvent *event) { } void BinaryView::setMessage(const QString &message_id) { - model->setMessage(message_id); + model->msg_id = message_id; + verticalScrollBar()->setValue(0); + refresh(); +} + +void BinaryView::refresh() { + if (model->msg_id.isEmpty()) return; + clearSelection(); anchor_index = QModelIndex(); resize_sig = nullptr; hovered_sig = nullptr; highlightPosition(QCursor::pos()); - updateState(); + model->refresh(); } QSet BinaryView::getOverlappingSignals() const { @@ -155,9 +167,8 @@ std::tuple BinaryView::getSelection(QModelIndex index) { // BinaryViewModel -void BinaryViewModel::setMessage(const QString &message_id) { +void BinaryViewModel::refresh() { beginResetModel(); - msg_id = message_id; items.clear(); if ((dbc_msg = dbc()->msg(msg_id))) { row_count = dbc_msg->size; @@ -184,6 +195,7 @@ void BinaryViewModel::setMessage(const QString &message_id) { items.resize(row_count * column_count); } endResetModel(); + updateState(); } void BinaryViewModel::updateState() { @@ -200,7 +212,7 @@ void BinaryViewModel::updateState() { } char hex[3] = {'\0'}; for (int i = 0; i < binary.size(); ++i) { - for (int j = 0; j < column_count - 1; ++j) { + for (int j = 0; j < 8; ++j) { items[i * column_count + j].val = ((binary[i] >> (7 - j)) & 1) != 0 ? '1' : '0'; } hex[0] = toHex(binary[i] >> 4); @@ -214,7 +226,7 @@ void BinaryViewModel::updateState() { } } - for (int i = 0; i < row_count * column_count; ++i) { + for (int i = 0; i < items.size(); ++i) { if (i >= prev_items.size() || prev_items[i].val != items[i].val || prev_items[i].bg_color != items[i].bg_color) { auto idx = index(i / column_count, i % column_count); emit dataChanged(idx, idx); diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index 55de71572e..f697ea28cb 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -21,7 +21,7 @@ public: class BinaryViewModel : public QAbstractTableModel { public: BinaryViewModel(QObject *parent) : QAbstractTableModel(parent) {} - void setMessage(const QString &message_id); + void refresh(); void updateState(); QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const { return {}; } @@ -39,12 +39,11 @@ public: QColor bg_color = QApplication::style()->standardPalette().color(QPalette::Base); bool is_msb = false; bool is_lsb = false; - QString val = "0"; + QString val = "-"; QList sigs; }; std::vector items; -private: QString msg_id; const DBCMsg *dbc_msg = nullptr; int row_count = 0; @@ -69,6 +68,7 @@ signals: void resizeSignal(const Signal *sig, int from, int size); private: + void refresh(); std::tuple getSelection(QModelIndex index); void setSelection(const QRect &rect, QItemSelectionModel::SelectionFlags flags) override; void mousePressEvent(QMouseEvent *event) override; diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index d9b6b5948b..e028e383c2 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -4,6 +4,7 @@ #include "common/prefix.h" #include "selfdrive/ui/qt/util.h" #include "tools/cabana/mainwin.h" +#include "tools/cabana/route.h" #include "tools/cabana/streams/livestream.h" #include "tools/cabana/streams/replaystream.h" @@ -24,11 +25,8 @@ int main(int argc, char *argv[]) { cmd_parser.addOption({"zmq", "the ip address on which to receive zmq messages", "zmq"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); cmd_parser.addOption({"no-vipc", "do not output video"}); + cmd_parser.addOption({"dbc", "dbc file to open", "dbc"}); cmd_parser.process(app); - const QStringList args = cmd_parser.positionalArguments(); - if (args.empty() && !cmd_parser.isSet("demo") && !cmd_parser.isSet("stream")) { - cmd_parser.showHelp(); - } std::unique_ptr op_prefix; std::unique_ptr stream; @@ -40,7 +38,6 @@ int main(int argc, char *argv[]) { #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; @@ -49,14 +46,33 @@ int main(int argc, char *argv[]) { } else if (cmd_parser.isSet("no-vipc")) { replay_flags |= REPLAY_FLAG_NO_VIPC; } - auto replay_stream = new ReplayStream(&app); + + const QStringList args = cmd_parser.positionalArguments(); + QString route; + if (args.size() > 0) { + route = args.first(); + } else if (cmd_parser.isSet("demo")) { + route = DEMO_ROUTE; + } + + auto replay_stream = new ReplayStream(replay_flags, &app); stream.reset(replay_stream); - if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { + if (route.isEmpty()) { + if (OpenRouteDialog dlg(nullptr); !dlg.exec()) { + return 0; + } + } else if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"))) { return 0; } } MainWindow w; + + // Load DBC + if (cmd_parser.isSet("dbc")) { + w.loadFile(cmd_parser.value("dbc")); + } + w.show(); return app.exec(); } diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 3ecdbb08bd..54cacf56cd 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -7,14 +7,13 @@ #include #include #include +#include #include #include #include #include #include -#include "selfdrive/ui/qt/util.h" - // ChartsWidget ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { @@ -24,14 +23,14 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { QToolBar *toolbar = new QToolBar(tr("Charts"), this); toolbar->setIconSize({16, 16}); - QAction *new_plot_btn = toolbar->addAction(bootstrapPixmap("file-plus"), ""); + QAction *new_plot_btn = toolbar->addAction(utils::icon("file-plus"), ""); new_plot_btn->setToolTip(tr("New Plot")); toolbar->addWidget(title_label = new QLabel()); title_label->setContentsMargins(0, 0, 12, 0); columns_cb = new QComboBox(this); columns_cb->addItems({"1", "2", "3", "4"}); - toolbar->addWidget(new QLabel(tr("Columns:"))); - toolbar->addWidget(columns_cb); + columns_lb_action = toolbar->addWidget(new QLabel(tr("Columns:"))); + columns_cb_action = toolbar->addWidget(columns_cb); QLabel *stretch_label = new QLabel(this); stretch_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); @@ -46,19 +45,21 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { 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 = toolbar->addAction(utils::icon("zoom-out"), ""); reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); - remove_all_btn = toolbar->addAction(bootstrapPixmap("x"), ""); + remove_all_btn = toolbar->addAction(utils::icon("x"), ""); remove_all_btn->setToolTip(tr("Remove all charts")); dock_btn = toolbar->addAction(""); main_layout->addWidget(toolbar); // charts + charts_layout = new QGridLayout(); + charts_layout->setSpacing(10); + QWidget *charts_container = new QWidget(this); 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->addLayout(charts_layout); charts_main_layout->addStretch(0); QScrollArea *charts_scroll = new QScrollArea(this); @@ -136,7 +137,7 @@ void ChartsWidget::updateState() { if (!is_zoomed) { 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(); + const double min_event_sec = can->events()->empty() ? 0 : (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); @@ -156,7 +157,7 @@ void ChartsWidget::updateState() { 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(); + const double min_event_sec = can->events()->empty() ? 0 : (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))); @@ -167,9 +168,8 @@ void ChartsWidget::setMaxChartRange(int value) { void ChartsWidget::updateToolBar() { 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->setIcon(utils::icon(docking ? "arrow-up-right" : "arrow-down-left")); dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts")); remove_all_btn->setEnabled(!charts.isEmpty()); reset_zoom_btn->setEnabled(is_zoomed); @@ -179,6 +179,7 @@ void ChartsWidget::settingChanged() { range_slider->setRange(1, settings.max_cached_minutes * 60); for (auto c : charts) { c->setFixedHeight(settings.chart_height); + c->setSeriesType(settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter); } } @@ -228,10 +229,16 @@ void ChartsWidget::setColumnCount(int n) { } void ChartsWidget::updateLayout() { - int n = column_count; + int n = columns_cb->count(); for (; n > 1; --n) { - if ((n * (CHART_MIN_WIDTH + charts_layout->spacing())) < rect().width()) break; + if ((n * CHART_MIN_WIDTH + (n - 1) * charts_layout->spacing()) < charts_layout->geometry().width()) break; } + + bool show_column_cb = n > 1; + columns_lb_action->setVisible(show_column_cb); + columns_cb_action->setVisible(show_column_cb); + + n = std::min(column_count, n); for (int i = 0; i < charts.size(); ++i) { charts_layout->addWidget(charts[charts.size() - i - 1], i / n, i % n); } @@ -292,18 +299,21 @@ bool ChartsWidget::eventFilter(QObject *obj, QEvent *event) { // ChartView ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { + series_type = settings.chart_series_type == 0 ? QAbstractSeries::SeriesTypeLine : QAbstractSeries::SeriesTypeScatter; + QChart *chart = new QChart(); chart->setBackgroundRoundness(0); axis_x = new QValueAxis(this); axis_y = new QValueAxis(this); chart->addAxis(axis_x, Qt::AlignBottom); chart->addAxis(axis_y, Qt::AlignLeft); + chart->legend()->layout()->setContentsMargins(0, 0, 40, 0); chart->legend()->setShowToolTips(true); chart->layout()->setContentsMargins(0, 0, 0, 0); chart->setMargins({20, 11, 11, 11}); QToolButton *remove_btn = new QToolButton(); - remove_btn->setIcon(bootstrapPixmap("x")); + remove_btn->setIcon(utils::icon("x")); remove_btn->setAutoRaise(true); remove_btn->setToolTip(tr("Remove Chart")); close_btn_proxy = new QGraphicsProxyWidget(chart); @@ -311,9 +321,20 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { close_btn_proxy->setZValue(chart->zValue() + 11); QToolButton *manage_btn = new QToolButton(); - manage_btn->setIcon(bootstrapPixmap("gear")); + manage_btn->setToolButtonStyle(Qt::ToolButtonIconOnly); + manage_btn->setIcon(utils::icon("list")); manage_btn->setAutoRaise(true); - manage_btn->setToolTip(tr("Manage series")); + QMenu *menu = new QMenu(this); + line_series_action = menu->addAction(tr("Line"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeLine); }); + line_series_action->setCheckable(true); + line_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeLine); + scatter_series_action = menu->addAction(tr("Scatter"), [this]() { setSeriesType(QAbstractSeries::SeriesTypeScatter); }); + scatter_series_action->setCheckable(true); + scatter_series_action->setChecked(series_type == QAbstractSeries::SeriesTypeScatter); + menu->addSeparator(); + menu->addAction(tr("Manage series"), this, &ChartView::manageSeries); + manage_btn->setMenu(menu); + manage_btn->setPopupMode(QToolButton::InstantPopup); manage_btn_proxy = new QGraphicsProxyWidget(chart); manage_btn_proxy->setWidget(manage_btn); manage_btn_proxy->setZValue(chart->zValue() + 11); @@ -328,7 +349,6 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) { QObject::connect(dbc(), &DBCManager::msgRemoved, this, &ChartView::msgRemoved); QObject::connect(dbc(), &DBCManager::msgUpdated, this, &ChartView::msgUpdated); QObject::connect(remove_btn, &QToolButton::clicked, this, &ChartView::remove); - QObject::connect(manage_btn, &QToolButton::clicked, this, &ChartView::manageSeries); } qreal ChartView::getYAsixLabelWidth() const { @@ -348,13 +368,7 @@ 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 + QXYSeries *series = createSeries(series_type); chart()->addSeries(series); series->attachAxis(axis_x); series->attachAxis(axis_y); @@ -362,13 +376,14 @@ 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); + updateSeriesPoints(); emit seriesAdded(msg_id, sig); } void ChartView::removeSeries(const QString &msg_id, const Signal *sig) { auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); if (it != sigs.end()) { - it = removeSeries(it); + it = removeItem(it); } } @@ -376,7 +391,7 @@ bool ChartView::hasSeries(const QString &msg_id, const Signal *sig) const { return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; }); } -QList::iterator ChartView::removeSeries(const QList::iterator &it) { +QList::iterator ChartView::removeItem(const QList::iterator &it) { chart()->removeSeries(it->series); it->series->deleteLater(); QString msg_id = it->msg_id; @@ -401,7 +416,7 @@ void ChartView::signalUpdated(const Signal *sig) { void ChartView::signalRemoved(const Signal *sig) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->sig == sig) ? removeSeries(it) : ++it; + it = (it->sig == sig) ? removeItem(it) : ++it; } } @@ -412,7 +427,7 @@ void ChartView::msgUpdated(uint32_t address) { void ChartView::msgRemoved(uint32_t address) { for (auto it = sigs.begin(); it != sigs.end(); /**/) { - it = (it->address == address) ? removeSeries(it) : ++it; + it = (it->address == address) ? removeItem(it) : ++it; } } @@ -444,7 +459,7 @@ void ChartView::manageSeries() { bool exists = std::any_of(series_list.cbegin(), series_list.cend(), [&](auto &s) { return s[0] == it->msg_id && s[2] == it->sig->name.c_str(); }); - it = exists ? ++it : removeSeries(it); + it = exists ? ++it : removeItem(it); } } } @@ -468,10 +483,44 @@ void ChartView::updatePlot(double cur, double min, double max) { if (min != axis_x->min() || max != axis_x->max()) { axis_x->setRange(min, max); updateAxisY(); + updateSeriesPoints(); } + scene()->invalidate({}, QGraphicsScene::ForegroundLayer); } +void ChartView::updateSeriesPoints() { + // Show points when zoomed in enough + for (auto &s : sigs) { + auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + auto end = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->max(), [](auto &p, double x) { return p.x() < x; }); + + int num_points = std::max(end - begin, 1); + int pixels_per_point = width() / num_points; + + if (series_type == QAbstractSeries::SeriesTypeScatter) { + ((QScatterSeries *)s.series)->setMarkerSize(std::clamp(pixels_per_point / 3, 1, 8)); + } else { + s.series->setPointsVisible(pixels_per_point > 20); + + // TODO: On MacOS QChartWidget doesn't work with the OpenGL settings that CameraWidget needs. +#ifndef __APPLE + // OpenGL mode lacks certain features (such as showing points), only use when drawing many points + bool use_opengl = pixels_per_point < 1; + s.series->setUseOpenGL(use_opengl); + + // Qt doesn't properly apply device pixel ratio in OpenGL mode + QApplication *application = static_cast(QApplication::instance()); + float scale = use_opengl ? application->devicePixelRatio() : 1.0; + + QPen pen = s.series->pen(); + pen.setWidth(2.0 * scale); + s.series->setPen(pen); +#endif + } + } +} + void ChartView::updateSeries(const Signal *sig, const std::vector *events, bool clear) { events = events ? events : can->events(); for (auto &s : sigs) { @@ -521,8 +570,8 @@ void ChartView::updateAxisY() { if (min_y == std::numeric_limits::max()) min_y = 0; if (max_y == std::numeric_limits::lowest()) max_y = 0; - if (max_y == min_y) { - axis_y->setRange(min_y - 1, max_y + 1); + if (std::abs(max_y - min_y) < 1e-3) { + applyNiceNumbers(min_y - 1, max_y + 1); } else { double range = max_y - min_y; applyNiceNumbers(min_y - range * 0.05, max_y + range * 0.05); @@ -539,6 +588,7 @@ void ChartView::applyNiceNumbers(qreal min, qreal max) { tick_count = int(max - min) + 1; axis_y->setRange(min * step, max * step); axis_y->setTickCount(tick_count); + axis_y->setLabelFormat("%.1f"); } // nice numbers can be expressed as form of 1*10^n, 2* 10^n or 5*10^n @@ -560,14 +610,14 @@ qreal ChartView::niceNumber(qreal x, bool ceiling) { } void ChartView::leaveEvent(QEvent *event) { - track_pt = {0, 0}; + track_pts.clear(); scene()->update(); QChartView::leaveEvent(event); } void ChartView::mousePressEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton && !chart()->plotArea().contains(event->pos()) && - !manage_btn_proxy->widget()->underMouse() && !close_btn_proxy->widget()->underMouse()) { + !manage_btn_proxy->geometry().contains(event->pos()) && !close_btn_proxy->geometry().contains(event->pos())) { QMimeData *mimeData = new QMimeData; mimeData->setData(mime_type, QByteArray::number((qulonglong)this)); QDrag *drag = new QDrag(this); @@ -588,14 +638,21 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton && rubber && rubber->isVisible()) { rubber->hide(); QRectF rect = rubber->geometry().normalized(); - double min = std::floor(chart()->mapToValue(rect.topLeft()).x() * 10.0) / 10.0; - double max = std::floor(chart()->mapToValue(rect.bottomRight()).x() * 10.0) / 10.0; + double min = chart()->mapToValue(rect.topLeft()).x(); + double max = chart()->mapToValue(rect.bottomRight()).x(); + + // Prevent zooming/seeking past the end of the route + min = std::clamp(min, can->routeStartTime(), can->routeStartTime() + can->totalSeconds()); + max = std::clamp(max, can->routeStartTime(), can->routeStartTime() + can->totalSeconds()); + + double min_rounded = std::floor(min * 10.0) / 10.0; + double max_rounded = std::floor(max * 10.0) / 10.0; if (rubber->width() <= 0) { // no rubber dragged, seek to mouse position can->seekTo(min); - } else if ((max - min) >= 0.5) { + } else if ((max_rounded - min_rounded) >= 0.5) { // zoom in if selected range is greater than 0.5s - emit zoomIn(min, max); + emit zoomIn(min_rounded, max_rounded); } event->accept(); } else if (!can->liveStreaming() && event->button() == Qt::RightButton) { @@ -610,32 +667,40 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { auto rubber = findChild(); bool is_zooming = rubber && rubber->isVisible(); const auto plot_area = chart()->plotArea(); - track_pt = {0, 0}; + track_pts.clear(); if (!is_zooming && plot_area.contains(ev->pos())) { + track_pts.resize(sigs.size()); QStringList text_list; const double sec = chart()->mapToValue(ev->pos()).x(); - for (auto &s : sigs) { + for (int i = 0; i < sigs.size(); ++i) { QString value = "--"; // use reverse iterator to find last item <= sec. - auto it = std::lower_bound(s.vals.rbegin(), s.vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); - if (it != s.vals.rend() && it->x() >= axis_x->min()) { + auto it = std::lower_bound(sigs[i].vals.rbegin(), sigs[i].vals.rend(), sec, [](auto &p, double x) { return p.x() > x; }); + if (it != sigs[i].vals.rend() && it->x() >= axis_x->min()) { value = QString::number(it->y()); - auto value_pos = chart()->mapToPosition(*it); - if (value_pos.x() > track_pt.x()) track_pt = value_pos; + track_pts[i] = chart()->mapToPosition(*it); } - text_list.push_back(QString(" %1 : %2 ").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value)); + text_list.push_back(QString("■ %2: %3").arg(sigs[i].series->color().name(), sigs[i].sig->name.c_str(), value)); } - if (track_pt.x() == 0) track_pt = ev->pos(); - QString text = QString("
 Time: %1  
%2
") - .arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3) - .arg(text_list.join("
")); - QPoint pt((int)track_pt.x() + 20, plot_area.top() - 20); - QToolTip::showText(mapToGlobal(pt), text, this, plot_area.toRect()); + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + auto pt = (max == track_pts.end()) ? ev->pos() : *max; + text_list.push_front(QString::number(chart()->mapToValue(pt).x(), 'f', 3)); + QPointF tooltip_pt(pt.x() + 12, plot_area.top() - 20); + QToolTip::showText(mapToGlobal(tooltip_pt.toPoint()), pt.isNull() ? "" : text_list.join("
"), this, plot_area.toRect()); scene()->update(); } else { QToolTip::hideText(); } + QChartView::mouseMoveEvent(ev); + if (is_zooming) { + QRect rubber_rect = rubber->geometry(); + rubber_rect.setLeft(std::max(rubber_rect.left(), (int)plot_area.left())); + rubber_rect.setRight(std::min(rubber_rect.right(), (int)plot_area.right())); + if (rubber_rect != rubber->geometry()) { + rubber->setGeometry(rubber_rect); + } + } } void ChartView::dragMoveEvent(QDragMoveEvent *event) { @@ -674,11 +739,56 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) { qreal y2 = chart()->plotArea().bottom() + 2; painter->setPen(QPen(chart()->titleBrush().color(), 2)); painter->drawLine(QPointF{x, y1}, QPointF{x, y2}); - if (!track_pt.isNull()) { + + auto max = std::max_element(track_pts.begin(), track_pts.end(), [](auto &a, auto &b) { return a.x() < b.x(); }); + if (max != track_pts.end() && !max->isNull()) { painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine)); - painter->drawLine(QPointF{track_pt.x(), y1}, QPointF{track_pt.x(), y2}); - painter->setBrush(Qt::darkGray); - painter->drawEllipse(track_pt, 5, 5); + painter->drawLine(QPointF{max->x(), y1}, QPointF{max->x(), y2}); + painter->setPen(Qt::NoPen); + for (int i = 0; i < track_pts.size(); ++i) { + if (!track_pts[i].isNull() && i < sigs.size()) { + painter->setBrush(sigs[i].series->color().darker(125)); + painter->drawEllipse(track_pts[i], 5.5, 5.5); + } + } + } +} + +QXYSeries *ChartView::createSeries(QAbstractSeries::SeriesType type) { + QXYSeries *series = nullptr; + if (type == QAbstractSeries::SeriesTypeLine) { + series = new QLineSeries(this); + } else { + series = new QScatterSeries(this); + } + // TODO: Due to a bug in CameraWidget the camera frames + // are drawn instead of the graphs on MacOS. Re-enable OpenGL when fixed +#ifndef __APPLE__ + series->setUseOpenGL(true); +#endif + return series; +} + +void ChartView::setSeriesType(QAbstractSeries::SeriesType type) { + if (type != series_type) { + series_type = type; + line_series_action->setChecked(type == QAbstractSeries::SeriesTypeLine); + scatter_series_action->setChecked(type == QAbstractSeries::SeriesTypeScatter); + + for (auto &s : sigs) { + chart()->removeSeries(s.series); + s.series->deleteLater(); + } + for (auto &s : sigs) { + auto series = createSeries(series_type); + chart()->addSeries(series); + series->attachAxis(axis_x); + series->attachAxis(axis_y); + series->replace(s.vals); + s.series = series; + } + updateSeriesPoints(); + updateTitle(); } } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index f134136323..8c50993971 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include "tools/cabana/dbcmanager.h" @@ -30,13 +31,14 @@ public: void updatePlot(double cur, double min, double max); void setPlotAreaLeftPosition(int pos); qreal getYAsixLabelWidth() const; + void setSeriesType(QAbstractSeries::SeriesType type); struct SigItem { QString msg_id; uint8_t source = 0; uint32_t address = 0; const Signal *sig = nullptr; - QLineSeries *series = nullptr; + QXYSeries *series = nullptr; QVector vals; uint64_t last_value_mono_time = 0; }; @@ -57,7 +59,7 @@ private slots: void manageSeries(); private: - QList::iterator removeSeries(const QList::iterator &it); + QList::iterator removeItem(const QList::iterator &it); void mousePressEvent(QMouseEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override; void mouseMoveEvent(QMouseEvent *ev) override; @@ -70,15 +72,20 @@ private: void drawForeground(QPainter *painter, const QRectF &rect) override; void applyNiceNumbers(qreal min, qreal max); qreal niceNumber(qreal x, bool ceiling); + QXYSeries *createSeries(QAbstractSeries::SeriesType type); + void updateSeriesPoints(); QValueAxis *axis_x; QValueAxis *axis_y; - QPointF track_pt; + QVector track_pts; QGraphicsProxyWidget *close_btn_proxy; QGraphicsProxyWidget *manage_btn_proxy; QList sigs; double cur_sec = 0; const QString mime_type = "application/x-cabanachartview"; + QAbstractSeries::SeriesType series_type = QAbstractSeries::SeriesTypeLine; + QAction *line_series_action; + QAction *scatter_series_action; }; class ChartsWidget : public QWidget { @@ -91,6 +98,7 @@ public: public slots: void setColumnCount(int n); + void removeAll(); signals: void dock(bool floating); @@ -108,7 +116,6 @@ private: void zoomIn(double min, double max); void zoomReset(); void updateToolBar(); - void removeAll(); void setMaxChartRange(int value); void updateLayout(); void settingChanged(); @@ -116,7 +123,6 @@ private: ChartView *findChart(const QString &id, const Signal *sig); QLabel *title_label; - QLabel *zoom_range_lb; QLabel *range_lb; QSlider *range_slider; bool docking = true; @@ -131,6 +137,8 @@ private: std::pair display_range; std::pair zoomed_range; bool use_dark_theme = false; + QAction *columns_lb_action; + QAction *columns_cb_action; QComboBox *columns_cb; int column_count = 1; const int CHART_MIN_WIDTH = 300; diff --git a/tools/cabana/commands.cc b/tools/cabana/commands.cc index b3f5cb1c66..e4bf999062 100644 --- a/tools/cabana/commands.cc +++ b/tools/cabana/commands.cc @@ -1,3 +1,5 @@ +#include + #include "tools/cabana/commands.h" // EditMsgCommand @@ -73,3 +75,12 @@ EditSignalCommand::EditSignalCommand(const QString &id, const Signal *sig, const void EditSignalCommand::undo() { dbc()->updateSignal(id, new_signal.name.c_str(), old_signal); } void EditSignalCommand::redo() { dbc()->updateSignal(id, old_signal.name.c_str(), new_signal); } + +namespace UndoStack { + +QUndoStack *instance() { + static QUndoStack *undo_stack = new QUndoStack(qApp); + return undo_stack; +} + +} // namespace UndoStack diff --git a/tools/cabana/commands.h b/tools/cabana/commands.h index e46223d630..c07a00b760 100644 --- a/tools/cabana/commands.h +++ b/tools/cabana/commands.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "tools/cabana/dbcmanager.h" @@ -60,3 +61,8 @@ private: Signal old_signal = {}; Signal new_signal = {}; }; + +namespace UndoStack { + QUndoStack *instance(); + inline void push(QUndoCommand *cmd) { instance()->push(cmd); } +}; diff --git a/tools/cabana/dbcmanager.cc b/tools/cabana/dbcmanager.cc index 01bdff17a1..83f5fdb74a 100644 --- a/tools/cabana/dbcmanager.cc +++ b/tools/cabana/dbcmanager.cc @@ -68,7 +68,7 @@ void DBCManager::addSignal(const QString &id, const Signal &sig) { if (auto m = const_cast(msg(id))) { auto &s = m->sigs[sig.name.c_str()]; s = sig; - emit signalAdded(&s); + emit signalAdded(parseId(id).second, &s); } } @@ -110,6 +110,7 @@ DBCManager *dbc() { std::vector DBCMsg::getSignals() const { std::vector ret; + ret.reserve(sigs.size()); for (auto &[_, sig] : sigs) ret.push_back(&sig); std::sort(ret.begin(), ret.end(), [](auto l, auto r) { return l->start_bit < r->start_bit; }); return ret; diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index 5db6737be8..03bd16f2a5 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -8,7 +8,9 @@ struct DBCMsg { QString name; uint32_t size; + // signal must be saved as value in map to make undo stack work properly. std::map sigs; + // return vector, sort by start_bits std::vector getSignals() const; }; @@ -39,7 +41,7 @@ public: } signals: - void signalAdded(const Signal *sig); + void signalAdded(uint32_t address, const Signal *sig); void signalRemoved(const Signal *sig); void signalUpdated(const Signal *sig); void msgUpdated(uint32_t address); diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 233bc493df..46f7a148f4 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -4,10 +4,8 @@ #include #include #include -#include -#include +#include -#include "selfdrive/ui/qt/util.h" #include "tools/cabana/commands.h" #include "tools/cabana/dbcmanager.h" #include "tools/cabana/streams/abstractstream.h" @@ -15,7 +13,6 @@ // DetailWidget DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { - undo_stack = new QUndoStack(this); QWidget *main_widget = new QWidget(this); QVBoxLayout *main_layout = new QVBoxLayout(main_widget); main_layout->setContentsMargins(0, 0, 0, 0); @@ -29,7 +26,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart main_layout->addWidget(tabbar); // message title - toolbar = new QToolBar(this); + QToolBar *toolbar = new QToolBar(this); toolbar->setIconSize({16, 16}); toolbar->addWidget(new QLabel("time:")); time_label = new QLabel(this); @@ -40,8 +37,8 @@ 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(bootstrapPixmap("pencil"), "", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); - remove_msg_act = toolbar->addAction(bootstrapPixmap("x-lg"), "", this, &DetailWidget::removeMsg); + toolbar->addAction(utils::icon("pencil"), "", this, &DetailWidget::editMsg)->setToolTip(tr("Edit Message")); + remove_msg_act = toolbar->addAction(utils::icon("x-lg"), "", this, &DetailWidget::removeMsg); remove_msg_act->setToolTip(tr("Remove Message")); main_layout->addWidget(toolbar); @@ -54,41 +51,35 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart main_layout->addWidget(warning_widget); // msg widget - QWidget *msg_widget = new QWidget(this); - QVBoxLayout *msg_layout = new QVBoxLayout(msg_widget); - msg_layout->setContentsMargins(0, 0, 0, 0); - // binary view - binary_view = new BinaryView(this); - msg_layout->addWidget(binary_view); - // signals - signals_layout = new QVBoxLayout(); - signals_layout->setSpacing(0); - msg_layout->addLayout(signals_layout); - msg_layout->addStretch(0); - - scroll = new QScrollArea(this); - scroll->setFrameShape(QFrame::NoFrame); - scroll->setWidget(msg_widget); - scroll->setWidgetResizable(true); - scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + splitter = new QSplitter(Qt::Vertical, this); + splitter->setAutoFillBackground(true); + splitter->addWidget(binary_view = new BinaryView(this)); + splitter->addWidget(signal_view = new SignalView(charts, this)); + binary_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); + signal_view->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + splitter->setStretchFactor(0, 0); + splitter->setStretchFactor(1, 1); tab_widget = new QTabWidget(this); tab_widget->setTabPosition(QTabWidget::South); - tab_widget->addTab(scroll, bootstrapPixmap("file-earmark-ruled"), "&Msg"); - history_log = new LogsWidget(this); - tab_widget->addTab(history_log, bootstrapPixmap("stopwatch"), "&Logs"); + tab_widget->addTab(splitter, utils::icon("file-earmark-ruled"), "&Msg"); + tab_widget->addTab(history_log = new LogsWidget(this), utils::icon("stopwatch"), "&Logs"); main_layout->addWidget(tab_widget); stacked_layout = new QStackedLayout(this); stacked_layout->addWidget(new WelcomeWidget(this)); stacked_layout->addWidget(main_widget); - QObject::connect(binary_view, &BinaryView::signalClicked, this, &DetailWidget::showForm); - QObject::connect(binary_view, &BinaryView::resizeSignal, this, &DetailWidget::resizeSignal); - QObject::connect(binary_view, &BinaryView::addSignal, this, &DetailWidget::addSignal); + QObject::connect(binary_view, &BinaryView::resizeSignal, signal_view->model, &SignalModel::resizeSignal); + QObject::connect(binary_view, &BinaryView::addSignal, signal_view->model, &SignalModel::addSignal); + QObject::connect(binary_view, &BinaryView::signalHovered, signal_view, &SignalView::signalHovered); + QObject::connect(binary_view, &BinaryView::signalClicked, signal_view, &SignalView::expandSignal); + QObject::connect(signal_view, &SignalView::showChart, charts, &ChartsWidget::showChart); + QObject::connect(signal_view, &SignalView::highlight, binary_view, &BinaryView::highlight); QObject::connect(tab_widget, &QTabWidget::currentChanged, [this]() { updateState(); }); QObject::connect(can, &AbstractStream::msgsReceived, this, &DetailWidget::updateState); - QObject::connect(dbc(), &DBCManager::DBCFileChanged, [this]() { dbcMsgChanged(); }); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &DetailWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &DetailWidget::refresh); QObject::connect(tabbar, &QTabBar::customContextMenuRequested, this, &DetailWidget::showTabBarContextMenu); QObject::connect(tabbar, &QTabBar::currentChanged, [this](int index) { if (index != -1 && tabbar->tabText(index) != msg_id) { @@ -96,14 +87,7 @@ DetailWidget::DetailWidget(ChartsWidget *charts, QWidget *parent) : charts(chart } }); QObject::connect(tabbar, &QTabBar::tabCloseRequested, tabbar, &QTabBar::removeTab); - QObject::connect(charts, &ChartsWidget::seriesChanged, this, &DetailWidget::updateChartState); - QObject::connect(history_log, &LogsWidget::openChart, [this](const QString &id, const Signal *sig) { - this->charts->showChart(id, sig, true, false); - }); - QObject::connect(undo_stack, &QUndoStack::indexChanged, [this]() { - if (undo_stack->count() > 0) - dbcMsgChanged(); - }); + QObject::connect(charts, &ChartsWidget::seriesChanged, signal_view, &SignalView::updateChartState); } void DetailWidget::showTabBarContextMenu(const QPoint &pt) { @@ -120,6 +104,16 @@ void DetailWidget::showTabBarContextMenu(const QPoint &pt) { } } +void DetailWidget::removeAll() { + msg_id = ""; + tabbar->blockSignals(true); + while (tabbar->count() > 0) { + tabbar->removeTab(0); + } + tabbar->blockSignals(false); + stacked_layout->setCurrentIndex(0); +} + void DetailWidget::setMessage(const QString &message_id) { msg_id = message_id; int index = tabbar->count() - 1; @@ -128,41 +122,27 @@ void DetailWidget::setMessage(const QString &message_id) { index = tabbar->addTab(message_id); tabbar->setTabToolTip(index, msgName(message_id)); } - tabbar->setCurrentIndex(index); - dbcMsgChanged(); - scroll->verticalScrollBar()->setValue(0); - stacked_layout->setCurrentIndex(1); -} - -void DetailWidget::dbcMsgChanged(int show_form_idx) { - if (msg_id.isEmpty()) return; setUpdatesEnabled(false); + signal_view->setMessage(msg_id); binary_view->setMessage(msg_id); history_log->setMessage(msg_id); - int i = 0; + stacked_layout->setCurrentIndex(1); + tabbar->setCurrentIndex(index); + refresh(); + splitter->setSizes({1, 2}); + + setUpdatesEnabled(true); +} + +void DetailWidget::refresh() { + if (msg_id.isEmpty()) return; + QStringList warnings; const DBCMsg *msg = dbc()->msg(msg_id); if (msg) { - for (auto sig : msg->getSignals()) { - SignalEdit *form = i < signal_list.size() ? signal_list[i] : nullptr; - if (!form) { - form = new SignalEdit(i, this); - QObject::connect(form, &SignalEdit::remove, this, &DetailWidget::removeSignal); - QObject::connect(form, &SignalEdit::save, this, &DetailWidget::saveSignal); - QObject::connect(form, &SignalEdit::showFormClicked, this, &DetailWidget::showForm); - QObject::connect(form, &SignalEdit::highlight, binary_view, &BinaryView::highlight); - QObject::connect(binary_view, &BinaryView::signalHovered, form, &SignalEdit::signalHovered); - QObject::connect(form, &SignalEdit::showChart, charts, &ChartsWidget::showChart); - signals_layout->addWidget(form); - signal_list.push_back(form); - } - form->setSignal(msg_id, sig); - form->setChartOpened(charts->hasSignal(msg_id, sig)); - ++i; - } if (msg->size != can->lastMessage(msg_id).dat.size()) { warnings.push_back(tr("Message size (%1) is incorrect.").arg(msg->size)); } @@ -172,22 +152,17 @@ void DetailWidget::dbcMsgChanged(int show_form_idx) { } else { warnings.push_back(tr("Drag-Select in binary view to create new signal.")); } - for (/**/; i < signal_list.size(); ++i) - signal_list[i]->hide(); - - toolbar->setVisible(!msg_id.isEmpty()); remove_msg_act->setEnabled(msg != nullptr); name_label->setText(msgName(msg_id)); if (!warnings.isEmpty()) { warning_label->setText(warnings.join('\n')); - warning_icon->setPixmap(bootstrapPixmap(msg ? "exclamation-triangle" : "info-circle")); + warning_icon->setPixmap(utils::icon(msg ? "exclamation-triangle" : "info-circle")); } warning_widget->setVisible(!warnings.isEmpty()); - setUpdatesEnabled(true); } -void DetailWidget::updateState(const QHash * msgs) { +void DetailWidget::updateState(const QHash *msgs) { time_label->setText(QString::number(can->currentSec(), 'f', 3)); if (msg_id.isEmpty() || (msgs && !msgs->contains(msg_id))) return; @@ -198,86 +173,18 @@ void DetailWidget::updateState(const QHash * msgs) { history_log->updateState(); } -void DetailWidget::showForm(const Signal *sig) { - setUpdatesEnabled(false); - for (auto f : signal_list) { - f->updateForm(f->sig == sig && !f->form->isVisible()); - if (f->sig == sig && f->form->isVisible()) { - QTimer::singleShot(0, [=]() { scroll->ensureWidgetVisible(f); }); - } - } - setUpdatesEnabled(true); -} - -void DetailWidget::updateChartState() { - for (auto f : signal_list) - f->setChartOpened(charts->hasSignal(f->msg_id, f->sig)); -} - void DetailWidget::editMsg() { QString id = msg_id; auto msg = dbc()->msg(id); int size = msg ? msg->size : can->lastMessage(id).dat.size(); EditMessageDialog dlg(id, msgName(id), size, this); if (dlg.exec()) { - undo_stack->push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); + UndoStack::push(new EditMsgCommand(msg_id, dlg.name_edit->text(), dlg.size_spin->value())); } } void DetailWidget::removeMsg() { - undo_stack->push(new RemoveMsgCommand(msg_id)); -} - -void DetailWidget::addSignal(int start_bit, int size, bool little_endian) { - auto msg = dbc()->msg(msg_id); - if (!msg) { - for (int i = 1; /**/; ++i) { - QString name = QString("NEW_MSG_%1").arg(i); - auto it = std::find_if(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; }); - if (it == dbc()->messages().end()) { - undo_stack->push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); - msg = dbc()->msg(msg_id); - break; - } - } - } - Signal sig = {.is_little_endian = little_endian, .factor = 1}; - for (int i = 1; /**/; ++i) { - sig.name = "NEW_SIGNAL_" + std::to_string(i); - if (msg->sigs.count(sig.name.c_str()) == 0) break; - } - updateSigSizeParamsFromRange(sig, start_bit, size); - undo_stack->push(new AddSigCommand(msg_id, sig)); -} - -void DetailWidget::resizeSignal(const Signal *sig, int start_bit, int size) { - Signal s = *sig; - updateSigSizeParamsFromRange(s, start_bit, size); - saveSignal(sig, s); -} - -void DetailWidget::saveSignal(const Signal *sig, const Signal &new_sig) { - auto msg = dbc()->msg(msg_id); - if (new_sig.name != sig->name) { - auto it = msg->sigs.find(new_sig.name.c_str()); - if (it != msg->sigs.end()) { - QString warning_str = tr("There is already a signal with the same name '%1'").arg(new_sig.name.c_str()); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - } - auto [start, end] = getSignalRange(&new_sig); - if (start < 0 || end >= msg->size * 8) { - QString warning_str = tr("Signal size [%1] exceed limit").arg(new_sig.size); - QMessageBox::warning(this, tr("Failed to save signal"), warning_str); - return; - } - - undo_stack->push(new EditSignalCommand(msg_id, sig, new_sig)); -} - -void DetailWidget::removeSignal(const Signal *sig) { - undo_stack->push(new RemoveSigCommand(msg_id, sig)); + UndoStack::push(new RemoveMsgCommand(msg_id)); } // EditMessageDialog @@ -288,7 +195,7 @@ EditMessageDialog::EditMessageDialog(const QString &msg_id, const QString &title form_layout->addRow("ID", new QLabel(msg_id)); name_edit = new QLineEdit(title, this); - name_edit->setValidator(new QRegExpValidator(QRegExp("^(\\w+)"), name_edit)); + name_edit->setValidator(new NameValidator(name_edit)); form_layout->addRow(tr("Name"), name_edit); size_spin = new QSpinBox(this); diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index c59a6fca50..a62e23f226 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -1,10 +1,9 @@ #pragma once +#include #include -#include #include #include -#include #include "tools/cabana/binaryview.h" #include "tools/cabana/chartswidget.h" @@ -30,18 +29,12 @@ class DetailWidget : public QWidget { public: DetailWidget(ChartsWidget *charts, QWidget *parent); void setMessage(const QString &message_id); - void dbcMsgChanged(int show_form_idx = -1); + void refresh(); + void removeAll(); QSize minimumSizeHint() const override { return binary_view->minimumSizeHint(); } - QUndoStack *undo_stack = nullptr; private: - void showForm(const Signal *sig); - void updateChartState(); void showTabBarContextMenu(const QPoint &pt); - void addSignal(int start_bit, int size, bool little_endian); - void resizeSignal(const Signal *sig, int from, int to); - void saveSignal(const Signal *sig, const Signal &new_sig); - void removeSignal(const Signal *sig); void editMsg(); void removeMsg(); void updateState(const QHash * msgs = nullptr); @@ -49,15 +42,13 @@ private: QString msg_id; QLabel *name_label, *time_label, *warning_icon, *warning_label; QWidget *warning_widget; - QVBoxLayout *signals_layout; QTabBar *tabbar; QTabWidget *tab_widget; - QToolBar *toolbar; QAction *remove_msg_act; LogsWidget *history_log; BinaryView *binary_view; - QScrollArea *scroll; + SignalView *signal_view; ChartsWidget *charts; + QSplitter *splitter; QStackedLayout *stacked_layout; - QList signal_list; }; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index ccc2c5533d..95bd94e76b 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -4,6 +4,8 @@ #include #include +#include "tools/cabana/commands.h" + // HistoryLogModel QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { @@ -15,24 +17,23 @@ QVariant HistoryLogModel::data(const QModelIndex &index, int role) const { } 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 ChangeTracker::toVariantList(m.colors); } return {}; } void HistoryLogModel::setMessage(const QString &message_id) { msg_id = message_id; - sigs.clear(); - if (auto dbc_msg = dbc()->msg(msg_id)) { - sigs = dbc_msg->getSignals(); - } - filter_cmp = nullptr; - refresh(); } void HistoryLogModel::refresh() { beginResetModel(); + sigs.clear(); + if (auto dbc_msg = dbc()->msg(msg_id)) { + sigs = dbc_msg->getSignals(); + } last_fetch_time = 0; + has_more_data = true; messages.clear(); hex_colors.clear(); updateState(); @@ -74,17 +75,15 @@ void HistoryLogModel::setFilter(int sig_idx, const QString &value, std::function filter_sig_idx = sig_idx; filter_value = value.toDouble(); filter_cmp = value.isEmpty() ? nullptr : cmp; - refresh(); } void HistoryLogModel::updateState() { if (!msg_id.isEmpty()) { - uint64_t current_time = (can->currentSec() + can->routeStartTime()) * 1e9; + uint64_t current_time = (can->lastMessage(msg_id).ts + can->routeStartTime()) * 1e9 + 1; auto new_msgs = dynamic_mode ? fetchData(current_time, last_fetch_time) : fetchData(0); if ((has_more_data = !new_msgs.empty())) { beginInsertRows({}, 0, new_msgs.size() - 1); messages.insert(messages.begin(), std::move_iterator(new_msgs.begin()), std::move_iterator(new_msgs.end())); - updateColors(); endInsertRows(); } last_fetch_time = current_time; @@ -97,29 +96,11 @@ 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; @@ -128,7 +109,7 @@ std::deque HistoryLogModel::fetchData(InputIt first, I for (auto it = first; it != last && (*it)->mono_time > min_time; ++it) { if ((*it)->which == cereal::Event::Which::CAN) { for (const auto &c : (*it)->event.getCan()) { - if (src == c.getSrc() && address == c.getAddress()) { + if (address == c.getAddress() && src == c.getSrc()) { const auto dat = c.getDat(); for (int i = 0; i < sigs.size(); ++i) { values[i] = get_raw_value((uint8_t *)dat.begin(), dat.size(), *(sigs[i])); @@ -153,17 +134,30 @@ template std::deque HistoryLogModel::fetchData<>(std:: std::deque HistoryLogModel::fetchData(uint64_t from_time, uint64_t min_time) { auto events = can->events(); + const auto freq = can->lastMessage(msg_id).freq; + const bool update_colors = !display_signals_mode || sigs.empty(); + if (dynamic_mode) { - auto it = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { - return e->mono_time < ts; - }); - return fetchData(it, events->rend(), min_time); + auto first = std::upper_bound(events->rbegin(), events->rend(), from_time, [=](uint64_t ts, auto &e) { return e->mono_time < ts; }); + auto msgs = fetchData(first, events->rend(), min_time); + if (update_colors && (min_time > 0 || messages.empty())) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } else { assert(min_time == 0); - auto it = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { - return ts < e->mono_time; - }); - return fetchData(it, events->end(), 0); + auto first = std::upper_bound(events->begin(), events->end(), from_time, [=](uint64_t ts, auto &e) { return ts < e->mono_time; }); + auto msgs = fetchData(first, events->end(), 0); + if (update_colors) { + for (auto it = msgs.rbegin(); it != msgs.rend(); ++it) { + hex_colors.compute(it->data, it->mono_time / (double)1e9, freq); + it->colors = hex_colors.colors; + } + } + return msgs; } } @@ -240,11 +234,21 @@ LogsWidget::LogsWidget(QWidget *parent) : QWidget(parent) { QObject::connect(comp_box, SIGNAL(activated(int)), this, SLOT(setFilter())); QObject::connect(value_edit, &QLineEdit::textChanged, this, &LogsWidget::setFilter); QObject::connect(can, &AbstractStream::seekedTo, model, &HistoryLogModel::refresh); + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &LogsWidget::refresh); + QObject::connect(UndoStack::instance(), &QUndoStack::indexChanged, this, &LogsWidget::refresh); QObject::connect(can, &AbstractStream::eventsMerged, model, &HistoryLogModel::segmentsMerged); } void LogsWidget::setMessage(const QString &message_id) { model->setMessage(message_id); + refresh(); +} + +void LogsWidget::refresh() { + if (model->msg_id.isEmpty()) return; + + model->setFilter(0, "", nullptr); + model->refresh(); bool has_signal = model->sigs.size(); if (has_signal) { signals_cb->clear(); @@ -268,4 +272,5 @@ void LogsWidget::setFilter() { case 3: cmp = std::less{}; break; } model->setFilter(signals_cb->currentIndex(), value_edit->text(), cmp); + model->refresh(); } diff --git a/tools/cabana/historylog.h b/tools/cabana/historylog.h index 83b5f623ea..2458fc1c31 100644 --- a/tools/cabana/historylog.h +++ b/tools/cabana/historylog.h @@ -33,7 +33,6 @@ public: int columnCount(const QModelIndex &parent = QModelIndex()) const override { return display_signals_mode && !sigs.empty() ? sigs.size() + 1 : 2; } - void updateColors(); void refresh(); public slots: @@ -54,7 +53,7 @@ public: std::deque fetchData(uint64_t from_time, uint64_t min_time = 0); QString msg_id; - HexColors hex_colors; + ChangeTracker hex_colors; bool has_more_data = true; const int batch_size = 50; int filter_sig_idx = -1; @@ -76,13 +75,12 @@ public: 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); - private slots: void setFilter(); private: + void refresh(); + QTableView *logs; HistoryLogModel *model; QCheckBox *dynamic_mode; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index d674ec9059..dfd38e267d 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -2,7 +2,6 @@ #include #include -#include #include #include #include @@ -15,6 +14,9 @@ #include #include +#include "tools/cabana/commands.h" +#include "tools/cabana/route.h" + static MainWindow *main_win = nullptr; void qLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { if (type == QtDebugMsg) std::cout << msg.toStdString() << std::endl; @@ -54,21 +56,25 @@ MainWindow::MainWindow() : QMainWindow() { fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); } - QObject::connect(dbc_combo, SIGNAL(activated(const QString &)), SLOT(loadDBCFromName(const QString &))); QObject::connect(this, &MainWindow::showMessage, statusBar(), &QStatusBar::showMessage); QObject::connect(this, &MainWindow::updateProgressBar, this, &MainWindow::updateDownloadProgress); QObject::connect(messages_widget, &MessagesWidget::msgSelectionChanged, detail_widget, &DetailWidget::setMessage); QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); QObject::connect(can, &AbstractStream::streamStarted, this, &MainWindow::loadDBCFromFingerprint); QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &MainWindow::DBCFileChanged); - QObject::connect(detail_widget->undo_stack, &QUndoStack::cleanChanged, [this](bool clean) { setWindowModified(!clean); }); + QObject::connect(UndoStack::instance(), &QUndoStack::cleanChanged, this, &MainWindow::undoStackCleanChanged); } void MainWindow::createActions() { QMenu *file_menu = menuBar()->addMenu(tr("&File")); + if (!can->liveStreaming()) { + file_menu->addAction(tr("Open Route..."), this, &MainWindow::openRoute); + file_menu->addSeparator(); + } + file_menu->addAction(tr("New DBC File"), this, &MainWindow::newFile)->setShortcuts(QKeySequence::New); file_menu->addAction(tr("Open DBC File..."), this, &MainWindow::openFile)->setShortcuts(QKeySequence::Open); - file_menu->addAction(tr("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); @@ -78,6 +84,17 @@ void MainWindow::createActions() { } updateRecentFileActions(); + file_menu->addSeparator(); + QMenu *load_opendbc_menu = file_menu->addMenu(tr("Load DBC from commaai/opendbc")); + // load_opendbc_menu->setStyleSheet("QMenu { menu-scrollable: true; }"); + auto dbc_names = dbc()->allDBCNames(); + std::sort(dbc_names.begin(), dbc_names.end()); + for (const auto &name : dbc_names) { + load_opendbc_menu->addAction(QString::fromStdString(name), this, &MainWindow::openOpendbcFile); + } + + file_menu->addAction(tr("Load DBC From Clipboard"), this, &MainWindow::loadDBCFromClipboard); + file_menu->addSeparator(); file_menu->addAction(tr("Save DBC..."), this, &MainWindow::save)->setShortcuts(QKeySequence::Save); file_menu->addAction(tr("Save DBC As..."), this, &MainWindow::saveAs)->setShortcuts(QKeySequence::SaveAs); @@ -89,16 +106,16 @@ void MainWindow::createActions() { file_menu->addAction(tr("E&xit"), qApp, &QApplication::closeAllWindows)->setShortcuts(QKeySequence::Quit); QMenu *edit_menu = menuBar()->addMenu(tr("&Edit")); - auto undo_act = detail_widget->undo_stack->createUndoAction(this, tr("&Undo")); + auto undo_act = UndoStack::instance()->createUndoAction(this, tr("&Undo")); undo_act->setShortcuts(QKeySequence::Undo); edit_menu->addAction(undo_act); - auto redo_act = detail_widget->undo_stack->createRedoAction(this, tr("&Rndo")); + auto redo_act = UndoStack::instance()->createRedoAction(this, tr("&Rndo")); redo_act->setShortcuts(QKeySequence::Redo); edit_menu->addAction(redo_act); edit_menu->addSeparator(); QMenu *commands_menu = edit_menu->addMenu(tr("Command &List")); - auto undo_view = new QUndoView(detail_widget->undo_stack); + auto undo_view = new QUndoView(UndoStack::instance()); undo_view->setWindowTitle(tr("Command List")); QWidgetAction *commands_act = new QWidgetAction(this); commands_act->setDefaultWidget(undo_view); @@ -113,18 +130,12 @@ void MainWindow::createActions() { void MainWindow::createDockWindows() { // left panel - QWidget *messages_container = new QWidget(this); - QVBoxLayout *messages_layout = new QVBoxLayout(messages_container); - dbc_combo = createDBCSelector(); - messages_layout->addWidget(dbc_combo); messages_widget = new MessagesWidget(this); - messages_layout->addWidget(messages_widget); - QDockWidget *dock = new QDockWidget(tr("MESSAGES"), this); dock->setObjectName("MessagesPanel"); dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea | Qt::TopDockWidgetArea | Qt::BottomDockWidgetArea); dock->setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - dock->setWidget(messages_container); + dock->setWidget(messages_widget); addDockWidget(Qt::LeftDockWidgetArea, dock); // right panel @@ -156,23 +167,6 @@ void MainWindow::createDockWindows() { addDockWidget(Qt::RightDockWidgetArea, video_dock); } -QComboBox *MainWindow::createDBCSelector() { - QComboBox *c = new QComboBox(this); - c->setEditable(true); - c->lineEdit()->setPlaceholderText(tr("Select from an existing DBC file")); - c->setInsertPolicy(QComboBox::NoInsert); - c->completer()->setCompletionMode(QCompleter::PopupCompletion); - c->completer()->setFilterMode(Qt::MatchContains); - - auto dbc_names = dbc()->allDBCNames(); - std::sort(dbc_names.begin(), dbc_names.end()); - for (const auto &name : dbc_names) { - c->addItem(QString::fromStdString(name)); - } - c->setCurrentIndex(-1); - return c; -} - void MainWindow::createStatusBar() { progress_bar = new QProgressBar(); progress_bar->setRange(0, 100); @@ -188,13 +182,26 @@ void MainWindow::createShortcuts() { // TODO: add more shortcuts here. } +void MainWindow::undoStackCleanChanged(bool clean) { + setWindowModified(!clean); +} + void MainWindow::DBCFileChanged() { - detail_widget->undo_stack->clear(); - int index = dbc_combo->findText(QFileInfo(dbc()->name()).baseName()); - dbc_combo->setCurrentIndex(index); + UndoStack::instance()->clear(); setWindowFilePath(QString("%1").arg(dbc()->name())); } +void MainWindow::openRoute() { + OpenRouteDialog dlg(this); + if (dlg.exec()) { + detail_widget->removeAll(); + charts_widget->removeAll(); + statusBar()->showMessage(tr("Route %1 loaded").arg(can->routeName()), 2000); + } else if (dlg.failedToLoad()) { + close(); + } +} + void MainWindow::newFile() { remindSaveChanges(); dbc()->open("untitled.dbc", ""); @@ -220,6 +227,13 @@ void MainWindow::loadFile(const QString &fn) { } } +void MainWindow::openOpendbcFile() { + if (auto action = qobject_cast(sender())) { + remindSaveChanges(); + loadDBCFromOpendbc(action->text()); + } +} + void MainWindow::openRecentFile() { if (auto action = qobject_cast(sender())) { remindSaveChanges(); @@ -227,7 +241,7 @@ void MainWindow::openRecentFile() { } } -void MainWindow::loadDBCFromName(const QString &name) { +void MainWindow::loadDBCFromOpendbc(const QString &name) { if (name != dbc()->name()) { remindSaveChanges(); dbc()->open(name); @@ -246,13 +260,18 @@ void MainWindow::loadDBCFromClipboard() { } void MainWindow::loadDBCFromFingerprint() { + // Don't overwrite already loaded DBC + if (!dbc()->name().isEmpty()) { + return; + } + remindSaveChanges(); auto fingerprint = can->carFingerprint(); video_dock->setWindowTitle(tr("ROUTE: %1 FINGERPINT: %2").arg(can->routeName()).arg(fingerprint.isEmpty() ? tr("Unknown Car") : fingerprint)); if (!fingerprint.isEmpty()) { auto dbc_name = fingerprint_to_dbc[fingerprint]; if (dbc_name != QJsonValue::Undefined) { - loadDBCFromName(dbc_name.toString()); + loadDBCFromOpendbc(dbc_name.toString()); return; } } @@ -271,7 +290,7 @@ void MainWindow::saveFile(const QString &fn) { QFile file(fn); if (file.open(QIODevice::WriteOnly)) { file.write(dbc()->generateDBC().toUtf8()); - detail_widget->undo_stack->setClean(); + UndoStack::instance()->setClean(); setCurrentFile(fn); statusBar()->showMessage(tr("File saved"), 2000); } @@ -318,7 +337,7 @@ void MainWindow::updateRecentFileActions() { void MainWindow::remindSaveChanges() { bool discard_changes = false; - while (!detail_widget->undo_stack->isClean() && !discard_changes) { + while (!UndoStack::instance()->isClean() && !discard_changes) { int ret = (QMessageBox::question(this, tr("Unsaved Changes"), tr("You have unsaved changes. Press ok to save them, cancel to discard."), QMessageBox::Ok | QMessageBox::Cancel)); @@ -328,7 +347,7 @@ void MainWindow::remindSaveChanges() { discard_changes = true; } } - detail_widget->undo_stack->clear(); + UndoStack::instance()->clear(); current_file = ""; } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index 4e65fa01cf..5e627df58b 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -21,12 +20,15 @@ public: MainWindow(); void dockCharts(bool dock); void showStatusMessage(const QString &msg, int timeout = 0) { statusBar()->showMessage(msg, timeout); } + void loadFile(const QString &fn); public slots: + void openRoute(); void newFile(); void openFile(); void openRecentFile(); - void loadDBCFromName(const QString &name); + void openOpendbcFile(); + void loadDBCFromOpendbc(const QString &name); void loadDBCFromFingerprint(); void loadDBCFromClipboard(); void save(); @@ -40,12 +42,10 @@ signals: 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(); void createStatusBar(); void createShortcuts(); void closeEvent(QCloseEvent *event) override; @@ -53,6 +53,7 @@ protected: void updateDownloadProgress(uint64_t cur, uint64_t total, bool success); void setOption(); void findSimilarBits(); + void undoStackCleanChanged(bool clean); VideoWidget *video_widget = nullptr; QDockWidget *video_dock; @@ -63,7 +64,6 @@ protected: QVBoxLayout *charts_layout; QProgressBar *progress_bar; QJsonDocument fingerprint_to_dbc; - QComboBox *dbc_combo; QSplitter *video_splitter;; QString current_file = ""; enum { MAX_RECENT_FILES = 15 }; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index 69c6d298c0..359fe10f50 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,19 +1,19 @@ #include "tools/cabana/messageswidget.h" -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include "tools/cabana/dbcmanager.h" MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); // message filter - QLineEdit *filter = new QLineEdit(this); + filter = new QLineEdit(this); filter->setClearButtonEnabled(true); filter->setPlaceholderText(tr("filter messages")); main_layout->addWidget(filter); @@ -27,17 +27,22 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget->setSelectionMode(QAbstractItemView::SingleSelection); table_widget->setSortingEnabled(true); table_widget->sortByColumn(0, Qt::AscendingOrder); - table_widget->setColumnWidth(0, 250); - table_widget->setColumnWidth(1, 80); - table_widget->setColumnWidth(2, 80); table_widget->horizontalHeader()->setStretchLastSection(true); table_widget->verticalHeader()->hide(); - main_layout->addWidget(table_widget); + // suppress + QHBoxLayout *suppress_layout = new QHBoxLayout(); + suppress_add = new QPushButton("Suppress Highlighted"); + suppress_clear = new QPushButton(); + suppress_layout->addWidget(suppress_add); + suppress_layout->addWidget(suppress_clear); + main_layout->addLayout(suppress_layout); + // signals/slots - QObject::connect(filter, &QLineEdit::textChanged, model, &MessageListModel::setFilterString); + QObject::connect(filter, &QLineEdit::textEdited, model, &MessageListModel::setFilterString); QObject::connect(can, &AbstractStream::msgsReceived, model, &MessageListModel::msgsReceived); + QObject::connect(can, &AbstractStream::streamStarted, this, &MessagesWidget::reset); QObject::connect(dbc(), &DBCManager::DBCFileChanged, model, &MessageListModel::sortMessages); QObject::connect(dbc(), &DBCManager::msgUpdated, model, &MessageListModel::sortMessages); QObject::connect(dbc(), &DBCManager::msgRemoved, model, &MessageListModel::sortMessages); @@ -50,6 +55,16 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { } } }); + QObject::connect(suppress_add, &QPushButton::clicked, [=]() { + model->suppress(); + updateSuppressedButtons(); + }); + QObject::connect(suppress_clear, &QPushButton::clicked, [=]() { + model->clearSuppress(); + updateSuppressedButtons(); + }); + + updateSuppressedButtons(); } void MessagesWidget::selectMessage(const QString &msg_id) { @@ -58,6 +73,24 @@ void MessagesWidget::selectMessage(const QString &msg_id) { } } +void MessagesWidget::updateSuppressedButtons() { + if (model->suppressed_bytes.empty()) { + suppress_clear->setEnabled(false); + suppress_clear->setText("Clear Suppressed"); + } else { + suppress_clear->setEnabled(true); + suppress_clear->setText(QString("Clear Suppressed (%1)").arg(model->suppressed_bytes.size())); + } +} + +void MessagesWidget::reset() { + model->reset(); + filter->clear(); + current_msg_id = ""; + updateSuppressedButtons(); +} + + // MessageListModel QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, int role) const { @@ -79,33 +112,38 @@ QVariant MessageListModel::data(const QModelIndex &index, int role) const { case 4: return toHex(can_data.dat); } } else if (role == Qt::UserRole && index.column() == 4) { - return HexColors::toVariantList(can_data.colors); + QList colors; + colors.reserve(can_data.dat.size()); + for (int i = 0; i < can_data.dat.size(); i++){ + if (suppressed_bytes.contains({id, i})) { + colors.append(QColor(255, 255, 255, 0)); + } else { + colors.append(i < can_data.colors.size() ? can_data.colors[i] : QColor(255, 255, 255, 0)); + } + } + return colors; + } return {}; } void MessageListModel::setFilterString(const QString &string) { + auto contains = [](const QString &id, const QString &txt) { + auto cs = Qt::CaseInsensitive; + if (id.contains(txt, cs) || msgName(id).contains(txt, cs)) return true; + // Search by signal name + if (const auto msg = dbc()->msg(id)) { + for (auto &signal : msg->getSignals()) { + if (QString::fromStdString(signal->name).contains(txt, cs)) return true; + } + } + return false; + }; + filter_str = string; 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) { + if (filter_str.isEmpty() || contains(it.key(), filter_str)) { msgs.push_back(it.key()); } } @@ -122,8 +160,8 @@ void MessageListModel::sortMessages() { }); } else if (sort_column == 1) { std::sort(msgs.begin(), msgs.end(), [this](auto &l, auto &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}; + auto ll = DBCManager::parseId(l); + auto rr = DBCManager::parseId(r); return sort_order == Qt::AscendingOrder ? ll < rr : ll > rr; }); } else if (sort_column == 2) { @@ -144,10 +182,11 @@ void MessageListModel::sortMessages() { void MessageListModel::msgsReceived(const QHash *new_msgs) { int prev_row_count = msgs.size(); - if (filter_str.isEmpty() && msgs.size() != can->can_msgs.size()) { + bool update_all = new_msgs->size() == can->can_msgs.size(); + if (update_all || (filter_str.isEmpty() && msgs.size() != can->can_msgs.size())) { msgs = can->can_msgs.keys(); } - if (msgs.size() != prev_row_count) { + if (update_all || msgs.size() != prev_row_count) { sortMessages(); return; } @@ -166,3 +205,29 @@ void MessageListModel::sort(int column, Qt::SortOrder order) { sortMessages(); } } + +void MessageListModel::suppress() { + const double cur_ts = can->currentSec(); + + for (auto &id : msgs) { + auto &can_data = can->lastMessage(id); + for (int i = 0; i < can_data.dat.size(); i++) { + const double dt = cur_ts - can_data.last_change_t[i]; + if (dt < 2.0) { + suppressed_bytes.insert({id, i}); + } + } + } +} + +void MessageListModel::clearSuppress() { + suppressed_bytes.clear(); +} + +void MessageListModel::reset() { + beginResetModel(); + filter_str = ""; + msgs.clear(); + clearSuppress(); + endResetModel(); +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 69fdd26170..81ee36cd6f 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -2,8 +2,10 @@ #include #include -#include +#include +#include #include +#include #include "tools/cabana/streams/abstractstream.h" @@ -20,7 +22,11 @@ public: void setFilterString(const QString &string); void msgsReceived(const QHash *new_msgs = nullptr); void sortMessages(); + void suppress(); + void clearSuppress(); + void reset(); QStringList msgs; + QSet> suppressed_bytes; private: QString filter_str; @@ -36,6 +42,8 @@ public: void selectMessage(const QString &message_id); QByteArray saveHeaderState() const { return table_widget->horizontalHeader()->saveState(); } bool restoreHeaderState(const QByteArray &state) const { return table_widget->horizontalHeader()->restoreState(state); } + void updateSuppressedButtons(); + void reset(); signals: void msgSelectionChanged(const QString &message_id); @@ -43,5 +51,9 @@ signals: protected: QTableView *table_widget; QString current_msg_id; + QLineEdit *filter; MessageListModel *model; + QPushButton *suppress_add; + QPushButton *suppress_clear; + }; diff --git a/tools/cabana/route.cc b/tools/cabana/route.cc new file mode 100644 index 0000000000..ab322cdf90 --- /dev/null +++ b/tools/cabana/route.cc @@ -0,0 +1,68 @@ +#include "tools/cabana/route.h" + +#include +#include +#include +#include +#include +#include + +#include "tools/cabana/streams/replaystream.h" + +OpenRouteDialog::OpenRouteDialog(QWidget *parent) : QDialog(parent) { + // TODO: get route list from api.comma.ai + QHBoxLayout *edit_layout = new QHBoxLayout; + edit_layout->addWidget(new QLabel(tr("Route:"))); + edit_layout->addWidget(route_edit = new QLineEdit(this)); + route_edit->setPlaceholderText(tr("Enter remote route name or click browse to select a local route")); + auto file_btn = new QPushButton(tr("Browse..."), this); + edit_layout->addWidget(file_btn); + + btn_box = new QDialogButtonBox(QDialogButtonBox::Open | QDialogButtonBox::Cancel); + btn_box->button(QDialogButtonBox::Open)->setEnabled(false); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->addStretch(0); + main_layout->addLayout(edit_layout); + main_layout->addStretch(0); + main_layout->addWidget(btn_box); + setMinimumSize({550, 120}); + + QObject::connect(btn_box, &QDialogButtonBox::accepted, this, &OpenRouteDialog::loadRoute); + QObject::connect(btn_box, &QDialogButtonBox::rejected, this, &QDialog::reject); + QObject::connect(route_edit, &QLineEdit::textChanged, [this]() { + btn_box->button(QDialogButtonBox::Open)->setEnabled(!route_edit->text().isEmpty()); + }); + QObject::connect(file_btn, &QPushButton::clicked, [=]() { + QString dir = QFileDialog::getExistingDirectory(this, tr("Open Local Route"), settings.last_route_dir); + if (!dir.isEmpty()) { + route_edit->setText(dir); + settings.last_route_dir = QFileInfo(dir).absolutePath(); + } + }); +} + +void OpenRouteDialog::loadRoute() { + btn_box->setEnabled(false); + + QString route = route_edit->text(); + QString data_dir; + if (int idx = route.lastIndexOf('/'); idx != -1) { + data_dir = route.mid(0, idx + 1); + route = route.mid(idx + 1); + } + + bool is_valid_format = Route::parseRoute(route).str.size() > 0; + if (!is_valid_format) { + QMessageBox::warning(nullptr, tr("Warning"), tr("Invalid route format: '%1'").arg(route)); + } else { + failed_to_load = !dynamic_cast(can)->loadRoute(route, data_dir); + if (failed_to_load) { + QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to load route: '%1'").arg(route)); + } else { + accept(); + } + } + + btn_box->setEnabled(true); +} diff --git a/tools/cabana/route.h b/tools/cabana/route.h new file mode 100644 index 0000000000..ceda71d585 --- /dev/null +++ b/tools/cabana/route.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include + +class OpenRouteDialog : public QDialog { + Q_OBJECT + +public: + OpenRouteDialog(QWidget *parent); + void loadRoute(); + inline bool failedToLoad() const { return failed_to_load; } + +private: + QLineEdit *route_edit; + QDialogButtonBox *btn_box; + bool failed_to_load = false; +}; diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 78969c76c3..6cbd16cabf 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -20,11 +20,13 @@ void Settings::save() { s.setValue("chart_range", chart_range); s.setValue("chart_column_count", chart_column_count); s.setValue("last_dir", last_dir); + s.setValue("last_route_dir", last_route_dir); s.setValue("window_state", window_state); s.setValue("geometry", geometry); s.setValue("video_splitter_state", video_splitter_state); s.setValue("recent_files", recent_files); s.setValue("message_header_state", message_header_state); + s.setValue("chart_series_type", chart_series_type); } void Settings::load() { @@ -35,11 +37,13 @@ void Settings::load() { 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(); + last_route_dir = s.value("last_route_dir", QDir::homePath()).toString(); window_state = s.value("window_state").toByteArray(); geometry = s.value("geometry").toByteArray(); video_splitter_state = s.value("video_splitter_state").toByteArray(); recent_files = s.value("recent_files").toStringList(); message_header_state = s.value("message_header_state").toByteArray(); + chart_series_type = s.value("chart_series_type", 0).toInt(); } // SettingsDlg @@ -60,6 +64,11 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { cached_minutes->setValue(settings.max_cached_minutes); form_layout->addRow(tr("Max Cached Minutes"), cached_minutes); + chart_series_type = new QComboBox(this); + chart_series_type->addItems({tr("Line"), tr("Scatter")}); + chart_series_type->setCurrentIndex(settings.chart_series_type); + form_layout->addRow(tr("Chart Default Series Type"), chart_series_type); + chart_height = new QSpinBox(this); chart_height->setRange(100, 500); chart_height->setSingleStep(10); @@ -77,6 +86,7 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { void SettingsDlg::save() { settings.fps = fps->value(); settings.max_cached_minutes = cached_minutes->value(); + settings.chart_series_type = chart_series_type->currentIndex(); settings.chart_height = chart_height->value(); settings.save(); accept(); diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index 56c6a992ae..a302d20077 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -18,7 +18,9 @@ public: int chart_height = 200; int chart_column_count = 1; int chart_range = 3 * 60; // e minutes + int chart_series_type = 0; QString last_dir; + QString last_route_dir; QByteArray geometry; QByteArray video_splitter_state; QByteArray window_state; @@ -38,6 +40,7 @@ public: QSpinBox *fps; QSpinBox *cached_minutes; QSpinBox *chart_height; + QComboBox *chart_series_type; }; extern Settings settings; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index 112c2807f9..b871ffa21a 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -1,174 +1,192 @@ #include "tools/cabana/signaledit.h" -#include -#include #include #include +#include +#include +#include +#include #include -#include "selfdrive/ui/qt/util.h" +#include "tools/cabana/commands.h" -// SignalForm +// SignalModel -SignalForm::SignalForm(QWidget *parent) : QWidget(parent) { - 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); - hl->addWidget(size); - endianness = new QComboBox(); - endianness->addItems({"Little Endianness", "Big Endianness"}); - hl->addWidget(endianness); - sign = new QComboBox(); - sign->addItems({"Signed", "Unsigned"}); - 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(); - extra_layout->addRow(tr("Unit"), unit); - comment = new QLineEdit(); - extra_layout->addRow(tr("Comment"), comment); - min_val = new QLineEdit(); - min_val->setValidator(double_validator); - extra_layout->addRow(tr("Minimum value"), min_val); - max_val = new QLineEdit(); - max_val->setValidator(double_validator); - extra_layout->addRow(tr("Maximum value"), max_val); - val_desc = new QLineEdit(); - 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); - QObject::connect(offset, &QLineEdit::editingFinished, this, &SignalForm::textBoxEditingFinished); - 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...")); - }); +SignalModel::SignalModel(QObject *parent) : root(new Item), QAbstractItemModel(parent) { + QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &SignalModel::refresh); + QObject::connect(dbc(), &DBCManager::msgUpdated, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::msgRemoved, this, &SignalModel::handleMsgChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, this, &SignalModel::handleSignalAdded); + QObject::connect(dbc(), &DBCManager::signalUpdated, this, &SignalModel::handleSignalUpdated); + QObject::connect(dbc(), &DBCManager::signalRemoved, this, &SignalModel::handleSignalRemoved); + QObject::connect(can, &AbstractStream::msgsReceived, this, &SignalModel::updateState); } -void SignalForm::textBoxEditingFinished() { - QLineEdit *edit = qobject_cast(QObject::sender()); - if (edit && edit->isModified()) { - edit->setModified(false); - emit changed(); +void SignalModel::insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig) { + Item *item = new Item{.sig = sig, .parent = parent_item, .title = sig->name.c_str(), .type = Item::Sig}; + parent_item->children.insert(pos, item); + QString titles[]{"Name", "Size", "Little Endian", "Signed", "Offset", "Factor", "Extra Info", "Unit", "Comment", "Minimum", "Maximum", "Description"}; + for (int i = 0; i < std::size(titles); ++i) { + item->children.push_back(new Item{.sig = sig, .parent = item, .title = titles[i], .type = (Item::Type)(i + Item::Name)}); } } -// SignalEdit - -SignalEdit::SignalEdit(int index, QWidget *parent) : form_idx(index), QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); - main_layout->setSpacing(0); - - bg_color = QColor(getColor(form_idx)); - - // title bar - auto title_bar = new QWidget(this); - title_bar->setFixedHeight(32); - QHBoxLayout *title_layout = new QHBoxLayout(title_bar); - title_layout->setContentsMargins(0, 0, 0, 0); - title_bar->setStyleSheet("QToolButton {width:15px;height:15px;font-size:15px}"); - color_label = new QLabel(this); - color_label->setFixedWidth(25); - color_label->setContentsMargins(5, 0, 0, 0); - title_layout->addWidget(color_label); - icon = new QLabel(this); - title_layout->addWidget(icon); - title = new ElidedLabel(this); - title->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - title_layout->addWidget(title); - - plot_btn = new QToolButton(this); - plot_btn->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->setIcon(bootstrapPixmap("x")); - remove_btn->setToolTip(tr("Remove signal")); - title_layout->addWidget(remove_btn); - main_layout->addWidget(title_bar); - - // signal form - form = new SignalForm(this); - form->setVisible(false); - main_layout->addWidget(form); - - // bottom line - QFrame *hline = new QFrame(); - hline->setFrameShape(QFrame::HLine); - hline->setFrameShadow(QFrame::Sunken); - main_layout->addWidget(hline); - - QObject::connect(title, &ElidedLabel::clicked, [this]() { emit showFormClicked(sig); }); - QObject::connect(plot_btn, &QToolButton::clicked, [this](bool checked) { - emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); - }); - QObject::connect(remove_btn, &QToolButton::clicked, [this]() { emit remove(sig); }); - QObject::connect(form, &SignalForm::changed, this, &SignalEdit::saveSignal); - setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); +void SignalModel::setMessage(const QString &id) { + msg_id = id; + filter_str = ""; + refresh(); + updateState(nullptr); } -void SignalEdit::setSignal(const QString &message_id, const Signal *signal) { - sig = signal; - updateForm(msg_id == message_id && form->isVisible()); - msg_id = message_id; - color_label->setText(QString::number(form_idx + 1)); - color_label->setStyleSheet(QString("color:black; background-color:%2").arg(bg_color.name())); - title->setText(sig->name.c_str()); - show(); +void SignalModel::setFilter(const QString &txt) { + filter_str = txt; + refresh(); } -void SignalEdit::saveSignal() { - Signal s = *sig; - s.name = form->name->text().toStdString(); - s.size = form->size->text().toInt(); - s.offset = form->offset->text().toDouble(); - s.factor = form->factor->text().toDouble(); - s.is_signed = form->sign->currentIndex() == 0; - bool little_endian = form->endianness->currentIndex() == 0; - if (little_endian != s.is_little_endian) { +void SignalModel::refresh() { + beginResetModel(); + root.reset(new SignalModel::Item); + if (auto msg = dbc()->msg(msg_id)) { + for (auto &s : msg->getSignals()) { + if (filter_str.isEmpty() || QString::fromStdString(s->name).contains(filter_str, Qt::CaseInsensitive)) { + insertItem(root.get(), root->children.size(), s); + } + } + } + endResetModel(); +} + +void SignalModel::updateState(const QHash *msgs) { + if (!msgs || (msgs->contains(msg_id))) { + auto &dat = can->lastMessage(msg_id).dat; + int row = 0; + for (auto item : root->children) { + double value = get_raw_value((uint8_t *)dat.begin(), dat.size(), *item->sig); + item->sig_val = QString::number(value); + emit dataChanged(index(row, 1), index(row, 1), {Qt::DisplayRole}); + ++row; + } + } +} + +int SignalModel::rowCount(const QModelIndex &parent) const { + if (parent.column() > 0) return 0; + + auto parent_item = getItem(parent); + int row_count = parent_item->children.size(); + if (parent_item->type == Item::Sig && !parent_item->extra_expanded) { + row_count -= (Item::Desc - Item::ExtraInfo); + } + return row_count; +} + +Qt::ItemFlags SignalModel::flags(const QModelIndex &index) const { + if (!index.isValid()) return Qt::NoItemFlags; + + auto item = getItem(index); + Qt::ItemFlags flags = Qt::ItemIsSelectable | Qt::ItemIsEnabled; + if (index.column() == 1 && item->type != Item::Sig && item->type != Item::ExtraInfo) { + flags |= (item->type == Item::Endian || item->type == Item::Signed) ? Qt::ItemIsUserCheckable : Qt::ItemIsEditable; + } + return flags; +} + +int SignalModel::signalRow(const Signal *sig) const { + auto &children = root->children; + for (int i = 0; i < children.size(); ++i) { + if (children[i]->sig == sig) return i; + } + return -1; +} + +QModelIndex SignalModel::index(int row, int column, const QModelIndex &parent) const { + if (!hasIndex(row, column, parent)) return {}; + return createIndex(row, column, getItem(parent)->children[row]); +} + +QModelIndex SignalModel::parent(const QModelIndex &index) const { + if (!index.isValid()) return {}; + Item *parent_item = getItem(index)->parent; + return parent_item == root.get() ? QModelIndex() : createIndex(parent_item->row(), 0, parent_item); +} + +QVariant SignalModel::data(const QModelIndex &index, int role) const { + if (index.isValid()) { + const Item *item = getItem(index); + if (role == Qt::DisplayRole || role == Qt::EditRole) { + if (index.column() == 0) { + return item->type == Item::Sig ? QString::fromStdString(item->sig->name) : item->title; + } else { + switch (item->type) { + case Item::Sig: return item->sig_val; + case Item::Name: return QString::fromStdString(item->sig->name); + case Item::Size: return item->sig->size; + case Item::Offset: return QString::number(item->sig->offset, 'f', 6); + case Item::Factor: return QString::number(item->sig->factor, 'f', 6); + default: break; + } + } + } else if (role == Qt::CheckStateRole && index.column() == 1) { + if (item->type == Item::Endian) return item->sig->is_little_endian ? Qt::Checked : Qt::Unchecked; + if (item->type == Item::Signed) return item->sig->is_signed ? Qt::Checked : Qt::Unchecked; + } else if (role == Qt::DecorationRole && index.column() == 0 && item->type == Item::ExtraInfo) { + return utils::icon(item->parent->extra_expanded ? "chevron-compact-down" : "chevron-compact-up"); + } + } + return {}; +} + +bool SignalModel::setData(const QModelIndex &index, const QVariant &value, int role) { + if (role != Qt::EditRole && role != Qt::CheckStateRole) return false; + + Item *item = getItem(index); + Signal s = *item->sig; + switch (item->type) { + case Item::Name: s.name = value.toString().toStdString(); break; + case Item::Size: s.size = value.toInt(); break; + case Item::Endian: s.is_little_endian = value.toBool(); break; + case Item::Signed: s.is_signed = value.toBool(); break; + case Item::Offset: s.offset = value.toDouble(); break; + case Item::Factor: s.factor = value.toDouble(); break; + default: return false; + } + bool ret = saveSignal(item->sig, s); + emit dataChanged(index, index, {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + return ret; +} + +void SignalModel::showExtraInfo(const QModelIndex &index) { + auto item = getItem(index); + if (item->type == Item::ExtraInfo) { + if (!item->parent->extra_expanded) { + item->parent->extra_expanded = true; + beginInsertRows(index.parent(), 7, 13); + endInsertRows(); + } else { + item->parent->extra_expanded = false; + beginRemoveRows(index.parent(), 7, 13); + endRemoveRows(); + } + } +} + +bool SignalModel::saveSignal(const Signal *origin_s, Signal &s) { + auto msg = dbc()->msg(msg_id); + if (s.name != origin_s->name && msg->sigs.count(s.name.c_str()) != 0) { + QString text = tr("There is already a signal with the same name '%1'").arg(s.name.c_str()); + QMessageBox::warning(nullptr, tr("Failed to save signal"), text); + return false; + } + + if (s.is_little_endian != origin_s->is_little_endian) { int start = std::floor(s.start_bit / 8); - if (little_endian) { + if (s.is_little_endian) { int end = std::floor((s.start_bit - s.size + 1) / 8); s.start_bit = start == end ? s.start_bit - s.size + 1 : bigEndianStartBitsIndex(s.start_bit); } else { int end = std::floor((s.start_bit + s.size - 1) / 8); s.start_bit = start == end ? s.start_bit + s.size - 1 : bigEndianBitIndex(s.start_bit); } - s.is_little_endian = little_endian; } if (s.is_little_endian) { s.lsb = s.start_bit; @@ -177,43 +195,257 @@ void SignalEdit::saveSignal() { s.lsb = bigEndianStartBitsIndex(bigEndianBitIndex(s.start_bit) + s.size - 1); s.msb = s.start_bit; } - if (s != *sig) { - emit save(this->sig, s); - } + + UndoStack::push(new EditSignalCommand(msg_id, origin_s, s)); + return true; } -void SignalEdit::setChartOpened(bool opened) { - plot_btn->setToolTip(opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened chart")); - plot_btn->setChecked(opened); -} - -void SignalEdit::updateForm(bool visible) { - if (visible && sig) { - if (form->name->text() != sig->name.c_str()) { - form->name->setText(sig->name.c_str()); +void SignalModel::addSignal(int start_bit, int size, bool little_endian) { + auto msg = dbc()->msg(msg_id); + for (int i = 1; !msg; ++i) { + QString name = QString("NEW_MSG_%1").arg(i); + if (std::none_of(dbc()->messages().begin(), dbc()->messages().end(), [&](auto &m) { return m.second.name == name; })) { + UndoStack::push(new EditMsgCommand(msg_id, name, can->lastMessage(msg_id).dat.size())); + msg = dbc()->msg(msg_id); } - form->endianness->setCurrentIndex(sig->is_little_endian ? 0 : 1); - form->sign->setCurrentIndex(sig->is_signed ? 0 : 1); - form->factor->setText(QString::number(sig->factor)); - form->offset->setText(QString::number(sig->offset)); - form->size->setValue(sig->size); } - form->setVisible(visible); - icon->setText(visible ? "▼ " : "> "); + + Signal sig = {.is_little_endian = little_endian, .factor = 1}; + for (int i = 1; /**/; ++i) { + sig.name = "NEW_SIGNAL_" + std::to_string(i); + if (msg->sigs.count(sig.name.c_str()) == 0) break; + } + updateSigSizeParamsFromRange(sig, start_bit, size); + UndoStack::push(new AddSigCommand(msg_id, sig)); } -void SignalEdit::signalHovered(const Signal *s) { - 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 SignalModel::resizeSignal(const Signal *sig, int start_bit, int size) { + Signal s = *sig; + updateSigSizeParamsFromRange(s, start_bit, size); + saveSignal(sig, s); } -void SignalEdit::enterEvent(QEvent *event) { - emit highlight(sig); - QWidget::enterEvent(event); +void SignalModel::removeSignal(const Signal *sig) { + UndoStack::push(new RemoveSigCommand(msg_id, sig)); } -void SignalEdit::leaveEvent(QEvent *event) { - emit highlight(nullptr); - QWidget::leaveEvent(event); +void SignalModel::handleMsgChanged(uint32_t address) { + if (address == DBCManager::parseId(msg_id).second) { + refresh(); + } +} + +void SignalModel::handleSignalAdded(uint32_t address, const Signal *sig) { + if (address == DBCManager::parseId(msg_id).second) { + int i = 0; + for (; i < root->children.size(); ++i) { + if (sig->start_bit < root->children[i]->sig->start_bit) break; + } + beginInsertRows({}, i, i); + insertItem(root.get(), i, sig); + endInsertRows(); + } +} + +void SignalModel::handleSignalUpdated(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + emit dataChanged(index(row, 0), index(row, 1), {Qt::DisplayRole, Qt::EditRole, Qt::CheckStateRole}); + } +} + +void SignalModel::handleSignalRemoved(const Signal *sig) { + if (int row = signalRow(sig); row != -1) { + beginRemoveRows({}, row, row); + delete root->children.takeAt(row); + endRemoveRows(); + } +} + +// SignalItemDelegate + +SignalItemDelegate::SignalItemDelegate(QObject *parent) { + name_validator = new NameValidator(this); + double_validator = new QDoubleValidator(this); + double_validator->setLocale(QLocale::C); // Match locale of QString::toDouble() instead of system +} + +void SignalItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item && !index.parent().isValid() && index.column() == 0) { + painter->save(); + painter->setRenderHint(QPainter::Antialiasing); + if (option.state & QStyle::State_Selected) { + painter->fillRect(option.rect, option.palette.highlight()); + } + + // color label + auto bg_color = QColor(getColor(item->row())); + QRect rc{option.rect.left() + 3, option.rect.top(), 22, option.rect.height()}; + painter->setPen(Qt::NoPen); + painter->setBrush(item->highlight ? bg_color.darker(125) : bg_color); + painter->drawRoundedRect(rc.adjusted(0, 2, 0, -2), 5, 5); + painter->setPen(item->highlight ? Qt::white : Qt::black); + painter->drawText(rc, Qt::AlignCenter, QString::number(item->row() + 1)); + + // signal name + QFont font; + font.setBold(true); + painter->setFont(font); + painter->setPen((option.state & QStyle::State_Selected ? option.palette.highlightedText() : option.palette.text()).color()); + QString text = index.data(Qt::DisplayRole).toString(); + QRect text_rect = option.rect.adjusted(rc.width() + 9, 0, 0, 0); + text = painter->fontMetrics().elidedText(text, Qt::ElideRight, text_rect.width()); + painter->drawText(text_rect, option.displayAlignment, text); + painter->restore(); + } else { + QStyledItemDelegate::paint(painter, option, index); + } +} + +QWidget *SignalItemDelegate::createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const { + auto item = (SignalModel::Item *)index.internalPointer(); + if (item->type == SignalModel::Item::Name || item->type == SignalModel::Item::Offset || item->type == SignalModel::Item::Factor) { + QLineEdit *e = new QLineEdit(parent); + e->setFrame(false); + e->setValidator(index.row() == 0 ? name_validator : double_validator); + return e; + } else if (item->type == SignalModel::Item::Size) { + QSpinBox *spin = new QSpinBox(parent); + spin->setFrame(false); + spin->setRange(1, 64); + return spin; + } + return QStyledItemDelegate::createEditor(parent, option, index); +} + +// SignalView + +SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts), QWidget(parent) { + // title bar + QWidget *title_bar = new QWidget(this); + title_bar->setAutoFillBackground(true); + QHBoxLayout *hl = new QHBoxLayout(title_bar); + hl->addWidget(signal_count_lb = new QLabel()); + filter_edit = new QLineEdit(this); + filter_edit->setClearButtonEnabled(true); + filter_edit->setPlaceholderText(tr("filter signals by name")); + hl->addWidget(filter_edit); + hl->addStretch(1); + auto collapse_btn = new QToolButton(); + collapse_btn->setIcon(utils::icon("dash-square")); + collapse_btn->setIconSize({12, 12}); + collapse_btn->setAutoRaise(true); + collapse_btn->setToolTip(tr("Collapse All")); + hl->addWidget(collapse_btn); + + // tree view + tree = new QTreeView(this); + tree->setModel(model = new SignalModel(this)); + tree->setItemDelegate(new SignalItemDelegate(this)); + tree->setFrameShape(QFrame::NoFrame); + tree->setHeaderHidden(true); + tree->setMouseTracking(true); + tree->setExpandsOnDoubleClick(false); + tree->header()->setSectionResizeMode(QHeaderView::Stretch); + tree->setMinimumHeight(300); + tree->setStyleSheet("QSpinBox{background-color:white;border:none;} QLineEdit{background-color:white;}"); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + main_layout->addWidget(title_bar); + main_layout->addWidget(tree); + + QObject::connect(filter_edit, &QLineEdit::textEdited, model, &SignalModel::setFilter); + QObject::connect(collapse_btn, &QPushButton::clicked, tree, &QTreeView::collapseAll); + QObject::connect(tree, &QAbstractItemView::clicked, this, &SignalView::rowClicked); + QObject::connect(tree, &QTreeView::viewportEntered, [this]() { emit highlight(nullptr); }); + QObject::connect(tree, &QTreeView::entered, [this](const QModelIndex &index) { emit highlight(model->getItem(index)->sig); }); + QObject::connect(model, &QAbstractItemModel::modelReset, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsInserted, this, &SignalView::rowsChanged); + QObject::connect(model, &QAbstractItemModel::rowsRemoved, this, &SignalView::rowsChanged); + QObject::connect(dbc(), &DBCManager::signalAdded, [this](uint32_t address, const Signal *sig) { expandSignal(sig); }); +} + +void SignalView::setMessage(const QString &id) { + msg_id = id; + filter_edit->clear(); + model->setMessage(id); +} + +void SignalView::rowsChanged() { + auto create_btn = [](const QString &id, const QString &tooltip) { + auto btn = new QToolButton(); + btn->setIcon(utils::icon(id)); + btn->setToolTip(tooltip); + btn->setAutoRaise(true); + return btn; + }; + + signal_count_lb->setText(tr("Signals: %1").arg(model->rowCount())); + + for (int i = 0; i < model->rowCount(); ++i) { + auto index = model->index(i, 1); + if (!tree->indexWidget(index)) { + QWidget *w = new QWidget(this); + QHBoxLayout *h = new QHBoxLayout(w); + h->setContentsMargins(0, 2, 0, 2); + h->addStretch(1); + + auto remove_btn = create_btn("x", tr("Remove signal")); + auto plot_btn = create_btn("graph-up", ""); + plot_btn->setCheckable(true); + h->addWidget(plot_btn); + h->addWidget(remove_btn); + + tree->setIndexWidget(index, w); + auto sig = model->getItem(index)->sig; + QObject::connect(remove_btn, &QToolButton::clicked, [=]() { model->removeSignal(sig); }); + QObject::connect(plot_btn, &QToolButton::clicked, [=](bool checked) { + emit showChart(msg_id, sig, checked, QGuiApplication::keyboardModifiers() & Qt::ShiftModifier); + }); + } + } + updateChartState(); +} + +void SignalView::rowClicked(const QModelIndex &index) { + auto item = model->getItem(index); + if (item->type == SignalModel::Item::Sig) { + auto sig_index = model->index(index.row(), 0, index.parent()); + tree->setExpanded(sig_index, !tree->isExpanded(sig_index)); + } else if (item->type == SignalModel::Item::ExtraInfo) { + model->showExtraInfo(index); + } +} + +void SignalView::expandSignal(const Signal *sig) { + if (int row = model->signalRow(sig); row != -1) { + auto idx = model->index(row, 0); + bool expand = !tree->isExpanded(idx); + tree->setExpanded(idx, expand); + tree->scrollTo(idx, QAbstractItemView::PositionAtTop); + if (expand) tree->setCurrentIndex(idx); + } +} + +void SignalView::updateChartState() { + int i = 0; + for (auto item : model->root->children) { + auto plot_btn = tree->indexWidget(model->index(i, 1))->findChildren()[0]; + bool chart_opened = charts->hasSignal(msg_id, item->sig); + plot_btn->setChecked(chart_opened); + plot_btn->setToolTip(chart_opened ? tr("Close Plot") : tr("Show Plot\nSHIFT click to add to previous opened plot")); + ++i; + } +} + +void SignalView::signalHovered(const Signal *sig) { + auto &children = model->root->children; + for (int i = 0; i < children.size(); ++i) { + bool highlight = children[i]->sig == sig; + if (std::exchange(children[i]->highlight, highlight) != highlight) { + emit model->dataChanged(model->index(i, 0), model->index(i, 0)); + } + } } diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h index 90df3fd3f4..ae88a5b13a 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signaledit.h @@ -1,59 +1,97 @@ #pragma once -#include +#include #include #include -#include -#include +#include +#include -#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/chartswidget.h" #include "tools/cabana/dbcmanager.h" #include "tools/cabana/streams/abstractstream.h" -class SignalForm : public QWidget { +class SignalModel : public QAbstractItemModel { Q_OBJECT public: - SignalForm(QWidget *parent); - void textBoxEditingFinished(); + struct Item { + enum Type {Root, Sig, Name, Size, Endian, Signed, Offset, Factor, ExtraInfo, Unit, Comment, Min, Max, Desc }; + ~Item() { qDeleteAll(children); } + inline int row() { return parent->children.indexOf(this); } - QLineEdit *name, *unit, *comment, *val_desc, *offset, *factor, *min_val, *max_val; - QSpinBox *size; - QComboBox *sign, *endianness; - QToolButton *expand_btn; + Type type = Type::Root; + Item *parent = nullptr; + QList children; -signals: - void changed(); + const Signal *sig = nullptr; + QString title; + bool highlight = false; + bool extra_expanded = false; + QString sig_val = "-"; + }; + + SignalModel(QObject *parent); + int rowCount(const QModelIndex &parent = QModelIndex()) const override; + int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 2; } + QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; + QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; + QModelIndex parent(const QModelIndex &index) const override; + Qt::ItemFlags flags(const QModelIndex &index) const override; + bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; + void setMessage(const QString &id); + void setFilter(const QString &txt); + void addSignal(int start_bit, int size, bool little_endian); + bool saveSignal(const Signal *origin_s, Signal &s); + void resizeSignal(const Signal *sig, int start_bit, int size); + void removeSignal(const Signal *sig); + inline Item *getItem(const QModelIndex &index) const { return index.isValid() ? (Item *)index.internalPointer() : root.get(); } + int signalRow(const Signal *sig) const; + void showExtraInfo(const QModelIndex &index); + +private: + void insertItem(SignalModel::Item *parent_item, int pos, const Signal *sig); + void handleSignalAdded(uint32_t address, const Signal *sig); + void handleSignalUpdated(const Signal *sig); + void handleSignalRemoved(const Signal *sig); + void handleMsgChanged(uint32_t address); + void refresh(); + void updateState(const QHash *msgs); + + QString msg_id; + QString filter_str; + std::unique_ptr root; + friend class SignalView; }; -class SignalEdit : public QWidget { +class SignalItemDelegate : public QStyledItemDelegate { +public: + SignalItemDelegate(QObject *parent); + void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QValidator *name_validator, *double_validator; +}; + +class SignalView : public QWidget { Q_OBJECT public: - SignalEdit(int index, QWidget *parent = nullptr); - void setSignal(const QString &msg_id, const Signal *sig); - void setChartOpened(bool opened); + SignalView(ChartsWidget *charts, QWidget *parent); + void setMessage(const QString &id); void signalHovered(const Signal *sig); - void updateForm(bool show); - const Signal *sig = nullptr; - SignalForm *form = nullptr; - QString msg_id; + void updateChartState(); + void expandSignal(const Signal *sig); + void rowClicked(const QModelIndex &index); + SignalModel *model = nullptr; signals: void highlight(const Signal *sig); void showChart(const QString &name, const Signal *sig, bool show, bool merge); - void remove(const Signal *sig); - void save(const Signal *sig, const Signal &new_sig); - void showFormClicked(const Signal *sig); -protected: - void enterEvent(QEvent *event) override; - void leaveEvent(QEvent *event) override; - void saveSignal(); +private: + void rowsChanged(); - ElidedLabel *title; - QLabel *color_label; - QLabel *icon; - int form_idx = 0; - QColor bg_color; - QToolButton *plot_btn; + QString msg_id; + QTreeView *tree; + QLineEdit *filter_edit; + ChartsWidget *charts; + QLabel *signal_count_lb; }; diff --git a/tools/cabana/streams/abstractstream.cc b/tools/cabana/streams/abstractstream.cc index b188c41826..8a77e0eb39 100644 --- a/tools/cabana/streams/abstractstream.cc +++ b/tools/cabana/streams/abstractstream.cc @@ -4,7 +4,9 @@ AbstractStream *can = nullptr; AbstractStream::AbstractStream(QObject *parent, bool is_live_streaming) : is_live_streaming(is_live_streaming), QObject(parent) { can = this; + new_msgs = std::make_unique>(); QObject::connect(this, &AbstractStream::received, this, &AbstractStream::process, Qt::QueuedConnection); + QObject::connect(this, &AbstractStream::seekedTo, this, &AbstractStream::updateLastMsgsTo); } void AbstractStream::process(QHash *messages) { @@ -18,31 +20,20 @@ void AbstractStream::process(QHash *messages) { } 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) { + double current_sec = event->mono_time / 1e9 - routeStartTime(); + for (const auto &c : event->event.getCan()) { QString id = QString("%1:%2").arg(c.getSrc()).arg(c.getAddress(), 1, 16); CanData &data = (*new_msgs)[id]; data.ts = current_sec; - data.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); + data.freq = data.count / std::max(1.0, current_sec); + change_trackers[id].compute(data.dat, data.ts, data.freq); + data.colors = change_trackers[id].colors; + data.last_change_t = change_trackers[id].last_change_t; } double ts = millis_since_boot(); @@ -58,3 +49,46 @@ bool AbstractStream::updateEvent(const Event *event) { } return true; } + +const CanData &AbstractStream::lastMessage(const QString &id) { + static CanData empty_data; + auto it = can_msgs.find(id); + return it != can_msgs.end() ? it.value() : empty_data; +} + +void AbstractStream::updateLastMsgsTo(double sec) { + QHash, CanData> last_msgs; // Much faster than QHash + last_msgs.reserve(can_msgs.size()); + double route_start_time = routeStartTime(); + uint64_t last_ts = (sec + route_start_time) * 1e9; + auto last = std::upper_bound(events()->rbegin(), events()->rend(), last_ts, [](uint64_t ts, auto &e) { return e->mono_time < ts; }); + for (auto it = last; it != events()->rend(); ++it) { + if ((*it)->which == cereal::Event::Which::CAN) { + for (const auto &c : (*it)->event.getCan()) { + auto &m = last_msgs[{c.getSrc(), c.getAddress()}]; + if (++m.count == 1) { + m.ts = ((*it)->mono_time / 1e9) - route_start_time; + m.dat = QByteArray((char *)c.getDat().begin(), c.getDat().size()); + m.colors = QVector(m.dat.size(), QColor(0, 0, 0, 0)); + m.last_change_t = QVector(m.dat.size(), m.ts); + } else { + m.freq = m.count / std::max(1.0, m.ts); + } + } + } + } + + // it is thread safe to update data here. + // updateEvent will not be called before replayStream::seekedTo return. + new_msgs->clear(); + change_trackers.clear(); + counters.clear(); + can_msgs.clear(); + for (auto it = last_msgs.cbegin(); it != last_msgs.cend(); ++it) { + QString msg_id = QString("%1:%2").arg(it.key().first).arg(it.key().second, 1, 16); + can_msgs[msg_id] = it.value(); + counters[msg_id] = it.value().count; + } + emit updated(); + emit msgsReceived(&can_msgs); +} diff --git a/tools/cabana/streams/abstractstream.h b/tools/cabana/streams/abstractstream.h index 9a0187da5e..0dbb3d96a6 100644 --- a/tools/cabana/streams/abstractstream.h +++ b/tools/cabana/streams/abstractstream.h @@ -11,12 +11,11 @@ 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; + QVector last_change_t; }; class AbstractStream : public QObject { @@ -33,7 +32,7 @@ public: 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 const CanData &lastMessage(const QString &id); virtual VisionStreamType visionStreamType() const { return VISION_STREAM_ROAD; } virtual const Route *route() const { return nullptr; } virtual const std::vector *events() const = 0; @@ -53,16 +52,18 @@ signals: void received(QHash *); public: - QMap can_msgs; + QHash can_msgs; protected: void process(QHash *); bool updateEvent(const Event *event); + void updateLastMsgsTo(double sec); bool is_live_streaming = false; - std::atomic counters_begin_sec = 0; std::atomic processing = false; QHash counters; + std::unique_ptr> new_msgs; + QHash change_trackers; }; // A global pointer referring to the unique AbstractStream object diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc index fd58f7a409..b768b94327 100644 --- a/tools/cabana/streams/replaystream.cc +++ b/tools/cabana/streams/replaystream.cc @@ -2,7 +2,7 @@ #include "tools/cabana/dbcmanager.h" -ReplayStream::ReplayStream(QObject *parent) : AbstractStream(parent, false) { +ReplayStream::ReplayStream(uint32_t replay_flags, QObject *parent) : replay_flags(replay_flags), AbstractStream(parent, false) { QObject::connect(&settings, &Settings::changed, [this]() { if (replay) replay->setSegmentCacheLimit(settings.max_cached_minutes); }); @@ -16,13 +16,13 @@ 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); +bool ReplayStream::loadRoute(const QString &route, const QString &data_dir) { + replay.reset(new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this)); replay->setSegmentCacheLimit(settings.max_cached_minutes); replay->installEventFilter(event_filter, this); - QObject::connect(replay, &Replay::seekedTo, this, &AbstractStream::seekedTo); - QObject::connect(replay, &Replay::segmentsMerged, this, &AbstractStream::eventsMerged); - QObject::connect(replay, &Replay::streamStarted, this, &AbstractStream::streamStarted); + QObject::connect(replay.get(), &Replay::seekedTo, this, &AbstractStream::seekedTo); + QObject::connect(replay.get(), &Replay::segmentsMerged, this, &AbstractStream::eventsMerged); + QObject::connect(replay.get(), &Replay::streamStarted, this, &AbstractStream::streamStarted); 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; })) { @@ -42,12 +42,6 @@ bool ReplayStream::eventFilter(const Event *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 index 1688915212..69fb738ab8 100644 --- a/tools/cabana/streams/replaystream.h +++ b/tools/cabana/streams/replaystream.h @@ -8,11 +8,11 @@ class ReplayStream : public AbstractStream { Q_OBJECT public: - ReplayStream(QObject *parent); + ReplayStream(uint32_t replay_flags, QObject *parent); ~ReplayStream(); - bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); + bool loadRoute(const QString &route, const QString &data_dir); bool eventFilter(const Event *event); - void seekTo(double ts) override; + void seekTo(double ts) override { replay->seekTo(std::max(double(0), ts), false); }; inline QString routeName() const override { return replay->route()->name(); } inline QString carFingerprint() const override { return replay->carFingerprint().c_str(); } inline VisionStreamType visionStreamType() const override { return replay->hasFlag(REPLAY_FLAG_ECAM) ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD; } @@ -28,5 +28,6 @@ public: inline const std::vector> getTimeline() override { return replay->getTimeline(); } private: - Replay *replay = nullptr; + std::unique_ptr replay = nullptr; + uint32_t replay_flags = REPLAY_FLAG_NONE; }; diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc index 0684d3c59a..f9d19dfda3 100644 --- a/tools/cabana/util.cc +++ b/tools/cabana/util.cc @@ -1,13 +1,16 @@ #include "tools/cabana/util.h" +#include #include #include +#include "selfdrive/ui/qt/util.h" + static QColor blend(QColor a, QColor b) { return QColor((a.red() + b.red()) / 2, (a.green() + b.green()) / 2, (a.blue() + b.blue()) / 2, (a.alpha() + b.alpha()) / 2); } -const QVector &HexColors::compute(const QByteArray &dat, double ts, uint32_t freq) { +void ChangeTracker::compute(const QByteArray &dat, double ts, uint32_t freq) { if (prev_dat.size() != dat.size()) { colors.resize(dat.size()); last_change_t.resize(dat.size()); @@ -42,16 +45,15 @@ const QVector &HexColors::compute(const QByteArray &dat, double ts, uint } prev_dat = dat; - return colors; } -void HexColors::clear() { +void ChangeTracker::clear() { prev_dat.clear(); last_change_t.clear(); colors.clear(); } -QList HexColors::toVariantList(const QVector &colors) { +QList ChangeTracker::toVariantList(const QVector &colors) { QList ret; ret.reserve(colors.size()); for (auto &c : colors) ret.append(c); @@ -65,11 +67,6 @@ MessageBytesDelegate::MessageBytesDelegate(QObject *parent) : QStyledItemDelegat } 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); @@ -85,8 +82,9 @@ void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem & pos.moveLeft(pos.x() + space.width()); int m = space.width() / 2; - const QMargins margins(m + 1, m, m, m); + const QMargins margins(m, m, m, m); + QList colors = index.data(Qt::UserRole).toList(); int i = 0; for (auto &byte : opt.text.split(" ")) { if (i < colors.size()) { @@ -97,3 +95,24 @@ void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem & i++; } } + +NameValidator::NameValidator(QObject *parent) : QRegExpValidator(QRegExp("^(\\w+)"), parent) { } + +QValidator::State NameValidator::validate(QString &input, int &pos) const { + input.replace(' ', '_'); + return QRegExpValidator::validate(input, pos); +} + +namespace utils { +QPixmap icon(const QString &id) { + static bool dark_theme = QApplication::style()->standardPalette().color(QPalette::WindowText).value() > + QApplication::style()->standardPalette().color(QPalette::Background).value(); + QPixmap pm = bootstrapPixmap(id); + if (dark_theme) { + QPainter p(&pm); + p.setCompositionMode(QPainter::CompositionMode_SourceIn); + p.fillRect(pm.rect(), Qt::lightGray); + } + return pm; +} +} // namespace utils diff --git a/tools/cabana/util.h b/tools/cabana/util.h index 146410d7c6..b76652f4b0 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -3,22 +3,24 @@ #include #include #include +#include #include #include -class HexColors { +class ChangeTracker { public: - const QVector &compute(const QByteArray &dat, double ts, uint32_t freq); - static QList toVariantList(const QVector &colors); - void clear(); + void compute(const QByteArray &dat, double ts, uint32_t freq); + static QList toVariantList(const QVector &colors); + void clear(); + + QVector last_change_t; + QVector colors; private: const int periodic_threshold = 10; const int start_alpha = 128; const float fade_time = 2.0; QByteArray prev_dat; - QVector last_change_t; - QVector colors; }; class MessageBytesDelegate : public QStyledItemDelegate { @@ -36,3 +38,15 @@ inline const QString &getColor(int i) { static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)]; } + +class NameValidator : public QRegExpValidator { + Q_OBJECT + +public: + NameValidator(QObject *parent=nullptr); + QValidator::State validate(QString &input, int &pos) const override; +}; + +namespace utils { +QPixmap icon(const QString &id); +} diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 445a21c755..d8d89a708c 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -13,8 +13,6 @@ #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"); } @@ -130,7 +128,7 @@ void VideoWidget::updateState() { } void VideoWidget::updatePlayBtnState() { - play_btn->setIcon(bootstrapPixmap(can->isPaused() ? "play" : "pause")); + play_btn->setIcon(utils::icon(can->isPaused() ? "play" : "pause")); play_btn->setToolTip(can->isPaused() ? tr("Play") : tr("Pause")); } diff --git a/tools/gpstest/test_laikad.py b/tools/gpstest/test_laikad.py new file mode 100644 index 0000000000..613ac991b1 --- /dev/null +++ b/tools/gpstest/test_laikad.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import os +import time +import unittest + +import cereal.messaging as messaging +import selfdrive.sensord.pigeond as pd + +from common.params import Params +from system.hardware import TICI +from selfdrive.manager.process_config import managed_processes +from selfdrive.test.helpers import with_processes + + +def wait_for_location(sm, timeout, con=10): + cons_meas = 0 + start_time = time.monotonic() + while (time.monotonic() - start_time) < timeout: + sm.update() + if not sm.updated["gnssMeasurements"]: + continue + + msg = sm["gnssMeasurements"] + cons_meas = (cons_meas + 1) if 'positionECEF' in msg.to_dict() else 0 + if cons_meas >= con: + return True + return False + + +class TestLaikad(unittest.TestCase): + @classmethod + def setUpClass(self): + if not TICI: + raise unittest.SkipTest + + ublox_available = Params().get_bool("UbloxAvailable") + if not ublox_available: + raise unittest.SkipTest + + def setUp(self): + # ensure laikad cold start + Params().remove("LaikadEphemeris") + os.environ["LAIKAD_NO_INTERNET"] = "1" + managed_processes['laikad'].start() + + def tearDown(self): + managed_processes['laikad'].stop() + + + @with_processes(['pigeond', 'ubloxd']) + def test_laikad_cold_start(self): + time.sleep(5) + + start_time = time.monotonic() + sm = messaging.SubMaster(["gnssMeasurements"]) + + success = wait_for_location(sm, 60*2, con=10) + duration = time.monotonic() - start_time + + assert success, "Waiting for location timed out (2min)!" + assert duration < 60, f"Received Location {duration}!" + + + @with_processes(['ubloxd']) + def test_laikad_ublox_reset_start(self): + time.sleep(2) + + pigeon, pm = pd.create_pigeon() + pd.init_baudrate(pigeon) + assert pigeon.reset_device(), "Could not reset device!" + + laikad_sock = messaging.sub_sock("gnssMeasurements", timeout=0.1) + ublox_gnss_sock = messaging.sub_sock("ubloxGnss", timeout=0.1) + + pd.init_baudrate(pigeon) + pd.initialize_pigeon(pigeon) + pd.run_receiving(pigeon, pm, 180) + + ublox_msgs = messaging.drain_sock(ublox_gnss_sock) + laikad_msgs = messaging.drain_sock(laikad_sock) + + gps_ephem_cnt = 0 + glonass_ephem_cnt = 0 + for um in ublox_msgs: + if um.ubloxGnss.which() == 'ephemeris': + gps_ephem_cnt += 1 + elif um.ubloxGnss.which() == 'glonassEphemeris': + glonass_ephem_cnt += 1 + + assert gps_ephem_cnt > 0, "NO gps ephemeris collected!" + assert glonass_ephem_cnt > 0, "NO glonass ephemeris collected!" + + pos_meas = 0 + duration = -1 + for lm in laikad_msgs: + pos_meas = (pos_meas + 1) if 'positionECEF' in lm.gnssMeasurements.to_dict() else 0 + if pos_meas > 5: + duration = (lm.logMonoTime - laikad_msgs[0].logMonoTime)*1e-9 + break + + assert pos_meas > 5, "NOT enough positions at end of read!" + assert duration < 120, "Laikad took too long to get a Position!" + +if __name__ == "__main__": + unittest.main() diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 998c93b938..178b116a87 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -25,6 +25,13 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s s.push_back(it.name); } } + + if (!allow_list.empty()) { + // the following events are needed for replay to work properly. + allow_list.insert(cereal::Event::Which::INIT_DATA); + allow_list.insert(cereal::Event::Which::CAR_PARAMS); + } + qDebug() << "services " << s; qDebug() << "loading route " << route; @@ -107,9 +114,9 @@ void Replay::seekTo(double seconds, bool relative) { rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; + emit seekedTo(seconds); return isSegmentMerged(seg); }); - emit seekedTo(seconds); queueSegment(); }