From b464cebff5bcad45267b8032decb4d053ed90198 Mon Sep 17 00:00:00 2001
From: chrispypatt <christriplenickel@yahoo.com>
Date: Fri, 18 Oct 2024 13:05:50 -0500
Subject: [PATCH] Add secoc ACC command

---
 opendbc/car/toyota/carcontroller.py | 12 ++++++++++++
 opendbc/car/toyota/interface.py     | 12 ++++++------
 opendbc/car/toyota/toyotacan.py     |  5 +++++
 3 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py
index d6bdd9782e..2499a13f9b 100644
--- a/opendbc/car/toyota/carcontroller.py
+++ b/opendbc/car/toyota/carcontroller.py
@@ -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
 
@@ -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
         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']))
@@ -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))
diff --git a/opendbc/car/toyota/interface.py b/opendbc/car/toyota/interface.py
index 3691f9f78c..98a7a86a5c 100644
--- a/opendbc/car/toyota/interface.py
+++ b/opendbc/car/toyota/interface.py
@@ -117,12 +117,12 @@ 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)
+    # 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.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 = {