From 4eaaebc59205d46c033c76c0b1b8cc81e2366e5f Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 18 Jun 2023 22:29:54 -0400
Subject: [PATCH] HKG CAN: Smoother Stopping Performance toggle (#173)

* HKG CAN: Smoother Stopping Performance toggle

* only toggleable offroad
---
 CHANGELOGS.md                                  |  4 +++-
 selfdrive/car/hyundai/carcontroller.py         |  3 +++
 selfdrive/car/hyundai/interface.py             |  4 ++++
 selfdrive/ui/qt/offroad/sunnypilot_settings.cc | 15 +++++++++++++++
 4 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/CHANGELOGS.md b/CHANGELOGS.md
index e1b68e0525..8249125e30 100644
--- a/CHANGELOGS.md
+++ b/CHANGELOGS.md
@@ -1,6 +1,8 @@
 sunnypilot - 0.9.3.1 (2023-XX-XX)
 ========================
-
+* NEW❗: HKG CAN: Smoother Stopping Performance (Beta) toggle
+  * Smoother stopping behind a stopped car or desired stopping event.
+  * This is only applicable to HKG CAN platforms using openpilot longitudinal control
 
 sunnypilot - 0.9.2.3 (2023-06-18)
 ========================
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index a11f8753ed..0db9f7258a 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -92,6 +92,7 @@ class CarController:
     self.v_tsc = 0
     self.m_tsc = 0
     self.steady_speed = 0
+    self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
 
   def update(self, CC, CS, now_nanos):
     if not self.CP.pcmCruiseSpeed:
@@ -285,6 +286,8 @@ class CarController:
                 self.last_button_frame = self.frame
 
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
+        if self.hkg_can_smooth_stop:
+          stopping = stopping and CS.out.vEgoRaw < 0.05
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 581cec3b78..3f2c219923 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -2,6 +2,7 @@
 from cereal import car
 from panda import Panda
 from common.conversions import Conversions as CV
+from common.params import Params
 from selfdrive.car.hyundai.hyundaicanfd import CanBus
 from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, NON_SCC_CAR, Buttons
 from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
@@ -326,6 +327,9 @@ class CarInterface(CarInterfaceBase):
     if 0x2AA in fingerprint[0]:
       ret.minSteerSpeed = 0.
 
+    if Params().get_bool("HkgSmoothStop"):
+      ret.vEgoStopping = 0.1
+
     return ret
 
   @staticmethod
diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
index f38911c24c..cbb1ee6f0f 100644
--- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
+++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
@@ -528,6 +528,16 @@ SPVehiclesPanel::SPVehiclesPanel(QWidget *parent) : QWidget(parent) {
 
 SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(SPVehiclesPanel *parent) : ListWidget(parent) {
   setSpacing(50);
+  addItem(new LabelControl(tr("Hyundai/Kia/Genesis")));
+  auto hkgSmoothStop = new ParamControl(
+    "HkgSmoothStop",
+    "HKG CAN: Smoother Stopping Performance (Beta)",
+    "Smoother stopping behind a stopped car or desired stopping event. This is only applicable to HKG CAN platforms using openpilot longitudinal control.",
+    "../assets/offroad/icon_blank.png"
+  );
+  hkgSmoothStop->setConfirmation(true, false);
+  addItem(hkgSmoothStop);
+
   addItem(new LabelControl(tr("Toyota/Lexus")));
   stockLongToyota = new ParamControl(
     "StockLongToyota",
@@ -549,6 +559,11 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(SPVehiclesPanel *parent) : ListWi
   );
   lkasToggle->setConfirmation(true, false);
   addItem(lkasToggle);
+
+  // trigger offroadTransition when going onroad/offroad
+  connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
+    hkgSmoothStop->setEnabled(offroad);
+  });
 }
 
 SPVisualsPanel::SPVisualsPanel(QWidget *parent) : ListWidget(parent) {