diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1779947ff9..0b021adfe4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,11 +4,12 @@ repos: - id: check-hooks-apply - id: check-useless-excludes - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.4.0 hooks: - id: check-ast exclude: '^(third_party)/' - id: check-json + - id: check-toml - id: check-xml - id: check-yaml - id: check-merge-conflict @@ -16,7 +17,7 @@ repos: - id: check-added-large-files args: ['--maxkb=100'] - repo: https://github.com/codespell-project/codespell - rev: v2.2.1 + rev: v2.2.4 hooks: - id: codespell exclude: '^(third_party/)|(body/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/ui/translations/.*.ts)|(poetry.lock)' @@ -33,7 +34,7 @@ repos: types: [python] exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(laika/)|(laika_repo/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)' - repo: https://github.com/PyCQA/flake8 - rev: 4.0.1 + rev: 6.0.0 hooks: - id: flake8 exclude: '^(third_party/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(selfdrive/debug/)/' @@ -79,6 +80,6 @@ repos: language: script pass_filenames: false - repo: https://github.com/python-poetry/poetry - rev: '1.2.2' + rev: '1.4.0' hooks: - id: poetry-check diff --git a/RELEASES.md b/RELEASES.md index a61e8b4ebd..4f5708cc1a 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -2,8 +2,9 @@ Version 0.9.2 (2023-03-XX) ======================== * New driving model, trained on a new dataset * Draw MPC path instead of model predicted path, this is a more accurate representation of what the car will do. -* Chevrolet Trailblazer 2021-22 support thanks to TurboCE! * Buick LaCrosse 2017-19 support thanks to koch-cf! +* Chevrolet Trailblazer 2021-22 support thanks to TurboCE! +* Honda HR-V 2023 support thanks to AlexandreSato and galegozi! * Kia Niro EV 2023 support thanks to JosselinLecocq! * Lexus ES 2017-18 support * Škoda Fabia 2022-23 support thanks to jyoung8607! diff --git a/common/i2c.cc b/common/i2c.cc index eb10cd64bb..ef788ac9ea 100644 --- a/common/i2c.cc +++ b/common/i2c.cc @@ -15,7 +15,7 @@ #define UNUSED(x) (void)(x) #ifdef QCOM2 -// TODO: decide if we want to isntall libi2c-dev everywhere +// TODO: decide if we want to install libi2c-dev everywhere extern "C" { #include #include diff --git a/docs/CARS.md b/docs/CARS.md index 56d909a64a..7ae8c06646 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. Supported vehicles reference the US market unless otherwise specified. -# 242 Supported Cars +# 244 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -55,6 +55,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| |Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| |Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| +|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| |Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| |Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| |Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| @@ -99,8 +100,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| |Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| |Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (with HDA II) 2022[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| +|Kia|EV6 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| +|Kia|EV6 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| |Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| |Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| @@ -149,7 +150,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B|| -|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| +|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| |Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| |Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| |Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram|| @@ -249,6 +250,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| |Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| diff --git a/opendbc b/opendbc index 3c81860270..933d784a0d 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 3c81860270e918d1a08f14a833522fd798aa39ca +Subproject commit 933d784a0d901657a278ee6e56a154e7e8214f1f diff --git a/panda b/panda index 00339119e7..77690944d1 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 00339119e7377d39910ae34a6b15880388060335 +Subproject commit 77690944d169e277885f1a2e192888c15e7dbf0d diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index f292b921cf..f0ab76e1dd 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -115,6 +115,14 @@ def main() -> NoReturn: cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") panda.reset() + # Ensure internal panda is present if expected + internal_pandas = [panda for panda in pandas if panda.is_internal()] + if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: + cloudlog.error("Internal panda is missing, resetting") + HARDWARE.reset_internal_panda() + time.sleep(2) # wait to come back up + continue + # sort pandas to have deterministic order pandas.sort(key=cmp_to_key(panda_sort_cmp)) panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) # type: ignore diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py index 790baa094f..2159eba5eb 100755 --- a/selfdrive/boardd/set_time.py +++ b/selfdrive/boardd/set_time.py @@ -24,7 +24,6 @@ def set_time(logger): # Set system time from panda RTC time panda_time = p.get_datetime() - logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") if panda_time > MIN_DATE: logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") os.system(f"TZ=UTC date -s '{panda_time}'") diff --git a/selfdrive/boardd/tests/test_pandad.py b/selfdrive/boardd/tests/test_pandad.py index 94da43edf7..09dba6ec7a 100755 --- a/selfdrive/boardd/tests/test_pandad.py +++ b/selfdrive/boardd/tests/test_pandad.py @@ -4,9 +4,11 @@ import unittest import cereal.messaging as messaging from panda import Panda +from common.gpio import gpio_set, gpio_init from selfdrive.test.helpers import phone_only from selfdrive.manager.process_config import managed_processes from system.hardware import HARDWARE +from system.hardware.tici.pins import GPIO class TestPandad(unittest.TestCase): @@ -40,6 +42,18 @@ class TestPandad(unittest.TestCase): managed_processes['pandad'].start() self._wait_for_boardd() + @phone_only + def test_internal_panda_reset(self): + gpio_init(GPIO.STM_RST_N, True) + gpio_set(GPIO.STM_RST_N, 1) + time.sleep(0.5) + assert all(not Panda(s).is_internal() for s in Panda.list()) + + managed_processes['pandad'].start() + self._wait_for_boardd() + + assert any(Panda(s).is_internal() for s in Panda.list()) + #def test_out_of_date_fw(self): # pass diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 110bf59d55..4bd93b266b 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -150,6 +150,7 @@ FW_VERSIONS = { b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NX6A-14C204-BLE\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.shiftByWire, 0x732, None): [ diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index eea78758a8..bcc239c2df 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -103,7 +103,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): else: checks.append(("CRUISE_PARAMS", 50)) - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK")) elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS")) @@ -120,7 +120,10 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) - if CP.openpilotLongitudinalControl: + if CP.carFingerprint in HONDA_BOSCH_RADARLESS: + signals.append(("CRUISE_FAULT", "CRUISE_FAULT_STATUS")) + checks.append(("CRUISE_FAULT_STATUS", 50)) + elif CP.openpilotLongitudinalControl: signals += [ ("BRAKE_ERROR_1", "STANDSTILL"), ("BRAKE_ERROR_2", "STANDSTILL") @@ -176,7 +179,7 @@ class CarState(CarStateBase): # panda checks if the signal is non-zero ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 # TODO: find a common signal across all cars - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) @@ -191,7 +194,9 @@ class CarState(CarStateBase): # NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2") - if self.CP.openpilotLongitudinalControl: + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + self.brake_error = cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"] + elif self.CP.openpilotLongitudinalControl: self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"] ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3a40f03dd9..31152a783a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -193,15 +193,18 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - elif candidate == CAR.HRV: + elif candidate in (CAR.HRV, CAR.HRV_3G): ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.61 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] tire_stiffness_factor = 0.5 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] - ret.wheelSpeedFactor = 1.025 + if candidate == CAR.HRV: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] + ret.wheelSpeedFactor = 1.025 + else: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning elif candidate == CAR.ACURA_RDX: ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index f6be1b1e3a..68e60746ac 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -87,6 +87,7 @@ class CAR: FIT = "HONDA FIT 2018" FREED = "HONDA FREED 2020" HRV = "HONDA HRV 2019" + HRV_3G = "HONDA HR-V 2023" ODYSSEY = "HONDA ODYSSEY 2018" ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" ACURA_RDX = "ACURA RDX 2018" @@ -138,6 +139,7 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS), CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS), CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS), + CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"), CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"), CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), @@ -1419,6 +1421,32 @@ FW_VERSIONS = { b'78109-THW-A110\x00\x00', ], }, + CAR.HRV_3G: { + (Ecu.eps, 0x18DA30F1, None): [ + b'39990-3W0-A030\x00\x00', + ], + (Ecu.gateway, 0x18DAEFF1, None): [ + b'38897-3W1-A010\x00\x00', + ], + (Ecu.srs, 0x18DA53F1, None): [ + b'77959-3V0-A820\x00\x00', + ], + (Ecu.combinationMeter, 0x18DA60F1, None): [ + b'78108-3V1-A220\x00\x00', + ], + (Ecu.vsa, 0x18DA28F1, None): [ + b'57114-3W0-A040\x00\x00', + ], + (Ecu.transmission, 0x18DA1EF1, None): [ + b'28101-6EH-A010\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18DA10F1, None): [ + b'37805-6CT-A710\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18DA2BF1, None): [ + b'46114-3W0-A020\x00\x00', + ], + }, CAR.ACURA_ILX: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TX6-A010\x00\x00', @@ -1537,6 +1565,7 @@ DBC = { CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), @@ -1556,6 +1585,6 @@ HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, CAR.PILOT, CAR.RIDGELINE} HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022} -HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} -HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022} + CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G} +HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G, CAR.HRV_3G} +HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G} diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 2572038492..ac74d2cc5b 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -5,6 +5,7 @@ from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.hyundai import hyundaicanfd, hyundaican +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -44,6 +45,7 @@ def process_hud_alert(enabled, fingerprint, hud_control): class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.CAN = CanBus(CP) self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_name) self.angle_limit_counter = 0 @@ -85,12 +87,12 @@ class CarController: # for longitudinal control, either radar or ADAS driving ECU addr, bus = 0x7d0, 0 if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, self.CAN.ECAN can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) # for blinkers if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5]) + can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) # >90 degree steering fault prevention # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault @@ -112,25 +114,25 @@ class CarController: hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer)) # disable LFA on HDA2 if self.frame % 5 == 0 and hda2: - can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4)) + can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, self.CAN, CS.cam_0x2a4)) # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, CC.leftBlinker, CC.rightBlinker)) + can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker)) if self.CP.openpilotLongitudinalControl: if hda2: - can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) + can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) self.accel_last = accel else: @@ -139,11 +141,11 @@ class CarController: # cruise cancel if CC.cruiseControl.cancel: if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, CS.cruise_info)) + can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CAN, CS.cruise_info)) self.last_button_frame = self.frame else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.CANCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL)) self.last_button_frame = self.frame # cruise standstill resume @@ -153,7 +155,7 @@ class CarController: pass else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.RES_ACCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame else: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active, diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 427f8cedb1..cec2c3e69e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase @@ -516,7 +516,7 @@ class CarState(CarStateBase): ("ACCELERATOR_BRAKE_ALT", 100), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_e_can_bus(CP)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).ECAN) @staticmethod def get_cam_can_parser_canfd(CP): @@ -543,4 +543,4 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).CAM) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index af7239571c..3717a45909 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,14 +1,44 @@ +import math + from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags -def get_e_can_bus(CP): - # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars - # have a different harness than the HDA1 and non-HDA variants in order to split - # a different bus, since the steering is done by different ECUs. - return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 + +class CanBus: + def __init__(self, CP, hda2=None, fingerprint=None): + if CP is None: + assert None not in (hda2, fingerprint) + num = math.ceil(max([k for k, v in fingerprint.items() if len(v)], default=1) / 4) + else: + hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value + num = len(CP.safetyConfigs) + + # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars + # have a different harness than the HDA1 and non-HDA variants in order to split + # a different bus, since the steering is done by different ECUs. + self._a, self._e = 1, 0 + if hda2: + self._a, self._e = 0, 1 + + offset = 4*(num - 1) + self._a += offset + self._e += offset + self._cam = 2 + offset + + @property + def ECAN(self): + return self._e + + @property + def ACAN(self): + return self._a + + @property + def CAM(self): + return self._cam -def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): +def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): ret = [] @@ -26,45 +56,45 @@ def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): if CP.flags & HyundaiFlags.CANFD_HDA2: if CP.openpilotLongitudinalControl: - ret.append(packer.make_can_msg("LFA", 5, values)) - ret.append(packer.make_can_msg("LKAS", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) + ret.append(packer.make_can_msg("LKAS", CAN.ACAN, values)) else: - ret.append(packer.make_can_msg("LFA", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) return ret -def create_cam_0x2a4(packer, camera_values): +def create_cam_0x2a4(packer, CAN, camera_values): camera_values.update({ "BYTE7": 0, }) - return packer.make_can_msg("CAM_0x2a4", 4, camera_values) + return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, camera_values) -def create_buttons(packer, CP, cnt, btn): +def create_buttons(packer, CP, CAN, cnt, btn): values = { "COUNTER": cnt, "SET_ME_1": 1, "CRUISE_BUTTONS": btn, } - bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 6 + bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM return packer.make_can_msg("CRUISE_BUTTONS", bus, values) -def create_acc_cancel(packer, CP, cruise_info_copy): +def create_acc_cancel(packer, CAN, cruise_info_copy): values = cruise_info_copy values.update({ "ACCMode": 4, }) - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CP, enabled): +def create_lfahda_cluster(packer, CAN, enabled): values = { "HDA_ICON": 1 if enabled else 0, "LFA_ICON": 2 if enabled else 0, } - return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) + return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -92,15 +122,15 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove "DISTANCE_SETTING": 4, } - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_spas_messages(packer, frame, left_blink, right_blink): +def create_spas_messages(packer, CAN, frame, left_blink, right_blink): ret = [] values = { } - ret.append(packer.make_can_msg("SPAS1", 5, values)) + ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values)) blink = 0 if left_blink: @@ -110,12 +140,12 @@ def create_spas_messages(packer, frame, left_blink, right_blink): values = { "BLINKER_CONTROL": blink, } - ret.append(packer.make_can_msg("SPAS2", 5, values)) + ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values)) return ret -def create_adrv_messages(packer, frame): +def create_adrv_messages(packer, CAN, frame): # messages needed to car happy after disabling # the ADAS Driving ECU to do longitudinal control @@ -123,7 +153,7 @@ def create_adrv_messages(packer, frame): values = { } - ret.append(packer.make_can_msg("ADRV_0x51", 4, values)) + ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values)) if frame % 2 == 0: values = { @@ -133,7 +163,7 @@ def create_adrv_messages(packer, frame): 'SET_ME_FC': 0xfc, 'SET_ME_9': 0x9, } - ret.append(packer.make_can_msg("ADRV_0x160", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) if frame % 5 == 0: values = { @@ -142,25 +172,25 @@ def create_adrv_messages(packer, frame): 'SET_ME_TMP_F': 0xf, 'SET_ME_TMP_F_2': 0xf, } - ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values)) values = { 'SET_ME_E1': 0xe1, 'SET_ME_3A': 0x3a, } - ret.append(packer.make_can_msg("ADRV_0x200", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values)) if frame % 20 == 0: values = { 'SET_ME_15': 0x15, } - ret.append(packer.make_can_msg("ADRV_0x345", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values)) if frame % 100 == 0: values = { 'SET_ME_22': 0x22, 'SET_ME_41': 0x41, } - ret.append(packer.make_can_msg("ADRV_0x1da", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values)) return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index bdb75c1fb9..93f3a75beb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config @@ -28,17 +28,20 @@ class CarInterface(CarInterfaceBase): # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } + hda2 = Ecu.adas in [fw.ecu for fw in car_fw] + CAN = CanBus(None, hda2, fingerprint) + if candidate in CANFD_CAR: # detect HDA2 with ADAS Driving ECU - if Ecu.adas in [fw.ecu for fw in car_fw]: + if hda2: ret.flags |= HyundaiFlags.CANFD_HDA2.value else: # non-HDA2 - if 0x1cf not in fingerprint[4]: + if 0x1cf not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value # ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead - if 0x130 not in fingerprint[4]: - if 0x40 not in fingerprint[4]: + if 0x130 not in fingerprint[CAN.ECAN]: + if 0x40 not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value else: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value @@ -248,21 +251,23 @@ class CarInterface(CarInterfaceBase): # *** feature detection *** if candidate in CANFD_CAR: - ret.enableBsm = 0x1e5 in fingerprint[get_e_can_bus(ret)] + ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN] else: ret.enableBsm = 0x58b in fingerprint[0] # *** panda safety config *** if candidate in CANFD_CAR: - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput), - get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)] + cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ] + if CAN.ECAN >= 4: + cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput)) + ret.safetyConfigs = cfgs if ret.flags & HyundaiFlags.CANFD_HDA2: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC else: if candidate in LEGACY_SAFETY_MODE_CAR: # these cars require a special panda safety mode due to missing counters and checksums in the messages @@ -297,12 +302,12 @@ class CarInterface(CarInterfaceBase): if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value): addr, bus = 0x7d0, 0 if CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, CanBus(CP).ECAN disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') # for blinkers if CP.flags & HyundaiFlags.ENABLE_BLINKERS: - disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + disable_ecu(logcan, sendcan, bus=CanBus(CP.ECAN), addr=0x7B1, com_cont_req=b'\x28\x83\x01') def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index a793774937..a4aad7def5 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -234,8 +234,8 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e), CAR.KIA_EV6: [ HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_p), - HyundaiCarInfo("Kia EV6 (without HDA II) 2022", "Highway Driving Assist", harness=Harness.hyundai_l), - HyundaiCarInfo("Kia EV6 (with HDA II) 2022", "Highway Driving Assist II", harness=Harness.hyundai_p) + HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_l), + HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_p) ], # Genesis diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 358cf0d0cd..d0f0aa93c9 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -39,7 +39,7 @@ class NissanCarInfo(CarInfo): CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), - CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22", video_link="https://youtu.be/vaMbtAh_0cY"), + CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"), CAR.LEAF_IC: None, # same platforms CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 32acacdd4f..3935461e61 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -69,6 +69,7 @@ routes = [ CarTestRoute("52f3e9ae60c0d886|2021-05-23--15-59-43", HONDA.FIT), CarTestRoute("2c4292a5cd10536c|2021-08-19--21-32-15", HONDA.FREED), CarTestRoute("03be5f2fd5c508d1|2020-04-19--18-44-15", HONDA.HRV), + CarTestRoute("320098ff6c5e4730|2023-04-13--17-47-46", HONDA.HRV_3G), CarTestRoute("917b074700869333|2021-05-24--20-40-20", HONDA.ACURA_ILX), CarTestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.ACCORD), # 1.5T CarTestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.ACCORD), # 2.0T @@ -127,6 +128,7 @@ routes = [ CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.VELOSTER), 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("9b25e8c1484a1b67|2023-04-13--10-41-45", HYUNDAI.KIA_EV6), 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), diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index e9f23145cd..c069280d11 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -1,19 +1,29 @@ #!/usr/bin/env python3 import random +import time import unittest from collections import defaultdict from parameterized import parameterized +import threading from cereal import car -from selfdrive.car.car_helpers import get_interface_attr, interfaces +from common.params import Params +from selfdrive.car.car_helpers import interfaces from selfdrive.car.fingerprints import FW_VERSIONS -from selfdrive.car.fw_versions import FW_QUERY_CONFIGS, match_fw_to_car +from selfdrive.car.fw_versions import FW_QUERY_CONFIGS, VERSIONS, match_fw_to_car, get_fw_versions CarFw = car.CarParams.CarFw Ecu = car.CarParams.Ecu ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} -VERSIONS = get_interface_attr("FW_VERSIONS", ignore_none=True) + + +class FakeSocket: + def receive(self, non_blocking=False): + pass + + def send(self, msg): + pass class TestFwFingerprint(unittest.TestCase): @@ -104,5 +114,69 @@ class TestFwFingerprint(unittest.TestCase): f'{brand.title()}: FW query whitelist missing ecus: {ecu_strings}') +class TestFwFingerprintTiming(unittest.TestCase): + @staticmethod + def _benchmark(brand, num_pandas, n): + params = Params() + fake_socket = FakeSocket() + + times = [] + for _ in range(n): + params.put_bool("ObdMultiplexingEnabled", True) + thread = threading.Thread(target=get_fw_versions, args=(fake_socket, fake_socket, brand), kwargs=dict(num_pandas=num_pandas)) + thread.start() + t = time.perf_counter() + while thread.is_alive(): + time.sleep(0.02) + if not params.get_bool("ObdMultiplexingChanged"): + params.put_bool("ObdMultiplexingChanged", True) + times.append(time.perf_counter() - t) + + return round(sum(times) / len(times), 2) + + def _assert_timing(self, avg_time, ref_time, tol): + self.assertLess(avg_time, ref_time + tol) + self.assertGreater(avg_time, ref_time - tol, "Performance seems to have improved, update test refs.") + + def test_fw_query_timing(self): + tol = 0.1 + total_ref_time = 4.6 + brand_ref_times = { + 1: { + 'body': 0.1, + 'chrysler': 0.3, + 'ford': 0.2, + 'honda': 0.5, + 'hyundai': 0.7, + 'mazda': 0.1, + 'nissan': 0.3, + 'subaru': 0.1, + 'tesla': 0.2, + 'toyota': 0.7, + 'volkswagen': 0.2, + }, + 2: { + 'hyundai': 1.1, + } + } + + total_time = 0 + for num_pandas in (1, 2): + for brand, config in FW_QUERY_CONFIGS.items(): + with self.subTest(brand=brand, num_pandas=num_pandas): + multi_panda_requests = [r for r in config.requests if r.bus > 3] + if not len(multi_panda_requests) and num_pandas > 1: + raise unittest.SkipTest("No multi-panda FW queries") + + avg_time = self._benchmark(brand, num_pandas, 10) + total_time += avg_time + self._assert_timing(avg_time, brand_ref_times[num_pandas][brand], tol) + print(f'{brand=}, {num_pandas=}, {len(config.requests)=}, avg FW query time={avg_time} seconds') + + with self.subTest(brand='all_brands'): + self._assert_timing(total_time, total_ref_time, tol) + print(f'all brands, total FW query time={total_time} seconds') + + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py index d3e4de1798..7ccd5e3c97 100755 --- a/selfdrive/car/tests/test_lateral_limits.py +++ b/selfdrive/car/tests/test_lateral_limits.py @@ -10,16 +10,24 @@ from common.realtime import DT_CTRL from selfdrive.car.car_helpers import interfaces from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.interfaces import get_torque_params +from selfdrive.car.subaru.values import CAR as SUBARU CAR_MODELS = all_known_cars() -# ISO 11270 -MAX_LAT_JERK = 2.5 # m/s^3 -MAX_LAT_JERK_TOLERANCE = 0.75 # m/s^3 -MAX_LAT_ACCEL = 3.0 # m/s^2 +# ISO 11270 - allowed up jerk is strictly lower than recommended limits +MAX_LAT_ACCEL = 3.0 # m/s^2 +MAX_LAT_JERK_UP = 2.5 # m/s^3 +MAX_LAT_JERK_DOWN = 5.0 # m/s^3 +MAX_LAT_JERK_UP_TOLERANCE = 0.5 # m/s^3 # jerk is measured over half a second -JERK_MEAS_FRAMES = 0.5 / DT_CTRL +JERK_MEAS_T = 0.5 + +# TODO: put these cars within limits +ABOVE_LIMITS_CARS = [ + SUBARU.LEGACY, + SUBARU.OUTBACK, +] car_model_jerks: DefaultDict[str, Dict[str, float]] = defaultdict(dict) @@ -43,6 +51,9 @@ class TestLateralLimits(unittest.TestCase): if CP.notCar: raise unittest.SkipTest + if CP.carFingerprint in ABOVE_LIMITS_CARS: + raise unittest.SkipTest + CarControllerParams = importlib.import_module(f'selfdrive.car.{CP.carName}.values').CarControllerParams cls.control_params = CarControllerParams(CP) cls.torque_params = get_torque_params(cls.car_model) @@ -50,20 +61,24 @@ class TestLateralLimits(unittest.TestCase): @staticmethod def calculate_0_5s_jerk(control_params, torque_params): steer_step = control_params.STEER_STEP - steer_up_per_frame = (control_params.STEER_DELTA_UP / control_params.STEER_MAX) / steer_step - steer_down_per_frame = (control_params.STEER_DELTA_DOWN / control_params.STEER_MAX) / steer_step + max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED'] - steer_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_FRAMES, 1.0) - steer_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_FRAMES, 1.0) + # Steer up/down delta per 10ms frame, in percentage of max torque + steer_up_per_frame = control_params.STEER_DELTA_UP / control_params.STEER_MAX / steer_step + steer_down_per_frame = control_params.STEER_DELTA_DOWN / control_params.STEER_MAX / steer_step - max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED'] - return steer_up_0_5_sec * max_lat_accel, steer_down_0_5_sec * max_lat_accel + # Lateral acceleration reached in 0.5 seconds, clipping to max torque + accel_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel + accel_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel + + # Convert to m/s^3 + return accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T def test_jerk_limits(self): up_jerk, down_jerk = self.calculate_0_5s_jerk(self.control_params, self.torque_params) car_model_jerks[self.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk} - self.assertLessEqual(up_jerk, MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE) - self.assertLessEqual(down_jerk, MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE) + self.assertLessEqual(up_jerk, MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE) + self.assertLessEqual(down_jerk, MAX_LAT_JERK_DOWN) def test_max_lateral_accel(self): self.assertLessEqual(self.torque_params["MAX_LAT_ACCEL_MEASURED"], MAX_LAT_ACCEL) @@ -76,7 +91,8 @@ if __name__ == "__main__": max_car_model_len = max([len(car_model) for car_model in car_model_jerks]) for car_model, _jerks in sorted(car_model_jerks.items(), key=lambda i: i[1]['up_jerk'], reverse=True): - violation = any([_jerk >= MAX_LAT_JERK + MAX_LAT_JERK_TOLERANCE for _jerk in _jerks.values()]) + violation = _jerks["up_jerk"] > MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE or \ + _jerks["down_jerk"] > MAX_LAT_JERK_DOWN violation_str = " - VIOLATION" if violation else "" print(f"{car_model:{max_car_model_len}} - up jerk: {round(_jerks['up_jerk'], 2):5} m/s^3, down jerk: {round(_jerks['down_jerk'], 2):5} m/s^3{violation_str}") diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 02b586d578..2aa049a89d 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -52,3 +52,4 @@ mock: [10.0, 10, 0.0] # Manually checked HONDA CIVIC 2022: [2.5, 1.2, 0.15] +HONDA HR-V 2023: [2.5, 1.2, 0.2] diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 4d8ca5afb7..e36cb63fa1 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -223,7 +223,10 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { ], CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), - CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2018-23"), + CAR.TIGUAN_MK2: [ + VWCarInfo("Volkswagen Tiguan 2018-23"), + VWCarInfo("Volkswagen Tiguan eHybrid 2021-23"), + ], CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ VWCarInfo("Volkswagen Caravelle 2020"), @@ -289,6 +292,7 @@ FW_QUERY_CONFIG = FwQueryConfig( FW_VERSIONS = { CAR.ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x873G0906259M \xf1\x890003', b'\xf1\x873G0906259F \xf1\x890004', b'\xf1\x873G0906259N \xf1\x890004', b'\xf1\x873G0906259P \xf1\x890001', @@ -296,18 +300,21 @@ FW_VERSIONS = { b'\xf1\x873G0906259G \xf1\x890004', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870DL300014C \xf1\x893704', b'\xf1\x8709G927158L \xf1\x893611', b'\xf1\x870GC300011L \xf1\x891401', b'\xf1\x870GC300014M \xf1\x892802', b'\xf1\x870GC300040P \xf1\x891401', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875QF959655AP\xf1\x890755\xf1\x82\x1311110011111311111100110200--1611125F49', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121157161111572900', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900', b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00', b'\xf1\x873Q0959655CK\xf1\x890711\xf1\x82\x0e1712141712141105121122052900', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002NB4202N7N', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020800', @@ -397,6 +404,7 @@ FW_VERSIONS = { b'\xf1\x8704L906021DT\xf1\x895520', b'\xf1\x8704L906021DT\xf1\x898127', b'\xf1\x8704L906021N \xf1\x895518', + b'\xf1\x8704L906021N \xf1\x898138', b'\xf1\x8704L906026BN\xf1\x891197', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026NF\xf1\x899528', @@ -443,6 +451,7 @@ FW_VERSIONS = { b'\xf1\x870CW300045 \xf1\x894531', b'\xf1\x870CW300047D \xf1\x895261', b'\xf1\x870CW300048J \xf1\x890611', + b'\xf1\x870CW300049H \xf1\x890905', b'\xf1\x870D9300012 \xf1\x894904', b'\xf1\x870D9300012 \xf1\x894913', b'\xf1\x870D9300012 \xf1\x894937', @@ -536,6 +545,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\x0101', b'\xf1\x875Q0907567J \xf1\x890396\xf1\x82\x0101', + b'\xf1\x875Q0907567L \xf1\x890098\xf1\x82\x0101', b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\x0101', b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\x0101', b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\x0101', @@ -749,6 +759,7 @@ FW_VERSIONS = { b'\xf1\x8783A907115F \xf1\x890002', b'\xf1\x8783A907115G \xf1\x890001', b'\xf1\x8783A907115K \xf1\x890001', + b'\xf1\x8704E906024AP\xf1\x891461', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158DT\xf1\x893698', @@ -764,6 +775,7 @@ FW_VERSIONS = { b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870DL300013G \xf1\x892120', b'\xf1\x870DL300014C \xf1\x893703', + b'\xf1\x870DD300046K \xf1\x892302', ], (Ecu.srs, 0x715, None): [ b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100', @@ -775,6 +787,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x1331310031333300314240024050409333613100', + b'\xf1\x875Q0959655CD\xf1\x890421\xf1\x82\x13123112313333003145406F6154619333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', @@ -787,6 +800,7 @@ FW_VERSIONS = { b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1', b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002SA6092SOM', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6017A00', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', @@ -816,6 +830,7 @@ FW_VERSIONS = { }, CAR.TRANSPORTER_T61: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906056AG\xf1\x899970', b'\xf1\x8704L906056AL\xf1\x899970', b'\xf1\x8704L906057AP\xf1\x891186', b'\xf1\x8704L906057N \xf1\x890413', @@ -823,12 +838,15 @@ FW_VERSIONS = { (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870BT300012G \xf1\x893102', b'\xf1\x870BT300012E \xf1\x893105', + b'\xf1\x870BT300046R \xf1\x893102', ], (Ecu.srs, 0x715, None): [ b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\x1316170411110411--04041704161611152S1411', + b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\x1316170411110411--04041704171711152S1411', b'\xf1\x872Q0959655AF\xf1\x890506\xf1\x82\x1316171111110411--04041711121211152S1413', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\x0532380518A2', b'\xf1\x877LA909144G \xf1\x897160\xf1\x82\x05333A5519A2', b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2', ], @@ -1197,6 +1215,7 @@ FW_VERSIONS = { b'\xf1\x8704L906026FP\xf1\x891196', b'\xf1\x8704L906026KB\xf1\x894071', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x8704L906026MT\xf1\x893076', b'\xf1\x873G0906259 \xf1\x890004', b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906264A \xf1\x890002', @@ -1205,6 +1224,7 @@ FW_VERSIONS = { b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870D9300011T \xf1\x894801', b'\xf1\x870D9300012 \xf1\x894940', + b'\xf1\x870D9300013A \xf1\x894905', b'\xf1\x870D9300041H \xf1\x894905', b'\xf1\x870GC300043 \xf1\x892301', b'\xf1\x870D9300043F \xf1\x895202', @@ -1227,6 +1247,7 @@ FW_VERSIONS = { b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572B \xf1\x890194', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x875Q0907572R \xf1\x890771', ], }, } diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index dc6688324f..0bb9aa5e54 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -8,13 +8,11 @@ import traceback from tqdm import tqdm from tools.lib.logreader import LogReader from tools.lib.route import Route -from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.car_helpers import interface_names -from selfdrive.car.fw_versions import match_fw_to_car +from selfdrive.car.fw_versions import VERSIONS, match_fw_to_car NO_API = "NO_API" in os.environ -VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) SUPPORTED_BRANDS = VERSIONS.keys() SUPPORTED_CARS = [brand for brand in SUPPORTED_BRANDS for brand in interface_names[brand]] UNKNOWN_BRAND = "unknown" diff --git a/system/hardware/base.py b/system/hardware/base.py index 31df1babe0..0b6ca44c3c 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -135,6 +135,9 @@ class HardwareBase(ABC): def get_networks(self): pass + def has_internal_panda(self) -> bool: + return False + def reset_internal_panda(self): pass diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index fdddf4d063..ee1d5f5279 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -467,10 +467,10 @@ class Tici(HardwareBase): os.system("sudo chmod a+w /dev/kmsg") # TODO: remove the if once agnos 7 ships - # Turn off fan, turned on by the ABL + # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL if os.path.exists('/sys/class/gpio/gpio49/'): gpio_init(GPIO.SOM_ST_IO, True) - gpio_set(GPIO.SOM_ST_IO, 0) + gpio_set(GPIO.SOM_ST_IO, 1) # *** IRQ config *** @@ -565,6 +565,9 @@ class Tici(HardwareBase): except Exception: return -1, -1 + def has_internal_panda(self): + return True + def reset_internal_panda(self): gpio_init(GPIO.STM_RST_N, True) diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 329b6070ae..10b394cec5 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -57,13 +57,13 @@ int main(int argc, char *argv[]) { route = DEMO_ROUTE; } - auto replay_stream = new ReplayStream(replay_flags, &app); + auto replay_stream = new ReplayStream(&app); stream.reset(replay_stream); if (route.isEmpty()) { if (OpenRouteDialog dlg(nullptr); !dlg.exec()) { return 0; } - } else if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"))) { + } else if (!replay_stream->loadRoute(route, cmd_parser.value("data_dir"), replay_flags)) { return 0; } } diff --git a/tools/cabana/chart/chart.cc b/tools/cabana/chart/chart.cc index ee1cdab6d2..4285c8a7f4 100644 --- a/tools/cabana/chart/chart.cc +++ b/tools/cabana/chart/chart.cc @@ -11,7 +11,9 @@ #include #include #include +#include #include +#include #include "tools/cabana/chart/chartswidget.h" @@ -40,6 +42,7 @@ ChartView::ChartView(const std::pair &x_range, ChartsWidget *par QObject::connect(axis_y, &QValueAxis::rangeChanged, [this]() { resetChartCache(); }); QObject::connect(axis_y, &QAbstractAxis::titleTextChanged, [this]() { resetChartCache(); }); + QObject::connect(window()->windowHandle(), &QWindow::screenChanged, [this]() { resetChartCache(); }); QObject::connect(dbc(), &DBCManager::signalRemoved, this, &ChartView::signalRemoved); QObject::connect(dbc(), &DBCManager::signalUpdated, this, &ChartView::signalUpdated); @@ -382,39 +385,42 @@ void ChartView::leaveEvent(QEvent *event) { QChartView::leaveEvent(event); } -QPixmap getBlankShadowPixmap(const QSize &size, int extent) { +QPixmap getBlankShadowPixmap(const QPixmap &px, int radius) { QGraphicsDropShadowEffect *e = new QGraphicsDropShadowEffect; e->setColor(QColor(40, 40, 40, 245)); - e->setOffset(0, 2); - e->setBlurRadius(10); + e->setOffset(0, 0); + e->setBlurRadius(radius); + + qreal dpr = px.devicePixelRatio(); + QPixmap blank(px.size()); + blank.setDevicePixelRatio(dpr); + blank.fill(Qt::white); QGraphicsScene scene; - QGraphicsPixmapItem item; - QPixmap src(size); - src.fill(Qt::white); - item.setPixmap(src); + QGraphicsPixmapItem item(blank); item.setGraphicsEffect(e); scene.addItem(&item); - QImage target(src.size() + QSize(extent * 2, extent * 2), QImage::Format_ARGB32); - target.fill(Qt::transparent); - QPainter p(&target); - scene.render(&p, QRectF(), QRectF(-extent, -extent, src.width() + extent * 2, src.height() + extent * 2)); - return QPixmap::fromImage(target); + + QPixmap shadow(px.size() + QSize(radius * dpr * 2, radius * dpr * 2)); + shadow.setDevicePixelRatio(dpr); + shadow.fill(Qt::transparent); + QPainter p(&shadow); + scene.render(&p, {QPoint(), shadow.size() / dpr}, item.boundingRect().adjusted(-radius, -radius, radius, radius)); + return shadow; } static QPixmap getDropPixmap(const QPixmap &src) { static QPixmap shadow_px; - const int extent = 10; - if (shadow_px.size() != src.size() + QSize(extent * 2, extent * 2)) { - shadow_px = getBlankShadowPixmap(src.size(), extent); + const int radius = 10; + if (shadow_px.size() != src.size() + QSize(radius * 2, radius * 2)) { + shadow_px = getBlankShadowPixmap(src, radius); } QPixmap px = shadow_px; QPainter p(&px); - int delta_w = px.width() - src.width(); - int delta_h = px.height() - src.height(); - p.drawPixmap(QPoint(delta_w / 2, delta_h / 2), src); + QRectF target_rect(QPointF(radius, radius), src.size() / src.devicePixelRatio()); + p.drawPixmap(target_rect.topLeft(), src); p.setCompositionMode(QPainter::CompositionMode_DestinationIn); - p.fillRect(delta_w / 2, delta_h / 2, src.width(), src.height(), QColor(0, 0, 0, 200)); + p.fillRect(target_rect, QColor(0, 0, 0, 200)); return px; } @@ -422,7 +428,7 @@ void ChartView::mousePressEvent(QMouseEvent *event) { if (event->button() == Qt::LeftButton && move_icon->sceneBoundingRect().contains(event->pos())) { QMimeData *mimeData = new QMimeData; mimeData->setData(CHART_MIME_TYPE, QByteArray::number((qulonglong)this)); - QPixmap px = grab().scaledToWidth(CHART_MIN_WIDTH, Qt::SmoothTransformation); + QPixmap px = grab().scaledToWidth(CHART_MIN_WIDTH * viewport()->devicePixelRatio(), Qt::SmoothTransformation); QDrag *drag = new QDrag(this); drag->setMimeData(mimeData); drag->setPixmap(getDropPixmap(px)); @@ -512,6 +518,13 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) { } void ChartView::showTip(double sec) { + QRect tip_area(0, chart()->plotArea().top(), rect().width(), chart()->plotArea().height()); + QRect visible_rect = charts_widget->chartVisibleRect(this).intersected(tip_area); + if (visible_rect.isEmpty()) { + tip_label.hide(); + return; + } + tooltip_x = chart()->mapToPosition({sec, 0}).x(); qreal x = tooltip_x; QStringList text_list(QString::number(chart()->mapToValue({x, 0}).x(), 'f', 3)); @@ -532,9 +545,9 @@ void ChartView::showTip(double sec) { .arg(s.series->color().name(), name, value, min, max); } } - QPointF tooltip_pt(x, chart()->plotArea().top()); - int plot_right = mapToGlobal(chart()->plotArea().topRight().toPoint()).x(); - tip_label.showText(mapToGlobal(tooltip_pt.toPoint()), "

" + text_list.join("
") + "

", plot_right); + QPoint pt(x, chart()->plotArea().top()); + QString text = "

" % text_list.join("
") % "

"; + tip_label.showText(pt, text, this, visible_rect); viewport()->update(); } @@ -607,7 +620,7 @@ void ChartView::paintEvent(QPaintEvent *event) { p.setRenderHints(QPainter::Antialiasing); drawBackground(&p, viewport()->rect()); scene()->setSceneRect(viewport()->rect()); - scene()->render(&p); + scene()->render(&p, viewport()->rect()); } QPainter painter(viewport()); diff --git a/tools/cabana/chart/chartswidget.cc b/tools/cabana/chart/chartswidget.cc index 2700f6519e..84bd3885cf 100644 --- a/tools/cabana/chart/chartswidget.cc +++ b/tools/cabana/chart/chartswidget.cc @@ -105,6 +105,7 @@ ChartsWidget::ChartsWidget(QWidget *parent) : align_timer(this), auto_scroll_tim QObject::connect(reset_zoom_btn, &QToolButton::clicked, this, &ChartsWidget::zoomReset); QObject::connect(&settings, &Settings::changed, this, &ChartsWidget::settingChanged); QObject::connect(new_tab_btn, &QToolButton::clicked, this, &ChartsWidget::newTab); + QObject::connect(this, &ChartsWidget::seriesChanged, this, &ChartsWidget::updateTabBar); QObject::connect(tabbar, &QTabBar::tabCloseRequested, this, &ChartsWidget::removeTab); QObject::connect(tabbar, &QTabBar::currentChanged, [this](int index) { if (index != -1) updateLayout(true); @@ -126,10 +127,8 @@ void ChartsWidget::newTab() { static int tab_unique_id = 0; int idx = tabbar->addTab(""); tabbar->setTabData(idx, tab_unique_id++); - for (int i = 0; i < tabbar->count(); ++i) { - tabbar->setTabText(i, QString("Tab %1").arg(i + 1)); - } tabbar->setCurrentIndex(idx); + updateTabBar(); } void ChartsWidget::removeTab(int index) { @@ -139,6 +138,14 @@ void ChartsWidget::removeTab(int index) { } tab_charts.erase(id); tabbar->removeTab(index); + updateTabBar(); +} + +void ChartsWidget::updateTabBar() { + for (int i = 0; i < tabbar->count(); ++i) { + const auto &charts_in_tab = tab_charts[tabbar->tabData(i).toInt()]; + tabbar->setTabText(i, QString("Tab %1 (%2)").arg(i + 1).arg(charts_in_tab.count())); + } } void ChartsWidget::eventsMerged() { @@ -161,14 +168,14 @@ void ChartsWidget::zoomReset() { zoom_undo_stack->clear(); } -void ChartsWidget::showValueTip(double sec) { +QRect ChartsWidget::chartVisibleRect(ChartView *chart) { const QRect visible_rect(-charts_container->pos(), charts_scroll->viewport()->size()); + return chart->rect().intersected(QRect(chart->mapFrom(charts_container, visible_rect.topLeft()), visible_rect.size())); +} + +void ChartsWidget::showValueTip(double sec) { for (auto c : currentCharts()) { - if (sec >= 0 && visible_rect.contains(QRect(c->mapTo(charts_container, QPoint(0, 0)), c->size()))) { - c->showTip(sec); - } else { - c->hideTip(); - } + sec >= 0 ? c->showTip(sec) : c->hideTip(); } } @@ -291,7 +298,7 @@ void ChartsWidget::updateLayout(bool force) { auto charts_layout = charts_container->charts_layout; int n = MAX_COLUMN_COUNT; for (; n > 1; --n) { - if ((n * CHART_MIN_WIDTH + (n - 1) * charts_layout->spacing()) < charts_layout->geometry().width()) break; + if ((n * CHART_MIN_WIDTH + (n - 1) * charts_layout->horizontalSpacing()) < charts_layout->geometry().width()) break; } bool show_column_cb = n > 1; @@ -381,18 +388,19 @@ void ChartsWidget::removeChart(ChartView *chart) { } void ChartsWidget::removeAll() { + while (tabbar->count() > 1) { + tabbar->removeTab(1); + } + tab_charts.clear(); + if (!charts.isEmpty()) { for (auto c : charts) { c->deleteLater(); } charts.clear(); - tab_charts.clear(); updateToolBar(); emit seriesChanged(); } - while (tabbar->count() > 1) { - tabbar->removeTab(1); - } } void ChartsWidget::alignCharts() { @@ -475,6 +483,7 @@ void ChartsContainer::dropEvent(QDropEvent *event) { int to = w ? charts_widget->currentCharts().indexOf(w) + 1 : 0; charts_widget->currentCharts().insert(to, chart); charts_widget->updateLayout(true); + charts_widget->updateTabBar(); event->acceptProposedAction(); chart->startAnimation(); } diff --git a/tools/cabana/chart/chartswidget.h b/tools/cabana/chart/chartswidget.h index 5fe227b156..25e5b8e194 100644 --- a/tools/cabana/chart/chartswidget.h +++ b/tools/cabana/chart/chartswidget.h @@ -59,6 +59,7 @@ private: ChartView *createChart(); void removeChart(ChartView *chart); void splitChart(ChartView *chart); + QRect chartVisibleRect(ChartView *chart); void eventsMerged(); void updateState(); void zoomReset(); @@ -66,6 +67,7 @@ private: void stopAutoScroll(); void doAutoScroll(); void updateToolBar(); + void updateTabBar(); void setMaxChartRange(int value); void updateLayout(bool force = false); void settingChanged(); diff --git a/tools/cabana/chart/tiplabel.cc b/tools/cabana/chart/tiplabel.cc index db686e44fa..3ee5f00978 100644 --- a/tools/cabana/chart/tiplabel.cc +++ b/tools/cabana/chart/tiplabel.cc @@ -22,18 +22,22 @@ TipLabel::TipLabel(QWidget *parent) : QLabel(parent, Qt::ToolTip | Qt::Frameless setVisible(false); } -void TipLabel::showText(const QPoint &pt, const QString &text, int right_edge) { +void TipLabel::showText(const QPoint &pt, const QString &text, QWidget *w, const QRect &rect) { setText(text); if (!text.isEmpty()) { QSize extra(1, 1); resize(sizeHint() + extra); - QPoint tip_pos(pt.x() + 12, pt.y()); - if (tip_pos.x() + size().width() >= right_edge) { + QPoint tip_pos(pt.x() + 12, rect.top() + 2); + if (tip_pos.x() + size().width() >= rect.right()) { tip_pos.rx() = pt.x() - size().width() - 12; } - move(tip_pos); + if (rect.contains({tip_pos, size()})) { + move(w->mapToGlobal(tip_pos)); + setVisible(true); + return; + } } - setVisible(!text.isEmpty()); + setVisible(false); } void TipLabel::paintEvent(QPaintEvent *ev) { diff --git a/tools/cabana/chart/tiplabel.h b/tools/cabana/chart/tiplabel.h index 568aa3d726..ac6e09e976 100644 --- a/tools/cabana/chart/tiplabel.h +++ b/tools/cabana/chart/tiplabel.h @@ -5,6 +5,6 @@ class TipLabel : public QLabel { public: TipLabel(QWidget *parent = nullptr); - void showText(const QPoint &pt, const QString &sec, int right_edge); + void showText(const QPoint &pt, const QString &sec, QWidget *w, const QRect &rect); void paintEvent(QPaintEvent *ev) override; }; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index 8dd0bac820..abc8c755ff 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,6 +1,7 @@ #include "tools/cabana/messageswidget.h" #include +#include #include #include @@ -9,29 +10,30 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { main_layout->setContentsMargins(0 ,0, 0, 0); // message filter - filter = new QLineEdit(this); + QHBoxLayout *title_layout = new QHBoxLayout(); + title_layout->addWidget(filter = new QLineEdit(this)); QRegularExpression re("\\S+"); filter->setValidator(new QRegularExpressionValidator(re, this)); filter->setClearButtonEnabled(true); filter->setPlaceholderText(tr("filter messages")); - main_layout->addWidget(filter); + title_layout->addWidget(multiple_lines_bytes = new QCheckBox(tr("Multiple Lines Bytes"), this)); + multiple_lines_bytes->setToolTip(tr("Display bytes in multiple lines")); + multiple_lines_bytes->setChecked(settings.multiple_lines_bytes); + main_layout->addLayout(title_layout); // message table - table_widget = new QTableView(this); + view = new MessageView(this); model = new MessageListModel(this); - table_widget->setModel(model); - table_widget->setItemDelegateForColumn(5, new MessageBytesDelegate(table_widget)); - table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); - table_widget->setSelectionMode(QAbstractItemView::SingleSelection); - table_widget->setSortingEnabled(true); - table_widget->sortByColumn(0, Qt::AscendingOrder); - table_widget->setColumnWidth(0, 150); - table_widget->setColumnWidth(1, 50); - table_widget->setColumnWidth(2, 50); - table_widget->setColumnWidth(3, 50); - table_widget->horizontalHeader()->setStretchLastSection(true); - table_widget->verticalHeader()->hide(); - main_layout->addWidget(table_widget); + auto delegate = new MessageBytesDelegate(view, settings.multiple_lines_bytes); + view->setItemDelegateForColumn(5, delegate); + view->setModel(model); + view->setSortingEnabled(true); + view->sortByColumn(0, Qt::AscendingOrder); + view->setItemsExpandable(false); + view->setIndentation(0); + view->setRootIsDecorated(false); + view->header()->setStretchLastSection(true); + main_layout->addWidget(view); // suppress QHBoxLayout *suppress_layout = new QHBoxLayout(); @@ -43,6 +45,11 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { // signals/slots QObject::connect(filter, &QLineEdit::textEdited, model, &MessageListModel::setFilterString); + QObject::connect(multiple_lines_bytes, &QCheckBox::stateChanged, [=](int state) { + settings.multiple_lines_bytes = (state == Qt::Checked); + delegate->setMultipleLines(settings.multiple_lines_bytes); + model->sortMessages(); + }); QObject::connect(can, &AbstractStream::msgsReceived, model, &MessageListModel::msgsReceived); QObject::connect(can, &AbstractStream::streamStarted, this, &MessagesWidget::reset); QObject::connect(dbc(), &DBCManager::DBCFileChanged, model, &MessageListModel::sortMessages); @@ -53,7 +60,7 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { selectMessage(*current_msg_id); } }); - QObject::connect(table_widget->selectionModel(), &QItemSelectionModel::currentChanged, [=](const QModelIndex ¤t, const QModelIndex &previous) { + QObject::connect(view->selectionModel(), &QItemSelectionModel::currentChanged, [=](const QModelIndex ¤t, const QModelIndex &previous) { if (current.isValid() && current.row() < model->msgs.size()) { auto &id = model->msgs[current.row()]; if (!current_msg_id || id != *current_msg_id) { @@ -85,7 +92,7 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { void MessagesWidget::selectMessage(const MessageId &msg_id) { if (int row = model->msgs.indexOf(msg_id); row != -1) { - table_widget->selectionModel()->setCurrentIndex(model->index(row, 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); + view->selectionModel()->setCurrentIndex(model->index(row, 0), QItemSelectionModel::Rows | QItemSelectionModel::ClearAndSelect); } } @@ -101,7 +108,7 @@ void MessagesWidget::updateSuppressedButtons() { void MessagesWidget::reset() { current_msg_id = std::nullopt; - table_widget->selectionModel()->clear(); + view->selectionModel()->clear(); model->reset(); filter->clear(); updateSuppressedButtons(); @@ -111,8 +118,10 @@ void MessagesWidget::reset() { // MessageListModel QVariant MessageListModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (orientation == Qt::Horizontal && role == Qt::DisplayRole) - return (QString[]){"Name", "Bus", "ID", "Freq", "Count", "Bytes"}[section]; + if (orientation == Qt::Horizontal && role == Qt::DisplayRole) { + static const QString titles[] = {"Name", "Bus", "ID", "Freq", "Count", "Bytes"}; + return titles[section]; + } return {}; } @@ -254,3 +263,22 @@ void MessageListModel::reset() { clearSuppress(); endResetModel(); } + +// MessageView + +void MessageView::drawRow(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { + QTreeView::drawRow(painter, option, index); + painter->save(); + const int gridHint = style()->styleHint(QStyle::SH_Table_GridLineColor, &option, this); + const QColor gridColor = QColor::fromRgba(static_cast(gridHint)); + painter->setPen(gridColor); + painter->drawLine(option.rect.left(), option.rect.bottom(), option.rect.right(), option.rect.bottom()); + + auto y = option.rect.y(); + painter->translate(visualRect(model()->index(0, 0)).x() - indentation() - .5, -.5); + for (int i = 0; i < header()->count(); ++i) { + painter->translate(header()->sectionSize(i), 0); + painter->drawLine(0, y, 0, y + option.rect.height()); + } + painter->restore(); +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index d4ef896519..cc48b70bf4 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -1,10 +1,11 @@ #pragma once #include +#include #include #include #include -#include +#include #include "tools/cabana/dbc/dbcmanager.h" #include "tools/cabana/streams/abstractstream.h" @@ -34,14 +35,21 @@ private: Qt::SortOrder sort_order = Qt::AscendingOrder; }; +class MessageView : public QTreeView { + Q_OBJECT +public: + MessageView(QWidget *parent) : QTreeView(parent) {} + void drawRow(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; +}; + class MessagesWidget : public QWidget { Q_OBJECT public: MessagesWidget(QWidget *parent); void selectMessage(const MessageId &message_id); - QByteArray saveHeaderState() const { return table_widget->horizontalHeader()->saveState(); } - bool restoreHeaderState(const QByteArray &state) const { return table_widget->horizontalHeader()->restoreState(state); } + QByteArray saveHeaderState() const { return view->header()->saveState(); } + bool restoreHeaderState(const QByteArray &state) const { return view->header()->restoreState(state); } void updateSuppressedButtons(); void reset(); @@ -49,11 +57,11 @@ signals: void msgSelectionChanged(const MessageId &message_id); protected: - QTableView *table_widget; + MessageView *view; std::optional current_msg_id; QLineEdit *filter; + QCheckBox *multiple_lines_bytes; MessageListModel *model; QPushButton *suppress_add; QPushButton *suppress_clear; - }; diff --git a/tools/cabana/route.cc b/tools/cabana/route.cc index ab322cdf90..f71c8dc67b 100644 --- a/tools/cabana/route.cc +++ b/tools/cabana/route.cc @@ -1,8 +1,7 @@ #include "tools/cabana/route.h" -#include #include -#include +#include #include #include #include @@ -11,21 +10,28 @@ 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)); + QGridLayout *grid_layout = new QGridLayout(); + grid_layout->addWidget(new QLabel(tr("Route")), 0, 0); + grid_layout->addWidget(route_edit = new QLineEdit(this), 0, 1); 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); + grid_layout->addWidget(file_btn, 0, 2); + + grid_layout->addWidget(new QLabel(tr("Video")), 1, 0); + grid_layout->addWidget(choose_video_cb = new QComboBox(this), 1, 1); + QString items[] = {tr("No Video"), tr("Road Camera"), tr("Wide Road Camera"), tr("Driver Camera"), tr("QCamera")}; + for (int i = 0; i < std::size(items); ++i) { + choose_video_cb->addItem(items[i]); + } + choose_video_cb->setCurrentIndex(1); // default is road camera; 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->addLayout(grid_layout); main_layout->addWidget(btn_box); + main_layout->addStretch(0); setMinimumSize({550, 120}); QObject::connect(btn_box, &QDialogButtonBox::accepted, this, &OpenRouteDialog::loadRoute); @@ -56,7 +62,8 @@ void OpenRouteDialog::loadRoute() { 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); + uint32_t flags[] = {REPLAY_FLAG_NO_VIPC, REPLAY_FLAG_NONE, REPLAY_FLAG_ECAM, REPLAY_FLAG_DCAM, REPLAY_FLAG_QCAMERA}; + failed_to_load = !dynamic_cast(can)->loadRoute(route, data_dir, flags[choose_video_cb->currentIndex()]); if (failed_to_load) { QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to load route: '%1'").arg(route)); } else { diff --git a/tools/cabana/route.h b/tools/cabana/route.h index ceda71d585..d13897d756 100644 --- a/tools/cabana/route.h +++ b/tools/cabana/route.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -14,6 +15,7 @@ public: private: QLineEdit *route_edit; + QComboBox *choose_video_cb; QDialogButtonBox *btn_box; bool failed_to_load = false; }; diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 3a1fc14751..4d8c4fcc5a 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -3,8 +3,11 @@ #include #include #include +#include #include +#include #include +#include #include "tools/cabana/util.h" @@ -31,6 +34,9 @@ void Settings::save() { s.setValue("chart_series_type", chart_series_type); s.setValue("theme", theme); s.setValue("sparkline_range", sparkline_range); + s.setValue("multiple_lines_bytes", multiple_lines_bytes); + s.setValue("log_livestream", log_livestream); + s.setValue("log_path", log_path); } void Settings::load() { @@ -50,13 +56,21 @@ void Settings::load() { chart_series_type = s.value("chart_series_type", 0).toInt(); theme = s.value("theme", 0).toInt(); sparkline_range = s.value("sparkline_range", 15).toInt(); + multiple_lines_bytes = s.value("multiple_lines_bytes", true).toBool(); + log_livestream = s.value("log_livestream", true).toBool(); + log_path = s.value("log_path").toString(); + if (log_path.isEmpty()) { + log_path = QStandardPaths::writableLocation(QStandardPaths::HomeLocation) + "/cabana_live_stream/"; + } } // SettingsDlg SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { setWindowTitle(tr("Settings")); - QFormLayout *form_layout = new QFormLayout(this); + QVBoxLayout *main_layout = new QVBoxLayout(this); + QGroupBox *groupbox = new QGroupBox("General"); + QFormLayout *form_layout = new QFormLayout(groupbox); theme = new QComboBox(this); theme->setToolTip(tr("You may need to restart cabana after changes theme")); @@ -75,7 +89,10 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { cached_minutes->setSingleStep(1); cached_minutes->setValue(settings.max_cached_minutes); form_layout->addRow(tr("Max Cached Minutes"), cached_minutes); + main_layout->addWidget(groupbox); + groupbox = new QGroupBox("Chart"); + form_layout = new QFormLayout(groupbox); chart_series_type = new QComboBox(this); chart_series_type->addItems({tr("Line"), tr("Step Line"), tr("Scatter")}); chart_series_type->setCurrentIndex(settings.chart_series_type); @@ -86,12 +103,31 @@ SettingsDlg::SettingsDlg(QWidget *parent) : QDialog(parent) { chart_height->setSingleStep(10); chart_height->setValue(settings.chart_height); form_layout->addRow(tr("Chart Height"), chart_height); + main_layout->addWidget(groupbox); - auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel | QDialogButtonBox::Apply); - form_layout->addRow(buttonBox); + log_livestream = new QGroupBox(tr("Enable live stream logging"), this); + log_livestream->setCheckable(true); + QHBoxLayout *path_layout = new QHBoxLayout(log_livestream); + path_layout->addWidget(log_path = new QLineEdit(settings.log_path, this)); + log_path->setReadOnly(true); + auto browse_btn = new QPushButton(tr("B&rowse...")); + path_layout->addWidget(browse_btn); + main_layout->addWidget(log_livestream); - setFixedWidth(360); - connect(buttonBox, &QDialogButtonBox::clicked, [=](QAbstractButton *button) { + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel | QDialogButtonBox::Apply); + main_layout->addWidget(buttonBox); + main_layout->addStretch(1); + + QObject::connect(browse_btn, &QPushButton::clicked, [this]() { + QString fn = QFileDialog::getExistingDirectory( + this, tr("Log File Location"), + QStandardPaths::writableLocation(QStandardPaths::HomeLocation), + QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks); + if (!fn.isEmpty()) { + log_path->setText(fn); + } + }); + QObject::connect(buttonBox, &QDialogButtonBox::clicked, [=](QAbstractButton *button) { auto role = buttonBox->buttonRole(button); if (role == QDialogButtonBox::AcceptRole) { save(); @@ -113,6 +149,8 @@ void SettingsDlg::save() { settings.max_cached_minutes = cached_minutes->value(); settings.chart_series_type = chart_series_type->currentIndex(); settings.chart_height = chart_height->value(); + settings.log_livestream = log_livestream->isChecked(); + settings.log_path = log_path->text(); settings.save(); emit settings.changed(); } diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index dd01b602e7..07aafe1682 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -1,8 +1,11 @@ #pragma once #include +#include #include #include +#include +#include #include #define LIGHT_THEME 1 @@ -24,6 +27,9 @@ public: int chart_series_type = 0; int theme = 0; int sparkline_range = 15; // 15 seconds + bool multiple_lines_bytes = true; + bool log_livestream = true; + QString log_path; QString last_dir; QString last_route_dir; QByteArray geometry; @@ -47,6 +53,8 @@ public: QSpinBox *chart_height; QComboBox *chart_series_type; QComboBox *theme; + QGroupBox *log_livestream; + QLineEdit *log_path; }; extern Settings settings; diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc index 62ad635152..0fcee4455a 100644 --- a/tools/cabana/streams/livestream.cc +++ b/tools/cabana/streams/livestream.cc @@ -19,6 +19,13 @@ void LiveStream::streamThread() { if (!zmq_address.isEmpty()) { setenv("ZMQ", "1", 1); } + + std::unique_ptr fs; + if (settings.log_livestream) { + std::string path = (settings.log_path + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd--hh-mm-ss") + "--0").toStdString(); + util::create_directories(path, 0755); + fs.reset(new std::ofstream(path + "/rlog" , std::ios::binary | std::ios::out)); + } std::unique_ptr context(Context::create()); std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString(); std::unique_ptr sock(SubSocket::create(context.get(), "can", address)); @@ -31,9 +38,12 @@ void LiveStream::streamThread() { QThread::msleep(50); continue; } + + if (fs) { + fs->write(msg->getData(), msg->getSize()); + } std::lock_guard lk(lock); handleEvent(messages.emplace_back(msg).event); - // TODO: write stream to log file to replay it with cabana --data_dir flag. } } diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc index 8bc9af8ab2..409ba25841 100644 --- a/tools/cabana/streams/replaystream.cc +++ b/tools/cabana/streams/replaystream.cc @@ -1,6 +1,6 @@ #include "tools/cabana/streams/replaystream.h" -ReplayStream::ReplayStream(uint32_t replay_flags, QObject *parent) : replay_flags(replay_flags), AbstractStream(parent, false) { +ReplayStream::ReplayStream(QObject *parent) : AbstractStream(parent, false) { QObject::connect(&settings, &Settings::changed, [this]() { if (replay) replay->setSegmentCacheLimit(settings.max_cached_minutes); }); @@ -25,7 +25,7 @@ void ReplayStream::mergeSegments() { } } -bool ReplayStream::loadRoute(const QString &route, const QString &data_dir) { +bool ReplayStream::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { 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); diff --git a/tools/cabana/streams/replaystream.h b/tools/cabana/streams/replaystream.h index 10dc804716..666f64437e 100644 --- a/tools/cabana/streams/replaystream.h +++ b/tools/cabana/streams/replaystream.h @@ -7,9 +7,9 @@ class ReplayStream : public AbstractStream { Q_OBJECT public: - ReplayStream(uint32_t replay_flags, QObject *parent); + ReplayStream(QObject *parent); ~ReplayStream(); - bool loadRoute(const QString &route, const QString &data_dir); + bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE); bool eventFilter(const Event *event); void seekTo(double ts) override { replay->seekTo(std::max(double(0), ts), false); }; inline QString routeName() const override { return replay->route()->name(); } @@ -29,6 +29,5 @@ public: private: void mergeSegments(); std::unique_ptr replay = nullptr; - uint32_t replay_flags = REPLAY_FLAG_NONE; std::set processed_segments; }; diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc index 58c1deb349..584938d0d9 100644 --- a/tools/cabana/util.cc +++ b/tools/cabana/util.cc @@ -41,9 +41,35 @@ std::pair SegmentTree::get_minmax(int n, int left, int right, in // MessageBytesDelegate -MessageBytesDelegate::MessageBytesDelegate(QObject *parent) : QStyledItemDelegate(parent) { +MessageBytesDelegate::MessageBytesDelegate(QObject *parent, bool multiple_lines) : multiple_lines(multiple_lines), QStyledItemDelegate(parent) { fixed_font = QFontDatabase::systemFont(QFontDatabase::FixedFont); - byte_width = QFontMetrics(fixed_font).width("00 "); + byte_size = QFontMetrics(fixed_font).size(Qt::TextSingleLine, "00 ") + QSize(0, 2); +} + +void MessageBytesDelegate::setMultipleLines(bool v) { + if (std::exchange(multiple_lines, v) != multiple_lines) { + std::fill_n(size_cache, std::size(size_cache), QSize{}); + } +} + +QSize MessageBytesDelegate::sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const { + int n = index.data(BytesRole).toByteArray().size(); + if (n <= 0 || n > 64) return {}; + + QSize size = size_cache[n - 1]; + if (size.isEmpty()) { + int h_margin = QApplication::style()->pixelMetric(QStyle::PM_FocusFrameHMargin) + 1; + int v_margin = QApplication::style()->pixelMetric(QStyle::PM_FocusFrameVMargin) + 1; + if (!multiple_lines) { + size.setWidth(h_margin * 2 + n * byte_size.width()); + size.setHeight(byte_size.height() + 2 * v_margin); + } else { + size.setWidth(h_margin * 2 + 8 * byte_size.width()); + size.setHeight(byte_size.height() * std::max(1, n / 8) + 2 * v_margin); + } + size_cache[n - 1] = size; + } + return size; } void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { @@ -52,18 +78,24 @@ void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem & int v_margin = option.widget->style()->pixelMetric(QStyle::PM_FocusFrameVMargin); int h_margin = option.widget->style()->pixelMetric(QStyle::PM_FocusFrameHMargin); - QRect rc{option.rect.left() + h_margin, option.rect.top() + v_margin, byte_width, option.rect.height() - 2 * v_margin}; + painter->save(); + if (option.state & QStyle::State_Selected) { + painter->fillRect(option.rect, option.palette.highlight()); + painter->setPen(option.palette.color(QPalette::HighlightedText)); + } - auto color_role = option.state & QStyle::State_Selected ? QPalette::HighlightedText : QPalette::Text; - painter->setPen(option.palette.color(color_role)); + const QPoint pt{option.rect.left() + h_margin, option.rect.top() + v_margin}; painter->setFont(fixed_font); for (int i = 0; i < byte_list.size(); ++i) { + int row = !multiple_lines ? 0 : i / 8; + int column = !multiple_lines ? i : i % 8; + QRect r = QRect({pt.x() + column * byte_size.width(), pt.y() + row * byte_size.height()}, byte_size); if (i < colors.size() && colors[i].alpha() > 0) { - painter->fillRect(rc, colors[i]); + painter->fillRect(r, colors[i]); } - painter->drawText(rc, Qt::AlignCenter, toHex(byte_list[i])); - rc.moveLeft(rc.right() + 1); + painter->drawText(r, Qt::AlignCenter, toHex(byte_list[i])); } + painter->restore(); } QColor getColor(const cabana::Signal *sig) { diff --git a/tools/cabana/util.h b/tools/cabana/util.h index 3b3a10ea17..f9eaf7bc2b 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -63,10 +63,16 @@ private: class MessageBytesDelegate : public QStyledItemDelegate { Q_OBJECT public: - MessageBytesDelegate(QObject *parent); + MessageBytesDelegate(QObject *parent, bool multiple_lines = false); void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; + QSize sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const override; + void setMultipleLines(bool v); + +private: QFont fixed_font; - int byte_width; + QSize byte_size = {}; + bool multiple_lines = false; + mutable QSize size_cache[64] = {}; }; inline QString toHex(const QByteArray &dat) { return dat.toHex(' ').toUpper(); }