diff --git a/opendbc/car/tests/routes.py b/opendbc/car/tests/routes.py index 2e1eca29a0..4c595bdd1a 100644 --- a/opendbc/car/tests/routes.py +++ b/opendbc/car/tests/routes.py @@ -207,6 +207,7 @@ class CarTestRoute(NamedTuple): 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), # SecOC + CarTestRoute("41ba5b181f29435d/00000075--b8cdba16c8", TOYOTA.TOYOTA_RAV4_PRIME), # SecOC CarTestRoute("8bfb000e03b2a257/00000004--f9eee5f52e", TOYOTA.TOYOTA_SIENNA_4TH_GEN), # SecOC CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ES), # hybrid diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index e1b4003b84..86ca51f3bd 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -51,6 +51,7 @@ def __init__(self, dbc_name, CP): self.secoc_lka_message_counter = 0 self.secoc_lta_message_counter = 0 + self.secoc_acc_message_counter = 0 self.secoc_prev_reset_counter = 0 self.secoc_mismatch_counter = 0 @@ -68,6 +69,7 @@ def update(self, CC, CS, now_nanos): 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_acc_message_counter = 0 self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT'] expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT'])) @@ -210,8 +212,19 @@ def update(self, CC, CS, now_nanos): # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, + main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd + can_sends.append(toyotacan.create_accel_command(self.packer, main_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button)) + if self.CP.flags & ToyotaFlags.SECOC.value: + acc_cmd_2 = toyotacan.create_accel_command_2(self.packer, pcm_accel_cmd) + acc_cmd_2 = add_mac(self.secoc_key, + int(CS.secoc_synchronization['TRIP_CNT']), + int(CS.secoc_synchronization['RESET_CNT']), + self.secoc_acc_message_counter, + acc_cmd_2) + self.secoc_acc_message_counter += 1 + can_sends.append(acc_cmd_2) + self.accel = pcm_accel_cmd else: can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button)) diff --git a/opendbc/car/toyota/interface.py b/opendbc/car/toyota/interface.py index 3691f9f78c..825e78db28 100644 --- a/opendbc/car/toyota/interface.py +++ b/opendbc/car/toyota/interface.py @@ -117,12 +117,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime # openpilot longitudinal behind experimental long toggle: # - TSS2 radar ACC cars (disables radar) - if ret.flags & ToyotaFlags.SECOC.value: - ret.openpilotLongitudinalControl = False - else: - ret.openpilotLongitudinalControl = ret.enableDsu or \ - candidate in (TSS2_CAR - RADAR_ACC_CAR) or \ - bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) + ret.openpilotLongitudinalControl = 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 diff --git a/opendbc/car/toyota/toyotacan.py b/opendbc/car/toyota/toyotacan.py index c4f27510d6..13ae8c11ec 100644 --- a/opendbc/car/toyota/toyotacan.py +++ b/opendbc/car/toyota/toyotacan.py @@ -55,6 +55,11 @@ def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_r } return packer.make_can_msg("ACC_CONTROL", 0, values) +def create_accel_command_2(packer, accel): + values = { + "ACCEL_CMD": accel, + } + return packer.make_can_msg("ACC_CONTROL_2", 0, values) def create_pcs_commands(packer, accel, active, mass): values1 = {