hard code?

Update longcontrol.py

smooth

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,8 @@ 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
@@ -68,7 +70,7 @@ class LongControl:
       if output_accel > self.CP.stopAccel:
         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 -= self.stopping_decel_rate(CS.vEgo) * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,6 +1,10 @@
+from types import SimpleNamespace
+from typing import cast
+
 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 +46,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(cast(LongControlSP, 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(cast(LongControlSP, 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

@@ -0,0 +1,25 @@
+"""
+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.
+"""
+
+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 = 0.05
+
+
+class LongControlSP:
+  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)
+
+  @staticmethod
+  def stopping_decel_rate(v_ego: float) -> float:
+    return float(np.interp(v_ego, [0.0, 0.5, 1.5], [STOPPED_DECEL_RATE, 0.15, STOPPING_DECEL_RATE]))