Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Toyota: Add SECOC longitudinal control #1385

Draft
wants to merge 4 commits into
base: master
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions opendbc/car/tests/routes.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 12 additions & 0 deletions opendbc/car/toyota/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,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

Expand All @@ -70,6 +71,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
sshane marked this conversation as resolved.
Show resolved Hide resolved
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']))
Expand Down Expand Up @@ -211,6 +213,16 @@ def update(self, CC, CS, now_nanos):

can_sends.append(toyotacan.create_accel_command(self.packer, pcm_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))
Expand Down
9 changes: 3 additions & 6 deletions opendbc/car/toyota/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
5 changes: 5 additions & 0 deletions opendbc/car/toyota/toyotacan.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 = {
Expand Down
Loading