From 9dfd0579d4bcf4065ee289a2486bb77f0700655a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 14 Sep 2023 14:48:12 -0700 Subject: [PATCH] Toyota: use torque control for Japanese-made 2023 RAV4 (#29896) * use torque for 2023 RAV4s that allow it * FW * better cmt * better * better values comment * switch * add torque params * simpler * better to use steerControlType * ??? * fix condition * adjust params * pid is only for old cars * add comment * move comment up * one if fix * invert * better comment * better here * for * add torque hybrid test route * fix condition * this might be simpler * back to one if fix * better name * other way * this reads so much better * last change * better * did i break ci? --- selfdrive/car/tests/routes.py | 2 +- selfdrive/car/torque_data/override.yaml | 6 +-- selfdrive/car/toyota/interface.py | 49 ++++++++++++++----------- selfdrive/car/toyota/values.py | 3 +- 4 files changed, 33 insertions(+), 27 deletions(-) diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index c8701e8f0a..69a935fece 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -25,7 +25,6 @@ non_tested_cars = [ HONDA.ODYSSEY_CHN, VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter TOYOTA.RAV4_TSS2_2023, - TOYOTA.RAV4H_TSS2_2023, SUBARU.FORESTER_HYBRID, ] @@ -184,6 +183,7 @@ routes = [ CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022), CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2), CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), + CarTestRoute("49e041422a032273|2023-09-14--09-21-32", TOYOTA.RAV4H_TSS2_2023), CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), CarTestRoute("da23c367491f53e2|2021-05-21--09-09-11", TOYOTA.LEXUS_CTH, segment=3), diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index fc39cd76cb..61ed8a5a77 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -12,10 +12,6 @@ SUBARU FORESTER 2022: [.nan, 3.0, .nan] SUBARU OUTBACK 7TH GEN: [.nan, 3.0, .nan] SUBARU ASCENT 2023: [.nan, 3.0, .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] @@ -61,6 +57,8 @@ HYUNDAI KONA ELECTRIC 2ND GEN: [2.5, 2.5, 0.1] HYUNDAI IONIQ 6 2023: [2.5, 2.5, 0.1] HYUNDAI AZERA 6TH GEN: [1.8, 1.8, 0.1] KIA K8 HYBRID 1ST GEN: [2.5, 2.5, 0.1] +TOYOTA RAV4 2023: [2.5, 2.5, 0.1] +TOYOTA RAV4 HYBRID 2023: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d6f428ab1d..d51026b547 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -27,7 +27,15 @@ class CarInterface(CarInterfaceBase): if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated": ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE - if candidate in ANGLE_CONTROL_CAR: + # Allow angle control cars with whitelisted EPSs to use torque control (made in Japan) + # So far only hybrid RAV4 2023 has been seen with this FW version + angle_car_torque_fw = any(fw.ecu == "eps" and fw.fwVersion == b'8965B42371\x00\x00\x00\x00\x00\x00' for fw in car_fw) + if candidate not in ANGLE_CONTROL_CAR or (angle_car_torque_fw and candidate == CAR.RAV4H_TSS2_2023): + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + ret.steerLimitTimer = 0.4 + else: ret.dashcamOnly = True ret.steerControlType = SteerControlType.angle ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA @@ -35,11 +43,6 @@ class CarInterface(CarInterfaceBase): # LTA control can be more delayed and winds up more often ret.steerActuatorDelay = 0.25 ret.steerLimitTimer = 0.8 - 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 @@ -121,21 +124,25 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.3 ret.tireStiffnessFactor = 0.7933 ret.mass = 3585. * CV.LB_TO_KG # Average between ICE and Hybrid - ret.lateralTuning.init('pid') - ret.lateralTuning.pid.kiBP = [0.0] - ret.lateralTuning.pid.kpBP = [0.0] - ret.lateralTuning.pid.kpV = [0.6] - ret.lateralTuning.pid.kiV = [0.1] - ret.lateralTuning.pid.kf = 0.00007818594 - - # 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']): - ret.lateralTuning.pid.kpV = [0.15] - ret.lateralTuning.pid.kiV = [0.05] - ret.lateralTuning.pid.kf = 0.00004 - break + + # Only specific EPS FW accept torque on 2023 RAV4, so they likely are all the same + # TODO: revisit this disparity if there is a divide for 2023 + if candidate not in (CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023): + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kiBP = [0.0] + ret.lateralTuning.pid.kpBP = [0.0] + ret.lateralTuning.pid.kpV = [0.6] + ret.lateralTuning.pid.kiV = [0.1] + ret.lateralTuning.pid.kf = 0.00007818594 + + # 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']): + ret.lateralTuning.pid.kpV = [0.15] + ret.lateralTuning.pid.kiV = [0.05] + ret.lateralTuning.pid.kf = 0.00004 + break elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2): ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index bb9672fb0e..fd0a54b349 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -2389,7 +2389,8 @@ 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.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 +# these cars manufactured in U.S., Canada have EPSs that reject Lane Keep Assist (LKA, torque) messages and require +# Lane Tracing Assist (LTA, angle) to steer properly. cars manufactured in Japan still work with the older LKA messages which is detected 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,