refactor: improve stopping acceleration logic in LongControlSP

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -69,7 +69,7 @@ class LongControl(LongControlSP):
       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 -= np.interp(CS.vEgo, [0.0, 1.0, 3.0], [0.0, 0.25, 0.5]) * DT_CTRL
+        output_accel -= self.stopping_decel_rate(CS.vEgo) * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,6 +1,9 @@
+from types import SimpleNamespace
+
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import custom
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -42,3 +45,21 @@ class TestLongControlStateTransition(OpenpilotTestCase):
     next_state = long_control_state_trans(CP_SP, active, current_state,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.pid
+
+
+class TestStoppingHold:
+  def test_weak_brake_does_not_hold_stopping(self):
+    control = SimpleNamespace(last_output_accel=-0.109)
+    car_state = SimpleNamespace(vEgo=0.277, aEgo=-0.406)
+    assert not LongControlSP.should_hold_stopping(control, car_state, -0.072)
+
+  def test_sufficient_brake_holds_stopping(self):
+    control = SimpleNamespace(last_output_accel=-0.543)
+    car_state = SimpleNamespace(vEgo=0.209, aEgo=-0.534)
+    assert LongControlSP.should_hold_stopping(control, car_state, -0.469)
+
+  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

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

@@ -5,12 +5,21 @@ 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.
 """
 
+import numpy as np
+
 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 = 2.0
 
 
 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
+    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)
+
+  @staticmethod
+  def stopping_decel_rate(v_ego: float) -> float:
+    return float(np.interp(v_ego, [0.0, STOPPED_SPEED], [STOPPED_DECEL_RATE, STOPPING_DECEL_RATE]))