From f463191213d59976fdc7278744300f640a7b53c5 Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Wed, 19 Jun 2024 20:21:47 +0800
Subject: [PATCH] ZSS Controller

---
 common/params.cc                 | 1 +
 selfdrive/car/toyota/carstate.py | 7 +++++++
 system/manager/manager.py        | 1 +
 3 files changed, 9 insertions(+)

diff --git a/common/params.cc b/common/params.cc
index 62d71d31e..188751f2f 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -227,6 +227,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_toyota_auto_lock", PERSISTENT},
     {"dp_toyota_auto_unlock", PERSISTENT},
     {"dp_device_disable_onroad_uploads", PERSISTENT},
+    {"dp_toyota_zss", PERSISTENT},
 };
 
 } // namespace
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 8315f24ae..3178be7fc 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -11,6 +11,8 @@ from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
+from dp_ext.selfdrive.car.toyota.zss_controller import ZSSController
+
 SteerControlType = car.CarParams.SteerControlType
 
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
@@ -49,6 +51,8 @@ class CarState(CarStateBase):
     self.acc_type = 1
     self.lkas_hud = {}
 
+    self.zssc = ZSSController()
+
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
@@ -78,6 +82,9 @@ class CarState(CarStateBase):
     ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
+    # dp - zss controller
+    ret.steeringAngleDeg = self.zssc.get_steering_angle_deg(cp.vl["PCM_CRUISE_2"]["MAIN_ON"], cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"], ret.steeringAngleDeg)
+
     # On some cars, the angle measurement is non-zero while initializing
     if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
       self.accurate_steer_angle_seen = True
diff --git a/system/manager/manager.py b/system/manager/manager.py
index ef1cf9a61..24f03f819 100755
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -59,6 +59,7 @@ def manager_init() -> None:
     ("dp_toyota_auto_lock", "0"),
     ("dp_toyota_auto_unlock", "0"),
     ("dp_device_disable_onroad_uploads", "0"),
+    ("dp_toyota_zss", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))