feat(long): improve stopping behavior to prevent hard PID brake

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -66,11 +66,14 @@ class LongControl:
       output_accel = 0.
 
     elif self.long_control_state == LongCtrlState.stopping:
+      # Smoothly move the previous command toward the stationary hold acceleration
+      # in either direction. This prevents a hard PID brake from being carried
+      # unchanged into the terminal stop state or a sudden release at its entry.
       output_accel = self.last_output_accel
       if output_accel > self.CP.stopAccel:
-        output_accel = min(output_accel, 0.0)
-        # TODO: can we just go straight to stopAccel?
-        output_accel -= STOPPING_DECEL_RATE * DT_CTRL
+        output_accel = max(self.CP.stopAccel, min(output_accel, 0.0) - STOPPING_DECEL_RATE * DT_CTRL)
+      elif output_accel < self.CP.stopAccel:
+        output_accel = min(self.CP.stopAccel, output_accel + STOPPING_DECEL_RATE * DT_CTRL)
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,7 +1,9 @@
 from openpilot.common.test import OpenpilotTestCase
+from opendbc.car.structs import car
 from openpilot.cereal import custom
+from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import STOPPING_SPEED, should_stop
-from openpilot.selfdrive.controls.lib.longcontrol import STOPPING_DECEL_RATE, LongCtrlState, long_control_state_trans
+from openpilot.selfdrive.controls.lib.longcontrol import STOPPING_DECEL_RATE, LongControl, LongCtrlState, long_control_state_trans
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -44,6 +46,7 @@ class TestLongControlStateTransition(OpenpilotTestCase):
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.pid
 
+
 class TestTerminalStop(OpenpilotTestCase):
   def test_stopping_tune_is_gentler_than_upstream_default(self):
     # Upstream #38394 hardcoded a 1.0 m/s^2/s ramp and a 0.3 m/s latch. comma's own one-stopping-tune uses
@@ -53,3 +56,22 @@ class TestTerminalStop(OpenpilotTestCase):
     assert 0.0 < STOPPING_SPEED <= 0.3
     assert should_stop(STOPPING_SPEED - 0.01, 0.0)
     assert not should_stop(0.29, 0.0)  # the band upstream would latch in and we do not
+
+  def test_stopping_caps_a_hard_pid_brake(self):
+    cp = car.CarParams.new_message()
+    cp.stopAccel = -2.0
+    cp.longitudinalTuning.kiBP = [0.0]
+    cp.longitudinalTuning.kiV = [0.0]
+    cp_sp = custom.CarParamsSP.new_message()
+    cs = car.CarState.new_message()
+    cs.vEgo = 0.2
+    cs.cruiseState.standstill = False
+
+    long_control = LongControl(cp, cp_sp)
+    long_control.long_control_state = LongCtrlState.pid
+    long_control.last_output_accel = -3.5
+
+    output_accel = long_control.update(True, cs, 0.0, True, (-3.5, 2.0))
+
+    assert output_accel == cp.stopAccel
+    assert output_accel > -3.5