diff --git a/openpilot/selfdrive/controls/lib/drive_helpers.py b/openpilot/selfdrive/controls/lib/drive_helpers.py index 35ca45e56e..695babb0e8 100644 --- a/openpilot/selfdrive/controls/lib/drive_helpers.py +++ b/openpilot/selfdrive/controls/lib/drive_helpers.py @@ -14,8 +14,11 @@ MAX_LATERAL_JERK = 5.0 # m/s^3 MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 +STOPPING_SPEED = 0.25 # m/s, speed at which the car goes into the stopping state + + def should_stop(v_ego: float, a_target: float) -> bool: - return bool(v_ego < 0.3 and a_target < 0.1) + return bool(v_ego < STOPPING_SPEED and a_target < 0.1) def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 3a20601c48..c5e2aa6567 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -7,6 +7,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +STOPPING_DECEL_RATE = 0.3 # m/s^2/s while trying to stop + LongCtrlState = car.CarControl.Actuators.LongControlState @@ -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 -= STOPPING_DECEL_RATE * 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..2337893544 100644 --- a/openpilot/selfdrive/controls/tests/test_longcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_longcontrol.py @@ -1,6 +1,7 @@ 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.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 class TestLongControlStateTransition(OpenpilotTestCase): @@ -42,3 +43,13 @@ 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 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 + # 0.3 / 0.25, and every stop recorded on this car was driven with that pair. Both must stay on the less + # braking side, or a future edit re-deepens the terminal brake unnoticed - which already happened once. + assert 0.0 < STOPPING_DECEL_RATE <= 1.0 + 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