refactor: add rate limiting for stop release

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -58,9 +58,16 @@ class LongControl(LongControlSP):
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
+    previous_state = self.long_control_state
     self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
+
+    if not self.should_exit_stopping(previous_state == LongCtrlState.stopping,
+                                     self.long_control_state == LongCtrlState.pid,
+                                     a_target):
+      self.long_control_state = LongCtrlState.stopping
+
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
@@ -78,5 +85,8 @@ class LongControl(LongControlSP):
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
 
+      if previous_state == LongCtrlState.stopping:
+        output_accel = self.limit_stop_release(self.last_output_accel, output_accel)
+
     self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -61,5 +61,17 @@ class TestStoppingHold:
   def test_stopping_decel_rate_is_smooth_while_rolling(self):
     assert LongControlSP.stopping_decel_rate(0.1) == 0.3
 
-  def test_stopping_decel_rate_builds_holding_brake_at_stop(self):
-    assert LongControlSP.stopping_decel_rate(0.0) == 2.0
+  def test_stopping_decel_rate_remains_gentle_at_stop(self):
+    assert LongControlSP.stopping_decel_rate(0.0) == 0.05
+
+  def test_stop_exit_waits_for_real_start_request(self):
+    assert not LongControlSP.should_exit_stopping(True, True, 0.1)
+    assert LongControlSP.should_exit_stopping(True, True, 0.101)
+
+  def test_stop_release_is_rate_limited(self):
+    last_accel = -0.67
+    output_accel = LongControlSP.limit_stop_release(last_accel, 0.31)
+    assert abs(output_accel - (-0.667)) < 1e-9
+
+  def test_stop_release_does_not_limit_stronger_braking(self):
+    assert LongControlSP.limit_stop_release(-0.67, -0.8) == -0.8

openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py

@@ -7,15 +7,28 @@ See the LICENSE.md file in the root directory for more details.
 
 import numpy as np
 
+from openpilot.common.realtime import DT_CTRL
+
 STOPPING_DISTANCE = 1.5
 STOPPED_SPEED = 0.02
 STOPPING_TIME = 2.5
 MIN_STOPPING_HOLD_ACCEL = -0.2
 STOPPING_DECEL_RATE = 0.3
 STOPPED_DECEL_RATE = 0.05
+STOP_EXIT_ACCEL = 0.1
 
 
 class LongControlSP:
+  @staticmethod
+  def should_exit_stopping(was_stopping: bool, is_pid: bool, a_target: float) -> bool:
+    return not (was_stopping and is_pid and a_target <= STOP_EXIT_ACCEL)
+
+  @staticmethod
+  def limit_stop_release(last_output_accel: float, output_accel: float) -> float:
+    if last_output_accel < 0.0 and output_accel > last_output_accel:
+      return min(output_accel, last_output_accel + STOPPING_DECEL_RATE * DT_CTRL)
+    return output_accel
+
   def should_hold_stopping(self, CS, a_target: float) -> bool:
     return (self.last_output_accel <= MIN_STOPPING_HOLD_ACCEL 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)