From 8c5eb73674f86547486d7b4b8eeffa503ee81305 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Thu, 6 Aug 2026 10:31:52 -0700
Subject: [PATCH] refine stopping acceleration calculation in longcontrol.py

Update longcontrol.py

smooth
---
 openpilot/selfdrive/controls/lib/longcontrol.py  |  7 ++++---
 .../selfdrive/controls/lib/longcontrol.py        | 16 ++++++++++++++++
 2 files changed, 20 insertions(+), 3 deletions(-)
 create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py

diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py
index 3a20601c48..e7e68912ab 100644
--- a/openpilot/selfdrive/controls/lib/longcontrol.py
+++ b/openpilot/selfdrive/controls/lib/longcontrol.py
@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -39,7 +40,7 @@ def long_control_state_trans(CP_SP, active, long_control_state,
 
   return long_control_state
 
-class LongControl:
+class LongControl(LongControlSP):
   def __init__(self, CP, CP_SP):
     self.CP = CP
     self.CP_SP = CP_SP
@@ -65,10 +66,10 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if output_accel > self.CP.stopAccel:
+      if output_accel > self.CP.stopAccel and not LongControlSP.should_hold_stopping(self, CS, a_target):
         output_accel = min(output_accel, 0.0)
         # TODO: can we just go straight to stopAccel?
-        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
+        output_accel -= np.interp(CS.vEgo, [0.0, 1.0, 3.0], [0.0, 0.25, 0.5]) * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py
new file mode 100644
index 0000000000..45bbefed0d
--- /dev/null
+++ b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py
@@ -0,0 +1,16 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+STOPPING_DISTANCE = 1.5
+STOPPED_SPEED = 0.02
+STOPPING_TIME = 2.5
+
+
+class LongControlSP:
+  def should_hold_stopping(self, CS, a_target: float) -> bool:
+    return (self.last_output_accel <= 0.0 and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0
+            and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE)