Prevent pace handoff lurches without damping acceleration

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -37,15 +37,12 @@ class AccelControllerState(IntEnum):
 @dataclass(frozen=True)
 class ProfileConfig:
   comfort_decel: float
-  release_rate: float
-  release_confirm: float
-  departure_rate: float
 
 
 PROFILE_CONFIGS = {
-  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=1.55, release_confirm=0.50, departure_rate=12.0),
-  AccelProfile.normal: ProfileConfig(comfort_decel=0.32, release_rate=1.75, release_confirm=0.35, departure_rate=16.0),
-  AccelProfile.sport: ProfileConfig(comfort_decel=0.38, release_rate=2.00, release_confirm=0.20, departure_rate=20.0),
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.32),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.38),
 }
 
 # Keep launch responsive, then separate the profiles above walking speed.
@@ -60,6 +57,7 @@ CAP_FILTER_FRAMES = 5
 RESTRICT_DEADBAND = 0.15
 RESTRICT_EXIT_DEADBAND = 0.05
 RELIEF_DEADBAND = 0.35
+RELIEF_CONFIRM_FRAMES = 2
 PACE_ACQUISITION_MARGIN = 0.45
 EARLY_APPROACH_HEADWAY = 5.0
 EARLY_APPROACH_MIN_SPEED = 8.0
@@ -70,13 +68,14 @@ MOVING_LEAD_DECEL_CONFIRM_FRAMES = 3
 MOVING_LEAD_DECEL_ENTER_REQUIRED_DECEL = 0.15
 MOVING_LEAD_DECEL_ACCEL_MAX = -0.42
 MOVING_LEAD_DECEL_ACCEL_SLEW_RATE = 0.50
+MOVING_LEAD_DECEL_RELEASE_RATE = 1.00
 MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL = 0.05
 MOVING_LEAD_DECEL_EXIT_FRAMES = 4
 STOP_HOLD_EGO_SPEED = 0.30
 STOP_HOLD_CAP = 0.50
-STOP_HOLD_ACCEL_MAX = 0.0
-STOP_HOLD_GUARD_ACCEL_MAX = -0.42
-STOP_HOLD_GUARD_EXTRA = 0.20
+STOP_HOLD_PRECONDITION_SPEED = 0.05
+STOP_HOLD_PRECONDITION_ACCEL_MAX = -0.25
+STOP_HOLD_PRECONDITION_EXTRA = 0.20
 STOPPED_LEAD_SPEED = 0.30
 STOP_HOLD_EXIT_FRAMES = 4
 MAX_LEAD_ACCEL_TAU = 10.0
@@ -135,13 +134,14 @@ class _PacePath:
   cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
   pace: float | None = None
   state: AccelControllerState = AccelControllerState.inactive
-  relief_time: float = 0.0
+  relief_frames: int = 0
   departure_frames: int = 0
   departing_from_stop: bool = False
   filtered_lead_active: bool = False
   previous_lead_speed: float | None = None
   lead_decel_frames: int = 0
   moving_lead_decel: bool = False
+  moving_lead_release: bool = False
   moving_lead_relief_frames: int = 0
   moving_lead_accel_max: float | None = None
 
@@ -149,13 +149,14 @@ class _PacePath:
     self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
     self.pace = None
     self.state = AccelControllerState.inactive
-    self.relief_time = 0.0
+    self.relief_frames = 0
     self.departure_frames = 0
     self.departing_from_stop = False
     self.filtered_lead_active = False
     self.previous_lead_speed = None
     self.lead_decel_frames = 0
     self.moving_lead_decel = False
+    self.moving_lead_release = False
     self.moving_lead_relief_frames = 0
     self.moving_lead_accel_max = None
 
@@ -331,7 +332,9 @@ class AccelController:
       and envelope.closing_speed > CLOSING_SPEED_ENTER
     ):
       path.moving_lead_decel = True
-      path.moving_lead_accel_max = None
+      if not path.moving_lead_release:
+        path.moving_lead_accel_max = None
+      path.moving_lead_release = False
 
     matched_moving_lead = (
       path.moving_lead_decel
@@ -340,10 +343,15 @@ class AccelController:
       and envelope.required_decel <= MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL
     )
     path.moving_lead_relief_frames = path.moving_lead_relief_frames + 1 if matched_moving_lead else 0
-    if path.moving_lead_relief_frames >= MOVING_LEAD_DECEL_EXIT_FRAMES or envelope.selected_lead < 0 or path.state == AccelControllerState.stopHold:
+    if path.state == AccelControllerState.stopHold:
       path.moving_lead_decel = False
+      path.moving_lead_release = False
       path.moving_lead_relief_frames = 0
       path.moving_lead_accel_max = None
+    elif path.moving_lead_relief_frames >= MOVING_LEAD_DECEL_EXIT_FRAMES or not path.filtered_lead_active:
+      path.moving_lead_decel = False
+      path.moving_lead_release = path.moving_lead_accel_max is not None and path.moving_lead_accel_max < 0.0
+      path.moving_lead_relief_frames = 0
 
     path.previous_lead_speed = lead_speed if math.isfinite(lead_speed) else None
 
@@ -360,11 +368,11 @@ class AccelController:
       path.state = AccelControllerState.free
 
     early_approach = envelope.usable_gap >= EARLY_APPROACH_HEADWAY * max(v_ego, 1.0)
-    if acquired_restrictive_lead and filtered_cap < path.pace - RESTRICT_DEADBAND:
-      if not early_approach or v_ego >= EARLY_APPROACH_MIN_SPEED:
-        acquisition_margin = PACE_ACQUISITION_MARGIN if early_approach else 0.0
-        acquisition_pace = max(envelope.selected_lead_speed, v_ego - acquisition_margin)
-        path.pace = min(path.pace, acquisition_pace)
+    if acquired_restrictive_lead and path.pace >= base_speed - RESTRICT_DEADBAND and filtered_cap < path.pace - RESTRICT_DEADBAND:
+      if not early_approach:
+        path.pace = min(path.pace, v_ego)
+      elif v_ego >= EARLY_APPROACH_MIN_SPEED:
+        path.pace = min(path.pace, max(envelope.selected_lead_speed, v_ego - PACE_ACQUISITION_MARGIN))
 
     path.pace = min(path.pace, base_speed)
     if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace:
@@ -378,10 +386,9 @@ class AccelController:
         path.pace = 0.0
         return filtered_cap
 
-      departure_ceiling = min(base_speed, filtered_cap) if math.isfinite(filtered_cap) else base_speed
-      path.pace = max(departure_ceiling, 0.0)
+      path.pace = base_speed
       path.state = AccelControllerState.release
-      path.relief_time = 0.0
+      path.relief_frames = 0
       path.departure_frames = 0
       path.departing_from_stop = True
       return filtered_cap
@@ -393,7 +400,7 @@ class AccelController:
     if v_ego < STOP_HOLD_EGO_SPEED and (stop_evidence or stale_plan_stop):
       path.pace = 0.0
       path.state = AccelControllerState.stopHold
-      path.relief_time = 0.0
+      path.relief_frames = 0
       path.departure_frames = 0
       path.departing_from_stop = False
       return filtered_cap
@@ -401,10 +408,16 @@ class AccelController:
     if path.departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED:
       path.departing_from_stop = False
 
-    transient_relief = math.isfinite(filtered_cap) and (not math.isfinite(raw_cap) or raw_cap > filtered_cap + RELIEF_DEADBAND)
-    if transient_relief and not path.departing_from_stop:
+    filter_warmup_relief = math.isfinite(raw_cap) and not math.isfinite(filtered_cap) and path.pace < base_speed - RESTRICT_DEADBAND
+    if filter_warmup_relief:
       path.state = AccelControllerState.hold
-      path.relief_time = 0.0
+      path.relief_frames = 0
+      return filtered_cap
+
+    transient_relief = math.isfinite(filtered_cap) and (not math.isfinite(raw_cap) or raw_cap > filtered_cap + RELIEF_DEADBAND)
+    if transient_relief and not path.departing_from_stop and path.state != AccelControllerState.release:
+      path.state = AccelControllerState.hold
+      path.relief_frames = 0
       return filtered_cap
 
     ceiling = base_speed if path.departing_from_stop else min(base_speed, filtered_cap)
@@ -415,37 +428,33 @@ class AccelController:
     if cap_restriction:
       path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt)
       path.state = AccelControllerState.restrict
-      path.relief_time = 0.0
+      path.relief_frames = 0
       return filtered_cap
 
     if closing_guard:
       path.state = AccelControllerState.hold
-      path.relief_time = 0.0
+      path.relief_frames = 0
       return filtered_cap
 
     relief = ceiling - path.pace
-    release_active = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND
-    waiting_for_release = False
-    if relief > RELIEF_DEADBAND and not release_active:
-      if path.state != AccelControllerState.release:
-        path.relief_time += self.dt
-        path.state = AccelControllerState.hold
-        if path.relief_time >= config.release_confirm:
-          path.state = AccelControllerState.release
-      release_active = path.state == AccelControllerState.release
-      waiting_for_release = not release_active
+    if relief > RELIEF_DEADBAND and path.state != AccelControllerState.release:
+      confirmed_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap)
+      path.relief_frames = path.relief_frames + 1 if confirmed_lead_loss else RELIEF_CONFIRM_FRAMES
+      path.state = AccelControllerState.release if path.relief_frames >= RELIEF_CONFIRM_FRAMES else AccelControllerState.hold
+    elif path.state != AccelControllerState.release:
+      path.relief_frames = 0
+    release_active = path.state == AccelControllerState.release and relief >= 0.0
 
     if release_active:
-      release_rate = config.departure_rate if path.departing_from_stop else config.release_rate
-      path.pace = min(ceiling, path.pace + release_rate * self.dt)
-    elif waiting_for_release:
+      path.pace = ceiling
+      path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.release
+      path.relief_frames = 0
+    elif path.state == AccelControllerState.release:
       pass
     elif not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) and 0.0 < relief <= RELIEF_DEADBAND:
       path.pace = ceiling
       path.state = AccelControllerState.free
-      path.relief_time = 0.0
     else:
-      path.relief_time = 0.0
       path.state = AccelControllerState.free if path.pace >= ceiling - RESTRICT_DEADBAND else AccelControllerState.hold
 
     return filtered_cap
@@ -474,15 +483,17 @@ class AccelController:
     requested_accel_max = min(profile_accel_max, stock_accel_max)
     if stop_hold:
       self._reset_accel_limit()
-      guard_time = self._delay() + STOP_HOLD_GUARD_EXTRA
-      hold_accel_max = tuple(STOP_HOLD_GUARD_ACCEL_MAX if t <= guard_time else ACCEL_MAX for t in T_IDXS)
-      return min(requested_accel_max, STOP_HOLD_ACCEL_MAX), hold_accel_max, True, True
+      if v_ego > STOP_HOLD_PRECONDITION_SPEED:
+        return requested_accel_max, None, False, False
+      guard_time = self._delay() + STOP_HOLD_PRECONDITION_EXTRA
+      hold_accel_max = tuple(STOP_HOLD_PRECONDITION_ACCEL_MAX if t <= guard_time else ACCEL_MAX for t in T_IDXS)
+      return STOP_HOLD_PRECONDITION_ACCEL_MAX, hold_accel_max, True, True
 
     if self.live.departing_from_stop:
       self._reset_accel_limit(requested_accel_max)
       return requested_accel_max, None, False, False
 
-    if self.live.moving_lead_decel and lead_present:
+    if self.live.moving_lead_decel and self.live.filtered_lead_active:
       if self.live.moving_lead_accel_max is None:
         self.live.moving_lead_accel_max = max(min(planner_accel, 0.0), MOVING_LEAD_DECEL_ACCEL_MAX)
       self.live.moving_lead_accel_max = max(
@@ -492,6 +503,16 @@ class AccelController:
       moving_lead_accel_max = min(stock_accel_max, self.live.moving_lead_accel_max)
       return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True
 
+    if self.live.moving_lead_release:
+      self.live.moving_lead_accel_max = min(0.0, self.live.moving_lead_accel_max + MOVING_LEAD_DECEL_RELEASE_RATE * self.dt)
+      moving_lead_accel_max = min(stock_accel_max, self.live.moving_lead_accel_max)
+      if self.live.moving_lead_accel_max < 0.0:
+        return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True
+      self.live.moving_lead_release = False
+      self.live.moving_lead_accel_max = None
+      self._reset_accel_limit(requested_accel_max)
+      return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True
+
     if requested_accel_max <= 0.0:
       self._reset_accel_limit()
       return requested_accel_max, None, False, False
@@ -573,7 +594,7 @@ class AccelController:
       live_filtered_cap = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source,
                                             planner_speed, previous_should_stop)
       self._update_moving_lead_decel(self.live, envelope, base_speed, sanitized_v_ego)
-      target_speed = base_speed if self.live.departing_from_stop else min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
+      target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
       effective_accel_max, mpc_accel_max, mpc_shape_cruise, mpc_apply_accel_constraint = self._build_mpc_accel_max(
         profile_accel_max, stock_accel_max, planner_accel, sanitized_v_ego, envelope.selected_lead >= 0,
         envelope.closing_speed, self.live.state == AccelControllerState.stopHold,

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -16,9 +16,11 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
   ACCEL_SHAPE_WARMUP_FRAMES,
   CAP_FILTER_FRAMES,
   MOVING_LEAD_DECEL_ACCEL_MAX,
-  MOVING_LEAD_DECEL_ACCEL_SLEW_RATE,
   MOVING_LEAD_DECEL_CONFIRM_FRAMES,
   MOVING_LEAD_DECEL_EXIT_FRAMES,
+  MOVING_LEAD_DECEL_ACCEL_SLEW_RATE,
+  MOVING_LEAD_DECEL_RELEASE_RATE,
+  RELIEF_CONFIRM_FRAMES,
   STOP_HOLD_EXIT_FRAMES,
   AccelController,
   AccelControllerState,
@@ -431,7 +433,7 @@ class TestPaceGovernor:
     assert all(not result.mpc_shape_cruise for result in results)
     assert all(not result.mpc_apply_accel_constraint for result in results)
 
-  def test_decelerating_moving_lead_uses_smooth_ceiling_until_confirmed_match(self):
+  def test_decelerating_moving_lead_uses_gradual_bounded_ceiling_until_confirmed_match(self):
     controller = make_controller()
 
     def moving_lead(speed):
@@ -441,13 +443,7 @@ class TestPaceGovernor:
     confirmation = []
     for frame in range(MOVING_LEAD_DECEL_CONFIRM_FRAMES):
       confirmation.append(
-        update(
-          controller,
-          moving_lead(15.0 - 0.01 * (frame + 1)),
-          base_speed=30.0,
-          v_ego=20.0,
-          planner_speed=20.0,
-        )
+        update(controller, moving_lead(15.0 - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0)
       )
       if frame < MOVING_LEAD_DECEL_CONFIRM_FRAMES - 1:
         assert not controller.live.moving_lead_decel
@@ -457,17 +453,55 @@ class TestPaceGovernor:
     assert confirmation[-1].mpc_apply_accel_constraint
 
     falling_speed = 15.0 - 0.01 * MOVING_LEAD_DECEL_CONFIRM_FRAMES
-    constrained = [update(controller, moving_lead(falling_speed - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0) for frame in range(20)]
-    limits = np.array([result.effective_accel_max for result in [confirmation[-1], *constrained]])
+    constrained = [
+      update(controller, moving_lead(falling_speed - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0)
+      for frame in range(20)
+    ]
+    limits = np.array([confirmation[-1].effective_accel_max, *(result.effective_accel_max for result in constrained)])
     assert constrained[-1].effective_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX
     assert np.all(np.diff(limits) <= 0.0)
     assert np.max(np.abs(np.diff(limits))) <= MOVING_LEAD_DECEL_ACCEL_SLEW_RATE * DT_MDL + 1e-9
+    assert np.min(limits) >= MOVING_LEAD_DECEL_ACCEL_MAX
+    assert np.all(np.diff([result.live_pace for result in constrained]) <= 1e-9)
 
     matched = [update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) for _ in range(MOVING_LEAD_DECEL_EXIT_FRAMES)]
     assert all(result.effective_accel_max <= 0.0 for result in matched[:-1])
     assert not controller.live.moving_lead_decel
+    assert controller.live.moving_lead_release
+    assert matched[-1].effective_accel_max < 0.0
+
+    unwind_frames = math.ceil(abs(matched[-1].effective_accel_max) / (MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL))
+    unwinding = [update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) for _ in range(unwind_frames)]
+    unwind_limits = np.array([matched[-1].effective_accel_max, *(result.effective_accel_max for result in unwinding)])
+    assert np.all(np.diff(unwind_limits) >= 0.0)
+    assert np.max(np.diff(unwind_limits)) <= MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL + 1e-9
+    assert unwinding[-1].effective_accel_max == 0.0
+    assert not controller.live.moving_lead_release
     assert controller.live.moving_lead_accel_max is None
-    assert matched[-1].effective_accel_max > 0.0
+
+    released = update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05)
+    assert released.effective_accel_max == min(released.profile_accel_max, 1.2, ACCEL_MAX)
+
+  def test_moving_lead_ceiling_unwinds_smoothly_after_filtered_dropout(self):
+    controller = make_controller()
+
+    def moving_lead(speed):
+      return make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=speed))
+
+    update(controller, moving_lead(15.0), base_speed=30.0, v_ego=20.0, planner_speed=20.0)
+    for frame in range(MOVING_LEAD_DECEL_CONFIRM_FRAMES + 20):
+      update(controller, moving_lead(14.99 - 0.01 * frame), base_speed=30.0, v_ego=20.0, planner_speed=20.0)
+
+    assert controller.live.moving_lead_decel
+    assert controller.live.moving_lead_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX
+    dropouts = [update(controller, make_radar(), base_speed=30.0, v_ego=20.0, planner_speed=20.0) for _ in range(3)]
+    assert all(result.effective_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX for result in dropouts[:2])
+    assert dropouts[-1].effective_accel_max == pytest.approx(MOVING_LEAD_DECEL_ACCEL_MAX + MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL)
+    assert controller.live.moving_lead_release
+
+    stock_coast = update(controller, make_radar(), base_speed=30.0, v_ego=20.0, planner_speed=20.0, stock_accel_max=-0.5)
+    assert stock_coast.effective_accel_max == -0.5
+    assert stock_coast.mpc_accel_max[-1] == -0.5
 
   def test_far_or_noisy_decelerating_lead_does_not_force_braking(self):
     far_controller = make_controller()
@@ -497,21 +531,31 @@ class TestPaceGovernor:
     assert not far_controller.live.moving_lead_decel
     assert not noisy_controller.live.moving_lead_decel
     assert all(result.effective_accel_max >= 0.0 for result in [*far_results, *noisy_results])
+    assert all(result.mpc_accel_max is None or min(result.mpc_accel_max) >= 0.0 for result in [*far_results, *noisy_results])
 
-  def test_release_waits_then_raises_pace_at_profile_rate(self):
+  def test_filtered_relief_returns_pace_authority_to_accel_profile(self):
     controller = make_controller()
     restricted = self.establish_gentle_restriction(controller, frames=30)[-1]
     restricted_pace = restricted.live_pace
 
-    confirmation_frames = math.ceil(PROFILE_CONFIGS[AccelProfile.normal].release_confirm / DT_MDL)
-    results = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + confirmation_frames)]
+    results = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + RELIEF_CONFIRM_FRAMES)]
     released = results[-1]
 
-    assert all(result.state == AccelControllerState.hold for result in results[:-1])
-    assert all(result.live_pace == restricted_pace for result in results[:-1])
-    assert released.state == AccelControllerState.release
-    assert released.live_pace > restricted_pace
-    assert released.live_pace == pytest.approx(restricted_pace + PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL)
+    assert all(result.state != AccelControllerState.release for result in results[:-1])
+    assert all(result.live_pace <= restricted_pace for result in results[:-1])
+    assert released.state == AccelControllerState.free
+    assert released.live_pace == released.base_speed
+
+  def test_restrictive_lead_reacquisition_holds_pace_until_filter_recovers(self):
+    controller = make_controller()
+    restricted = self.establish_gentle_restriction(controller, frames=30)[-1]
+    missing = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + 1)]
+    reacquired = [update(controller, make_radar(self.gentle_restrictive_lead)) for _ in range(CAP_FILTER_FRAMES // 2 + 1)]
+
+    assert math.isinf(missing[-1].live_filtered_cap)
+    assert all(result.live_pace <= restricted.live_pace for result in missing)
+    assert all(result.live_pace == missing[-1].live_pace for result in reacquired[:-1])
+    assert reacquired[-1].live_pace == pytest.approx(missing[-1].live_pace - PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL)
 
   def test_confirmed_clear_finishes_release_at_base(self):
     controller = make_controller()
@@ -583,6 +627,28 @@ class TestPaceGovernor:
     assert handed_off.mpc_accel_max is None
     assert not handed_off.mpc_apply_accel_constraint
 
+  @pytest.mark.parametrize(("v_ego", "uses_standstill_preconditioner"), [(0.0, True), (0.05, True), (0.051, False), (0.1, False)])
+  def test_stop_hold_negative_bound_is_limited_to_stationary_preconditioning(self, v_ego, uses_standstill_preconditioner):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+
+    result = update(controller, stopped, base_speed=8.0, v_ego=v_ego, planner_speed=v_ego, stock_accel_max=1.6)
+
+    assert result.state == AccelControllerState.stopHold
+    assert result.target_speed == 0.0
+    if uses_standstill_preconditioner:
+      guard_time = controller._delay() + 0.20
+      expected = tuple(-0.25 if t <= guard_time else ACCEL_MAX for t in T_IDXS)
+      assert result.effective_accel_max == -0.25
+      assert result.mpc_accel_max == expected
+      assert result.mpc_shape_cruise
+      assert result.mpc_apply_accel_constraint
+    else:
+      assert result.effective_accel_max > 0.0
+      assert result.mpc_accel_max is None
+      assert not result.mpc_shape_cruise
+      assert not result.mpc_apply_accel_constraint
+
   def test_stop_hold_requires_four_moving_lead_frames_then_targets_base(self):
     controller = make_controller()
     stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
@@ -595,14 +661,16 @@ class TestPaceGovernor:
     assert held.state == AccelControllerState.stopHold
     assert held.target_speed == 0.0
     held_results = [held, *waiting, *confirmations[:-1]]
-    assert all(result.effective_accel_max == 0.0 for result in held_results)
-    assert all(result.mpc_accel_max[0] < 0.0 and result.mpc_accel_max[-1] == ACCEL_MAX for result in held_results)
-    assert all(result.mpc_shape_cruise for result in held_results)
-    assert all(result.mpc_apply_accel_constraint for result in held_results)
+    assert all(result.effective_accel_max > 0.0 for result in held_results)
+    assert all(result.mpc_accel_max is None for result in held_results)
+    assert all(not result.mpc_shape_cruise for result in held_results)
+    assert all(not result.mpc_apply_accel_constraint for result in held_results)
     assert all(result.target_speed == 0.0 for result in confirmations[:-1])
-    assert confirmations[-1].target_speed > 0.0
+    assert confirmations[-1].target_speed == confirmations[-1].base_speed
     assert confirmations[-1].effective_accel_max <= ACCEL_MAX
     assert confirmations[-1].mpc_accel_max is None
+    assert not confirmations[-1].mpc_shape_cruise
+    assert not confirmations[-1].mpc_apply_accel_constraint
     assert confirmations[-1].launching
     assert released.target_speed >= confirmations[-1].target_speed
     assert released.launching

sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py

@@ -236,11 +236,13 @@ def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority():
   lead_plant = Plant(lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_delay=0.15, actuator_lag=0.20)
   lead_plant.step(v_lead=0.0, v_cruise=15.0)
   controller = lead_plant.planner.accel_controller_result
-  assert controller.mpc_accel_max[0] < 0.0
+  assert controller.target_speed == 0.0
+  assert min(controller.mpc_accel_max) == -0.25
   assert controller.mpc_accel_max[-1] == ACCEL_MAX
   assert controller.mpc_shape_cruise
   assert controller.mpc_apply_accel_constraint
-  assert np.max(lead_plant.planner.mpc.params[T_IDXS <= 0.40, 1]) <= 0.0
+  preconditioned = lead_plant.planner.mpc.params[(T_IDXS > 0.0) & (T_IDXS <= 0.40), 1]
+  assert len(preconditioned) and np.max(preconditioned) <= -0.25
 
 
 def test_clear_road_launch_is_immediate_and_profiles_separate():
@@ -453,7 +455,9 @@ def test_low_speed_stopped_lead_never_accelerates_during_stop_hold():
   stop_hold = trace.state == int(AccelControllerState.stopHold)
   assert urgent_demand.any() and stop_hold.any()
   assert np.max(trace.a_target[urgent_demand]) < 0.0
-  assert np.max(trace.a_target[stop_hold]) <= 0.0
+  assert np.max(trace.a_target[stop_hold]) < 0.1
+  assert np.max(trace.speed[stop_hold]) < 0.30
+  assert np.max(trace.acceleration[stop_hold]) <= 0.0
   assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0])
   assert _first_time_below(trace, -1.0) <= _first_time_below(baseline, -1.0) + 1e-9
   assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3
@@ -488,6 +492,37 @@ def test_moving_lead_dropout_and_false_relief_do_not_release_pace():
   assert trace.solver_failures <= baseline.solver_failures
 
 
+def test_confirmed_finite_relief_stays_latched_and_smooth():
+  def lead_speed(current_time: float) -> float:
+    return 8.0 if current_time < 5.0 else min(15.0, 8.0 + 3.5 * (current_time - 5.0))
+
+  common = dict(
+    duration=9.0,
+    profile=1,
+    lead_relevancy=True,
+    speed=12.0,
+    distance_lead=50.0,
+    v_lead=lead_speed,
+    v_cruise=20.0,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  released = np.flatnonzero((trace.time >= 5.0) & (trace.state == int(AccelControllerState.release)))
+  assert len(released)
+  reached_base = np.flatnonzero((np.arange(len(trace.time)) >= released[0]) & (trace.target_speed >= 20.0 - 1e-6))
+  release_end = reached_base[0] if len(reached_base) else len(trace.time)
+  rising = (np.arange(len(trace.time)) >= released[0]) & (np.arange(len(trace.time)) < release_end)
+  assert rising.any()
+  assert np.all(trace.state[rising] == int(AccelControllerState.release))
+  assert np.all(np.diff(trace.target_speed[rising]) >= -1e-9)
+  assert not _has_brake_gas_brake(trace.a_target[trace.time >= 5.0])
+  assert np.max(np.abs(_command_jerk(trace, after=5.0))) <= np.max(np.abs(_command_jerk(baseline, after=5.0))) + 0.1
+  assert trace.solver_failures <= baseline.solver_failures
+
+
 def test_low_speed_far_lead_acquisition_does_not_fault_or_lurch():
   acquisition_time = 5.0
 
@@ -601,9 +636,11 @@ def test_decelerating_moving_lead_unwinds_without_brake_gas_brake(profile):
   baseline = _run(controller_enabled=False, **common)
   trace = _run(controller_enabled=True, **common)
   after_lead_settles = trace.time >= 8.0
-  moving_lead_decel = trace.effective_accel_max < 0.0
-  assert moving_lead_decel.any()
-  assert np.min(trace.effective_accel_max[moving_lead_decel]) >= MOVING_LEAD_DECEL_ACCEL_MAX - 1e-9
+  lead_decelerating = (trace.time >= 2.0) & (trace.time <= 8.0) & trace.active
+  moving_lead_guard = trace.effective_accel_max < 0.0
+  assert moving_lead_guard.any()
+  assert np.min(trace.effective_accel_max[moving_lead_guard]) >= MOVING_LEAD_DECEL_ACCEL_MAX - 1e-9
+  assert np.all(np.diff(trace.pace[lead_decelerating]) <= 1e-9)
   assert not trace.should_stop[after_lead_settles].any()
   assert np.max(np.abs(_command_jerk(trace, after=1.0))) < 3.5
   assert np.min(trace.speed[after_lead_settles]) >= 2.0