final stop

openpilot/selfdrive/controls/controlsd.py

@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurv
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
+from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
@@ -57,6 +58,7 @@ class Controls(ControlsExt):
     self.calibrated_pose: Pose | None = None
 
     self.LoC = LongControl(self.CP, self.CP_SP)
+    self.stopping_controller = StoppingController(self.CP.stopAccel)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
@@ -135,7 +137,16 @@ class Controls(ControlsExt):
 
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
-    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
+    prev_state = self.LoC.long_control_state
+    prev_accel = self.LoC.last_output_accel
+    accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
+    stock_state = self.LoC.long_control_state
+    self.LoC.long_control_state, accel = self.stopping_controller.update(
+      prev_state, stock_state, CS, long_plan.aTarget, prev_accel, accel, pid_accel_limits, long_plan.hasLead)
+    if self.LoC.long_control_state != stock_state:
+      self.LoC.reset()
+    self.LoC.last_output_accel = accel
+    actuators.accel = float(accel)
 
     # Steering PID loop and lateral MPC
     # Reset desired curvature to current to avoid violating the limits on engage

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,6 +1,22 @@
 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 LongControl, LongCtrlState, long_control_state_trans
+from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController
+
+
+def update_long_control(LoC, StopC, active, CS, a_target, should_stop, accel_limits, has_lead=False):
+  prev_state = LoC.long_control_state
+  prev_accel = LoC.last_output_accel
+  stock_accel = LoC.update(active, CS, a_target, should_stop, accel_limits)
+  stock_state = LoC.long_control_state
+  state, accel = StopC.update(prev_state, stock_state, CS, a_target, prev_accel, stock_accel,
+                              accel_limits, has_lead)
+  if state != stock_state:
+    LoC.reset()
+  LoC.long_control_state = state
+  LoC.last_output_accel = accel
+  return accel
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -42,3 +58,156 @@ 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 TestStoppingController(OpenpilotTestCase):
+  def test_stock_long_control_remains_available(self):
+    from opendbc.car.structs import car
+    CP = car.CarParams.new_message()
+    CP_SP = custom.CarParamsSP.new_message()
+    stock = LongControl(CP, CP_SP)
+    tuned = StoppingController(CP.stopAccel)
+    assert type(stock) is LongControl
+    assert not isinstance(tuned, LongControl)
+
+  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 < StoppingController.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
+
+  def test_standstill_hold_firms_faster_than_the_stopping_ramp(self):
+    # The faster standstill ramp builds the hold without changing the approach-to-stop ramp.
+    assert StoppingController.STANDSTILL_HOLD_RATE >= StoppingController.STOPPING_DECEL_RATE
+    assert 0.5 <= StoppingController.STANDSTILL_HOLD_RATE <= 2.0
+
+  def test_standstill_hold_waits_after_the_standstill_flag(self):
+    """The request is frozen through the stop and for the configured delay after standstill."""
+    from opendbc.car.structs import car
+    from openpilot.common.realtime import DT_CTRL
+    hold_delays = ((True, StoppingController.STANDSTILL_HOLD_DELAY_LEAD),
+                   (False, StoppingController.STANDSTILL_HOLD_DELAY_NO_LEAD))
+    for has_lead, delay in hold_delays:
+      with self.subTest(has_lead=has_lead):
+        CP = car.CarParams.new_message(stopAccel=-1.0)
+        CP_SP = custom.CarParamsSP.new_message()
+        LoC = LongControl(CP, CP_SP)
+        StopC = StoppingController(CP.stopAccel)
+        moving = car.CarState.new_message(vEgo=0.2, standstill=False)
+        still = car.CarState.new_message(vEgo=0.0, standstill=True)
+        LoC.long_control_state = LongCtrlState.stopping
+        LoC.last_output_accel = -0.3
+        # 0.5 s of stopping state before the wheels read zero: nothing changes
+        for _ in range(int(0.5 / DT_CTRL)):
+          a = float(update_long_control(LoC, StopC, True, moving, 0.0, True, (-3.5, 1.5), has_lead=has_lead))
+        assert a == -0.3
+        a = [-0.3]
+        for _ in range(int(2.5 / DT_CTRL)):
+          a.append(float(update_long_control(LoC, StopC, True, still, 0.0, True, (-3.5, 1.5), has_lead=has_lead)))
+        n_delay = int(delay / DT_CTRL)
+        assert a[n_delay - 1] == -0.3                              # still frozen at the end of the wait
+        assert a[n_delay + 5] < -0.3                               # and falling right after it
+        late = (a[n_delay] - a[n_delay + 50]) / (50 * DT_CTRL)
+        assert abs(late - StoppingController.STANDSTILL_HOLD_RATE) < 0.05, late  # fast once the delay has passed
+    assert StoppingController.STANDSTILL_HOLD_DELAY_NO_LEAD > StoppingController.STANDSTILL_HOLD_DELAY_LEAD
+
+  def test_stopping_ramp_resumes_if_the_car_never_stops(self):
+    from opendbc.car.structs import car
+    from openpilot.common.realtime import DT_CTRL
+    CP = car.CarParams.new_message(stopAccel=-1.0)
+    LoC = LongControl(CP, custom.CarParamsSP.new_message())
+    StopC = StoppingController(CP.stopAccel)
+    LoC.long_control_state = LongCtrlState.stopping
+    LoC.last_output_accel = -0.1
+    creeping = car.CarState.new_message(vEgo=0.15, standstill=False)
+    for _ in range(int(StoppingController.STOPPING_FREEZE_MAX / DT_CTRL) - 1):
+      a = float(update_long_control(LoC, StopC, True, creeping, 0.0, True, (-3.5, 1.5)))
+    assert a == -0.1                                           # frozen for the whole grace period
+    for _ in range(int(1.0 / DT_CTRL)):
+      a = float(update_long_control(LoC, StopC, True, creeping, 0.0, True, (-3.5, 1.5)))
+    assert abs((-0.1 - a) - StoppingController.STOPPING_DECEL_RATE) < 0.02  # then the upstream ramp
+
+  def test_standstill_hold_timer_resets_when_the_car_moves(self):
+    from opendbc.car.structs import car
+    CP = car.CarParams.new_message(stopAccel=-1.0)
+    LoC = LongControl(CP, custom.CarParamsSP.new_message())
+    StopC = StoppingController(CP.stopAccel)
+    LoC.long_control_state = LongCtrlState.stopping
+    LoC.last_output_accel = -0.3
+    still = car.CarState.new_message(standstill=True)
+    moving = car.CarState.new_message(standstill=False, vEgo=0.1)
+    for _ in range(40):
+      update_long_control(LoC, StopC, True, still, 0.0, True, (-3.5, 1.5))
+    update_long_control(LoC, StopC, True, moving, 0.0, True, (-3.5, 1.5))
+    assert StopC.standstill_t == 0.0
+
+  def test_one_frame_go_does_not_release_the_hold_at_standstill(self):
+    from opendbc.car.structs import car
+    from openpilot.common.realtime import DT_CTRL
+    CP = car.CarParams.new_message(stopAccel=-1.0)
+    CP.longitudinalTuning.kiBP = [0.0]
+    CP.longitudinalTuning.kiV = [0.0]
+    LoC = LongControl(CP, custom.CarParamsSP.new_message())
+    StopC = StoppingController(CP.stopAccel)
+    still = car.CarState.new_message(vEgo=0.0, standstill=True)
+    LoC.long_control_state = LongCtrlState.stopping
+    LoC.last_output_accel = -1.0
+    for _ in range(200):
+      update_long_control(LoC, StopC, True, still, 0.0, True, (-3.5, 1.5), has_lead=True)
+    a = float(update_long_control(LoC, StopC, True, still, 1.2, False, (-3.5, 1.5), has_lead=True))   # one frame of "go"
+    assert LoC.long_control_state == LongCtrlState.stopping and a <= -0.99, (LoC.long_control_state, a)
+    for _ in range(5):
+      a = float(update_long_control(LoC, StopC, True, still, 0.0, True, (-3.5, 1.5), has_lead=True))  # stop again
+    assert a <= -0.99
+    # a sustained go leaves the hold after the debounce
+    n = 0
+    while LoC.long_control_state == LongCtrlState.stopping and n < 100:
+      update_long_control(LoC, StopC, True, still, 1.2, False, (-3.5, 1.5), has_lead=True); n += 1
+    assert abs(n * DT_CTRL - StoppingController.STOPPING_EXIT_DEBOUNCE) < 0.03, n
+
+  def test_request_keeps_easing_with_the_plan_before_the_wheels_stop(self):
+    from opendbc.car.structs import car
+    CP = car.CarParams.new_message(stopAccel=-1.0)
+    LoC = LongControl(CP, custom.CarParamsSP.new_message())
+    StopC = StoppingController(CP.stopAccel)
+    LoC.long_control_state = LongCtrlState.stopping
+    LoC.last_output_accel = -0.40
+    rolling = car.CarState.new_message(vEgo=0.2, standstill=False)
+    for _ in range(30):
+      a = float(update_long_control(LoC, StopC, True, rolling, -0.20, True, (-3.5, 1.5), has_lead=True))   # plan eases to -0.20
+    assert abs(a + 0.20) < 1e-6, a                                                   # followed (lighter)
+    a = float(update_long_control(LoC, StopC, True, rolling, -0.60, True, (-3.5, 1.5), has_lead=True))     # plan firmer: not followed
+    assert abs(a + 0.20) < 1e-6, a
+    for _ in range(10):
+      a = float(update_long_control(LoC, StopC, True, rolling, 0.0, True, (-3.5, 1.5), has_lead=True))     # plan at 0 (flicker): not followed
+    assert abs(a + 0.20) < 1e-6, a
+    assert StoppingController.STOPPING_FOLLOW_MIN < 0.0
+
+  def test_creep_after_the_stop_adds_brake_gently_then_more(self):
+    from opendbc.car.structs import car
+    from openpilot.common.realtime import DT_CTRL
+    CP = car.CarParams.new_message(stopAccel=-1.0)
+    LoC = LongControl(CP, custom.CarParamsSP.new_message())
+    StopC = StoppingController(CP.stopAccel)
+    LoC.long_control_state = LongCtrlState.stopping
+    LoC.last_output_accel = -0.20
+    still = car.CarState.new_message(vEgo=0.0, standstill=True)
+    creep = car.CarState.new_message(vEgo=0.08, standstill=False)
+    for _ in range(10):
+      a = float(update_long_control(LoC, StopC, True, still, -0.1, True, (-3.5, 1.5), has_lead=True))
+    assert abs(a + 0.20) < 1e-6, a
+    a0 = a
+    for _ in range(int(0.25 / DT_CTRL)):
+      a = float(update_long_control(LoC, StopC, True, creep, -0.1, True, (-3.5, 1.5), has_lead=True))
+    first = a0 - a
+    assert 0.0 < first < 0.2, first
+    for _ in range(int(0.25 / DT_CTRL)):
+      a2 = float(update_long_control(LoC, StopC, True, creep, -0.1, True, (-3.5, 1.5), has_lead=True))
+    second = a - a2
+    assert second > first * 1.5, (first, second)
+    for _ in range(5):
+      a3 = float(update_long_control(LoC, StopC, True, still, -0.1, True, (-3.5, 1.5), has_lead=True))
+    assert abs(a3 - a2) < 1e-6
+    assert StoppingController.CREEP_RATE_GROWTH > 0.0

openpilot/sunnypilot/selfdrive/controls/lib/stopping_controller.py

@@ -0,0 +1,75 @@
+"""
+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
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+
+
+class StoppingController:
+  """Optional terminal-stop policy applied after stock LongControl.update()."""
+
+  STOPPING_DECEL_RATE = 0.3  # m/s^2/s
+  STANDSTILL_HOLD_RATE = 0.5  # m/s^2/s
+  STANDSTILL_HOLD_DELAY_LEAD = 0.6  # s
+  STANDSTILL_HOLD_DELAY_NO_LEAD = 0.9  # s
+  STOPPING_FREEZE_MAX = 2.0  # s
+  STOPPING_EXIT_DEBOUNCE = 0.2  # s
+  STOPPING_FOLLOW_MIN = -0.10  # m/s^2
+  STOPPING_FOLLOW_RATE = 2.0  # m/s^3
+  CREEP_V_MIN = 0.03  # m/s
+  CREEP_RATE_GROWTH = 1.5  # m/s^2/s per second of creep
+  CREEP_RATE_MAX = 1.0  # m/s^2/s
+
+  def __init__(self, stop_accel):
+    self.stop_accel = stop_accel
+    self.standstill_t = 0.0
+    self.stopping_t = 0.0
+    self.go_t = 0.0
+    self.stopped_once = False
+    self.creep_t = 0.0
+
+  def update(self, prev_state, state, CS, a_target, prev_accel, stock_accel, accel_limits, has_lead=False):
+    if prev_state == LongCtrlState.stopping and state == LongCtrlState.pid and CS.standstill:
+      self.go_t += DT_CTRL
+      if self.go_t < self.STOPPING_EXIT_DEBOUNCE:
+        state = LongCtrlState.stopping
+    else:
+      self.go_t = 0.0
+
+    self.standstill_t = self.standstill_t + DT_CTRL if CS.standstill else 0.0
+    self.stopping_t = self.stopping_t + DT_CTRL if state == LongCtrlState.stopping else 0.0
+    if state != LongCtrlState.stopping:
+      self.stopped_once = False
+    elif CS.standstill:
+      self.stopped_once = True
+
+    creeping = self.stopped_once and not CS.standstill and CS.vEgo > self.CREEP_V_MIN
+    self.creep_t = self.creep_t + DT_CTRL if creeping else 0.0
+
+    if state != LongCtrlState.stopping:
+      return state, stock_accel
+
+    output_accel = prev_accel
+    if output_accel > self.stop_accel:
+      output_accel = min(output_accel, 0.0)
+      if not CS.standstill and not self.stopped_once and a_target < self.STOPPING_FOLLOW_MIN and a_target > output_accel:
+        output_accel = min(a_target, output_accel + self.STOPPING_FOLLOW_RATE * DT_CTRL)
+
+      hold_delay = self.STANDSTILL_HOLD_DELAY_LEAD if has_lead else self.STANDSTILL_HOLD_DELAY_NO_LEAD
+      if self.standstill_t >= hold_delay:
+        rate = self.STANDSTILL_HOLD_RATE
+      elif creeping:
+        rate = min(self.STOPPING_DECEL_RATE + self.CREEP_RATE_GROWTH * self.creep_t, self.CREEP_RATE_MAX)
+      elif self.stopping_t >= self.STOPPING_FREEZE_MAX:
+        rate = self.STOPPING_DECEL_RATE
+      else:
+        rate = 0.0
+      output_accel -= rate * DT_CTRL
+
+    return state, float(np.clip(output_accel, accel_limits[0], accel_limits[1]))