mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-25 20:03:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "long: taper stopping decel instead of ramping to stopAccel"

Browse Source 
This reverts commit fb95b72e23.
This commit is contained in:
rav4kumar
2026-10-21 10:46:17 -07:00
parent 5fb17cf83a
commit 356d6a79fa
2 changed files with 6 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longcontrol.py
+5 -18
View File
@@ -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
openpilot/selfdrive/controls/tests/test_longcontrol.py
+1 -77
View File
@@ -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