From 93d4719bb1e012e81bbeba85d770547f626055cf Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Wed, 19 Jun 2024 21:10:38 +0800
Subject: [PATCH] HKG Low Speed Turn Enhancer for EV6/CAN-FD vehicles

---
 common/params.cc                       |  1 +
 selfdrive/car/hyundai/carcontroller.py | 18 +++++++++++++++++-
 system/manager/manager.py              |  1 +
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/common/params.cc b/common/params.cc
index 188751f2f..9695ebfe8 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -228,6 +228,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_toyota_auto_unlock", PERSISTENT},
     {"dp_device_disable_onroad_uploads", PERSISTENT},
     {"dp_toyota_zss", PERSISTENT},
+    {"dp_hkg_canfd_low_speed_turn_enhancer", PERSISTENT},
 };
 
 } // namespace
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 4038ddcca..3ee59c9ff 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -9,6 +9,10 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 
+# dp
+from dp_ext.selfdrive.car.hyundai.taco_car_controller_params import TacoCarControllerParams
+from openpilot.common.params import Params
+
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -47,7 +51,11 @@ class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.CAN = CanBus(CP)
-    self.params = CarControllerParams(CP)
+    self.dp_hkg_canfd_low_speed_turn_enhancer = Params().get_bool("dp_hkg_canfd_low_speed_turn_enhancer")
+    if self.dp_hkg_canfd_low_speed_turn_enhancer:
+      self.params = TacoCarControllerParams(CP)
+    else:
+      self.params = CarControllerParams(CP)
     self.packer = CANPacker(dbc_name)
     self.angle_limit_counter = 0
     self.frame = 0
@@ -61,10 +69,18 @@ class CarController(CarControllerBase):
     actuators = CC.actuators
     hud_control = CC.hudControl
 
+    # dp
+    if self.dp_hkg_canfd_low_speed_turn_enhancer:
+      self.params.update(CS.out.vEgoRaw)
+
     # steering torque
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
 
+    # dp
+    if self.dp_hkg_canfd_low_speed_turn_enhancer:
+      apply_steer = clip(apply_steer, -self.params.STEER_MAX, self.params.STEER_MAX)
+
     # >90 degree steering fault prevention
     self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
                                                                        self.angle_limit_counter, MAX_ANGLE_FRAMES,
diff --git a/system/manager/manager.py b/system/manager/manager.py
index 24f03f819..940965f09 100755
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -60,6 +60,7 @@ def manager_init() -> None:
     ("dp_toyota_auto_unlock", "0"),
     ("dp_device_disable_onroad_uploads", "0"),
     ("dp_toyota_zss", "0"),
+    ("dp_hkg_canfd_low_speed_turn_enhancer", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))