From 754b93997a9bd0d4e7d24fae08ff0de3d850391f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 22:37:46 -0700 Subject: [PATCH] Support RAV4 Hybrid 2022 with stock longitudinal (#23969) * add panda flag for toyota stock long with camera * clean up * Add 2022 RAV4 Hybrid from Philly * fix wrong fw in interface, did this ever work? * Must be a hybrid * no radar parsing * fix can error * move to own platform * generate docs * fix * Add 2022 Rav4 XSE Australia fingerprint parameters (#24303) * Update values.py Add 2022 Rav4 XSE Australia * add commas Co-authored-by: Shane Smiskol * bump panda * wait, the camera doesn't even send 0x343, right? * use a set instead, more obvious * don't test without a parser * bump panda * flip panda flag * bump panda * add commas * regen and update refs * set to none by default * revert parenthesis * update comment * bump panda * regen and update refs * add test models and update readme * bump to master Co-authored-by: BrettLynch123 <34538435+BrettLynch123@users.noreply.github.com> old-commit-hash: c1caca104f8553d485334e33446bd7b8ec1fb8ca --- RELEASES.md | 1 + docs/CARS.md | 1 + panda | 2 +- selfdrive/car/interfaces.py | 1 + selfdrive/car/tests/routes.py | 1 + selfdrive/car/tests/test_car_interfaces.py | 3 +- selfdrive/car/toyota/carstate.py | 23 ++++++++++---- selfdrive/car/toyota/interface.py | 11 ++++--- selfdrive/car/toyota/radar_interface.py | 5 +++- selfdrive/car/toyota/values.py | 30 +++++++++++++++++-- selfdrive/test/process_replay/ref_commit | 2 +- .../test/process_replay/test_processes.py | 2 +- 12 files changed, 65 insertions(+), 17 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index c8f2d73c6e..75088c19a4 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -18,6 +18,7 @@ Version 0.8.14 (2022-0X-XX) * Hyundai Tucson Diesel 2019 support thanks to sunnyhaibin! * Toyota Alphard Hybrid 2021 support * Toyota Avalon Hybrid 2022 support + * Toyota RAV4 Hybrid 2022 support Version 0.8.13 (2022-02-18) ======================== diff --git a/docs/CARS.md b/docs/CARS.md index 5bdd138748..4367286507 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -130,6 +130,7 @@ How We Rate The Cars |Toyota|Highlander 2017-19|All|[3](#footnotes)||||| |Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|RAV4 Hybrid 2022|All|||||| |Toyota|Sienna 2018-20|All|[3](#footnotes)||||| |Volkswagen|Arteon 2018, 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|||||| diff --git a/panda b/panda index cf8fb0b883..326cc2a8db 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit cf8fb0b8834f4e63eea27fc92289742d75d1d37d +Subproject commit 326cc2a8dbabbfe6c442f4b0192b18178a83b6a6 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ac15b2e1f8..23822fe454 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -187,6 +187,7 @@ class CarInterfaceBase(ABC): class RadarInterfaceBase(ABC): def __init__(self, CP): + self.rcp = None self.pts = {} self.delay = 0 self.radar_ts = CP.radarTimeStep diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index a26d6dcd04..88309e571d 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -128,6 +128,7 @@ routes = [ TestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H), TestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), TestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2), + TestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), TestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), TestRoute("25057fa6a5a63dfb|2020-03-04--08-44-23", TOYOTA.LEXUS_CTH), TestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ESH_TSS2), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index d97b16e682..92024ab0c2 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -64,7 +64,8 @@ class TestCarInterfaces(unittest.TestCase): # Run radar interface once radar_interface.update([]) - if not car_params.radarOffCan and hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): + if not car_params.radarOffCan 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]) if __name__ == "__main__": diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 614e1a6910..54922ac2de 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,7 +6,7 @@ from common.realtime import DT_CTRL from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE class CarState(CarStateBase): @@ -91,8 +91,12 @@ class CarState(CarStateBase): ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS - if self.CP.carFingerprint in TSS2_CAR: + if self.CP.carFingerprint in RADAR_ACC_CAR: + self.acc_type = cp.vl["ACC_CONTROL"]["ACC_TYPE"] + ret.stockFcw = bool(cp.vl["ACC_HUD"]["FCW"]) + elif self.CP.carFingerprint in TSS2_CAR: self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"] + ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars # these cars are identified by an ACC_TYPE value of 2. @@ -115,9 +119,6 @@ class CarState(CarStateBase): ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - if self.CP.carFingerprint in TSS2_CAR: - ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) - ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]["LKA_STATE"] @@ -207,6 +208,16 @@ class CarState(CarStateBase): ] checks.append(("BSM", 1)) + if CP.carFingerprint in RADAR_ACC_CAR: + signals += [ + ("ACC_TYPE", "ACC_CONTROL"), + ("FCW", "ACC_HUD"), + ] + checks += [ + ("ACC_CONTROL", 33), + ("ACC_HUD", 1), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod @@ -222,7 +233,7 @@ class CarState(CarStateBase): ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent ] - if CP.carFingerprint in TSS2_CAR: + if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b3d0aefa84..872a27c645 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,7 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune -from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -115,7 +115,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid set_lat_tune(ret.lateralTuning, LatTunes.PID_H) - elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2): + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): stop_and_go = True ret.wheelbase = 2.68986 ret.steerRatio = 14.3 @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid set_lat_tune(ret.lateralTuning, LatTunes.PID_D) - # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary. + # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 for fw in car_fw: if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']): @@ -221,7 +221,10 @@ class CarInterface(CarInterfaceBase): ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not smartDsu) ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") - ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR + ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + + if not ret.openpilotLongitudinalControl: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL # we can't use the fingerprint to detect this reliably, since # the EV gas pedal signal can take a couple seconds to appear diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 590840851d..8c87704ff2 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -6,6 +6,9 @@ from selfdrive.car.interfaces import RadarInterfaceBase def _create_radar_can_parser(car_fingerprint): + if DBC[car_fingerprint]['radar'] is None: + return None + if car_fingerprint in TSS2_CAR: RADAR_A_MSGS = list(range(0x180, 0x190)) RADAR_B_MSGS = list(range(0x190, 0x1a0)) @@ -48,7 +51,7 @@ class RadarInterface(RadarInterfaceBase): self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR def update(self, can_strings): - if self.no_radar: + if self.no_radar or self.rcp is None: return super().update(None) vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0e74be2454..68e8b36f30 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -63,6 +63,7 @@ class CAR: RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4_TSS2 = "TOYOTA RAV4 2019" RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" + RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022" MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 SIENNA = "TOYOTA SIENNA 2018" @@ -142,6 +143,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.RAV4H: ToyotaCarInfo("Toyota RAV4 Hybrid 2016-18", "TSS-P", footnotes=[Footnote.DSU]), CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), + CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022"), CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU]), @@ -1330,6 +1332,26 @@ FW_VERSIONS = { b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, + CAR.RAV4H_TSS2_2022: { + (Ecu.esp, 0x7b0, None): [ + b'\x01F15264283100\x00\x00\x00\x00', + b'\x01F15264286200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', + b'8965B42182\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896634A62000\x00\x00\x00\x00', + b'\x01896634A08000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F0R01100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', + ], + }, CAR.SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', @@ -1840,6 +1862,7 @@ DBC = { CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), @@ -1853,14 +1876,17 @@ DBC = { EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 -TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, +TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} +# these cars have a radar which sends ACC messages instead of the camera +RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022} + EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, - CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, + CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} # no resume button press required diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cb18cd0067..39c8add0e1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d3c925db251cda03d6661987ff6cec9df256a575 \ No newline at end of file +e37c2ea1447363af1403e8260450c30e2d862101 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 65d72f16f5..c59fe197a4 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -33,7 +33,7 @@ original_segments = [ segments = [ ("BODY", "bd6a637565e91581|2022-04-04--22-05-08--0"), ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), - ("TOYOTA", "fakedata|2022-04-13--18-53-16--0"), + ("TOYOTA", "fakedata|2022-04-28--18-59-34--0"), ("TOYOTA2", "fakedata|2022-04-28--15-52-38--0"), ("TOYOTA3", "fakedata|2022-04-13--19-09-53--0"), ("HONDA", "fakedata|2022-01-20--17-56-40--0"),