simplify

2026-08-20 13:43:45 +08:00 · 2026-08-16 20:59:10 -07:00
parent 71cfc383b2
commit bdff333046
17 changed files with 1365 additions and 665 deletions
@@ -101,9 +101,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
    throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
    throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
    stock_allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
-    self.allow_throttle = self.accel_controller.update_allow_throttle(
-      stock_allow_throttle, throttle_prob, force_allow=v_ego <= MIN_ALLOW_THROTTLE_SPEED,
-    )
+    self.allow_throttle = stock_allow_throttle

    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 class PlannerSM(dict):
  def __init__(self, radar_frame: int, services: dict):
    super().__init__(services)
+    self.frame = radar_frame
    self.logMonoTime = {"radarState": radar_frame}
    self.valid = {"radarState": True}
    self.alive = {"radarState": True}
@@ -28,10 +28,10 @@ DESCRIPTIONS = {
    "your steering wheel distance button."
  ),
  "AccelPersonalityEnabled": tr_noop(
-    "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+    "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop."
  ),
  "AccelPersonality": tr_noop(
-    "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
+    "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
  ),
  "IsLdwEnabled": tr_noop(
    "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
@@ -112,11 +112,11 @@ class TogglesLayout(Widget):
      icon="speed_limit.png"
    )

-    self._accel_personality_enabled = toggle_item(
+    self._accel_controller_enabled = toggle_item(
      lambda: tr("Enable Accel Controller"),
      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
      self._params.get_bool("AccelPersonalityEnabled"),
-      callback=self._set_accel_personality_enabled,
+      callback=self._set_accel_controller_enabled,
      icon="speed_limit.png",
    )

@@ -162,7 +162,7 @@ class TogglesLayout(Widget):
      # insert longitudinal personality and Accel Controller settings after NDOG toggle
      if param == "DisengageOnAccelerator":
        self._toggles["LongitudinalPersonality"] = self._long_personality_setting
-        self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
+        self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
        self._toggles["AccelPersonality"] = self._accel_personality_setting

    self._update_experimental_mode_icon()
@@ -184,7 +184,7 @@ class TogglesLayout(Widget):

  def _update_toggles(self):
    ui_state.update_params()
-    accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled")
+    accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")

    e2e_description = tr(
      "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
@@ -203,14 +203,14 @@ class TogglesLayout(Widget):
        self._toggles["ExperimentalMode"].action_item.set_enabled(True)
        self._toggles["ExperimentalMode"].set_description(e2e_description)
        self._long_personality_setting.action_item.set_enabled(True)
-        self._accel_personality_enabled.action_item.set_enabled(True)
-        self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled)
+        self._accel_controller_enabled.action_item.set_enabled(True)
+        self._accel_personality_setting.action_item.set_enabled(True)
      else:
        # no long for now
        self._toggles["ExperimentalMode"].action_item.set_enabled(False)
        self._toggles["ExperimentalMode"].action_item.set_state(False)
        self._long_personality_setting.action_item.set_enabled(False)
-        self._accel_personality_enabled.action_item.set_enabled(False)
+        self._accel_controller_enabled.action_item.set_enabled(False)
        self._accel_personality_setting.action_item.set_enabled(False)
        self._params.remove("ExperimentalMode")

@@ -234,10 +234,8 @@ class TogglesLayout(Widget):
    # refresh toggles from params to mirror external changes
    for param in self._toggle_defs:
      self._toggles[param].action_item.set_state(self._params.get_bool(param))
-    self._accel_personality_enabled.action_item.set_state(accel_personality_enabled)
-    self._accel_personality_setting.action_item.set_selected_button(
-      self._params.get("AccelPersonality", return_default=True)
-    )
+    self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
+    self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))

    # these toggles need restart, block while engaged
    for toggle_def in self._toggle_defs:
@@ -286,6 +284,5 @@ class TogglesLayout(Widget):
  def _set_accel_personality(self, button_index: int):
    self._params.put("AccelPersonality", button_index, block=True)

-  def _set_accel_personality_enabled(self, state: bool):
+  def _set_accel_controller_enabled(self, state: bool):
    self._params.put_bool("AccelPersonalityEnabled", state, block=True)
-    self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)
@@ -42,7 +42,7 @@ class TogglesLayoutMici(NavScroller):
    super().__init__()

    self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
-    self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
+    self._accel_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
    self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
                                       toggle_callback=self._on_experimental_mode)
@@ -55,7 +55,7 @@ class TogglesLayoutMici(NavScroller):

    self._scroller.add_widgets([
      self._personality_toggle,
-      self._accel_personality_enabled,
+      self._accel_controller_enabled,
      self._accel_personality_toggle,
      self._experimental_btn,
      is_metric_toggle,
@@ -69,7 +69,7 @@ class TogglesLayoutMici(NavScroller):
    # Toggle lists
    self._refresh_toggles = (
      ("ExperimentalMode", self._experimental_btn),
-      ("AccelPersonalityEnabled", self._accel_personality_enabled),
+      ("AccelPersonalityEnabled", self._accel_controller_enabled),
      ("IsMetric", is_metric_toggle),
      ("IsLdwEnabled", ldw_toggle),
      ("AlwaysOnDM", always_on_dm_toggle),
@@ -79,9 +79,6 @@ class TogglesLayoutMici(NavScroller):
    )

    enable_openpilot.set_enabled(lambda: not ui_state.engaged)
-    self._accel_personality_toggle.set_enabled(
-      lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
-    )
    record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
    record_mic.set_enabled(lambda: not ui_state.engaged)

@@ -112,14 +109,14 @@ class TogglesLayoutMici(NavScroller):
      if ui_state.has_longitudinal_control:
        self._experimental_btn.set_visible(True)
        self._personality_toggle.set_visible(True)
-        self._accel_personality_enabled.set_visible(True)
+        self._accel_controller_enabled.set_visible(True)
        self._accel_personality_toggle.set_visible(True)
      else:
        # no long for now
        self._experimental_btn.set_visible(False)
        self._experimental_btn.set_checked(False)
        self._personality_toggle.set_visible(False)
-        self._accel_personality_enabled.set_visible(False)
+        self._accel_controller_enabled.set_visible(False)
        self._accel_personality_toggle.set_visible(False)
        ui_state.params.remove("ExperimentalMode")

@@ -1,16 +1,16 @@
 import math
 from dataclasses import dataclass

-from openpilot.cereal import custom
-from openpilot.common.params import Params
+import numpy as np
+
+from openpilot.cereal import custom, log
 from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  COMFORT_DECEL, EARLY_DECEL_EPSILON, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_RESPONSE_TIME,
-  EARLY_DECEL_SPEED_DEADBAND, EARLY_DECEL_TIGHTEN_RATE, PARAM_READ_INTERVAL, THROTTLE_REENABLE_PROB,
-  VEGO_NOISE_TOLERANCE, AccelProfile, profile_accel_max, sanitize_profile,
+  ACCEL_V, BRAKE_BUILD_JERK, BRAKE_ONSET_JERK, DECEL_V, DESIRED_STOP_DISTANCE, FOLLOW_HEADWAY, GAP_DEADBAND_METERS,
+  GAP_DEADBAND_SECONDS, LAUNCH_JERK, NEUTRAL_ACCEL, PACE_BUFFER_TIME, PACE_GAIN, PACE_MAX_CLOSING_SPEED, PACE_MAX_OPENING_SPEED,
+  PACE_STABILITY_MARGIN, RELEASE_JERK, ROUTINE_DECEL, SPEED_BP, SPEED_RESPONSE_TIME, STOP_MARGIN_BP, STOP_MARGIN_V, TERMINAL_MAX_DECEL,
+  TERMINAL_PREVIEW_TIME, TERMINAL_TIME_CONSTANT, URGENT_BRAKE_JERK, AccelProfile,
 )
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan


 AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
@@ -18,9 +18,29 @@ AccelControllerState = custom.LongitudinalPlanSP.AccelController.State

@dataclass(frozen=True)
 class AccelDecision:
-  cruise_accel_max: float | None = None
-  early_decel: float | None = None
-  active: bool = False
+  a_target: float | None = None
+  should_stop: bool = False
+  stock_safety_required: bool = False
+
+
+@dataclass(frozen=True)
+class LeadObservation:
+  index: int
+  distance: float
+  speed: float
+  accel: float
+  track_id: int
+  model_prob: float
+
+
+@dataclass(frozen=True)
+class ProjectedLead:
+  observation: LeadObservation
+  distance: float
+  speed: float
+  relative_speed: float
+  relative_speed_target: float
+  accel: float


 class AccelController:
@@ -29,132 +49,342 @@ class AccelController:
      raise ValueError("dt must be finite and positive")

    self.dt = float(dt)
-    self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
-    self.params = Params()
-    self.available = bool(CP.openpilotLongitudinalControl)
-    self.enabled = False
-    self.profile = AccelProfile.normal
-    self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / self.dt)))
-    self._param_frame = 0
+    self.action_time = float(np.clip(float(CP.longitudinalActuatorDelay) + DT_MDL, DT_MDL, 1.0))
+
+    self._a_command: float | None = None
+    self._last_primary: LeadObservation | None = None
+    self._last_secondary: LeadObservation | None = None
+    self._primary_frames = 0
+    self._secondary_frames = 0
+    self._lead_clear_frames = 0
+    self._braking_latched = False
+    self._radar_faulted = False
+    self._radar_recovery_frames = 0

-    self._early_decel: float | None = None
-    self.is_active = False
-    self.cruise_accel_max: float | None = None
-    self.early_decel: float | None = None
    self.state = AccelControllerState.inactive
    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0
-    self._allow_throttle = True

  @property
-  def is_enabled(self) -> bool:
-    return self.available and self.enabled
-
-  def update_params(self) -> None:
-    if self._param_frame % self._param_read_frames == 0:
-      self.enabled = self.params.get_bool("AccelPersonalityEnabled")
-      self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
-    self._param_frame += 1
+  def is_active(self) -> bool:
+    return self.state != AccelControllerState.inactive

  def reset(self) -> None:
-    self._early_decel = None
-    self._allow_throttle = True
-    self.is_active = False
-    self.cruise_accel_max = None
-    self.early_decel = None
+    self._a_command = None
+    self._last_primary = None
+    self._last_secondary = None
+    self._primary_frames = 0
+    self._secondary_frames = 0
+    self._lead_clear_frames = 0
+    self._braking_latched = False
+    self._radar_faulted = False
+    self._radar_recovery_frames = 0
    self.state = AccelControllerState.inactive
    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0

-  def update_allow_throttle(self, stock_allowed: bool, throttle_prob: float, *, force_allow: bool = False) -> bool:
-    if not self.is_enabled or force_allow:
-      self._allow_throttle = bool(stock_allowed)
-    elif self._allow_throttle:
-      self._allow_throttle = bool(stock_allowed)
-    elif stock_allowed and math.isfinite(throttle_prob) and throttle_prob > THROTTLE_REENABLE_PROB:
-      self._allow_throttle = True
-    return self._allow_throttle
+  @staticmethod
+  def _read_lead(lead, index: int) -> LeadObservation | None:
+    try:
+      if not bool(lead.present):
+        return None
+      distance = float(lead.dRel)
+      speed = float(lead.vLeadK)
+      if not math.isfinite(distance) or distance < 0.0 or not math.isfinite(speed) or not -1.0 <= speed <= 100.0:
+        return None

-  def _valid_context(self, *, v_ego: float, a_ego: float, v_cruise: float, stock_accel_max: float,
-                     engaged: bool, cruise_initialized: bool) -> bool:
-    values = (v_ego, a_ego, v_cruise, stock_accel_max, self.delay)
-    return (engaged and cruise_initialized and v_ego >= -VEGO_NOISE_TOLERANCE and v_cruise >= 0.0
-            and self.delay >= 0.0 and all(math.isfinite(value) for value in values))
+      accel = float(lead.aLeadK)
+      accel = float(np.clip(accel, -5.0, 3.0)) if math.isfinite(accel) else 0.0
+      track_id_value = float(lead.radarTrackId)
+      track_id = int(track_id_value) if math.isfinite(track_id_value) else -1
+      model_prob = float(lead.modelProb)
+      model_prob = float(np.clip(model_prob, 0.0, 1.0)) if math.isfinite(model_prob) else 0.0
+    except (AttributeError, TypeError, ValueError):
+      return None

-  def _raw_early_decel(self, lead_plan: LeadPlan) -> float:
-    speed_error = lead_plan.speed_ceiling - lead_plan.v_ego_projected
-    if (lead_plan.selected_lead < 0 or lead_plan.closing_speed <= 0.0
-        or speed_error >= -EARLY_DECEL_SPEED_DEADBAND):
+    return LeadObservation(index, distance, max(speed, 0.0), accel, max(track_id, -1), model_prob)
+
+  @staticmethod
+  def _claims_present(lead) -> bool:
+    try:
+      return bool(lead.present)
+    except (AttributeError, TypeError, ValueError):
+      return False
+
+  @staticmethod
+  def _project_lead(lead: LeadObservation, action_time: float) -> tuple[float, float]:
+    accel = min(lead.accel, 0.0)
+    travel_time = min(action_time, -lead.speed / accel) if accel < 0.0 else action_time
+    speed = max(lead.speed + accel * travel_time, 0.0)
+    travel = (lead.speed + speed) * travel_time * 0.5
+    return max(lead.distance + travel, 0.0), speed
+
+  @staticmethod
+  def _same_track(previous: LeadObservation | None, current: LeadObservation) -> bool:
+    if previous is None or previous.index != current.index:
+      return False
+    if previous.track_id >= 0 and current.track_id >= 0:
+      return previous.track_id == current.track_id
+    return abs(current.distance - previous.distance) <= 5.0
+
+  @staticmethod
+  def _same_physical_lead(first: ProjectedLead, second: ProjectedLead) -> bool:
+    first_track = first.observation.track_id
+    second_track = second.observation.track_id
+    if first_track >= 0 and second_track >= 0:
+      return first_track == second_track
+    return abs(first.distance - second.distance) <= 0.5 and abs(first.speed - second.speed) <= 0.5
+
+  def _update_lead_continuity(self, primary: LeadObservation | None, secondary: LeadObservation | None) -> None:
+    if primary is None:
+      self._primary_frames = 0
+      self._lead_clear_frames = self._lead_clear_frames + 1 if self._last_primary is not None else 0
+      if self._lead_clear_frames >= 3:
+        self._last_primary = None
+    elif self._same_track(self._last_primary, primary):
+      self._primary_frames += 1
+      self._lead_clear_frames = 0
+    else:
+      self._primary_frames = 1
+      self._lead_clear_frames = 0
+
+    if secondary is not None and self._same_track(self._last_secondary, secondary):
+      self._secondary_frames += 1
+    else:
+      self._secondary_frames = int(secondary is not None)
+
+  @staticmethod
+  def _pace_target(lead: LeadObservation, ego_distance: float, v_ego: float, headway: float, action_time: float) -> ProjectedLead:
+    lead_distance, lead_speed = AccelController._project_lead(lead, action_time)
+    projected_gap = max(lead_distance - ego_distance, 0.0)
+    desired_gap = DESIRED_STOP_DISTANCE + headway * v_ego
+    gap_error = projected_gap - desired_gap
+    gap_deadband = GAP_DEADBAND_METERS + GAP_DEADBAND_SECONDS * v_ego
+    effective_gap_error = math.copysign(max(abs(gap_error) - gap_deadband, 0.0), gap_error)
+    relative_speed = lead_speed - v_ego
+    relative_speed_target = -float(np.clip(effective_gap_error / PACE_BUFFER_TIME, -PACE_MAX_OPENING_SPEED, PACE_MAX_CLOSING_SPEED))
+    nominal_denominator = headway ** 2 / PACE_BUFFER_TIME + 2.0 * headway
+    pace_gain = max(PACE_GAIN, PACE_STABILITY_MARGIN / nominal_denominator)
+    accel = pace_gain * (relative_speed - relative_speed_target)
+    return ProjectedLead(lead, projected_gap, lead_speed, relative_speed, relative_speed_target, accel)
+
+  @staticmethod
+  def _terminal_target(projected: ProjectedLead, v_ego: float) -> float | None:
+    usable_distance = max(projected.distance - DESIRED_STOP_DISTANCE, 0.0)
+    if v_ego > 0.0 and usable_distance / v_ego > TERMINAL_PREVIEW_TIME:
+      return None
+    brake_tau = ROUTINE_DECEL * TERMINAL_TIME_CONSTANT
+    stop_speed = math.sqrt(brake_tau ** 2 + 2.0 * ROUTINE_DECEL * usable_distance) - brake_tau
+    if v_ego <= stop_speed:
      return 0.0
-    comfort_decel = COMFORT_DECEL[self.profile]
-    return max(speed_error / EARLY_DECEL_RESPONSE_TIME, -comfort_decel)
+    curve_decel = ROUTINE_DECEL * v_ego / (v_ego + brake_tau) if v_ego > 0.0 else 0.0
+    braking_distance = max(usable_distance, 0.1)
+    required_decel = v_ego ** 2 / (2.0 * braking_distance) * float(np.interp(v_ego, STOP_MARGIN_BP, STOP_MARGIN_V))
+    return -min(max(curve_decel, required_decel), TERMINAL_MAX_DECEL)

-  def _update_early_decel(self, raw_target: float, previous_plan_accel: float) -> None:
-    raw_target = min(float(raw_target), 0.0)
-    plan_accel = float(previous_plan_accel) if math.isfinite(previous_plan_accel) else 0.0
-    previous = self._early_decel if self._early_decel is not None else max(plan_accel, 0.0)
+  @staticmethod
+  def _follow_target(projected: ProjectedLead, max_decel: float) -> float:
+    demand = max(-projected.accel, 0.0)
+    limit = min(ROUTINE_DECEL + 0.15 * max(demand - ROUTINE_DECEL, 0.0), max_decel)
+    return max(projected.accel, -limit)

-    if raw_target < previous - EARLY_DECEL_EPSILON:
-      updated = max(raw_target, previous - EARLY_DECEL_TIGHTEN_RATE * self.dt)
-      state = AccelControllerState.restrict
-    elif raw_target > previous + EARLY_DECEL_EPSILON:
-      updated = min(raw_target, previous + EARLY_DECEL_RELEASE_RATE * self.dt)
-      state = AccelControllerState.release
+  @staticmethod
+  def _stopped(speed: float) -> bool:
+    return speed < 0.3
+
+  def _departure_confirmed(self, primary: LeadObservation | None, standstill: bool) -> bool:
+    if not standstill or self._radar_faulted:
+      return False
+    if primary is None:
+      return self._last_primary is None
+    if self._last_primary is None or not self._same_track(self._last_primary, primary):
+      return False
+    range_opening = primary.distance - self._last_primary.distance
+    return self._primary_frames >= 2 and not self._stopped(primary.speed) and range_opening >= 0.02
+
+  @staticmethod
+  def _lead_clear_for_launch(lead: ProjectedLead) -> bool:
+    distance_clear = lead.distance >= DESIRED_STOP_DISTANCE + 1.0 or not AccelController._stopped(lead.speed)
+    return lead.accel >= -NEUTRAL_ACCEL and distance_clear
+
+  def _fault_fallback(self) -> AccelDecision:
+    self.reset()
+    self._radar_faulted = True
+    return AccelDecision()
+
+  def _govern_accel(self, raw_target: float, max_accel: float, previous_plan_accel: float, *, launch: bool, terminal: bool) -> float:
+    if self._a_command is None:
+      initial = previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0
+      self._a_command = 0.0 if launch else min(initial, max_accel)
+
+    previous = self._a_command
+    if raw_target < previous:
+      if raw_target < -ROUTINE_DECEL - 0.75:
+        jerk = URGENT_BRAKE_JERK
+      elif previous > -0.5:
+        jerk = BRAKE_ONSET_JERK
+      else:
+        jerk = BRAKE_BUILD_JERK
+      updated = max(raw_target, previous - jerk * self.dt)
    else:
-      updated = raw_target
-      state = AccelControllerState.hold if updated < -EARLY_DECEL_EPSILON else AccelControllerState.free
+      if launch:
+        jerk = LAUNCH_JERK
+      elif terminal:
+        jerk = min(RELEASE_JERK, 0.6)
+      else:
+        jerk = RELEASE_JERK
+      updated = min(raw_target, previous + jerk * self.dt)

-    if raw_target >= -EARLY_DECEL_EPSILON and updated >= -EARLY_DECEL_EPSILON:
-      self._early_decel = None
-      self.early_decel = None
-      self.state = AccelControllerState.free
-    else:
-      self._early_decel = updated
-      self.early_decel = updated
-      self.state = state
+    self._a_command = float(updated)
+    return self._a_command

  def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality,
-             engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float,
-             radar_fresh: bool = True, radar_healthy: bool = True, force_decel: bool = False,
-             previous_plan_accel: float = 0.0) -> AccelDecision:
-    self.profile = sanitize_profile(self.profile)
-    valid_context = self._valid_context(
-      v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max,
-      engaged=engaged, cruise_initialized=cruise_initialized,
+             radar_valid: bool, stock_cruise_accel: float, stock_mpc_accel: float,
+             stock_should_stop: bool, fcw: bool, standstill: bool, stock_mpc_lead: int = -1,
+             previous_plan_accel: float = 0.0, profile: int = AccelProfile.normal) -> AccelDecision:
+    numeric_context = (v_ego, a_ego, v_cruise, stock_cruise_accel, stock_mpc_accel, self.action_time)
+    valid_context = (radar_valid and -0.1 <= v_ego <= 100.0 and 0.0 <= v_cruise <= 100.0
+                     and abs(a_ego) <= 10.0 and abs(stock_cruise_accel) <= 10.0
+                     and abs(stock_mpc_accel) <= 10.0 and all(math.isfinite(value) for value in numeric_context))
+    if not valid_context:
+      return self._fault_fallback()
+    if self._a_command is not None and math.isfinite(previous_plan_accel):
+      self._a_command = min(self._a_command, previous_plan_accel)
+
+    try:
+      raw_leads = (radar_state.leadOne, radar_state.leadTwo)
+      leads = [self._read_lead(raw_leads[0], 0), self._read_lead(raw_leads[1], 1)]
+    except AttributeError:
+      return self._fault_fallback()
+    if any(self._claims_present(raw_lead) and lead is None for raw_lead, lead in zip(raw_leads, leads, strict=True)):
+      return self._fault_fallback()
+    primary = leads[0]
+    secondary = leads[1]
+    if standstill and primary is None and secondary is not None:
+      return self._fault_fallback()
+    if self._radar_faulted:
+      if primary is None:
+        self._radar_recovery_frames += 1
+        self._radar_faulted = self._radar_recovery_frames < 3
+      else:
+        self._radar_faulted = False
+        self._radar_recovery_frames = 0
+    self._update_lead_continuity(primary, secondary)
+    primary_recently_missing = primary is None and self._last_primary is not None
+
+    current_v_ego = max(v_ego, 0.0)
+    projected_v_ego = max(current_v_ego + a_ego * self.action_time, 0.0)
+    ego_distance = (current_v_ego + projected_v_ego) * self.action_time * 0.5
+    accel_v = ACCEL_V.get(profile, ACCEL_V[AccelProfile.normal])
+    max_accel = float(np.interp(projected_v_ego, SPEED_BP, accel_v))
+    max_decel = float(np.interp(projected_v_ego, SPEED_BP, DECEL_V))
+    headway = FOLLOW_HEADWAY.get(follow_personality, FOLLOW_HEADWAY[log.LongitudinalPersonality.standard])
+    projected = [None if lead is None else self._pace_target(lead, ego_distance, projected_v_ego, headway, self.action_time) for lead in leads]
+    primary_projected, secondary_projected = projected
+    observed_leads = [lead for lead in projected if lead is not None]
+
+    free_accel = float(np.clip((v_cruise - projected_v_ego) / SPEED_RESPONSE_TIME, -ROUTINE_DECEL, max_accel))
+    raw_target = free_accel
+    selected_projected = None
+    secondary_trusted = secondary is not None and self._secondary_frames >= 3 and secondary.model_prob >= 0.5
+    trusted_leads = [lead for lead in (primary_projected, secondary_projected if secondary_trusted else None) if lead is not None]
+    terminal = False
+    for lead in trusted_leads:
+      if self._stopped(lead.speed):
+        lead_target = self._terminal_target(lead, projected_v_ego)
+        if lead_target is None:
+          continue
+        terminal = True
+      else:
+        lead_target = self._follow_target(lead, max_decel)
+      if lead_target < raw_target:
+        raw_target = lead_target
+        selected_projected = lead
+    if secondary_projected is not None and not secondary_trusted and secondary_projected.accel < -NEUTRAL_ACCEL:
+      raw_target = min(raw_target, 0.0)
+
+    all_leads_clear = all(self._lead_clear_for_launch(lead) for lead in observed_leads)
+    departure_confirmed = self._departure_confirmed(primary, bool(standstill)) and (primary is not None or secondary is None)
+    launch = bool(standstill and departure_confirmed and all_leads_clear and v_cruise > 0.3)
+    if launch:
+      self._braking_latched = False
+      raw_target = min(max_accel, max(0.8, free_accel))
+      selected_projected = primary_projected
+    elif standstill:
+      raw_target = 0.0
+      self._a_command = 0.0
+
+    if primary_projected is not None:
+      pace_margin = primary_projected.relative_speed - primary_projected.relative_speed_target
+      if self._braking_latched and raw_target > 0.0 and pace_margin < 0.2 and not launch:
+        raw_target = 0.0
+      elif pace_margin >= 0.2:
+        self._braking_latched = False
+    elif not primary_recently_missing:
+      self._braking_latched = False
+
+    if primary_recently_missing:
+      raw_target = min(raw_target, 0.0)
+
+    if abs(raw_target) < NEUTRAL_ACCEL and not launch and not terminal:
+      raw_target = 0.0
+    raw_target = float(np.clip(raw_target, -(TERMINAL_MAX_DECEL if terminal else max_decel), max_accel))
+    if not standstill or launch:
+      raw_target = min(raw_target, stock_cruise_accel)
+
+    a_target = self._govern_accel(raw_target, max_accel, previous_plan_accel, launch=launch, terminal=terminal)
+    if not standstill or launch:
+      a_target = min(a_target, stock_cruise_accel)
+      self._a_command = a_target
+    if a_target <= -0.15 and primary_projected is not None:
+      self._braking_latched = True
+
+    should_stop = bool(standstill and not launch or
+      not launch and primary_projected is not None and self._stopped(primary_projected.speed)
+      and self._stopped(max(v_ego, 0.0))
+      and primary_projected.distance <= DESIRED_STOP_DISTANCE + 1.0
+    )
+    terminal_feasible = False
+    if terminal and selected_projected is not None and self._stopped(selected_projected.speed):
+      available_decel = min(max(-a_target, 0.0), max(-a_ego, 0.0))
+      usable_distance = max(selected_projected.distance - DESIRED_STOP_DISTANCE, 0.0)
+      stopping_distance = projected_v_ego ** 2 / (2.0 * max(available_decel, 0.1))
+      terminal_feasible = available_decel > NEUTRAL_ACCEL and usable_distance >= stopping_distance
+    stock_projected = projected[stock_mpc_lead] if 0 <= stock_mpc_lead < len(projected) else None
+    terminal_source_covered = (terminal_feasible and selected_projected is not None and stock_projected is not None
+                               and self._same_physical_lead(selected_projected, stock_projected))
+    urgent_ttc = False
+    danger_gap = False
+    current_desired_gap = DESIRED_STOP_DISTANCE + headway * current_v_ego
+    projected_desired_gap = DESIRED_STOP_DISTANCE + headway * projected_v_ego
+    for observed_lead in observed_leads:
+      current_closing_speed = max(current_v_ego - observed_lead.observation.speed, 0.0)
+      projected_closing_speed = max(-observed_lead.relative_speed, 0.0)
+      current_ttc = observed_lead.observation.distance / current_closing_speed if current_closing_speed > 1e-3 else math.inf
+      projected_ttc = observed_lead.distance / projected_closing_speed if projected_closing_speed > 1e-3 else math.inf
+      lead_covered = terminal_source_covered and selected_projected is not None and self._same_physical_lead(selected_projected, observed_lead)
+      urgent_ttc |= min(current_ttc, projected_ttc) < 4.0 and not lead_covered
+      danger_gap |= (observed_lead.observation.distance < 0.75 * current_desired_gap
+                     or observed_lead.distance < 0.75 * projected_desired_gap)
+    stock_more_urgent = not terminal_source_covered and stock_mpc_accel <= -1.2 and stock_mpc_accel < a_target - 0.3
+    stock_source_confirmed = (primary is None and stock_mpc_lead == -1) or (primary is not None and stock_mpc_lead == 0)
+    stale_stock_stop = bool(stock_should_stop and launch and stock_source_confirmed and not fcw and not danger_gap)
+    stock_safety_required = bool(
+      fcw or danger_gap or urgent_ttc or stock_more_urgent or stock_should_stop and not stale_stock_stop
    )
-    if not (self.is_enabled and valid_context and bool(acc_selected) and not force_decel):
-      self.reset()
-      return AccelDecision()

-    sanitized_v_ego = max(float(v_ego), 0.0)
-    positive_stock_max = max(float(stock_accel_max), 0.0)
-    self.cruise_accel_max = profile_accel_max(self.profile, sanitized_v_ego, positive_stock_max)
-
-    if radar_healthy and not radar_fresh:
-      if self._early_decel is not None:
-        self.state = AccelControllerState.hold
+    self.selected_lead = -1 if selected_projected is None else selected_projected.observation.index
+    if should_stop:
+      self.state = AccelControllerState.stopHold
+    elif launch or a_target > (previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0) + 1e-6:
+      self.state = AccelControllerState.release
+    elif a_target < -NEUTRAL_ACCEL:
+      self.state = AccelControllerState.restrict
+    elif primary_projected is not None:
+      self.state = AccelControllerState.hold
    else:
-      lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
-      if radar_healthy and radar_state is not None:
-        try:
-          lead_plan = calculate_lead_plan(
-            radar_state, sanitized_v_ego, float(a_ego), self.delay, self.profile, follow_personality,
-          )
-        except (AttributeError, TypeError, ValueError):
-          lead_plan = LeadPlan(v_ego_projected=sanitized_v_ego)
+      self.state = AccelControllerState.free

-      self.selected_lead = lead_plan.selected_lead
-      self.selected_lead_track_id = lead_plan.selected_lead_track_id
-      self.required_decel = lead_plan.required_decel
-      self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel)
-
-    profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON
-    self.is_active = profile_binding or self.early_decel is not None
-
-    return AccelDecision(
-      cruise_accel_max=self.cruise_accel_max,
-      early_decel=self.early_decel,
-      active=self.is_active,
-    )
+    decision = AccelDecision(a_target, should_stop, stock_safety_required)
+    if primary is not None:
+      self._last_primary = primary
+    self._last_secondary = secondary
+    return decision
@@ -1,54 +1,43 @@
-import math
-
-import numpy as np
-
-from openpilot.cereal import custom
+from openpilot.cereal import custom, log


 AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
-ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())

-# Scale the stock cruise candidate; stock turn and throttle limits stay authoritative.
-ACCEL_SCALE_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
-ACCEL_SCALE_V = {
-  AccelProfile.eco: [0.82, 0.76, 0.63, 0.53, 0.42],
-  AccelProfile.normal: [0.95, 0.90, 0.84, 0.76, 0.63],
-  AccelProfile.sport: [1.00, 1.00, 1.00, 1.00, 1.00],
+SPEED_BP = (0.0, 3.0, 10.0, 20.0, 30.0, 40.0)
+ACCEL_V = {
+  AccelProfile.eco: (0.90, 0.85, 0.78, 0.65, 0.52, 0.40),
+  AccelProfile.normal: (1.10, 1.05, 0.95, 0.80, 0.65, 0.50),
+  AccelProfile.sport: (1.20, 1.15, 1.05, 0.90, 0.72, 0.56),
+}
+DECEL_V = (1.0, 1.2, 2.3, 2.5, 2.5, 2.5)
+
+FOLLOW_HEADWAY = {
+  log.LongitudinalPersonality.relaxed: 1.8,
+  log.LongitudinalPersonality.standard: 1.6,
+  log.LongitudinalPersonality.aggressive: 1.4,
 }

-# Smaller values start slowing earlier; stock MPC still owns lead braking.
-COMFORT_DECEL = {
-  AccelProfile.eco: 0.25,
-  AccelProfile.normal: 0.30,
-  AccelProfile.sport: 0.35,
-}
+PACE_BUFFER_TIME = 10.0
+PACE_MAX_CLOSING_SPEED = 1.2
+PACE_MAX_OPENING_SPEED = 1.0
+PACE_GAIN = 0.65
+PACE_STABILITY_MARGIN = 2.2
+SPEED_RESPONSE_TIME = 3.0
+ROUTINE_DECEL = DECEL_V[0]

-EARLY_DECEL_RESPONSE_TIME = 1.0
-EARLY_DECEL_SPEED_DEADBAND = 0.15
-EARLY_DECEL_TIGHTEN_RATE = 1.0
-EARLY_DECEL_RELEASE_RATE = 0.35
-EARLY_DECEL_EPSILON = 1e-6
+BRAKE_ONSET_JERK = 1.0
+BRAKE_BUILD_JERK = 0.45
+URGENT_BRAKE_JERK = 2.2
+RELEASE_JERK = 0.8
+LAUNCH_JERK = 1.8

-STOP_GAP_RESERVE = 0.75
-MAX_LEAD_ACCEL_TAU = 10.0
-MIN_LEAD_SPEED = -1.0
-VEGO_NOISE_TOLERANCE = 0.10
-PARAM_READ_INTERVAL = 0.25
-THROTTLE_REENABLE_PROB = 0.45
+DESIRED_STOP_DISTANCE = 6.0
+TERMINAL_TIME_CONSTANT = 3.0
+TERMINAL_PREVIEW_TIME = 12.0
+TERMINAL_MAX_DECEL = DECEL_V[-1]
+STOP_MARGIN_BP = (0.0, 5.0, 15.0)
+STOP_MARGIN_V = (1.0, 1.0, 1.6)

-
-def sanitize_profile(profile: int) -> int:
-  return profile if profile in ACCEL_PROFILES else AccelProfile.normal
-
-
-def profile_accel_scale(profile: int, v_ego: float) -> float:
-  if not math.isfinite(v_ego):
-    return math.nan
-  return float(np.interp(max(v_ego, 0.0), ACCEL_SCALE_BP, ACCEL_SCALE_V[sanitize_profile(profile)]))
-
-
-def profile_accel_max(profile: int, v_ego: float, stock_accel_max: float) -> float:
-  if not math.isfinite(stock_accel_max):
-    return math.nan
-  stock_positive_max = max(float(stock_accel_max), 0.0)
-  return stock_positive_max * profile_accel_scale(profile, v_ego)
+GAP_DEADBAND_METERS = 2.0
+GAP_DEADBAND_SECONDS = 0.15
+NEUTRAL_ACCEL = 0.08
@@ -1,107 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import math
-from typing import NamedTuple
-
-import numpy as np
-
-from openpilot.cereal import log
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  LongitudinalMpc, STOP_DISTANCE, T_IDXS, get_T_FOLLOW,
-)
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, sanitize_profile,
-)
-
-
-class LeadPlan(NamedTuple):
-  speed_ceiling: float = math.inf
-  selected_lead: int = -1
-  selected_lead_track_id: int = -1
-  closing_speed: float = 0.0
-  required_decel: float = 0.0
-  v_ego_projected: float = 0.0
-
-
-def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
-  if a_ego < 0.0:
-    stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
-    if stop_time <= delay:
-      distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
-      return distance, 0.0
-  return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
-
-
-def _lead_values(lead) -> tuple[float, float, float, float, int] | None:
-  if not lead.present:
-    return None
-
-  d_rel = float(lead.dRel)
-  v_lead = float(lead.vLeadK)
-  if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
-    return None
-
-  a_lead = float(lead.aLeadK)
-  if not math.isfinite(a_lead):
-    a_lead = 0.0
-  a_lead_tau = float(lead.aLeadTau)
-  if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
-    a_lead_tau = _LEAD_ACCEL_TAU
-  track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
-  return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau, track_id
-
-
-def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
-                        follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
-  if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
-    return LeadPlan()
-
-  try:
-    t_follow = float(get_T_FOLLOW(follow_personality))
-  except (NotImplementedError, TypeError, ValueError):
-    return LeadPlan()
-  if not math.isfinite(t_follow) or t_follow < 0.0:
-    return LeadPlan()
-
-  profile = sanitize_profile(profile)
-  x_ego, v_ego_projected = _project_ego(v_ego, a_ego, delay)
-  comfort_decel = COMFORT_DECEL[profile]
-  candidates: list[LeadPlan] = []
-
-  for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
-    values = _lead_values(lead)
-    if values is None:
-      continue
-
-    d_rel, v_lead, a_lead, a_lead_tau, track_id = values
-    lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
-    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
-    v_lead_projected = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
-    # Match the stock MPC gap convention, including ego-speed time headway.
-    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_ego_projected, 0.0)
-    closing_speed = max(v_ego_projected - v_lead_projected, 0.0)
-    required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
-    usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0)
-    speed_ceiling = v_lead_projected + math.sqrt(2.0 * comfort_decel * usable_gap)
-
-    finite_values = (x_lead, v_lead_projected, safety_gap, usable_gap, closing_speed, speed_ceiling)
-    if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel)
-        or required_decel < 0.0):
-      continue
-
-    candidates.append(LeadPlan(
-      speed_ceiling=speed_ceiling,
-      selected_lead=lead_index,
-      selected_lead_track_id=track_id,
-      closing_speed=closing_speed,
-      required_decel=required_decel,
-      v_ego_projected=v_ego_projected,
-    ))
-
-  return min(candidates, key=lambda candidate: candidate.speed_ceiling) if candidates else LeadPlan(v_ego_projected=v_ego_projected)
@@ -2,23 +2,29 @@ import math
 from dataclasses import FrozenInstanceError
 from types import SimpleNamespace

+import numpy as np
+
 from openpilot.cereal import log
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
-  AccelController, AccelControllerState, AccelDecision,
-)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelDecision
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_PROFILES, ACCEL_SCALE_BP, ACCEL_SCALE_V, EARLY_DECEL_RELEASE_RATE, EARLY_DECEL_TIGHTEN_RATE,
-  AccelProfile, profile_accel_max, profile_accel_scale, sanitize_profile,
+  ACCEL_V, BRAKE_ONSET_JERK, DECEL_V, RELEASE_JERK, SPEED_BP, AccelProfile,
 )
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import calculate_lead_plan


-def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1):
+def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1, probability=1.0):
  return SimpleNamespace(
-    present=present, dRel=distance, vLeadK=speed, aLeadK=accel,
-    aLeadTau=tau, radarTrackId=track_id,
+    present=present,
+    dRel=distance,
+    vRel=0.0,
+    vLead=speed,
+    vLeadK=speed,
+    aLeadK=accel,
+    aLeadTau=tau,
+    radarTrackId=track_id,
+    modelProb=probability,
+    radar=True,
  )


@@ -26,11 +32,9 @@ def radar(lead_one=None, lead_two=None):
  return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead())


-def controller(*, enabled=True, profile=AccelProfile.normal, dt=DT_MDL):
-  instance = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.10, openpilotLongitudinalControl=True), dt=dt)
-  instance.enabled = enabled
-  instance.profile = profile
-  return instance
+def controller(*, delay=0.10, dt=DT_MDL):
+  CP = SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True)
+  return AccelController(CP, dt=dt)


 def update(instance, radar_state=None, **overrides):
@@ -39,204 +43,480 @@ def update(instance, radar_state=None, **overrides):
    "a_ego": 0.0,
    "v_cruise": 25.0,
    "follow_personality": log.LongitudinalPersonality.standard,
-    "engaged": True,
-    "cruise_initialized": True,
-    "acc_selected": True,
-    "stock_accel_max": 1.0,
-    "radar_fresh": True,
-    "radar_healthy": True,
-    "force_decel": False,
+    "radar_valid": True,
+    "stock_cruise_accel": 1.0,
+    "stock_mpc_accel": 1.0,
+    "stock_should_stop": False,
+    "stock_mpc_lead": -1,
+    "fcw": False,
+    "standstill": False,
+    "profile": AccelProfile.normal,
  }
  arguments.update(overrides)
  return instance.update(radar() if radar_state is None else radar_state, **arguments)


-def restrictive_radar():
-  return radar(lead(present=True, distance=25.0, speed=5.0, accel=-0.2, track_id=101))
+def settle(instance, radar_state=None, frames=80, **overrides):
+  decision = AccelDecision()
+  for _ in range(frames):
+    decision = update(instance, radar_state, **overrides)
+  return decision


-class TestProfiles(OpenpilotTestCase):
-  def test_profiles_are_faster_without_exceeding_stock(self):
-    self.assertEqual(ACCEL_SCALE_V[AccelProfile.eco], [0.82, 0.76, 0.63, 0.53, 0.42])
-    self.assertEqual(ACCEL_SCALE_V[AccelProfile.normal], [0.95, 0.90, 0.84, 0.76, 0.63])
-    self.assertEqual(ACCEL_SCALE_V[AccelProfile.sport], [1.0] * len(ACCEL_SCALE_BP))
-
-  def test_profile_order_and_stock_scaling(self):
-    for speed in (*ACCEL_SCALE_BP, 17.0, 50.0):
-      with self.subTest(speed=speed):
-        scales = [profile_accel_scale(profile, speed) for profile in ACCEL_PROFILES]
-        self.assertLess(scales[0], scales[1])
-        self.assertLess(scales[1], scales[2])
-        self.assertEqual(scales[2], 1.0)
-        for profile, scale in zip(ACCEL_PROFILES, scales, strict=True):
-          self.assertAlmostEqual(profile_accel_max(profile, speed, 0.73), 0.73 * scale)
-
-  def test_profiles_never_expand_stock_candidate(self):
-    for profile in ACCEL_PROFILES:
-      for stock_limit in (-0.5, 0.0, 0.4, 1.6):
-        with self.subTest(profile=profile, stock_limit=stock_limit):
-          limit = profile_accel_max(profile, 12.0, stock_limit)
-          self.assertGreaterEqual(limit, 0.0)
-          self.assertLessEqual(limit, max(stock_limit, 0.0))
-
-  def test_invalid_profile_is_normal_and_nonfinite_propagates(self):
-    self.assertEqual(sanitize_profile(999), AccelProfile.normal)
-    self.assertTrue(math.isnan(profile_accel_scale(AccelProfile.normal, math.nan)))
-    self.assertTrue(math.isnan(profile_accel_max(AccelProfile.normal, 10.0, math.inf)))
-
-
-class TestAccelDecision(OpenpilotTestCase):
+class TestAccelControllerContract(OpenpilotTestCase):
  def test_decision_is_a_small_immutable_contract(self):
-    decision = AccelDecision(cruise_accel_max=0.4, early_decel=-0.2, active=True)
-    self.assertEqual((decision.cruise_accel_max, decision.early_decel, decision.active), (0.4, -0.2, True))
-    field = "active"
+    decision = AccelDecision(a_target=-0.4, should_stop=True, stock_safety_required=True)
+    self.assertEqual(
+      (decision.a_target, decision.should_stop, decision.stock_safety_required),
+      (-0.4, True, True),
+    )
    with self.assertRaises(FrozenInstanceError):
-      setattr(decision, field, False)
+      decision.should_stop = False

-  def test_context_gates_reset_without_actuating(self):
+  def test_invalid_dt_is_rejected(self):
+    for dt in (0.0, -0.1, math.nan, math.inf):
+      with self.subTest(dt=dt), self.assertRaises(ValueError):
+        controller(dt=dt)
+
+  def test_inactive_context_resets_without_actuating(self):
    cases = (
-      {"enabled": False},
-      {"engaged": False},
-      {"cruise_initialized": False},
-      {"acc_selected": False},
-      {"force_decel": True},
+      {"radar_valid": False},
      {"v_ego": math.nan},
+      {"a_ego": math.inf},
+      {"a_ego": -100.0},
+      {"v_ego": 1e308},
      {"v_cruise": -1.0},
-      {"stock_accel_max": math.inf},
+      {"stock_cruise_accel": math.nan},
+      {"stock_mpc_accel": math.nan},
    )
    for case in cases:
      with self.subTest(case=case):
-        arguments = dict(case)
-        enabled = arguments.pop("enabled", True)
-        instance = controller(enabled=enabled)
-        decision = update(instance, restrictive_radar(), **arguments)
-        self.assertEqual(decision, AccelDecision())
+        instance = controller()
+        self.assertEqual(update(instance, **case), AccelDecision())
        self.assertFalse(instance.is_active)
-        self.assertEqual(instance.state, AccelControllerState.inactive)

-  def test_allow_throttle_hysteresis_filters_route_chatter(self):
+  def test_acceleration_limit_never_exceeds_stock(self):
+    decision = settle(controller(), v_ego=5.0, stock_cruise_accel=1.1, stock_mpc_accel=1.1)
+    self.assertIsNotNone(decision.a_target)
+    self.assertLessEqual(decision.a_target, min(1.1, np.interp(5.0, SPEED_BP, ACCEL_V[AccelProfile.normal])))
+
+  def test_selected_profile_changes_positive_acceleration_only(self):
+    outputs = {}
+    for profile in ACCEL_V:
+      instance = controller()
+      previous = 0.0
+      for _ in range(80):
+        decision = update(instance, v_ego=5.0, stock_cruise_accel=2.0, stock_mpc_accel=2.0, previous_plan_accel=previous, profile=profile)
+        previous = decision.a_target
+      outputs[profile] = decision.a_target
+    self.assertLess(outputs[AccelProfile.eco], outputs[AccelProfile.normal])
+    self.assertLess(outputs[AccelProfile.normal], outputs[AccelProfile.sport])
+
+    slower_lead = radar(lead(present=True, distance=55.0, speed=14.0, track_id=7))
+    braking = {
+      profile: settle(controller(), slower_lead, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7, profile=profile).a_target
+      for profile in ACCEL_V
+    }
+    self.assertEqual(len({round(value, 9) for value in braking.values()}), 1)
+
+  def test_acceleration_profiles_are_ordered_and_linearly_interpolated(self):
+    self.assertEqual(tuple(ACCEL_V), (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport))
+    self.assertEqual(set(ACCEL_V), {AccelProfile.eco, AccelProfile.normal, AccelProfile.sport})
+    for values in ACCEL_V.values():
+      self.assertEqual(len(values), len(SPEED_BP))
+      self.assertTrue(all(after <= before for before, after in zip(values[:-1], values[1:], strict=True)))
+      for speed, expected in zip(SPEED_BP, values, strict=True):
+        self.assertEqual(np.interp(speed, SPEED_BP, values), expected)
+      for index in range(len(SPEED_BP) - 1):
+        midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0
+        expected = (values[index] + values[index + 1]) / 2.0
+        self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, values), expected)
+    for speed in SPEED_BP:
+      self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.eco]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]))
+      self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.sport]))
+
+  def test_deceleration_curve_is_ascending_and_linearly_interpolated(self):
+    self.assertEqual(len(DECEL_V), len(SPEED_BP))
+    self.assertTrue(all(after >= before for before, after in zip(DECEL_V[:-1], DECEL_V[1:], strict=True)))
+    for speed, expected in zip(SPEED_BP, DECEL_V, strict=True):
+      self.assertEqual(np.interp(speed, SPEED_BP, DECEL_V), expected)
+    for index in range(len(SPEED_BP) - 1):
+      midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0
+      expected = (DECEL_V[index] + DECEL_V[index + 1]) / 2.0
+      self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, DECEL_V), expected)
+
+
+class TestElasticPace(OpenpilotTestCase):
+  def test_slower_lead_requests_early_decel_before_ttc_is_urgent(self):
+    instance = controller()
+    slower_lead = lead(present=True, distance=55.0, speed=14.0, track_id=7)
+    decision = settle(instance, radar(slower_lead), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+
+    self.assertGreater(55.0 / (20.0 - 14.0), 5.0)
+    self.assertIsNotNone(decision.a_target)
+    self.assertLess(decision.a_target, 0.0)
+    self.assertTrue(instance.is_active)
+    self.assertFalse(decision.stock_safety_required)
+
+  def test_closing_gap_produces_one_monotonic_braking_decision(self):
    instance = controller()
    outputs = []
-    for probability in (0.32, 0.41, 0.36, 0.42, 0.31, 0.44, 0.35):
-      stock_allowed = probability > 0.4
-      outputs.append(instance.update_allow_throttle(stock_allowed, probability))
+    for distance in (65.0, 60.0, 55.0, 50.0, 45.0, 40.0, 35.0):
+      decision = settle(
+        instance,
+        radar(lead(present=True, distance=distance, speed=14.0, track_id=11)),
+        frames=4,
+        v_ego=20.0,
+        v_cruise=30.0,
+        stock_cruise_accel=0.7,
+        stock_mpc_accel=0.7,
+      )
+      outputs.append(decision.a_target)

-    self.assertEqual(outputs, [False] * len(outputs))
-    self.assertTrue(instance.update_allow_throttle(True, 0.46))
+    self.assertTrue(all(value is not None and math.isfinite(value) for value in outputs))
+    self.assertTrue(all(after <= before + 1e-9 for before, after in zip(outputs[:-1], outputs[1:], strict=True)))

-  def test_allow_throttle_uses_stock_when_disabled_and_low_speed_override(self):
-    instance = controller(enabled=False)
-    self.assertFalse(instance.update_allow_throttle(False, 0.39))
-    self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True))
+  def test_actuator_delay_makes_the_same_closing_lead_no_less_restrictive(self):
+    radar_state = radar(lead(present=True, distance=45.0, speed=14.0, track_id=12))
+    short_delay = settle(controller(delay=0.05), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    long_delay = settle(controller(delay=0.50), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)

-    instance.enabled = True
-    self.assertFalse(instance.update_allow_throttle(False, 0.0))
-    self.assertTrue(instance.update_allow_throttle(True, 0.0, force_allow=True))
+    self.assertLessEqual(long_delay.a_target, short_delay.a_target + 1e-9)

-  def test_allow_throttle_hysteresis_resets_with_controller(self):
+  def test_routine_brake_onset_is_jerk_limited(self):
    instance = controller()
-    self.assertFalse(instance.update_allow_throttle(False, 0.39))
+    settle(instance, frames=30, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    restrictive = radar(lead(present=True, distance=45.0, speed=12.0, track_id=13))
+    outputs = [
+      update(instance, restrictive, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7).a_target
+      for _ in range(20)
+    ]

-    instance.reset()
+    self.assertTrue(all(value is not None for value in outputs))
+    drops = [before - after for before, after in zip(outputs[:-1], outputs[1:], strict=True)]
+    self.assertLessEqual(max(drops), BRAKE_ONSET_JERK * DT_MDL + 1e-9)
+    self.assertLess(outputs[-1], outputs[0])

-    self.assertTrue(instance.update_allow_throttle(True, 0.41))
-
-  def test_profile_limit_is_only_active_when_binding(self):
-    for profile in ACCEL_PROFILES:
-      with self.subTest(profile=profile):
-        instance = controller(profile=profile)
-        decision = update(instance, v_ego=10.0, stock_accel_max=1.0)
-        expected = profile_accel_max(profile, 10.0, 1.0)
-        self.assertAlmostEqual(decision.cruise_accel_max, expected)
-        self.assertEqual(decision.active, expected < 1.0)
-
-
-class TestEarlyDecel(OpenpilotTestCase):
-  def test_early_decel_starts_from_the_previous_positive_plan(self):
+  def test_duplicate_radar_falls_back_to_stock(self):
    instance = controller()
-    decision = update(instance, restrictive_radar(), previous_plan_accel=0.48)
+    tracked = radar(lead(present=True, distance=50.0, speed=14.0, track_id=14))
+    settle(instance, tracked, frames=20, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    duplicate_with_different_data = radar(lead(present=True, distance=15.0, speed=2.0, track_id=14))

-    self.assertIsNotNone(decision.early_decel)
-    self.assertAlmostEqual(decision.early_decel, 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL)
-
-  def test_early_decel_is_nonpositive_and_tightens_at_bound(self):
-    instance = controller()
-    samples = [0.0]
-    for _ in range(10):
-      decision = update(instance, restrictive_radar())
-      samples.append(decision.early_decel or 0.0)
-
-    self.assertTrue(all(value <= 0.0 for value in samples))
-    self.assertTrue(any(value < 0.0 for value in samples))
-    for before, after in zip(samples[:-1], samples[1:], strict=True):
-      self.assertGreaterEqual(after - before, -EARLY_DECEL_TIGHTEN_RATE * DT_MDL - 1e-9)
-
-  def test_fresh_dropout_and_unhealthy_radar_release_at_bound(self):
-    releases = ((radar(), True, True), (restrictive_radar(), False, False))
-    for radar_state, radar_fresh, radar_healthy in releases:
-      with self.subTest(radar_fresh=radar_fresh, radar_healthy=radar_healthy):
-        instance = controller()
-        for _ in range(10):
-          update(instance, restrictive_radar())
-        previous = instance.early_decel
-        self.assertIsNotNone(previous)
-
-        for _ in range(30):
-          decision = update(instance, radar_state, radar_fresh=radar_fresh, radar_healthy=radar_healthy)
-          current = decision.early_decel or 0.0
-          self.assertLessEqual(current, 0.0)
-          self.assertGreaterEqual(current, previous - 1e-9)
-          self.assertLessEqual(current - previous, EARLY_DECEL_RELEASE_RATE * DT_MDL + 1e-9)
-          previous = current
-          if decision.early_decel is None:
-            break
-        self.assertIsNone(instance.early_decel)
-
-  def test_healthy_duplicate_radar_holds_early_decel(self):
-    instance = controller()
-    for _ in range(10):
-      update(instance, restrictive_radar())
-    previous = instance.early_decel
-
-    decision = update(instance, restrictive_radar(), radar_fresh=False, radar_healthy=True)
-
-    self.assertEqual(decision.early_decel, previous)
-    self.assertEqual(instance.state, AccelControllerState.hold)
-
-  def test_early_decel_is_bounded_by_profile_comfort(self):
-    for profile in ACCEL_PROFILES:
-      with self.subTest(profile=profile):
-        instance = controller(profile=profile)
-        for _ in range(30):
-          decision = update(instance, restrictive_radar())
-        self.assertIsNotNone(decision.early_decel)
-        self.assertLessEqual(decision.early_decel, 0.0)
-        self.assertGreaterEqual(decision.early_decel, -0.35)
-
-
-class TestLeadPlan(OpenpilotTestCase):
-  def test_more_restrictive_of_two_leads_is_selected(self):
-    radar_state = radar(
-      lead(present=True, distance=80.0, speed=12.0, track_id=10),
-      lead(present=True, distance=25.0, speed=6.0, track_id=20),
+    fallback = update(
+      instance,
+      duplicate_with_different_data,
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=0.7,
+      radar_valid=False,
    )
-    plan = calculate_lead_plan(radar_state, 10.0, 0.0, 0.15, AccelProfile.normal)
-    self.assertEqual((plan.selected_lead, plan.selected_lead_track_id), (1, 20))
-    self.assertGreater(plan.closing_speed, 0.0)
-    self.assertGreater(plan.required_decel, 0.0)
+    self.assertEqual(fallback, AccelDecision())

-  def test_malformed_lead_is_ignored_without_hiding_valid_second_lead(self):
-    malformed = lead(present=True, distance=math.nan, speed=8.0)
-    valid = lead(present=True, distance=30.0, speed=7.0, accel=math.nan, tau=math.inf, track_id=math.nan)
-    plan = calculate_lead_plan(radar(malformed, valid), 10.0, 0.0, 0.15, AccelProfile.normal)
-    self.assertEqual(plan.selected_lead, 1)
-    self.assertEqual(plan.selected_lead_track_id, -1)
-    self.assertTrue(math.isfinite(plan.speed_ceiling))
+  def test_equal_speed_lead_does_not_chase_a_small_gap_error(self):
+    instance = controller()
+    decision = settle(instance, radar(lead(present=True, distance=24.0, speed=10.0, track_id=15)), v_ego=10.0, v_cruise=25.0)
+    self.assertAlmostEqual(decision.a_target, 0.0)

-  def test_malformed_radar_and_scalar_inputs_fail_closed_to_no_extension(self):
-    for radar_state in (None, SimpleNamespace(), radar(lead(present=True, distance=-1.0, speed=5.0))):
-      with self.subTest(radar_state=radar_state):
-        instance = controller(profile=AccelProfile.sport)
-        decision = update(instance, radar_state)
-        self.assertIsNone(decision.early_decel)
-        self.assertFalse(decision.active)
+  def test_fresh_lead_dropout_does_not_immediately_reaccelerate(self):
+    instance = controller()
+    tracked = radar(lead(present=True, distance=35.0, speed=6.0, track_id=16))
+    settle(instance, tracked, v_ego=12.0, v_cruise=25.0)
+    for _ in range(2):
+      self.assertLessEqual(update(instance, radar(), v_ego=12.0, v_cruise=25.0).a_target, 0.0)
+
+  def test_second_lead_requires_persistence_and_can_only_add_braking(self):
+    instance = controller()
+    primary = lead(present=True, distance=80.0, speed=20.0, track_id=21, probability=0.95)
+    second = lead(present=True, distance=55.0, speed=14.0, track_id=22, probability=0.95)
+    decisions = [
+      update(instance, radar(primary, second), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+      for _ in range(3)
+    ]
+
+    self.assertGreaterEqual(decisions[1].a_target, decisions[0].a_target)
+    self.assertLess(decisions[2].a_target, decisions[1].a_target)
+    self.assertLess(decisions[2].a_target, 0.7)
+
+
+class TestStopAndLaunch(OpenpilotTestCase):
+  def test_distant_stationary_lead_does_not_start_a_long_brake_event(self):
+    decision = settle(controller(), radar(lead(present=True, distance=300.0, speed=0.0, track_id=29)), v_ego=20.0, v_cruise=25.0,
+                      stock_cruise_accel=0.8, stock_mpc_accel=0.8)
+    self.assertGreaterEqual(decision.a_target, 0.0)
+
+  def test_stopped_lead_converges_to_stop_hold_without_positive_rebound(self):
+    instance = controller()
+    outputs = []
+    should_stop = []
+    samples = ((2.0, 11.0), (1.2, 8.5), (0.6, 7.0), (0.25, 6.3), (0.05, 6.05), (0.0, 6.0))
+    for speed, distance in samples:
+      decision = settle(
+        instance,
+        radar(lead(present=True, distance=distance, speed=0.0, track_id=30)),
+        frames=10,
+        v_ego=speed,
+        v_cruise=8.0,
+        stock_cruise_accel=0.8,
+        stock_mpc_accel=-0.5,
+        stock_should_stop=speed < 0.3,
+        standstill=speed < 0.01,
+      )
+      outputs.append(decision.a_target)
+      should_stop.append(decision.should_stop)
+
+    self.assertTrue(all(value is not None and value <= 0.0 for value in outputs))
+    self.assertFalse(any(should_stop[:3]))
+    self.assertTrue(any(should_stop[3:]))
+    self.assertTrue(should_stop[-1])
+
+  def test_same_track_departure_has_no_controller_added_dwell(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    held = settle(
+      instance,
+      stopped,
+      frames=8,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+    self.assertTrue(held.should_stop)
+
+    departing = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31))
+    released = update(
+      instance,
+      departing,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=0.8,
+      stock_should_stop=False,
+      standstill=True,
+    )
+    self.assertFalse(released.should_stop)
+    self.assertGreater(released.a_target, 0.0)
+
+  def test_radar_fault_requires_clear_road_confirmation_before_launch(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+           stock_should_stop=True, stock_mpc_lead=0, standstill=True)
+    self.assertEqual(update(instance, stopped, v_ego=0.0, v_cruise=8.0, radar_valid=False, standstill=True), AccelDecision())
+
+    clear_road = radar()
+    for _ in range(2):
+      held = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+                    stock_should_stop=True, stock_mpc_lead=-1, standstill=True)
+      self.assertTrue(held.should_stop)
+      self.assertLessEqual(held.a_target, 0.0)
+
+    released = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+                      stock_should_stop=True, stock_mpc_lead=-1, standstill=True)
+    self.assertFalse(released.should_stop)
+    self.assertGreater(released.a_target, 0.0)
+
+  def test_new_track_departure_requires_one_confirmation_frame(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    settle(
+      instance,
+      stopped,
+      frames=8,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+
+    changed_track = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=99))
+    unconfirmed = update(
+      instance,
+      changed_track,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+    self.assertTrue(unconfirmed.should_stop)
+    self.assertLessEqual(unconfirmed.a_target, 0.0)
+
+    confirmed_track = radar(lead(present=True, distance=6.2, speed=1.0, accel=1.0, track_id=99))
+    confirmed = update(
+      instance,
+      confirmed_track,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      standstill=True,
+    )
+    self.assertFalse(confirmed.should_stop)
+    self.assertGreater(confirmed.a_target, 0.0)
+
+  def test_secondary_lead_can_veto_but_not_authorize_launch(self):
+    def stopped_instance():
+      instance = controller()
+      stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=60))
+      settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+             stock_should_stop=True, stock_mpc_lead=0, standstill=True)
+      return instance
+
+    departing = lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=60)
+    blocked = update(stopped_instance(), radar(departing, lead(present=True, distance=4.0, speed=0.0, track_id=61)),
+                     v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                     stock_mpc_lead=0, standstill=True)
+    self.assertTrue(blocked.should_stop)
+    self.assertLessEqual(blocked.a_target, 0.0)
+
+    buffered = update(stopped_instance(), radar(departing, lead(present=True, distance=6.1, speed=0.0, track_id=61)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                      stock_mpc_lead=0, standstill=True)
+    self.assertTrue(buffered.should_stop)
+    self.assertLessEqual(buffered.a_target, 0.0)
+
+    released = update(stopped_instance(), radar(departing, lead(present=True, distance=20.0, speed=2.0, track_id=62)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                      stock_mpc_lead=0, standstill=True)
+    self.assertFalse(released.should_stop)
+    self.assertGreater(released.a_target, 0.0)
+
+  def test_secondary_only_at_standstill_falls_back_to_stock(self):
+    decision = update(controller(), radar(lead_two=lead(present=True, distance=20.0, speed=2.0, track_id=63)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=0.8, stock_mpc_lead=1, standstill=True)
+    self.assertEqual(decision, AccelDecision())
+
+
+class TestSafetyAndFallback(OpenpilotTestCase):
+  def test_nonurgent_stock_braking_does_not_own_normal_driving(self):
+    decision = settle(
+      controller(),
+      v_ego=15.0,
+      v_cruise=25.0,
+      stock_cruise_accel=1.0,
+      stock_mpc_accel=-0.6,
+      stock_should_stop=False,
+    )
+    self.assertFalse(decision.stock_safety_required)
+    self.assertGreater(decision.a_target, -0.6)
+
+  def test_stock_cruise_candidate_is_always_an_upper_bound(self):
+    decision = settle(controller(), v_ego=15.0, v_cruise=25.0, stock_cruise_accel=-0.3, stock_mpc_accel=0.8)
+    self.assertLessEqual(decision.a_target, -0.3)
+
+  def test_fcw_bypasses_comfort_ramp_and_preserves_stock_braking(self):
+    instance = controller()
+    settle(instance, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    decision = update(
+      instance,
+      radar(lead(present=True, distance=15.0, speed=5.0, track_id=40)),
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-2.5,
+      fcw=True,
+    )
+    self.assertTrue(decision.stock_safety_required)
+    self.assertTrue(math.isfinite(decision.a_target))
+
+  def test_urgent_ttc_uses_stock_floor_without_fcw(self):
+    decision = update(
+      controller(),
+      radar(lead(present=True, distance=20.0, speed=10.0, track_id=41)),
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-1.8,
+    )
+    self.assertTrue(decision.stock_safety_required)
+    self.assertTrue(math.isfinite(decision.a_target))
+
+  def test_current_ttc_stays_authoritative_during_measured_deceleration(self):
+    decision = update(
+      controller(delay=0.95),
+      radar(lead(present=True, distance=70.0, speed=0.0, track_id=42)),
+      v_ego=20.0,
+      a_ego=-10.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-1.0,
+    )
+    self.assertTrue(decision.stock_safety_required)
+
+  def test_feasible_terminal_braking_does_not_escalate_to_late_stock_braking(self):
+    instance = controller(delay=0.20)
+    stationary = radar(lead(present=True, distance=60.0, speed=0.0, track_id=43))
+    decision = settle(
+      instance,
+      stationary,
+      frames=60,
+      v_ego=15.0,
+      a_ego=-2.5,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-3.3,
+      stock_mpc_lead=0,
+    )
+    self.assertFalse(decision.stock_safety_required)
+    self.assertGreaterEqual(decision.a_target, -2.5)
+
+    unsafe = update(instance, radar(lead(present=True, distance=45.0, speed=0.0, track_id=43)), v_ego=15.0, a_ego=-2.5,
+                    v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0)
+    self.assertTrue(unsafe.stock_safety_required)
+
+  def test_terminal_feasibility_only_covers_the_selected_stock_source(self):
+    instance = controller()
+    primary = lead(present=True, distance=180.0, speed=0.0, track_id=44)
+    settle(instance, radar(primary), frames=60, v_ego=20.0, a_ego=-2.0, v_cruise=30.0,
+           stock_cruise_accel=0.7, stock_mpc_accel=-2.0, stock_mpc_lead=0)
+
+    secondary = lead(present=True, distance=70.0, speed=0.0, track_id=45)
+    decision = update(instance, radar(primary, secondary), v_ego=20.0, a_ego=-2.0, v_cruise=30.0,
+                      stock_cruise_accel=0.7, stock_mpc_accel=-3.0, stock_mpc_lead=1)
+    self.assertTrue(decision.stock_safety_required)
+
+  def test_duplicate_radar_leads_share_terminal_feasibility(self):
+    instance = controller(delay=0.20)
+    primary = lead(present=True, distance=60.0, speed=0.0, track_id=-1)
+    settle(instance, radar(primary), frames=60, v_ego=15.0, a_ego=-2.5, v_cruise=30.0,
+           stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0)
+
+    duplicate = lead(present=True, distance=60.0, speed=0.0, track_id=-1)
+    decision = update(instance, radar(primary, duplicate), v_ego=15.0, a_ego=-2.5, v_cruise=30.0,
+                      stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=1)
+    self.assertFalse(decision.stock_safety_required)
+
+  def test_stock_emergency_releases_through_the_jerk_governor(self):
+    instance = controller()
+    settle(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8)
+    update(instance, stock_cruise_accel=0.8, stock_mpc_accel=-2.5, fcw=True)
+    released = update(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8, previous_plan_accel=-2.5)
+    self.assertLessEqual(released.a_target - (-2.5), RELEASE_JERK * DT_MDL + 1e-9)
+
+  def test_unhealthy_radar_falls_back_to_stock(self):
+    decision = update(
+      controller(),
+      radar_valid=False,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-0.4,
+      stock_should_stop=True,
+    )
+    self.assertEqual(decision, AccelDecision())
+
+  def test_malformed_lead_never_produces_nonfinite_output(self):
+    malformed = radar(lead(present=True, distance=math.nan, speed=math.inf, accel=math.nan, track_id=50))
+    decision = update(controller(), malformed, stock_cruise_accel=0.7, stock_mpc_accel=-0.3)
+
+    self.assertEqual(decision, AccelDecision())
@@ -2,22 +2,27 @@ from types import SimpleNamespace
 from typing import Any

 from openpilot.cereal import custom, log
-from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
-from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel, get_max_accel
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_PROFILES, EARLY_DECEL_TIGHTEN_RATE, AccelProfile, profile_accel_scale,
-)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource


-def lead(*, present=False, distance=0.0, speed=0.0):
+def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, track_id=-1, probability=1.0):
  return SimpleNamespace(
-    present=present, dRel=distance, vLeadK=speed, aLeadK=0.0,
-    aLeadTau=1.5, radarTrackId=-1,
+    present=present,
+    dRel=distance,
+    vRel=0.0,
+    vLead=speed,
+    vLeadK=speed,
+    aLeadK=accel,
+    aLeadTau=1.5,
+    radarTrackId=track_id,
+    modelProb=probability,
+    radar=True,
  )


@@ -26,74 +31,194 @@ def radar(lead_one=None, lead_two=None):


 class PlannerSM(dict):
-  def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100):
+  def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100, standstill=False, frame=0):
    super().__init__(
      radarState=radar_state or radar(),
-      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0),
+      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0, standstill=standstill),
      selfdriveState=SimpleNamespace(
-        enabled=True, experimentalMode=experimental, personality=log.LongitudinalPersonality.standard,
+        enabled=True,
+        experimentalMode=experimental,
+        personality=log.LongitudinalPersonality.standard,
      ),
      controlsState=SimpleNamespace(forceDecel=force_decel, longControlState=LongCtrlState.pid),
    )
    self.valid = {"radarState": True}
    self.alive = {"radarState": True}
    self.logMonoTime = {"radarState": radar_time}
+    self.frame = frame

  def all_checks(self, service_list=None):
    return True


-def accel_controller(*, enabled=True, profile=AccelProfile.normal):
-  cp = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True)
-  instance = AccelController(cp)
-  instance.enabled = enabled
-  instance.profile = profile
-  return instance
+def accel_controller():
+  CP = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True)
+  return AccelController(CP)


-def planner_for_hook(*, enabled=True, profile=AccelProfile.normal):
+def planner_for_hook(*, available=True, enabled=True, profile=AccelProfile.normal):
  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
  dynamic_planner: Any = planner
-  dynamic_planner.accel_controller = accel_controller(enabled=enabled, profile=profile)
+  dynamic_planner.accel_controller = accel_controller()
+  dynamic_planner.accel_controller_available = available
+  dynamic_planner.accel_controller_enabled = enabled
+  dynamic_planner.accel_controller_profile = profile
  dynamic_planner.dec = SimpleNamespace(active=lambda: False)
+  dynamic_planner.model_accel_transition = SimpleNamespace(active=False)
  dynamic_planner.output_v_target = 30.0
  dynamic_planner.previous_plan_accel = 0.0
  dynamic_planner.a_cruise = 0.0
+  dynamic_planner.fcw = False
  dynamic_planner._radar_fresh_this_cycle = True
  dynamic_planner._radar_healthy_this_cycle = True
  return planner


-class TestPlannerHook(OpenpilotTestCase):
-  def test_disabled_e2e_and_force_decel_preserve_exact_candidate_tuple(self):
+class TestPlannerOwnership(OpenpilotTestCase):
+  def test_active_controller_owns_routine_acc_instead_of_stock_mpc(self):
+    planner = planner_for_hook()
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(PlannerSM(), candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1:], (MpcSource.cruise, False))
+    self.assertGreater(result[0][0], -0.6)
+    self.assertLessEqual(result[0][0], 0.7)
+    self.assertTrue(planner.accel_controller.is_active)
+
+  def test_controller_candidate_uses_the_selected_primary_lead_source(self):
+    planner = planner_for_hook()
+    planner.output_v_target = 30.0
+    sm = PlannerSM(radar_state=radar(lead(present=True, distance=55.0, speed=14.0, track_id=10)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1], MpcSource.lead0)
+    self.assertLess(result[0][0], 0.0)
+
+  def test_only_second_lead_is_published_as_lead_one_source(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(radar_state=radar(lead_two=lead(present=True, distance=55.0, speed=14.0, track_id=20)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    for _ in range(3):
+      result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1], MpcSource.lead1)
+
+
+class TestPlannerSafetyArbitration(OpenpilotTestCase):
+  def test_fake_mpc_lead_does_not_block_free_road_launch(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(standstill=True)
+    sm["carState"].vEgo = 0.0
+    candidates = [(0.0, MpcSource.lead1, True), (0.8, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertGreater(result[0][0], 0.0)
+    self.assertFalse(result[0][2])
+
+  def test_fcw_keeps_stock_emergency_candidate_in_parallel(self):
+    planner = planner_for_hook()
+    planner.fcw = True
+    candidates = [(-2.5, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(PlannerSM(), candidates)
+
+    self.assertEqual(len(result), 2)
+    self.assertTrue(planner.accel_controller.is_active)
+    self.assertIn((-2.5, MpcSource.lead0, False), result)
+    self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-2.5, MpcSource.lead0, False))
+
+  def test_urgent_ttc_keeps_stock_stop_and_braking_authoritative(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(radar_state=radar(lead(present=True, distance=20.0, speed=10.0, track_id=30)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-1.8, MpcSource.lead0, True), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 2)
+    self.assertIn((-1.8, MpcSource.lead0, True), result)
+    self.assertTrue(any(stop for _, _, stop in result))
+    self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-1.8, MpcSource.lead0, True))
+
+  def test_confirmed_departure_suppresses_stale_stock_stop_in_same_frame(self):
+    planner = planner_for_hook()
+    stopped_sm = PlannerSM(
+      radar_state=radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)),
+      standstill=True,
+    )
+    stopped_sm["carState"].vEgo = 0.0
+    stopped_candidates = [(-0.5, MpcSource.lead0, True), (0.8, MpcSource.cruise, False)]
+    for _ in range(2):
+      held = planner.update_accel_controller(stopped_sm, stopped_candidates)
+    self.assertTrue(any(stop for _, _, stop in held))
+
+    departure_sm = PlannerSM(
+      radar_state=radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31)),
+      standstill=True,
+    )
+    departure_sm["carState"].vEgo = 0.0
+    released = planner.update_accel_controller(departure_sm, stopped_candidates)
+
+    self.assertEqual(len(released), 1)
+    self.assertGreater(released[0][0], 0.0)
+    self.assertFalse(released[0][2])
+
+
+class TestPlannerFallbacks(OpenpilotTestCase):
+  def test_disabled_e2e_force_decel_and_unhealthy_radar_preserve_exact_candidates(self):
    candidates = (
      (-0.4, MpcSource.lead0, True),
      (0.6, MpcSource.cruise, False),
      (0.2, MpcSource.e2e, False),
    )
+    disabled = planner_for_hook(enabled=False)
+    blended = planner_for_hook()
+    forced = planner_for_hook()
+    unhealthy = planner_for_hook()
+    unhealthy._radar_healthy_this_cycle = False
    cases = (
-      (planner_for_hook(enabled=False), PlannerSM()),
-      (planner_for_hook(profile=AccelProfile.eco), PlannerSM(experimental=True)),
-      (planner_for_hook(profile=AccelProfile.eco), PlannerSM(force_decel=True)),
+      (disabled, PlannerSM()),
+      (blended, PlannerSM(experimental=True)),
+      (forced, PlannerSM(force_decel=True)),
+      (unhealthy, PlannerSM()),
    )
+
    for planner, sm in cases:
-      with self.subTest(experimental=sm["selfdriveState"].experimentalMode,
-                        force_decel=sm["controlsState"].forceDecel,
-                        enabled=planner.accel_controller.enabled):
+      with self.subTest(planner=planner, sm=sm):
        result = planner.update_accel_controller(sm, candidates)
        self.assertIs(result, candidates)
-        self.assertEqual(result, candidates)

-  def test_missing_cruise_candidate_is_exact_noop(self):
-    planner = planner_for_hook(profile=AccelProfile.eco)
-    candidates = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False))
-    self.assertIs(planner.update_accel_controller(PlannerSM(), candidates), candidates)
+  def test_missing_cruise_or_mpc_candidate_is_exact_noop(self):
+    planner = planner_for_hook()
+    candidates_without_cruise = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False))
+    candidates_without_mpc = ((0.6, MpcSource.cruise, False), (0.2, MpcSource.e2e, False))

-  def test_inactive_acc_does_not_publish_a_stale_positive_cruise_candidate(self):
-    planner = planner_for_hook(profile=AccelProfile.eco)
+    self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_cruise), candidates_without_cruise)
+    self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_mpc), candidates_without_mpc)
+
+  def test_malformed_radar_is_exact_stock_fallback(self):
+    planner = planner_for_hook()
+    candidates = ((-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False))
+    malformed_sm = PlannerSM(radar_state=SimpleNamespace())
+
+    self.assertIs(planner.update_accel_controller(malformed_sm, candidates), candidates)
+
+  def test_disengaged_acc_cannot_publish_stale_positive_acceleration(self):
+    planner = planner_for_hook()
    sm = PlannerSM()
    sm["controlsState"].longControlState = LongCtrlState.off
-    candidates = [(-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False)]
+    candidates = [(-0.4, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)]

    result = planner.update_accel_controller(sm, candidates)

@@ -101,63 +226,6 @@ class TestPlannerHook(OpenpilotTestCase):
    self.assertEqual(planner.a_cruise, 0.0)
    self.assertEqual(planner.accel_controller.state, AccelControllerState.inactive)

-    stale_positive = [(1.3, MpcSource.lead0, False), (0.9, MpcSource.cruise, False)]
-    self.assertEqual(min(candidate[0] for candidate in planner.update_accel_controller(sm, stale_positive)), 0.0)
-
-    braking = [(-0.4, MpcSource.lead0, True), (-0.2, MpcSource.cruise, True)]
-    self.assertIs(planner.update_accel_controller(sm, braking), braking)
-
-    disabled = planner_for_hook(enabled=False)
-    self.assertIs(disabled.update_accel_controller(sm, candidates), candidates)
-
-    blended = planner_for_hook(profile=AccelProfile.eco)
-    self.assertIs(blended.update_accel_controller(PlannerSM(experimental=True), candidates), candidates)
-
-  def test_profiles_scale_final_stock_turn_and_coast_candidates(self):
-    cp = SimpleNamespace(steerRatio=15.0, wheelbase=2.7)
-    scenarios = (
-      (25.0, 9.0, 0.35, 2.0, True),
-      (4.0, 0.0, 0.40, get_coast_accel(0.0), False),
-    )
-    for v_ego, steering_angle, previous, accel_coast, allow_throttle in scenarios:
-      stock = get_cruise_accel(
-        False, 30.0, v_ego, previous, steering_angle, cp, DT_MDL, accel_coast, allow_throttle,
-      )
-      self.assertGreater(stock, 0.0)
-      self.assertLess(stock, get_max_accel(v_ego))
-      for profile in ACCEL_PROFILES:
-        with self.subTest(v_ego=v_ego, profile=profile):
-          planner = planner_for_hook(profile=profile)
-          sm = PlannerSM()
-          sm["carState"].vEgo = v_ego
-          candidates = [(-0.5, MpcSource.lead0, True), (stock, MpcSource.cruise, False)]
-          result = planner.update_accel_controller(sm, candidates)
-          self.assertEqual(result[0], candidates[0])
-          self.assertEqual(result[1][1:], candidates[1][1:])
-          self.assertAlmostEqual(result[1][0], stock * profile_accel_scale(profile, v_ego))
-
-  def test_early_decel_is_an_additive_nonpositive_cruise_candidate(self):
-    planner = planner_for_hook(profile=AccelProfile.sport)
-    sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0)))
-    candidates = [(-0.02, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)]
-    result = planner.update_accel_controller(sm, candidates)
-    self.assertEqual(result[:2], candidates)
-    self.assertEqual(len(result), 3)
-    self.assertEqual(result[-1][1:], (MpcSource.cruise, False))
-    self.assertLessEqual(result[-1][0], 0.0)
-
-  def test_early_decel_enters_from_the_previous_positive_command(self):
-    planner = planner_for_hook(profile=AccelProfile.sport)
-    planner.previous_plan_accel = 0.48
-    sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0)))
-    candidates = [(-0.02, MpcSource.lead0, False), (0.60, MpcSource.cruise, False)]
-
-    result = planner.update_accel_controller(sm, candidates)
-
-    self.assertEqual(result[:2], candidates)
-    self.assertAlmostEqual(result[-1][0], 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL)
-    self.assertEqual(result[-1][1:], (MpcSource.cruise, False))
-
  def test_radar_freshness_requires_a_healthy_advanced_message(self):
    planner = planner_for_hook()
    planner._radar_log_mono_time = None
@@ -173,19 +241,41 @@ class TestPlannerHook(OpenpilotTestCase):


 class TestParamsSchemaAndTelemetry(OpenpilotTestCase):
-  def test_params_enable_and_sanitize_profile(self):
-    instance = accel_controller(enabled=False)
-    instance.params.put_bool("AccelPersonalityEnabled", True, block=True)
-    instance.params.put("AccelPersonality", 99, block=True)
-    instance.update_params()
-    self.assertTrue(instance.is_enabled)
-    self.assertEqual(instance.profile, AccelProfile.sport)
-    self.assertEqual(instance.params.get("AccelPersonality"), AccelProfile.sport)
+  def test_planner_reads_and_sanitizes_controller_params(self):
+    planner = planner_for_hook(enabled=False)
+    planner.params = Params()
+    planner.params.put_bool("AccelPersonalityEnabled", False, block=True)

-    instance.params.put_bool("AccelPersonalityEnabled", False, block=True)
-    instance._param_frame = 0
-    instance.update_params()
-    self.assertFalse(instance.is_enabled)
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      planner.params.put("AccelPersonality", profile, block=True)
+      planner.read_accel_controller_params()
+      self.assertFalse(planner.accel_controller_enabled)
+      self.assertEqual(planner.accel_controller_profile, profile)
+
+    planner.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    for value, expected in ((-99, AccelProfile.eco), (99, AccelProfile.sport)):
+      planner.params.put("AccelPersonality", value, block=True)
+      planner.read_accel_controller_params()
+      self.assertTrue(planner.accel_controller_enabled)
+      self.assertEqual(planner.accel_controller_profile, expected)
+      self.assertEqual(planner.params.get("AccelPersonality"), expected)
+
+  def test_planner_refreshes_controller_params_every_five_frames(self):
+    planner = planner_for_hook(enabled=False)
+    planner._radar_log_mono_time = None
+    planner.events_sp = SimpleNamespace(clear=lambda: None)
+    planner.dec.update = lambda sm: None
+    planner.e2e_alerts_helper = SimpleNamespace(update=lambda sm, events: None)
+    calls = []
+    sm = PlannerSM()
+    planner.read_accel_controller_params = lambda: calls.append(sm.frame)
+
+    for frame in range(10):
+      sm.frame = frame
+      sm.logMonoTime["radarState"] = frame
+      planner.update(sm)
+
+    self.assertEqual(calls, [0, 5])

  def test_schema_contract_and_round_trip(self):
    fields = custom.LongitudinalPlanSP.AccelController.schema.fields
@@ -201,15 +291,15 @@ class TestParamsSchemaAndTelemetry(OpenpilotTestCase):
    message = custom.LongitudinalPlanSP.new_message()
    message.accelController.enabled = True
    message.accelController.active = True
-    message.accelController.profile = AccelProfile.sport
-    message.accelController.state = AccelControllerState.release
+    message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
+    message.accelController.state = AccelControllerState.stopHold
    with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
      self.assertTrue(reader.accelController.enabled)
      self.assertTrue(reader.accelController.active)
-      self.assertEqual(reader.accelController.profile, AccelProfile.sport)
-      self.assertEqual(reader.accelController.state, AccelControllerState.release)
+      self.assertEqual(reader.accelController.profile, custom.LongitudinalPlanSP.AccelController.Profile.sport)
+      self.assertEqual(reader.accelController.state, AccelControllerState.stopHold)

-  def test_minimal_controller_telemetry_is_published(self):
+  def test_controller_telemetry_is_published(self):
    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
    dynamic_planner: Any = planner
    dynamic_planner.source = LongitudinalPlanSource.cruise
@@ -217,36 +307,55 @@ class TestParamsSchemaAndTelemetry(OpenpilotTestCase):
    dynamic_planner.output_a_target = -0.1
    dynamic_planner.events_sp = SimpleNamespace(to_msg=list)
    dynamic_planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
-    dynamic_planner.accel_controller = accel_controller(profile=AccelProfile.eco)
-    dynamic_planner.accel_controller.is_active = True
+    dynamic_planner.accel_controller = accel_controller()
+    dynamic_planner.accel_controller_available = True
+    dynamic_planner.accel_controller_enabled = True
+    dynamic_planner.accel_controller_profile = AccelProfile.sport
    dynamic_planner.accel_controller.state = AccelControllerState.restrict
    dynamic_planner.scc = SimpleNamespace(
      vision=SimpleNamespace(
-        state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0,
-        max_pred_lat_acc=0.0, is_enabled=False, is_active=False,
+        state=0,
+        output_v_target=20.0,
+        output_a_target=0.0,
+        current_lat_acc=0.0,
+        max_pred_lat_acc=0.0,
+        is_enabled=False,
+        is_active=False,
      ),
      map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
    )
    dynamic_planner.resolver = SimpleNamespace(
-      speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
-      speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0,
+      speed_limit=0.0,
+      speed_limit_last=0.0,
+      speed_limit_final=0.0,
+      speed_limit_final_last=0.0,
+      speed_limit_valid=False,
+      speed_limit_last_valid=False,
+      speed_limit_offset=0.0,
+      distance=0.0,
      source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
    )
    dynamic_planner.sla = SimpleNamespace(
-      state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False,
-      is_active=False, output_v_target=20.0, output_a_target=0.0,
+      state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled,
+      is_enabled=False,
+      is_active=False,
+      output_v_target=20.0,
+      output_a_target=0.0,
    )
    dynamic_planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
    sent = {}
+
    dynamic_planner.publish_longitudinal_plan_sp(
-      PlannerSM(), SimpleNamespace(send=lambda service, message: sent.update({service: message})),
+      PlannerSM(),
+      SimpleNamespace(send=lambda service, message: sent.update({service: message})),
    )

    telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
    self.assertTrue(telemetry.enabled)
    self.assertTrue(telemetry.active)
-    self.assertEqual(telemetry.profile, AccelProfile.eco)
+    self.assertEqual(telemetry.profile, AccelProfile.sport)
    self.assertEqual(telemetry.state, AccelControllerState.restrict)
-    self.assertEqual(set(custom.LongitudinalPlanSP.AccelController.schema.fields), {
-      "enabled", "active", "shadowOnlyDEPRECATED", "profile", "state",
-    })
+    self.assertEqual(
+      set(custom.LongitudinalPlanSP.AccelController.schema.fields),
+      {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"},
+    )
@@ -8,10 +8,13 @@ See the LICENSE.md file in the root directory for more details.
 from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
+from openpilot.common.params import Params
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
+from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -27,6 +30,11 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 class LongitudinalPlannerSP:
  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
    self.accel_controller = AccelController(CP, dt=mpc.dt)
+    self.accel_controller_available = bool(CP.openpilotLongitudinalControl)
+    self.accel_controller_enabled = False
+    self.accel_controller_profile = AccelProfile.normal
+    self.params = Params()
+    self.read_accel_controller_params()
    self.events_sp = EventsSP()
    self.dec = DynamicExperimentalController(CP, mpc)
    self.model_accel_transition = ModelAccelTransition(mpc.dt)
@@ -44,6 +52,10 @@ class LongitudinalPlannerSP:
    self.output_a_target = 0.
    self.previous_plan_accel = 0.

+  def read_accel_controller_params(self) -> None:
+    self.accel_controller_enabled = self.params.get_bool("AccelPersonalityEnabled")
+    self.accel_controller_profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+
  def is_e2e(self, sm: messaging.SubMaster) -> bool:
    experimental_mode = sm['selfdriveState'].experimentalMode
    if not self.dec.active():
@@ -58,42 +70,56 @@ class LongitudinalPlannerSP:
    )

  def update_accel_controller(self, sm: messaging.SubMaster, candidates):
-    cruise_index = next((i for i, candidate in enumerate(candidates) if candidate[1] == MpcSource.cruise), -1)
+    if not candidates:
+      return candidates
+    cruise_index = next((i for i, candidate in enumerate(candidates[1:], start=1) if candidate[1] == MpcSource.cruise), -1)
    if cruise_index < 0:
      return candidates

    CS = sm['carState']
    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
-    reset_state = ((long_control_off if self.accel_controller.available else not sm['selfdriveState'].enabled)
+    reset_state = ((long_control_off if self.accel_controller_available else not sm['selfdriveState'].enabled)
                   or CS.vCruise == V_CRUISE_UNSET)
    cruise_accel = candidates[cruise_index][0]
-    acc_selected = not self.is_e2e(sm)
-    decision = self.accel_controller.update(
-      sm['radarState'], v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target,
-      follow_personality=sm['selfdriveState'].personality, engaged=not reset_state,
-      cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=acc_selected,
-      stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle,
-      radar_healthy=self._radar_healthy_this_cycle,
-      force_decel=sm['controlsState'].forceDecel, previous_plan_accel=self.previous_plan_accel,
-    )
-    if self.accel_controller.is_enabled and acc_selected and reset_state:
-      # Do not carry an unactuated cruise ramp into engagement.
-      self.a_cruise = 0.0
-      if cruise_accel > 0.0:
-        candidates = list(candidates)
-        _, source, stop = candidates[cruise_index]
-        candidates[cruise_index] = (0.0, source, stop)
-        return candidates
-    if decision.cruise_accel_max is None and decision.early_decel is None:
+    mpc_accel = candidates[0][0]
+    mpc_source = candidates[0][1]
+    radar_state = sm['radarState']
+    lead_one_present = bool(getattr(getattr(radar_state, 'leadOne', None), 'present', False))
+    lead_two_present = bool(getattr(getattr(radar_state, 'leadTwo', None), 'present', False))
+    stock_mpc_lead = 0 if mpc_source == MpcSource.lead0 and lead_one_present else 1 if mpc_source == MpcSource.lead1 and lead_two_present else -1
+    stock_should_stop = any(candidate[2] for candidate in candidates)
+    transitioning_from_e2e = self.model_accel_transition.active
+    acc_selected = not self.is_e2e(sm) and not transitioning_from_e2e
+    active = not reset_state and acc_selected and not sm['controlsState'].forceDecel
+    radar_valid = self._radar_fresh_this_cycle and self._radar_healthy_this_cycle
+    if not self.accel_controller_available or not self.accel_controller_enabled or not active:
+      self.accel_controller.reset()
+      if self.accel_controller_available and self.accel_controller_enabled and acc_selected and reset_state:
+        self.a_cruise = 0.0
+        if cruise_accel > 0.0:
+          candidates = list(candidates)
+          _, source, stop = candidates[cruise_index]
+          candidates[cruise_index] = (0.0, source, stop)
      return candidates

-    candidates = list(candidates)
-    if decision.cruise_accel_max is not None:
-      _, source, stop = candidates[cruise_index]
-      candidates[cruise_index] = (min(cruise_accel, decision.cruise_accel_max), source, stop)
-    if decision.early_decel is not None:
-      candidates.append((decision.early_decel, MpcSource.cruise, False))
-    return candidates
+    decision = self.accel_controller.update(
+      radar_state, v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target, follow_personality=sm['selfdriveState'].personality,
+      radar_valid=radar_valid, stock_cruise_accel=cruise_accel, stock_mpc_accel=mpc_accel, stock_should_stop=stock_should_stop,
+      fcw=self.fcw, standstill=CS.standstill, stock_mpc_lead=stock_mpc_lead, previous_plan_accel=self.previous_plan_accel,
+      profile=self.accel_controller_profile,
+    )
+    if decision.a_target is None:
+      return candidates
+
+    controller_source = (MpcSource.lead1 if self.accel_controller.selected_lead == 1 else
+                         MpcSource.lead0 if self.accel_controller.selected_lead == 0 else MpcSource.cruise)
+    controller_candidate = (decision.a_target, controller_source, decision.should_stop)
+    if not decision.stock_safety_required:
+      return [controller_candidate]
+
+    stock_candidate = min(candidates, key=lambda candidate: candidate[0])
+    stock_safety_candidate = (stock_candidate[0], stock_candidate[1], stock_should_stop)
+    return [controller_candidate, stock_safety_candidate]

  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
    radar_log_mono_time = sm.logMonoTime['radarState']
@@ -136,7 +162,8 @@ class LongitudinalPlannerSP:

  def update(self, sm: messaging.SubMaster) -> None:
    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
-    self.accel_controller.update_params()
+    if sm.frame % 5 == 0:
+      self.read_accel_controller_params()
    self.events_sp.clear()
    self.dec.update(sm)
    self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -159,9 +186,9 @@ class LongitudinalPlannerSP:
    dec.active = self.dec.active()

    accel_controller = longitudinalPlanSP.accelController
-    accel_controller.enabled = self.accel_controller.is_enabled
+    accel_controller.enabled = self.accel_controller_available and self.accel_controller_enabled
    accel_controller.active = self.accel_controller.is_active
-    accel_controller.profile = self.accel_controller.profile
+    accel_controller.profile = self.accel_controller_profile
    accel_controller.state = self.accel_controller.state

    # Smart Cruise Control
@@ -1,3 +1,4 @@
+from contextlib import contextmanager
 import inspect
 from typing import Any
 from unittest import mock
@@ -6,16 +7,16 @@ import numpy as np

 from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
 from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc, LongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import NEUTRAL_ACCEL
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP as Plant


-def configure(plant, *, enabled=True, profile=AccelProfile.normal):
-  controller: Any = plant.planner.accel_controller
-  controller.enabled = enabled
-  controller.profile = profile
-  controller.update_params = lambda: None
+def configure(plant, *, enabled=True):
+  planner: Any = plant.planner
+  planner.accel_controller_enabled = enabled
+  planner.read_accel_controller_params = lambda: None
  dec: Any = plant.planner.dec
  dec._enabled = False
  dec._read_params = lambda: None
@@ -36,10 +37,32 @@ def record_candidates(plant):
  return snapshots


+@contextmanager
+def scripted_stock_candidates(plant, *, mpc_accel, cruise_accel, source=LongitudinalPlanSource.cruise):
+  values = {"mpc": float(mpc_accel), "cruise": float(cruise_accel)}
+
+  def update_mpc(_radar_state, personality):
+    plant.planner.mpc.source = source
+    plant.planner.mpc.crash_cnt = 0
+
+  with (
+    mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc),
+    mock.patch(
+      "openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan",
+      side_effect=lambda *_args, **_kwargs: values["mpc"],
+    ),
+    mock.patch(
+      "openpilot.selfdrive.controls.lib.longitudinal_planner.get_cruise_accel",
+      side_effect=lambda *_args, **_kwargs: values["cruise"],
+    ),
+  ):
+    yield values
+
+
 class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
  def test_one_stock_mpc_solve_with_unmodified_bounds_and_api(self):
    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=70.0)
-    configure(plant, profile=AccelProfile.eco)
+    configure(plant)
    mpc: Any = plant.planner.mpc
    original_run = mpc.run
    run_calls = []
@@ -65,7 +88,7 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):

  def test_stock_lead_mpc_braking_remains_authoritative(self):
    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=30.0)
-    configure(plant, profile=AccelProfile.eco)
+    configure(plant)
    snapshots = record_candidates(plant)

    stock_mpc_won = False
@@ -84,7 +107,7 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):

  def test_stock_should_stop_survives_controller_candidates(self):
    plant = Plant(enabled=True, lead_relevancy=True, speed=0.2, distance_lead=3.0)
-    configure(plant, profile=AccelProfile.normal)
+    configure(plant)
    snapshots = record_candidates(plant)

    result = plant.step(v_lead=0.0, v_cruise=8.0)
@@ -180,7 +203,6 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
      plant.planner.mpc.source = LongitudinalPlanSource.cruise
      plant.planner.mpc.crash_cnt = 0

-    # Exercise final candidate arbitration without depending on the local ACADOS build.
    with (
      mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc),
      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.94),
@@ -197,9 +219,9 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
    self.assertLessEqual(second - first, 0.15 + 1e-9)
    self.assertEqual(plant.planner.mpc.source, LongitudinalPlanSource.cruise)

-  def test_disengaged_cruise_state_cannot_leak_into_first_sport_accel(self):
+  def test_disengaged_cruise_state_cannot_leak_into_first_accel(self):
    plant = Plant(enabled=False, speed=10.0)
-    configure(plant, profile=AccelProfile.sport)
+    configure(plant)

    with (
      mock.patch.object(plant.planner.mpc, "update", return_value=None),
@@ -214,3 +236,166 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):

    self.assertGreater(first, 0.0)
    self.assertLessEqual(first, 0.10)
+
+
+class TestAccelControllerClosedLoopAcceptance(OpenpilotTestCase):
+  def test_confirmed_lead_departure_releases_brake_in_the_same_planner_frame(self):
+    plant = Plant(
+      enabled=True,
+      lead_relevancy=True,
+      speed=0.0,
+      distance_lead=6.0,
+      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+      run_long_control=True,
+    )
+    configure(plant)
+
+    with scripted_stock_candidates(plant, mpc_accel=-0.5, cruise_accel=1.1, source=LongitudinalPlanSource.lead0) as stock:
+      for _ in range(12):
+        held = plant.step(v_lead=0.0, v_cruise=8.0)
+      self.assertTrue(held["should_stop"])
+      self.assertLess(held["actuator_command"], 0.0)
+
+      stock["mpc"] = 1.1
+      unconfirmed = plant.step(v_lead=1.0, v_cruise=8.0)
+      self.assertTrue(unconfirmed["should_stop"])
+
+      confirmed = plant.step(v_lead=1.0, v_cruise=8.0)
+      self.assertFalse(confirmed["should_stop"])
+      self.assertGreater(confirmed["a_target"], 0.0)
+      self.assertGreater(confirmed["actuator_command"], 0.0)
+      self.assertEqual(confirmed["long_control_state"], LongCtrlState.pid)
+
+      for _ in range(12):
+        moving = plant.step(v_lead=1.0, v_cruise=8.0)
+        if moving["speed"] > 0.0:
+          break
+      self.assertGreater(moving["speed"], 0.0)
+
+  def test_slower_lead_causes_routine_decel_while_ttc_is_still_long(self):
+    initial_gap = 55.0
+    initial_ego_speed = 20.0
+    lead_speed = 14.0
+    plant = Plant(enabled=True, lead_relevancy=True, speed=initial_ego_speed, distance_lead=initial_gap)
+    configure(plant)
+
+    with scripted_stock_candidates(plant, mpc_accel=0.7, cruise_accel=0.7):
+      result = plant.step(v_lead=lead_speed, v_cruise=30.0)
+
+    self.assertGreater(initial_gap / (initial_ego_speed - lead_speed), 8.0)
+    self.assertTrue(result["controller_active"])
+    self.assertLess(result["a_target"], 0.0)
+
+  def test_monotonically_slowing_lead_does_not_cause_brake_gas_brake(self):
+    plant = Plant(
+      enabled=True,
+      lead_relevancy=True,
+      speed=18.0,
+      distance_lead=65.0,
+      actuator_delay=0.15,
+      actuator_lag=0.20,
+    )
+    configure(plant)
+    targets = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for frame in range(80):
+        lead_speed = max(8.0, 20.0 - 0.15 * frame)
+        targets.append(plant.step(v_lead=lead_speed, v_cruise=30.0)["a_target"])
+
+    phases = []
+    for target in targets:
+      phase = 1 if target > NEUTRAL_ACCEL else -1 if target < -NEUTRAL_ACCEL else 0
+      if phase and (not phases or phase != phases[-1]):
+        phases.append(phase)
+
+    self.assertIn(-1, phases)
+    first_brake = phases.index(-1)
+    self.assertNotIn(1, phases[first_brake + 1:])
+
+  def test_terminal_stop_is_bounded_and_has_no_positive_rebound(self):
+    plant = Plant(
+      enabled=True,
+      lead_relevancy=True,
+      speed=5.0,
+      distance_lead=24.0,
+      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+      run_long_control=True,
+    )
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(400):
+        result = plant.step(v_lead=0.0, v_cruise=8.0)
+        samples.append(result)
+        if result["speed"] == 0.0:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertLessEqual(plant.distance_lead - plant.distance, 7.1)
+
+    moving_samples = samples[:-1]
+    terminal_samples = [sample for sample in moving_samples if sample["speed"] <= 0.3]
+    self.assertTrue(terminal_samples)
+    self.assertLessEqual(max(sample["a_target"] for sample in terminal_samples), 1e-9)
+    self.assertLessEqual(max(sample["realized_acceleration"] for sample in terminal_samples), 0.02)
+    self.assertGreaterEqual(min(sample["realized_acceleration"] for sample in moving_samples), -1.2)
+
+    realized_jerk = [
+      abs(current["realized_acceleration"] - previous["realized_acceleration"]) / plant.ts
+      for previous, current in zip(moving_samples[:-1], moving_samples[1:], strict=True)
+    ]
+    self.assertLessEqual(max(realized_jerk), 1.2)
+
+  def test_high_speed_stop_uses_planned_decel_before_stock_emergency(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=120.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(800):
+        result = plant.step(v_lead=0.0, v_cruise=25.0)
+        samples.append(result)
+        if result["speed"] == 0.0 and result["should_stop"]:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5)
+
+  def test_terminal_decel_ceiling_does_not_fade_before_highway_stop(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(1000):
+        result = plant.step(v_lead=0.0, v_cruise=30.0)
+        samples.append(result)
+        if result["speed"] == 0.0 and result["should_stop"]:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5)
+
+  def test_feasible_highway_stop_does_not_fall_back_to_late_stock_braking(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    for _ in range(500):
+      result = plant.step(v_lead=0.0, v_cruise=30.0)
+      samples.append(result)
+      if result["speed"] == 0.0 and result["should_stop"]:
+        break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.55)
@@ -613,8 +613,7 @@ class TestLongControlSP(OpenpilotTestCase):
      run_long_control=True,
      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
    )
-    plant.planner.accel_controller.enabled = True
-    plant.planner.accel_controller.profile = 1
+    plant.planner.accel_controller_enabled = True
    plant.planner.dec._enabled = False
    commands = []
    speeds = []
@@ -622,7 +621,7 @@ class TestLongControlSP(OpenpilotTestCase):
    solver_statuses = []

    with (
-      mock.patch.object(plant.planner.accel_controller, "update_params", return_value=None),
+      mock.patch.object(plant.planner, "read_accel_controller_params", return_value=None),
      mock.patch.object(plant.planner.dec, "_read_params", return_value=None),
    ):
      while plant.current_time < 5.0:
@@ -385,8 +385,6 @@ class PlantSP(Plant):
      "mpc_source": self.planner.mpc.source,
      "dec_mode": self.planner.dec.mode(),
      "controller_active": accel_controller.is_active,
-      "controller_accel_max": accel_controller.cruise_accel_max,
-      "controller_early_decel": accel_controller.early_decel,
      "model_action": {
        "desiredAcceleration": float(model_acceleration),
        "shouldStop": bool(model_should_stop),
@@ -135,8 +135,6 @@ class TestPlantSP(OpenpilotTestCase):
    assert first["mpc_source"] is not None
    assert first["dec_mode"] in ("acc", "blended")
    assert "controller_active" in first
-    assert "controller_accel_max" in first
-    assert "controller_early_decel" in first
    assert first["lead_one_observation"] is not None
    assert first["truth_lead"] == first["lead_one_observation"]

@@ -656,7 +656,7 @@
              "key": "AccelPersonalityEnabled",
              "widget": "toggle",
              "title": "Enable Accel Controller",
-              "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
+              "description": "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop.",
              "visibility": [
                {
                  "type": "capability",
@@ -676,7 +676,7 @@
              "key": "AccelPersonality",
              "widget": "multiple_button",
              "title": "Acceleration Profile",
-              "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
+              "description": "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles.",
              "options": [
                {
                  "value": 0,
@@ -696,11 +696,6 @@
                  "type": "capability",
                  "field": "has_longitudinal_control",
                  "equals": true
-                },
-                {
-                  "type": "param",
-                  "key": "AccelPersonalityEnabled",
-                  "equals": true
                }
              ]
            },
@@ -46,8 +46,8 @@ sections:
  - key: AccelPersonalityEnabled
    widget: toggle
    title: Enable Accel Controller
-    description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
-      and stopping authority.
+    description: Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking
+      remains available as a safety backstop.
    visibility:
    - $ref: '#/macros/longitudinal'
    enablement:
@@ -55,8 +55,8 @@ sections:
  - key: AccelPersonality
    widget: multiple_button
    title: Acceleration Profile
-    description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
-      and recovers more quickly.
+    description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
+      profiles.
    options:
    - value: 0
      label: Eco
@@ -66,9 +66,6 @@ sections:
      label: Sport
    enablement:
    - $ref: '#/macros/longitudinal'
-    - type: param
-      key: AccelPersonalityEnabled
-      equals: true
  - key: IntelligentCruiseButtonManagement
    widget: toggle
    title: Intelligent Cruise Button Management (ICBM) (Alpha)
@@ -287,10 +287,17 @@ class TestKnownPanels(OpenpilotTestCase):
      {"value": 2, "label": "Sport"},
    ]
    assert {
-      "type": "param",
-      "key": "AccelPersonalityEnabled",
+      "type": "capability",
+      "field": "has_longitudinal_control",
+      "equals": True,
+    } in items["AccelPersonalityEnabled"]["enablement"]
+    assert {
+      "type": "capability",
+      "field": "has_longitudinal_control",
      "equals": True,
    } in items["AccelPersonality"]["enablement"]
+    profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"}
+    assert "AccelPersonalityEnabled" not in profile_enable_keys


 class TestKnownVehicleSettings(OpenpilotTestCase):