fix(long): preserve smooth braking through the final stop

2026-08-21 09:13:46 +08:00 · 2026-08-15 00:58:42 -07:00
parent 86eac9f044
commit dd7392f304
3 changed files with 447 additions and 2 deletions
@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP

 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]

@@ -39,8 +40,9 @@ def long_control_state_trans(CP_SP, active, long_control_state,

  return long_control_state

-class LongControl:
+class LongControl(LongControlSP):
  def __init__(self, CP, CP_SP):
+    LongControlSP.__init__(self)
    self.CP = CP
    self.CP_SP = CP_SP
    self.long_control_state = LongCtrlState.off
@@ -60,6 +62,7 @@ class LongControl:
    self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
                                                       should_stop, CS.brakePressed,
                                                       CS.cruiseState.standstill)
+    LongControlSP.update_state(self, self.long_control_state == LongCtrlState.stopping)
    if self.long_control_state == LongCtrlState.off:
      self.reset()
      output_accel = 0.
@@ -69,7 +72,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 -= LongControlSP.stopping_decel_rate(self, CS, a_target) * DT_CTRL
      self.reset()

    else:  # LongCtrlState.pid
@@ -0,0 +1,51 @@
+"""
+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 math
+
+STOPPING_DISTANCE = 0.75
+STOPPING_TIME = 2.5
+STOPPING_ACCEL_TOLERANCE = 0.1
+STOPPING_SPEED_TOLERANCE = 0.05
+STOPPING_SETTLE_FRAMES = 30
+
+
+class LongControlSP:
+  def __init__(self):
+    self._stopping_settle_frames: int | None = None
+
+  def update_state(self, stopping: bool) -> None:
+    if not stopping:
+      self._stopping_settle_frames = None
+
+  def stopping_decel_rate(self, CS, a_target: float) -> float:
+    if not all(math.isfinite(value) for value in (self.last_output_accel, a_target, CS.vEgo, CS.aEgo)):
+      return 1.0
+    can_hold = self.last_output_accel <= 0.0 and a_target >= self.last_output_accel
+    terminal_speed = (0.0 <= CS.vEgo <= STOPPING_SPEED_TOLERANCE
+                      or CS.standstill and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE)
+    if self.last_output_accel > 0.0 or CS.vEgo < 0.0 and not terminal_speed:
+      return 1.0
+    if terminal_speed and self._stopping_settle_frames is None:
+      if not can_hold or self.last_output_accel > -STOPPING_ACCEL_TOLERANCE or CS.aEgo >= -STOPPING_ACCEL_TOLERANCE:
+        return 1.0
+      self._stopping_settle_frames = 0
+
+    time_decel = 0.0 if self._stopping_settle_frames is not None else CS.vEgo / STOPPING_TIME
+    required_decel = max(time_decel, CS.vEgo ** 2 / (2.0 * STOPPING_DISTANCE), 1e-3)
+    adequacy = min(max(-CS.aEgo / required_decel, 0.0), 1.0)
+    if not terminal_speed and self._stopping_settle_frames is None and can_hold and adequacy >= 1.0:
+      self._stopping_settle_frames = 0
+
+    motion_need = 1.0 - adequacy ** 2
+    planner_need = min(max((self.last_output_accel - a_target) / max(required_decel, STOPPING_ACCEL_TOLERANCE), 0.0), 1.0)
+    terminal_need = 0.0
+    if terminal_speed or self._stopping_settle_frames not in (None, 0):
+      self._stopping_settle_frames = min(self._stopping_settle_frames + 1, STOPPING_SETTLE_FRAMES)
+      terminal_need = (self._stopping_settle_frames / STOPPING_SETTLE_FRAMES) ** 2
+
+    return max(motion_need, planner_need, terminal_need)
@@ -0,0 +1,391 @@
+import numpy as np
+import pytest
+
+from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import STOPPING_SETTLE_FRAMES
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+STOP_ACCEL_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1, RIVIAN.RIVIAN_R1)
+SETTLE_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1)
+ROUTE_STOP_ONSETS = (
+  (0.280, -0.290, -0.220, -0.220), (0.290, -0.497, -0.270, -0.302), (0.464, -0.223, -0.264, -0.292),
+  (0.467, -0.582, -0.316, -0.359),
+  (0.530, -0.311, -0.309, -0.333), (0.581, -0.467, -0.312, -0.352), (0.398, -0.557, -0.311, -0.348),
+  (0.517, -0.290, -0.301, -0.327), (0.312, -0.420, -0.271, -0.304), (0.474, -0.509, -0.303, -0.347),
+  (0.241, -0.554, -0.573, -0.617), (0.292, -0.154, -0.302, -0.326),
+)
+
+
+def get_car_params(candidate):
+  fingerprint = gen_empty_fingerprint()
+  interface = interfaces[candidate]
+  CP = interface.get_params(candidate, fingerprint, [], True, False, False)
+  return CP, interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
+
+
+def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False) -> structs.CarState:
+  state = structs.CarState(vEgo=float(v_ego), aEgo=float(a_ego), standstill=standstill)
+  state.cruiseState.standstill = standstill
+  return state
+
+
+def make_control(candidate, initial_accel=-0.33):
+  CP, CP_SP = get_car_params(candidate)
+  control = LongControl(CP, CP_SP)
+  control.long_control_state = LongCtrlState.pid
+  control.last_output_accel = initial_accel
+  return CP, control
+
+
+def stock_stopping_output(output_accel, stop_accel):
+  return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel
+
+
+def test_stop_threshold_remains_unchanged():
+  assert should_stop(0.24, 0.0)
+  assert not should_stop(0.26, 0.0)
+  assert not should_stop(0.24, 0.1)
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "a_target", "initial_accel"), ROUTE_STOP_ONSETS)
+def test_logged_stop_onsets_hold_the_existing_brake(v_ego, a_ego, a_target, initial_accel):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+  assert control.long_control_state == LongCtrlState.stopping
+  assert output == pytest.approx(initial_accel)
+
+
+def test_glide_hold_survives_a_soft_deceleration_sample():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+  samples = ((0.388, -0.201, -0.164), (0.330, -0.120, -0.140), (0.283, -0.0675, -0.120))
+  outputs = [control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+             for v_ego, a_ego, a_target in samples]
+
+  assert outputs == pytest.approx([-0.166] * len(samples))
+
+
+def test_glide_response_reaches_the_stock_rate_when_deceleration_stops():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+  control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+  output = control.update(True, make_car_state(0.330, -0.01), -0.140, True, (-3.5, 2.0))
+
+  assert -0.176 < output < -0.175
+
+
+def test_glide_response_increases_with_stopping_distance_error():
+  _, nominal = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+  _, distance_error = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+  for control in (nominal, distance_error):
+    control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+  nominal_output = nominal.update(True, make_car_state(0.330, -0.050), -0.140, True, (-3.5, 2.0))
+  distance_error_output = distance_error.update(True, make_car_state(0.400, -0.050), -0.140, True, (-3.5, 2.0))
+
+  assert -0.176 < distance_error_output < nominal_output
+
+
+@pytest.mark.parametrize(("decel_fraction", "expected_rate"), ((1.0, 0.0), (0.75, 0.4375), (0.5, 0.75), (0.0, 1.0)))
+def test_stopping_rate_scales_with_realized_deceleration(decel_fraction, expected_rate):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(0.3, -0.12 * decel_fraction), 0.0, True, (-3.5, 2.0))
+
+  assert (-0.33 - output) / DT_CTRL == pytest.approx(expected_rate, abs=1e-6)
+
+
+def test_stopping_rate_scales_with_planner_demand():
+  _, gentle = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  _, urgent = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+
+  gentle_output = gentle.update(True, make_car_state(0.3, -0.12), -0.34, True, (-3.5, 2.0))
+  urgent_output = urgent.update(True, make_car_state(0.3, -0.12), -1.0, True, (-3.5, 2.0))
+
+  assert -0.331 < gentle_output < -0.33
+  assert urgent_output == pytest.approx(-0.34)
+
+
+def test_glide_hold_yields_to_stronger_planner_braking():
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+  control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+  output = control.update(True, make_car_state(0.330, -0.120), -1.0, True, (-3.5, 2.0))
+
+  assert output == pytest.approx(stock_stopping_output(-0.166, CP.stopAccel))
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_urgent_braking_matches_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  CS = make_car_state(0.8, -0.1)
+  output = control.last_output_accel
+
+  for _ in range(round(1.0 / DT_CTRL)):
+    output = control.update(True, CS, -3.0, True, (-3.5, 2.0))
+
+  expected = -0.33
+  for _ in range(round(1.0 / DT_CTRL)):
+    expected = stock_stopping_output(expected, CP.stopAccel)
+  assert output == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_stronger_planner_brake_matches_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)]
+  expected = []
+  output = -0.33
+  for _ in range(10):
+    output = stock_stopping_output(output, CP.stopAccel)
+    expected.append(output)
+  assert outputs == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_insufficient_deceleration_uses_most_of_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0))
+  if -0.33 > CP.stopAccel:
+    assert -0.34 < output < -0.338
+  else:
+    assert output == pytest.approx(-0.33)
+
+
+def test_deceleration_noise_cannot_release_the_brake():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+
+def test_planner_noise_cannot_release_the_brake():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "a_target"), (
+  (float("nan"), -0.3, -0.1), (0.3, float("nan"), -0.1), (0.3, -0.3, float("nan")),
+  (float("inf"), -0.3, -0.1), (0.3, -float("inf"), -0.1), (0.3, -0.3, float("inf")),
+))
+def test_invalid_state_uses_the_stock_ramp(v_ego, a_ego, a_target):
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+@pytest.mark.parametrize(("speed", "initial_accel", "grade_accel", "actuator_lag", "actuator_delay"), (
+  (0.24, 0.0, -0.49, 0.15, 0.0), (0.53, -0.31, -0.49, 0.35, 0.1),
+  (0.24, 0.0, 0.0, 0.15, 0.0), (0.464, -0.223, 0.0, 0.25, 0.05), (0.53, -0.31, 0.0, 0.35, 0.1),
+  (0.24, 0.0, 0.49, 0.15, 0.0), (0.53, -0.31, 0.49, 0.25, 0.05), (0.6, -0.3, 0.49, 0.35, 0.1), (0.6, -0.3, 0.49, 0.5, 0.1),
+))
+def test_smooth_stop_distance_is_bounded(speed, initial_accel, grade_accel, actuator_lag, actuator_delay):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+  applied_accel = initial_accel
+  delay = [initial_accel] * round(actuator_delay / DT_CTRL)
+  distance = 0.0
+  outputs = []
+
+  for _ in range(round(4.0 / DT_CTRL)):
+    command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0))
+    outputs.append(command)
+    delayed_command = command
+    if delay:
+      delay.append(command)
+      delayed_command = delay.pop(0)
+    applied_accel += DT_CTRL / actuator_lag * (delayed_command + grade_accel - applied_accel)
+    speed = max(0.0, speed + applied_accel * DT_CTRL)
+    distance += speed * DT_CTRL
+    if speed == 0.0:
+      break
+
+  assert speed == 0.0
+  assert distance < 1.0
+  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_standstill_uses_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  control.long_control_state = LongCtrlState.off
+  CS = make_car_state(0.0, 0.0, standstill=True)
+  outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))]
+  expected = -0.33
+  for _ in range(round(2.0 / DT_CTRL)):
+    expected = stock_stopping_output(expected, CP.stopAccel)
+  assert outputs[0] == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+  assert outputs[-1] == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
+def test_final_stop_builds_brake_smoothly_while_vehicle_settles(candidate):
+  _, control = make_control(candidate)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  outputs = [control.update(True, make_car_state(0.0006, a_ego, standstill=True), -0.032, True, (-3.5, 2.0))
+             for a_ego in (-1.098, -0.950, -0.609, -0.286)]
+  changes = -np.diff([-0.33, *outputs])
+  assert np.all(changes > 0.0)
+  assert np.all(np.diff(changes) > 0.0)
+  assert changes[-1] < 0.001
+
+
+@pytest.mark.parametrize("a_ego", (-0.09, 0.0, 0.1))
+def test_settled_vehicle_uses_the_stock_hold_ramp(a_ego):
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(0.0, a_ego, standstill=True), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
+def test_direct_terminal_entry_builds_brake_smoothly(candidate):
+  _, control = make_control(candidate)
+  CS = make_car_state(0.0006, -0.3, standstill=True)
+  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(4)]
+
+  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+  assert rates == pytest.approx([(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, 5)])
+
+
+def test_direct_terminal_entry_keeps_urgent_stock_braking():
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), -1.0, True, (-3.5, 2.0))
+
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+@pytest.mark.parametrize("initial_accel", (0.0, -0.05))
+def test_direct_terminal_entry_first_builds_meaningful_brake(initial_accel):
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+  output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), 0.0, True, (-3.5, 2.0))
+
+  assert output == pytest.approx(stock_stopping_output(initial_accel, CP.stopAccel))
+
+
+@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
+def test_final_settling_ramp_is_bounded(candidate):
+  _, control = make_control(candidate)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  CS = make_car_state(0.0, -0.3, standstill=True)
+  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
+
+  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+  expected = [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)] + [1.0]
+  assert rates == pytest.approx(expected)
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "standstill"), ((0.6, -0.1, False), (0.0, 0.0, True)))
+def test_stopping_never_releases_a_stronger_command(v_ego, a_ego, standstill):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0)
+  output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0))
+  assert output == pytest.approx(-3.0)
+
+
+def test_reported_standstill_while_moving_can_hold_the_brake():
+  _, control = make_control(GM.CHEVROLET_BOLT_EUV)
+  control.long_control_state = LongCtrlState.off
+  output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0))
+  assert output == pytest.approx(-0.33)
+
+
+def test_stopping_removes_positive_acceleration_immediately():
+  _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2)
+  output = control.update(True, make_car_state(0.2, -0.2), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(-DT_CTRL)
+
+
+def test_rollback_uses_the_stock_ramp():
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+def test_rollback_after_settling_arms_uses_the_stock_ramp():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  control.update(True, make_car_state(0.01, -0.3), -0.1, True, (-3.5, 2.0))
+  previous = control.last_output_accel
+  output = control.update(True, make_car_state(-0.04, -0.3), -0.1, True, (-3.5, 2.0))
+
+  assert output == pytest.approx(previous - DT_CTRL)
+
+
+def test_small_velocity_noise_does_not_trigger_the_stock_rate():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  output = control.update(True, make_car_state(-0.04, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
+  assert -0.331 < output < -0.33
+
+
+def test_terminal_speed_chatter_cannot_extend_settling_ramp():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  outputs = [control.update(True, make_car_state(0.049 if frame % 2 == 0 else 0.051, -0.3), -0.1, True, (-3.5, 2.0))
+             for frame in range(STOPPING_SETTLE_FRAMES + 2)]
+
+  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+  assert rates[:STOPPING_SETTLE_FRAMES] == pytest.approx([(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)])
+  assert rates[-2:] == pytest.approx([1.0, 1.0])
+
+
+def test_terminal_speed_plateau_cannot_extend_settling_ramp():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  CS = make_car_state(0.03, -0.3)
+  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
+
+  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+  assert rates[-2:] == pytest.approx([1.0, 1.0])
+
+
+def test_interrupted_stop_cannot_reuse_settling_hold():
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  control.update(False, make_car_state(0.0, 0.0, standstill=True), 0.0, False, (-3.5, 2.0))
+  output = control.update(True, make_car_state(0.0, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
+
+  assert output == pytest.approx(stock_stopping_output(0.0, CP.stopAccel))
+
+
+def test_departure_uses_the_stock_pid_path():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.long_control_state = LongCtrlState.stopping
+  output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0))
+  assert control.long_control_state == LongCtrlState.pid
+  assert output > 0.0
+
+
+def test_planner_mpc_and_longcontrol_complete_a_smooth_stop():
+  plant = PlantSP(
+    lead_relevancy=True, speed=0.6, distance_lead=3.6, run_long_control=True,
+    actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+  )
+  plant.planner.accel_controller.enabled = True
+  plant.planner.accel_controller.profile = 1
+  plant.planner.accel_controller.update_params = lambda: None
+  plant.planner.dec._enabled = False
+  plant.planner.dec._read_params = lambda: None
+  commands = []
+  speeds = []
+  states = []
+  solver_statuses = []
+
+  while plant.current_time < 5.0:
+    result = plant.step(v_lead=0.0, v_cruise=8.0)
+    commands.append(result["actuator_command"])
+    speeds.append(result["speed"])
+    states.append(result["long_control_state"])
+    solver_statuses.append(plant.planner.mpc.last_solution_status)
+
+  stopping = states.index(LongCtrlState.stopping)
+  moving_stop_commands = [command for command, state, speed in zip(commands, states, speeds, strict=True)
+                          if state == LongCtrlState.stopping and speed > 0.02]
+  assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1:], strict=True))
+  assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9
+  assert plant.speed == 0.0 and plant.distance < 1.0
+  assert plant.distance_lead - plant.distance > 3.0
+  assert all(status == 0 for status in solver_statuses)