Prevent rubber-banding without damping profile acceleration

selfdrive/controls/lib/longitudinal_planner.py

@@ -171,6 +171,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
+    self.output_should_stop = self.accel_controller_should_stop(self.output_should_stop, is_e2e)
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

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

@@ -14,14 +14,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
 )
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD,
-  CAP_FILTER_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_GAIN,
-  LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED,
-  PACE_RELIEF_DEADBAND, PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
+  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD, CAP_FILTER_FRAMES,
+  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW,
+  LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_TAPER_GAIN, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU,
+  MIN_LEAD_SPEED, PACE_RELIEF_DEADBAND, PACE_TARGET_ARM_MARGIN, PACE_TARGET_RESERVE,
+  PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
   STOP_GAP_RESERVE_LEAD_SPEED,
   STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
-  STOP_HOLD_ACCEL_MAX, STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_MAX_LEAD_DISTANCE,
-  STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
+  STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
 )
 
 
@@ -59,6 +59,7 @@ class AccelControllerResult:
   active: bool
   shadow_active: bool
   launching: bool
+  departure_launching: bool
   profile: AccelProfile
   profile_accel_max: float
   positive_accel_max: float
@@ -99,7 +100,8 @@ class _ControllerPath:
   departure_launch: bool = False
   matched_lead: bool = False
   braking_limited: bool = False
-  accel_limit: float | None = None
+  braking_handoff: bool = False
+  pace_reserve_armed: bool = False
   matched_accel_limit: float | None = None
 
   @property
@@ -137,7 +139,8 @@ class _ControllerPath:
     self.departure_launch = False
     self.matched_lead = False
     self.braking_limited = False
-    self.accel_limit = None
+    self.braking_handoff = False
+    self.pace_reserve_armed = False
     self.matched_accel_limit = None
 
 
@@ -346,6 +349,7 @@ class AccelController:
     path.launching = False
     path.departure_launch = False
     path.matched_lead = False
+    path.pace_reserve_armed = False
     path.matched_accel_limit = None
 
   def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float,
@@ -365,6 +369,9 @@ class AccelController:
       path.lead_switch_guard_frames = self.lead_loss_hold_frames
     elif path.lead_switch_guard_frames > 0:
       path.lead_switch_guard_frames -= 1
+    if slot_changed or track_changed:
+      path.matched_lead = False
+      path.matched_accel_limit = None
     if has_lead:
       path.selected_lead = envelope.selected_lead
       path.selected_lead_track_id = envelope.selected_lead_track_id
@@ -383,12 +390,9 @@ class AccelController:
                      and (not has_lead or envelope.departure_lead_speed < STOP_HOLD_EXIT_SPEED))
     stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50
                      or (previous_stop and not path.launching) or invalid_lead)
-    confirmed_creep_departure = path.launching and path.departure_launch and self._creep_departure(path, envelope)
-
-    if path.accel_limit is None:
-      path.accel_limit = profile_accel_max
-    else:
-      path.accel_limit = self._move(path.accel_limit, profile_accel_max, ACCEL_LIMIT_TRANSITION_JERK, self.dt)
+    confirmed_creep_departure = (path.launching and path.departure_launch and has_lead
+                                 and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
+                                      or self._creep_departure(path, envelope)))
     if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap)
         and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD):
       path.braking_limited = True
@@ -396,13 +400,18 @@ class AccelController:
       path.braking_limited = False
 
     if path.pace is None:
-      path.pace = min(base_speed, v_ego) if has_lead else base_speed
+      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
+      seed_from_ego = has_lead and planner_accel > BRAKING_ACCEL_LIMIT_THRESHOLD and not e2e_handoff
+      path.pace = min(base_speed, v_ego) if seed_from_ego else base_speed
+      path.braking_handoff = e2e_handoff and planner_accel < 0.0
       path.state = AccelControllerState.free
       if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
         path.pace = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
         path.state = AccelControllerState.release
         path.launching = True
         path.departure_launch = False
+    elif path.braking_handoff and planner_accel >= 0.0:
+      path.braking_handoff = False
 
     path.pace = min(path.pace, base_speed)
     if (v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not confirmed_creep_departure
@@ -415,8 +424,8 @@ class AccelController:
         separation = path.robust_departure_separation(lead_index)
         if math.isfinite(separation) and path.departure_references[lead_index] is None:
           path.departure_references[lead_index] = separation
-      raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_EXIT_SPEED
-                        and envelope.departure_cap > STOP_HOLD_EXIT_SPEED)
+      raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
+                        and envelope.departure_cap > STOP_HOLD_CREEP_SPEED)
                        or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
       departed = self._creep_departure(path, envelope) or raw_departure
       path.departure_frames = path.departure_frames + 1 if departed else 0
@@ -465,7 +474,8 @@ class AccelController:
         path.departure_launch = False
 
     comfort_decel = PROFILE_CONFIGS[profile].comfort_decel
-    if (has_lead and not path.launching and path.state == AccelControllerState.restrict and envelope.closing_speed <= 0.0
+    if (has_lead and not path.launching and path.state == AccelControllerState.restrict
+        and envelope.closing_speed <= 0.0
         and v_ego >= path.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
       path.matched_lead = True
     elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames:
@@ -479,23 +489,23 @@ class AccelController:
         return path.pace
       if math.isfinite(path.filtered_lead_speed):
         recovery_speed = min(base_speed, path.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * envelope.usable_gap))
-        recovery_error = max(recovery_speed - v_ego, 0.0)
-        desired_accel_limit = min(path.accel_limit, LEAD_MATCH_ACCEL_GAIN[profile] * recovery_error)
+        desired_accel_limit = min(profile_accel_max, LEAD_MATCH_TAPER_GAIN * max(recovery_speed - v_ego, 0.0))
       else:
         desired_accel_limit = 0.0
       if path.filtered_lead_accel < BRAKING_ACCEL_LIMIT_THRESHOLD:
-        desired_accel_limit = path.accel_limit
+        desired_accel_limit = profile_accel_max
       if path.matched_accel_limit is None:
-        path.matched_accel_limit = path.accel_limit
+        path.matched_accel_limit = profile_accel_max
       if path.lead_switch_guard_frames > 0:
         desired_accel_limit = min(desired_accel_limit, path.matched_accel_limit)
-      path.matched_accel_limit = min(path.accel_limit, self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt))
+      path.matched_accel_limit = min(profile_accel_max,
+                                     self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt))
       matched_ceiling = min(base_speed, filtered_cap)
       if matched_ceiling <= path.pace - PACE_RESTRICT_DEADBAND:
         path.pace = max(matched_ceiling, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
         path.state = AccelControllerState.restrict
       elif path.lead_switch_guard_frames == 0 and matched_ceiling >= path.pace + PACE_RELIEF_DEADBAND:
-        path.pace = min(matched_ceiling, path.pace + path.accel_limit * self.dt)
+        path.pace = min(matched_ceiling, path.pace + profile_accel_max * self.dt)
         path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
       else:
         path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
@@ -527,7 +537,7 @@ class AccelController:
     relief = not has_lead or envelope.closing_speed <= 0.0
     if relief and (ceiling >= path.pace + PACE_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > path.pace)):
       if path.lead_switch_guard_frames == 0:
-        path.pace = min(ceiling, path.pace + path.accel_limit * self.dt)
+        path.pace = ceiling
       path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
     else:
       path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
@@ -621,27 +631,40 @@ class AccelController:
       envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
 
     stop_hold_active = live_active and self.live.state == AccelControllerState.stopHold
-    valid_lead_stop_hold = stop_hold_active and envelope.selected_lead >= 0
-    matched_limit_active = live_active and self.live.matched_lead and self.live.matched_accel_limit is not None
-    braking_limited = live_active and self.live.braking_limited
-    confirmed_clear_road = (live_active and not stop_hold_active and not envelope.lead_status
-                            and not math.isfinite(self.live.filtered_cap) and self.live.lead_loss_frames >= self.lead_loss_hold_frames)
-    if stop_hold_active:
-      stop_hold_accel_max = STOP_HOLD_DEPARTURE_ACCEL_MAX if self.live.departure_frames > 1 else STOP_HOLD_ACCEL_MAX
-      effective_accel_max = min(stop_hold_accel_max, stock_accel_max) if valid_lead_stop_hold else 0.0
-    elif matched_limit_active:
-      effective_accel_max = min(self.live.matched_accel_limit, stock_accel_max)
+    matched_limit_active = (live_active and self.live.matched_lead and self.live.matched_accel_limit is not None
+                            and not self.live.braking_handoff)
+    lead_accel_request = (live_active and envelope.selected_lead >= 0
+                          and envelope.closing_speed <= 0.0 and planner_accel >= 0.0)
+    profile_limit_active = live_active and not stop_hold_active and (self.live.launching or not envelope.lead_status or lead_accel_request)
+    if matched_limit_active:
+      effective_accel_max = min(positive_accel_max, self.live.matched_accel_limit)
+    elif profile_limit_active:
+      effective_accel_max = positive_accel_max
     else:
-      effective_accel_max = (min(self.live.accel_limit, stock_accel_max)
-                             if (braking_limited or confirmed_clear_road) and self.live.accel_limit is not None else math.inf)
-    if stop_hold_active or matched_limit_active or braking_limited or confirmed_clear_road:
+      effective_accel_max = math.inf
+    if matched_limit_active or profile_limit_active:
       mpc_accel_max = self._build_accel_ceiling(effective_accel_max, planner_accel)
     else:
       mpc_accel_max = None
+    guarded_lead_loss = (not envelope.lead_status and self.live.selected_lead >= 0
+                         and self.live.lead_loss_frames < self.lead_loss_hold_frames)
+    lead_context = envelope.lead_status or math.isfinite(self.live.filtered_cap) or guarded_lead_loss
+    reserve_eligible = (live_active and lead_context and not stop_hold_active and self.live.lead_switch_guard_frames == 0
+                        and not self.live.launching and not self.live.braking_handoff)
+    if not lead_context:
+      self.live.pace_reserve_armed = False
+    elif (reserve_eligible and not self.live.pace_reserve_armed and math.isfinite(self.live.filtered_cap)
+          and self.live.filtered_cap <= pace_target + PACE_TARGET_ARM_MARGIN):
+      self.live.pace_reserve_armed = True
+
+    target_speed = 0.0 if stop_hold_active else pace_target
+    if reserve_eligible and self.live.pace_reserve_armed:
+      target_speed = max(0.0, target_speed - PACE_TARGET_RESERVE)
 
     return AccelControllerResult(
-      target_speed=0.0 if stop_hold_active else pace_target,
+      target_speed=target_speed,
       enabled=bool(enabled), active=live_active, shadow_active=shadow_active, launching=live_active and self.live.launching,
+      departure_launching=live_active and self.live.launching and self.live.departure_launch,
       profile=selected_profile, profile_accel_max=profile_accel_max if live_active else math.inf,
       positive_accel_max=positive_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max,
       mpc_accel_max=mpc_accel_max, state=self.live.state,

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

@@ -21,8 +21,8 @@ PROFILE_CONFIGS = {
 
 ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
 ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16],
-  AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30],
+  AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
+  AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
   AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
 }
 
@@ -30,18 +30,15 @@ CAP_FILTER_FRAMES = 5
 LEAD_LOSS_HOLD_TIME = 0.50
 PACE_RESTRICT_DEADBAND = 0.15
 PACE_RELIEF_DEADBAND = 0.35
+PACE_TARGET_ARM_MARGIN = 1.0
+PACE_TARGET_RESERVE = 0.10
 LAUNCH_TARGET_HEADROOM = 3.0
 LAUNCH_TARGET_SLEW = 8.75
 LAUNCH_END_SPEED = 3.0
-ACCEL_LIMIT_TRANSITION_JERK = 0.50
 ACCEL_LIMIT_HORIZON_JERK = 1.0
 LEAD_MATCH_GAP_GAIN = 0.04
-LEAD_MATCH_SPEED_HEADROOM = 2.50
-LEAD_MATCH_ACCEL_GAIN = {
-  AccelProfile.eco: 0.20,
-  AccelProfile.normal: 0.24,
-  AccelProfile.sport: 0.26,
-}
+LEAD_MATCH_SPEED_HEADROOM = 1.25
+LEAD_MATCH_TAPER_GAIN = 1.00
 LEAD_MATCH_ACCEL_SLEW = 0.25
 MATCHED_PACE_DECEL_RATE = 0.50
 BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
@@ -53,8 +50,6 @@ STOP_HOLD_EXIT_FRAMES = 4
 STOP_HOLD_CREEP_SPEED = 0.15
 STOP_HOLD_CREEP_DISTANCE = 0.30
 STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
-STOP_HOLD_ACCEL_MAX = 0.10
-STOP_HOLD_DEPARTURE_ACCEL_MAX = 0.25
 STOP_GAP_RESERVE = 0.75
 STOP_GAP_RESERVE_LEAD_SPEED = 2.0
 STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80)

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

@@ -12,10 +12,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
 )
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES,
-  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_GAIN, LEAD_MATCH_ACCEL_SLEW,
-  MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_ACCEL_MAX,
-  STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_EXIT_FRAMES,
+  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
+  LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS,
+  PACE_TARGET_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES,
 )
 
 
@@ -63,15 +62,10 @@ class TestProfiles:
   def test_lookup_table_is_explicit_and_tunable(self):
     assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
     assert ACCEL_PROFILE_MAX_V == {
-      AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16],
-      AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30],
+      AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
+      AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
       AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
     }
-    assert LEAD_MATCH_ACCEL_GAIN == {
-      AccelProfile.eco: 0.20,
-      AccelProfile.normal: 0.24,
-      AccelProfile.sport: 0.26,
-    }
 
   @pytest.mark.parametrize("profile", list(AccelProfile))
   def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
@@ -98,22 +92,17 @@ class TestProfiles:
     assert result.profile_accel_max == pytest.approx(1.15)
     assert result.positive_accel_max == pytest.approx(0.30)
     assert result.effective_accel_max == pytest.approx(0.30)
-    assert all(sample.mpc_accel_max is None for sample in results[:-1])
-    assert result.mpc_accel_max is not None
-    assert max(result.mpc_accel_max) <= ACCEL_MAX
+    assert all(sample.mpc_accel_max is not None for sample in results)
+    assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results)
 
-  def test_runtime_profile_switch_smooths_the_scalar_limit(self):
+  def test_runtime_profile_switch_applies_the_lookup_value_directly(self):
     controller = make_controller()
     sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
              for _ in range(controller.lead_loss_hold_frames)][-1]
     eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
-    max_step = ACCEL_LIMIT_TRANSITION_JERK * DT_MDL
 
     assert sport.effective_accel_max == pytest.approx(1.15)
-    assert sport.effective_accel_max - eco.effective_accel_max == pytest.approx(max_step)
-
-    settled = [update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20) for _ in range(30)][-1]
-    assert settled.effective_accel_max == pytest.approx(0.72)
+    assert eco.effective_accel_max == pytest.approx(0.72)
 
   def test_matched_lead_waits_until_ego_catches_the_lead(self):
     radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
@@ -199,34 +188,46 @@ class TestMpcCeiling:
     assert result.mpc_accel_max is None
     assert math.isinf(result.effective_accel_max)
 
-  def test_braking_ceiling_waits_for_stable_live_history_and_resets(self):
+  def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
     controller = make_controller()
     radar = restrictive_radar()
     warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)]
 
-    assert all(not sample.mpc_accel_max for sample in warmup[:-1])
+    assert all(sample.mpc_accel_max is None for sample in warmup)
     assert controller.live.braking_limited
-    assert warmup[-1].mpc_accel_max is not None
 
     bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
     assert not bypassed.active and bypassed.mpc_accel_max is None
     assert not controller.live.braking_limited
 
-  def test_matched_lead_ceiling_has_no_planner_accel_handoff(self):
+  def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES + 5):
+      update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2)
+
+    pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0))
+    result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2)
+
+    assert result.state == AccelControllerState.restrict
+    assert result.effective_accel_max == pytest.approx(result.positive_accel_max)
+    assert result.mpc_accel_max is not None
+
+  def test_matched_lead_terminal_taper_changes_smoothly(self):
     controller = make_controller()
     radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
     for _ in range(CAP_FILTER_FRAMES + 5):
       update(controller, radar, v_ego=10.0, planner_accel=-0.2)
 
     braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
+    braking_limit = controller.live.matched_accel_limit
     accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2)
 
     assert controller.live.matched_lead
     assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None
-    assert abs(accelerating.effective_accel_max - braking.effective_accel_max) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
-    target_drop = braking.target_speed - accelerating.target_speed
-    assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
-    assert accelerating.state == AccelControllerState.restrict
+    assert braking_limit is not None
+    assert abs(controller.live.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
+    assert braking.effective_accel_max <= braking.positive_accel_max
+    assert accelerating.effective_accel_max <= accelerating.positive_accel_max
 
   def test_matched_lead_ignores_two_frame_speed_jump(self):
     clean_controller, noisy_controller = make_controller(), make_controller()
@@ -329,13 +330,15 @@ class TestPaceAndLifecycle:
     assert math.isinf(results[1].live_filtered_cap)
     assert math.isfinite(results[2].live_filtered_cap)
 
-  def test_restriction_uses_comfort_rate_without_acquisition_step(self):
+  def test_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self):
     controller = make_controller()
     results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)]
     targets = np.asarray([result.target_speed for result in results])
     max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
-    assert np.max(-np.diff(targets)) <= max_step + 1e-9
+    target_steps = -np.diff(targets)
+    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
     assert results[-1].state == AccelControllerState.restrict
     assert results[-1].target_speed < results[0].target_speed
 
@@ -350,7 +353,9 @@ class TestPaceAndLifecycle:
     targets = np.asarray([25.0, *(result.target_speed for result in results)])
     max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
-    assert np.max(-np.diff(targets)) <= max_step + 1e-9
+    target_steps = -np.diff(targets)
+    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
 
   def test_lead_slot_is_forgotten_before_reacquisition(self):
     controller = make_controller()
@@ -368,7 +373,9 @@ class TestPaceAndLifecycle:
     targets = np.asarray([before.target_speed, *(result.target_speed for result in results)])
     max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
-    assert np.max(-np.diff(targets)) <= max_step + 1e-9
+    target_steps = -np.diff(targets)
+    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
 
   @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
   def test_false_relief_track_replacement_freezes_bounded_pace_release(self, replacement_track_id):
@@ -384,8 +391,8 @@ class TestPaceAndLifecycle:
     switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2)
 
     target_drop = before.target_speed - switched.target_speed
-    assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
-    assert switched.effective_accel_max <= before.effective_accel_max + 1e-9
+    assert -PACE_TARGET_RESERVE - 1e-9 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
+    assert switched.effective_accel_max <= switched.positive_accel_max + 1e-9
     assert switched.target_speed < switched.base_speed
     assert controller.live.lead_switch_guard_frames == controller.lead_loss_hold_frames
 
@@ -400,7 +407,7 @@ class TestPaceAndLifecycle:
 
     assert controller.live.lead_switch_guard_frames == 0
 
-  def test_short_dropout_holds_then_releases_at_profile_rate(self):
+  def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self):
     controller = make_controller()
     for _ in range(CAP_FILTER_FRAMES + 20):
       restricted = update(controller, restrictive_radar())
@@ -409,8 +416,7 @@ class TestPaceAndLifecycle:
     assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
 
     released = update(controller)
-    assert released.target_speed >= held[-1].target_speed
-    assert released.target_speed - held[-1].target_speed <= released.effective_accel_max * DT_MDL + 1e-9
+    assert released.target_speed == released.base_speed
 
   def test_previous_lead_source_synchronizes_down_to_planner(self):
     controller = make_controller()
@@ -496,12 +502,12 @@ class TestPaceAndLifecycle:
     assert result.state == AccelControllerState.stopHold
     assert controller.live.pace == 0.0
     assert result.target_speed == 0.0
-    assert result.effective_accel_max == STOP_HOLD_ACCEL_MAX
-    assert result.mpc_accel_max is not None and max(result.mpc_accel_max) <= STOP_HOLD_ACCEL_MAX
+    assert math.isinf(result.effective_accel_max)
+    assert result.mpc_accel_max is None
 
     stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0)
-    assert stock_limited.effective_accel_max == 0.0
-    assert stock_limited.mpc_accel_max is not None and max(stock_limited.mpc_accel_max) == 0.0
+    assert math.isinf(stock_limited.effective_accel_max)
+    assert stock_limited.mpc_accel_max is None
 
   def test_stop_hold_needs_four_confirmed_departure_frames(self):
     controller = make_controller()
@@ -512,10 +518,13 @@ class TestPaceAndLifecycle:
     launch_index = next(index for index, result in enumerate(results) if result.launching)
 
     assert held.state == AccelControllerState.stopHold
-    assert held.target_speed == 0.0 and held.effective_accel_max == STOP_HOLD_ACCEL_MAX
+    assert held.target_speed == 0.0 and math.isinf(held.effective_accel_max)
+    assert held.mpc_accel_max is None
     assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index])
     assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
     assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM
+    assert results[launch_index].departure_launching
+    assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max)
 
   def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
     controller = make_controller()
@@ -532,13 +541,15 @@ class TestPaceAndLifecycle:
       assert held.effective_accel_max == pytest.approx(fresh.effective_accel_max)
       if frame < STOP_HOLD_EXIT_FRAMES - 1:
         assert fresh.state == AccelControllerState.stopHold
-        expected_limit = STOP_HOLD_ACCEL_MAX if frame == 0 else STOP_HOLD_DEPARTURE_ACCEL_MAX
-        assert fresh.effective_accel_max == expected_limit
+        assert math.isinf(fresh.effective_accel_max)
+        assert fresh.mpc_accel_max is None
 
     assert fresh.launching and held.launching
+    assert fresh.departure_launching and held.departure_launching
     assert fresh.target_speed == held.target_speed == 8.0
+    assert fresh.effective_accel_max == pytest.approx(fresh.positive_accel_max)
 
-  def test_single_frame_departure_stays_at_the_stationary_hold_limit(self):
+  def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self):
     controller = make_controller()
     enter_stop_hold(controller)
     departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
@@ -549,8 +560,8 @@ class TestPaceAndLifecycle:
 
     assert warm.state == held.state == AccelControllerState.stopHold
     assert not warm.launching and not held.launching
-    assert warm.effective_accel_max == STOP_HOLD_ACCEL_MAX
-    assert held.effective_accel_max == STOP_HOLD_ACCEL_MAX
+    assert math.isinf(warm.effective_accel_max) and warm.mpc_accel_max is None
+    assert math.isinf(held.effective_accel_max) and held.mpc_accel_max is None
     assert held.target_speed == 0.0
 
   def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self):
@@ -570,7 +581,8 @@ class TestPaceAndLifecycle:
 
     assert result.state == AccelControllerState.stopHold
     assert result.target_speed == 0.0
-    assert result.effective_accel_max == 0.0
+    assert math.isinf(result.effective_accel_max)
+    assert result.mpc_accel_max is None
 
   def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self):
     controller = make_controller()
@@ -579,8 +591,8 @@ class TestPaceAndLifecycle:
 
     assert missing.state == AccelControllerState.stopHold
     assert missing.target_speed == 0.0
-    assert missing.effective_accel_max == 0.0
-    assert missing.mpc_accel_max is not None and max(missing.mpc_accel_max) == 0.0
+    assert math.isinf(missing.effective_accel_max)
+    assert missing.mpc_accel_max is None
 
   def test_confirmed_creep_departure_does_not_reenter_stop_hold(self):
     controller = make_controller()
@@ -646,7 +658,7 @@ class TestPaceAndLifecycle:
     assert not result.active
     assert result.target_speed == result.base_speed
     assert result.mpc_accel_max is None
-    assert controller.live.pace is None and controller.live.accel_limit is None
+    assert controller.live.pace is None
 
   def test_shadow_history_never_steps_into_live_actuation(self):
     controller = make_controller()
@@ -667,8 +679,9 @@ class TestPaceAndLifecycle:
 
     assert controller._held_envelope is None
     for path in (controller.live, controller.shadow):
-      assert path.pace is None and path.accel_limit is None and path.matched_accel_limit is None
+      assert path.pace is None and path.matched_accel_limit is None
       assert path.state == AccelControllerState.inactive
       assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0
-      assert not path.launching and not path.departure_launch and not path.matched_lead and not path.braking_limited
+      assert not path.launching and not path.departure_launch and not path.matched_lead
+      assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed
       assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0

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

@@ -120,24 +120,41 @@ def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve():
   assert calls == [({}, 15.0, True, ceiling)]
 
 
-def test_valid_lead_stop_hold_keeps_raw_mpc_target_with_bounded_ceiling():
-  ceiling = tuple(np.linspace(0.1, 0.1, N + 1))
+def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling():
   planner, _ = planner_for_mpc_test(
-    target_speed=0.0, mpc_accel_max=ceiling, state=AccelControllerState.stopHold, selected_lead=0,
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0,
   )
   _, calls = run_controller_mpc(planner)
 
-  assert calls == [({}, 20.0, True, ceiling)]
+  assert calls == [({}, 20.0, True, None)]
 
 
-def test_missing_lead_stop_hold_keeps_zero_mpc_target_and_ceiling():
-  ceiling = tuple(np.zeros(N + 1))
+def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling():
   planner, _ = planner_for_mpc_test(
-    target_speed=0.0, mpc_accel_max=ceiling, state=AccelControllerState.stopHold, selected_lead=-1,
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1,
   )
   _, calls = run_controller_mpc(planner)
 
-  assert calls == [({}, 0.0, True, ceiling)]
+  assert calls == [({}, 0.0, True, None)]
+
+
+@pytest.mark.parametrize(
+  ("active", "departure_launching", "is_e2e", "expected"),
+  [
+    (True, True, False, False),
+    (True, False, False, True),
+    (False, True, False, True),
+    (True, True, True, True),
+  ],
+)
+def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, is_e2e, expected):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller_result = SimpleNamespace(
+    active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold,
+  )
+  assert planner.accel_controller_should_stop(True, is_e2e) is expected
+  expected_hold = active and not departure_launching and not is_e2e
+  assert planner.accel_controller_should_stop(False, is_e2e) is expected_hold
 
 
 @pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -143,13 +143,21 @@ class LongitudinalPlannerSP:
     result = self.accel_controller_result
     actuating = result.active and not is_e2e and not force_decel and not previous_mpc_failed
     valid_lead_stop_hold = (actuating and result.state == AccelControllerState.stopHold
-                            and getattr(result, 'selected_lead', -1) >= 0)
+                            and result.selected_lead >= 0)
     controller_v_cruise = mpc_v_cruise if valid_lead_stop_hold else min(mpc_v_cruise, result.target_speed) if actuating else mpc_v_cruise
     accel_max = result.mpc_accel_max if actuating else None
     self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max)
 
     return is_e2e
 
+  def accel_controller_should_stop(self, should_stop: bool, is_e2e: bool) -> bool:
+    result = self.accel_controller_result
+    if result is None or not result.active or is_e2e:
+      return should_stop
+    if result.departure_launching:
+      return False
+    return should_stop or result.state == AccelControllerState.stopHold
+
   def update(self, sm: messaging.SubMaster) -> None:
     self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
     self._read_accel_controller_params()

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

@@ -12,7 +12,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
 from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_SPEED_HEADROOM, MATCHED_PACE_DECEL_RATE,
+  MATCHED_PACE_DECEL_RATE, PACE_TARGET_RESERVE,
 )
 
 ACTUATOR_DYNAMICS = (
@@ -26,6 +26,7 @@ ACTUATOR_IDS = ("toyota", "honda", "gm", "hyundai", "ford")
 ROUTINE_GAP_TOLERANCE = 0.10
 ROUTINE_DECEL_TOLERANCE = 0.10
 DROPOUT_GAP_TOLERANCE = 0.15
+MOVING_LEAD_GAP_TOLERANCE = 0.12
 
 
 @dataclass
@@ -399,7 +400,7 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
     moving = np.flatnonzero(trace.speed > 0.01)
     assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
     assert len(moving) and trace.time[moving[0]] <= 1.0
-    assert np.interp(1.0, trace.time, trace.speed) >= 0.35
+    assert np.interp(1.0, trace.time, trace.speed) >= 0.33
     assert not np.any(trace.a_target < -0.05)
     assert trace.solver_failures == 0
 
@@ -408,7 +409,10 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
   np.testing.assert_allclose(traces[2].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0)
   speed_at_eight = [float(np.interp(8.0, trace.time, trace.speed)) for trace in traces]
   assert speed_at_eight[0] + 0.75 < speed_at_eight[1]
-  assert speed_at_eight[1] + 0.50 < speed_at_eight[2]
+  assert speed_at_eight[1] + 0.30 < speed_at_eight[2]
+  final_speed = [float(trace.speed[-1]) for trace in traces]
+  assert final_speed[0] + 1.25 < final_speed[1]
+  assert final_speed[1] + 0.75 < final_speed[2]
   ceiling_at_ten = [float(np.interp(10.0, trace.speed, trace.mpc_upper_min)) for trace in traces]
   assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2]
 
@@ -458,7 +462,9 @@ def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_sec
   moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
 
   assert not trace.launching[first_three].any()
+  assert trace.should_stop[first_three].all()
   assert len(release) and trace.time[release[0]] >= departure_time + 3 * DT_MDL
+  assert not trace.should_stop[release[0]]
   assert len(moving) and trace.time[moving[0]] <= departure_time + 3 * DT_MDL + 1.0
   assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
   assert trace.solver_failures == 0
@@ -473,10 +479,12 @@ def test_reused_radar_frames_do_not_pulse_stop_state_during_departure():
   )
   after_departure = trace.time >= departure_time
   should_stop = trace.should_stop[after_departure]
+  release = np.flatnonzero(after_departure & trace.launching)
   moving = np.flatnonzero(after_departure & (trace.speed > 0.05))
 
   assert np.count_nonzero(np.diff(should_stop.astype(int))) <= 1
-  assert len(moving) and trace.time[moving[0]] <= departure_time + 0.8
+  assert len(release) and len(moving)
+  assert trace.time[moving[0]] <= trace.time[release[0]] + 1.0
   assert not _has_propulsion_brake_cycle(trace.a_target[after_departure])
   assert trace.solver_failures == 0
 
@@ -491,6 +499,7 @@ def test_short_false_departure_does_not_launch_the_vehicle(departure_frames):
 
   assert np.max(trace.speed) < 0.01
   assert not trace.launching.any()
+  assert trace.should_stop.all()
   assert trace.state[-1] == int(AccelControllerState.stopHold)
   assert trace.solver_failures == 0
 
@@ -511,7 +520,7 @@ def test_matched_lead_recovery_preserves_profile_ordering(actuator_delay, actuat
   assert mean_accel[0] + 0.06 < mean_accel[1]
   assert mean_accel[1] + 0.025 < mean_accel[2]
   assert final_speed[0] < final_speed[1] < final_speed[2]
-  assert max(final_speed) < 10.0 + LEAD_MATCH_SPEED_HEADROOM
+  assert max(final_speed) < 13.5
   assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces)
   assert all(trace.solver_failures == 0 for trace in traces)
 
@@ -680,7 +689,7 @@ def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, a
   assert not _has_propulsion_brake_cycle(trace.a_target[response])
   assert not _has_brake_coast_brake(trace.a_target[response])
   assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
-  assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
+  assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(PACE_TARGET_RESERVE, MATCHED_PACE_DECEL_RATE * DT_MDL) + 1e-9
   assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE
   assert not trace.fcw.any()
   assert trace.solver_failures == 0
@@ -722,16 +731,16 @@ def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile)
   response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0)
   effective_accel_max = trace.effective_accel_max[response]
   selected_leads = trace.selected_lead[glitch]
+  finite_limits = np.isfinite(effective_accel_max)
   stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]])
 
   assert set(selected_leads) == {0, 1}
   assert np.count_nonzero(np.diff(selected_leads)) > 20
-  assert np.all(np.isfinite(effective_accel_max))
-  assert np.min(trace.profile_accel_max[response] - effective_accel_max) > 0.1
-  assert np.min(stock_accel_max - effective_accel_max) > 0.1
-  assert np.max(np.abs(np.diff(effective_accel_max))) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-12
+  assert np.all(effective_accel_max[finite_limits] <= trace.profile_accel_max[response][finite_limits] + 1e-9)
+  assert np.all(effective_accel_max[finite_limits] <= stock_accel_max[finite_limits] + 1e-9)
   assert np.max(trace.a_target[response]) > 0.2
   assert not _has_propulsion_brake_cycle(trace.a_target[response])
+  assert not _has_brake_coast_brake(trace.a_target[response])
   assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
   assert trace.raw_radar_passthrough.all()
   assert not trace.fcw.any()
@@ -739,7 +748,7 @@ def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile)
 
 
 @pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-def test_matched_lead_dropout_keeps_the_stored_acceleration_ceiling(profile):
+def test_matched_lead_dropout_keeps_the_profile_acceleration_ceiling(profile):
   dropout_start = 25.0
   dropout_end = 25.15
 
@@ -839,7 +848,7 @@ def test_decelerating_moving_lead_is_stock_safe_without_propulsion_reversal():
   assert not _has_propulsion_after_braking(trace.a_target[response])
   assert trace_p95 <= baseline_p95 + 0.02
   assert np.min(trace.acceleration) >= np.min(baseline.acceleration) - ROUTINE_DECEL_TOLERANCE
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE
+  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - MOVING_LEAD_GAP_TOLERANCE
   assert not trace.fcw.any()
   _assert_no_new_solver_failures(trace, baseline)
 
@@ -889,7 +898,7 @@ def test_far_lead_profiles_start_early_in_order_without_solver_failures(actuator
   for trace in traces:
     assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1
     assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) < 0.45
-    assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 4.0
+    assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 4.5
     assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
     assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0])
     assert not trace.fcw.any()
@@ -939,7 +948,7 @@ def test_matched_lead_slowdown_stays_smooth_without_a_second_braking_stage():
   desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed
 
   assert abs(np.mean(trace.speed[matched]) - 10.0) < 0.5
-  assert np.mean(trace.profile_accel_max[matched] - trace.effective_accel_max[matched]) > 0.1
+  np.testing.assert_allclose(trace.effective_accel_max[matched], trace.profile_accel_max[matched], atol=1e-9)
   assert not np.any(trace.state[response] == int(AccelControllerState.stopHold))
   assert not trace.launching[response].any()
   assert not _has_brake_coast_brake(trace.a_target[response])