From 356d6a79fae187b671b5bf04672a1b2598cf552b Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Wed, 21 Oct 2026 10:46:17 -0700 Subject: [PATCH] Revert "long: taper stopping decel instead of ramping to stopAccel" This reverts commit fb95b72e23763a24677eb369c7bd8ca9e82daa4e. --- .../selfdrive/controls/lib/longcontrol.py | 23 ++---- .../controls/tests/test_longcontrol.py | 78 +------------------ 2 files changed, 6 insertions(+), 95 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 84470ad592..3a20601c48 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -9,11 +9,6 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] LongCtrlState = car.CarControl.Actuators.LongControlState -# Stopping is shaped as a constant-jerk taper so the brake is released as the car comes to rest. -# The old behavior ramped towards CP.stopAccel while still rolling, making the last meter the hardest. -STOPPING_JERK = 2.0 # m/s^3, rate the stopping decel is released at (higher eases off faster) -STOPPING_HOLD_SPEED = 0.02 # m/s, below this command CP.stopAccel to hold the car - def long_control_state_trans(CP_SP, active, long_control_state, should_stop, brake_pressed, cruise_standstill): @@ -56,18 +51,6 @@ class LongControl: def reset(self): self.pid.reset() - def _stopping_accel(self, v_ego: float) -> float: - v_ego = max(float(v_ego), 0.0) - if v_ego < STOPPING_HOLD_SPEED: - return float(self.CP.stopAccel) - - # Decel that reaches zero at standstill with constant jerk, never stronger than what we already have - taper = -np.sqrt(2.0 * STOPPING_JERK * v_ego) - goal = min(0.0, max(self.last_output_accel, taper)) - output_accel = np.clip(goal, self.last_output_accel - STOPPING_JERK * DT_CTRL, - self.last_output_accel + STOPPING_JERK * DT_CTRL) - return float(min(output_accel, 0.0)) - def update(self, active, CS, a_target, should_stop, accel_limits): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] @@ -81,7 +64,11 @@ class LongControl: output_accel = 0. elif self.long_control_state == LongCtrlState.stopping: - output_accel = self._stopping_accel(CS.vEgo) + 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 -= 1.0 * DT_CTRL # m/s^2/s while trying to stop self.reset() else: # LongCtrlState.pid diff --git a/openpilot/selfdrive/controls/tests/test_longcontrol.py b/openpilot/selfdrive/controls/tests/test_longcontrol.py index f7299446c1..9469545204 100644 --- a/openpilot/selfdrive/controls/tests/test_longcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_longcontrol.py @@ -1,10 +1,6 @@ -from opendbc.car.structs import car - -from openpilot.common.realtime import DT_CTRL from openpilot.common.test import OpenpilotTestCase from openpilot.cereal import custom -from openpilot.selfdrive.controls.lib.longcontrol import (LongControl, LongCtrlState, STOPPING_HOLD_SPEED, - STOPPING_JERK, long_control_state_trans) +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans class TestLongControlStateTransition(OpenpilotTestCase): @@ -46,75 +42,3 @@ 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 TestLongControlStopping(OpenpilotTestCase): - def _long_control(self, stop_accel=-2.0, entry_accel=-1.2, v_ego=0.3): - CP = car.CarParams.new_message() - CP.longitudinalTuning.kiBP = [0.0] - CP.longitudinalTuning.kiV = [0.0] - CP.stopAccel = stop_accel - lc = LongControl(CP, custom.CarParamsSP.new_message()) - lc.long_control_state = LongCtrlState.stopping - lc.last_output_accel = entry_accel - - CS = car.CarState.new_message() - CS.vEgo = float(v_ego) - CS.brakePressed = False - CS.cruiseState.standstill = False - return lc, CS - - def _run_stop(self, lc, CS, frames=500): - accels = [] - distance = 0.0 - for _ in range(frames): - accels.append(lc.update(True, CS, 0.0, True, (-4.0, 2.0))) - if CS.vEgo < STOPPING_HOLD_SPEED: - break - distance += CS.vEgo * DT_CTRL - CS.vEgo = float(max(0.0, CS.vEgo + accels[-1] * DT_CTRL)) - return accels, distance - - def test_releases_braking_instead_of_ramping_to_stop_accel(self): - lc, CS = self._long_control(entry_accel=-1.2) - accels, _ = self._run_stop(lc, CS) - # the stop state must ease off the brake, not add more of it as the car slows - assert accels[0] > -1.2 - assert accels[-2] > accels[0] - # and it must actually get gentle before standstill instead of stopping at the entry decel - assert accels[-2] > -0.8 - - def test_never_increases_decel_while_rolling(self): - lc, CS = self._long_control(entry_accel=-1.2) - accels, _ = self._run_stop(lc, CS) - rolling = accels[:-1] - assert all(b >= a - 1e-9 for a, b in zip(rolling[:-1], rolling[1:], strict=False)), rolling - - def test_jerk_is_bounded(self): - lc, CS = self._long_control(entry_accel=-1.5) - accels, _ = self._run_stop(lc, CS) - rolling = accels[:-1] - max_step = max(abs(b - a) for a, b in zip(rolling[:-1], rolling[1:], strict=False)) - assert max_step <= STOPPING_JERK * DT_CTRL + 1e-9 - - def test_holds_with_stop_accel_at_standstill(self): - lc, CS = self._long_control(stop_accel=-2.0) - accels, _ = self._run_stop(lc, CS) - assert CS.vEgo < STOPPING_HOLD_SPEED - assert accels[-1] == -2.0 - - def test_weak_entry_does_not_add_braking(self): - lc, CS = self._long_control(entry_accel=-0.2) - accels, _ = self._run_stop(lc, CS) - assert min(accels[:-1]) >= -0.2 - 1e-9 - - def test_never_commands_positive_accel(self): - lc, CS = self._long_control(entry_accel=0.5) - accels, _ = self._run_stop(lc, CS) - assert max(accels) <= 0.0 - - def test_stops_within_short_distance(self): - lc, CS = self._long_control(entry_accel=-1.2) - _, distance = self._run_stop(lc, CS) - assert CS.vEgo < STOPPING_HOLD_SPEED - assert distance < 0.25