rav4 prime

no engine RPM message

verify mac of sync message

fix PCM_CRUISE

fix line too long

add placeholder test route

use default SecOC key in CI

add dbc to release files

always in drive for now

fix radar dbc file

fix gear shifter parsing

add gasPressed

bump submodules

move secoc stuff in own file

typo

turn alt gas msg into flag

bump panda

name fixes

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"]
@@ -144,17 +157,23 @@ def update(self, cp, cp_cam):
        (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
       self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
 
-    self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
-    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
+    if self.CP.carFingerprint not in [CAR.TOYOTA_RAV4_PRIME]:
+      self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
+      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)
+
+    if self.CP.carFingerprint not in [CAR.TOYOTA_RAV4_PRIME]:
+      ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6)
+    else:
+      ret.cruiseState.nonAdaptive = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) and not bool(cp.vl["PCM_CRUISE"]["ADAPTIVE_CRUISE"])
 
     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 +194,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 +216,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 +255,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 +273,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,6 +1,8 @@
 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
 from openpilot.selfdrive.car import create_button_events, get_safety_config
@@ -52,6 +54,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR:
       ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value
 
+    # Detect missing GEAR_PACKET msg, but has GEAR_PACKET_HYBRID
+    if 0x3BC not in fingerprint[0] and 0x127 in fingerprint[0]:
+      ret.flags |= ToyotaFlags.GEAR_PACKET_HYBRID.value
+
     # In TSS2 cars, the camera does long control
     found_ecus = [fw.ecu for fw in car_fw]
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
@@ -74,7 +80,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 +137,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 +155,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]