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 <shane@smiskol.com> * 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>
2026-02-18 17:43:54 +08:00 · 2022-04-28 22:37:46 -07:00
parent ef7ece408a
commit c1caca104f
12 changed files with 65 additions and 17 deletions
--- 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)
 ========================
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -130,6 +130,7 @@ How We Rate The Cars
 |Toyota|Highlander 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Toyota|Highlander Hybrid 2017-19|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Toyota|RAV4 Hybrid 2016-18|TSS-P|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
+|Toyota|RAV4 Hybrid 2022|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Toyota|Sienna 2018-20|All|<a href="#"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|Arteon 2018, 2021[<sup>8</sup>](#footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|Atlas 2018-19, 2022[<sup>8</sup>](#footnotes)|Driver Assistance|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
--- a/2
+++ b/2
--- 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
--- 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),
--- 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__":
--- 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"),
--- 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
--- 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)
--- 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
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-d3c925db251cda03d6661987ff6cec9df256a575
+e37c2ea1447363af1403e8260450c30e2d862101
--- 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"),