From d923f2fa5989cc78389e406159a092a82682f0a6 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 21 Jul 2024 16:54:19 -0400
Subject: [PATCH 01/53] only in stopping state at 0.5 (same as stock), and
 track accel_last

---
 selfdrive/car/hyundai/carcontroller.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 74b9bf1811..686d9ec7db 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -299,6 +299,7 @@ class CarController(CarControllerBase):
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
+        self.accel_last = accel
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -473,10 +474,10 @@ class CarController(CarControllerBase):
       cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
     return cruise_button
 
-  def cal_jerk(self, accel, actuators):
+  def cal_jerk(self, CS, accel, actuators):
     if actuators.longControlState == LongCtrlState.off:
       accel_diff = 0.0
-    elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
+    elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:# or hud_control.softHold > 0:
       accel_diff = 0.0
     else:
       accel_diff = accel - self.accel_last
@@ -486,7 +487,7 @@ class CarController(CarControllerBase):
     return self.jerk
 
   def make_jerk(self, CS, accel, actuators):
-    jerk = self.cal_jerk(accel, actuators)
+    jerk = self.cal_jerk(CS, accel, actuators)
     a_error = accel - CS.out.aEgo
     jerk = jerk + (a_error * 2.0)
 
@@ -499,7 +500,7 @@ class CarController(CarControllerBase):
         self.jerk_u = jerkLimit
         self.jerk_l = jerkLimit
         self.jerk_count = 0
-      elif actuators.longControlState == LongCtrlState.stopping:
+      elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
         self.jerk_u += 0.1 if self.jerk_u < 1.5 else -0.1
         self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1
         self.jerk_count = 0
@@ -518,7 +519,7 @@ class CarController(CarControllerBase):
         self.jerk_u = jerkLimit
         self.jerk_l = jerkLimit
         self.jerk_count = 0
-      elif actuators.longControlState == LongCtrlState.stopping:
+      elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
         self.jerk_u += 0.1 if self.jerk_u < 0.5 else -0.1
         self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1
         self.jerk_count = 0

From 5f2c104ae5ce38894664c45f867ad630935d9b1e Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 21 Jul 2024 17:40:06 -0400
Subject: [PATCH 02/53] try this out (pls don't die)

---
 selfdrive/car/hyundai/carcontroller.py | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 686d9ec7db..c9cf066f2f 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -110,6 +110,9 @@ class CarController(CarControllerBase):
     self.cb_lower = 0.0
     self.jerk_count = 0.0
 
+    self.accel_val = 0
+    self.accel_raw = 0
+
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
     lead_one = self.sm["radarState"].leadOne
     lead_two = self.sm["radarState"].leadTwo
@@ -284,6 +287,7 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
+        self.make_accel(CS, accel, stopping)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -296,10 +300,10 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        stopping = stopping and CS.out.vEgo < self.CP.vEgoStopping
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
-        self.accel_last = accel
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -529,3 +533,10 @@ class CarController(CarControllerBase):
         #self.jerk_l = min(max(1.2, -jerk * 2.0), jerkLimit) ## 1.0으로 하니 덜감속, 1.5로하니 너무감속, 1.2로 한번해보자(231228)
         self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
+
+  def make_accel(self, CS, accel, stopping):
+    self.accel_raw = accel
+    if stopping and CS.out.vEgo < self.CP.vEgoStopping:
+      self.accel_raw = 0
+    self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l)
+    self.accel_last = self.accel_val

From b32b216a6b411fa8ee224df2e398a6a166b33097 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 21 Jul 2024 17:41:16 -0400
Subject: [PATCH 03/53] try this out (pls don't die)

---
 selfdrive/car/hyundai/hyundaican.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index f3776c46ef..58b05d708a 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -130,7 +130,7 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
+def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
                         CS, escc, CP, lead_distance, cb_lower, cb_upper):
   commands = []
 
@@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud
   scc12_values = {
     "ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
     "StopReq": 1 if stopping else 0,
-    "aReqRaw": accel,
-    "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
+    "aReqRaw": accel_raw,
+    "aReqValue": accel_val,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
     "CR_VSM_Alive": idx % 0xF,
   }
 

From c5af23a01067e378c603d89b4907db4227387ad4 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 21 Jul 2024 17:44:55 -0400
Subject: [PATCH 04/53] clear values if not engaged

---
 selfdrive/car/hyundai/carcontroller.py | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index c9cf066f2f..eaadfa1800 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -287,7 +287,7 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
-        self.make_accel(CS, accel, stopping)
+        self.make_accel(CS, accel, actuators)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -534,9 +534,12 @@ class CarController(CarControllerBase):
         self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
-  def make_accel(self, CS, accel, stopping):
+  def make_accel(self, CS, accel, actuators):
     self.accel_raw = accel
-    if stopping and CS.out.vEgo < self.CP.vEgoStopping:
-      self.accel_raw = 0
-    self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l)
+    if actuators.longControlState == LongCtrlState.off:
+      self.accel_raw, self.accel_val = 0, 0
+    else:
+      if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
+        self.accel_raw = 0
+      self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l)
     self.accel_last = self.accel_val

From 56cf00230717bda8e26878ad5c3222c1657030b5 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sun, 21 Jul 2024 21:43:53 -0400
Subject: [PATCH 05/53] change in 0.1 like stock

---
 selfdrive/car/hyundai/carcontroller.py | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index eaadfa1800..f393e1935f 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -300,7 +300,6 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        stopping = stopping and CS.out.vEgo < self.CP.vEgoStopping
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
@@ -478,10 +477,10 @@ class CarController(CarControllerBase):
       cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
     return cruise_button
 
-  def cal_jerk(self, CS, accel, actuators):
+  def cal_jerk(self, accel, actuators):
     if actuators.longControlState == LongCtrlState.off:
       accel_diff = 0.0
-    elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:# or hud_control.softHold > 0:
+    elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
       accel_diff = 0.0
     else:
       accel_diff = accel - self.accel_last
@@ -491,7 +490,7 @@ class CarController(CarControllerBase):
     return self.jerk
 
   def make_jerk(self, CS, accel, actuators):
-    jerk = self.cal_jerk(CS, accel, actuators)
+    jerk = self.cal_jerk(accel, actuators)
     a_error = accel - CS.out.aEgo
     jerk = jerk + (a_error * 2.0)
 
@@ -504,7 +503,7 @@ class CarController(CarControllerBase):
         self.jerk_u = jerkLimit
         self.jerk_l = jerkLimit
         self.jerk_count = 0
-      elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
+      elif actuators.longControlState == LongCtrlState.stopping:
         self.jerk_u += 0.1 if self.jerk_u < 1.5 else -0.1
         self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1
         self.jerk_count = 0
@@ -523,7 +522,7 @@ class CarController(CarControllerBase):
         self.jerk_u = jerkLimit
         self.jerk_l = jerkLimit
         self.jerk_count = 0
-      elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
+      elif actuators.longControlState == LongCtrlState.stopping:
         self.jerk_u += 0.1 if self.jerk_u < 0.5 else -0.1
         self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1
         self.jerk_count = 0
@@ -534,12 +533,12 @@ class CarController(CarControllerBase):
         self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
-  def make_accel(self, CS, accel, actuators):
+  def make_accel(self, accel, actuators):
     self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
-      if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:
+      if actuators.longControlState == LongCtrlState.stopping:
         self.accel_raw = 0
-      self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l)
+      self.accel_val = clip(self.accel_raw, self.accel_last - 0.1, self.accel_last + 0.1)
     self.accel_last = self.accel_val

From 807873e6a8cb237cad7cea8e1b091ef2ed536e69 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 10:12:03 -0400
Subject: [PATCH 06/53] fix

---
 selfdrive/car/hyundai/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index f393e1935f..b43079a714 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -287,7 +287,7 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
-        self.make_accel(CS, accel, actuators)
+        self.make_accel(accel, actuators)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:

From d25d83288582337380d37b577c828e0b1ae2bb5f Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 11:29:42 -0400
Subject: [PATCH 07/53] Should run these at 50 Hz

---
 selfdrive/car/hyundai/carcontroller.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index b43079a714..4d2a2902fa 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -287,7 +287,6 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
-        self.make_accel(accel, actuators)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -300,6 +299,8 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
+        self.make_jerk(CS, accel, actuators)
+        self.make_accel(accel, actuators)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))

From 0c95fe140869d4646e66c414005a034f859aa9d7 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 11:46:01 -0400
Subject: [PATCH 08/53] send 2.0 startAccel to both aReqRaw/aReqValue for 0.1
 second (2 frames), then pid

---
 selfdrive/car/hyundai/carcontroller.py | 7 +++++++
 selfdrive/car/hyundai/interface.py     | 2 +-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 4d2a2902fa..a051e13c1d 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -112,6 +112,7 @@ class CarController(CarControllerBase):
 
     self.accel_val = 0
     self.accel_raw = 0
+    self.accel_frame = 0
 
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
     lead_one = self.sm["radarState"].leadOne
@@ -538,8 +539,14 @@ class CarController(CarControllerBase):
     self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
+    elif actuators.longControlState == LongCtrlState.starting and self.accel_frame <= 1:
+      self.accel_frame += 1
+      self.accel_raw, self.accel_val = accel, accel
     else:
       if actuators.longControlState == LongCtrlState.stopping:
         self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - 0.1, self.accel_last + 0.1)
     self.accel_last = self.accel_val
+
+    if not actuators.longControlState == LongCtrlState.starting or self.accel_frame >= 2:
+      self.accel_frame = 0
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index e801b47771..88827e61ab 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -104,7 +104,7 @@ class CarInterface(CarInterfaceBase):
     ret.stoppingControl = True
     ret.startingState = True
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.0
+    ret.startAccel = 2.0
     ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:

From ab72cf0457f6898b6654e44a8cbdc40a82fc8a85 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:14:45 -0400
Subject: [PATCH 09/53] handle this in long control

---
 selfdrive/car/hyundai/carcontroller.py | 17 +++--------------
 selfdrive/car/hyundai/interface.py     |  1 +
 selfdrive/controls/lib/longcontrol.py  | 15 +++++++++++++--
 3 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index a051e13c1d..ee700ffc9c 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -111,8 +111,6 @@ class CarController(CarControllerBase):
     self.jerk_count = 0.0
 
     self.accel_val = 0
-    self.accel_raw = 0
-    self.accel_frame = 0
 
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
     lead_one = self.sm["radarState"].leadOne
@@ -302,7 +300,7 @@ class CarController(CarControllerBase):
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         self.make_jerk(CS, accel, actuators)
         self.make_accel(accel, actuators)
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
 
@@ -536,17 +534,8 @@ class CarController(CarControllerBase):
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
   def make_accel(self, accel, actuators):
-    self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.off:
-      self.accel_raw, self.accel_val = 0, 0
-    elif actuators.longControlState == LongCtrlState.starting and self.accel_frame <= 1:
-      self.accel_frame += 1
-      self.accel_raw, self.accel_val = accel, accel
+      self.accel_val = 0
     else:
-      if actuators.longControlState == LongCtrlState.stopping:
-        self.accel_raw = 0
-      self.accel_val = clip(self.accel_raw, self.accel_last - 0.1, self.accel_last + 0.1)
+      self.accel_val = clip(accel, self.accel_last - 0.1, self.accel_last + 0.1)
     self.accel_last = self.accel_val
-
-    if not actuators.longControlState == LongCtrlState.starting or self.accel_frame >= 2:
-      self.accel_frame = 0
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 88827e61ab..627328c0d9 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -105,6 +105,7 @@ class CarInterface(CarInterfaceBase):
     ret.startingState = True
     ret.vEgoStarting = 0.1
     ret.startAccel = 2.0
+    ret.stopAccel = 0.0
     ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index a661c33361..a477b48780 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -51,6 +51,8 @@ class LongControl:
                              k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
     self.last_output_accel = 0.0
 
+    self.frame = 0
+
   def reset(self):
     self.pid.reset()
 
@@ -74,13 +76,22 @@ class LongControl:
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:
-      output_accel = self.CP.startAccel
-      self.reset()
+      if self.frame < 10 and self.CP.carName == "hyundai":
+        self.frame += 1
+        output_accel = self.CP.startAccel
+        self.reset()
+      else:  # LongCtrlState.pid
+        error = a_target - CS.aEgo
+        output_accel = self.pid.update(error, speed=CS.vEgo,
+                                       feedforward=a_target)
 
     else:  # LongCtrlState.pid
       error = a_target - CS.aEgo
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
 
+    if not self.long_control_state == LongCtrlState.starting:
+      self.frame = 0
+
     self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

From 5e3cf24afee785e7576e1fcaa3256da5962b613c Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:25:51 -0400
Subject: [PATCH 10/53] update rate of change properly

---
 selfdrive/car/hyundai/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index ee700ffc9c..f41a10c841 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -534,8 +534,10 @@ class CarController(CarControllerBase):
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
   def make_accel(self, accel, actuators):
+    rate_up = 0.02 * self.jerk_u
+    rate_down = 0.02 * self.jerk_l
     if actuators.longControlState == LongCtrlState.off:
       self.accel_val = 0
     else:
-      self.accel_val = clip(accel, self.accel_last - 0.1, self.accel_last + 0.1)
+      self.accel_val = clip(accel, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val

From 8191c951c7149cb55afcaccaba6b9f7d50aaf270 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:33:25 -0400
Subject: [PATCH 11/53] oops it's doing it twice

---
 selfdrive/car/hyundai/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index f41a10c841..16994e76ea 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -284,8 +284,8 @@ class CarController(CarControllerBase):
                   self.last_button_frame = self.frame
             elif self.frame % 2 == 0:
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
-      else:
-        self.make_jerk(CS, accel, actuators)
+      #else:
+      #  self.make_jerk(CS, accel, actuators)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:

From e5b19ddc1762a01c48ac4fad0f013d62323fda89 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:46:27 -0400
Subject: [PATCH 12/53] only different rate of change if not in pid

---
 selfdrive/car/hyundai/carcontroller.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 16994e76ea..8b252ad58e 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -534,8 +534,11 @@ class CarController(CarControllerBase):
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
   def make_accel(self, accel, actuators):
-    rate_up = 0.02 * self.jerk_u
-    rate_down = 0.02 * self.jerk_l
+    if actuators.longControlState == LongCtrlState.pid:
+      rate_up, rate_down = 0, 0
+    else:
+      rate_up = 0.02 * self.jerk_u
+      rate_down = 0.02 * self.jerk_l
     if actuators.longControlState == LongCtrlState.off:
       self.accel_val = 0
     else:

From be300df85c858388cc6b5e8b7a33185142299b33 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:53:37 -0400
Subject: [PATCH 13/53] should only go up to 3 lol

---
 selfdrive/car/hyundai/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 8b252ad58e..1a9f8458e5 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -514,7 +514,7 @@ class CarController(CarControllerBase):
         self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit)
     else:
       startingJerk = self.jerkStartLimit
-      jerkLimit = 5.0
+      jerkLimit = 3.0
       self.jerk_count += DT_CTRL
       jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
       self.cb_upper = self.cb_lower = 0

From e64d209f54314f0307e64f27e3f5333d3e002a95 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:54:21 -0400
Subject: [PATCH 14/53] set speed stuck

---
 selfdrive/car/hyundai/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 1a9f8458e5..1374b60e5a 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -535,7 +535,7 @@ class CarController(CarControllerBase):
 
   def make_accel(self, accel, actuators):
     if actuators.longControlState == LongCtrlState.pid:
-      rate_up, rate_down = 0, 0
+      rate_up, rate_down = 0.1, 0.1
     else:
       rate_up = 0.02 * self.jerk_u
       rate_down = 0.02 * self.jerk_l

From 8affb2fcaddb0771a7060f6155457cfc5ee5e463 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 16:56:48 -0400
Subject: [PATCH 15/53] make sure accel_last changes with raw if in pid

---
 selfdrive/car/hyundai/carcontroller.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 1374b60e5a..c51e385d2c 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -535,7 +535,8 @@ class CarController(CarControllerBase):
 
   def make_accel(self, accel, actuators):
     if actuators.longControlState == LongCtrlState.pid:
-      rate_up, rate_down = 0.1, 0.1
+      rate_up, rate_down = 0.0, 0.0
+      self.accel_last = accel
     else:
       rate_up = 0.02 * self.jerk_u
       rate_down = 0.02 * self.jerk_l

From 0d499b0857c65b790989dff5fbb0d157130d02b0 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 21:49:03 -0400
Subject: [PATCH 16/53] doesn't work well

---
 selfdrive/controls/lib/longcontrol.py | 15 ++-------------
 1 file changed, 2 insertions(+), 13 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index a477b48780..a661c33361 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -51,8 +51,6 @@ class LongControl:
                              k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
     self.last_output_accel = 0.0
 
-    self.frame = 0
-
   def reset(self):
     self.pid.reset()
 
@@ -76,22 +74,13 @@ class LongControl:
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:
-      if self.frame < 10 and self.CP.carName == "hyundai":
-        self.frame += 1
-        output_accel = self.CP.startAccel
-        self.reset()
-      else:  # LongCtrlState.pid
-        error = a_target - CS.aEgo
-        output_accel = self.pid.update(error, speed=CS.vEgo,
-                                       feedforward=a_target)
+      output_accel = self.CP.startAccel
+      self.reset()
 
     else:  # LongCtrlState.pid
       error = a_target - CS.aEgo
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
 
-    if not self.long_control_state == LongCtrlState.starting:
-      self.frame = 0
-
     self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

From 58004f4fc65f6999f201a4d2c501e4c23bee072f Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 21:50:31 -0400
Subject: [PATCH 17/53] fix pulsing

---
 selfdrive/car/hyundai/carcontroller.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index c51e385d2c..53aa9b6111 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -284,8 +284,8 @@ class CarController(CarControllerBase):
                   self.last_button_frame = self.frame
             elif self.frame % 2 == 0:
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
-      #else:
-      #  self.make_jerk(CS, accel, actuators)
+      else:
+        self.make_jerk(CS, accel, actuators)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -298,7 +298,6 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        self.make_jerk(CS, accel, actuators)
         self.make_accel(accel, actuators)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,

From f64193924044f741e5779bc47b10c2a5b8206d0f Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 23:19:58 -0400
Subject: [PATCH 18/53] trigger stop earlier

---
 selfdrive/car/hyundai/interface.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 627328c0d9..87408a738b 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -103,6 +103,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.stoppingControl = True
     ret.startingState = True
+    ret.vEgoStopping = 1.0
     ret.vEgoStarting = 0.1
     ret.startAccel = 2.0
     ret.stopAccel = 0.0

From 7f06773c26f1a072efad53a061ffb798510d0ae4 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Jul 2024 23:22:33 -0400
Subject: [PATCH 19/53] higher jerk limits

---
 selfdrive/car/hyundai/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 53aa9b6111..028bc796d6 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -513,7 +513,7 @@ class CarController(CarControllerBase):
         self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit)
     else:
       startingJerk = self.jerkStartLimit
-      jerkLimit = 3.0
+      jerkLimit = 5.0
       self.jerk_count += DT_CTRL
       jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
       self.cb_upper = self.cb_lower = 0

From 7560d8db9e44938380b07c428c515d8a3dfd3f10 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 11:11:00 -0400
Subject: [PATCH 20/53] do stopping accel in carcontroller

---
 selfdrive/car/hyundai/carcontroller.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 028bc796d6..a345c53762 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -110,6 +110,7 @@ class CarController(CarControllerBase):
     self.cb_lower = 0.0
     self.jerk_count = 0.0
 
+    self.accel_raw = 0
     self.accel_val = 0
 
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
@@ -299,7 +300,7 @@ class CarController(CarControllerBase):
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         self.make_accel(accel, actuators)
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
 
@@ -533,14 +534,14 @@ class CarController(CarControllerBase):
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
   def make_accel(self, accel, actuators):
+    self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.pid:
-      rate_up, rate_down = 0.0, 0.0
-      self.accel_last = accel
+      rate_up, rate_down = 0.1, 0.1
     else:
       rate_up = 0.02 * self.jerk_u
       rate_down = 0.02 * self.jerk_l
     if actuators.longControlState == LongCtrlState.off:
-      self.accel_val = 0
+      self.accel_raw, self.accel_val = 0, 0
     else:
-      self.accel_val = clip(accel, self.accel_last - rate_down, self.accel_last + rate_up)
+      self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val

From 8ec69008ce2158536406cb78007e32e165881d14 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 11:11:12 -0400
Subject: [PATCH 21/53] lower starAccel

---
 selfdrive/car/hyundai/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 87408a738b..9e618c3f6e 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -105,7 +105,7 @@ class CarInterface(CarInterfaceBase):
     ret.startingState = True
     ret.vEgoStopping = 1.0
     ret.vEgoStarting = 0.1
-    ret.startAccel = 2.0
+    ret.startAccel = 1.0
     ret.stopAccel = 0.0
     ret.longitudinalActuatorDelay = 0.5
 

From c02b3fca53b315ff88789db62862e98b538ce4a7 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 11:32:42 -0400
Subject: [PATCH 22/53] fast resume: disengage acc enabled for 1 frame, then
 re-engage

---
 selfdrive/car/hyundai/carcontroller.py | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index a345c53762..52c1544ab1 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -113,6 +113,9 @@ class CarController(CarControllerBase):
     self.accel_raw = 0
     self.accel_val = 0
 
+    self.acc_enabled = False
+    self.prev_stop_req = False
+
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
     lead_one = self.sm["radarState"].leadOne
     lead_two = self.sm["radarState"].leadTwo
@@ -300,7 +303,8 @@ class CarController(CarControllerBase):
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         self.make_accel(accel, actuators)
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping)
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, self.acc_enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
 
@@ -545,3 +549,11 @@ class CarController(CarControllerBase):
     else:
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val
+
+  def fast_resume(self, actuators, enabled, stopping):
+    if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req:
+      self.acc_enabled = False
+    else:
+      self.acc_enabled = enabled
+
+    self.prev_stop_req = stopping

From b20ce279086b0404fce1d821ce1af35e4976bd09 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 11:40:40 -0400
Subject: [PATCH 23/53] matches stock with 0 m/s^2 at stopping

---
 selfdrive/car/hyundai/carcontroller.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 52c1544ab1..f3e7fa4c3f 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -538,15 +538,18 @@ class CarController(CarControllerBase):
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
   def make_accel(self, accel, actuators):
+    long_control = actuators.longControlState
     self.accel_raw = accel
-    if actuators.longControlState == LongCtrlState.pid:
+    if long_control == LongCtrlState.pid:
       rate_up, rate_down = 0.1, 0.1
     else:
       rate_up = 0.02 * self.jerk_u
       rate_down = 0.02 * self.jerk_l
-    if actuators.longControlState == LongCtrlState.off:
+    if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
+      if long_control == LongCtrlState.stopping:
+        self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val
 

From 68149b71c494c15a341d81810c9fbd4c414e9f43 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 11:41:35 -0400
Subject: [PATCH 24/53] gracefully when stopping

---
 selfdrive/car/hyundai/carcontroller.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index f3e7fa4c3f..56c9bae40e 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -540,11 +540,11 @@ class CarController(CarControllerBase):
   def make_accel(self, accel, actuators):
     long_control = actuators.longControlState
     self.accel_raw = accel
-    if long_control == LongCtrlState.pid:
-      rate_up, rate_down = 0.1, 0.1
-    else:
+    if long_control == LongCtrlState.stopping:
       rate_up = 0.02 * self.jerk_u
       rate_down = 0.02 * self.jerk_l
+    else:
+      rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:

From 63754678d137a6f65aeb5400155b9bdeac6053b3 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:01:42 -0400
Subject: [PATCH 25/53] 0.1 rate of change all around

---
 selfdrive/car/hyundai/carcontroller.py | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 56c9bae40e..0be27a3523 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -540,11 +540,7 @@ class CarController(CarControllerBase):
   def make_accel(self, accel, actuators):
     long_control = actuators.longControlState
     self.accel_raw = accel
-    if long_control == LongCtrlState.stopping:
-      rate_up = 0.02 * self.jerk_u
-      rate_down = 0.02 * self.jerk_l
-    else:
-      rate_up, rate_down = 0.1, 0.1
+    rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:

From 2507ec7abf6a03b1b2154200f29f0b47e3bc6658 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:12:18 -0400
Subject: [PATCH 26/53] same vEgoStopping, higher startAccel

---
 selfdrive/car/hyundai/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 9e618c3f6e..8b800be0e5 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -103,9 +103,9 @@ class CarInterface(CarInterfaceBase):
 
     ret.stoppingControl = True
     ret.startingState = True
-    ret.vEgoStopping = 1.0
+    ret.vEgoStopping = 0.5
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.0
+    ret.startAccel = 1.8
     ret.stopAccel = 0.0
     ret.longitudinalActuatorDelay = 0.5
 

From 63a61783607d33f75d2999e5e2885c5cba3a848c Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:14:09 -0400
Subject: [PATCH 27/53] faster stops

---
 selfdrive/car/hyundai/interface.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 8b800be0e5..ca96df01b5 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -107,6 +107,7 @@ class CarInterface(CarInterfaceBase):
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
     ret.stopAccel = 0.0
+    ret.stoppingDecelRate = 2.0  # reach standstill quickly after entering stopping state
     ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:

From 6ab66834f02b9314095e130f576204aadea1e166 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:54:47 -0400
Subject: [PATCH 28/53] adapt to 0 stopAccel

---
 selfdrive/controls/lib/longcontrol.py | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index a661c33361..12aac4487c 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -68,9 +68,12 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if output_accel > self.CP.stopAccel:
-        output_accel = min(output_accel, 0.0)
-        output_accel -= self.CP.stoppingDecelRate * DT_CTRL
+      if output_accel < self.CP.stopAccel:
+        output_accel = max(output_accel, 0.0)
+        output_accel += self.CP.stoppingDecelRate * DT_CTRL
+      #if output_accel > self.CP.stopAccel:
+      #  output_accel = min(output_accel, 0.0)
+      #  output_accel -= self.CP.stoppingDecelRate * DT_CTRL
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:

From 067bd2b8fd0734f1f79fa55045f4eadf11ff5273 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:55:49 -0400
Subject: [PATCH 29/53] less if/else

---
 selfdrive/car/hyundai/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 0be27a3523..4d70930f21 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -550,9 +550,9 @@ class CarController(CarControllerBase):
     self.accel_last = self.accel_val
 
   def fast_resume(self, actuators, enabled, stopping):
+    self.acc_enabled = enabled
+
     if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req:
       self.acc_enabled = False
-    else:
-      self.acc_enabled = enabled
 
     self.prev_stop_req = stopping

From b95933d6527163667db9c54ad0c90a0a9fa0519a Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 17:56:22 -0400
Subject: [PATCH 30/53] disable fast stop attempt for now

---
 selfdrive/car/hyundai/carcontroller.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 4d70930f21..4ff26ebdf7 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -551,6 +551,7 @@ class CarController(CarControllerBase):
 
   def fast_resume(self, actuators, enabled, stopping):
     self.acc_enabled = enabled
+    return
 
     if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req:
       self.acc_enabled = False

From 63b86a80a6f1e631f11471727c72ab694dad92fe Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 18:18:35 -0400
Subject: [PATCH 31/53] handle stopaccel in long control

---
 selfdrive/car/hyundai/carcontroller.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 4ff26ebdf7..5565c1c4b2 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -544,8 +544,6 @@ class CarController(CarControllerBase):
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
-      if long_control == LongCtrlState.stopping:
-        self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val
 

From fd48800940f1279b7c23a30dfa20a05c6cd1b374 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 18:31:33 -0400
Subject: [PATCH 32/53] try disengaging for 250 ms

---
 selfdrive/car/hyundai/carcontroller.py | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 5565c1c4b2..7810e5e895 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -114,6 +114,8 @@ class CarController(CarControllerBase):
     self.accel_val = 0
 
     self.acc_enabled = False
+    self.accel_start = False
+    self.accel_frame = 0
     self.prev_stop_req = False
 
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
@@ -290,6 +292,7 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
+        self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -303,7 +306,6 @@ class CarController(CarControllerBase):
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         self.make_accel(accel, actuators)
-        self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, self.acc_enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
@@ -549,9 +551,19 @@ class CarController(CarControllerBase):
 
   def fast_resume(self, actuators, enabled, stopping):
     self.acc_enabled = enabled
-    return
 
-    if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req:
+    if actuators.longControlState == LongCtrlState.off:
+      return
+
+    if actuators.longControlState == LongCtrlState.starting and self.prev_stop_req:
+      self.accel_frame = 0
+      self.accel_start = True
+
+    if self.accel_start and self.accel_frame < 25:
+      self.accel_frame += 1
       self.acc_enabled = False
+    else:
+      self.accel_frame = 0
+      self.accel_start = False
 
     self.prev_stop_req = stopping

From 80c1aa0f0c1f59c95284bdc414d9a5ee4ae2b467 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 19:04:39 -0400
Subject: [PATCH 33/53] only do rate of change in pid mode

---
 selfdrive/car/hyundai/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 7810e5e895..34cd3c31e9 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -545,8 +545,10 @@ class CarController(CarControllerBase):
     rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
-    else:
+    elif long_control == LongCtrlState.pid:
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
+    else:
+      self.accel_val = accel
     self.accel_last = self.accel_val
 
   def fast_resume(self, actuators, enabled, stopping):

From 85f726298cd13608010c85dffa90e6b947484f95 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 21:49:16 -0400
Subject: [PATCH 34/53] simplify

---
 selfdrive/car/hyundai/carcontroller.py | 31 ++------------------------
 selfdrive/car/hyundai/interface.py     |  2 --
 2 files changed, 2 insertions(+), 31 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 34cd3c31e9..9f3734bba1 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -113,11 +113,6 @@ class CarController(CarControllerBase):
     self.accel_raw = 0
     self.accel_val = 0
 
-    self.acc_enabled = False
-    self.accel_start = False
-    self.accel_frame = 0
-    self.prev_stop_req = False
-
   def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
     lead_one = self.sm["radarState"].leadOne
     lead_two = self.sm["radarState"].leadTwo
@@ -292,7 +287,6 @@ class CarController(CarControllerBase):
               can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
       else:
         self.make_jerk(CS, accel, actuators)
-        self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping)
 
       # Parse lead distance from radarState and display the corresponding distance in the car's cluster
       if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
@@ -306,7 +300,7 @@ class CarController(CarControllerBase):
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         self.make_accel(accel, actuators)
-        can_sends.extend(hyundaican.create_acc_commands(self.packer, self.acc_enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
 
@@ -545,27 +539,6 @@ class CarController(CarControllerBase):
     rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
-    elif long_control == LongCtrlState.pid:
+    else:
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
-    else:
-      self.accel_val = accel
     self.accel_last = self.accel_val
-
-  def fast_resume(self, actuators, enabled, stopping):
-    self.acc_enabled = enabled
-
-    if actuators.longControlState == LongCtrlState.off:
-      return
-
-    if actuators.longControlState == LongCtrlState.starting and self.prev_stop_req:
-      self.accel_frame = 0
-      self.accel_start = True
-
-    if self.accel_start and self.accel_frame < 25:
-      self.accel_frame += 1
-      self.acc_enabled = False
-    else:
-      self.accel_frame = 0
-      self.accel_start = False
-
-    self.prev_stop_req = stopping
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index ca96df01b5..33b6da95fd 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -103,10 +103,8 @@ class CarInterface(CarInterfaceBase):
 
     ret.stoppingControl = True
     ret.startingState = True
-    ret.vEgoStopping = 0.5
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
-    ret.stopAccel = 0.0
     ret.stoppingDecelRate = 2.0  # reach standstill quickly after entering stopping state
     ret.longitudinalActuatorDelay = 0.5
 

From 597dadd45b6b2514b971161eaa10c51c7f54250a Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 21:50:50 -0400
Subject: [PATCH 35/53] revert

---
 selfdrive/controls/lib/longcontrol.py | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 12aac4487c..a661c33361 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -68,12 +68,9 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if output_accel < self.CP.stopAccel:
-        output_accel = max(output_accel, 0.0)
-        output_accel += self.CP.stoppingDecelRate * DT_CTRL
-      #if output_accel > self.CP.stopAccel:
-      #  output_accel = min(output_accel, 0.0)
-      #  output_accel -= self.CP.stoppingDecelRate * DT_CTRL
+      if output_accel > self.CP.stopAccel:
+        output_accel = min(output_accel, 0.0)
+        output_accel -= self.CP.stoppingDecelRate * DT_CTRL
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:

From a3e60f92cf1bb966a5757d25177f36c5e7e26ef7 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 21:53:35 -0400
Subject: [PATCH 36/53] only send 0 when fully stopped

---
 selfdrive/car/hyundai/carcontroller.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 9f3734bba1..120be3d1ef 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -299,7 +299,7 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        self.make_accel(accel, actuators)
+        self.make_accel(CS, accel, actuators)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
@@ -533,12 +533,14 @@ class CarController(CarControllerBase):
         self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
-  def make_accel(self, accel, actuators):
+  def make_accel(self, CS, accel, actuators):
     long_control = actuators.longControlState
     self.accel_raw = accel
     rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
+      if long_control == LongCtrlState.stopping and CS.out.standstill:
+        self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val

From 3f25d6f5308ec9cbe5b287b65f2929520302e872 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 23:28:49 -0400
Subject: [PATCH 37/53] disable starting state

---
 selfdrive/car/hyundai/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 33b6da95fd..a80b3199b0 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -102,9 +102,9 @@ class CarInterface(CarInterfaceBase):
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.stoppingControl = True
-    ret.startingState = True
+    ret.startingState = False
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.8
+    ret.startAccel = 1.0
     ret.stoppingDecelRate = 2.0  # reach standstill quickly after entering stopping state
     ret.longitudinalActuatorDelay = 0.5
 

From c288a7792ce8ddfa47ac98fa4ec3f6708606fea3 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 23 Jul 2024 23:30:01 -0400
Subject: [PATCH 38/53] super fast take off!!!

---
 .../lib/longitudinal_mpc_lib/long_mpc.py      | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index a2df527a55..0762e204ee 100755
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -105,6 +105,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
 
+def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
+  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
+  # away, resulting in an early demand for acceleration.
+  v_diff_offset = 0
+  if np.all(v_lead - v_ego > 0):
+    v_diff_offset = ((v_lead - v_ego) * 1.)
+    v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
+    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
+  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
+  return distance
+
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
@@ -302,11 +313,11 @@ class LongitudinalMpc:
   def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
     jerk_factor = get_jerk_factor(personality)
     if self.mode == 'acc':
-      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
+      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40
       cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     elif self.mode == 'blended':
-      a_change_cost = 40.0 if prev_accel_constraint else 0
+      a_change_cost = 40.0 if prev_accel_constraint else 40
       cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
     else:
@@ -369,8 +380,8 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
 
     cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
     e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])

From 2cc53b6e6972c6a86aef32bdc6004f4179538713 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 24 Jul 2024 16:54:27 -0400
Subject: [PATCH 39/53] Revert "disable starting state"

This reverts commit 3f25d6f5308ec9cbe5b287b65f2929520302e872.
---
 selfdrive/car/hyundai/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index a80b3199b0..33b6da95fd 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -102,9 +102,9 @@ class CarInterface(CarInterfaceBase):
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.stoppingControl = True
-    ret.startingState = False
+    ret.startingState = True
     ret.vEgoStarting = 0.1
-    ret.startAccel = 1.0
+    ret.startAccel = 1.8
     ret.stoppingDecelRate = 2.0  # reach standstill quickly after entering stopping state
     ret.longitudinalActuatorDelay = 0.5
 

From e0414b452833e5cc2e6a844e6e233b9caa3ec996 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 24 Jul 2024 16:57:42 -0400
Subject: [PATCH 40/53] 0.1 interval to stopping

---
 selfdrive/car/hyundai/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 33b6da95fd..cb722d875d 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -105,7 +105,7 @@ class CarInterface(CarInterfaceBase):
     ret.startingState = True
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
-    ret.stoppingDecelRate = 2.0  # reach standstill quickly after entering stopping state
+    ret.stoppingDecelRate = 10.0  # reach standstill quickly after entering stopping state
     ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:

From 1b084d7eb9dff29f26420361731c0d455e9de42d Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 24 Jul 2024 21:22:40 -0400
Subject: [PATCH 41/53] revert long mpc

---
 .../lib/longitudinal_mpc_lib/long_mpc.py      | 19 ++++---------------
 1 file changed, 4 insertions(+), 15 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 0762e204ee..a2df527a55 100755
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -105,17 +105,6 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
 
-def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
-  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
-  # away, resulting in an early demand for acceleration.
-  v_diff_offset = 0
-  if np.all(v_lead - v_ego > 0):
-    v_diff_offset = ((v_lead - v_ego) * 1.)
-    v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
-    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
-  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
-  return distance
-
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
@@ -313,11 +302,11 @@ class LongitudinalMpc:
   def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
     jerk_factor = get_jerk_factor(personality)
     if self.mode == 'acc':
-      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40
+      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
       cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     elif self.mode == 'blended':
-      a_change_cost = 40.0 if prev_accel_constraint else 40
+      a_change_cost = 40.0 if prev_accel_constraint else 0
       cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
     else:
@@ -380,8 +369,8 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
     cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
     e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])

From bf31b2ab33c51d9afa205851cfcfe4084adb00a0 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 24 Jul 2024 21:22:58 -0400
Subject: [PATCH 42/53] further stop distance

---
 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index a2df527a55..7296a28b11 100755
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 6.0
+STOP_DISTANCE = 8.0
 
 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:

From a2eb4545da7228885ff09f593cb5d3d93be3adde Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 13:54:38 -0400
Subject: [PATCH 43/53] a lot less delay

---
 selfdrive/car/hyundai/interface.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index cb722d875d..b1226440bb 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -106,7 +106,6 @@ class CarInterface(CarInterfaceBase):
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
     ret.stoppingDecelRate = 10.0  # reach standstill quickly after entering stopping state
-    ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:
       if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):

From 2038d9b469401ea7a2708aa5b6994595affc5baa Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 15:18:34 -0400
Subject: [PATCH 44/53] send 0 at stop req

---
 selfdrive/car/hyundai/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 120be3d1ef..70fb05f0d8 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -540,7 +540,7 @@ class CarController(CarControllerBase):
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
-      if long_control == LongCtrlState.stopping and CS.out.standstill:
+      if long_control == LongCtrlState.stopping:
         self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val

From 7eee21f293589802d32712fb135813c0d90f41ea Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 16:12:09 -0400
Subject: [PATCH 45/53] add back delay

---
 selfdrive/car/hyundai/interface.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index b1226440bb..07a500eb41 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -105,6 +105,7 @@ class CarInterface(CarInterfaceBase):
     ret.startingState = True
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
+    ret.longitudinalActuatorDelay = 0.5
     ret.stoppingDecelRate = 10.0  # reach standstill quickly after entering stopping state
 
     if DBC[ret.carFingerprint]["radar"] is None:

From d4096d62343e0ba92cab4ec50c9a0e4d4974bc5b Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 16:26:30 -0400
Subject: [PATCH 46/53] not really needed

---
 selfdrive/car/hyundai/interface.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 07a500eb41..0a4076ebae 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -106,7 +106,6 @@ class CarInterface(CarInterfaceBase):
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
     ret.longitudinalActuatorDelay = 0.5
-    ret.stoppingDecelRate = 10.0  # reach standstill quickly after entering stopping state
 
     if DBC[ret.carFingerprint]["radar"] is None:
       if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):

From c943126d26866ee4fb0fb1ae7066d784f419e3b2 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 16:32:52 -0400
Subject: [PATCH 47/53] send 0 to car when stopping

---
 selfdrive/car/hyundai/carcontroller.py | 10 ++++------
 selfdrive/controls/lib/longcontrol.py  |  4 +++-
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 70fb05f0d8..d1d0c40ac5 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -299,7 +299,7 @@ class CarController(CarControllerBase):
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
-        self.make_accel(CS, accel, actuators)
+        self.make_accel(actuators)
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
                                                         CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
@@ -478,12 +478,13 @@ class CarController(CarControllerBase):
     return cruise_button
 
   def cal_jerk(self, accel, actuators):
+    self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.off:
       accel_diff = 0.0
     elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
       accel_diff = 0.0
     else:
-      accel_diff = accel - self.accel_last
+      accel_diff = self.accel_raw - self.accel_last
 
     accel_diff /= DT_CTRL
     self.jerk = self.jerk * 0.9 + accel_diff * 0.1
@@ -533,14 +534,11 @@ class CarController(CarControllerBase):
         self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
         self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
 
-  def make_accel(self, CS, accel, actuators):
+  def make_accel(self, actuators):
     long_control = actuators.longControlState
-    self.accel_raw = accel
     rate_up, rate_down = 0.1, 0.1
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:
-      if long_control == LongCtrlState.stopping:
-        self.accel_raw = 0
       self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
     self.accel_last = self.accel_val
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index a661c33361..085320db0a 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -68,7 +68,9 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if output_accel > self.CP.stopAccel:
+      if self.CP.stoppingControl:
+        output_accel = 0.0
+      elif output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= self.CP.stoppingDecelRate * DT_CTRL
       self.reset()

From a33d4e51c7b7587b971026d5b48c7b3b230e680b Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Thu, 25 Jul 2024 16:44:54 -0400
Subject: [PATCH 48/53] use jerk limit to change accel to car

---
 selfdrive/car/hyundai/carcontroller.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index d1d0c40ac5..44c7135fcc 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -536,7 +536,8 @@ class CarController(CarControllerBase):
 
   def make_accel(self, actuators):
     long_control = actuators.longControlState
-    rate_up, rate_down = 0.1, 0.1
+    rate_up = 0.1 * self.jerk_u
+    rate_down = 0.1 * self.jerk_l
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:

From 72a0142bf97d07895254817e2c26291c66218df3 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Fri, 26 Jul 2024 09:52:30 -0400
Subject: [PATCH 49/53] do this at the car level

---
 selfdrive/car/hyundai/interface.py    | 2 ++
 selfdrive/controls/lib/longcontrol.py | 4 +---
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 0a4076ebae..5b284abdcb 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -105,6 +105,8 @@ class CarInterface(CarInterfaceBase):
     ret.startingState = True
     ret.vEgoStarting = 0.1
     ret.startAccel = 1.8
+    ret.stopAccel = 0.0
+    ret.stoppingDecelRate = 10
     ret.longitudinalActuatorDelay = 0.5
 
     if DBC[ret.carFingerprint]["radar"] is None:
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 085320db0a..a661c33361 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -68,9 +68,7 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if self.CP.stoppingControl:
-        output_accel = 0.0
-      elif output_accel > self.CP.stopAccel:
+      if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= self.CP.stoppingDecelRate * DT_CTRL
       self.reset()

From 981d150a3b75b0395a5cd55e3e7a13fb93e345d4 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Fri, 26 Jul 2024 10:13:06 -0400
Subject: [PATCH 50/53] different rate of change for ICE, HEV, PHEV, EV

---
 selfdrive/car/hyundai/carcontroller.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 44c7135fcc..bd84c773d7 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -536,8 +536,9 @@ class CarController(CarControllerBase):
 
   def make_accel(self, actuators):
     long_control = actuators.longControlState
-    rate_up = 0.1 * self.jerk_u
-    rate_down = 0.1 * self.jerk_l
+    is_ice = not self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV)
+    rate_up = 0.1 * 1 if is_ice else self.jerk_u
+    rate_down = 0.1 * 1 if is_ice else self.jerk_l
     if long_control == LongCtrlState.off:
       self.accel_raw, self.accel_val = 0, 0
     else:

From 796e6ae92416f1c856ccdaf177f7985a43cfcd18 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Fri, 26 Jul 2024 23:59:40 -0400
Subject: [PATCH 51/53] SCC "Fast" mode

---
 selfdrive/car/hyundai/hyundaican.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index 58b05d708a..200e1a9682 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -207,7 +207,7 @@ def create_acc_opt(packer, escc, CS, CP):
   commands = []
 
   scc13_values = {
-    "SCCDrvModeRValue": 2,
+    "SCCDrvModeRValue": 3,
     "SCC_Equip": 1,
     "Lead_Veh_Dep_Alert_USM": 2,
   }

From 348cd735f2ab0f0effbdc0b892c974535a054323 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sat, 27 Jul 2024 00:25:37 -0400
Subject: [PATCH 52/53] handle stopAccel = 0

---
 selfdrive/controls/lib/longcontrol.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index a661c33361..da2f0e302b 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -71,6 +71,9 @@ class LongControl:
       if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= self.CP.stoppingDecelRate * DT_CTRL
+      elif output_accel < self.CP.stopAccel:
+        output_accel = min(output_accel, 0.0)
+        output_accel += self.CP.stoppingDecelRate * DT_CTRL
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:

From 5ba3e90676c3cf6d989aec8a9ec61192b3c9a538 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Sat, 27 Jul 2024 02:03:15 -0400
Subject: [PATCH 53/53] comment

---
 selfdrive/car/hyundai/carcontroller.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index bd84c773d7..858bc0757c 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -477,6 +477,7 @@ class CarController(CarControllerBase):
       cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev)  # MPH/KPH based button presses
     return cruise_button
 
+  # jerk calculations thanks to apilot!
   def cal_jerk(self, accel, actuators):
     self.accel_raw = accel
     if actuators.longControlState == LongCtrlState.off: