diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index e7e68912ab..99e0cf7e45 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/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 diff --git a/openpilot/selfdrive/controls/tests/test_longcontrol.py b/openpilot/selfdrive/controls/tests/test_longcontrol.py index 9469545204..4658c655b6 100644 --- a/openpilot/selfdrive/controls/tests/test_longcontrol.py +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py index 45bbefed0d..c2179bec3d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py +++ b/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]))