ZSS Controller

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

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

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')))