feat(long): smooth lead following with acceleration profiles

openpilot/cereal/custom.capnp

@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -305,6 +306,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    shadowOnlyDEPRECATED @2 :Bool;
+    profile @3 :Profile;
+    state @4 :State;
+
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+
+    enum State {
+      inactive @0;
+      free @1;
+      restrict @2;
+      hold @3;
+      release @4;
+      stopHold @5;
+    }
+  }
+
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

openpilot/common/params_keys.h

@@ -232,6 +232,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // Accel Controller profiles (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

openpilot/common/tests/test_params.py

@@ -112,12 +112,16 @@ class TestParams:
   def test_params_default_value(self):
     self.params.remove("LanguageSetting")
     self.params.remove("LongitudinalPersonality")
+    self.params.remove("AccelPersonalityEnabled")
+    self.params.remove("AccelPersonality")
     self.params.remove("LiveParametersV2")
 
     assert self.params.get("LanguageSetting") is None
     assert self.params.get("LanguageSetting", return_default=False) is None
     assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
     assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
+    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
+    assert self.params.get("AccelPersonality", return_default=True) == 1
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -9,6 +9,7 @@ from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
 
 if __name__ == '__main__':  # generating code
   from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -213,8 +214,9 @@ def gen_long_ocp():
   return ocp
 
 
-class LongitudinalMpc:
+class LongitudinalMpc(LongitudinalMpcSP):
   def __init__(self, dt=DT_MDL):
+    LongitudinalMpcSP.__init__(self)
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
@@ -266,7 +268,8 @@ class LongitudinalMpc:
   def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
     jerk_factor = get_jerk_factor(personality)
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost,
+                    LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)]
     constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
@@ -326,7 +329,7 @@ class LongitudinalMpc:
     # when the leads are no factor.
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
+    v_upper = v_ego + (T_IDXS * self.cruise_accel_max(CRUISE_MAX_ACCEL) * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 
@@ -340,6 +343,7 @@ class LongitudinalMpc:
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
+    LongitudinalMpcSP.apply_accel_limits(self)
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -359,6 +363,7 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
+    LongitudinalMpcSP.save_solution_status(self)
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
 
     for i in range(N+1):

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
-    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -110,15 +110,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast = max(accel_coast, accel_clip[0])
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
-
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
-
     if force_slow_decel:
       v_cruise = 0.0
 
+    is_e2e, v_cruise = LongitudinalPlannerSP.update_accel_controller(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state)
+
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.mpc_accel_seed)
     self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -135,13 +135,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    action_t = self.CP.longitudinalActuatorDelay + DT_MDL
     output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
                                                                         action_t=action_t)
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.is_e2e(sm):
+    if is_e2e:
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:
@@ -149,6 +149,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
+    self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop)
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

openpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -11,6 +11,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
+class PlannerSM(dict):
+  def __init__(self, radar_frame: int, services: dict):
+    super().__init__(services)
+    self.logMonoTime = {"radarState": radar_frame}
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+
+
 class Plant:
   messaging_initialized = False
 
@@ -132,7 +140,7 @@ class Plant:
     car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
-    sm = {'radarState': radar.radarState,
+    sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
           'carState': car_state.carState,
           'carControl': car_control.carControl,
           'controlsState': control.controlsState,
@@ -141,7 +149,7 @@ class Plant:
           'modelV2': model.modelV2,
           'carStateSP': car_state_sp.carStateSP,
           'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation}
+          'gpsLocation': gps_data.gpsLocation})
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target
     if self.planner.output_should_stop:

openpilot/selfdrive/ui/layouts/settings/toggles.py

@@ -27,6 +27,12 @@ DESCRIPTIONS = {
     "In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
     "your steering wheel distance button."
   ),
+  "AccelPersonalityEnabled": tr_noop(
+    "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+  ),
+  "AccelPersonality": tr_noop(
+    "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
+  ),
   "IsLdwEnabled": tr_noop(
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
     "without a turn signal activated while driving over 31 mph (50 km/h)."
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
+    self._accel_personality_enabled = toggle_item(
+      lambda: tr("Enable Accel Controller"),
+      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
+      self._params.get_bool("AccelPersonalityEnabled"),
+      callback=self._set_accel_personality_enabled,
+      icon="speed_limit.png",
+    )
+
+    self._accel_personality_setting = multiple_button_item(
+      lambda: tr("Acceleration Profile"),
+      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
+      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
+      button_width=300,
+      callback=self._set_accel_personality,
+      selected_index=self._params.get("AccelPersonality", return_default=True),
+      icon="speed_limit.png"
+    )
+
     self._toggles = {}
     self._locked_toggles = set()
     for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality after NDOG toggle
+      # 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["AccelPersonality"] = self._accel_personality_setting
 
     self._update_experimental_mode_icon()
     self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):
 
   def _update_toggles(self):
     ui_state.update_params()
+    accel_personality_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. " +
@@ -176,11 +203,15 @@ 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)
       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_personality_setting.action_item.set_enabled(False)
         self._params.remove("ExperimentalMode")
 
         unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -203,6 +234,10 @@ 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)
+    )
 
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
@@ -247,3 +282,10 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
+
+  def _set_accel_personality(self, button_index: int):
+    self._params.put("AccelPersonality", button_index, block=True)
+
+  def _set_accel_personality_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)

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -14,6 +14,8 @@ 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_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
     self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode")
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
@@ -24,6 +26,8 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
+      self._accel_personality_enabled,
+      self._accel_personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
       ldw_toggle,
@@ -36,6 +40,7 @@ class TogglesLayoutMici(NavScroller):
     # Toggle lists
     self._refresh_toggles = (
       ("ExperimentalMode", self._experimental_btn),
+      ("AccelPersonalityEnabled", self._accel_personality_enabled),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -45,6 +50,9 @@ 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)
 
@@ -75,13 +83,18 @@ 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_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_personality_toggle.set_visible(False)
         ui_state.params.remove("ExperimentalMode")
 
     # Refresh toggles from params to mirror external changes
     for key, item in self._refresh_toggles:
       item.set_checked(ui_state.params.get_bool(key))
+    self._accel_personality_toggle.refresh()

openpilot/selfdrive/ui/mici/widgets/button.py

@@ -383,13 +383,18 @@ class BigMultiParamToggle(BigMultiToggle):
     self._load_value()
 
   def _load_value(self):
-    self.set_value(self._options[self._params.get(self._param) or 0])
+    value = self._params.get(self._param, return_default=True)
+    index = value if isinstance(value, int) else 0
+    self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     super()._handle_mouse_release(mouse_pos)
     new_idx = self._options.index(self.value)
     self._params.put(self._param, new_idx)
 
+  def refresh(self):
+    self._load_value()
+
 
 class BigParamControl(BigToggle):
   def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):

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

@@ -0,0 +1,241 @@
+import math
+
+from opendbc.car.interfaces import ACCEL_MAX
+from openpilot.cereal import custom
+from openpilot.common.params import Params
+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 (
+  LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, LEAD_DROPOUT_COAST_TIME, MPC_DECEL_JERK_COST_MULTIPLIER,
+  MPC_DECEL_JERK_LONG_TREND_FRAMES, MPC_DECEL_JERK_LONG_TREND_RATE, MPC_DECEL_JERK_MAX_REQUIRED_DECEL,
+  MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES,
+  PARAM_READ_INTERVAL, LEAD_RELEASE_CONFIRM_TIME, RADAR_STALE_TIMEOUT, SPEED_DEADBAND, VEGO_NOISE_TOLERANCE,
+  AccelProfile, profile_accel_max, sanitize_profile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+
+AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
+
+
+class AccelController:
+  def __init__(self, CP, dt: float = DT_MDL):
+    if not math.isfinite(dt) or dt <= 0.0:
+      raise ValueError("dt must be finite and positive")
+
+    self.dt = dt
+    self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
+    self.lead_confirm_frames = max(LEAD_SAMPLE_FILTER_FRAMES, math.ceil(LEAD_RELEASE_CONFIRM_TIME / dt))
+    self.dropout_frames = max(self.lead_confirm_frames, math.ceil(LEAD_DROPOUT_COAST_TIME / dt))
+    self.dropout_release_frames = max(1, self.dropout_frames - self.lead_confirm_frames)
+    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
+    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 / dt)))
+    self._param_frame = 0
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples: list[float] = []
+    self._required_decel_long_samples: list[float] = []
+    self._required_decel_lead = -1
+    self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+
+    self.lead_controller = LeadController()
+    self._stale_frames = 0
+    self._cruise_accel_limited = False
+
+    self.is_active = False
+    self.output_v_target = 0.0
+    self.mpc_accel_max: tuple[float, ...] | None = None
+    self.cruise_accel_max: float | None = None
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.required_decel = 0.0
+
+  @property
+  def is_enabled(self) -> bool:
+    return self.available and self.enabled
+
+  @property
+  def launching(self) -> bool:
+    return self.lead_controller.launching
+
+  @property
+  def departure_launching(self) -> bool:
+    return self.lead_controller.departure_launching
+
+  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 reset(self) -> None:
+    self.lead_controller = LeadController()
+    self._stale_frames = 0
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples.clear()
+    self._required_decel_long_samples.clear()
+    self._required_decel_lead = self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+    self._cruise_accel_limited = False
+    self.is_active = False
+    self.output_v_target = 0.0
+    self.mpc_accel_max = None
+    self.cruise_accel_max = None
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.required_decel = 0.0
+
+  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool,
+             engaged: bool, cruise_initialized: bool, stock_accel_max: float, radar_fresh: bool = True,
+             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0,
+             previous_plan_accel: float = 0.0) -> None:
+    self.profile = sanitize_profile(self.profile)
+    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
+    profile_max_accel = profile_accel_max(self.profile, sanitized_v_ego)
+    stock_accel_max = float(stock_accel_max)
+    positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX))
+                          if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan)
+    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
+    valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay,
+                                     engaged, cruise_initialized)
+    enabled_context = valid_context and self.is_enabled and bool(acc_selected)
+
+    lead_plan = None
+    if enabled_context:
+      if radar_fresh:
+        self._stale_frames = 0
+        lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality)
+      else:
+        self._stale_frames += 1
+        new_acc_handoff = (self.lead_controller.target_speed is None and previous_mpc_source == LongitudinalPlanSource.e2e
+                           and math.isfinite(previous_plan_accel))
+        if new_acc_handoff:
+          lead_plan = LeadPlan()
+
+      if lead_plan is not None:
+        self.lead_controller.update(lead_plan, base_speed, sanitized_v_ego, COMFORT_DECEL[self.profile], profile_max_accel, self.dt,
+                         self.lead_confirm_frames, self.dropout_frames, planner_speed, planner_accel,
+                         previous_mpc_source, previous_plan_accel)
+      stale_limit = self.dropout_frames if self.lead_controller.launching else self.radar_stale_frames
+      if not radar_fresh and self._stale_frames >= stale_limit and not self.lead_controller.stop_hold:
+        self.lead_controller.reset()
+    else:
+      self._stale_frames = 0
+
+    active = enabled_context and (radar_fresh or self.lead_controller.target_speed is not None)
+    self.is_active = active
+    if not active:
+      self.lead_controller.reset()
+      self._cruise_accel_limited = False
+      self.output_v_target = base_speed
+      self.mpc_accel_max = None
+      self.cruise_accel_max = None
+      self.state = AccelControllerState.inactive
+      self.selected_lead = -1
+      self.selected_lead_track_id = -1
+      self.required_decel = 0.0
+      return
+
+    lead_controller = self.lead_controller
+    self.output_v_target = lead_controller.target_speed if lead_controller.target_speed is not None else base_speed
+    self.selected_lead = lead_controller.selected_lead
+    self.selected_lead_track_id = lead_controller.selected_lead_track_id
+    self.required_decel = lead_controller.required_decel
+
+    recovery_limit_active = lead_controller.lead_recovery and lead_controller.recovery_accel_limit is not None and not lead_controller.e2e_braking_handoff
+    lead_recovery_accel = lead_controller.lead_recovery and planner_accel >= 0.0
+    dropout_coast = lead_controller.should_coast_on_dropout
+    profile_limit_active = (not lead_controller.stop_hold and (not lead_controller.has_lead or lead_recovery_accel
+                            or lead_controller.departure_launching or lead_controller.leadless_departure)
+                            and not lead_controller.e2e_braking_handoff)
+    if recovery_limit_active:
+      effective_accel_max = min(positive_accel_max, lead_controller.recovery_accel_limit)
+    elif profile_limit_active:
+      effective_accel_max = positive_accel_max
+    else:
+      effective_accel_max = math.inf
+    self.mpc_accel_max = (build_accel_ceiling(effective_accel_max, planner_accel)
+                          if recovery_limit_active or profile_limit_active else None)
+
+    lead_context = lead_controller.has_lead or math.isfinite(lead_controller.lead_speed_ceiling)
+    start_cruise_accel_limit = (lead_controller.target_speed is not None and not lead_controller.restricting and not lead_controller.releasing
+                                and lead_controller.has_lead and previous_mpc_source == LongitudinalPlanSource.cruise)
+    keep_cruise_accel_limit = (self._cruise_accel_limited and lead_context and not lead_controller.restricting and not lead_controller.releasing
+                               and not lead_controller.e2e_braking_handoff)
+    self._cruise_accel_limited = start_cruise_accel_limit or keep_cruise_accel_limit
+    if dropout_coast:
+      release_frame = max(0, lead_controller.lead_loss_frames - self.lead_confirm_frames)
+      self.cruise_accel_max = positive_accel_max * release_frame / self.dropout_release_frames
+    else:
+      self.cruise_accel_max = positive_accel_max if self._cruise_accel_limited else None
+
+    if lead_controller.stop_hold:
+      self.state = AccelControllerState.stopHold
+    elif lead_controller.restricting:
+      self.state = AccelControllerState.restrict
+    elif lead_controller.releasing:
+      self.state = AccelControllerState.release
+    elif lead_controller.target_speed >= base_speed - SPEED_DEADBAND:
+      self.state = AccelControllerState.free
+    else:
+      self.state = AccelControllerState.hold
+
+  def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float,
+                               previous_mpc_failed: bool) -> float:
+    lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0
+      and not self.launching and target_reduction > 1e-6)
+    same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id
+    lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead
+    if lead_changed:
+      self._lead_trend_warmup = True
+    elif not lead_restriction:
+      self._lead_trend_warmup = False
+    if not lead_restriction or not same_lead or not math.isfinite(self.required_decel):
+      self._required_decel_samples.clear()
+      self._required_decel_long_samples.clear()
+    if lead_restriction and math.isfinite(self.required_decel):
+      self._required_decel_samples.append(self.required_decel)
+      if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES:
+        self._required_decel_samples.pop(0)
+      self._required_decel_long_samples.append(self.required_decel)
+      if len(self._required_decel_long_samples) > MPC_DECEL_JERK_LONG_TREND_FRAMES:
+        self._required_decel_long_samples.pop(0)
+    self._required_decel_lead = self.selected_lead if lead_restriction else -1
+    self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1
+
+    history = self._required_decel_samples
+    history_ready = len(history) == MPC_DECEL_TREND_FRAMES
+    tightening_lead = (history_ready
+      and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
+      and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
+    long_history = self._required_decel_long_samples
+    long_history_ready = len(long_history) == MPC_DECEL_JERK_LONG_TREND_FRAMES
+    sustained_tightening = (long_history_ready
+      and (long_history[-1] - long_history[0]) / (self.dt * (len(long_history) - 1)) > MPC_DECEL_JERK_LONG_TREND_RATE)
+    modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
+      and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
+    smoothing_eligible = (modest_decel and (not self._lead_trend_warmup or history_ready)
+      and not tightening_lead and not sustained_tightening)
+    if history_ready:
+      self._lead_trend_warmup = False
+    if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked
+                               and (not modest_decel or tightening_lead or sustained_tightening)):
+      self._jerk_smoothing_blocked = True
+    elif not lead_restriction:
+      self._jerk_smoothing_blocked = False
+    return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0
+
+  def update_should_stop(self, should_stop: bool, departure_authorized: bool = True) -> bool:
+    if not self.is_active:
+      return should_stop
+    if self.lead_controller.departure_launching and departure_authorized:
+      return False
+    return should_stop or self.lead_controller.stop_hold

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py

@@ -0,0 +1,72 @@
+import math
+
+import numpy as np
+
+from openpilot.cereal import custom
+
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
+
+COMFORT_DECEL = {
+  AccelProfile.eco: 0.25,
+  AccelProfile.normal: 0.30,
+  AccelProfile.sport: 0.35,
+}
+
+ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
+ACCEL_PROFILE_MAX_V = {
+  AccelProfile.eco: [1.56, 1.30, 0.72, 0.32, 0.24],
+  AccelProfile.normal: [1.58, 1.51, 0.98, 0.53, 0.35],
+  AccelProfile.sport: [2.00, 1.91, 1.16, 0.73, 0.47],
+}
+
+BRAKING_ACCEL_THRESHOLD = -0.11
+
+LEAD_SAMPLE_FILTER_FRAMES = 5
+LEAD_RELEASE_CONFIRM_TIME = 0.50
+LEAD_DROPOUT_COAST_TIME = 1.50
+
+SPEED_DEADBAND = 0.15
+
+TARGET_RELEASE_SLEW = 8.75
+LAUNCH_TARGET_HEADROOM = 3.0
+LAUNCH_END_SPEED = 3.0
+
+LEAD_RECOVERY_HEADROOM = 1.25
+LEAD_RECOVERY_ACCEL_SLEW = 0.25
+LEAD_RECOVERY_DECEL_RATE = 0.50
+
+STOP_HOLD_EGO_SPEED = 0.30
+STOP_HOLD_SPEED_FLOOR = 0.15
+STOP_HOLD_EXIT_FRAMES = 4
+STOP_HOLD_CREEP_DISTANCE = 0.30
+STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
+DISTANCE_JUMP_CONFIRM_FRAMES = 3
+
+STOP_GAP_RESERVE = 0.75
+
+RADAR_STALE_TIMEOUT = 0.50
+MAX_LEAD_ACCEL_TAU = 10.0
+MIN_LEAD_SPEED = -1.0
+VEGO_NOISE_TOLERANCE = 0.10
+PARAM_READ_INTERVAL = 0.25
+ACCEL_LIMIT_HORIZON_JERK = 1.0
+
+MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
+MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
+MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
+MPC_DECEL_JERK_LONG_TREND_FRAMES = 6
+MPC_DECEL_JERK_LONG_TREND_RATE = 0.02
+MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
+MPC_DECEL_TREND_FRAMES = 4
+
+
+def sanitize_profile(profile: int) -> int:
+  return profile if profile in ACCEL_PROFILES else AccelProfile.normal
+
+
+def profile_accel_max(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_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)]))

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py

@@ -0,0 +1,22 @@
+import math
+
+import numpy as np
+
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_LIMIT_HORIZON_JERK, VEGO_NOISE_TOLERANCE
+
+
+def is_valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float,
+                     delay: float, engaged: bool, cruise_initialized: bool) -> bool:
+  values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay)
+  return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
+          and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values))
+
+
+def build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None:
+  if limit >= ACCEL_MAX - 1e-9:
+    return None
+  a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
+  ceiling = np.clip(np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS), 0.0, ACCEL_MAX)
+  return tuple(float(value) for value in ceiling)

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py

@@ -0,0 +1,134 @@
+"""
+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, get_stopped_equivalence_factor,
+)
+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, STOP_HOLD_SPEED_FLOOR, sanitize_profile,
+)
+
+
+class LeadPlan(NamedTuple):
+  speed_ceiling: float = math.inf
+  selected_lead: int = -1
+  selected_lead_track_id: int = -1
+  selected_lead_speed: float = math.inf
+  selected_lead_accel: float = 0.0
+  departure_lead: int = -1
+  departure_lead_track_id: int = -1
+  departure_lead_speed: float = math.inf
+  departure_lead_raw_speed: float = math.inf
+  departure_lead_distance: float = math.inf
+  departure_lead_separation: float = math.inf
+  departure_speed_ceiling: float = math.inf
+  closing_speed: float = 0.0
+  required_decel: float = 0.0
+  has_nearly_stopped_lead: bool = False
+  lead_status: bool = False
+
+
+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, float] | None:
+  if not lead.present:
+    return None
+  d_rel, v_lead = float(lead.dRel), 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
+  raw_v_lead = float(getattr(lead, "vLead", v_lead))
+  if not math.isfinite(raw_v_lead):
+    raw_v_lead = 0.0
+  return d_rel, max(v_lead, 0.0), max(raw_v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
+
+
+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()
+
+  leads = (radar_state.leadOne, radar_state.leadTwo)
+  lead_status = any(lead.present for lead in leads)
+  t_follow = get_T_FOLLOW(follow_personality)
+  if not math.isfinite(t_follow) or t_follow < 0.0:
+    return LeadPlan(lead_status=lead_status)
+
+  profile = sanitize_profile(profile)
+  x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay)
+  comfort_decel = COMFORT_DECEL[profile]
+  candidates: list[LeadPlan] = []
+  departure_candidates: list[tuple[float, LeadPlan]] = []
+
+  for lead_index, lead in enumerate(leads):
+    values = _lead_values(lead)
+    if values is None:
+      continue
+
+    d_rel, v_lead, raw_v_lead, a_lead, a_lead_tau = 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_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
+    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
+    closing_speed = max(v_ego_delay - v_lead_delay, 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_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
+    departure_speed_ceiling = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
+    separation = x_lead - x_ego
+    departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
+
+    finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, speed_ceiling, departure_speed_ceiling, departure_distance)
+    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 or not math.isfinite(separation)):
+      continue
+
+    track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
+    candidate = LeadPlan(
+      speed_ceiling=speed_ceiling, selected_lead=lead_index, selected_lead_track_id=track_id,
+      selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead, departure_lead=lead_index,
+      departure_lead_track_id=track_id, departure_lead_speed=v_lead_delay, departure_lead_raw_speed=raw_v_lead,
+      departure_lead_distance=d_rel, departure_lead_separation=separation,
+      departure_speed_ceiling=departure_speed_ceiling, closing_speed=closing_speed, required_decel=required_decel,
+      has_nearly_stopped_lead=v_lead_delay < STOP_HOLD_SPEED_FLOOR, lead_status=lead_status,
+    )
+    candidates.append(candidate)
+    departure_candidates.append((departure_distance, candidate))
+
+  if not candidates:
+    return LeadPlan(lead_status=lead_status)
+
+  selected = min(candidates, key=lambda candidate: candidate.speed_ceiling)
+  departure = min(departure_candidates, key=lambda candidate: candidate[0])[1]
+  return selected._replace(
+    departure_lead=departure.selected_lead, departure_lead_track_id=departure.selected_lead_track_id,
+    departure_lead_speed=departure.selected_lead_speed, departure_lead_raw_speed=departure.departure_lead_raw_speed,
+    departure_lead_distance=departure.departure_lead_distance,
+    departure_lead_separation=departure.departure_lead_separation,
+    departure_speed_ceiling=departure.departure_speed_ceiling,
+    has_nearly_stopped_lead=departure.has_nearly_stopped_lead,
+  )

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py

@@ -0,0 +1,386 @@
+"""
+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
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  BRAKING_ACCEL_THRESHOLD, LEAD_SAMPLE_FILTER_FRAMES, DISTANCE_JUMP_CONFIRM_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM,
+  LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, LEAD_RECOVERY_DECEL_RATE, SPEED_DEADBAND, STOP_HOLD_CREEP_DISTANCE,
+  STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_MAX_LEAD_DISTANCE, STOP_HOLD_SPEED_FLOOR, TARGET_RELEASE_SLEW,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
+
+
+def _median(samples: list[float]) -> float:
+  return sorted(samples)[len(samples) // 2]
+
+
+def _slew(current: float, target: float, rate: float, dt: float) -> float:
+  return float(np.clip(target, current - rate * dt, current + rate * dt))
+
+
+def _max_distance_step(lead_speed: float, dt: float) -> float:
+  return max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * max(lead_speed, 0.0) * dt)
+
+
+def _same_lead(first: int, first_track_id: int, second: int, second_track_id: int) -> bool:
+  if first < 0 or second < 0:
+    return False
+  if first_track_id >= 0 or second_track_id >= 0:
+    return first_track_id >= 0 and first_track_id == second_track_id
+  return first == second
+
+
+class LeadController:
+  def __init__(self) -> None:
+    self.lead_speed_samples = [math.inf] * LEAD_SAMPLE_FILTER_FRAMES
+    self.lead_accel_samples = [0.0] * LEAD_SAMPLE_FILTER_FRAMES
+
+    self.lead_speed_ceiling = math.inf
+    self.release_confirm_frames = 0
+    self.lead_loss_frames = 0
+    self._dropout_was_restricting = False
+    self._dropout_was_braking = False
+
+    self.target_speed: float | None = None
+    self.e2e_braking_handoff = False
+    self.lead_recovery = False
+    self.recovery_accel_limit: float | None = None
+
+    self.stop_hold = False
+    self.launching = False
+    self.departure_launching = False
+    self.leadless_departure = False
+    self.held_lead = -1
+    self.held_lead_track_id = -1
+    self.held_lead_trusted = False
+    self.departure_confirm_frames = 0
+    self.no_departure_lead_frames = 0
+    self.departure_distance_ref: float | None = None
+    self.last_departure_distance: float | None = None
+    self.pending_distance_jump: float | None = None
+    self.distance_jump_frames = 0
+    self.braking_for_lead = False
+    self._lead_frames = 0
+
+    self.restricting = False
+    self.releasing = False
+    self.has_lead = False
+    self.required_decel = 0.0
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.raw_speed_ceiling = math.inf
+
+  @property
+  def filtered_lead_speed(self) -> float:
+    return _median(self.lead_speed_samples)
+
+  @property
+  def filtered_lead_accel(self) -> float:
+    return _median(self.lead_accel_samples)
+
+  @property
+  def should_coast_on_dropout(self) -> bool:
+    return not self.has_lead and self._dropout_was_restricting and self._dropout_was_braking and math.isfinite(self.lead_speed_ceiling)
+
+  def reset(self) -> None:
+    self.__init__()
+
+  def _update_lead_bookkeeping(self, lead_plan: LeadPlan, was_restricting: bool) -> None:
+    self.has_lead = lead_plan.selected_lead >= 0
+    self.raw_speed_ceiling = lead_plan.speed_ceiling if self.has_lead else math.inf
+    if self.has_lead:
+      self.lead_loss_frames = 0
+      self._dropout_was_braking = False
+    else:
+      if self.lead_loss_frames == 0:
+        self._dropout_was_restricting = was_restricting
+      self.lead_loss_frames += 1
+    self.selected_lead = lead_plan.selected_lead
+    self.selected_lead_track_id = lead_plan.selected_lead_track_id if self.has_lead else -1
+
+  def _update_speed_sample(self, lead_plan: LeadPlan) -> None:
+    if not self.has_lead:
+      self.lead_speed_samples.append(math.inf)
+      self.lead_speed_samples.pop(0)
+      self.lead_accel_samples.append(0.0)
+      self.lead_accel_samples.pop(0)
+      return
+    if self.raw_speed_ceiling <= self.lead_speed_ceiling + 1e-9:
+      self.lead_speed_samples.append(lead_plan.selected_lead_speed)
+      self.lead_speed_samples.pop(0)
+      self.lead_accel_samples.append(lead_plan.selected_lead_accel)
+      self.lead_accel_samples.pop(0)
+
+  def _update_speed_ceiling(self, lead_confirm_frames: int, dropout_frames: int) -> None:
+    candidate = self.raw_speed_ceiling
+    if candidate <= self.lead_speed_ceiling:
+      self.lead_speed_ceiling = candidate
+      self.release_confirm_frames = 0
+      return
+
+    if not self.has_lead:
+      hold_frames = dropout_frames if self._dropout_was_restricting else lead_confirm_frames
+      if self.lead_loss_frames <= hold_frames:
+        return
+      self.lead_speed_ceiling = candidate
+      self.release_confirm_frames = 0
+      return
+
+    if candidate >= self.lead_speed_ceiling + SPEED_DEADBAND:
+      self.release_confirm_frames += 1
+    else:
+      self.release_confirm_frames = 0
+    if self.release_confirm_frames > lead_confirm_frames:
+      self.lead_speed_ceiling = candidate
+      self.release_confirm_frames = 0
+
+  def _guarded_distance(self, raw: float, lead_speed: float, dt: float) -> float:
+    if self.last_departure_distance is not None:
+      delta = raw - self.last_departure_distance
+      if abs(delta) > _max_distance_step(lead_speed, dt):
+        consistent = self.pending_distance_jump is not None and delta * self.pending_distance_jump > 0.0
+        self.distance_jump_frames = self.distance_jump_frames + 1 if consistent else 1
+        self.pending_distance_jump = delta
+        if self.distance_jump_frames >= DISTANCE_JUMP_CONFIRM_FRAMES:
+          self.pending_distance_jump = None
+          self.distance_jump_frames = 0
+          self.departure_confirm_frames = 0
+          if abs(delta) >= STOP_HOLD_CREEP_DISTANCE:
+            self.held_lead_trusted = False
+        else:
+          raw = self.last_departure_distance
+      else:
+        self.pending_distance_jump = None
+        self.distance_jump_frames = 0
+    self.last_departure_distance = raw
+    return raw
+
+  def _reset_distance_guard(self) -> None:
+    self.last_departure_distance = self.pending_distance_jump = None
+    self.distance_jump_frames = 0
+
+  def _reset_departure_confirmation(self) -> None:
+    self.departure_confirm_frames = 0
+    self.departure_distance_ref = None
+    self.no_departure_lead_frames = 0
+
+  def _set_held_lead(self, lead_plan: LeadPlan, replacement: bool = False) -> None:
+    self.held_lead = lead_plan.departure_lead
+    self.held_lead_track_id = lead_plan.departure_lead_track_id
+    self.held_lead_trusted = not replacement and self.held_lead_track_id >= 0
+    self._reset_distance_guard()
+    if self.held_lead >= 0:
+      self.last_departure_distance = lead_plan.departure_lead_distance
+    self._reset_departure_confirmation()
+
+  def _update_stop_hold(self, lead_plan: LeadPlan, v_ego: float, base_speed: float, dt: float, lead_confirm_frames: int) -> bool:
+    if self.stop_hold:
+      has_departure_lead = _same_lead(self.held_lead, self.held_lead_track_id,
+                                      lead_plan.departure_lead, lead_plan.departure_lead_track_id)
+      if lead_plan.departure_lead >= 0 and not has_departure_lead:
+        continuous_vision_lead = (self.held_lead_track_id < 0 and lead_plan.departure_lead_track_id < 0
+                                  and self.last_departure_distance is not None
+                                  and abs(lead_plan.departure_lead_distance - self.last_departure_distance)
+                                  <= _max_distance_step(lead_plan.departure_lead_speed, dt))
+        if continuous_vision_lead:
+          self.held_lead = lead_plan.departure_lead
+          self.held_lead_trusted = False
+        else:
+          self._set_held_lead(lead_plan, replacement=True)
+        has_departure_lead = True
+      if has_departure_lead or lead_plan.lead_status:
+        self.no_departure_lead_frames = 0
+      else:
+        self.no_departure_lead_frames += 1
+      lead_speed = lead_plan.departure_lead_speed if has_departure_lead else 0.0
+      raw_lead_speed = lead_plan.departure_lead_raw_speed if has_departure_lead else 0.0
+      evidence = has_departure_lead and min(lead_speed, raw_lead_speed) > STOP_HOLD_SPEED_FLOOR
+      distance = None
+      if has_departure_lead:
+        distance = self._guarded_distance(lead_plan.departure_lead_distance, lead_speed, dt)
+
+      if evidence:
+        if self.departure_confirm_frames == 0:
+          self.departure_distance_ref = distance
+        self.departure_confirm_frames += 1
+      else:
+        self.departure_confirm_frames = 0
+        self.departure_distance_ref = None
+        if not has_departure_lead:
+          self.held_lead_trusted = False
+          self._reset_distance_guard()
+
+      growth = 0.0
+      if self.departure_confirm_frames > 0 and self.departure_distance_ref is not None and distance is not None:
+        growth = distance - self.departure_distance_ref
+
+      dwell_ready = self.departure_confirm_frames >= STOP_HOLD_EXIT_FRAMES
+      departing_with_lead = has_departure_lead and dwell_ready and growth + 1e-9 >= STOP_HOLD_CREEP_DISTANCE
+      departing_no_lead = (not lead_plan.lead_status and self.no_departure_lead_frames >= lead_confirm_frames
+                           and base_speed > STOP_HOLD_SPEED_FLOOR)
+
+      if departing_with_lead or departing_no_lead:
+        trusted_departure = departing_with_lead and self.held_lead_trusted
+        self.stop_hold = False
+        self.launching = True
+        self.departure_launching = trusted_departure
+        self.leadless_departure = not trusted_departure
+        self.held_lead = self.held_lead_track_id = -1
+        self.held_lead_trusted = False
+        self._reset_departure_confirmation()
+        self.target_speed = min(v_ego, base_speed)
+      else:
+        self.target_speed = 0.0
+      return self.stop_hold
+
+    departure_separation = lead_plan.departure_lead_separation if lead_plan.departure_lead >= 0 else math.inf
+    stopped_lead_hold = lead_plan.has_nearly_stopped_lead and (
+      lead_plan.departure_speed_ceiling < STOP_HOLD_SPEED_FLOOR
+      or (self.braking_for_lead and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)
+    )
+    retained_stop_hold = not self.has_lead and math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR
+    if not self.launching and v_ego < STOP_HOLD_EGO_SPEED and (
+      stopped_lead_hold or retained_stop_hold
+    ):
+      self.stop_hold = True
+      self._set_held_lead(lead_plan)
+      self.launching = self.departure_launching = self.leadless_departure = False
+      self.lead_recovery = False
+      self.recovery_accel_limit = None
+      self.target_speed = 0.0
+      return True
+
+    return False
+
+  def _update_launch(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, dt: float, lead_confirm_frames: int) -> None:
+    if not self.launching:
+      return
+    if v_ego >= LAUNCH_END_SPEED:
+      self.launching = self.departure_launching = self.leadless_departure = False
+      return
+    invalid_lead = lead_plan.lead_status and not self.has_lead
+    renewed_stop = self.has_lead and lead_plan.has_nearly_stopped_lead
+    if invalid_lead or renewed_stop:
+      self.launching = self.departure_launching = self.leadless_departure = False
+      if v_ego < STOP_HOLD_EGO_SPEED:
+        self.stop_hold = True
+        self._set_held_lead(lead_plan)
+        self.target_speed = 0.0
+      return
+    self.releasing = True
+    if self.departure_launching:
+      self.target_speed = base_speed
+    elif self.leadless_departure:
+      self.target_speed = min(base_speed, max(self.target_speed or 0.0, v_ego) + TARGET_RELEASE_SLEW * dt)
+    elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames:
+      self.target_speed = min(base_speed, self.lead_speed_ceiling)
+    else:
+      launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+      self.target_speed = min(base_speed, max(self.target_speed or 0.0, launch_target) + TARGET_RELEASE_SLEW * dt)
+
+  def _update_recovery(self, ceiling: float, base_speed: float, v_ego: float, profile_max_accel: float, dt: float) -> None:
+    if math.isfinite(self.filtered_lead_speed):
+      recovery_speed = min(base_speed, self.filtered_lead_speed + LEAD_RECOVERY_HEADROOM)
+      desired_accel_limit = float(np.clip(recovery_speed - v_ego, 0.0, profile_max_accel))
+    else:
+      desired_accel_limit = 0.0
+    if self.filtered_lead_accel < BRAKING_ACCEL_THRESHOLD:
+      # Avoid stacking the recovery slew on top of a braking lead.
+      desired_accel_limit = profile_max_accel
+    if self.recovery_accel_limit is None:
+      self.recovery_accel_limit = profile_max_accel
+    self.recovery_accel_limit = _slew(self.recovery_accel_limit, desired_accel_limit, LEAD_RECOVERY_ACCEL_SLEW, dt)
+
+    if ceiling <= self.target_speed - SPEED_DEADBAND:
+      self.target_speed = max(ceiling, self.target_speed - LEAD_RECOVERY_DECEL_RATE * dt)
+      self.restricting = True
+    elif ceiling >= self.target_speed + SPEED_DEADBAND:
+      self.target_speed = min(ceiling, self.target_speed + profile_max_accel * dt)
+      self.releasing = True
+
+  def _update_target_law(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float,
+                         profile_max_accel: float, dt: float, planner_speed: float,
+                         dropout_frames: int, was_restricting: bool) -> None:
+    ceiling = min(base_speed, self.lead_speed_ceiling)
+    new_recovery = self.has_lead and lead_plan.closing_speed <= 0.0
+    still_within_dropout = not self.has_lead and self.lead_loss_frames <= dropout_frames
+    self.lead_recovery = new_recovery or (self.lead_recovery and (self.has_lead or still_within_dropout))
+    if self.lead_recovery:
+      self._update_recovery(ceiling, base_speed, v_ego, profile_max_accel, dt)
+      return
+
+    self.recovery_accel_limit = None
+    synced_to_planner = ceiling < self.target_speed and planner_speed < self.target_speed
+    if synced_to_planner:
+      self.target_speed = max(planner_speed, self.target_speed - comfort_decel * dt)
+
+    if ceiling <= self.target_speed - SPEED_DEADBAND or (was_restricting and ceiling < self.target_speed):
+      if not synced_to_planner:
+        self.target_speed = max(ceiling, self.target_speed - comfort_decel * dt)
+      self.restricting = True
+    elif ceiling >= self.target_speed + SPEED_DEADBAND:
+      self.target_speed = min(ceiling, self.target_speed + TARGET_RELEASE_SLEW * dt)
+      self.releasing = True
+
+  def update(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float, profile_max_accel: float,
+             dt: float, lead_confirm_frames: int, dropout_frames: int, planner_speed: float,
+             planner_accel: float, previous_mpc_source, previous_plan_accel: float) -> float:
+    was_restricting = self.restricting
+    was_braking_for_lead = self.braking_for_lead
+    previous_lead = self.selected_lead
+    previous_track_id = self.selected_lead_track_id
+    holding_below_cruise = (not self.lead_recovery and self.target_speed is not None and math.isfinite(self.lead_speed_ceiling)
+                            and self.lead_speed_ceiling < base_speed - SPEED_DEADBAND
+                            and self.lead_speed_ceiling - v_ego < LEAD_RECOVERY_HEADROOM)
+    self.restricting = self.releasing = False
+    self._update_lead_bookkeeping(lead_plan, was_restricting or holding_below_cruise)
+    lead_changed = not _same_lead(previous_lead, previous_track_id, self.selected_lead, self.selected_lead_track_id)
+    if not self.has_lead or lead_changed:
+      self._lead_frames = 0
+    if self.braking_for_lead and lead_changed:
+      self.braking_for_lead = False
+    if not self.has_lead and self.lead_loss_frames == 1:
+      self._dropout_was_braking = was_braking_for_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD
+    self._update_speed_ceiling(lead_confirm_frames, dropout_frames)
+    self._update_speed_sample(lead_plan)
+    self.required_decel = lead_plan.required_decel
+
+    self._lead_frames += int(self.has_lead)
+    if (self._lead_frames >= lead_confirm_frames and math.isfinite(self.lead_speed_ceiling)
+        and self.has_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD):
+      self.braking_for_lead = True
+    elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames:
+      self.braking_for_lead = False
+
+    if self.target_speed is None:
+      self.target_speed = min(base_speed, v_ego)
+      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
+      self.e2e_braking_handoff = e2e_handoff and math.isfinite(previous_plan_accel) and previous_plan_accel <= BRAKING_ACCEL_THRESHOLD
+      stop_hold_reason = lead_plan.has_nearly_stopped_lead or (math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR)
+      if v_ego < STOP_HOLD_EGO_SPEED and not stop_hold_reason:
+        self.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+        self.launching = True
+        self.departure_launching = False
+    elif self.e2e_braking_handoff and planner_accel > BRAKING_ACCEL_THRESHOLD:
+      self.e2e_braking_handoff = False
+
+    self.target_speed = min(self.target_speed, base_speed)
+
+    if self._update_stop_hold(lead_plan, v_ego, base_speed, dt, lead_confirm_frames):
+      return self.target_speed
+
+    self._update_launch(lead_plan, base_speed, v_ego, dt, lead_confirm_frames)
+    if self.launching or self.stop_hold:
+      return self.target_speed
+
+    self._update_target_law(lead_plan, base_speed, v_ego, comfort_decel, profile_max_accel, dt, planner_speed,
+                            dropout_frames, was_restricting)
+    return self.target_speed

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -0,0 +1,789 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.cereal import log
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
+  STOP_DISTANCE, T_IDXS, LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL,
+  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, MPC_DECEL_JERK_COST_MULTIPLIER,
+  STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile, profile_accel_max, sanitize_profile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan
+
+
+def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
+  return SimpleNamespace(present=status, dRel=d_rel, vLead=v_lead_k, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau,
+                         radarTrackId=radar_track_id)
+
+
+def make_radar(lead_one=None, lead_two=None):
+  return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead())
+
+
+def make_controller(delay=0.10):
+  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True))
+
+
+def get_lead_plan(controller, radar_state, v_ego: float, a_ego: float, profile: int):
+  return calculate_lead_plan(radar_state, v_ego, a_ego, controller.delay, profile)
+
+
+def update(controller, radar_state=None, **overrides):
+  args = {
+    "base_speed": 25.0,
+    "v_ego": 10.0,
+    "a_ego": 0.0,
+    "profile": AccelProfile.normal,
+    "follow_personality": log.LongitudinalPersonality.standard,
+    "enabled": True,
+    "acc_selected": True,
+    "engaged": True,
+    "cruise_initialized": True,
+    "stock_accel_max": ACCEL_MAX,
+  }
+  args.update(overrides)
+  controller.profile = args.pop("profile")
+  controller.enabled = args.pop("enabled")
+  controller.update(radar_state or make_radar(), **args)
+  lead_controller = controller.lead_controller
+  return SimpleNamespace(
+    target_speed=controller.output_v_target, active=controller.is_active, launching=lead_controller.launching,
+    departure_launching=lead_controller.departure_launching, mpc_accel_max=controller.mpc_accel_max,
+    cruise_accel_max=controller.cruise_accel_max, state=controller.state, selected_lead=controller.selected_lead,
+    required_decel=controller.required_decel, stop_hold=lead_controller.stop_hold, lead_recovery=lead_controller.lead_recovery,
+    recovery_accel_limit=lead_controller.recovery_accel_limit, lead_speed_ceiling=lead_controller.lead_speed_ceiling, restricting=lead_controller.restricting,
+    releasing=lead_controller.releasing,
+  )
+
+
+def effective_accel_max(result):
+  return math.inf if result.mpc_accel_max is None else min(result.mpc_accel_max)
+
+
+def restrictive_radar():
+  return make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
+
+
+def enter_stop_hold(controller, *, base_speed=8.0, v_ego=0.0, frames=6, track_id=-1):
+  stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=track_id))
+  result = None
+  for _ in range(frames):
+    result = update(controller, stopped, base_speed=base_speed, v_ego=v_ego)
+  assert result.stop_hold
+  return result
+
+
+class TestProfiles:
+  @pytest.mark.parametrize("profile", ACCEL_PROFILES)
+  def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
+    for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
+      assert profile_accel_max(profile, speed) == expected
+
+    limits = [profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)]
+    assert all(0.0 <= limit <= ACCEL_MAX for limit in limits)
+    assert np.all(np.diff(limits) <= 0.0)
+
+  @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
+  def test_profile_order_is_distinct(self, speed):
+    eco, normal, sport = (profile_accel_max(profile, speed) for profile in ACCEL_PROFILES)
+    assert eco < normal < sport
+
+  def test_invalid_profile_defaults_to_normal(self):
+    assert sanitize_profile(999) == AccelProfile.normal
+
+  def test_stock_limit_intersects_profile_before_mpc(self):
+    controller = make_controller()
+    results = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
+               for _ in range(controller.lead_confirm_frames)]
+    result = results[-1]
+    assert profile_accel_max(AccelProfile.sport, 10.0) > 0.30
+    assert effective_accel_max(result) == pytest.approx(0.30)
+    assert all(sample.mpc_accel_max is not None for sample in results)
+    assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results)
+
+  def test_runtime_profile_switch_applies_the_lookup_value_directly(self):
+    controller = make_controller()
+    sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
+             for _ in range(controller.lead_confirm_frames)][-1]
+    eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
+
+    assert effective_accel_max(sport) == pytest.approx(profile_accel_max(AccelProfile.sport, 10.0))
+    assert effective_accel_max(eco) == pytest.approx(profile_accel_max(AccelProfile.eco, 10.0))
+
+  def test_stock_limit_reduction_applies_immediately(self):
+    controller = make_controller()
+    for _ in range(controller.lead_confirm_frames):
+      update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
+
+    reduced = update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
+    assert effective_accel_max(reduced) == pytest.approx(0.30)
+    assert reduced.mpc_accel_max is not None
+    assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9
+
+  def test_one_frame_stock_zero_does_not_poison_profile_recovery(self):
+    clean_controller, glitch_controller = make_controller(), make_controller()
+    for _ in range(clean_controller.lead_confirm_frames + 10):
+      clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
+      update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
+
+    limited = update(glitch_controller, v_ego=10.0, stock_accel_max=0.0)
+    clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
+    recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
+
+    assert effective_accel_max(limited) == 0.0
+    assert effective_accel_max(recovered) == pytest.approx(effective_accel_max(clean))
+
+  @pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
+  def test_lead_recovery_ceiling_obeys_current_stock_limit(self, radar_fresh):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
+    for _ in range(controller.lead_confirm_frames + 10):
+      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
+    for _ in range(20):
+      update(controller, radar, v_ego=8.0, planner_accel=-0.2)
+    assert controller.lead_controller.lead_recovery
+
+    limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh)
+    assert effective_accel_max(limited) == 0.0
+    assert limited.mpc_accel_max is not None
+    assert max(limited.mpc_accel_max) == 0.0
+
+  def test_exact_global_max_uses_stock_ceiling(self):
+    result = update(make_controller(), base_speed=8.0, v_ego=0.0, profile=AccelProfile.sport)
+    assert profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX
+    assert result.mpc_accel_max is None
+
+
+class TestBuildAccelCeiling:
+  @pytest.mark.parametrize("planner_accel", (-1.0, 0.0, 1.2, ACCEL_MAX))
+  def test_ceiling_is_finite_feasible_and_jerk_bounded(self, planner_accel):
+    limit = 0.50
+    ceiling = np.asarray(build_accel_ceiling(limit, planner_accel))
+    a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
+
+    assert ceiling.shape == T_IDXS.shape
+    assert np.all(np.isfinite(ceiling))
+    assert np.all((0.0 <= ceiling) & (ceiling <= ACCEL_MAX))
+    assert ceiling[0] + 1e-9 >= a0
+    assert np.all(ceiling + 1e-9 >= limit)
+    assert np.all(np.diff(ceiling) <= 1e-9)
+    assert np.all(-np.diff(ceiling) <= ACCEL_LIMIT_HORIZON_JERK * np.diff(T_IDXS) + 1e-9)
+
+  def test_zero_limit_remains_feasible_for_positive_x0(self):
+    ceiling = np.asarray(build_accel_ceiling(0.0, 0.8))
+    assert ceiling[0] == pytest.approx(0.8)
+    assert ceiling[-1] == pytest.approx(0.0)
+    assert np.all(ceiling >= 0.0)
+
+
+class TestMpcCeilingIntegration:
+  def test_inactive_controller_has_no_custom_ceiling(self):
+    controller = make_controller()
+    result = update(controller, enabled=False)
+    assert not result.active
+    assert result.mpc_accel_max is None
+    assert math.isinf(effective_accel_max(result))
+    assert controller.lead_controller.target_speed is None
+
+  def test_closing_on_a_lead_has_no_ceiling_regardless_of_planner_accel_sign(self):
+    # Leave the lead MPC unconstrained while ego is closing.
+    controller = make_controller()
+    radar = restrictive_radar()
+    for planner_accel in (-0.2, 0.2, -0.2):
+      result = update(controller, radar, v_ego=10.0, planner_accel=planner_accel)
+      assert result.mpc_accel_max is None
+    assert not controller.lead_controller.lead_recovery
+
+    bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
+    assert not bypassed.active and bypassed.mpc_accel_max is None
+
+  def test_eco_cruise_limit_remains_active_while_closing_on_a_lead(self):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=80.0, v_lead_k=19.25))
+    result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco, planner_accel=0.16,
+                    previous_mpc_source=LongitudinalPlanSource.cruise)
+
+    assert result.state == AccelControllerState.free
+    assert result.mpc_accel_max is None
+    assert result.cruise_accel_max == pytest.approx(profile_accel_max(AccelProfile.eco, 20.0))
+
+  def test_lead_cruise_limit_does_not_follow_mpc_source_or_accel_sign(self):
+    controller = make_controller()
+    expected = profile_accel_max(AccelProfile.eco, 20.0)
+    inputs = (
+      (LongitudinalPlanSource.cruise, 0.02, 19.9, 100),
+      (LongitudinalPlanSource.lead0, -0.02, 20.1, -1),
+      (LongitudinalPlanSource.cruise, -0.10, 19.9, 101),
+      (LongitudinalPlanSource.lead0, -0.12, 20.1, -1),
+      (LongitudinalPlanSource.lead1, 0.02, 20.1, -1),
+    )
+
+    for source, planner_accel, lead_speed, track_id in inputs:
+      radar = make_radar(make_lead(status=True, d_rel=150.0, v_lead_k=lead_speed, radar_track_id=track_id))
+      result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco,
+                      planner_accel=planner_accel, previous_mpc_source=source)
+      assert result.cruise_accel_max == pytest.approx(expected)
+
+  def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
+    controller = make_controller()
+    for _ in range(controller.lead_confirm_frames):
+      update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2)
+
+    pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0))
+    result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2)
+
+    assert result.state == AccelControllerState.restrict
+    assert result.lead_recovery
+    assert result.mpc_accel_max is not None
+    profile_ceiling = profile_accel_max(AccelProfile.normal, 10.0)
+    assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL - 1e-9 <= effective_accel_max(result) <= profile_ceiling + 1e-9
+
+
+class TestLead:
+  def test_speed_ceiling_matches_stopping_energy_formula(self):
+    controller = make_controller()
+    lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
+    result = get_lead_plan(controller, make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    delay = controller.delay
+    lead_xv = LongitudinalMpc.extrapolate_lead(lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)
+    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
+    v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
+    x_ego, _ = _project_ego(10.0, 0.0, delay)
+    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
+    usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0)
+    expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * usable_gap)
+
+    assert result.speed_ceiling == pytest.approx(expected)
+
+  def test_profile_order_controls_approach_timing(self):
+    radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
+    ceilings = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).speed_ceiling for profile in ACCEL_PROFILES]
+    assert ceilings[0] < ceilings[1] < ceilings[2]
+
+  @pytest.mark.parametrize("v_lead_k", (0.0, 8.0), ids=("stopped", "moving"))
+  def test_reserve_is_flat_not_speed_or_decel_scaled(self, v_lead_k):
+    lead = get_lead_plan(make_controller(), make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=v_lead_k)),
+                         5.0, 0.0, AccelProfile.normal)
+    comfort_decel = COMFORT_DECEL[AccelProfile.normal]
+    safety_gap = (lead.departure_speed_ceiling - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
+    usable_gap = (lead.speed_ceiling - lead.selected_lead_speed) ** 2 / (2.0 * comfort_decel)
+    assert safety_gap - usable_gap == pytest.approx(STOP_GAP_RESERVE)
+    assert lead.departure_speed_ceiling > lead.speed_ceiling
+
+  def test_more_restrictive_lead_is_selected(self):
+    radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
+    assert get_lead_plan(make_controller(), radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
+
+  def test_departure_lead_prefers_nearer_lead_over_speed_governing_lead(self):
+    radar = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100),
+                       make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
+    result = get_lead_plan(make_controller(), radar, 0.0, 0.0, AccelProfile.normal)
+    assert result.selected_lead == 1
+    assert result.departure_lead == 0
+
+  @pytest.mark.parametrize("field,value", [
+    ("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan),
+  ])
+  def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
+    setattr(lead, field, value)
+    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert result.selected_lead == 0
+    assert math.isfinite(result.speed_ceiling)
+
+  @pytest.mark.parametrize("field,value", [("dRel", math.nan), ("dRel", -1.0), ("vLeadK", math.nan), ("vLeadK", -2.0)])
+  def test_invalid_geometry_is_not_used(self, field, value):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
+    setattr(lead, field, value)
+    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert result.selected_lead == -1
+    assert result.lead_status
+    assert math.isinf(result.speed_ceiling)
+
+  def test_raw_radar_is_never_mutated(self):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan)
+    before = vars(lead).copy()
+    get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert vars(lead) == before
+
+
+class TestTargetAndSpeedCeiling:
+  def test_lead_recovery_accel_limit_ignores_a_two_frame_speed_jump(self):
+    clean_controller, noisy_controller = make_controller(), make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
+    for controller in (clean_controller, noisy_controller):
+      for _ in range(controller.lead_confirm_frames + 10):
+        update(controller, radar, v_ego=10.0, planner_accel=-0.2)
+      for _ in range(20):
+        update(controller, radar, v_ego=8.0, planner_accel=-0.2)
+
+    speed_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=16.0))
+    for _ in range(2):
+      clean = update(clean_controller, radar, v_ego=8.0)
+      noisy = update(noisy_controller, speed_jump, v_ego=8.0)
+      assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
+      assert noisy.target_speed == pytest.approx(clean.target_speed)
+  def test_restriction_target_speed_is_rate_limited(self):
+    controller = make_controller()
+    targets = [update(controller, restrictive_radar()).target_speed for _ in range(15)]
+    max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
+
+    steps = -np.diff(targets[1:])
+    assert np.all(steps <= max_step + 1e-9)
+    assert controller.state == AccelControllerState.restrict
+    assert targets[-1] < targets[1]
+
+  @pytest.mark.parametrize("previous_mpc_source", (None, LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0))
+  def test_target_speed_syncs_down_to_planner_speed_regardless_of_previous_mpc_source(self, previous_mpc_source):
+    controller = make_controller()
+    for _ in range(15):
+      restricted = update(controller, restrictive_radar())
+    planner_speed = restricted.target_speed - 2.0
+
+    synced = update(controller, restrictive_radar(), previous_mpc_source=previous_mpc_source, planner_speed=planner_speed,
+                    planner_accel=-0.2)
+    assert synced.target_speed == pytest.approx(restricted.target_speed - COMFORT_DECEL[AccelProfile.normal] * DT_MDL)
+
+  def test_short_dropout_holds_then_releases_at_a_bounded_rate(self):
+    controller = make_controller()
+    for _ in range(15):
+      restricted = update(controller, restrictive_radar())
+
+    held = [update(controller) for _ in range(controller.dropout_frames - 1)]
+    assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
+
+    released = [update(controller) for _ in range(60)]
+    targets = [restricted.target_speed, *(result.target_speed for result in released)]
+    assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
+    assert released[-1].target_speed >= 25.0 - 0.15 - 1e-9
+    assert released[-1].state == AccelControllerState.free
+
+  def test_restricting_lead_dropout_coasts_before_release(self):
+    controller = make_controller()
+    for _ in range(15):
+      update(controller, restrictive_radar(), planner_accel=-0.5)
+
+    coast = update(controller, planner_accel=-0.5)
+
+    assert coast.cruise_accel_max == 0.0
+    assert coast.mpc_accel_max is not None
+    assert effective_accel_max(coast) == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0))
+
+    ceilings = [coast.cruise_accel_max]
+    ceilings.extend(update(controller, planner_accel=-0.5).cruise_accel_max for _ in range(controller.dropout_frames - 1))
+    assert ceilings == sorted(ceilings)
+    assert ceilings[-1] == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0))
+
+  def test_should_coast_on_dropout_lifecycle(self):
+    controller = make_controller()
+    for _ in range(15):
+      update(controller, restrictive_radar(), planner_accel=-0.5)
+
+    update(controller, planner_accel=-0.5)
+    assert controller.lead_controller.should_coast_on_dropout
+
+    update(controller, restrictive_radar(), planner_accel=0.2)
+    assert not controller.lead_controller.should_coast_on_dropout
+    update(controller, planner_accel=0.2)
+    assert not controller.lead_controller.should_coast_on_dropout
+
+    controller.reset()
+    assert not controller.lead_controller.should_coast_on_dropout
+
+  def test_e2e_braking_handoff_clears_when_braking_ends(self):
+    controller = make_controller()
+    update(controller, previous_mpc_source=LongitudinalPlanSource.e2e, previous_plan_accel=-1.0, planner_accel=-0.5)
+    assert controller.lead_controller.e2e_braking_handoff
+
+    still_braking = update(controller, planner_accel=-0.12)
+    assert controller.lead_controller.e2e_braking_handoff
+    assert still_braking.mpc_accel_max is None
+
+    coasting = update(controller, planner_accel=-0.10)
+    assert not controller.lead_controller.e2e_braking_handoff
+    assert coasting.mpc_accel_max is not None
+
+
+class TestMatchedLead:
+  def test_recovery_accel_limit_unthrottled_when_ego_well_below_lead_speed(self):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
+    for _ in range(20):
+      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
+    result = update(controller, radar, v_ego=3.0, planner_accel=-0.2)
+
+    assert result.lead_recovery
+    assert result.recovery_accel_limit == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0))
+    assert effective_accel_max(result) == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0))
+
+  def test_recovery_accel_limit_throttles_toward_recovery_headroom_when_near_lead_speed(self):
+    controller = make_controller()
+    slow_radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=3.0))
+    for _ in range(20):
+      update(controller, slow_radar, v_ego=5.0, planner_accel=-0.2)
+    for _ in range(40):
+      result = update(controller, slow_radar, v_ego=3.0, planner_accel=-0.2)
+
+    assert result.lead_recovery
+    assert result.recovery_accel_limit == pytest.approx(LEAD_RECOVERY_HEADROOM)
+    assert result.recovery_accel_limit < profile_accel_max(AccelProfile.normal, 3.0)
+
+  def test_recovery_accel_limit_slew_bounded_and_independent_of_planner_accel_sign(self):
+    braking_controller, accelerating_controller = make_controller(), make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
+    for controller in (braking_controller, accelerating_controller):
+      for _ in range(20):
+        update(controller, radar, v_ego=10.0, planner_accel=-0.2)
+      for _ in range(20):
+        update(controller, radar, v_ego=8.0, planner_accel=-0.2)
+    before = braking_controller.lead_controller.recovery_accel_limit
+    assert accelerating_controller.lead_controller.recovery_accel_limit == pytest.approx(before)
+
+    braking = update(braking_controller, radar, v_ego=8.0, planner_accel=-0.2)
+    accelerating = update(accelerating_controller, radar, v_ego=8.0, planner_accel=0.2)
+
+    assert abs(braking.recovery_accel_limit - before) <= LEAD_RECOVERY_ACCEL_SLEW * DT_MDL + 1e-9
+    assert accelerating.recovery_accel_limit == pytest.approx(braking.recovery_accel_limit)
+
+
+class TestLaunchAndDeparture:
+  def test_clear_road_launch_has_immediate_headroom_and_bounded_target_slew(self):
+    controller = make_controller()
+    initial = update(controller, base_speed=12.0, v_ego=0.0, profile=AccelProfile.normal)
+    rolling = update(controller, base_speed=12.0, v_ego=0.31, profile=AccelProfile.normal)
+
+    assert initial.active and initial.launching
+    assert LAUNCH_TARGET_HEADROOM <= initial.target_speed <= LAUNCH_TARGET_HEADROOM + TARGET_RELEASE_SLEW * DT_MDL
+    assert rolling.launching
+    assert rolling.target_speed >= 0.31 + LAUNCH_TARGET_HEADROOM
+    assert rolling.target_speed - max(initial.target_speed, 0.31 + LAUNCH_TARGET_HEADROOM) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
+
+    finished = update(controller, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.normal)
+    assert not finished.launching
+
+  def test_departure_launch_keeps_profile_ceiling_through_lead_recovery_transition(self):
+    controller = make_controller()
+    enter_stop_hold(controller, base_speed=12.0, track_id=100)
+    for frame in range(STOP_HOLD_EXIT_FRAMES):
+      departing = make_radar(make_lead(status=True, d_rel=6.5 + 0.1 * frame, v_lead_k=5.0, radar_track_id=100))
+      launching = update(controller, departing, base_speed=12.0, v_ego=0.1, profile=AccelProfile.eco, planner_accel=1.4)
+
+    exited = update(controller, departing, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.eco, planner_accel=1.4)
+
+    assert launching.departure_launching and effective_accel_max(launching) == pytest.approx(profile_accel_max(AccelProfile.eco, 0.1))
+    profile_ceiling = profile_accel_max(AccelProfile.eco, LAUNCH_END_SPEED)
+    assert exited.lead_recovery
+    assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL <= effective_accel_max(exited) <= profile_ceiling
+
+  def test_e2e_braking_handoff_arms_only_on_seed_frame_from_previous_plan_accel(self):
+    armed = make_controller()
+    update(armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e,
+          previous_plan_accel=-1.0, planner_accel=0.5)
+    assert armed.lead_controller.e2e_braking_handoff
+
+    not_armed = make_controller()
+    update(not_armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e,
+          previous_plan_accel=0.5, planner_accel=-1.0)
+    assert not not_armed.lead_controller.e2e_braking_handoff
+
+  def test_stop_hold_needs_four_confirmed_departure_frames_with_real_radar(self):
+    controller = make_controller()
+    held = enter_stop_hold(controller, track_id=100)
+    assert held.stop_hold and held.target_speed == 0.0 and held.mpc_accel_max is None
+
+    results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)),
+                      base_speed=8.0, v_ego=0.1) for frame in range(STOP_HOLD_EXIT_FRAMES + 4)]
+    launch_index = next(index for index, result in enumerate(results) if result.launching)
+
+    assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
+    assert all(result.stop_hold and not result.launching for result in results[:launch_index])
+    assert results[launch_index].departure_launching
+    assert results[launch_index].target_speed == 8.0
+
+  def test_renewed_stop_mid_launch_aborts_back_to_stop_hold(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
+      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
+      update(controller, moving, base_speed=8.0, v_ego=0.1)
+    assert controller.lead_controller.launching and controller.lead_controller.departure_launching
+
+    renewed_stop_lead = make_radar(make_lead(status=True, d_rel=6.5, v_lead_k=0.05, radar_track_id=100))
+    result = update(controller, renewed_stop_lead, base_speed=8.0, v_ego=0.1)
+    assert result.stop_hold
+    assert result.target_speed == 0.0
+    assert not result.launching
+
+  def test_invalid_lead_mid_launch_aborts_launch_without_reentering_stop_hold(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
+      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
+      update(controller, moving, base_speed=8.0, v_ego=0.5)
+    assert controller.lead_controller.launching
+
+    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
+    result = update(controller, invalid, base_speed=8.0, v_ego=0.5)
+    assert not result.stop_hold
+    assert not result.launching
+    assert result.active
+
+  def test_genuine_departure_survives_lead_slot_and_track_flicker(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    results = []
+    for frame in range(STOP_HOLD_EXIT_FRAMES + 4):
+      moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)
+      secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200)
+      radar = make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving)
+      results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
+
+    launch_index = next(index for index, result in enumerate(results) if result.launching)
+    assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
+    assert all(result.stop_hold for result in results[:launch_index])
+    assert results[-1].launching and results[-1].departure_launching
+
+  def test_confirmed_creep_departure_departs_within_budget(self):
+    controller = make_controller()
+    enter_stop_hold(controller)
+    results = []
+    for frame in range(60):
+      creeping = make_radar(make_lead(status=True, d_rel=6.0 + frame * 0.01, v_lead_k=0.2))
+      results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0))
+
+    launch_index = next(index for index, result in enumerate(results) if result.launching)
+    assert launch_index * DT_MDL <= 2.0
+    assert all(not result.stop_hold for result in results[launch_index:])
+
+  def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
+    controller = make_controller()
+    enter_stop_hold(controller)
+    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
+      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
+      update(controller, moving, base_speed=8.0, v_ego=0.1)
+    assert controller.lead_controller.launching
+
+    dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_confirm_frames + 5)]
+    assert all(not result.stop_hold for result in dropout)
+    assert all(result.launching for result in dropout)
+
+  def test_departure_launch_expires_after_bounded_radar_staleness(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
+      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
+      update(controller, moving, base_speed=8.0, v_ego=0.1)
+    assert controller.lead_controller.departure_launching
+
+    held = [update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False)
+            for _ in range(controller.dropout_frames - 1)]
+    expired = update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False)
+
+    assert all(result.active and result.departure_launching for result in held)
+    assert not expired.active
+    assert not controller.lead_controller.departure_launching
+    assert controller.update_should_stop(True)
+
+  def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self):
+    controller = make_controller()
+    enter_stop_hold(controller)
+    missing = update(controller, base_speed=8.0, v_ego=0.1)
+
+    assert missing.stop_hold
+    assert missing.target_speed == 0.0
+    assert missing.mpc_accel_max is None
+
+  def test_leadless_departure_does_not_override_should_stop(self):
+    controller = make_controller()
+    enter_stop_hold(controller)
+    for _ in range(controller.lead_confirm_frames):
+      result = update(controller, base_speed=8.0, v_ego=0.0)
+
+    assert result.launching
+    assert not result.departure_launching
+    assert controller.update_should_stop(True)
+
+  def test_invalid_lead_does_not_release_stop_hold(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0, radar_track_id=100))
+
+    results = [update(controller, invalid, base_speed=8.0, v_ego=0.31) for _ in range(controller.lead_confirm_frames + 2)]
+    first_absent = update(controller, base_speed=8.0, v_ego=0.31)
+
+    assert all(result.stop_hold and result.target_speed == 0.0 for result in results)
+    assert first_absent.stop_hold and first_absent.target_speed == 0.0
+    assert controller.update_should_stop(False)
+
+  def test_vision_lead_departure_does_not_override_should_stop(self):
+    controller = make_controller()
+    enter_stop_hold(controller)
+    for frame in range(STOP_HOLD_EXIT_FRAMES):
+      moving = make_radar(make_lead(status=True, d_rel=6.0 + 0.1 * frame, v_lead_k=2.0))
+      result = update(controller, moving, base_speed=8.0, v_ego=0.0)
+
+    assert result.launching
+    assert not result.departure_launching
+    assert controller.update_should_stop(True)
+
+  def test_replacement_lead_cannot_claim_a_confirmed_departure(self):
+    controller = make_controller()
+    stopped = make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)
+    moving = make_lead(status=True, d_rel=20.0, v_lead_k=2.0, radar_track_id=200)
+    for _ in range(6):
+      held = update(controller, make_radar(stopped, moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
+    assert held.stop_hold
+
+    for _ in range(STOP_HOLD_EXIT_FRAMES):
+      moving.dRel += 0.1
+      result = update(controller, make_radar(lead_two=moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
+
+    assert result.launching
+    assert not result.departure_launching
+    assert controller.update_should_stop(True)
+
+  def test_moving_departure_does_not_reenter_stop_hold_once_launching(self):
+    controller = make_controller()
+    enter_stop_hold(controller, track_id=100)
+    distance = 6.0
+    results = []
+    for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72, 0.70):
+      distance += speed * DT_MDL
+      radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100))
+      results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
+
+    launch_index = next(index for index, result in enumerate(results) if result.launching)
+    assert all(not result.stop_hold for result in results[launch_index:])
+    assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:])
+
+  def test_far_stopped_lead_should_not_create_stop_hold(self):
+    controller = make_controller()
+    far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
+    results = [update(controller, far_stopped, base_speed=12.0, v_ego=0.0) for _ in range(4)]
+    assert all(not result.stop_hold for result in results)
+
+  def test_fast_speed_glitch_without_distance_progress_should_stay_in_stop_hold(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
+    update(controller, stopped, base_speed=8.0, v_ego=0.0)
+    glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100))
+    results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES + 2)]
+
+    assert all(result.stop_hold and result.target_speed == 0.0 and not result.launching for result in results)
+
+
+class TestFreshnessAndReset:
+  def test_frozen_output_during_a_single_non_fresh_radar_frame(self):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
+    for _ in range(15):
+      fresh = update(controller, radar, v_ego=10.0, planner_accel=-0.2)
+
+    held = update(controller, radar, v_ego=10.0, planner_accel=-0.2, radar_fresh=False)
+    assert held.target_speed == pytest.approx(fresh.target_speed)
+    assert held.state == fresh.state
+    assert held.selected_lead == fresh.selected_lead
+    assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh))
+
+  def test_stale_timeout_fully_resets_live_state(self):
+    controller = make_controller()
+    radar = restrictive_radar()
+    for _ in range(15):
+      update(controller, radar)
+    held = [update(controller, radar_fresh=False) for _ in range(controller.radar_stale_frames - 1)]
+    timed_out = update(controller, radar_fresh=False)
+
+    assert all(result.active for result in held)
+    assert not timed_out.active
+    assert timed_out.target_speed == 25.0
+    assert timed_out.mpc_accel_max is None
+    assert timed_out.selected_lead == -1
+    assert controller.lead_controller.target_speed is None
+
+  def test_acc_bypass_starts_a_new_stale_episode(self):
+    controller = make_controller()
+    radar = restrictive_radar()
+    update(controller, radar)
+    for _ in range(controller.radar_stale_frames - 1):
+      update(controller, radar, radar_fresh=False)
+
+    bypassed = update(controller, radar, acc_selected=False, radar_fresh=False)
+    handoff = update(controller, radar, radar_fresh=False, previous_mpc_source=LongitudinalPlanSource.e2e,
+                     previous_plan_accel=-0.5, planner_accel=0.5)
+
+    assert not bypassed.active
+    assert handoff.active
+    assert controller.lead_controller.e2e_braking_handoff
+
+  @pytest.mark.parametrize("override", [{"enabled": False}, {"acc_selected": False}, {"engaged": False},
+                                        {"cruise_initialized": False}, {"a_ego": math.inf}])
+  def test_bypass_or_invalid_context_resets_live_state(self, override):
+    controller = make_controller()
+    for _ in range(15):
+      update(controller, restrictive_radar())
+    result = update(controller, restrictive_radar(), **override)
+
+    assert not result.active
+    assert result.target_speed == 25.0
+    assert result.mpc_accel_max is None
+    assert controller.lead_controller.target_speed is None
+
+  def test_acc_bypass_does_not_retain_state_for_live_actuation(self):
+    controller = make_controller()
+    for _ in range(20):
+      bypassed = update(controller, restrictive_radar(), acc_selected=False)
+      assert not bypassed.active
+      assert controller.lead_controller.target_speed is None
+    live = update(controller)
+
+    assert live.active
+    assert 10.0 < live.target_speed <= 10.0 + TARGET_RELEASE_SLEW * DT_MDL + 1e-9
+
+  def test_explicit_reset_clears_lead_state(self):
+    controller = make_controller()
+    for _ in range(15):
+      update(controller, restrictive_radar())
+    controller._jerk_smoothing_blocked = True
+    controller._required_decel_samples = [0.2]
+    controller._required_decel_lead = controller._required_decel_lead_track_id = 1
+    controller._lead_trend_warmup = True
+    controller.reset()
+
+    assert not controller._jerk_smoothing_blocked
+    assert controller._required_decel_samples == []
+    assert controller._required_decel_lead == controller._required_decel_lead_track_id == -1
+    assert not controller._lead_trend_warmup
+    lead_controller = controller.lead_controller
+    assert lead_controller.target_speed is None and lead_controller.recovery_accel_limit is None
+    assert not lead_controller.stop_hold and not lead_controller.launching and not lead_controller.lead_recovery
+    assert math.isinf(lead_controller.lead_speed_ceiling) and math.isinf(lead_controller.filtered_lead_speed)
+    assert lead_controller.lead_speed_samples == [math.inf] * LEAD_SAMPLE_FILTER_FRAMES
+    assert controller.state == AccelControllerState.inactive
+    assert controller.selected_lead == controller.selected_lead_track_id == -1
+
+
+class TestJerkCostMultiplier:
+  @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
+  def test_track_id_change_requires_new_history_before_jerk_smoothing(self, replacement_track_id):
+    controller = make_controller()
+    controller.state = AccelControllerState.restrict
+    controller.selected_lead = 0
+    controller.selected_lead_track_id = 100
+    controller.required_decel = 0.2
+    original = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
+
+    controller.selected_lead_track_id = replacement_track_id
+    replacement = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
+
+    assert original == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
+    assert replacement == [1.0, 1.0, 1.0, MPC_DECEL_JERK_COST_MULTIPLIER]
+    assert controller._required_decel_samples == [0.2] * 4

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py

@@ -0,0 +1,613 @@
+import inspect
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.cereal import custom, log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def radar_state():
+  return messaging.new_message("radarState").radarState
+
+
+class PlannerSM(dict):
+  def __init__(self, radar_log_mono_time: int):
+    super().__init__(
+      radarState=radar_state(),
+      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0),
+      selfdriveState=SimpleNamespace(personality=0),
+      controlsState=SimpleNamespace(forceDecel=False),
+    )
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+    self.logMonoTime = {"radarState": radar_log_mono_time}
+
+
+class ControllerStub:
+  def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, cruise_accel_max=None,
+               state=AccelControllerState.free, selected_lead=-1,
+               selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0):
+    self.available = self.enabled = True
+    self.profile = AccelProfile.normal
+    self.output_v_target = target_speed
+    self.is_active = active
+    self.mpc_accel_max = mpc_accel_max
+    self.cruise_accel_max = cruise_accel_max
+    self.state = state
+    self.selected_lead = selected_lead
+    self.selected_lead_track_id = selected_lead_track_id
+    self.launching = launching
+    self.departure_launching = departure_launching
+    self.lead_controller = SimpleNamespace(departure_launching=departure_launching, stop_hold=state == AccelControllerState.stopHold)
+    self.required_decel = required_decel
+    self.dt = DT_MDL
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples = []
+    self._required_decel_long_samples = []
+    self._required_decel_lead = -1
+    self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+    self.update_kwargs = None
+    self.reset_calls = 0
+
+  def update(self, _radar_state, **kwargs):
+    self.update_kwargs = kwargs
+
+  @property
+  def is_enabled(self):
+    return self.available and self.enabled
+
+  def update_params(self):
+    pass
+
+  def reset(self):
+    self.reset_calls += 1
+
+  def get_jerk_cost_multiplier(self, *args):
+    return AccelController.get_jerk_cost_multiplier(self, *args)
+
+  def update_should_stop(self, should_stop, departure_authorized=True):
+    return AccelController.update_should_stop(self, should_stop, departure_authorized)
+
+
+def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None,
+                         cruise_accel_max=None,
+                         state=AccelControllerState.free, selected_lead=-1, launching=False,
+                         departure_launching=False, required_decel=0.0,
+                         mpc_source=MpcLongitudinalPlanSource.lead0):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  is_e2e_calls = []
+  planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
+  planner.output_v_target = 20.0
+  planner.output_should_stop = False
+  planner._long_active_last_cycle = True
+  planner.previous_plan_accel = 0.0
+  planner.mpc_accel_seed = 0.0
+  planner.allow_throttle = True
+  planner.a_desired = 0.0
+  planner.v_desired_filter = SimpleNamespace(x=10.0)
+  planner._radar_fresh_this_cycle = True
+  planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
+  planner.accel_controller = ControllerStub(
+    target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching,
+    departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max,
+    cruise_accel_max=cruise_accel_max,
+  )
+  return planner, is_e2e_calls
+
+
+def prepare_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False, stock_accel_max=ACCEL_MAX):
+  configs = []
+  sm = {
+    "radarState": radar_state(),
+    "controlsState": SimpleNamespace(forceDecel=force_decel),
+    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
+    "selfdriveState": SimpleNamespace(personality=0),
+  }
+  planner.mpc.set_accel_controller_params = lambda *args: configs.append(args)
+  is_e2e, target = planner.update_accel_controller(sm, mpc_v_cruise, True, stock_accel_max, False)
+  assert len(configs) == 1
+  return is_e2e, target, configs[0]
+
+
+def test_accel_controller_schema_contract():
+  expected = {"eco": 0, "normal": 1, "sport": 2}
+  state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5}
+  accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"]
+  fields = custom.LongitudinalPlanSP.AccelController.schema.fields
+
+  assert accel_controller.proto.ordinal.explicit == 8
+  assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == {
+    "enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4,
+  }
+  assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool"
+  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state
+
+
+def test_accel_controller_schema_round_trip_and_toyota_compatibility():
+  message = custom.LongitudinalPlanSP.new_message()
+  message.accelController.enabled = True
+  message.accelController.active = True
+  message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
+  message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release
+
+  with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
+    assert reader.accelController.enabled and reader.accelController.active
+    assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport
+    assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release
+
+  from opendbc.car.toyota.carstate import AccelPersonality, CarState
+
+  assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2}
+  assert CarState.__module__ == "opendbc.car.toyota.carstate"
+
+
+def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds():
+  assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,)
+  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality")
+  mpc = LongitudinalMpc()
+  radar = radar_state()
+  mpc.run = lambda: None
+
+  mpc.set_cur_state(10.0, 0.8)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+  assert mpc.cruise_accel_max(1.6) == 1.6
+
+  mpc.set_accel_controller_params(None, 1.0, 0.4)
+  assert mpc.cruise_accel_max(1.6) == 0.4
+  mpc.set_accel_controller_params(None, 1.0, 0.0)
+  assert mpc.cruise_accel_max(1.6) == 0.0
+
+  requested_ceiling = tuple(np.full(N + 1, 0.4))
+  mpc.set_accel_controller_params(requested_ceiling, 1.0)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  assert mpc.params[0, 1] == pytest.approx(0.8)
+  np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:])
+
+  for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)):
+    mpc.set_accel_controller_params(malformed_ceiling, 1.0)
+    mpc.update(radar, 30.0)
+    np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+    np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+  mpc.set_accel_controller_params(None, 1.0)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+
+def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs():
+  mpc = LongitudinalMpc.__new__(LongitudinalMpc)
+  LongitudinalMpcSP.__init__(mpc)
+  captured = []
+  mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints)))
+
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  default_costs, default_constraints = captured[-1]
+  mpc.set_accel_controller_params(None, 1.0)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  explicit_costs, explicit_constraints = captured[-1]
+  mpc.set_accel_controller_params(None, 1.2)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  smoothed_costs, smoothed_constraints = captured[-1]
+
+  np.testing.assert_array_equal(explicit_costs, default_costs)
+  np.testing.assert_array_equal(explicit_constraints, default_constraints)
+  np.testing.assert_array_equal(smoothed_costs[:-1], default_costs[:-1])
+  assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2)
+  np.testing.assert_array_equal(smoothed_constraints, default_constraints)
+
+  mpc.set_weights(False, personality=log.LongitudinalPersonality.standard)
+  assert captured[-1][0][-2] == 0.0
+  assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
+
+
+def test_accel_controller_hook_only_configures_mpc():
+  radar = radar_state()
+  planner, _ = planner_for_mpc_test(active=False)
+  calls = []
+  planner.mpc = SimpleNamespace(
+    source=MpcLongitudinalPlanSource.cruise,
+    last_solution_status=0,
+    set_accel_controller_params=lambda accel_max, multiplier, cruise_accel_max: calls.append(
+      ("configure", accel_max, multiplier, cruise_accel_max)),
+    set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)),
+    set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
+    update=lambda radar_arg, target, *, personality: calls.append(("update", radar_arg, target, personality)),
+  )
+  sm = {
+    "radarState": radar,
+    "controlsState": SimpleNamespace(forceDecel=False),
+    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
+    "selfdriveState": SimpleNamespace(personality=2),
+  }
+  is_e2e, target = planner.update_accel_controller(sm, 17.5, True, ACCEL_MAX, False)
+
+  assert not is_e2e and target == 17.5
+  assert calls == [("configure", None, 1.0, None)]
+
+
+def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
+  is_e2e, target, config = prepare_controller_mpc(planner)
+
+  assert not is_e2e
+  assert len(mode_calls) == 1
+  assert target == 15.0
+  assert config == (ceiling, 1.0, None)
+
+
+@pytest.mark.parametrize("cruise_accel_max", (0.0, 0.3))
+def test_cruise_accel_ceiling_is_forwarded_to_mpc(cruise_accel_max):
+  planner, _ = planner_for_mpc_test(cruise_accel_max=cruise_accel_max)
+  _, _, config = prepare_controller_mpc(planner)
+  assert config == (None, 1.0, cruise_accel_max)
+
+
+@pytest.mark.parametrize(("stock_accel_max", "expected"), ((1.2, 1.2), (-0.3, 0.0)))
+def test_controller_receives_stock_allow_throttle_ceiling(stock_accel_max, expected):
+  planner, _ = planner_for_mpc_test()
+  planner.allow_throttle = False
+  prepare_controller_mpc(planner, stock_accel_max=stock_accel_max)
+
+  assert planner.accel_controller.update_kwargs["stock_accel_max"] == expected
+
+
+def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling():
+  planner, _ = planner_for_mpc_test(
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 20.0
+  assert config == (None, 1.0, None)
+
+
+def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling():
+  planner, _ = planner_for_mpc_test(
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 0.0
+  assert config == (None, 1.0, None)
+
+
+@pytest.mark.parametrize(
+  ("active", "departure_launching", "expected"),
+  [
+    (True, True, False),
+    (True, False, True),
+    (False, True, True),
+  ],
+)
+def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold)
+  planner._accel_controller_actuating = active
+  planner._radar_fresh_this_cycle = True
+  planner.mpc = SimpleNamespace(last_solution_status=0)
+  assert planner.update_should_stop(True) is expected
+  assert planner.update_should_stop(False) is (active and not departure_launching)
+
+
+def test_stale_radar_cannot_authorize_departure():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = ControllerStub(active=True, departure_launching=True)
+  planner._accel_controller_actuating = True
+  planner._radar_fresh_this_cycle = False
+  planner.mpc = SimpleNamespace(last_solution_status=0)
+
+  assert planner.update_should_stop(True)
+
+
+@pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])
+def test_disabled_or_e2e_is_an_exact_mpc_bypass(active, is_e2e):
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(active=active, is_e2e=is_e2e, mpc_accel_max=ceiling)
+  returned_e2e, target, config = prepare_controller_mpc(planner)
+
+  assert returned_e2e is is_e2e
+  assert len(mode_calls) == 1
+  assert target == 20.0
+  assert config == (None, 1.0, None)
+
+
+def test_force_decel_target_remains_authoritative_and_disables_ceiling():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
+  _, target, config = prepare_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True)
+
+  assert len(mode_calls) == 1
+  assert target == 0.0
+  assert config == (None, 1.0, None)
+
+
+def test_previous_mpc_failure_gets_one_stock_recovery_cycle_without_resetting_controller_state():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling, departure_launching=True)
+  controller = planner.accel_controller
+  planner.mpc.last_solution_status = 4
+
+  _, failed_target, failed_config = prepare_controller_mpc(planner)
+  assert controller.reset_calls == 0
+  assert controller.update_kwargs["acc_selected"]
+  assert len(mode_calls) == 1
+  assert failed_target == 20.0
+  assert failed_config == (None, 1.0, None)
+  assert planner.update_should_stop(True)
+
+  planner.mpc.last_solution_status = 0
+  _, recovered_target, recovered_config = prepare_controller_mpc(planner)
+  assert controller.reset_calls == 0
+  assert len(mode_calls) == 2
+  assert recovered_target == 15.0
+  assert recovered_config == (ceiling, 1.0, None)
+  assert not planner.update_should_stop(True)
+
+
+@pytest.mark.parametrize(
+  "mpc_source",
+  (MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1),
+)
+def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source):
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
+    mpc_source=mpc_source,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 15.0
+  assert config == (None, MPC_DECEL_JERK_COST_MULTIPLIER, None)
+
+
+def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
+  )
+  _, _, initial_config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  assert initial_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+  controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL
+  _, _, ineligible_config = prepare_controller_mpc(planner)
+  assert ineligible_config[1] == 1.0
+
+  controller.required_decel = 0.30
+  _, _, flicker_config = prepare_controller_mpc(planner)
+  assert flicker_config[1] == 1.0
+
+  controller.state = AccelControllerState.free
+  controller.output_v_target = 20.0
+  prepare_controller_mpc(planner)
+  controller.state = AccelControllerState.restrict
+  controller.output_v_target = 15.0
+  _, _, rearmed_config = prepare_controller_mpc(planner)
+  assert rearmed_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+
+def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  multipliers = [config[1]]
+  for required_decel in (0.20, 0.23, 0.25):
+    controller.required_decel = required_decel
+    _, _, config = prepare_controller_mpc(planner)
+    multipliers.append(config[1])
+
+  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
+
+  controller.required_decel = 0.20
+  _, _, config = prepare_controller_mpc(planner)
+  assert config[1] == 1.0
+
+  controller.state = AccelControllerState.free
+  controller.output_v_target = 20.0
+  prepare_controller_mpc(planner)
+  controller.state = AccelControllerState.restrict
+  controller.output_v_target = 15.0
+  controller.required_decel = 0.18
+  _, _, config = prepare_controller_mpc(planner)
+  assert config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+
+def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  multipliers = [config[1]]
+  for required_decel in (0.24, 0.19, 0.22):
+    controller.required_decel = required_decel
+    _, _, config = prepare_controller_mpc(planner)
+    multipliers.append(config[1])
+
+  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
+
+
+@pytest.mark.parametrize(
+  ("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
+  [
+    (AccelControllerState.free, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.hold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.stopHold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, -1, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, True, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, math.inf, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, math.nan, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.0, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, -0.01, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise),
+  ],
+)
+def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
+  state, selected_lead, launching, required_decel, target_speed, mpc_source,
+):
+  planner, _ = planner_for_mpc_test(
+    state=state, selected_lead=selected_lead, launching=launching,
+    required_decel=required_decel, target_speed=target_speed, mpc_source=mpc_source,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+
+  assert config[1] == 1.0
+
+
+def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
+  planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
+  planner._radar_fresh_this_cycle = True
+  planner.a_desired = -0.4
+  planner.previous_plan_accel = -1.1
+  planner.v_desired_filter = SimpleNamespace(x=9.5)
+  prepare_controller_mpc(planner)
+  received = planner.accel_controller.update_kwargs
+
+  assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
+  assert received["planner_speed"] == 9.5
+  assert received["planner_accel"] == -0.4
+  assert received["previous_plan_accel"] == -1.1
+  assert received["radar_fresh"] is True
+
+
+@pytest.mark.parametrize(("previous_plan_accel", "a_desired", "expected"), (
+  (-1.1, 0.0, -1.1), (0.057, 1.032, 0.057), (0.4, -1.2, -1.2),
+))
+def test_e2e_to_acc_handoff_uses_one_sided_mpc_seed(previous_plan_accel, a_desired, expected):
+  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
+  planner.previous_plan_accel = previous_plan_accel
+  planner.a_desired = a_desired
+  prepare_controller_mpc(planner)
+
+  assert planner.mpc_accel_seed == expected
+  assert planner.a_desired == a_desired
+
+
+def test_disabled_controller_does_not_change_e2e_to_acc_seed():
+  planner, _ = planner_for_mpc_test(active=False, mpc_source=MpcLongitudinalPlanSource.e2e)
+  planner.accel_controller.enabled = False
+  planner.previous_plan_accel = -1.1
+  planner.a_desired = 0.0
+  prepare_controller_mpc(planner)
+
+  assert planner.mpc_accel_seed == planner.a_desired == 0.0
+
+
+def test_inactive_previous_cycle_does_not_restore_an_old_e2e_brake_plan():
+  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
+  planner._long_active_last_cycle = False
+  planner.previous_plan_accel = -1.1
+  planner.a_desired = 0.0
+  prepare_controller_mpc(planner)
+
+  assert planner.mpc_accel_seed == planner.a_desired == 0.0
+  assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"])
+
+
+def test_failed_e2e_solution_does_not_seed_the_next_mpc_cycle():
+  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
+  planner.mpc.last_solution_status = 1
+  planner.previous_plan_accel = -1.1
+  planner.a_desired = 0.0
+  prepare_controller_mpc(planner)
+
+  assert planner.mpc_accel_seed == planner.a_desired == 0.0
+  assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"])
+
+
+def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
+  controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False))
+  controller.enabled = True
+  assert not controller.is_enabled
+
+
+def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._radar_log_mono_time = None
+  planner._radar_fresh_this_cycle = True
+  planner.events_sp = SimpleNamespace(clear=lambda: None)
+  dec_freshness = []
+  planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
+  planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
+  planner.output_a_target = 0.0
+  planner.output_v_target = 20.0
+  planner.output_should_stop = False
+  planner._long_active_last_cycle = False
+  planner.previous_plan_accel = 0.0
+  planner.allow_throttle = True
+  planner.a_desired = 0.0
+  planner.v_desired_filter = SimpleNamespace(x=10.0)
+  planner.mpc = SimpleNamespace(
+    source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0,
+    set_accel_controller_params=lambda *_args: None,
+  )
+  planner.is_e2e = lambda _sm: False
+  planner.accel_controller = ControllerStub(target_speed=20.0, active=False)
+
+  sm = PlannerSM(100)
+  for expected in (True, False):
+    planner.update(sm)
+    planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
+    assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected
+
+  sm.logMonoTime["radarState"] = 101
+  planner.update(sm)
+  planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
+  assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True
+
+
+def test_accel_controller_status_publishes_minimal_fields():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.source = LongitudinalPlanSource.cruise
+  planner.output_v_target = 20.0
+  planner.output_a_target = 0.0
+  planner.events_sp = SimpleNamespace(to_msg=list)
+  planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
+  planner.accel_controller = ControllerStub(active=False, state=AccelControllerState.restrict)
+  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),
+    map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
+  )
+  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,
+    source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
+  )
+  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,
+  )
+  planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
+  sent = {}
+  planner.publish_longitudinal_plan_sp(
+    SimpleNamespace(all_checks=lambda service_list: True),
+    SimpleNamespace(send=lambda service, message: sent.update({service: message})),
+  )
+
+  telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
+  assert telemetry.enabled and not telemetry.active
+  assert telemetry.profile == int(AccelProfile.normal)
+  assert telemetry.state == int(AccelControllerState.restrict)
+  assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py

@@ -0,0 +1,341 @@
+import math
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, DISTANCE_JUMP_CONFIRM_FRAMES, LEAD_DROPOUT_COAST_TIME, LEAD_RELEASE_CONFIRM_TIME,
+  STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController
+
+DT = DT_MDL
+COMFORT_DECEL_NORMAL = COMFORT_DECEL[AccelProfile.normal]
+PROFILE_MAX_ACCEL = 1.5
+
+
+def _frames(seconds: float) -> int:
+  return math.ceil(seconds / DT)
+
+
+LEAD_CONFIRM_FRAMES = max(LEAD_SAMPLE_FILTER_FRAMES, _frames(LEAD_RELEASE_CONFIRM_TIME))
+DROPOUT_FRAMES = max(LEAD_CONFIRM_FRAMES, _frames(LEAD_DROPOUT_COAST_TIME))
+
+
+def _lead_plan(speed: float, distance: float, speed_ceiling: float = 0.0, closing_speed: float = 0.0,
+              required_decel: float = 0.0, track_id: int = 1) -> LeadPlan:
+  return LeadPlan(
+    speed_ceiling=speed_ceiling, selected_lead=0, selected_lead_track_id=track_id, selected_lead_speed=speed, selected_lead_accel=0.0,
+    departure_lead=0, departure_lead_track_id=track_id, departure_lead_speed=speed, departure_lead_distance=distance,
+    departure_lead_raw_speed=speed,
+    departure_lead_separation=distance, departure_speed_ceiling=speed_ceiling,
+    closing_speed=closing_speed, required_decel=required_decel,
+    has_nearly_stopped_lead=speed < 0.15, lead_status=True,
+  )
+
+
+def _no_lead() -> LeadPlan:
+  return LeadPlan(lead_status=False)
+
+
+def _run(lead_controller: LeadController, lead_plan: LeadPlan, base_speed: float, v_ego: float, planner_speed: float | None = None,
+        planner_accel: float = 0.0, previous_mpc_source=None, previous_plan_accel: float = 0.0) -> float:
+  return lead_controller.update(lead_plan, base_speed, v_ego, COMFORT_DECEL_NORMAL, PROFILE_MAX_ACCEL, DT,
+                      LEAD_CONFIRM_FRAMES, DROPOUT_FRAMES, v_ego if planner_speed is None else planner_speed,
+                      planner_accel, previous_mpc_source, previous_plan_accel)
+
+
+def test_repeated_stop_reseeds_departure_distance():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+  for frame in range(20):
+    _run(lead_controller, _lead_plan(2.0, 6.0 + 2.0 * (frame + 1) * DT), base_speed=8.0, v_ego=min(3.5, frame * 0.3))
+
+  for _ in range(10):
+    _run(lead_controller, _lead_plan(0.0, 3.0), base_speed=8.0, v_ego=0.0)
+  assert lead_controller.stop_hold
+
+  released_frame = None
+  for frame in range(60):
+    _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0)
+    if not lead_controller.stop_hold:
+      released_frame = frame
+      break
+
+  assert released_frame is not None
+  assert released_frame * DT <= 2.0
+
+
+def test_departure_dropout_reseeds_distance_guard():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+  _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
+
+  released_frame = None
+  for frame in range(60):
+    _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0)
+    if not lead_controller.stop_hold:
+      released_frame = frame
+      break
+
+  assert released_frame is not None
+  assert released_frame * DT <= 2.0
+
+
+def test_departure_dropout_revokes_track_identity_trust():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0)
+
+  _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
+  for frame in range(4):
+    _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0)
+
+  assert lead_controller.launching
+  assert lead_controller.leadless_departure
+  assert not lead_controller.departure_launching
+
+
+def test_range_discontinuity_revokes_track_identity_trust():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0)
+
+  for frame in range(DISTANCE_JUMP_CONFIRM_FRAMES + STOP_HOLD_EXIT_FRAMES + 2):
+    _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0)
+    if not lead_controller.stop_hold:
+      break
+
+  assert lead_controller.launching
+  assert lead_controller.leadless_departure
+  assert not lead_controller.departure_launching
+
+
+def test_fast_lead_speed_requires_full_departure_distance():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+
+  for distance in (6.00, 6.01, 6.02, 6.03):
+    target = _run(lead_controller, _lead_plan(1.0, distance), base_speed=8.0, v_ego=0.0)
+
+  assert lead_controller.stop_hold
+  assert target == 0.0
+
+
+def test_lead_disappearance_releases_stop_hold_monotonically():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+
+  targets = [_run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) for _ in range(DROPOUT_FRAMES + 10)]
+  first_release = next(index for index, target in enumerate(targets) if target > 0.0)
+  release_targets = targets[first_release:]
+
+  steps = [after - before for before, after in zip(release_targets[:-1], release_targets[1:], strict=True)]
+  assert all(0.0 <= step <= TARGET_RELEASE_SLEW * DT + 1e-9 for step in steps)
+
+
+def test_no_lead_departure_stays_bounded_when_lead_returns():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+  for _ in range(LEAD_CONFIRM_FRAMES):
+    _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
+
+  before = lead_controller.target_speed
+  target = _run(lead_controller, _lead_plan(2.0, 6.0, speed_ceiling=8.0), base_speed=8.0, v_ego=0.5)
+
+  assert target <= max(before, 0.5) + TARGET_RELEASE_SLEW * DT + 1e-9
+
+
+def test_leadless_stop_release_never_exceeds_current_base_speed():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+
+  targets = [_run(lead_controller, _no_lead(), base_speed=0.5, v_ego=4.0) for _ in range(LEAD_CONFIRM_FRAMES)]
+
+  assert max(targets) <= 0.5
+
+
+def test_retained_speed_ceiling_does_not_bind_a_moving_replacement():
+  lead_controller = LeadController()
+  _run(lead_controller, _lead_plan(0.0, 5.0, speed_ceiling=0.1, track_id=100), base_speed=8.0, v_ego=0.4)
+
+  _run(lead_controller, _lead_plan(2.0, 20.0, speed_ceiling=8.0, track_id=200), base_speed=8.0, v_ego=0.2)
+
+  assert lead_controller.lead_speed_ceiling == pytest.approx(0.1)
+  assert not lead_controller.stop_hold
+
+
+def test_braking_state_does_not_cross_confirmed_track_replacement():
+  lead_controller = LeadController()
+  for _ in range(LEAD_CONFIRM_FRAMES + 2):
+    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
+  assert lead_controller.braking_for_lead
+
+  _run(lead_controller, _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=99), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
+
+  assert not lead_controller.stop_hold
+
+
+def test_braking_state_does_not_cross_vision_slot_replacement():
+  lead_controller = LeadController()
+  for _ in range(LEAD_CONFIRM_FRAMES + 2):
+    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=-1), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
+  assert lead_controller.braking_for_lead
+
+  replacement = _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=-1)._replace(selected_lead=1, departure_lead=1)
+  _run(lead_controller, replacement, base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
+
+  assert not lead_controller.stop_hold
+
+
+def test_radar_track_keeps_braking_confirmation_across_slots():
+  lead_controller = LeadController()
+  for frame in range(LEAD_CONFIRM_FRAMES + 2):
+    plan = _lead_plan(0.0, 7.0, speed_ceiling=0.8, track_id=100)
+    if frame % 2:
+      plan = plan._replace(selected_lead=1, departure_lead=1)
+    _run(lead_controller, plan, base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
+
+  assert lead_controller.braking_for_lead
+  assert lead_controller.stop_hold
+
+
+def test_vision_slot_churn_releases_conservatively():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=-1), base_speed=8.0, v_ego=0.0)
+
+  for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
+    plan = _lead_plan(2.0, 6.0 + 0.1 * (frame + 1), speed_ceiling=8.0, track_id=-1)
+    if frame % 2:
+      plan = plan._replace(selected_lead=1, departure_lead=1)
+    _run(lead_controller, plan, base_speed=8.0, v_ego=0.0)
+    if not lead_controller.stop_hold:
+      break
+
+  assert lead_controller.launching
+  assert lead_controller.leadless_departure
+  assert not lead_controller.departure_launching
+
+
+def test_radar_track_dropout_keeps_braking_context():
+  lead_controller = LeadController()
+  for _ in range(LEAD_CONFIRM_FRAMES + 2):
+    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
+  assert lead_controller.braking_for_lead
+
+  _run(lead_controller, _no_lead(), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
+
+  assert lead_controller.should_coast_on_dropout
+
+
+def test_range_replacement_starts_a_new_departure_baseline():
+  lead_controller = LeadController()
+  for _ in range(20):
+    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
+  for _ in range(2):
+    _run(lead_controller, _lead_plan(0.2, 6.0), base_speed=8.0, v_ego=0.0)
+
+  released_frame = None
+  for frame in range(60):
+    distance = 3.0 + 0.2 * frame * DT
+    _run(lead_controller, _lead_plan(0.2, distance), base_speed=8.0, v_ego=0.0)
+    if not lead_controller.stop_hold:
+      released_frame = frame
+      break
+
+  assert released_frame is not None
+  assert released_frame * DT <= 2.0
+
+
+def test_stop_hold_clears_the_previous_recovery_limit():
+  lead_controller = LeadController()
+  for _ in range(LEAD_CONFIRM_FRAMES + 120):
+    _run(lead_controller, _lead_plan(5.0, 20.0, speed_ceiling=8.0), base_speed=12.0, v_ego=10.0)
+  assert lead_controller.recovery_accel_limit == pytest.approx(0.0)
+
+  _run(lead_controller, _lead_plan(0.0, 5.0), base_speed=8.0, v_ego=0.2, planner_accel=-0.5)
+  assert lead_controller.stop_hold
+  assert lead_controller.recovery_accel_limit is None
+
+  for frame in range(9):
+    _run(lead_controller, _lead_plan(1.0, 5.0 + 0.04 * frame, speed_ceiling=8.0), base_speed=8.0, v_ego=0.0)
+  assert lead_controller.departure_launching
+
+  _run(lead_controller, _lead_plan(5.0, 8.0, speed_ceiling=8.0), base_speed=8.0, v_ego=3.1)
+  assert lead_controller.recovery_accel_limit is not None
+  assert lead_controller.recovery_accel_limit > 1.4
+
+
+def test_speed_ceiling_tightens_immediately():
+  lead_controller = LeadController()
+  _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=25.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(25.0)
+  _run(lead_controller, _lead_plan(15.0, 40.0, speed_ceiling=10.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(10.0)
+
+
+def test_speed_ceiling_requires_confirmation_before_release():
+  lead_controller = LeadController()
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
+
+  glitch_len = LEAD_CONFIRM_FRAMES - 2
+  for _ in range(glitch_len):
+    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0), "brief relief spike must not be trusted"
+
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
+
+  for _ in range(LEAD_CONFIRM_FRAMES + LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(28.0), "sustained relief must eventually be trusted"
+
+
+def test_conflicting_relief_evidence_stays_restricted():
+  lead_controller = LeadController()
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
+
+  targets = []
+  for frame in range(300):
+    speed_ceiling = 28.0 if frame % 2 == 0 else 20.0
+    targets.append(_run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=speed_ceiling), base_speed=30.0, v_ego=20.0))
+
+  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
+  assert max(targets) == pytest.approx(20.0)
+
+
+def test_lead_dropout_holds_speed_ceiling_before_release():
+  lead_controller = LeadController()
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(6.0, 50.0, speed_ceiling=6.0), base_speed=18.0, v_ego=10.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(6.0)
+
+  for _ in range(DROPOUT_FRAMES - 1):
+    _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0)
+  assert lead_controller.lead_speed_ceiling == pytest.approx(6.0), "must coast, not snap, before the dropout window elapses"
+
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0)
+  assert math.isinf(lead_controller.lead_speed_ceiling), "must release promptly once the dropout window has elapsed"
+
+
+def test_target_release_is_rate_limited():
+  lead_controller = LeadController()
+  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
+    _run(lead_controller, _lead_plan(20.0, 60.0, speed_ceiling=22.0), base_speed=30.0, v_ego=22.0)
+  before = lead_controller.target_speed
+
+  target = _run(lead_controller, _lead_plan(28.0, 200.0, speed_ceiling=30.0), base_speed=30.0, v_ego=22.0)
+  assert target <= before + TARGET_RELEASE_SLEW * DT + 1e-9

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,46 @@
+"""
+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 numpy as np
+
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+
+
+class LongitudinalMpcSP:
+  def __init__(self) -> None:
+    self._accel_max_trajectory: tuple[float, ...] | None = None
+    self._cruise_accel_max: float | None = None
+    self._jerk_cost_multiplier = 1.0
+    self.last_solution_status = 0
+
+  def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float,
+                                  cruise_accel_max: float | None = None) -> None:
+    self._accel_max_trajectory = accel_max
+    self._cruise_accel_max = cruise_accel_max
+    self._jerk_cost_multiplier = jerk_cost_multiplier
+
+  def cruise_accel_max(self, stock_accel_max: float) -> float:
+    if self._cruise_accel_max is None or not np.isfinite(self._cruise_accel_max):
+      return stock_accel_max
+    return min(max(self._cruise_accel_max, 0.0), stock_accel_max)
+
+  def scale_jerk_cost(self, jerk_cost: float) -> float:
+    return jerk_cost * self._jerk_cost_multiplier
+
+  def apply_accel_limits(self) -> None:
+    if self._accel_max_trajectory is None:
+      return
+
+    accel_max = np.asarray(self._accel_max_trajectory)
+    if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)):
+      return
+
+    self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX)
+    self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
+
+  def save_solution_status(self) -> None:
+    self.last_solution_status = self.solution_status

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,10 +5,15 @@ 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 openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 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
@@ -22,9 +27,10 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
+    self.mpc = mpc
+    self.accel_controller = AccelController(CP, dt=dt)
     self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -32,9 +38,15 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self._radar_log_mono_time = None
+    self._radar_fresh_this_cycle = True
+    self._long_active_last_cycle = False
+    self._accel_controller_actuating = False
 
     self.output_v_target = 0.
     self.output_a_target = 0.
+    self.previous_plan_accel = 0.
+    self.mpc_accel_seed = 0.
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
@@ -43,6 +55,44 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def update_accel_controller(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool,
+                              stock_accel_max: float, reset_state: bool) -> tuple[bool, float]:
+    is_e2e = self.is_e2e(sm)
+    force_decel = sm['controlsState'].forceDecel
+    previous_mpc_failed = self.mpc.last_solution_status != 0
+    previous_plan_accel = self.previous_plan_accel if self._long_active_last_cycle and not previous_mpc_failed else float("inf")
+    self.mpc_accel_seed = self.a_desired
+
+    self.accel_controller.update(
+      sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
+      follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e,
+      engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET,
+      stock_accel_max=max(stock_accel_max, 0.0),
+      radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x,
+      planner_accel=self.a_desired, previous_plan_accel=previous_plan_accel,
+    )
+    controller = self.accel_controller
+    acc_handoff = (controller.is_active and not is_e2e and not previous_mpc_failed and self.mpc.source == MpcLongitudinalPlanSource.e2e
+                   and math.isfinite(previous_plan_accel))
+    if acc_handoff:
+      self.mpc_accel_seed = min(self.a_desired, previous_plan_accel)
+    actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed
+    self._accel_controller_actuating = actuating
+    valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0
+    controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise
+    accel_max = controller.mpc_accel_max if actuating else None
+    cruise_accel_max = controller.cruise_accel_max if actuating else None
+    jerk_cost_multiplier = controller.get_jerk_cost_multiplier(
+      actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed,
+    )
+    self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier, cruise_accel_max)
+    self._long_active_last_cycle = not reset_state and not force_decel
+    return is_e2e, controller_v_cruise
+
+  def update_should_stop(self, should_stop: bool) -> bool:
+    departure_authorized = self._accel_controller_actuating and self._radar_fresh_this_cycle and self.mpc.last_solution_status == 0
+    return self.accel_controller.update_should_stop(should_stop, departure_authorized)
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -73,7 +123,18 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
+    radar_log_mono_time = sm.logMonoTime['radarState']
+    radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
+    radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
+    if radar_advanced:
+      self._radar_log_mono_time = radar_log_mono_time
+    return radar_healthy and radar_advanced
+
   def update(self, sm: messaging.SubMaster) -> None:
+    self.previous_plan_accel = self.output_a_target
+    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
+    self.accel_controller.update_params()
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -95,6 +156,12 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    accelController = longitudinalPlanSP.accelController
+    accelController.enabled = self.accel_controller.is_enabled
+    accelController.active = self.accel_controller.is_active
+    accelController.profile = self.accel_controller.profile
+    accelController.state = self.accel_controller.state
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -0,0 +1,395 @@
+"""
+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.
+"""
+
+from collections import deque
+from collections.abc import Callable
+from dataclasses import dataclass
+import math
+import time
+from typing import Any
+
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
+
+
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
+
+
+@dataclass(frozen=True)
+class ActuatorModel:
+  planner_delay: float
+  transport_delay: float
+  actuator_lag: float
+  command_rate_limit: float
+  stopping_acceleration: float
+  standstill_breakaway_acceleration: float
+  standstill_breakaway_time: float
+
+  def __post_init__(self):
+    nonnegative_fields = {
+      "planner_delay": self.planner_delay,
+      "transport_delay": self.transport_delay,
+      "actuator_lag": self.actuator_lag,
+      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
+      "standstill_breakaway_time": self.standstill_breakaway_time,
+    }
+    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
+      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
+    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
+      raise ValueError("command_rate_limit must be finite and positive")
+    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
+      raise ValueError("stopping_acceleration must be finite and non-positive")
+
+
+# Conservative Prius TSS2 actuator model.
+PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
+  planner_delay=0.05,
+  transport_delay=0.0,
+  actuator_lag=0.20,
+  command_rate_limit=4.0,
+  stopping_acceleration=-2.0,
+  standstill_breakaway_acceleration=1.0,
+  standstill_breakaway_time=0.05,
+)
+
+
+class PlantSP(Plant):
+  """Closed-loop plant with configurable observations and actuator response."""
+
+  def __init__(
+    self,
+    lead_relevancy=False,
+    speed=0.0,
+    distance_lead=2.0,
+    enabled=True,
+    only_lead2=False,
+    only_radar=False,
+    e2e=False,
+    personality=0,
+    force_decel=False,
+    lead_observation_fn: LeadObservationFn | None = None,
+    model_action_fn: ModelActionFn | None = None,
+    ego_observation_fn: EgoObservationFn | None = None,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+    actuator_model: ActuatorModel | None = None,
+    run_long_control: bool = False,
+  ):
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
+
+    if not Plant.messaging_initialized:
+      Plant.radar = messaging.pub_sock('radarState')
+      Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+      Plant.car_state = messaging.pub_sock('carState')
+      Plant.plan = messaging.sub_sock('longitudinalPlan')
+      Plant.messaging_initialized = True
+
+    self.v_lead_prev = 0.0
+
+    self.distance = 0.0
+    self.speed = speed
+    self.should_stop = False
+    self.acceleration = 0.0
+    self.a_target = 0.0
+    self.actuator_command = 0.0
+    self.applied_actuator_command = 0.0
+    self.breakaway_confirmed = False
+    self._breakaway_timer = 0.0
+
+    # lead car
+    self.lead_relevancy = lead_relevancy
+    self.distance_lead = distance_lead
+    self.enabled = enabled
+    self.only_lead2 = only_lead2
+    self.only_radar = only_radar
+    self.e2e = e2e
+    self.personality = personality
+    self.force_decel = force_decel
+    self.lead_observation_fn = lead_observation_fn
+    self.model_action_fn = model_action_fn
+    self.ego_observation_fn = ego_observation_fn
+    self.actuator_model = actuator_model
+    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
+    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
+    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
+                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
+
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+    self.ts = 1.0 / self.rate
+    time.sleep(0.1)
+    self.sm = messaging.SubMaster(['longitudinalPlan'])
+
+    from opendbc.car.honda.values import CAR
+    from opendbc.car.honda.interface import CarInterface
+
+    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    if self.actuator_delay is not None:
+      CP.longitudinalActuatorDelay = self.actuator_delay
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
+    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
+    self.long_control = LongControl(CP, CP_SP) if run_long_control else None
+
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    self.integration_dt = DT_CTRL if run_long_control else self.ts
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
+  @staticmethod
+  def _lead_message(observation: LeadObservation):
+    lead = log.RadarState.LeadData.new_message()
+    for field, value in observation.items():
+      setattr(lead, field, value)
+    return lead
+
+  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
+    if self.lead_observation_fn is None:
+      return dict(truth) if present_by_default else None
+
+    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
+    if observed is None:
+      return None
+
+    complete_observation = dict(truth)
+    complete_observation.update(observed)
+    return complete_observation
+
+  def _update_actuator(self, command: float) -> tuple[float, float]:
+    if self._actuator_delay_queue:
+      self._actuator_delay_queue.append(command)
+      delayed_command = self._actuator_delay_queue.popleft()
+    else:
+      delayed_command = command
+
+    if self.actuator_model is not None:
+      max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
+      self.applied_actuator_command = float(np.clip(delayed_command,
+                                                   self.applied_actuator_command - max_command_delta,
+                                                   self.applied_actuator_command + max_command_delta))
+
+      if self.speed < 0.01:
+        if self.applied_actuator_command <= 0.0:
+          self.breakaway_confirmed = False
+          self._breakaway_timer = 0.0
+        elif not self.breakaway_confirmed:
+          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
+          if breakaway_ready:
+            self._breakaway_timer += self.integration_dt
+          else:
+            self._breakaway_timer = 0.0
+
+          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
+        if not self.breakaway_confirmed:
+          self.acceleration = 0.0
+          return delayed_command, self.acceleration
+      else:
+        self.breakaway_confirmed = True
+
+      response_command = self.applied_actuator_command
+    else:
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
+    self.speed += self.acceleration * dt
+    if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
+      self.speed = self.acceleration = 0.0
+    self.distance += self.speed * dt
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+    # ******** publish a fake model going straight and fake calibration ********
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    radar = messaging.new_message('radarState')
+    control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
+    car_state = messaging.new_message('carState')
+    lp = messaging.new_message('liveParameters')
+    car_control = messaging.new_message('carControl')
+    model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
+    self.v_lead_prev = v_lead
+
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+      if self.only_radar:
+        status = True
+      elif prob_lead > 0.5:
+        status = True
+      else:
+        status = False
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+      prob_lead = 0.0
+      status = False
+
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "vLead": float(v_lead),
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "present": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
+
+    # Simulate model predicting slightly faster speed
+    # this is to ensure lead policy is effective when model
+    # does not predict slowdown in e2e mode
+    position = log.XYZTData.new_message()
+    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+    model.modelV2.position = position
+    if self.model_action_fn is None:
+      model_acceleration, model_should_stop = self.acceleration + 0.1, False
+    else:
+      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
+    model.modelV2.action.desiredAcceleration = float(model_acceleration)
+    model.modelV2.action.shouldStop = bool(model_should_stop)
+    velocity = log.XYZTData.new_message()
+    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+    velocity.x[0] = float(self.speed)  # always start at current speed
+    model.modelV2.velocity = velocity
+    acceleration = log.XYZTData.new_message()
+    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+    model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
+
+    control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
+      LongCtrlState.pid if self.enabled else LongCtrlState.off)
+    ss.selfdriveState.experimentalMode = self.e2e
+    ss.selfdriveState.personality = self.personality
+    control.controlsState.forceDecel = self.force_decel
+    true_v_ego = self.speed
+    true_a_ego = self.acceleration
+    published_v_ego = true_v_ego
+    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
+    if self.ego_observation_fn is not None:
+      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
+    car_state.carState.vEgo = float(published_v_ego)
+    car_state.carState.aEgo = float(published_a_ego)
+    car_state.carState.standstill = bool(self.speed < 0.01)
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
+
+    # ******** get controlsState messages for plotting ***
+    sm = PlannerSM(self.rk.frame, {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'liveParameters': lp.liveParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    })
+    self.planner.update(sm)
+    self.a_target = self.planner.output_a_target
+    if self.long_control is None:
+      self.actuator_command = self.a_target
+      if self.planner.output_should_stop:
+        stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
+        self.actuator_command = min(stopping_acceleration, self.actuator_command)
+      self._update_actuator(self.actuator_command)
+      self._integrate_ego(self.ts)
+    else:
+      for _ in range(round(self.ts / DT_CTRL)):
+        car_state.carState.vEgo = self.speed
+        car_state.carState.aEgo = self.acceleration
+        car_state.carState.standstill = self.speed < 0.01
+        self.actuator_command = self.long_control.update(
+          self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
+        )
+        self._update_actuator(self.actuator_command)
+        self._integrate_ego(DT_CTRL, stop_at_standstill=True)
+    self.should_stop = self.planner.output_should_stop
+    fcw = self.planner.fcw
+    self.distance_lead = self.distance_lead + v_lead * self.ts
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+
+    # print at 5hz
+    # if (self.rk.frame % (self.rate // 5)) == 0:
+    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
+
+    # ******** update prevs ********
+    self.rk.monitor_time()
+
+    accel_controller = self.planner.accel_controller
+    return {
+      "distance": self.distance,
+      "speed": self.speed,
+      "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "actuator_command": self.actuator_command,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "should_stop": self.should_stop,
+      "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
+                             else control.controlsState.longControlState.raw),
+      "distance_lead": self.distance_lead,
+      "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "controller_target": accel_controller.output_v_target,
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
+    }

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py

@@ -0,0 +1,157 @@
+from collections.abc import Callable
+import math
+from typing import cast
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+
+STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
+
+
+def departing_lead(current_time: float) -> float:
+  return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
+
+
+def stopped_lead(_current_time: float) -> float:
+  return 0.0
+
+
+PARITY_SCENARIOS = {
+  "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
+  "stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
+}
+
+
+def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
+  plant = cls(**kwargs)
+  plant.v_lead_prev = v_lead(0.0)
+  solver_failures = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counting_reset(*args, **kw):
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    return original_reset(*args, **kw)
+
+  plant.planner.mpc.reset = counting_reset
+  results = []
+  for _ in range(steps):
+    lead_speed = v_lead(plant.current_time)
+    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
+    results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
+  return results, solver_failures
+
+
+@pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS))
+def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario: str):
+  kwargs = dict(PARITY_SCENARIOS[scenario])
+  v_cruise = cast(float, kwargs.pop("v_cruise"))
+  v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
+  steps = cast(int, kwargs.pop("steps"))
+
+  stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+  sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+
+  assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
+  assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
+
+  for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
+    zip(stock_results, sp_results, strict=True),
+  ):
+    for key in STOCK_STEP_KEYS:
+      if isinstance(stock_result[key], float):
+        assert sp_result[key] == pytest.approx(stock_result[key]), f"{scenario} frame {frame} key {key}"
+      else:
+        assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
+    assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
+    assert sp_a_target == pytest.approx(stock_a_target), f"{scenario} frame {frame} output_a_target"
+
+  if scenario == "stop_then_depart":
+    departure_frame = round(1.0 / DT_MDL)
+    for results in (stock_results, sp_results):
+      assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
+      assert results[departure_frame - 1][0]["should_stop"]
+      assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
+      assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
+    stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and not result["should_stop"])
+    sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and not result["should_stop"])
+    stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and result["speed"] > 0.05)
+    sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and result["speed"] > 0.05)
+    assert sp_release == stock_release
+    assert sp_motion == stock_motion
+
+
+def test_full_lead_observation_is_independent_from_truth():
+  callback_inputs = []
+
+  def observe_lead(current_time, lead_name, truth):
+    callback_inputs.append((current_time, lead_name, truth))
+    if lead_name == "leadOne":
+      return {
+        "dRel": 12.5,
+        "vRel": -4.0,
+        "vLead": 6.0,
+        "vLeadK": 5.5,
+        "aLeadK": -1.25,
+        "aLeadTau": 0.7,
+        "present": True,
+        "modelProb": 0.9,
+        "radarTrackId": 42,
+      }
+    return None
+
+  plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
+  result = plant.step(v_lead=8.0)
+
+  assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
+  assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0)
+  assert result["truth_lead"]["dRel"] == pytest.approx(50.0)
+  assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5)
+  assert result["lead_one_observation"]["radarTrackId"] == 42
+  assert result["lead_two_observation"] is None
+  assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL)
+
+
+def test_model_action_realized_acceleration_and_source_logging():
+  def model_action(current_time, v_ego, a_ego):
+    return -1.25, True
+
+  plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
+  first = plant.step()
+  second = plant.step()
+
+  assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
+  assert first["published_a_ego"] == pytest.approx(0.0)
+  assert second["published_a_ego"] == pytest.approx(first["realized_acceleration"])
+  assert first["acceleration"] == first["realized_acceleration"]
+  assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
+  assert first["mpc_source"] is not None
+  assert first["dec_mode"] in ("acc", "blended")
+  assert "controller_target" in first
+  assert first["lead_one_observation"] is not None
+  assert first["truth_lead"] == first["lead_one_observation"]
+
+
+def test_configurable_transport_delay_and_first_order_lag():
+  plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
+
+  assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(2 * DT_MDL)
+  delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
+  assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
+
+  expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
+  assert delayed_commands[2][0] == -1.0
+  assert delayed_commands[2][1] == pytest.approx(expected_acceleration)
+
+
+@pytest.mark.parametrize(
+  ("delay", "lag"),
+  [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
+)
+def test_invalid_actuator_dynamics(delay, lag):
+  with pytest.raises(ValueError):
+    PlantSP(actuator_delay=delay, actuator_lag=lag)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -652,6 +652,58 @@
                 }
               ]
             },
+            {
+              "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.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "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.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                },
+                {
+                  "type": "param",
+                  "key": "AccelPersonalityEnabled",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,6 +43,32 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - 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.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - 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.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: AccelPersonalityEnabled
+      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -278,6 +278,22 @@ class TestKnownPanels:
     enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
     assert "NeuralNetworkLateralControl" in enhanced_enable_keys
 
+  def test_accel_controller_profile_mapping_and_enablement(self, schema):
+    cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
+    items = {item["key"]: item for item in _iter_panel_items(cruise)}
+
+    assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
+    assert items["AccelPersonality"]["options"] == [
+      {"value": 0, "label": "Eco"},
+      {"value": 1, "label": "Normal"},
+      {"value": 2, "label": "Sport"},
+    ]
+    assert {
+      "type": "param",
+      "key": "AccelPersonalityEnabled",
+      "equals": True,
+    } in items["AccelPersonality"]["enablement"]
+
 
 class TestKnownVehicleSettings:
   def test_hyundai_has_longitudinal_tuning(self, schema):