From 3aa25b00e467778ebb551ab86f550e0b82297611 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 4 Jun 2024 11:57:48 -0700
Subject: [PATCH] FrogPilot community - ZSS support

Credit goes to DragonPilot!

https: //github.com/dragonpilot-community/dragonpilot
Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
---
 selfdrive/car/toyota/carstate.py  | 37 +++++++++++++++++++++++++++++++
 selfdrive/car/toyota/interface.py |  3 +++
 selfdrive/car/toyota/values.py    |  2 ++
 3 files changed, 42 insertions(+)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 9099e2ed6..7dd5da32d 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -24,6 +24,8 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
 # - prolonged high driver torque: 17 (permanent)
 PERM_STEER_FAULTS = (3, 17)
 
+ZSS_THRESHOLD = 4.0
+ZSS_THRESHOLD_COUNT = 10
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -49,6 +51,13 @@ class CarState(CarStateBase):
     self.acc_type = 1
     self.lkas_hud = {}
 
+    # FrogPilot variables
+    self.zss_compute = False
+    self.zss_cruise_active_last = False
+
+    self.zss_angle_offset = 0
+    self.zss_threshold_count = 0
+
   def update(self, cp, cp_cam, frogpilot_toggles):
     ret = car.CarState.new_message()
     fp_ret = custom.FrogPilotCarState.new_message()
@@ -181,6 +190,32 @@ class CarState(CarStateBase):
       else:
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
+    # FrogPilot CarState functions
+
+    # ZSS Support - Credit goes to the DragonPilot team!
+    if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT:
+      zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
+
+      # Only compute ZSS offset when cruise is active
+      cruise_active = ret.cruiseState.available
+      if cruise_active and not self.zss_cruise_active_last:
+        self.zss_compute = True  # Cruise was just activated, so allow offset to be recomputed
+        self.zss_threshold_count = 0
+      self.zss_cruise_active_last = cruise_active
+
+      # Compute ZSS offset
+      if self.zss_compute:
+        if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
+          self.zss_compute = False
+          self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
+
+      # Safety checks
+      steering_angle_deg = zorro_steer - self.zss_angle_offset
+      if abs(ret.steeringAngleDeg - steering_angle_deg) > ZSS_THRESHOLD:
+        self.zss_threshold_count += 1
+      else:
+        ret.steeringAngleDeg = steering_angle_deg
+
     return ret, fp_ret
 
   @staticmethod
@@ -236,6 +271,8 @@ class CarState(CarStateBase):
         ("SDSU", 100),
       ]
 
+    messages += [("SECONDARY_STEER_ANGLE", 0)]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 5471d4932..c1e9ff611 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -40,6 +40,9 @@ class CarInterface(CarInterfaceBase):
       ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
       ret.steerLimitTimer = 0.4
 
+      if 0x23 in fingerprint[0]:  # Detect if ZSS is present
+        ret.flags |= ToyotaFlags.ZSS.value
+
     ret.stoppingControl = False  # Toyota starts braking more when it thinks you want to stop
 
     # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 617b65957..b441e6f72 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -59,6 +59,8 @@ class ToyotaFlags(IntFlag):
   # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU
   SNG_WITHOUT_DSU = 512
 
+  # FrogPilot Toyota flags
+  ZSS = 1024
 
 class Footnote(Enum):
   CAMRY = CarFootnote(