Toyota: add 2023 RAV4/RAV4 Hybrid as dashcam (#27609)

* test out lta message

* correctly send

* percentage is percentage driver isn't overriding

* closer to stock system

* should be able to send LTA at 100hz, counter is the same as LKA

* small amount of torque towards desired

* Offset commanded torque correctly

* Too little torque

* use car's SETME's

* Revert "use car's SETME's"

This reverts commit c888569699.

* try cutting steer every second

* Didn't seem to matter

This reverts commit 6923498b2a.

* steer to 0

* let packer set counter

* add for camry

* log steer faults

* comments

* bump opendbc

* add opParams

* add lta safety

* set safety param

* fix torque control bug

* bump panda

* fix missing signal

* [experiment] apply some rate limiting and anti-windup

* no faults, clip to 90 deg, decent torque blending

* clean up blending

* toyota angle rate limits

* use std angle limits

* assert we only add angle control to TSS2 cars

* clean up carcontroller a bit

* space space

* bump opendbc

* clean up toyotacan from opendbc change

* bump panda

* will tests run?

* steer at zero

* refactor angle to use LatControlPID with zero gains

* stop some faults and tuning

* possibly fix integral wind up at max torque

* Add 2023 rav4

* limit torque inside EPS when overriding (no huge windup in edge cases when overriding)

* fix wind up issue after turns (or prolonged saturation)

* this doesn't work that well

* try these limits

* try this

* log the angle!

* global variable

* Apply suggestions from code review

* clip angle to 3 m/s/s

* some tolerance for roll

* raise limits a bit

* bumppanda

* fix faults

* still not good

* offsetting causing hugging?

* Revert "offsetting causing hugging?"

This reverts commit a42ec0b772.

* reduce kp

* class var not needed

* limit up angle delta

* fix saturation check

* feedforward includes offset

* some threshold for roll

* bump panda

* surely we don't need this

* test stuff

* Toyota: Add FW for 2023 RAV4 Hybrid (#27494)

* Toyota: Add FW for 2023 RAV4 Hybrid

* Set RAVH_TSS2_2022 to use angle SteerControlType

* bump panda

* remove extras

* Revert "remove extras"

This reverts commit 87378e7349.

* no max angle limit for now

* add as separate platforms

* remove debug scripts

* revert to master

* remove these FW versions from 2022

* dashcam these cars :(

* fix test

* interface

* add to untested routes

* never send torque with LTA cars

* fix values

* clean up controlsd

* reset lat control files

* use the car param

* add to params

* bump panda to master

* Update selfdrive/car/toyota/carcontroller.py

* don't set torque params if angle control (fixes controlsd bug)

* reset controlsd

* keyword

* in another pr

* simplify test

* rm line

* Update selfdrive/car/toyota/tests/test_toyota.py

---------

Co-authored-by: crispbee <84819466+crispbee@users.noreply.github.com>
old-commit-hash: 57c4d78869

selfdrive/car/tests/routes.py

@@ -27,6 +27,8 @@ non_tested_cars = [
   HYUNDAI.KIA_OPTIMA_H,
   HONDA.ODYSSEY_CHN,
   VOLKSWAGEN.CRAFTER_MK2,  # need a route from an ACC-equipped Crafter
+  TOYOTA.RAV4_TSS2_2023,
+  TOYOTA.RAV4H_TSS2_2023,
 ]
 
 CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,))

selfdrive/car/torque_data/override.yaml

@@ -7,6 +7,10 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
 NISSAN LEAF 2018: [.nan, 1.5, .nan]
 NISSAN ROGUE 2019: [.nan, 1.5, .nan]
 
+# Toyota LTA also has torque
+TOYOTA RAV4 2023: [.nan, 3.0, .nan]
+TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan]
+
 # Tesla has high torque
 TESLA AP1 MODEL S: [.nan, 2.5, .nan]
 TESLA AP2 MODEL S: [.nan, 2.5, .nan]

selfdrive/car/toyota/carcontroller.py

@@ -76,6 +76,11 @@ class CarController:
       apply_steer_req = 0
       self.steer_rate_counter = 0
 
+    # Never actuate with LKA on cars that only support LTA
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+      apply_steer = 0
+      apply_steer_req = 0
+
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
     if not CC.enabled and CS.pcm_acc_status:

selfdrive/car/toyota/interface.py

@@ -2,7 +2,8 @@
 from cereal import car
 from common.conversions import Conversions as CV
 from panda import Panda
-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, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
+from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
+                                        MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
 from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -23,13 +24,18 @@ class CarInterface(CarInterfaceBase):
     if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH):
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
 
+    if candidate in ANGLE_CONTROL_CAR:
+      ret.dashcamOnly = True
+      ret.steerControlType = car.CarParams.SteerControlType.angle
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
+    else:
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     ret.stoppingControl = False  # Toyota starts braking more when it thinks you want to stop
 
     stop_and_go = False
-    steering_angle_deadzone_deg = 0.0
-    CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
 
     if candidate == CAR.PRIUS:
       stop_and_go = True
@@ -40,9 +46,8 @@ class CarInterface(CarInterfaceBase):
       # Only give steer angle deadzone to for bad angle sensor prius
       for fw in car_fw:
         if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
-          steering_angle_deadzone_deg = 0.2
           ret.steerActuatorDelay = 0.25
-          CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
+          CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
 
     elif candidate == CAR.PRIUS_V:
       stop_and_go = True
@@ -102,7 +107,8 @@ class CarInterface(CarInterfaceBase):
       tire_stiffness_factor = 0.7983
       ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
 
-    elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
+    elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
+                       CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
       stop_and_go = True
       ret.wheelbase = 2.68986
       ret.steerRatio = 14.3

selfdrive/car/toyota/tests/test_toyota.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python3
+import unittest
+
+from selfdrive.car.toyota.values import TSS2_CAR, ANGLE_CONTROL_CAR
+
+
+class TestToyotaInterfaces(unittest.TestCase):
+  def test_angle_car_set(self):
+    self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/car/toyota/values.py

@@ -67,8 +67,10 @@ class CAR:
   RAV4H = "TOYOTA RAV4 HYBRID 2017"
   RAV4_TSS2 = "TOYOTA RAV4 2019"
   RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
+  RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
   RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
   RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
+  RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023"
   MIRAI = "TOYOTA MIRAI 2021"  # TSS 2.5
   SIENNA = "TOYOTA SIENNA 2018"
 
@@ -157,8 +159,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
   ],
   CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
   CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
+  CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
   CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
   CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
+  CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
   CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
   CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
 
@@ -1459,6 +1463,23 @@ FW_VERSIONS = {
       b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
     ],
   },
+  CAR.RAV4_TSS2_2023: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
+    ],
+    (Ecu.engine, 0x700, None): [
+      b'\x01896634AJ2000\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F0R03100\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
+    ],
+  },
   CAR.RAV4H_TSS2: {
     (Ecu.engine, 0x700, None): [
       b'\x01896634A15000\x00\x00\x00\x00',
@@ -1546,6 +1567,23 @@ FW_VERSIONS = {
       b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
     ],
   },
+  CAR.RAV4H_TSS2_2023: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\x01F15264283200\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
+    ],
+    (Ecu.engine, 0x700, None): [
+      b'\x01896634AE1001\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F0R03100\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
+    ],
+  },
   CAR.SIENNA: {
     (Ecu.engine, 0x700, None): [
       b'\x01896630832100\x00\x00\x00\x00',
@@ -2104,6 +2142,7 @@ DBC = {
   CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
+  CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
   CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2114,6 +2153,7 @@ DBC = {
   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.RAV4H_TSS2_2023: 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'),
@@ -2128,8 +2168,8 @@ 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.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
+TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, 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.RAV4H_TSS2_2023, 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.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
 
 NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
@@ -2138,10 +2178,13 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
 UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
 
 # these cars have a radar which sends ACC messages instead of the camera
-RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.CHR_TSS2, CAR.CHRH_TSS2}
+RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2}
+
+# these cars use the Lane Tracing Assist (LTA) message for lateral control
+ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
 
 EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
-                 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.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
                  CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
 
 # no resume button press required