Toyota: 2021+ RAV4 Prime

common/params.cc

@@ -189,6 +189,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFront", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"SecOCKey", PERSISTENT},
     {"RouteCount", PERSISTENT},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"SshEnabled", PERSISTENT},

release/files_common

@@ -536,6 +536,7 @@ opendbc/subaru_forester_2017_generated.dbc
 opendbc/toyota_tnga_k_pt_generated.dbc
 opendbc/toyota_new_mc_pt_generated.dbc
 opendbc/toyota_nodsu_pt_generated.dbc
+opendbc/toyota_rav4_prime_generated.dbc
 opendbc/toyota_adas.dbc
 opendbc/toyota_tss2_adas.dbc
 

selfdrive/car/car_helpers.py

@@ -155,7 +155,8 @@ def fingerprint(logcan, sendcan, num_pandas):
   params.put("CarVin", vin)
 
   # disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts
-  set_obd_multiplexing(params, False)
+  if not skip_fw_query:
+    set_obd_multiplexing(params, False)
   params.put_bool("FirmwareQueryDone", True)
 
   fw_query_time = time.monotonic() - start_time

selfdrive/car/tests/routes.py

@@ -197,6 +197,7 @@ class CarTestRoute(NamedTuple):
   CarTestRoute("ad5a3fa719bc2f83|2023-10-17--19-48-42", TOYOTA.TOYOTA_RAV4_TSS2_2023),
   CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.TOYOTA_RAV4_TSS2),  # hybrid
   CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022),  # hybrid
+  CarTestRoute("20ba9ade056a8c7b|2021-02-08--21-57-35", TOYOTA.TOYOTA_RAV4_PRIME),
   CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES),
   CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ES),  # hybrid
   CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2),

selfdrive/car/torque_data/substitute.toml

@@ -9,6 +9,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 
 "TOYOTA_ALPHARD_TSS2" = "TOYOTA_SIENNA"
 "TOYOTA_PRIUS_V" = "TOYOTA_PRIUS"
+"TOYOTA_RAV4_PRIME" = "TOYOTA_RAV4_TSS2"
 "LEXUS_IS" = "LEXUS_NX"
 "LEXUS_CTH" = "LEXUS_NX"
 "LEXUS_ES" = "TOYOTA_CAMRY"

selfdrive/car/toyota/carcontroller.py

@@ -1,8 +1,10 @@
 from cereal import car
 from openpilot.common.numpy_fast import clip
+from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
+from openpilot.selfdrive.car.toyota import secoc
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                         CarControllerParams, ToyotaFlags, \
                                         UNSUPPORTED_DSU_CAR
@@ -42,6 +44,10 @@ def __init__(self, dbc_name, CP, VM):
     self.gas = 0
     self.accel = 0
 
+    self.secoc_lka_message_counter = 0
+    self.secoc_lta_message_counter = 0
+    self.secoc_prev_reset_counter = 0
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -51,6 +57,18 @@ def update(self, CC, CS, now_nanos):
     # *** control msgs ***
     can_sends = []
 
+    # *** handle secoc reset counter increase ***
+    if self.CP.flags & ToyotaFlags.SECOC.value:
+      if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter:
+        self.secoc_lka_message_counter = 0
+        self.secoc_lta_message_counter = 0
+        self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT']
+
+        # Verify mac of SecOC synchronization message to ensure we have the right key
+        expected_mac = secoc.build_sync_mac(self.CP.secOCKey, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']))
+        if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac:
+          cloudlog.warning("SecOC MAC mismatch")
+
     # *** steer torque ***
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
@@ -84,7 +102,16 @@ def update(self, CC, CS, now_nanos):
     # toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2;
     # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
     # on consecutive messages
-    can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
+    steer_command = toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req)
+    if self.CP.flags & ToyotaFlags.SECOC.value:
+      # TODO: check if this slow and needs to be done by the CANPacker
+      steer_command = secoc.add_mac(self.CP.secOCKey,
+                                    int(CS.secoc_synchronization['TRIP_CNT']),
+                                    int(CS.secoc_synchronization['RESET_CNT']),
+                                    self.secoc_lka_message_counter,
+                                    steer_command)
+      self.secoc_lka_message_counter += 1
+    can_sends.append(steer_command)
 
     # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier
     if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
@@ -99,6 +126,16 @@ def update(self, CC, CS, now_nanos):
       can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle,
                                                           lta_active, self.frame // 2, torque_wind_down))
 
+      if self.CP.flags & ToyotaFlags.SECOC.value:
+        lta_steer_2 = toyotacan.create_lta_steer_command_2(self.packer, self.frame // 2)
+        lta_steer_2 = secoc.add_mac(self.CP.secOCKey,
+                                    int(CS.secoc_synchronization['TRIP_CNT']),
+                                    int(CS.secoc_synchronization['RESET_CNT']),
+                                    self.secoc_lta_message_counter,
+                                    lta_steer_2)
+        self.secoc_lta_message_counter += 1
+        can_sends.append(lta_steer_2)
+
     # *** gas and brake ***
     pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
 

selfdrive/car/toyota/carstate.py

@@ -29,11 +29,15 @@ class CarState(CarStateBase):
   def __init__(self, CP):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
-    self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
     self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
     self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
     self.cluster_min_speed = CV.KPH_TO_MS / 2.
 
+    if CP.flags & ToyotaFlags.GEAR_PACKET_HYBRID.value:
+      self.shifter_values = can_define.dv["GEAR_PACKET_HYBRID"]["GEAR"]
+    else:
+      self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
+
     # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
     # the signal is zeroed to where the steering angle is at start.
     # Need to apply an offset as soon as the steering angle measurements are both received
@@ -44,6 +48,7 @@ def __init__(self, CP):
     self.distance_button = 0
 
     self.pcm_follow_distance = 0
+    self.pcm_acc_status = 0
 
     self.low_speed_lockout = False
     self.acc_type = 1
@@ -60,7 +65,11 @@ def update(self, cp, cp_cam):
     ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
     ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
 
-    ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
+    if self.CP.flags & ToyotaFlags.ALT_GAS_MSG.value:
+      ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"]
+      ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
+    else:
+      ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
 
     ret.wheelSpeeds = self.get_wheel_speeds(
       cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
@@ -91,12 +100,16 @@ def update(self, cp, cp_cam):
         ret.steeringAngleOffsetDeg = self.angle_offset.x
         ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
 
-    can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
+    if self.CP.flags & ToyotaFlags.GEAR_PACKET_HYBRID.value:
+      can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
+    else:
+      can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+
     ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
     ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
 
-    if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
+    if self.CP.carFingerprint not in [CAR.TOYOTA_MIRAI, CAR.TOYOTA_RAV4_PRIME]:
       ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
 
     ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
@@ -148,13 +161,14 @@ def update(self, cp, cp_cam):
     if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
       # ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request
       ret.cruiseState.standstill = self.pcm_acc_status == 7
+
     ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
-    ret.cruiseState.nonAdaptive = self.pcm_acc_status in (1, 2, 3, 4, 5, 6)
+    ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6)
 
     ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
     ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
 
-    if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+    if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value and not self.CP.flags & ToyotaFlags.SECOC.value:
       ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
 
     if self.CP.enableBsm:
@@ -175,12 +189,14 @@ def update(self, cp, cp_cam):
       else:
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
+    if self.CP.flags & ToyotaFlags.SECOC.value:
+      self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
+
     return ret
 
   @staticmethod
   def get_can_parser(CP):
     messages = [
-      ("GEAR_PACKET", 1),
       ("LIGHT_STALK", 1),
       ("BLINKERS_STATE", 0.15),
       ("BODY_CONTROL_STATE", 3),
@@ -195,7 +211,15 @@ def get_can_parser(CP):
       ("STEER_TORQUE_SENSOR", 50),
     ]
 
-    if CP.carFingerprint != CAR.TOYOTA_MIRAI:
+    if CP.flags & ToyotaFlags.GEAR_PACKET_HYBRID.value:
+      messages.append(("GEAR_PACKET_HYBRID", 60))
+    else:
+      messages.append(("GEAR_PACKET", 1))
+
+    if CP.flags & ToyotaFlags.ALT_GAS_MSG.value:
+      messages.append(("GAS_PEDAL", 42))
+
+    if CP.carFingerprint not in [CAR.TOYOTA_MIRAI, CAR.TOYOTA_RAV4_PRIME]:
       messages.append(("ENGINE_RPM", 42))
 
     if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
@@ -226,6 +250,11 @@ def get_can_parser(CP):
         ("SDSU", 100),
       ]
 
+    if CP.flags & ToyotaFlags.SECOC.value:
+      messages += [
+        ("SECOC_SYNCHRONIZATION", 10),
+      ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
@@ -239,9 +268,14 @@ def get_cam_can_parser(CP):
 
     if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
       messages += [
-        ("PRE_COLLISION", 33),
         ("ACC_CONTROL", 33),
         ("PCS_HUD", 1),
       ]
 
+      # TODO: Figure out new layout of the PRE_COLLISION message
+      if not CP.flags & ToyotaFlags.SECOC.value:
+        messages += [
+          ("PRE_COLLISION", 33),
+        ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

selfdrive/car/toyota/fingerprints.py

@@ -1138,6 +1138,23 @@
       b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
     ],
   },
+  CAR.TOYOTA_RAV4_PRIME: {
+    (Ecu.engine, 0x700, None): [
+      b'\x018966342S7000\x00\x00\x00\x00'
+    ],
+    (Ecu.abs, 0x7b0, None): [
+      b'\x01F15264228300\x00\x00\x00\x00'
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'\x018965B4209000\x00\x00\x00\x00'
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F3301400\x00\x00\x00\x00'
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F4205200\x00\x00\x00\x008646G4202000\x00\x00\x00\x00'
+    ],
+  },
   CAR.TOYOTA_RAV4_TSS2_2023: {
     (Ecu.abs, 0x7b0, None): [
       b'\x01F15260R450\x00\x00\x00\x00\x00\x00',

selfdrive/car/toyota/interface.py

@@ -1,8 +1,10 @@
 from cereal import car
 from panda import Panda
 from panda.python import uds
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
-                                        MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
+                                        MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, SECOC_CAR
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
@@ -25,7 +27,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
 
     # BRAKE_MODULE is on a different address for these cars
     if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated":
-      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE_224
+
+    if candidate in SECOC_CAR:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_SECOC_CAR
 
     if candidate in ANGLE_CONTROL_CAR:
       ret.steerControlType = SteerControlType.angle
@@ -74,7 +79,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
       # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
       stop_and_go = candidate != CAR.TOYOTA_AVALON
 
-    elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023):
+    elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME):
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP = [0.0]
       ret.lateralTuning.pid.kpBP = [0.0]
@@ -131,7 +136,15 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     #  - TSS2 radar ACC cars w/ smartDSU installed
     #  - TSS2 radar ACC cars w/o smartDSU installed (disables radar)
     #  - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
-    ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
+
+    if ret.flags & ToyotaFlags.SECOC.value: # Lateral only for now
+      ret.openpilotLongitudinalControl = False
+    else:
+      ret.openpilotLongitudinalControl = use_sdsu or \
+        ret.enableDsu or \
+        candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
+        bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
+
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 
     if not ret.openpilotLongitudinalControl:
@@ -141,6 +154,17 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
 
+    # Read SecOC key from param
+    if ret.flags & ToyotaFlags.SECOC.value:
+      key = Params().get("SecOCKey", encoding='utf8')
+
+      # TODO: show warning, and handle setting key in CI
+      if key is None:
+        cloudlog.warning("SecOCKey is not set")
+        key = "0" * 32
+
+      ret.secOCKey = bytes.fromhex(key.strip())
+
     tune = ret.longitudinalTuning
     tune.deadzoneBP = [0., 9.]
     tune.deadzoneV = [.0, .15]