Prevent launch hesitation and lead-handoff oscillation

selfdrive/controls/lib/longitudinal_planner.py

@@ -145,6 +145,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     if force_slow_decel:
       v_cruise = 0.0
 
+    if self.accel_controller_result.reset_mpc:
+      # An optimizer warm-start reset must not erase stock FCW evidence.
+      crash_cnt = self.mpc.crash_cnt
+      self.mpc.reset()
+      self.mpc.crash_cnt = crash_cnt
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     self.mpc.update(

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

@@ -42,20 +42,21 @@ class ProfileConfig:
 
 
 PROFILE_CONFIGS = {
-  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=0.65, release_confirm=0.50),
-  AccelProfile.normal: ProfileConfig(comfort_decel=0.335, release_rate=0.85, release_confirm=0.35),
-  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_rate=1.10, release_confirm=0.20),
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=0.90, release_confirm=0.50),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.335, release_rate=1.15, release_confirm=0.35),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_rate=1.45, release_confirm=0.20),
 }
 
 ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0]
 # These are pre-MPC profile requests. Values remain inside global ACCEL_MAX;
 # the planner's stock speed/turn/coast limit remains the final output authority.
 ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [1.55, 0.30, 0.20, 0.10],
-  AccelProfile.normal: [1.70, 0.90, 0.40, 0.20],
+  AccelProfile.eco: [1.55, 0.85, 0.45, 0.25],
+  AccelProfile.normal: [1.65, 1.10, 0.70, 0.45],
   AccelProfile.sport: [2.00, 1.70, 1.20, 0.90],
 }
 LAUNCH_DELTA_V = 3.0
+LAUNCH_TARGET_HANDOFF_SPEED = 1.0
 
 CAP_FILTER_FRAMES = 5
 RESTRICT_DEADBAND = 0.15
@@ -69,13 +70,20 @@ LAUNCH_PROFILE_HANDOFF_SPEED = 0.05
 VEGO_NOISE_TOLERANCE = 0.10
 ACCEL_LIMIT_JERK = 1.0
 DECEL_LIMIT_JERK = 1.10
-LAUNCH_ACCEL_RATE = 4.0
+LAUNCH_ACCEL_RATE = 5.0
 CLEAR_LAUNCH_ACCEL_RATE = 3.0
 INITIAL_LAUNCH_ACCEL_MAX = 0.95
 BREAKAWAY_ACCEL_MAX = 1.15
 HOLD_ACCEL_MAX = 0.10
-MATCHED_LEAD_CLOSING_SPEED = 0.02
 MATCHED_LEAD_PROFILE_ACCEL = -0.90
+MATCHED_LEAD_ACCEL_TAPER_SPEED = 1.25
+MATCHED_LEAD_ACCEL_GAIN = 0.80
+NO_PROPULSION_CLOSING_SPEED = 0.02
+MATERIAL_CLOSING_DECEL_ENTER = 0.08
+MATERIAL_CLOSING_DECEL_EXIT = 0.03
+MATERIAL_CLOSING_SPEED_ENTER = 0.25
+MATERIAL_CLOSING_SPEED_EXIT = 0.10
+MATERIAL_CLOSING_EXIT_FRAMES = 3
 LAUNCH_PACE_RATE = 5.0
 LEAD_ACQUISITION_INITIAL_AUTHORITY = 0.20
 LEAD_ACQUISITION_TIME = 0.30
@@ -85,6 +93,7 @@ LEAD_ACQUISITION_MIN_TTC = 10.0
 LEAD_ACQUISITION_MAX_DECEL = 0.25
 LEAD_ACQUISITION_MIN_LEAD_ACCEL = -0.50
 LEAD_ACQUISITION_MIN_PLANNER_ACCEL = -0.10
+LEAD_DEPARTURE_INITIAL_AUTHORITY = 0.0
 LEAD_DEPARTURE_HANDOFF_TIME = 0.50
 RELATIVE_PACE_PREVIEW_TIME = 3.0
 URGENT_BYPASS_REQUIRED_DECEL = 0.45
@@ -119,6 +128,7 @@ class AccelControllerResult:
   effective_accel_max: float
   mpc_accel_max: tuple[float, ...] | None
   mpc_shape_cruise: bool
+  reset_mpc: bool
   lead_obstacle_weights: tuple[float, float]
   state: AccelControllerState
   shadow_state: AccelControllerState
@@ -142,6 +152,7 @@ class _PacePath:
   relief_time: float = 0.0
   departure_frames: int = 0
   departing_from_stop: bool = False
+  launch_target_active: bool = False
   departure_handoff_active: bool = False
   stop_departure_confirmed: bool = False
   stopped_lead_hold: bool = False
@@ -150,6 +161,8 @@ class _PacePath:
   urgent_bypass_active: bool = False
   urgent_recovery_active: bool = False
   urgent_dropout_frames: int = 0
+  material_closing: bool = False
+  material_relief_frames: int = 0
   lead_seen: list[bool] = field(default_factory=lambda: [False, False])
   lead_track_ids: list[int] = field(default_factory=lambda: [-1, -1])
   lead_obstacle_weights: list[float] = field(default_factory=lambda: [1.0, 1.0])
@@ -161,6 +174,7 @@ class _PacePath:
     self.relief_time = 0.0
     self.departure_frames = 0
     self.departing_from_stop = False
+    self.launch_target_active = False
     self.departure_handoff_active = False
     self.stop_departure_confirmed = False
     self.stopped_lead_hold = False
@@ -169,6 +183,8 @@ class _PacePath:
     self.urgent_bypass_active = False
     self.urgent_recovery_active = False
     self.urgent_dropout_frames = 0
+    self.material_closing = False
+    self.material_relief_frames = 0
     self.lead_seen = [False, False]
     self.lead_track_ids = [-1, -1]
     self.lead_obstacle_weights = [1.0, 1.0]
@@ -377,7 +393,7 @@ class AccelController:
   ) -> tuple[float, float]:
     """Ramp benign new obstacles into MPC while making every urgent lead immediate."""
     authority_step = (1.0 - LEAD_ACQUISITION_INITIAL_AUTHORITY) * self.dt / LEAD_ACQUISITION_TIME
-    departure_step = self.dt / LEAD_DEPARTURE_HANDOFF_TIME
+    departure_step = (1.0 - LEAD_DEPARTURE_INITIAL_AUTHORITY) * self.dt / LEAD_DEPARTURE_HANDOFF_TIME
     for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
       if not self._valid_lead(lead):
         path.lead_seen[lead_index] = False
@@ -396,11 +412,20 @@ class AccelController:
         and v_ego <= float(lead.vLeadK)
       )
 
-      if safe_departure_handoff:
-        if path.departing_from_stop:
-          path.lead_obstacle_weights[lead_index] = 0.0
+      if path.departure_handoff_active:
+        if safe_departure_handoff:
+          # Keep enough cruise incentive to clear standstill actuator deadband,
+          # then restore full raw-lead authority over the configured handoff. The
+          # previous zero-until-motion handoff produced a large launch pulse.
+          previous_weight = path.lead_obstacle_weights[lead_index]
+          if previous_weight <= LEAD_DEPARTURE_INITIAL_AUTHORITY and v_ego < LAUNCH_PROFILE_HANDOFF_SPEED:
+            path.lead_obstacle_weights[lead_index] = LEAD_DEPARTURE_INITIAL_AUTHORITY
+          else:
+            path.lead_obstacle_weights[lead_index] = min(1.0, max(previous_weight, LEAD_DEPARTURE_INITIAL_AUTHORITY) + departure_step)
         else:
-          path.lead_obstacle_weights[lead_index] = min(1.0, path.lead_obstacle_weights[lead_index] + departure_step)
+          # Any renewed closing or stopped-lead evidence restores stock
+          # obstacle authority immediately.
+          path.lead_obstacle_weights[lead_index] = 1.0
       elif new_lead:
         path.lead_obstacle_weights[lead_index] = LEAD_ACQUISITION_INITIAL_AUTHORITY if allow_blend and benign else 1.0
       elif path.lead_obstacle_weights[lead_index] < 1.0:
@@ -426,7 +451,7 @@ class AccelController:
       # not require enough braking for a newly detected close stopped lead.
       hold_weight = 0.0 if v_ego < v_ego_stopping else 1.0
       path.lead_obstacle_weights = [hold_weight, hold_weight]
-    elif path.departure_handoff_active and all(
+    elif path.departure_handoff_active and not path.departing_from_stop and all(
       not seen or weight >= 1.0 for seen, weight in zip(path.lead_seen, path.lead_obstacle_weights, strict=True)
     ):
       path.departure_handoff_active = False
@@ -453,18 +478,50 @@ class AccelController:
     previous_should_stop: bool,
     has_nearly_stopped_lead: bool,
     departure_lead_speed: float,
+    selected_lead_speed: float,
     closing_speed: float,
+    required_decel: float,
     launch_delta_v: float,
   ) -> float:
     filtered_cap = path.update_filter(raw_cap)
+
+    # Treat only meaningful relative-energy demand as a closing event. Radar
+    # track switches routinely move raw relative speed a few cm/s across zero;
+    # using that sign directly made the full-horizon acceleration ceiling flip
+    # between braking and gas. Hysteresis keeps material approaches prompt while
+    # letting matched traffic coast through harmless lead noise.
+    if math.isfinite(raw_cap):
+      restrictive_evidence = (
+        required_decel >= MATERIAL_CLOSING_DECEL_ENTER
+        or closing_speed >= MATERIAL_CLOSING_SPEED_ENTER
+      )
+      relief_evidence = (
+        required_decel <= MATERIAL_CLOSING_DECEL_EXIT
+        and closing_speed <= MATERIAL_CLOSING_SPEED_EXIT
+      )
+      if restrictive_evidence:
+        path.material_closing = True
+        path.material_relief_frames = 0
+      elif path.material_closing and relief_evidence:
+        path.material_relief_frames += 1
+        if path.material_relief_frames >= MATERIAL_CLOSING_EXIT_FRAMES:
+          path.material_closing = False
+          path.material_relief_frames = 0
+      else:
+        path.material_relief_frames = 0
+    elif not math.isfinite(filtered_cap):
+      path.material_closing = False
+      path.material_relief_frames = 0
+
     just_initialized = path.pace is None
     if just_initialized:
       # A clear road has no prior restriction to release from, so expose base
-      # cruise immediately. With any lead present, seed at ego instead: the
-      # first radar frame can contain a large aLeadK spike, and using that
-      # transient energy cap as pace caused several seconds of acceleration
-      # before a late brake.
-      path.pace = base_speed if not math.isfinite(raw_cap) else min(base_speed, v_ego)
+      # cruise immediately. Any lead starts from ego pace; raw lead motion is
+      # not trusted to raise the target on its first observation.
+      if not math.isfinite(raw_cap):
+        path.pace = base_speed
+      else:
+        path.pace = min(base_speed, v_ego)
       path.state = AccelControllerState.free
 
     # A clear-road standstill engagement should request motion immediately. A
@@ -474,6 +531,7 @@ class AccelController:
       path.state = AccelControllerState.release
       path.relief_time = 0.0
       path.departing_from_stop = True
+      path.launch_target_active = True
       return filtered_cap
 
     # A lower non-controller target is authoritative, and is also the correct seed if it later clears.
@@ -491,6 +549,21 @@ class AccelController:
       if v_ego >= STOP_HOLD_EGO_SPEED:
         path.stop_departure_confirmed = False
 
+    if path.launch_target_active:
+      if v_ego >= LAUNCH_TARGET_HANDOFF_SPEED:
+        # Handoff from the base-cruise launch target without replacing it with
+        # a stale pace only a few tenths above ego in one frame. Keep the same
+        # bounded launch preview used at departure; profile acceleration and
+        # the ordinary release ramp retain authority from here.
+        path.pace = min(base_speed, max(path.pace, v_ego + launch_delta_v))
+        path.launch_target_active = False
+      elif (
+        has_nearly_stopped_lead
+        or closing_speed > MATERIAL_CLOSING_SPEED_EXIT
+        or required_decel >= MATERIAL_CLOSING_DECEL_ENTER
+      ):
+        path.launch_target_active = False
+
     renewed_stop_evidence = filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead
     stale_plan_stop = previous_should_stop and not path.departing_from_stop and not path.stop_departure_confirmed
     enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or stale_plan_stop)
@@ -500,6 +573,7 @@ class AccelController:
       path.relief_time = 0.0
       path.departure_frames = 0
       path.departing_from_stop = False
+      path.launch_target_active = False
       path.departure_handoff_active = False
       path.stop_departure_confirmed = False
       return filtered_cap
@@ -524,6 +598,7 @@ class AccelController:
       path.relief_time = config.release_confirm
       path.departure_frames = 0
       path.departing_from_stop = True
+      path.launch_target_active = True
       path.departure_handoff_active = True
       path.stop_departure_confirmed = True
       path.stopped_lead_hold = False
@@ -531,7 +606,20 @@ class AccelController:
       return filtered_cap
 
     ceiling = min(base_speed, filtered_cap)
-    if math.isfinite(raw_cap) and closing_speed > 0.0:
+    matched_moving_lead = (
+      math.isfinite(selected_lead_speed)
+      and closing_speed <= MATERIAL_CLOSING_SPEED_EXIT
+      and v_ego <= selected_lead_speed + MATERIAL_CLOSING_SPEED_EXIT
+    )
+    if matched_moving_lead:
+      # Once ego is no longer closing, surplus gap is not a reason to target a
+      # speed above the lead and manufacture another braking event. Upward
+      # changes still use the profile release ramp; only an already-high pace
+      # is synchronized down to the moving lead.
+      matched_ceiling = min(base_speed, max(selected_lead_speed, 0.0))
+      path.pace = min(path.pace, matched_ceiling)
+      ceiling = min(ceiling, matched_ceiling)
+    if math.isfinite(raw_cap) and path.material_closing:
       # Never spend stored gap by accelerating toward a slower lead. Hold the
       # current pace until relative speed is matched; the energy envelope may
       # still lower it at the configured comfort rate.
@@ -541,12 +629,15 @@ class AccelController:
       path.state = AccelControllerState.restrict
       path.relief_time = 0.0
       path.departing_from_stop = False
+      path.launch_target_active = False
       path.departure_handoff_active = False
       return filtered_cap
 
     relief = ceiling - path.pace
+    confirmed_clear = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap)
+    relief_deadband = RESTRICT_DEADBAND if confirmed_clear else RELIEF_DEADBAND
     release_allowed = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND
-    if relief >= RELIEF_DEADBAND and not release_allowed:
+    if relief >= relief_deadband and not release_allowed:
       path.relief_time += self.dt
       path.state = AccelControllerState.hold
       release_allowed = path.relief_time >= config.release_confirm
@@ -555,9 +646,15 @@ class AccelController:
       pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else config.release_rate
       path.pace = min(ceiling, path.pace + pace_rate * self.dt)
       path.state = AccelControllerState.release
-    elif relief <= RELIEF_DEADBAND:
+    elif relief <= relief_deadband:
       path.relief_time = 0.0
-      path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.hold
+      if confirmed_clear:
+        # Close the final sub-deadband clear-road gap instead of leaving the
+        # controller indefinitely in HOLD_ACCEL_MAX with no lead present.
+        path.pace = ceiling
+        path.state = AccelControllerState.free
+      else:
+        path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.hold
 
     return filtered_cap
 
@@ -568,8 +665,11 @@ class AccelController:
     planner_accel: float,
     profile_accel_max: float,
     config: ProfileConfig,
+    v_ego: float,
+    selected_lead_speed: float,
     closing_speed: float,
     lead_present: bool,
+    material_closing: bool,
   ) -> tuple[float, float]:
     """Return telemetry effective max and the controller's pre-MPC upper bound."""
     profile_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
@@ -597,8 +697,9 @@ class AccelController:
         path.accel_limit = min(launch_target, previous_limit + LAUNCH_ACCEL_RATE * self.dt)
       else:
         # Start below the solver's standstill cold-start edge, then reach the
-        # common breakaway floor over two controller frames. The lookup table
-        # takes over after the first few centimeters.
+        # common breakaway floor over two controller frames. The launch horizon
+        # exposes each profile's higher future ceiling without stepping the
+        # near-time constraint that keeps the one-iteration solver stable.
         if path.accel_limit is None:
           path.accel_limit = min(INITIAL_LAUNCH_ACCEL_MAX, BREAKAWAY_ACCEL_MAX)
         else:
@@ -621,7 +722,7 @@ class AccelController:
         path.decel_limit_active = path.state == AccelControllerState.restrict
         return min(stock_accel_max, path.accel_limit), path.accel_limit
 
-    if path.state == AccelControllerState.restrict and (not lead_present or closing_speed > MATCHED_LEAD_CLOSING_SPEED):
+    if path.state == AccelControllerState.restrict and (not lead_present or material_closing):
       # A negative full-horizon maximum is useful only while ego is still
       # closing. Keeping it after matching relative speed forces continued
       # braking below a slower moving lead. Once matched, retain the lowering
@@ -637,11 +738,25 @@ class AccelController:
         if path.accel_limit is None:
           path.accel_limit = max(0.0, planner_accel)
       path.decel_limit_active = True
-    elif lead_present and closing_speed > MATCHED_LEAD_CLOSING_SPEED:
+    elif lead_present and material_closing:
       # Do not spend the remaining gap by accelerating toward a slower lead,
       # regardless of whether the scalar pace state is currently releasing.
       requested_limit = HOLD_ACCEL_MAX
       path.decel_limit_active = False
+    elif lead_present and closing_speed > NO_PROPULSION_CLOSING_SPEED:
+      # Small closing rates do not justify a forced negative comfort ceiling,
+      # but they still must not receive positive propulsion. Keeping this
+      # separate from material-closing deceleration prevents both gas/brake
+      # cycling and centimeter-per-second radar-noise chatter.
+      requested_limit = HOLD_ACCEL_MAX
+      path.decel_limit_active = False
+    elif lead_present and math.isfinite(selected_lead_speed) and selected_lead_speed - v_ego <= MATCHED_LEAD_ACCEL_TAPER_SPEED:
+      # Taper propulsion before reaching a moving lead, accounting for the
+      # acceleration already in the planner/actuator path. Waiting until ego
+      # is faster leaves too much stored acceleration and creates overshoot.
+      speed_error = max(selected_lead_speed - v_ego, 0.0)
+      requested_limit = min(profile_limit, max(HOLD_ACCEL_MAX, MATCHED_LEAD_ACCEL_GAIN * speed_error))
+      path.decel_limit_active = False
     elif path.state in (AccelControllerState.restrict, AccelControllerState.hold):
       # No gas while waiting for relief confirmation. This is the main
       # anti-rubber-band rule for a still-closing lead.
@@ -664,13 +779,9 @@ class AccelController:
     effective_limit = min(stock_accel_max, path.accel_limit)
     return effective_limit, path.accel_limit
 
-  def _build_mpc_accel_max(
-    self,
-    path: _PacePath,
-    accel_limit: float,
-  ) -> tuple[float, ...] | None:
+  def _build_mpc_accel_max(self, accel_limit: float | None) -> tuple[float, ...] | None:
     """Build the controller's pre-MPC acceleration upper-bound trajectory."""
-    if not math.isfinite(accel_limit):
+    if accel_limit is None or not math.isfinite(accel_limit):
       return None
 
     bounded_limit = float(np.clip(accel_limit, ACCEL_MIN, ACCEL_MAX))
@@ -743,6 +854,9 @@ class AccelController:
     )
 
     envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
+    selected_lead_speed = math.inf
+    if envelope.selected_lead in (0, 1):
+      selected_lead_speed = float(getattr((radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead], "vLeadK", math.inf))
 
     if valid_context:
       shadow_filtered_cap = self._update_path(
@@ -756,7 +870,9 @@ class AccelController:
         previous_should_stop,
         envelope.has_nearly_stopped_lead,
         envelope.departure_lead_speed,
+        selected_lead_speed,
         envelope.closing_speed,
+        envelope.required_decel,
         launch_delta_v,
       )
       self._update_accel_limit(
@@ -765,8 +881,11 @@ class AccelController:
         planner_accel,
         profile_accel_max,
         config,
+        sanitized_v_ego,
+        selected_lead_speed,
         envelope.closing_speed,
         envelope.selected_lead >= 0,
+        self.shadow.material_closing,
       )
       shadow_active = True
     else:
@@ -774,6 +893,7 @@ class AccelController:
       shadow_filtered_cap = math.inf
       shadow_active = False
 
+    reset_mpc = False
     live_active = valid_context and bool(enabled) and bool(acc_selected)
     if live_active:
       live_was_initialized = self.live.pace is not None
@@ -795,7 +915,9 @@ class AccelController:
         previous_should_stop,
         envelope.has_nearly_stopped_lead,
         envelope.departure_lead_speed,
+        selected_lead_speed,
         envelope.closing_speed,
+        envelope.required_decel,
         launch_delta_v,
       )
       lead_obstacle_weights = self._update_lead_obstacle_weights(
@@ -808,18 +930,18 @@ class AccelController:
         allow_blend=live_was_initialized,
       )
       urgent_was_active = self.live.urgent_bypass_active
-      pre_urgent_accel_limit = self.live.accel_limit
-      selected_lead_speed = math.inf
-      if envelope.selected_lead in (0, 1):
-        selected_lead_speed = float(getattr((radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead], "vLeadK", math.inf))
       selected_has_full_authority = (
         envelope.selected_lead in (0, 1) and lead_obstacle_weights[envelope.selected_lead] >= 1.0
       )
       urgent_required_decel = envelope.required_decel
       if not live_was_initialized or not established_selected_lead:
         urgent_required_decel = max(urgent_required_decel, envelope.conservative_required_decel)
+      low_speed_urgent_lead = (
+        selected_lead_speed < URGENT_LOW_SPEED_LEAD_BYPASS
+        and self.live.state != AccelControllerState.stopHold
+      )
       urgent_trigger = (
-        sanitized_v_ego >= URGENT_BYPASS_MIN_SPEED
+        (sanitized_v_ego >= URGENT_BYPASS_MIN_SPEED or low_speed_urgent_lead)
         and urgent_required_decel >= URGENT_BYPASS_REQUIRED_DECEL
         and (not live_was_initialized or established_selected_lead or selected_has_full_authority)
       )
@@ -878,43 +1000,55 @@ class AccelController:
         self.live.decel_limit_active = self.live.accel_limit < 0.0
         self.live.urgent_recovery_active = False
         effective_accel_max = min(stock_accel_max, self.live.accel_limit)
-        mpc_accel_max = self._build_mpc_accel_max(self.live, self.live.accel_limit)
+        mpc_accel_max = self._build_mpc_accel_max(self.live.accel_limit)
         mpc_shape_cruise = True
         lead_obstacle_weights = (1.0, 1.0)
         target_speed = min(base_speed, self.live.pace if self.live.pace is not None else sanitized_v_ego)
       elif urgent_bypass:
-        # Comfort shaping must never compete with urgent braking. Hand the raw
-        # leads, base cruise target, and stock acceleration bounds directly to
-        # MPC. On the rising edge only, retain a prior nonnegative upper bound
-        # for one solve so introducing the raw obstacle does not simultaneously
-        # remove an optimizer constraint. A nonnegative maximum cannot weaken
-        # braking, and the following urgent frame returns to stock bounds.
-        urgent_entry_limit = None
-        if not urgent_was_active and pre_urgent_accel_limit is not None and math.isfinite(pre_urgent_accel_limit):
-          urgent_entry_limit = float(np.clip(max(pre_urgent_accel_limit, 0.0), 0.0, ACCEL_MAX))
+        # Hand raw leads, base cruise, and stock bounds directly to MPC. Clear
+        # the old comfort-control warm start once on urgent entry so its prior
+        # positive-acceleration solution cannot destabilize the abrupt raw-lead
+        # solve. The reset occurs before set_cur_state/update in the planner.
+        reset_mpc = reset_mpc or not urgent_was_active
+        self.live.launch_target_active = False
         self.live.accel_limit = None
         self.live.decel_limit_active = False
         self.live.urgent_recovery_active = False
         if self.live.pace is not None:
           self.live.pace = min(self.live.pace, planner_speed, sanitized_v_ego)
-        effective_accel_max = min(stock_accel_max, urgent_entry_limit) if urgent_entry_limit is not None else stock_accel_max
-        mpc_accel_max = self._build_mpc_accel_max(self.live, urgent_entry_limit) if urgent_entry_limit is not None else None
+        effective_accel_max = stock_accel_max
+        mpc_accel_max = None
         mpc_shape_cruise = False
         lead_obstacle_weights = (1.0, 1.0)
         target_speed = base_speed
       else:
+        recovery_was_active = self.live.urgent_recovery_active
         effective_accel_max, controller_accel_max = self._update_accel_limit(
           self.live,
           stock_accel_max,
           planner_accel,
           profile_accel_max,
           config,
+          sanitized_v_ego,
+          selected_lead_speed,
           envelope.closing_speed,
           envelope.selected_lead >= 0,
+          self.live.material_closing,
         )
+        if (
+          recovery_was_active
+          and not self.live.urgent_recovery_active
+          and envelope.closing_speed <= 0.10
+          and math.isfinite(selected_lead_speed)
+          and self.live.pace is not None
+        ):
+          # Once relative speed is matched, never carry a recovery pace below
+          # the moving lead. This avoids the old choice between a base-cruise
+          # gas pulse and prolonged braking below lead speed.
+          self.live.pace = max(self.live.pace, min(base_speed, selected_lead_speed))
         # Feed only the controller-owned ceiling into MPC. Stock's speed, turn,
         # coast, and no-throttle limits remain in their original output clip.
-        mpc_accel_max = self._build_mpc_accel_max(self.live, controller_accel_max)
+        mpc_accel_max = self._build_mpc_accel_max(controller_accel_max)
         mpc_shape_cruise = mpc_accel_max is not None
         if mpc_accel_max is None:
           effective_accel_max = stock_accel_max
@@ -924,16 +1058,34 @@ class AccelController:
           # 0.30 m/s hold threshold; keeping base cruise there can otherwise
           # permit a slow coast while lead authority is intentionally muted.
           target_speed = 0.0
-        elif self.live.departing_from_stop:
-          # Give all profiles the same prompt stock breakaway. The raw lead still
-          # owns obstacle braking, and profile separation begins after motion.
+        elif self.live.departing_from_stop or self.live.launch_target_active:
+          # Keep stock's normal cruise incentive through the first meter per
+          # second of a confirmed launch. The lookup ceiling shapes takeoff
+          # while raw-lead authority restores; renewed closing evidence
+          # cancels this path immediately.
           target_speed = base_speed
+        elif (
+          envelope.selected_lead >= 0
+          and math.isfinite(selected_lead_speed)
+          and selected_lead_speed > STOPPED_LEAD_SPEED
+          and envelope.closing_speed <= MATERIAL_CLOSING_SPEED_EXIT
+          and envelope.required_decel <= MATERIAL_CLOSING_DECEL_EXIT
+          and planner_accel < -0.20
+          and not self.live.material_closing
+        ):
+          # A moving lead that has already matched ego no longer needs the
+          # cruise obstacle to reinforce residual braking. Let raw lead
+          # geometry hold the gap while base cruise helps MPC unwind smoothly;
+          # any renewed relative-energy demand exits through material_closing.
+          # Keep mpc_accel_max as a hard bound, but do not also use that small
+          # comfort ceiling to clip the cruise speed trajectory below the lead.
+          target_speed = base_speed
+          mpc_shape_cruise = False
+          lead_obstacle_weights = (1.0, 1.0)
         elif self.live.urgent_recovery_active:
-          # Keep stock's cruise incentive while the positive recovery ceiling
-          # gently unwinds the hard lead-braking plan. Switching immediately
-          # to the synchronized pace can make MPC keep braking well below a
-          # moving lead even though the ceiling itself is nonnegative.
-          target_speed = base_speed
+          # Keep full raw-lead authority while the pre-MPC ceiling rejoins, but
+          # retain the synchronized pace so relief cannot become a gas pulse.
+          target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
           lead_obstacle_weights = (1.0, 1.0)
         else:
           target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
@@ -958,6 +1110,7 @@ class AccelController:
       effective_accel_max=effective_accel_max,
       mpc_accel_max=mpc_accel_max,
       mpc_shape_cruise=mpc_shape_cruise,
+      reset_mpc=reset_mpc,
       lead_obstacle_weights=lead_obstacle_weights,
       state=self.live.state,
       shadow_state=self.shadow.state,

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

@@ -83,8 +83,8 @@ class TestAccelProfileLimits:
   def test_profile_table_matches_tuned_values(self):
     assert ACCEL_PROFILE_MAX_BP == [0.0, 10.0, 25.0, 40.0]
     assert ACCEL_PROFILE_MAX_V == {
-      AccelProfile.eco: [1.55, 0.30, 0.20, 0.10],
-      AccelProfile.normal: [1.70, 0.90, 0.40, 0.20],
+      AccelProfile.eco: [1.55, 0.85, 0.45, 0.25],
+      AccelProfile.normal: [1.65, 1.10, 0.70, 0.45],
       AccelProfile.sport: [2.00, 1.70, 1.20, 0.90],
     }
 
@@ -139,7 +139,7 @@ class TestAccelProfileLimits:
 
     result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5)
 
-    assert result.profile_accel_max == pytest.approx(0.25)
+    assert result.profile_accel_max == pytest.approx(0.65)
 
   def test_clear_road_profile_is_a_separate_pre_mpc_trajectory(self):
     governor = make_governor()
@@ -149,7 +149,7 @@ class TestAccelProfileLimits:
     assert result.mpc_accel_max is not None
     assert result.mpc_shape_cruise
     assert len(result.mpc_accel_max) == N + 1
-    assert_profile_trajectory(result, 0.90)
+    assert_profile_trajectory(result, 1.10)
 
   def test_ordinary_closing_lead_uses_no_gas_pre_mpc_bound(self):
     governor = make_governor()
@@ -210,9 +210,9 @@ class TestAccelProfileLimits:
 
     result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
 
-    assert result.profile_accel_max == 0.90
-    assert result.effective_accel_max == 0.90
-    assert_profile_trajectory(result, 0.90)
+    assert result.profile_accel_max == 1.10
+    assert result.effective_accel_max == 1.10
+    assert_profile_trajectory(result, 1.10)
 
   def test_first_enable_seeds_from_positive_planner_accel_within_stock(self):
     governor = make_governor()
@@ -255,9 +255,9 @@ class TestAccelProfileLimits:
     released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
 
     assert tightened.effective_accel_max == 0.40
-    assert_profile_trajectory(tightened, 0.90)
-    assert released.effective_accel_max == 0.90
-    assert_profile_trajectory(released, 0.90)
+    assert_profile_trajectory(tightened, 1.10)
+    assert released.effective_accel_max == 1.10
+    assert_profile_trajectory(released, 1.10)
 
   def test_negative_stock_max_remains_authoritative_outside_the_mpc_profile_bound(self):
     governor = make_governor()
@@ -265,7 +265,7 @@ class TestAccelProfileLimits:
     result = update(governor, v_ego=10.0, planner_speed=10.0, stock_accel_max=-0.20, planner_accel=1.0)
 
     assert result.effective_accel_max == -0.20
-    assert_profile_trajectory(result, 1.0)
+    assert_profile_trajectory(result, 1.10)
 
   def test_profile_tightening_can_converge_below_positive_planner_accel(self):
     governor = make_governor()
@@ -337,6 +337,7 @@ class TestLeadObstacleAcquisition:
 
     assert result.required_decel > 1.0
     assert result.lead_obstacle_weights == (1.0, 1.0)
+    assert result.reset_mpc
 
   @pytest.mark.parametrize(
     "lead",
@@ -526,6 +527,66 @@ class TestAccelControllerState:
     assert result.mpc_accel_max is None
     assert result.lead_obstacle_weights == (1.0, 1.0)
 
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_e5_low_speed_urgency_hands_back_to_stock_immediately(self, profile):
+    governor = make_governor(delay=0.15)
+    established_lead = make_radar(make_lead(
+      status=True,
+      d_rel=18.68,
+      v_lead_k=2.876,
+      a_lead_k=-0.743,
+      radar_track_id=7,
+    ))
+    established_args = {
+      "profile": profile,
+      "base_speed": 23.056,
+      "v_ego": 5.015,
+      "a_ego": -0.949,
+      "planner_speed": 5.015,
+      "planner_accel": -1.021,
+      "stock_accel_max": 1.40,
+    }
+
+    for _ in range(5):
+      established = update(governor, established_lead, **established_args)
+
+    assert established.required_decel < URGENT_BYPASS_REQUIRED_DECEL
+    assert not governor.live.urgent_bypass_active
+    assert established.target_speed < established.base_speed
+    assert established.mpc_shape_cruise
+
+    urgent_lead = make_radar(make_lead(
+      status=True,
+      d_rel=16.32,
+      v_lead_k=1.89,
+      a_lead_k=-1.16,
+      radar_track_id=7,
+    ))
+    urgent_args = established_args | {
+      "v_ego": 4.499,
+      "a_ego": -0.75,
+      "planner_speed": 4.499,
+      "planner_accel": -0.95,
+    }
+    entering = update(governor, urgent_lead, **urgent_args)
+
+    assert entering.required_decel > URGENT_BYPASS_REQUIRED_DECEL
+    assert governor.live.urgent_bypass_active
+    assert entering.target_speed == entering.base_speed
+    assert entering.lead_obstacle_weights == (1.0, 1.0)
+    assert not entering.mpc_shape_cruise
+    assert entering.mpc_accel_max is None
+    assert entering.effective_accel_max == urgent_args["stock_accel_max"]
+
+    stock_owned = update(governor, urgent_lead, **urgent_args)
+
+    assert governor.live.urgent_bypass_active
+    assert stock_owned.target_speed == stock_owned.base_speed
+    assert stock_owned.lead_obstacle_weights == (1.0, 1.0)
+    assert not stock_owned.mpc_shape_cruise
+    assert stock_owned.mpc_accel_max is None
+    assert stock_owned.effective_accel_max == urgent_args["stock_accel_max"]
+
   def test_positive_lead_accel_spike_cannot_delay_first_urgent_bypass(self):
     governor = make_governor()
     radar_state = make_radar(make_lead(status=True, d_rel=90.0, v_lead_k=14.0, a_lead_k=5.0))
@@ -538,7 +599,7 @@ class TestAccelControllerState:
     assert result.mpc_accel_max is None
     assert result.lead_obstacle_weights == (1.0, 1.0)
 
-  def test_urgent_entry_bridges_existing_nonnegative_ceiling_for_one_frame(self):
+  def test_urgent_entry_removes_existing_ceiling_on_first_frame(self):
     governor = make_governor()
     clear = update(governor, base_speed=40.0, v_ego=34.8, planner_speed=34.8)
     urgent_radar = make_radar(make_lead(status=True, d_rel=94.0, v_lead_k=23.4, radar_track_id=22))
@@ -548,10 +609,13 @@ class TestAccelControllerState:
 
     assert clear.mpc_accel_max is not None
     assert entering.required_decel > URGENT_BYPASS_REQUIRED_DECEL
-    assert entering.mpc_accel_max == clear.mpc_accel_max
+    assert entering.mpc_accel_max is None
+    assert entering.reset_mpc
     assert not entering.mpc_shape_cruise
     assert entering.lead_obstacle_weights == (1.0, 1.0)
     assert established.mpc_accel_max is None
+    assert not established.reset_mpc
+    assert not established.mpc_shape_cruise
 
   def test_urgent_dropout_holds_pace_and_nonpositive_ceiling_without_lead_geometry(self):
     governor = make_governor()
@@ -574,7 +638,8 @@ class TestAccelControllerState:
     assert not governor.live.urgent_recovery_active
     assert expired.mpc_accel_max is not None
     assert expired.target_speed == expired.live_pace
-    assert expired.target_speed < expired.base_speed
+    assert expired.target_speed == expired.base_speed
+    assert expired.state == AccelControllerState.free
 
   def test_urgent_exit_slews_rejoin_ceiling_then_releases_after_speed_match(self):
     governor = make_governor()
@@ -626,8 +691,7 @@ class TestAccelControllerState:
     assert renewed.required_decel > URGENT_BYPASS_REQUIRED_DECEL
     assert governor.live.urgent_bypass_active
     assert not governor.live.urgent_recovery_active
-    assert renewed.mpc_accel_max is not None
-    assert min(renewed.mpc_accel_max) >= 0.0
+    assert renewed.mpc_accel_max is None
     assert not renewed.mpc_shape_cruise
     assert renewed.lead_obstacle_weights == (1.0, 1.0)
 
@@ -658,6 +722,21 @@ class TestAccelControllerState:
     expected_rate = PROFILE_CONFIGS[AccelProfile.normal].release_rate
     assert result.live_pace == pytest.approx(held_pace + expected_rate * DT_MDL)
 
+  def test_confirmed_clear_closes_sub_deadband_hold_to_base(self):
+    governor = make_governor()
+    base_speed = 20.0
+    governor.live.pace = base_speed - 0.10
+    governor.live.state = AccelControllerState.hold
+    governor.live.accel_limit = HOLD_ACCEL_MAX
+
+    result = update(governor, base_speed=base_speed)
+
+    assert math.isinf(result.raw_energy_cap)
+    assert math.isinf(result.live_filtered_cap)
+    assert result.state == AccelControllerState.free
+    assert result.live_pace == base_speed
+    assert result.target_speed == base_speed
+
   def test_live_state_never_adopts_shadow_history(self):
     governor = make_governor()
     radar_state = make_radar(self.restrictive_lead)
@@ -717,7 +796,40 @@ class TestAccelControllerState:
     assert departed.target_speed == stop_args["base_speed"]
     assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_RATE * DT_MDL)
     assert_profile_trajectory(departed, departed.effective_accel_max)
-    assert departed.lead_obstacle_weights == (0.0, 1.0)
+    assert 0.0 < departed.lead_obstacle_weights[0] < 1.0
+    assert departed.lead_obstacle_weights[1] == 1.0
+
+  def test_creeping_lead_departure_confirms_before_bounded_launch(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=7))
+    creeping = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=0.6, radar_track_id=7))
+    stop_args = {
+      "base_speed": 5.0,
+      "v_ego": 0.1,
+      "planner_speed": 0.1,
+      "previous_mpc_source": LongitudinalPlanSource.lead0,
+      "previous_should_stop": True,
+    }
+
+    for _ in range(3):
+      update(governor, stopped, **stop_args)
+
+    departure = [update(governor, creeping, **stop_args) for _ in range(4)]
+
+    for held in departure[:3]:
+      assert held.state == AccelControllerState.stopHold
+      assert not held.launching
+      assert held.live_pace == 0.0
+      assert held.target_speed == 0.0
+
+    launched = departure[3]
+    assert launched.state == AccelControllerState.release
+    assert launched.launching
+    assert launched.target_speed == launched.base_speed
+    assert launched.live_pace == pytest.approx(launched.live_filtered_cap)
+    assert launched.live_pace >= 0.6
+    assert launched.live_pace < launched.base_speed
+    assert 0.0 < launched.lead_obstacle_weights[0] < 1.0
 
   def test_far_irrelevant_stopped_lead_does_not_block_departure(self):
     governor = make_governor()
@@ -895,15 +1007,17 @@ class TestAccelControllerState:
     governor = make_governor()
     noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
 
-    first = update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
-    second = update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
+    results = [update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0) for _ in range(8)]
+    first, second = results[:2]
 
     assert first.selected_lead == 0
     assert first.live_pace == 0.0
     assert first.target_speed == first.live_pace
     assert not governor.live.stopped_lead_hold
     assert second.target_speed == second.live_pace
-    assert second.target_speed < second.base_speed
+    assert second.target_speed >= first.target_speed
+    assert results[-1].target_speed > 0.0
+    assert results[-1].target_speed <= noisy_moving_lead.leadOne.vLeadK
 
   def test_real_stopped_evidence_latches_hold_after_noisy_first_frame(self):
     governor = make_governor()
@@ -934,7 +1048,7 @@ class TestAccelControllerState:
     assert settled.selected_lead == 0
     assert not governor.live.stopped_lead_hold
     assert settled.target_speed == settled.live_pace
-    assert observations[0].target_speed == observations[0].base_speed
+    assert observations[0].target_speed == moving_lead.leadOne.vLeadK
     assert settled.target_speed < settled.base_speed
 
   def test_stop_hold_dropout_pins_target_without_losing_hold_state(self):
@@ -972,7 +1086,7 @@ class TestAccelControllerState:
     assert_profile_trajectory(first, INITIAL_LAUNCH_ACCEL_MAX)
     assert_profile_trajectory(third, BREAKAWAY_ACCEL_MAX)
 
-  def test_confirmed_departure_has_no_later_pace_jump(self):
+  def test_confirmed_departure_holds_base_target_then_hands_off_to_launch_preview(self):
     governor = make_governor()
     stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
     moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
@@ -996,10 +1110,16 @@ class TestAccelControllerState:
     expected_step = PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL
     assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step)
     assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap)
-    assert handed_back.target_speed == handed_back.live_pace
+    assert handed_back.target_speed == handed_back.base_speed
     assert handed_back.mpc_accel_max is not None
     assert handed_back.mpc_shape_cruise
 
+    handoff = update(governor, moving, **(lead_args | {"v_ego": 1.0, "planner_speed": 1.0}))
+
+    assert not governor.live.launch_target_active
+    assert min(handoff.base_speed, 1.0 + LAUNCH_DELTA_V) <= handoff.live_pace <= handoff.base_speed
+    assert handoff.target_speed == handoff.live_pace
+
   @pytest.mark.parametrize(
     "bypass",
     [

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

@@ -8,6 +8,7 @@ from opendbc.car.interfaces import ACCEL_MIN
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
 from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState
 
@@ -40,6 +41,8 @@ class ClosedLoopTrace:
   observed_acceleration: np.ndarray
   lead_obstacle_weight_0: np.ndarray
   lead_obstacle_weight_1: np.ndarray
+  state: np.ndarray
+  required_decel: np.ndarray
   solver_failures: int
 
 
@@ -106,6 +109,8 @@ def _run(
         result["observed_a_ego"],
         controller.lead_obstacle_weights[0],
         controller.lead_obstacle_weights[1],
+        controller.state,
+        controller.required_decel,
       )
     )
     sources.append(result["mpc_source"])
@@ -138,6 +143,8 @@ def _run(
     observed_acceleration=data[:, 22],
     lead_obstacle_weight_0=data[:, 23],
     lead_obstacle_weight_1=data[:, 24],
+    state=data[:, 25].astype(int),
+    required_decel=data[:, 26],
     solver_failures=solver_failures,
   )
 
@@ -180,6 +187,28 @@ def _has_propulsion_brake_reversal(trace: ClosedLoopTrace, after: float) -> bool
   return False
 
 
+def _has_stable_brake_gas_brake(values: np.ndarray, threshold: float, frames: int = 5) -> bool:
+  """Return whether brake, gas, then brake each persist for ``frames`` samples."""
+  phase = 0
+  brake_seen = False
+  gas_after_brake_seen = False
+  for index in range(len(values) - frames + 1):
+    window = values[index:index + frames]
+    candidate = -1 if np.all(window <= -threshold) else 1 if np.all(window >= threshold) else 0
+    if candidate == 0 or candidate == phase:
+      continue
+
+    phase = candidate
+    if candidate < 0:
+      if gas_after_brake_seen:
+        return True
+      brake_seen = True
+    elif brake_seen:
+      gas_after_brake_seen = True
+
+  return False
+
+
 @pytest.fixture(autouse=True)
 def _restore_controller_defaults():
   yield
@@ -278,7 +307,7 @@ def test_two_frame_dropout_and_false_relief_do_not_release_pace(record_property)
   assert not _has_propulsion_brake_reversal(trace, after=1.0)
   record_property("clean_base_solver_failures", baseline.solver_failures)
   record_property("accel_controller_solver_failures", trace.solver_failures)
-  assert trace.solver_failures <= baseline.solver_failures
+  assert trace.solver_failures == 0
   if trace.solver_failures:
     pytest.xfail("opt-in validation: absolute zero-solver-failure gate is unmet with raw two-frame all-lead dropout")
 
@@ -324,11 +353,12 @@ def test_benign_far_lead_acquisition_ramps_optimizer_authority_without_jerk():
   trace = _run(
     duration=3.0,
     controller_enabled=True,
+    profile=0,
     lead_relevancy=True,
     speed=20.0,
     distance_lead=126.0,
     v_lead=17.0,
-    v_cruise=30.0,
+    v_cruise=20.0,
     lead_observation_fn=observe,
     actuator_delay=0.15,
     actuator_lag=0.20,
@@ -372,12 +402,12 @@ def test_route_shaped_urgent_lead_acquisition_is_immediate_and_does_not_delay_br
   acquired = np.flatnonzero((trace.time >= acquisition_time) & (trace.selected_lead == 0))
   assert len(acquired)
   assert trace.lead_obstacle_weight_0[acquired[0]] == 1.0
-  for threshold in (-1.0, -2.0):
-    assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
   assert trace.solver_failures == 0
   record_property("clean_base_solver_failures", baseline.solver_failures)
   if baseline.solver_failures:
     pytest.xfail("provisional route gate: clean-base MPC loses the abrupt 34.8-to-23.4 m/s lead-acquisition solve")
+  for threshold in (-1.0, -2.0):
+    assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
 
   baseline_gap = baseline.distance_lead - baseline.distance
   controlled_gap = trace.distance_lead - trace.distance
@@ -389,6 +419,119 @@ def test_route_shaped_urgent_lead_acquisition_is_immediate_and_does_not_delay_br
   assert np.min(controlled_ttc) >= np.min(baseline_ttc) - 1e-3
 
 
+def test_moderate_urgent_lead_acquisition_does_not_delay_stock_braking():
+  acquisition_time = 1.0
+
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if current_time < acquisition_time or lead_name == "leadTwo":
+      return None
+    return truth | {"radarTrackId": 23}
+
+  common = dict(
+    duration=8.0,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=55.0,
+    v_lead=12.0,
+    v_cruise=20.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  assert trace.solver_failures <= baseline.solver_failures
+  for threshold in (-1.0, -2.0):
+    assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
+  if trace.solver_failures:
+    pytest.xfail("opt-in validation: clean base and controller both lose the moderate abrupt-acquisition solve on this platform")
+
+
+def test_urgent_warm_start_reset_preserves_fcw_history_until_mpc_update():
+  lead_visible = False
+
+  def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if not lead_visible or lead_name == "leadTwo":
+      return None
+    return truth | {"radarTrackId": 24}
+
+  _set_accel_controller_params(enabled=True)
+  plant = Plant(
+    lead_relevancy=True,
+    speed=34.8,
+    distance_lead=105.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  while plant.current_time < 1.0:
+    plant.step(v_lead=23.4, v_cruise=40.0)
+
+  lead_visible = True
+  plant.planner.mpc.crash_cnt = 2.0
+  crash_count_at_update = []
+  original_update = plant.planner.mpc.update
+
+  def capture_crash_count(*args, **kwargs):
+    crash_count_at_update.append(plant.planner.mpc.crash_cnt)
+    return original_update(*args, **kwargs)
+
+  plant.planner.mpc.update = capture_crash_count
+  plant.step(v_lead=23.4, v_cruise=40.0)
+
+  assert plant.planner.accel_controller_result.reset_mpc
+  assert crash_count_at_update == [2.0]
+
+
+def test_route_e5_low_speed_urgent_closing_stays_with_stock_braking():
+  # E5 reached its first urgent relative-energy sample below 5 m/s: ego was
+  # about 4.5 m/s and the decelerating lead was about 1.9 m/s at 16-18 m.
+  # The controller must hand that case to raw stock MPC immediately even
+  # though the old speed-only urgent gate was not met.
+  def lead_speed(current_time: float) -> float:
+    return max(0.0, 1.9 - 1.16 * current_time)
+
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if lead_name == "leadTwo":
+      return None
+    return truth | {
+      "aLeadK": -1.16 if lead_speed(current_time) > 0.0 else 0.0,
+      "radarTrackId": 7,
+      "radar": True,
+    }
+
+  common = dict(
+    duration=6.0,
+    profile=0,
+    lead_relevancy=True,
+    speed=4.5,
+    distance_lead=18.0,
+    v_lead=lead_speed,
+    v_cruise=23.056,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  urgent_demand = (trace.required_decel >= 0.45) & (trace.speed >= 0.30) & ~trace.should_stop
+  urgent_indices = np.flatnonzero(urgent_demand)
+  assert len(urgent_indices)
+  first_urgent = urgent_indices[0]
+  assert trace.speed[first_urgent] < 5.0
+  assert trace.urgent_bypass[first_urgent]
+  assert trace.urgent_bypass[urgent_demand].all()
+  assert np.max(trace.a_target[urgent_demand]) < 0.0
+
+  assert _first_time_below(trace, -1.0) <= _first_time_below(baseline, -1.0) + 1e-9
+  baseline_gap = baseline.distance_lead - baseline.distance
+  controlled_gap = trace.distance_lead - trace.distance
+  assert np.min(controlled_gap) >= np.min(baseline_gap) - 1e-3
+  assert trace.solver_failures == 0
+
+
 def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal():
   glitch_start = 5.0
   glitch_end = 5.5
@@ -426,6 +569,62 @@ def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal():
     assert not np.any(disturbance[positive[0] + 1:] < -0.2)
 
 
+def test_route_like_tiny_closing_track_noise_does_not_chatter_accel_authority():
+  noise_start = 3.0
+  phase_time = 2.5
+
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if lead_name == "leadTwo":
+      return None
+
+    closing_sample = current_time < noise_start or int((current_time - noise_start) // phase_time) % 2 == 0
+    v_ego = truth["vLead"] - truth["vRel"]
+    v_lead_observed = v_ego - 0.60 if closing_sample else v_ego + 0.10
+    a_lead_observed = -0.15 if closing_sample else 0.12
+    return truth | {
+      "vRel": v_lead_observed - v_ego,
+      "aRel": a_lead_observed + truth["aRel"],
+      "vLead": v_lead_observed,
+      "vLeadK": v_lead_observed,
+      "aLeadK": a_lead_observed,
+      "radarTrackId": 655 if closing_sample else 798,
+      "radar": True,
+    }
+
+  trace = _run(
+    duration=16.0,
+    controller_enabled=True,
+    profile=0,
+    lead_relevancy=True,
+    speed=30.0,
+    # E8 repeatedly switched radar tracks while following at roughly 55-79 m.
+    # Use the middle of that band so the fixture isolates tiny relative-speed
+    # noise rather than entering the desired-gap singularity as ego settles.
+    distance_lead=70.0,
+    v_lead=30.0,
+    v_cruise=30.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.10,
+    actuator_lag=0.20,
+  )
+
+  noise = trace.time >= noise_start
+  first_noise = np.flatnonzero(noise)[0]
+  filtered_acceleration = np.convolve(trace.acceleration[noise], np.ones(5) / 5.0, mode="valid")
+  assert np.max(trace.required_decel[noise]) < 0.03
+  assert not trace.urgent_bypass[noise].any()
+  assert np.all(trace.selected_lead[noise] == 0)
+  assert np.all(trace.state[noise] == AccelControllerState.free)
+  assert all(source == LongitudinalPlanSource.cruise for source in trace.source[first_noise:])
+
+  assert not _has_stable_brake_gas_brake(trace.a_target[noise], threshold=0.08)
+  assert not _has_stable_brake_gas_brake(trace.effective_accel_max[noise], threshold=0.05)
+  assert not _has_stable_brake_gas_brake(filtered_acceleration, threshold=0.15)
+  assert np.max(np.diff(trace.pace[noise])) <= 1e-9
+  assert np.min(trace.distance_lead - trace.distance) > 55.0
+  assert trace.solver_failures == 0
+
+
 def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal():
   def lead_speed(current_time: float) -> float:
     return float(0.1 * (1.0 - np.cos(np.pi * current_time)))
@@ -449,6 +648,52 @@ def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal():
   assert not _has_propulsion_brake_reversal(trace, after=4.0)
 
 
+def test_route_e7_creeping_lead_departure_has_no_stable_brake_gas_brake():
+  departure_time = 1.0
+
+  def lead_speed(current_time: float) -> float:
+    if current_time < departure_time:
+      return 0.0
+    if current_time < departure_time + 0.5:
+      return 1.6 * (current_time - departure_time)
+    if current_time < departure_time + 1.5:
+      return 0.8
+    return min(2.5, 0.8 + 1.13 * (current_time - departure_time - 1.5))
+
+  def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if lead_name == "leadTwo":
+      return None
+    return truth | {"radarTrackId": 2133, "radar": True}
+
+  trace = _run(
+    duration=7.0,
+    controller_enabled=True,
+    profile=0,
+    lead_relevancy=True,
+    speed=0.0,
+    # E7's bookmarked queue oscillation had a 3.6-3.9 m radar gap.
+    distance_lead=3.6,
+    v_lead=lead_speed,
+    v_cruise=22.352,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+
+  after_departure = trace.time >= departure_time
+  lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time])
+  filtered_acceleration = np.convolve(trace.acceleration[after_departure], np.ones(5) / 5.0, mode="valid")
+  moving = np.flatnonzero(after_departure & (trace.speed > 0.05))
+  assert len(moving)
+  assert trace.time[moving[0]] <= departure_time + 3.5
+  assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20)
+
+  assert not _has_stable_brake_gas_brake(trace.a_target[after_departure], threshold=0.20)
+  assert not _has_stable_brake_gas_brake(filtered_acceleration, threshold=0.20)
+  assert np.min(trace.distance_lead - trace.distance) >= 3.5
+  assert trace.solver_failures == 0
+
+
 def test_severe_closing_never_delays_braking_or_reduces_clearance():
   common = dict(
     duration=12.0,
@@ -570,6 +815,42 @@ def test_slow_lead_rejoin_is_smooth_across_profiles_and_actuator_dynamics(profil
   assert np.max(trace.a_target[trace.speed > lead_speed + 0.2]) <= 0.2
 
 
+@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
+def test_decelerating_moving_lead_unwinds_brake_without_false_stop(profile):
+  def lead_speed(current_time: float) -> float:
+    if current_time < 2.0:
+      return 15.0
+    if current_time >= 8.0:
+      return 5.0
+    progress = (current_time - 2.0) / 6.0
+    return 15.0 - 10.0 * (3.0 * progress * progress - 2.0 * progress * progress * progress)
+
+  trace = _run(
+    duration=18.0,
+    controller_enabled=True,
+    profile=profile,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=110.0,
+    v_lead=lead_speed,
+    v_cruise=30.0,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+
+  after_lead_settles = trace.time >= 8.0
+  command_jerk = _command_jerk(trace, after=1.0)
+  gap = trace.distance_lead - trace.distance
+
+  assert trace.urgent_bypass.any()
+  assert trace.solver_failures == 0
+  assert not trace.should_stop[after_lead_settles].any()
+  assert np.max(np.abs(command_jerk)) < 3.5
+  assert np.min(trace.speed[after_lead_settles]) >= 2.0
+  assert np.min(gap) > 20.0
+  assert not _has_propulsion_brake_reversal(trace, after=1.0)
+
+
 @pytest.mark.parametrize(
   ("actuator_delay", "actuator_lag"),
   [
@@ -767,6 +1048,58 @@ def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct():
   assert final_speeds[2] - final_speeds[1] > 0.4
 
 
+def test_accelerating_lead_departure_is_prompt_smooth_and_profiles_feel_distinct():
+  departure_time = 1.0
+
+  def lead_speed(current_time: float) -> float:
+    return 0.0 if current_time < departure_time else min(15.0, 2.0 * (current_time - departure_time))
+
+  traces = [
+    _run(
+      duration=10.0,
+      controller_enabled=True,
+      profile=profile,
+      lead_relevancy=True,
+      speed=0.0,
+      distance_lead=6.0,
+      v_lead=lead_speed,
+      v_cruise=22.352,
+      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+    )
+    for profile in range(3)
+  ]
+  first_credible_lead_time = departure_time + 0.20
+  movement_times = []
+  for trace in traces:
+    before_confirmation = trace.time < first_credible_lead_time + 3 * DT_MDL
+    assert np.max(trace.speed[before_confirmation]) < 1e-3
+
+    moving = np.flatnonzero((trace.time >= first_credible_lead_time) & (trace.speed > 0.05))
+    assert len(moving)
+    movement_times.append(float(trace.time[moving[0]]))
+    assert movement_times[-1] - first_credible_lead_time <= 1.0
+
+    lead_speeds = np.array([lead_speed(max(0.0, t - DT_MDL)) for t in trace.time])
+    assert np.all(trace.speed <= lead_speeds + 0.20)
+    assert np.min(trace.distance_lead - trace.distance) >= 5.99
+    assert not trace.fcw.any()
+    assert trace.solver_failures == 0
+    assert not _has_propulsion_brake_reversal(trace, after=departure_time)
+    departure_jerk = np.diff(trace.a_target)[trace.time[1:] >= departure_time] / DT_MDL
+    assert np.max(np.abs(departure_jerk)) < 4.0
+
+  assert max(movement_times) - min(movement_times) <= DT_MDL
+  steady = (traces[0].time >= 8.0) & (traces[0].time <= 10.0)
+  mean_speeds = [float(np.mean(trace.speed[steady])) for trace in traces]
+  assert mean_speeds[0] < mean_speeds[1] < mean_speeds[2]
+  assert mean_speeds[1] - mean_speeds[0] >= 0.60
+  assert mean_speeds[2] - mean_speeds[1] >= 0.20
+
+  terminal_distances = [float(trace.distance[-1]) for trace in traces]
+  assert terminal_distances[1] - terminal_distances[0] >= 2.0
+  assert terminal_distances[2] - terminal_distances[1] >= 0.50
+
+
 def test_profile_trajectory_is_pre_mpc_and_not_a_custom_output_clamp():
   _set_accel_controller_params(enabled=True, profile=0)
   plant = Plant(speed=10.0, actuator_delay=0.15, actuator_lag=0.20)