From 51c13c4b502d8094ba5de8e6594a5492e6d4c834 Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Mon, 31 Mar 2025 13:45:33 +0800
Subject: [PATCH] Toyota SDSU - 2025-06-13

---
 opendbc_repo/opendbc/car/toyota/carstate.py  |  9 +++++++--
 opendbc_repo/opendbc/car/toyota/interface.py | 17 ++++++++++++++++-
 opendbc_repo/opendbc/car/toyota/values.py    |  3 +++
 3 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py
index ce1b50884..dea27f9bb 100644
--- a/opendbc_repo/opendbc/car/toyota/carstate.py
+++ b/opendbc_repo/opendbc/car/toyota/carstate.py
@@ -153,7 +153,8 @@ class CarState(CarStateBase):
       ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
 
     if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
-      self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
+      if not (self.CP.flags & ToyotaFlags.SDSU.value):
+        self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
       ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
 
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
@@ -238,8 +239,12 @@ class CarState(CarStateBase):
     if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       pt_messages += [
         ("PCS_HUD", 1),
-        ("ACC_CONTROL", 33),
+        # ("ACC_CONTROL", 33),
       ]
+      if not CP.flags & ToyotaFlags.SDSU.value:
+        pt_messages += [
+          ("ACC_CONTROL", 33),
+        ]
 
     if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       pt_messages += [
diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py
index 87d523611..fa56c3d8a 100644
--- a/opendbc_repo/opendbc/car/toyota/interface.py
+++ b/opendbc_repo/opendbc/car/toyota/interface.py
@@ -118,6 +118,20 @@ class CarInterface(CarInterfaceBase):
       if alpha_long and candidate in RADAR_ACC_CAR:
         ret.flags |= ToyotaFlags.DISABLE_RADAR.value
 
+    sdsu_active = False
+    if not (candidate in (RADAR_ACC_CAR | NO_DSU_CAR)) and 0x2FF in fingerprint[0]:
+      print("----------------------------------------------")
+      print("dragonpilot: SDSU detected!")
+      print("----------------------------------------------")
+      ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value
+
+      ret.enableDsu = False
+      sdsu_active = True
+      stop_and_go = True
+
+      ret.flags |= ToyotaFlags.SDSU.value
+      ret.alphaLongitudinalAvailable = False
+
     # openpilot longitudinal enabled by default:
     #  - cars w/ DSU disconnected
     #  - TSS2 cars with camera sending ACC_CONTROL where we can block it
@@ -129,7 +143,8 @@ class CarInterface(CarInterfaceBase):
     else:
       ret.openpilotLongitudinalControl = ret.enableDsu or \
         candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
-        bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
+        bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) or \
+        sdsu_active
 
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 
diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py
index f10f7d0ce..c62be312a 100644
--- a/opendbc_repo/opendbc/car/toyota/values.py
+++ b/opendbc_repo/opendbc/car/toyota/values.py
@@ -55,11 +55,14 @@ class ToyotaSafetyFlags(IntFlag):
   STOCK_LONGITUDINAL = (2 << 8)
   LTA = (4 << 8)
   SECOC = (8 << 8)
+  LONG_FILTER = (16 << 8)
 
 
 class ToyotaFlags(IntFlag):
   # Detected flags
   HYBRID = 1
+  # use legacy id
+  SDSU = 2
   DISABLE_RADAR = 4
 
   # Static flags