From db5c1306af247715594680fcd7b86b002c1eb5a3 Mon Sep 17 00:00:00 2001
From: openpilot <5604101+axi82@user.noreply.gitee.com>
Date: Tue, 24 Mar 2026 21:12:02 +0800
Subject: [PATCH] =?UTF-8?q?longcontrol.py:=E5=A2=9E=E5=8A=A0jerk=E7=9A=84?=
 =?UTF-8?q?=E9=99=90=E5=88=B6=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 09f145b2..2556d90b 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -81,6 +81,7 @@ class LongControl:
     self.output_accel_init = False
     self.smooth_stop_mode = 0
     self.smooth_stop_disable = False
+    self.jerk_limit = 2.0  # 默认值，可调
 
   def reset(self):
     self.pid.reset()
@@ -166,7 +167,7 @@ class LongControl:
       if self.smooth_stop_disable:
         self.a_ego_curr_init = False
         self.output_accel_init = False
-      elif self.smooth_stop_mode == 2: #模式2
+      elif self.smooth_stop_mode == 2 or self.smooth_stop_mode == 3: #模式2
         alpha = 0.3  # 平滑系数
         if not self.output_accel_init:
           self.output_accel_filtered = output_accel
@@ -225,5 +226,32 @@ class LongControl:
       self.gas_release_smooth_max_last = self.gas_release_smooth_max_cnt
     # new
 
+    # ==========================
+    # NEW: jerk limit
+    # ==========================
+    if self.smooth_stop_mode == 3:
+      if self.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping]:
+        # NEW: 动态 jerk（低速更柔）
+        jerk_limit = np.interp(CS.vEgo,
+                               [0.0, 1.0, 5.0, 15.0],
+                               [0.2, 0.8, 1.5, 3.0])  # m/s^3
+
+        # NEW: 分开加速 / 减速 jerk（更细腻）
+        delta = output_accel - self.last_output_accel
+
+        if delta > 0:
+          jerk_limit_up = jerk_limit * 1.2  # 加速稍快一点
+          max_delta = jerk_limit_up * DT_CTRL
+        else:
+          jerk_limit_down = jerk_limit  # 刹车更平缓
+          max_delta = jerk_limit_down * DT_CTRL
+
+        delta = np.clip(delta, -max_delta, max_delta)
+        output_accel = self.last_output_accel + delta
+
+      if self.long_control_state in [LongCtrlState.off, LongCtrlState.starting]:
+        self.last_output_accel = output_accel
+    # NEW
+
     self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel, a_target_ff, j_target_now