From aa1eca0f77a34ab61f1c57cce7df01a7b2870230 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Sun, 17 Mar 2024 17:10:22 +0100 Subject: [PATCH] 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 | 1 + release/files_common | 1 + selfdrive/car/car_helpers.py | 3 +- selfdrive/car/tests/routes.py | 1 + selfdrive/car/torque_data/substitute.toml | 1 + selfdrive/car/toyota/carcontroller.py | 39 +++++++++++++- selfdrive/car/toyota/carstate.py | 65 ++++++++++++++++++----- selfdrive/car/toyota/fingerprints.py | 17 ++++++ selfdrive/car/toyota/interface.py | 29 +++++++++- selfdrive/car/toyota/secoc.py | 47 ++++++++++++++++ selfdrive/car/toyota/toyotacan.py | 6 ++- selfdrive/car/toyota/values.py | 11 +++- 12 files changed, 202 insertions(+), 19 deletions(-) create mode 100644 selfdrive/car/toyota/secoc.py diff --git a/common/params.cc b/common/params.cc index a00401d20af1bd..f6b443b9fa23d3 100644 --- a/common/params.cc +++ b/common/params.cc @@ -189,6 +189,7 @@ std::unordered_map 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}, diff --git a/release/files_common b/release/files_common index 34d8f009732f15..2d5e3d6c901bb9 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 66097339b182bc..b0a819f4fba46e 100644 --- a/selfdrive/car/car_helpers.py +++ b/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 diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index dd3a8f633d2693..73c7a763d42cc4 100755 --- a/selfdrive/car/tests/routes.py +++ b/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), diff --git a/selfdrive/car/torque_data/substitute.toml b/selfdrive/car/torque_data/substitute.toml index 8724a08010e3b0..3a107edf0f11fe 100644 --- a/selfdrive/car/torque_data/substitute.toml +++ b/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" diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f2d8a4a7bc5637..719850bd3b82df 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8315f24ae48d8b..5a491ddeb565bc 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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) diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index a57e58fbb7faab..87a565af64d8f6 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/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', diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3ea05f9fef57b9..d8ace927449652 100644 --- a/selfdrive/car/toyota/interface.py +++ b/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] diff --git a/selfdrive/car/toyota/secoc.py b/selfdrive/car/toyota/secoc.py new file mode 100644 index 00000000000000..82279d64b1685c --- /dev/null +++ b/selfdrive/car/toyota/secoc.py @@ -0,0 +1,47 @@ +import struct + +from Crypto.Hash import CMAC +from Crypto.Cipher import AES + +def add_mac(key, trip_cnt, reset_cnt, msg_cnt, msg): + # TODO: clean up conversion to and from hex + + addr, t, payload, bus = msg + reset_flag = reset_cnt & 0b11 + msg_cnt_flag = msg_cnt & 0b11 + payload = payload[:4] + + # Step 1: Build Freshness Value (48 bits) + # [Trip Counter (16 bit)][[Reset Counter (20 bit)][Message Counter (8 bit)][Reset Flag (2 bit)][Padding (2 bit)] + freshness_value = struct.pack('>HI', trip_cnt, (reset_cnt << 12) | ((msg_cnt & 0xff) << 4) | (reset_flag << 2)) + + # Step 2: Build data to authenticate (96 bits) + # [Message ID (16 bits)][Payload (32 bits)][Freshness Value (48 bits)] + to_auth = struct.pack('>H', addr) + payload + freshness_value + + # Step 3: Calculate CMAC (28 bit) + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + mac = cmac.digest().hex()[:7] # truncated MAC + + # Step 4: Build message + # [Payload (32 bit)][Message Counter Flag (2 bit)][Reset Flag (2 bit)][Authenticator (28 bit)] + msg_cnt_rst_flag = struct.pack('>B', (msg_cnt_flag << 2) | reset_flag).hex()[1] + msg = payload.hex() + msg_cnt_rst_flag + mac + payload = bytes.fromhex(msg) + + return (addr, t, payload, bus) + +def build_sync_mac(key, trip_cnt, reset_cnt, id_=0xf): + id_ = struct.pack('>H', id_) # 16 + trip_cnt = struct.pack('>H', trip_cnt) # 16 + reset_cnt = struct.pack('>I', reset_cnt << 12)[:-1] # 20 + 4 padding + + to_auth = id_ + trip_cnt + reset_cnt # SecOC 11.4.1.1 page 138 + + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + + msg = "0" + cmac.digest().hex()[:7] + msg = bytes.fromhex(msg) + return struct.unpack('>I', msg)[0] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 1cc99b41b57873..1bb1f31481986b 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -2,7 +2,6 @@ SteerControlType = car.CarParams.SteerControlType - def create_steer_command(packer, steer, steer_req): """Creates a CAN message for the Toyota Steer Command.""" @@ -32,6 +31,11 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, } return packer.make_can_msg("STEERING_LTA", 0, values) +def create_lta_steer_command_2(packer, frame): + values = { + "COUNTER": frame + 128, + } + return packer.make_can_msg("STEERING_LTA_2", 0, values) def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance): # TODO: find the exact canceling bit that does not create a chime diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index dbab2e92556d72..191936de3a1362 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -47,6 +47,7 @@ class ToyotaFlags(IntFlag): SMART_DSU = 2 DISABLE_RADAR = 4 RADAR_CAN_FILTER = 1024 + GEAR_PACKET_HYBRID = 2048 # Static flags TSS2 = 8 @@ -58,6 +59,8 @@ class ToyotaFlags(IntFlag): NO_STOP_TIMER = 256 # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU SNG_WITHOUT_DSU = 512 + SECOC = 4096 + ALT_GAS_MSG = 8192 class Footnote(Enum): @@ -246,7 +249,13 @@ class CAR(Platforms): TOYOTA_RAV4_TSS2.specs, flags=ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL, ) - TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( # TSS 2.5 + TOYOTA_RAV4_PRIME = PlatformConfig( + [], + CarSpecs(mass=3650. * CV.LB_TO_KG, wheelbase=2.65, steerRatio=16.88, tireStiffnessFactor=0.5533), + dbc_dict('toyota_rav4_prime_generated', 'toyota_tss2_adas'), + flags=ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU | ToyotaFlags.SECOC | ToyotaFlags.ALT_GAS_MSG, + ) + TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( [ToyotaCarDocs("Toyota Mirai 2021")], CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8), )