Temper high-speed acceleration for smoother cruising

cereal/custom.capnp

@@ -194,6 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -296,6 +297,47 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    shadowOnly @2 :Bool;
+    profile @3 :Profile;
+    state @4 :State;
+    vTargetBase @5 :Float32;
+    vTargetRaw @6 :Float32;
+    vTargetFiltered @7 :Float32;
+    vTargetShadow @8 :Float32;
+    leadIndex @9 :Int8 = -1;
+    usableGap @10 :Float32;
+    closingSpeed @11 :Float32;
+    requiredDecel @12 :Float32;
+    aMaxProfile @13 :Float32;
+    aMaxEffective @14 :Float32;
+
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+
+    enum State {
+      inactive @0;
+      free @1;
+      restrict @2;
+      hold @3;
+      release @4;
+      stopHold @5;
+    }
+  }
+
+  # Compatibility type for vehicle integrations that map physical drive modes
+  # onto AccelPersonality. New controller telemetry uses AccelController.Profile.
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

common/params_keys.h

@@ -235,6 +235,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"}},

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("LiveParameters")
 
     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("LiveParameters") is None
     assert self.params.get("LiveParameters", return_default=True) is None
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -217,6 +217,7 @@ class LongitudinalMpc:
   def __init__(self, dt=DT_MDL):
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.last_solution_status = 0
     self.reset()
     self.source = LongitudinalPlanSource.cruise
 
@@ -313,7 +314,8 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard,
+             accel_max: float | tuple[float, ...] | np.ndarray | None = None):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -345,6 +347,17 @@ class LongitudinalMpc:
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
+    if accel_max is not None:
+      try:
+        accel_max_trajectory = np.asarray(accel_max, dtype=float)
+      except (TypeError, ValueError):
+        accel_max_trajectory = np.empty(0)
+      if accel_max_trajectory.ndim == 0 and np.isfinite(accel_max_trajectory) and accel_max_trajectory >= 0.0:
+        accel_max_trajectory = np.full(N + 1, float(accel_max_trajectory))
+      valid_accel_max = accel_max_trajectory.shape == (N + 1,) and np.all(np.isfinite(accel_max_trajectory))
+      if valid_accel_max:
+        self.params[:,1] = np.clip(accel_max_trajectory, ACCEL_MIN, ACCEL_MAX)
+        self.params[0,1] = max(self.params[0,1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -364,6 +377,7 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
+    self.last_solution_status = self.solution_status
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
     self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
     self.time_linearization = float(self.solver.get_stats('time_lin')[0])

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
@@ -113,6 +113,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
     accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+    profile_accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, [ACCEL_MIN, ACCEL_MAX], self.CP)
 
     if reset_state:
       self.v_desired_filter.x = v_ego
@@ -129,16 +130,21 @@ 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)
+    controller_accel_max = profile_accel_clip[1] if self.allow_throttle else 0.0
 
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
+    previous_output_a_target = self.output_a_target
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+    base_v_cruise = v_cruise
 
     if force_slow_decel:
       v_cruise = 0.0
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    is_e2e = LongitudinalPlannerSP.update_accel_controller_mpc(
+      self, sm, base_v_cruise, v_cruise, prev_accel_constraint, reset_state=reset_state,
+      cruise_initialized=v_cruise_initialized, planner_accel=self.a_desired, previous_output_accel=previous_output_a_target,
+      available_accel_max=controller_accel_max, previous_should_stop=self.output_should_stop, force_decel=force_slow_decel,
+    )
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -154,13 +160,14 @@ 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
-    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, vEgoStopping=self.CP.vEgoStopping)
+    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, vEgoStopping=self.CP.vEgoStopping,
+    )
     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:

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,5 +1,11 @@
 #!/usr/bin/env python3
+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 cereal import log
@@ -11,12 +17,105 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
+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")
+
+
+# Route-derived conservative Prius TSS2 stress model for the acceleration-controller
+# regression suite. The 1.0 m/s² gate represents prompt takeoffs, not a universal
+# physical threshold: the supplied routes also contain low-command creep departures.
+# This models vehicle response only and does not emulate Toyota's CAN controller.
+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 Plant:
   messaging_initialized = False
 
-  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):
-    self.rate = 1. / DT_MDL
+  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,
+  ):
+    """Closed-loop longitudinal planner plant.
+
+    ``lead_observation_fn(time, lead_name, truth)`` may return a complete or partial
+    observed LeadData mapping, or ``None`` for an absent lead. It is called separately
+    for ``leadOne`` and ``leadTwo``. The supplied truth mapping is a copy, and observed
+    values never affect the physical lead trajectory.
+
+    ``model_action_fn(time, v_ego, a_ego)`` returns
+    ``(desired_acceleration, should_stop)``.
+
+    ``ego_observation_fn(time, true_v_ego, true_a_ego)`` returns the observed
+    ``(v_ego, a_ego)`` published in ``carState``. It can inject measurement noise
+    without changing the physical plant state.
+
+    Passing ``actuator_delay`` both overrides ``CP.longitudinalActuatorDelay`` and
+    adds the corresponding command transport delay to the plant. ``None`` keeps the
+    historical Honda planner delay with instantaneous plant response. ``actuator_lag``
+    is an optional first-order acceleration-response time constant. Both defaults keep
+    historical plant dynamics unchanged.
+
+    ``actuator_model`` opts into a staged vehicle-response model. Its planner delay
+    is used by MPC, while its independent transport delay is used by the command
+    queue before rate limiting, standstill breakaway confirmation, and first-order
+    lag. Leaving it unset preserves the historical actuator path.
+    """
+    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')
@@ -28,10 +127,15 @@ class Plant:
 
     self.v_lead_prev = 0.0
 
-    self.distance = 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
@@ -42,9 +146,18 @@ class Plant:
     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))
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1. / self.rate
+    self.ts = 1.0 / self.rate
     time.sleep(0.1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
@@ -52,14 +165,86 @@ class Plant:
     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)
 
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
   @property
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0):
+  @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
+
+    # Partial overrides are convenient for individual sensor glitches, while copying
+    # from truth ensures every field written to cereal is deterministic.
+    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.ts
+      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.ts
+          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:
+      # Preserve the historical response path exactly when no staged model is used.
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  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')
@@ -72,39 +257,48 @@ class Plant:
     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
+    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., self.distance_lead - self.distance)
+      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 > .5:
+      elif prob_lead > 0.5:
         status = True
       else:
         status = False
     else:
-      d_rel = 200.
-      v_rel = 0.
+      d_rel = 200.0
+      v_rel = 0.0
       prob_lead = 0.0
       status = False
 
-    lead = log.RadarState.LeadData.new_message()
-    lead.dRel = float(d_rel)
-    lead.yRel = 0.0
-    lead.vRel = float(v_rel)
-    lead.aRel = float(a_lead - self.acceleration)
-    lead.vLead = float(v_lead)
-    lead.vLeadK = float(v_lead)
-    lead.aLeadK = float(a_lead)
-    # TODO use real radard logic for this
-    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
-    lead.status = status
-    lead.modelProb = float(prob_lead)
-    if not self.only_lead2:
-      radar.radarState.leadOne = lead
-    radar.radarState.leadTwo = lead
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "aRel": float(a_lead - self.acceleration),
+      "vLead": float(v_lead),
+      "dPath": 0.0,
+      "vLat": 0.0,
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "fcw": False,
+      "status": 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
@@ -112,10 +306,15 @@ class Plant:
     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
-    model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
+    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
+    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)]
@@ -126,33 +325,45 @@ class Plant:
     ss.selfdriveState.experimentalMode = self.e2e
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
-    car_state.carState.vEgo = float(self.speed)
+    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., float(pitch), 0.]
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
 
     # ******** get controlsState messages for plotting ***
-    sm = {'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}
+    sm = {
+      '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.acceleration = self.planner.output_a_target
+    self.a_target = self.planner.output_a_target
+    self.actuator_command = self.a_target
     if self.planner.output_should_stop:
-      self.acceleration = min(-0.5, self.acceleration)
+      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)
+    delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
     self.speed = self.speed + self.acceleration * self.ts
     self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********
-    #print(self.distance, speed)
+    # print(self.distance, speed)
     if self.speed <= 0:
       self.speed = 0
       self.acceleration = 0
@@ -160,30 +371,64 @@ class Plant:
 
     # *** radar model ***
     if self.lead_relevancy:
-      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
     else:
-      d_rel = 200.
-      v_rel = 0.
+      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_result = getattr(self.planner, "accel_controller_result", None)
     return {
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "planner_acceleration": self.a_target,
+      "actuator_command": self.actuator_command,
+      "stop_clamped_actuator_command": self.actuator_command,
+      "delayed_actuator_command": delayed_actuator_command,
+      "applied_actuator_command": self.applied_actuator_command,
+      "vehicle_actuator_command": self.applied_actuator_command,
+      "true_v_ego": true_v_ego,
+      "true_a_ego": true_a_ego,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "observed_a_ego": published_a_ego,
+      "observed_v_ego": published_v_ego,
+      "planner_delay": self.actuator_delay,
+      "transport_delay": self.transport_delay,
+      "breakaway_confirmed": self.breakaway_confirmed,
+      "breakaway_time": self._breakaway_timer,
       "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "pace_cap": getattr(accel_controller_result, "target_speed", None),
+      "base_target": getattr(accel_controller_result, "base_speed", None),
+      "raw_energy_cap": getattr(accel_controller_result, "raw_energy_cap", None),
+      "live_filtered_cap": getattr(accel_controller_result, "live_filtered_cap", None),
+      "shadow_filtered_cap": getattr(accel_controller_result, "shadow_filtered_cap", None),
+      "accel_controller_selected_lead": getattr(accel_controller_result, "selected_lead", None),
+      "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),
     }
 
+
 # simple engage in standalone mode
 def plant_thread():
   plant = Plant()

selfdrive/test/longitudinal_maneuvers/test_plant.py

@@ -0,0 +1,80 @@
+import math
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+
+
+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,
+        "status": True,
+        "modelProb": 0.9,
+        "radarTrackId": 42,
+      }
+    return None
+
+  plant = Plant(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 = Plant(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 "pace_cap" in first
+  assert "raw_energy_cap" in first
+  assert "live_filtered_cap" 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 = Plant(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):
+    Plant(actuator_delay=delay, actuator_lag=lag)

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)

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()

selfdrive/ui/mici/widgets/button.py

@@ -382,13 +382,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):

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

@@ -0,0 +1,8 @@
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  AccelController,
+  AccelControllerResult,
+  AccelControllerState,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import AccelProfile
+
+__all__ = ["AccelController", "AccelControllerResult", "AccelControllerState", "AccelProfile"]

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

@@ -0,0 +1,523 @@
+#!/usr/bin/env python3
+from collections import deque
+from dataclasses import dataclass, field
+from enum import IntEnum
+import math
+
+import numpy as np
+
+from cereal import log
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.common.realtime import DT_MDL
+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_personality.constants import (
+  ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, APPROACH_CLOSING_SPEED, APPROACH_LEAD_DECEL, APPROACH_LEAD_SPEED_MARGIN, APPROACH_MIN_SPEED,
+  BRAKE_CAP_MARGIN, CAP_FILTER_FRAMES, CAP_RELAX_JERK, CAP_TIGHTEN_JERK, COAST_MATCH_CLOSING_SPEED, COAST_MATCH_USABLE_GAP,
+  DROPOUT_ACTION_ACCEL_MARGIN, HORIZON_DOWN_JERK, HORIZON_HOLD_TIME, HORIZON_SPEED_BUDGET, HORIZON_UP_JERK, MAX_LEAD_ACCEL_TAU,
+  MIN_LEAD_SPEED, POSITIVE_MPC_HEADROOM, PROFILE_CONFIGS, PROFILE_TRANSITION_JERK, RADAR_STALE_TIMEOUT, RELIEF_CAP_MARGIN,
+  RELIEF_CONFIRM_FRAMES, RELIEF_LEAD_SPEED_STEP, RELIEF_MPC_JERK, REQUIRED_DECEL_MARGIN, ROUTINE_DECEL_MAX, STOP_HOLD_EGO_SPEED,
+  STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
+  STOPPED_LEAD_SPEED, URGENT_CLOSING_SPEED, URGENT_RELEASE_ACCEL, URGENT_REQUIRED_DECEL, URGENT_TTC, URGENT_TTC_MIN_CLOSING,
+  VEGO_NOISE_TOLERANCE, AccelProfile,
+)
+
+
+class AccelControllerState(IntEnum):
+  inactive = 0
+  free = 1
+  restrict = 2
+  hold = 3
+  release = 4
+  stopHold = 5
+
+
+@dataclass(frozen=True)
+class EnergyEnvelope:
+  cap: float = math.inf
+  selected_lead: int = -1
+  selected_lead_speed: float = math.inf
+  selected_lead_decel: float = 0.0
+  departure_lead_speed: float = math.inf
+  usable_gap: float = math.inf
+  closing_speed: float = 0.0
+  required_decel: float = 0.0
+  has_nearly_stopped_lead: bool = False
+  lead_status: bool = False
+
+
+@dataclass(frozen=True)
+class AccelControllerResult:
+  target_speed: float
+  enabled: bool
+  active: bool
+  shadow_active: bool
+  launching: bool
+  stock_mode: bool
+  profile: AccelProfile
+  profile_accel_max: float
+  positive_accel_max: float
+  effective_accel_max: float
+  mpc_accel_max: tuple[float, ...] | None
+  state: AccelControllerState
+  shadow_state: AccelControllerState
+  base_speed: float
+  raw_energy_cap: float
+  live_filtered_cap: float
+  shadow_filtered_cap: float
+  selected_lead: int
+  selected_lead_speed: float
+  usable_gap: float
+  closing_speed: float
+  required_decel: float
+
+
+@dataclass
+class _ControllerPath:
+  cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
+  required_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
+  lead_decel_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
+  bound: float | None = None
+  state: AccelControllerState = AccelControllerState.inactive
+  relief_frames: int = 0
+  bound_relief_frames: int = 0
+  departure_frames: int = 0
+  stale_frames: int = 0
+  urgent: bool = False
+  urgent_severe: bool = False
+  urgent_safe_frames: int = 0
+  departing_from_stop: bool = False
+  previous_lead_speed: float | None = None
+  lead_speed_relief: bool = False
+
+  @property
+  def filtered_cap(self) -> float:
+    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def robust_required_decel(self) -> float:
+    return float(np.median(self.required_samples)) if self.required_samples else 0.0
+
+  @property
+  def robust_lead_decel(self) -> float:
+    return float(np.median(self.lead_decel_samples)) if self.lead_decel_samples else 0.0
+
+  def reset(self) -> None:
+    self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
+    self.required_samples.clear()
+    self.lead_decel_samples.clear()
+    self.bound = None
+    self.state = AccelControllerState.inactive
+    self.relief_frames = 0
+    self.bound_relief_frames = 0
+    self.departure_frames = 0
+    self.stale_frames = 0
+    self.urgent = False
+    self.urgent_severe = False
+    self.urgent_safe_frames = 0
+    self.departing_from_stop = False
+    self.previous_lead_speed = None
+    self.lead_speed_relief = False
+
+
+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.CP = CP
+    self.dt = dt
+    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
+    self.live = _ControllerPath()
+    self.shadow = _ControllerPath()
+
+  @staticmethod
+  def _profile(profile: int | AccelProfile) -> AccelProfile:
+    try:
+      return AccelProfile(profile)
+    except (TypeError, ValueError):
+      return AccelProfile.normal
+
+  @classmethod
+  def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float:
+    if not math.isfinite(v_ego):
+      return math.nan
+    selected_profile = cls._profile(profile)
+    return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[selected_profile]))
+
+  def _delay(self) -> float:
+    try:
+      return float(self.CP.longitudinalActuatorDelay) + DT_MDL
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      return math.nan
+
+  @staticmethod
+  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)
+
+  @staticmethod
+  def _lead_values(lead) -> tuple[float, float, float, float] | None:
+    try:
+      if not lead.status:
+        return None
+      d_rel = float(lead.dRel)
+      v_lead = float(lead.vLeadK)
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      return None
+    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
+
+    try:
+      a_lead = float(lead.aLeadK)
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      a_lead = 0.0
+    if not math.isfinite(a_lead):
+      a_lead = 0.0
+
+    try:
+      a_lead_tau = float(lead.aLeadTau)
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      a_lead_tau = _LEAD_ACCEL_TAU
+    if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
+      a_lead_tau = _LEAD_ACCEL_TAU
+    return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
+
+  def calculate_energy_envelope(self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile,
+                                follow_personality=log.LongitudinalPersonality.standard) -> EnergyEnvelope:
+    delay = self._delay()
+    if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
+      return EnergyEnvelope()
+
+    try:
+      leads = (radar_state.leadOne, radar_state.leadTwo)
+      lead_status = any(bool(lead.status) for lead in leads)
+    except (AttributeError, TypeError, ValueError):
+      return EnergyEnvelope()
+
+    try:
+      t_follow = get_T_FOLLOW(follow_personality)
+    except (NotImplementedError, TypeError, ValueError):
+      t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard)
+    if not math.isfinite(t_follow) or t_follow < 0.0:
+      return EnergyEnvelope(lead_status=lead_status)
+
+    x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
+    comfort_decel = PROFILE_CONFIGS[self._profile(profile)].comfort_decel
+    candidates: list[EnergyEnvelope] = []
+    departure_candidates: list[tuple[float, float]] = []
+    for lead_index, lead in enumerate(leads):
+      values = self._lead_values(lead)
+      if values is None:
+        continue
+      try:
+        d_rel, 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]))
+        usable_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 usable_gap == 0.0 else closing_speed**2 / (2.0 * usable_gap)
+        cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
+        departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
+      except (FloatingPointError, OverflowError, TypeError, ValueError):
+        continue
+      finite_values = (x_lead, v_lead_delay, usable_gap, closing_speed, cap, 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:
+        continue
+      candidates.append(EnergyEnvelope(cap, lead_index, v_lead_delay, max(-a_lead, 0.0), v_lead_delay, usable_gap, closing_speed,
+                                       required_decel, lead_status=lead_status))
+      departure_candidates.append((departure_distance, v_lead_delay))
+
+    if not candidates:
+      return EnergyEnvelope(lead_status=lead_status)
+    selected = min(candidates, key=lambda candidate: candidate.cap)
+    departure_lead_speed = min(departure_candidates, key=lambda candidate: candidate[0])[1]
+    return EnergyEnvelope(selected.cap, selected.selected_lead, selected.selected_lead_speed, selected.selected_lead_decel,
+                          departure_lead_speed, selected.usable_gap, selected.closing_speed, selected.required_decel,
+                          departure_lead_speed < STOPPED_LEAD_SPEED, lead_status)
+
+  @staticmethod
+  def _move(value: float, target: float, rate: float, dt: float) -> float:
+    return float(np.clip(target, value - rate * dt, value + rate * dt))
+
+  @staticmethod
+  def _ttc(envelope: EnergyEnvelope) -> float:
+    return envelope.usable_gap / envelope.closing_speed if envelope.closing_speed > 0.0 else math.inf
+
+  def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
+    has_lead = envelope.selected_lead >= 0
+    path.lead_speed_relief = (has_lead and path.previous_lead_speed is not None
+                              and envelope.selected_lead_speed > path.previous_lead_speed + RELIEF_LEAD_SPEED_STEP)
+    path.previous_lead_speed = envelope.selected_lead_speed if has_lead else None
+    path.cap_samples.append(envelope.cap if has_lead else math.inf)
+    if has_lead:
+      if math.isfinite(envelope.required_decel):
+        path.required_samples.append(envelope.required_decel)
+      if math.isfinite(envelope.selected_lead_decel):
+        path.lead_decel_samples.append(envelope.selected_lead_decel)
+    else:
+      path.required_samples.append(0.0)
+      path.lead_decel_samples.append(0.0)
+
+  def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float, action_accel: float,
+                   positive_accel_max: float, profile: AccelProfile, previous_should_stop: bool) -> bool:
+    self._update_samples(path, envelope)
+    has_lead = envelope.selected_lead >= 0
+    filtered_cap = path.filtered_cap
+    robust_required = path.robust_required_decel
+    robust_lead_decel = path.robust_lead_decel
+    ttc = self._ttc(envelope)
+    moving_away = (has_lead and not envelope.has_nearly_stopped_lead
+                   and envelope.selected_lead_speed > v_ego + APPROACH_CLOSING_SPEED
+                   and envelope.cap > v_ego + RELIEF_CAP_MARGIN)
+    stop_hold = (v_ego < STOP_HOLD_EGO_SPEED
+                 and ((previous_should_stop and not path.departing_from_stop)
+                      or (has_lead and (envelope.has_nearly_stopped_lead or envelope.cap < 0.50))))
+
+    if path.state == AccelControllerState.stopHold:
+      path.bound_relief_frames = 0
+      departed = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_EXIT_SPEED and envelope.cap > STOP_HOLD_EXIT_SPEED)
+                  or not has_lead)
+      path.departure_frames = path.departure_frames + 1 if departed else 0
+      path.bound = 0.0
+      if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
+        return False
+      path.state = AccelControllerState.free
+      path.bound = positive_accel_max
+      path.departure_frames = 0
+      path.departing_from_stop = True
+      return False
+
+    if stop_hold:
+      path.state = AccelControllerState.stopHold
+      path.bound = 0.0
+      path.relief_frames = 0
+      path.bound_relief_frames = 0
+      path.departure_frames = 0
+      path.urgent = False
+      path.urgent_severe = False
+      path.urgent_safe_frames = 0
+      path.departing_from_stop = False
+      return False
+
+    if path.departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED:
+      path.departing_from_stop = False
+
+    urgent_closing = envelope.closing_speed > URGENT_TTC_MIN_CLOSING
+    raw_urgent = (has_lead and v_ego >= STOP_HOLD_EGO_SPEED
+                  and (envelope.closing_speed >= URGENT_CLOSING_SPEED
+                       or (urgent_closing and envelope.required_decel >= URGENT_REQUIRED_DECEL)
+                       or (urgent_closing and ttc <= URGENT_TTC)))
+    if raw_urgent:
+      path.urgent = True
+      path.urgent_severe |= envelope.closing_speed >= URGENT_CLOSING_SPEED or envelope.required_decel >= URGENT_REQUIRED_DECEL
+      path.urgent_safe_frames = 0
+      path.bound = None
+      path.state = AccelControllerState.hold
+      path.relief_frames = 0
+      path.bound_relief_frames = 0
+      return True
+
+    if path.urgent:
+      matched = has_lead and envelope.closing_speed <= APPROACH_CLOSING_SPEED and robust_lead_decel <= 0.05
+      urgent_safe = (not has_lead or moving_away or matched) and (not path.urgent_severe or action_accel >= URGENT_RELEASE_ACCEL)
+      path.urgent_safe_frames = path.urgent_safe_frames + 1 if urgent_safe else 0
+      if path.urgent_safe_frames < RELIEF_CONFIRM_FRAMES:
+        path.bound = None
+        path.state = AccelControllerState.hold
+        path.bound_relief_frames = 0
+        return True
+      path.urgent = False
+      path.urgent_severe = False
+      path.urgent_safe_frames = 0
+      if not has_lead or moving_away:
+        path.state = AccelControllerState.free
+        path.bound = min(action_accel, 0.0)
+      else:
+        path.state = AccelControllerState.hold
+        path.bound = 0.0
+
+    if path.state == AccelControllerState.inactive and has_lead and not math.isfinite(filtered_cap):
+      path.bound = min(action_accel, 0.0)
+      path.bound_relief_frames = 0
+      return False
+
+    dropout_guard = (not has_lead and math.isfinite(filtered_cap)
+                     and path.state in (AccelControllerState.restrict, AccelControllerState.hold) and path.bound is not None)
+    if dropout_guard:
+      path.bound = min(path.bound, action_accel + DROPOUT_ACTION_ACCEL_MARGIN)
+
+    profile_config = PROFILE_CONFIGS[profile]
+    lead_demand = (envelope.closing_speed > APPROACH_CLOSING_SPEED
+                   or (robust_lead_decel > APPROACH_LEAD_DECEL
+                       and envelope.selected_lead_speed < v_ego + APPROACH_LEAD_SPEED_MARGIN))
+    braking_zone = filtered_cap < v_ego + BRAKE_CAP_MARGIN
+    anticipation = filtered_cap < base_speed - profile_config.anticipation_margin
+    approach = (has_lead and (v_ego > APPROACH_MIN_SPEED or path.state == AccelControllerState.restrict)
+                and lead_demand and (braking_zone or anticipation))
+    retaining_lead = path.state in (AccelControllerState.restrict, AccelControllerState.hold) and has_lead and not moving_away
+    if approach or retaining_lead:
+      entering = path.state not in (AccelControllerState.restrict, AccelControllerState.hold)
+      if path.bound is None or entering:
+        path.bound = action_accel
+      matched = envelope.closing_speed <= APPROACH_CLOSING_SPEED and robust_lead_decel <= 0.05
+      coast_cap = envelope.selected_lead_speed + math.sqrt(2.0 * profile_config.comfort_decel * COAST_MATCH_USABLE_GAP)
+      coast_to_match = (robust_lead_decel <= 0.05 and envelope.closing_speed <= COAST_MATCH_CLOSING_SPEED
+                        and filtered_cap > coast_cap)
+      if matched or coast_to_match:
+        target_decel = 0.0
+      elif braking_zone:
+        target_decel = min(max(robust_required + REQUIRED_DECEL_MARGIN, robust_lead_decel), ROUTINE_DECEL_MAX)
+      else:
+        target_decel = profile_config.glide_decel
+      target = -target_decel
+      bound_relief = has_lead and path.bound < 0.0 and target > path.bound + 1e-9
+      path.bound_relief_frames = path.bound_relief_frames + 1 if bound_relief else 0
+      if bound_relief and path.bound_relief_frames < RELIEF_CONFIRM_FRAMES:
+        target = path.bound
+      path.bound = self._move(path.bound, target, CAP_RELAX_JERK if target > path.bound else CAP_TIGHTEN_JERK, self.dt)
+      path.state = AccelControllerState.hold if matched or coast_to_match else AccelControllerState.restrict
+      path.relief_frames = 0
+      return False
+
+    if path.state in (AccelControllerState.restrict, AccelControllerState.hold):
+      path.bound_relief_frames = 0
+      relief = not has_lead or moving_away
+      path.relief_frames = path.relief_frames + 1 if relief else 0
+      path.bound = min(path.bound if path.bound is not None else action_accel, 0.0)
+      if path.relief_frames < RELIEF_CONFIRM_FRAMES:
+        path.state = AccelControllerState.hold
+        return False
+      path.state = AccelControllerState.free
+      path.relief_frames = 0
+      return False
+
+    if path.bound is None:
+      path.bound = positive_accel_max
+    else:
+      path.bound = self._move(path.bound, positive_accel_max, PROFILE_TRANSITION_JERK, self.dt)
+    path.bound_relief_frames = 0
+    path.state = AccelControllerState.free
+    return False
+
+  @staticmethod
+  def _build_accel_ceiling(bound: float, v_ego: float, planner_accel: float, action_time: float) -> tuple[float, ...] | None:
+    if bound >= ACCEL_MAX - 1e-9:
+      return None
+    a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
+    if bound > 0.0:
+      ceiling = np.full(len(T_IDXS), min(bound + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
+    elif bound == 0.0:
+      ceiling = np.maximum(0.0, a0 - HORIZON_DOWN_JERK * T_IDXS)
+    else:
+      descent = np.maximum(bound, a0 - HORIZON_DOWN_JERK * T_IDXS)
+      reach_time = max((a0 - bound) / HORIZON_DOWN_JERK, 0.0)
+      release_time = max(action_time + HORIZON_HOLD_TIME, reach_time + HORIZON_HOLD_TIME)
+      recovery = np.clip(bound + HORIZON_UP_JERK * np.maximum(T_IDXS - release_time, 0.0), bound, 0.0)
+      ceiling = np.where(T_IDXS <= release_time, descent, np.maximum(descent, recovery))
+      budget = HORIZON_SPEED_BUDGET * max(v_ego, 0.0)
+      negative_area = float(np.trapezoid(-np.minimum(ceiling, 0.0), T_IDXS))
+      if negative_area > budget and negative_area > 1e-9:
+        ceiling = np.where(ceiling < 0.0, ceiling * budget / negative_area, ceiling)
+    ceiling = np.clip(ceiling, ACCEL_MIN, ACCEL_MAX)
+    ceiling[0] = max(ceiling[0], a0)
+    return tuple(float(value) for value in ceiling)
+
+  @staticmethod
+  def _valid_context(base_speed: float, v_ego: float, a_ego: float, planner_accel: float, action_accel: float,
+                     positive_accel_max: float, delay: float, engaged: bool, cruise_initialized: bool, controller_fault: bool) -> bool:
+    values = (base_speed, v_ego, a_ego, planner_accel, action_accel, positive_accel_max, delay)
+    return (engaged and cruise_initialized and not controller_fault and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
+            and delay >= 0.0 and all(math.isfinite(value) for value in values))
+
+  def _update_freshness(self, path: _ControllerPath, radar_fresh: bool) -> bool:
+    if radar_fresh:
+      path.stale_frames = 0
+      return True
+    path.stale_frames += 1
+    if path.stale_frames < self.radar_stale_frames and path.bound is not None:
+      return False
+    path.reset()
+    return False
+
+  def reset(self) -> None:
+    self.live.reset()
+    self.shadow.reset()
+
+  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality,
+             enabled: bool, acc_selected: bool, engaged: bool, cruise_initialized: bool, planner_accel: float, action_accel: float,
+             stock_accel_max: float, previous_should_stop: bool, controller_fault: bool = False,
+             radar_fresh: bool = True) -> AccelControllerResult:
+    selected_profile = self._profile(profile)
+    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
+    profile_accel_max = self.get_profile_accel_max(selected_profile, sanitized_v_ego)
+    try:
+      stock_accel_max = float(stock_accel_max)
+    except (OverflowError, TypeError, ValueError):
+      stock_accel_max = math.nan
+    positive_accel_max = (max(0.0, min(profile_accel_max, stock_accel_max, ACCEL_MAX))
+                          if math.isfinite(profile_accel_max) and math.isfinite(stock_accel_max) else math.nan)
+    valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_accel, action_accel, positive_accel_max,
+                                        self._delay(), engaged, cruise_initialized, controller_fault)
+    envelope = (self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality)
+                if valid_context and radar_fresh else EnergyEnvelope(lead_status=self._radar_has_lead(radar_state)))
+
+    shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if valid_context else False
+    if valid_context and radar_fresh:
+      self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, action_accel, positive_accel_max,
+                        selected_profile, previous_should_stop)
+      shadow_active = True
+    elif valid_context and not shadow_fresh and self.shadow.bound is not None:
+      shadow_active = True
+    else:
+      self.shadow.reset()
+      shadow_active = False
+
+    live_context = valid_context and bool(enabled) and bool(acc_selected)
+    live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False
+    if live_context and radar_fresh:
+      stock_mode = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, action_accel,
+                                     positive_accel_max, selected_profile, previous_should_stop)
+      live_active = True
+    elif live_context and not live_fresh and self.live.bound is not None:
+      stock_mode = False
+      live_active = True
+    else:
+      self.live.reset()
+      stock_mode = False
+      live_active = False
+
+    if live_active and not stock_mode and self.live.bound is not None:
+      effective_accel_max = float(np.clip(self.live.bound, ACCEL_MIN, ACCEL_MAX))
+      if self.live.bound_relief_frames and self.live.lead_speed_relief:
+        effective_accel_max = min(effective_accel_max, action_accel + RELIEF_MPC_JERK * self.dt)
+      mpc_accel_max = self._build_accel_ceiling(effective_accel_max, sanitized_v_ego, planner_accel, self._delay())
+    else:
+      effective_accel_max = math.inf
+      mpc_accel_max = None
+
+    return AccelControllerResult(
+      target_speed=0.0 if live_active and self.live.state == AccelControllerState.stopHold else base_speed,
+      enabled=bool(enabled), active=live_active, shadow_active=shadow_active,
+      launching=live_active and self.live.departing_from_stop, stock_mode=stock_mode, profile=selected_profile,
+      profile_accel_max=profile_accel_max if live_active else math.inf,
+      positive_accel_max=positive_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max,
+      mpc_accel_max=mpc_accel_max,
+      state=self.live.state, shadow_state=self.shadow.state, base_speed=base_speed, raw_energy_cap=envelope.cap,
+      live_filtered_cap=self.live.filtered_cap if live_active else math.inf,
+      shadow_filtered_cap=self.shadow.filtered_cap if shadow_active else math.inf, selected_lead=envelope.selected_lead,
+      selected_lead_speed=envelope.selected_lead_speed, usable_gap=envelope.usable_gap,
+      closing_speed=envelope.closing_speed, required_decel=envelope.required_decel,
+    )
+
+  @staticmethod
+  def _radar_has_lead(radar_state) -> bool:
+    try:
+      return bool(radar_state.leadOne.status or radar_state.leadTwo.status)
+    except (AttributeError, TypeError, ValueError):
+      return True

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

@@ -0,0 +1,67 @@
+from dataclasses import dataclass
+from enum import IntEnum
+
+
+class AccelProfile(IntEnum):
+  eco = 0
+  normal = 1
+  sport = 2
+
+
+@dataclass(frozen=True)
+class ProfileConfig:
+  comfort_decel: float
+  anticipation_margin: float
+  glide_decel: float
+
+
+PROFILE_CONFIGS = {
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, anticipation_margin=0.15, glide_decel=0.12),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.35, anticipation_margin=1.00, glide_decel=0.16),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, anticipation_margin=2.00, glide_decel=0.20),
+}
+
+ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
+ACCEL_PROFILE_MAX_V = {
+  AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20],
+  AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35],
+  AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60],
+}
+
+CAP_FILTER_FRAMES = 5
+RELIEF_CONFIRM_FRAMES = 5
+STOP_HOLD_EXIT_FRAMES = 4
+STOP_HOLD_EGO_SPEED = 0.30
+STOPPED_LEAD_SPEED = 0.30
+STOP_HOLD_EXIT_SPEED = 0.80
+MPC_SEED_RISE_RATE = 6.0
+APPROACH_MIN_SPEED = 2.0
+APPROACH_CLOSING_SPEED = 0.15
+BRAKE_CAP_MARGIN = 0.50
+APPROACH_LEAD_DECEL = 0.10
+APPROACH_LEAD_SPEED_MARGIN = 0.50
+RELIEF_CAP_MARGIN = 0.35
+COAST_MATCH_CLOSING_SPEED = 2.50
+COAST_MATCH_USABLE_GAP = 4.0
+REQUIRED_DECEL_MARGIN = 0.03
+ROUTINE_DECEL_MAX = 1.0
+CAP_TIGHTEN_JERK = 0.60
+CAP_RELAX_JERK = 0.80
+RELIEF_MPC_JERK = 3.20
+RELIEF_LEAD_SPEED_STEP = 0.05
+DROPOUT_ACTION_ACCEL_MARGIN = 0.08
+PROFILE_TRANSITION_JERK = 1.50
+POSITIVE_MPC_HEADROOM = 0.02
+URGENT_CLOSING_SPEED = 12.0
+URGENT_REQUIRED_DECEL = 1.0
+URGENT_TTC = 3.2
+URGENT_TTC_MIN_CLOSING = 1.0
+URGENT_RELEASE_ACCEL = -0.20
+HORIZON_DOWN_JERK = 2.0
+HORIZON_UP_JERK = 1.0
+HORIZON_HOLD_TIME = 0.50
+HORIZON_SPEED_BUDGET = 0.80
+RADAR_STALE_TIMEOUT = 0.50
+MAX_LEAD_ACCEL_TAU = 10.0
+MIN_LEAD_SPEED = -1.0
+VEGO_NOISE_TOLERANCE = 0.10

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

@@ -0,0 +1,291 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from 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, get_T_FOLLOW
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  ACCEL_PROFILE_MAX_BP,
+  ACCEL_PROFILE_MAX_V,
+  CAP_FILTER_FRAMES,
+  HORIZON_SPEED_BUDGET,
+  POSITIVE_MPC_HEADROOM,
+  PROFILE_CONFIGS,
+  PROFILE_TRANSITION_JERK,
+  RADAR_STALE_TIMEOUT,
+  RELIEF_CONFIRM_FRAMES,
+  STOP_HOLD_EXIT_FRAMES,
+  AccelController,
+  AccelControllerState,
+  AccelProfile,
+)
+
+
+def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5):
+  return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau)
+
+
+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))
+
+
+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,
+    "planner_accel": 0.0,
+    "action_accel": 0.0,
+    "stock_accel_max": ACCEL_MAX,
+    "previous_should_stop": False,
+  }
+  args.update(overrides)
+  return controller.update(radar_state or make_radar(), **args)
+
+
+def restrictive_radar():
+  return make_radar(make_lead(status=True, d_rel=25.0, v_lead_k=8.0, a_lead_k=-0.5))
+
+
+class TestProfiles:
+  def test_lookup_table_is_explicit_and_tunable(self):
+    assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
+    assert ACCEL_PROFILE_MAX_V == {
+      AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20],
+      AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35],
+      AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60],
+    }
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  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 AccelController.get_profile_accel_max(profile, speed) == expected
+    limits = [AccelController.get_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)
+
+  @pytest.mark.parametrize("speed", [0.0, 3.0, 10.0, 25.0, 40.0])
+  def test_profile_order_is_distinct(self, speed):
+    limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
+    assert limits[0] < limits[1] < limits[2]
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_clear_road_applies_profile_immediately(self, profile):
+    result = update(make_controller(), v_ego=0.0, profile=profile)
+    expected = ACCEL_PROFILE_MAX_V[profile][0]
+    assert result.active and result.state == AccelControllerState.free
+    assert result.target_speed == result.base_speed == 25.0
+    assert result.positive_accel_max == expected
+    assert result.effective_accel_max == expected
+    if expected == ACCEL_MAX:
+      assert result.mpc_accel_max is None
+    else:
+      np.testing.assert_array_equal(result.mpc_accel_max, min(expected + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
+
+  def test_turn_or_throttle_limit_intersects_profile(self):
+    result = update(make_controller(), profile=AccelProfile.sport, stock_accel_max=0.0)
+    assert result.positive_accel_max == 0.0
+    assert result.effective_accel_max == 0.0
+    np.testing.assert_array_equal(result.mpc_accel_max, 0.0)
+
+  def test_profile_switch_changes_ceiling_without_a_step(self):
+    controller = make_controller()
+    sport = update(controller, profile=AccelProfile.sport, v_ego=10.0)
+    eco = update(controller, profile=AccelProfile.eco, v_ego=10.0)
+    assert sport.effective_accel_max > eco.effective_accel_max > eco.positive_accel_max
+    assert sport.effective_accel_max - eco.effective_accel_max == pytest.approx(PROFILE_TRANSITION_JERK * DT_MDL)
+
+  def test_invalid_profile_defaults_to_normal(self):
+    result = update(make_controller(), profile=999)
+    assert result.profile == AccelProfile.normal
+
+
+class TestEnergyEnvelope:
+  def test_relative_pace_energy_formula(self):
+    controller = make_controller()
+    lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
+    envelope = controller.calculate_energy_envelope(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, _ = controller._project_ego(10.0, 0.0, delay)
+    gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
+    expected = v_lead + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * gap)
+    assert envelope.cap == pytest.approx(expected)
+    assert envelope.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * gap))
+
+  def test_profile_order_controls_approach_timing(self):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
+    caps = [controller.calculate_energy_envelope(radar, 10.0, 0.0, profile).cap for profile in AccelProfile]
+    assert caps[0] < caps[1] < caps[2]
+
+  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))
+    envelope = make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == 1
+
+  @pytest.mark.parametrize("field,value", [("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0)])
+  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)
+    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == 0
+    assert math.isfinite(envelope.cap)
+
+  @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)
+    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == -1
+    assert envelope.lead_status
+
+  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()
+    make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert vars(lead) == before
+
+
+class TestAccelControllerState:
+  def test_five_frame_median_needs_three_restrictive_samples(self):
+    controller = make_controller()
+    results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES)]
+    assert math.isinf(results[1].live_filtered_cap)
+    assert math.isfinite(results[2].live_filtered_cap)
+
+  def test_routine_approach_builds_safe_finite_horizon_ceiling(self):
+    controller = make_controller()
+    result = None
+    for _ in range(CAP_FILTER_FRAMES):
+      result = update(controller, restrictive_radar())
+    assert result is not None and result.state == AccelControllerState.restrict
+    ceiling = np.asarray(result.mpc_accel_max)
+    assert ceiling.shape == T_IDXS.shape
+    assert np.all(np.isfinite(ceiling))
+    assert np.all((ceiling >= ACCEL_MIN) & (ceiling <= ACCEL_MAX))
+    assert ceiling[0] >= 0.0
+    assert np.min(ceiling) < -0.05 and ceiling[-1] == pytest.approx(0.0)
+    assert np.trapezoid(-np.minimum(ceiling, 0.0), T_IDXS) <= HORIZON_SPEED_BUDGET * 10.0 + 1e-9
+
+  def test_ongoing_mpc_braking_does_not_ratchet_the_controller(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      previous = update(controller, restrictive_radar())
+    result = update(controller, restrictive_radar(), action_accel=-1.2, planner_accel=-1.0)
+    assert result.effective_accel_max >= previous.effective_accel_max - 0.60 * DT_MDL - 1e-9
+
+  def test_two_dropouts_cannot_release_restriction(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      restricted = update(controller, restrictive_radar())
+    results = [update(controller) for _ in range(2)]
+    assert all(result.active and result.effective_accel_max <= 0.0 for result in results)
+    assert all(result.effective_accel_max >= restricted.effective_accel_max for result in results)
+
+  def test_relief_requires_consecutive_confirmation(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      update(controller, restrictive_radar())
+    moving_away = make_radar(make_lead(status=True, d_rel=45.0, v_lead_k=13.0))
+    early = [update(controller, moving_away) for _ in range(RELIEF_CONFIRM_FRAMES - 1)]
+    assert all(result.state == AccelControllerState.hold and result.effective_accel_max <= 0.0 for result in early)
+    released = update(controller, moving_away)
+    assert released.state == AccelControllerState.free
+    assert released.effective_accel_max <= 0.0
+    accelerating = update(controller, moving_away)
+    assert released.effective_accel_max < accelerating.effective_accel_max <= accelerating.positive_accel_max
+
+  def test_urgent_frame_uses_exact_stock_path(self):
+    urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
+    result = update(make_controller(), urgent, v_ego=20.0)
+    assert result.active and result.stock_mode
+    assert result.mpc_accel_max is None
+    assert math.isinf(result.effective_accel_max)
+
+  def test_urgent_relief_stays_stock_until_braking_has_recovered(self):
+    controller = make_controller()
+    urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
+    update(controller, urgent, v_ego=20.0)
+    result = update(controller, action_accel=-1.5, planner_accel=-1.2, v_ego=19.8)
+    assert result.stock_mode
+    assert result.mpc_accel_max is None
+    recovered = [update(controller, action_accel=0.0, planner_accel=0.0, v_ego=19.8) for _ in range(RELIEF_CONFIRM_FRAMES)]
+    assert all(sample.stock_mode for sample in recovered[:-1])
+    assert recovered[-1].state == AccelControllerState.free
+
+  def test_stop_hold_needs_four_departure_frames(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    held = update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+    assert held.state == AccelControllerState.stopHold
+    np.testing.assert_array_equal(held.mpc_accel_max, 0.0)
+
+    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
+    confirmation = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_EXIT_FRAMES)]
+    assert all(result.effective_accel_max == 0.0 for result in confirmation[:-1])
+    launched = confirmation[-1]
+    assert launched.launching and launched.state == AccelControllerState.free
+    assert launched.effective_accel_max == launched.positive_accel_max
+
+  def test_stale_radar_freezes_then_discards_live_state(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      restricted = update(controller, restrictive_radar())
+    hold_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) - 1
+    frozen = [update(controller, radar_fresh=False) for _ in range(hold_frames)]
+    assert all(result.active and result.effective_accel_max == restricted.effective_accel_max for result in frozen)
+    timed_out = update(controller, radar_fresh=False)
+    assert not timed_out.active and timed_out.mpc_accel_max is None
+
+  @pytest.mark.parametrize("override", [
+    {"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"controller_fault": True},
+  ])
+  def test_bypass_never_actuates(self, override):
+    result = update(make_controller(), restrictive_radar(), **override)
+    assert not result.active
+    assert result.target_speed == result.base_speed
+    assert result.mpc_accel_max is None
+    assert math.isinf(result.effective_accel_max)
+
+  def test_shadow_history_never_enters_live_actuation(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      shadow = update(controller, restrictive_radar(), enabled=False)
+    assert shadow.shadow_state == AccelControllerState.restrict
+    live = update(controller)
+    assert live.state == AccelControllerState.free
+    assert live.effective_accel_max > 0.0
+
+
+@pytest.mark.parametrize("v_ego", [0.0, 0.2, 0.5, 1.0, 2.0, 10.0, 40.0])
+@pytest.mark.parametrize("bound", [-3.5, -2.0, -1.0, -0.1, 0.0, 0.8, 2.0])
+def test_accel_ceiling_properties(v_ego, bound):
+  result = AccelController._build_accel_ceiling(bound, v_ego, planner_accel=0.3, action_time=0.25)
+  if bound >= ACCEL_MAX:
+    assert result is None
+    return
+  ceiling = np.asarray(result)
+  assert ceiling.shape == T_IDXS.shape
+  assert np.all(np.isfinite(ceiling))
+  assert np.all((ceiling >= ACCEL_MIN) & (ceiling <= ACCEL_MAX))
+  assert ceiling[0] >= 0.3 - 1e-9
+  if bound > 0.0:
+    np.testing.assert_array_equal(ceiling, min(bound + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
+  if bound < 0.0:
+    assert np.trapezoid(-np.minimum(ceiling, 0.0), T_IDXS) <= HORIZON_SPEED_BUDGET * v_ego + 1e-9

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

@@ -0,0 +1,146 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import custom, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def radar_state():
+  return messaging.new_message('radarState').radarState
+
+
+def test_legacy_profile_enum_keeps_toyota_importable():
+  expected = {"eco": 0, "normal": 1, "sport": 2}
+  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
+  from opendbc.car.toyota.carstate import AccelPersonality, CarState
+  assert AccelPersonality.schema.enumerants == expected
+  assert CarState.__module__ == "opendbc.car.toyota.carstate"
+
+
+def test_stock_mpc_parameters_are_unchanged():
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar_state(), 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+
+def test_positive_scalar_changes_only_acceleration_ceiling():
+  radar = radar_state()
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  stock_source = mpc.source
+  mpc.update(radar, 30.0, accel_max=0.8)
+  np.testing.assert_array_equal(mpc.params[:, 0], stock[:, 0])
+  np.testing.assert_array_equal(mpc.params[:, 1], 0.8)
+  np.testing.assert_array_equal(mpc.params[:, 2:], stock[:, 2:])
+  assert mpc.source == stock_source
+
+
+def test_negative_finite_horizon_ceiling_is_applied_exactly():
+  ceiling = np.linspace(0.2, -0.8, N + 1)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.2)
+  mpc.run = lambda: None
+  mpc.update(radar_state(), 30.0, accel_max=ceiling)
+  np.testing.assert_allclose(mpc.params[:, 1], ceiling)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+
+
+@pytest.mark.parametrize("accel_max", [np.inf, np.nan, -0.4, ACCEL_MIN, np.ones(N), "invalid"])
+def test_invalid_or_negative_scalar_limit_is_exact_stock(accel_max):
+  radar = radar_state()
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  mpc.update(radar, 30.0, accel_max=accel_max)
+  np.testing.assert_array_equal(mpc.params, stock)
+
+
+def test_custom_ceiling_keeps_raw_lead_obstacle_and_source_authoritative():
+  radar = radar_state()
+  radar.leadOne.status = True
+  radar.leadOne.dRel = 30.0
+  radar.leadOne.vLead = 5.0
+  radar.leadOne.aLeadK = 0.0
+  radar.leadOne.aLeadTau = 1.0
+  before = (radar.leadOne.dRel, radar.leadOne.vLead, radar.leadOne.aLeadK)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(20.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  stock_source = mpc.source
+  mpc.update(radar, 30.0, accel_max=np.linspace(0.0, -0.5, N + 1))
+  np.testing.assert_array_equal(mpc.params[:, 0], stock[:, 0])
+  np.testing.assert_array_equal(mpc.params[:, 2:], stock[:, 2:])
+  assert mpc.source == stock_source
+  assert (radar.leadOne.dRel, radar.leadOne.vLead, radar.leadOne.aLeadK) == before
+
+
+def test_retry_seed_is_bounded_and_nonnegative_in_speed():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.mpc = LongitudinalMpc()
+  states = []
+  planner.mpc.solver = SimpleNamespace(set=lambda _stage, field, value: states.append(np.asarray(value)) if field == 'x' else None)
+  planner.mpc.set_cur_state(0.0, ACCEL_MIN)
+  planner._seed_mpc_current_state()
+  states = np.asarray(states)
+  assert len(states) == N + 1
+  assert np.all(np.diff(states[:, 0]) >= 0.0)
+  assert np.all(states[:, 1] >= 0.0)
+  assert np.all((states[:, 2] >= ACCEL_MIN) & (states[:, 2] <= ACCEL_MAX))
+
+
+def test_last_solve_failure_survives_internal_reset():
+  mpc = LongitudinalMpc()
+  mpc.last_solution_status = 3
+  mpc.reset()
+  assert mpc.solution_status == 0
+  assert mpc.last_solution_status == 3
+
+
+def test_shadow_telemetry_publishes_controller_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_result = SimpleNamespace(
+    enabled=True, active=False, shadow_active=True, profile=AccelProfile.normal, state=AccelControllerState.inactive,
+    shadow_state=AccelControllerState.restrict, base_speed=20.0, raw_energy_cap=15.0, live_filtered_cap=np.inf,
+    shadow_filtered_cap=12.5, selected_lead=1, usable_gap=30.0, closing_speed=5.0, required_decel=0.4,
+    profile_accel_max=np.inf, effective_accel_max=np.inf,
+  )
+  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.vTargetShadow == pytest.approx(12.5)
+  assert telemetry.aMaxProfile == math.inf
+  assert telemetry.aMaxEffective == math.inf

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,10 +5,19 @@ 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 cereal import messaging, custom
 from opendbc.car import structs
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, T_IDXS
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import MPC_SEED_RISE_RATE
 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,7 +31,8 @@ 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.params = Params()
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
@@ -32,10 +42,27 @@ 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.accel_controller = AccelController(CP, dt=dt)
+    self.accel_controller_result = None
+    self.accel_controller_fault_latched = False
+
+    self._param_read_frames = max(1, int(round(0.25 / dt)))
+    self._param_frame = 0
+    self.accel_personality_enabled = False
+    self.accel_personality = int(AccelProfile.normal)
 
     self.output_v_target = 0.
     self.output_a_target = 0.
 
+  def _read_accel_controller_params(self) -> None:
+    if self._param_frame % self._param_read_frames == 0:
+      self.accel_personality_enabled = self.params.get_bool("AccelPersonalityEnabled")
+      self.accel_personality = get_sanitize_int_param(
+        "AccelPersonality", int(AccelProfile.eco), int(AccelProfile.sport), self.params,
+      )
+
+    self._param_frame += 1
+
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
     if not self.dec.active():
@@ -73,7 +100,101 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  @staticmethod
+  def _radar_fresh(sm: messaging.SubMaster) -> bool:
+    try:
+      return bool(sm.updated['radarState'] and sm.valid['radarState'] and sm.alive['radarState'])
+    except (AttributeError, KeyError, TypeError):
+      return True
+
+  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
+                              acc_selected: bool, planner_accel: float, action_accel: float, stock_accel_max: float,
+                              previous_should_stop: bool, controller_fault: bool = False) -> float:
+    self.accel_controller_result = self.accel_controller.update(
+      sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
+      profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality,
+      enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
+      planner_accel=planner_accel, action_accel=action_accel, stock_accel_max=stock_accel_max,
+      previous_should_stop=previous_should_stop, controller_fault=controller_fault, radar_fresh=self._radar_fresh(sm),
+    )
+    return self.accel_controller_result.target_speed
+
+  def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, seed=False,
+               seed_target=None, seed_rise_rate=MPC_SEED_RISE_RATE, retry_state=None) -> None:
+    if retry_state is not None:
+      self.mpc.a_prev = retry_state[0].copy()
+      self.mpc.crash_cnt = retry_state[1]
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    if seed or seed_target is not None:
+      self._seed_mpc_current_state(seed_target, seed_rise_rate)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=accel_max)
+
+  def _seed_mpc_current_state(self, accel_target=None, rise_rate=MPC_SEED_RISE_RATE) -> None:
+    target = float(np.clip(self.mpc.x0[2] if accel_target is None else accel_target, ACCEL_MIN, ACCEL_MAX))
+    desired_accel = target * np.ones(N + 1) if accel_target is None else np.minimum(self.mpc.x0[2] + rise_rate * T_IDXS, target)
+    acceleration = np.zeros(N + 1)
+    velocity = np.zeros(N + 1)
+    position = np.zeros(N + 1)
+    jerk = np.zeros(N)
+    acceleration[0] = self.mpc.x0[2]
+    velocity[0] = max(self.mpc.x0[1], 0.0)
+    position[0] = self.mpc.x0[0]
+    for idx in range(1, N + 1):
+      dt = T_IDXS[idx] - T_IDXS[idx - 1]
+      min_accel = 0.0 if velocity[idx - 1] <= 1e-3 and acceleration[idx - 1] < 0.0 else -2.0 * velocity[idx - 1] / dt - acceleration[idx - 1]
+      acceleration[idx] = np.clip(max(desired_accel[idx], min_accel), ACCEL_MIN, ACCEL_MAX)
+      jerk[idx - 1] = (acceleration[idx] - acceleration[idx - 1]) / dt
+      position[idx] = max(position[idx - 1], position[idx - 1] + velocity[idx - 1] * dt + 0.5 * acceleration[idx - 1] * dt**2
+                          + jerk[idx - 1] * dt**3 / 6.0)
+      velocity[idx] = max(0.0, velocity[idx - 1] + 0.5 * (acceleration[idx - 1] + acceleration[idx]) * dt)
+    for idx in range(N + 1):
+      self.mpc.solver.set(idx, 'x', np.array([position[idx], velocity[idx], acceleration[idx]]))
+    for idx in range(N):
+      self.mpc.solver.set(idx, 'u', np.array([jerk[idx]]))
+
+  def update_accel_controller_mpc(self, sm: messaging.SubMaster, base_v_cruise: float, mpc_v_cruise: float,
+                                  prev_accel_constraint: bool, *, reset_state: bool, cruise_initialized: bool,
+                                  planner_accel: float, previous_output_accel: float, available_accel_max: float,
+                                  previous_should_stop: bool, force_decel: bool):
+    is_e2e = self.is_e2e(sm)
+    if reset_state or not self.accel_personality_enabled:
+      self.accel_controller_fault_latched = False
+
+    self.update_accel_controller(
+      sm, base_v_cruise, engaged=not reset_state and not force_decel, cruise_initialized=cruise_initialized,
+      acc_selected=not is_e2e, planner_accel=planner_accel, action_accel=previous_output_accel,
+      stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop,
+      controller_fault=self.accel_controller_fault_latched,
+    )
+    result = self.accel_controller_result
+    controller_actuating = result.active and not result.stock_mode and not force_decel
+    accel_max = result.mpc_accel_max if controller_actuating else None
+    free_profile_limit = controller_actuating and result.state == AccelControllerState.free and result.effective_accel_max > 0.0
+    seed_target = result.effective_accel_max if free_profile_limit else None
+    custom_mpc = controller_actuating and (accel_max is not None or seed_target is not None)
+    retry_state = (self.mpc.a_prev.copy(), self.mpc.crash_cnt)
+    controller_v_cruise = min(mpc_v_cruise, result.target_speed)
+    self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, seed_target=seed_target)
+
+    finite_solution = all(np.all(np.isfinite(solution)) for solution in (self.mpc.v_solution, self.mpc.a_solution, self.mpc.j_solution))
+    custom_failed = custom_mpc and (self.mpc.last_solution_status != 0 or not finite_solution)
+    if custom_failed:
+      self.accel_controller_fault_latched = True
+      self.accel_controller.reset()
+      self._run_mpc(sm, mpc_v_cruise, prev_accel_constraint, seed=True, retry_state=retry_state)
+      self.update_accel_controller(
+        sm, base_v_cruise, engaged=not reset_state and not force_decel, cruise_initialized=cruise_initialized,
+        acc_selected=not is_e2e, planner_accel=planner_accel, action_accel=previous_output_accel,
+        stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop, controller_fault=True,
+      )
+    if custom_failed and self.mpc.last_solution_status != 0:
+      self.mpc.a_prev, self.mpc.crash_cnt = retry_state
+
+    return is_e2e
+
   def update(self, sm: messaging.SubMaster) -> None:
+    self._read_accel_controller_params()
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -95,6 +216,25 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    if self.accel_controller_result is not None:
+      result = self.accel_controller_result
+      accel_controller = longitudinalPlanSP.accelController
+      accel_controller.enabled = result.enabled
+      accel_controller.active = result.active
+      accel_controller.shadowOnly = result.shadow_active and not result.active
+      accel_controller.profile = int(result.profile)
+      accel_controller.state = int(result.state if result.active else result.shadow_state)
+      accel_controller.vTargetBase = float(result.base_speed)
+      accel_controller.vTargetRaw = float(result.raw_energy_cap)
+      accel_controller.vTargetFiltered = float(result.live_filtered_cap)
+      accel_controller.vTargetShadow = float(result.shadow_filtered_cap)
+      accel_controller.leadIndex = result.selected_lead
+      accel_controller.usableGap = float(result.usable_gap)
+      accel_controller.closingSpeed = float(result.closing_speed)
+      accel_controller.requiredDecel = float(result.required_decel)
+      accel_controller.aMaxProfile = float(result.profile_accel_max)
+      accel_controller.aMaxEffective = float(result.effective_accel_max)
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

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

@@ -0,0 +1,969 @@
+from collections.abc import Callable
+from dataclasses import dataclass
+import gc
+
+import numpy as np
+import pytest
+
+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, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+
+ROUTINE_GAP_TOLERANCE = 0.10
+
+
+@dataclass
+class ClosedLoopTrace:
+  time: np.ndarray
+  speed: np.ndarray
+  distance: np.ndarray
+  distance_lead: np.ndarray
+  a_target: np.ndarray
+  acceleration: np.ndarray
+  should_stop: np.ndarray
+  fcw: np.ndarray
+  source: list
+  active: np.ndarray
+  shadow_active: np.ndarray
+  launching: np.ndarray
+  target_speed: np.ndarray
+  stock_mode: np.ndarray
+  raw_cap: np.ndarray
+  filtered_cap: np.ndarray
+  selected_lead: np.ndarray
+  profile_accel_max: np.ndarray
+  effective_accel_max: np.ndarray
+  state: np.ndarray
+  required_decel: np.ndarray
+  controller_fault: np.ndarray
+  controller_fault_latched: np.ndarray
+  mpc_accel_max: np.ndarray
+  actuator_command: np.ndarray
+  solver_status: np.ndarray
+  solver_failures: int
+  solver_failure_times: list[float]
+
+
+def _configure_plant(plant: Plant, *, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None:
+  plant.planner.accel_personality_enabled = enabled
+  plant.planner.accel_personality = profile
+  plant.planner._read_accel_controller_params = lambda: None
+  plant.planner.dec._enabled = dec_enabled
+  plant.planner.dec._read_params = lambda: None
+
+
+def _run(
+  *,
+  duration: float,
+  controller_enabled: bool,
+  profile: int = 1,
+  v_lead: float | Callable[[float], float] = 0.0,
+  v_cruise: float = 30.0,
+  dec_enabled: bool = False,
+  **plant_kwargs,
+) -> ClosedLoopTrace:
+  gc.collect()
+  plant = Plant(**plant_kwargs)
+  _configure_plant(plant, enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled)
+  plant.v_lead_prev = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(0.0))
+  solver_failures = 0
+  solver_failure_times = []
+  original_mpc_reset = plant.planner.mpc.reset
+
+  def count_failed_solve() -> None:
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+      solver_failure_times.append(plant.current_time)
+    original_mpc_reset()
+
+  plant.planner.mpc.reset = count_failed_solve
+  rows = []
+  sources = []
+  while plant.current_time < duration:
+    lead_speed = float(v_lead) if isinstance(v_lead, (int, float)) else v_lead(plant.current_time)
+    controller_fault = plant.planner.mpc.last_solution_status != 0
+    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
+    controller = plant.planner.accel_controller_result
+    rows.append(
+      (
+        plant.current_time,
+        result["speed"],
+        result["distance"],
+        result["distance_lead"],
+        result["a_target"],
+        result["realized_acceleration"],
+        result["should_stop"],
+        result["fcw"],
+        controller.active,
+        controller.shadow_active,
+        controller.launching,
+        controller.target_speed,
+        controller.stock_mode,
+        controller.raw_energy_cap,
+        controller.live_filtered_cap,
+        controller.selected_lead,
+        controller.profile_accel_max,
+        controller.effective_accel_max,
+        controller.state,
+        controller.required_decel,
+        controller_fault,
+        plant.planner.accel_controller_fault_latched,
+        min(controller.mpc_accel_max) if controller.mpc_accel_max is not None else np.nan,
+        result["actuator_command"],
+        plant.planner.mpc.last_solution_status,
+      )
+    )
+    sources.append(result["mpc_source"])
+
+  data = np.asarray(rows, dtype=float)
+  trace = ClosedLoopTrace(
+    time=data[:, 0],
+    speed=data[:, 1],
+    distance=data[:, 2],
+    distance_lead=data[:, 3],
+    a_target=data[:, 4],
+    acceleration=data[:, 5],
+    should_stop=data[:, 6].astype(bool),
+    fcw=data[:, 7].astype(bool),
+    source=sources,
+    active=data[:, 8].astype(bool),
+    shadow_active=data[:, 9].astype(bool),
+    launching=data[:, 10].astype(bool),
+    target_speed=data[:, 11],
+    stock_mode=data[:, 12].astype(bool),
+    raw_cap=data[:, 13],
+    filtered_cap=data[:, 14],
+    selected_lead=data[:, 15].astype(int),
+    profile_accel_max=data[:, 16],
+    effective_accel_max=data[:, 17],
+    state=data[:, 18].astype(int),
+    required_decel=data[:, 19],
+    controller_fault=data[:, 20].astype(bool),
+    controller_fault_latched=data[:, 21].astype(bool),
+    mpc_accel_max=data[:, 22],
+    actuator_command=data[:, 23],
+    solver_status=data[:, 24].astype(int),
+    solver_failures=solver_failures,
+    solver_failure_times=solver_failure_times,
+  )
+  plant.planner.mpc.reset = original_mpc_reset
+  gc.collect()
+  return trace
+
+
+def _first_time_below(trace: ClosedLoopTrace, threshold: float) -> float:
+  indices = np.flatnonzero(trace.a_target <= threshold)
+  assert len(indices), f"never reached {threshold} m/s²"
+  return float(trace.time[indices[0]])
+
+
+def _sustained_time_below(trace: ClosedLoopTrace, threshold: float, *, after: float = 0.5, duration: float = 0.5) -> float:
+  required_frames = round(duration / DT_MDL)
+  below = (trace.time >= after) & (trace.a_target <= threshold)
+  sustained = np.convolve(below.astype(int), np.ones(required_frames, dtype=int), mode="valid") == required_frames
+  indices = np.flatnonzero(sustained)
+  assert len(indices), f"never sustained {threshold} m/s² for {duration} s"
+  return float(trace.time[indices[0]])
+
+
+def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray:
+  indices = np.flatnonzero(trace.time >= after)
+  assert len(indices) >= 2
+  return np.diff(trace.a_target[indices]) / DT_MDL
+
+
+def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0, min_speed: float = 0.0) -> np.ndarray:
+  filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid")
+  samples = (trace.time[2:-1] >= after) & (trace.speed[2:-1] >= min_speed)
+  return (np.diff(filtered_acceleration) / DT_MDL)[samples]
+
+
+def _has_brake_coast_brake(values: np.ndarray, brake: float = -0.8, coast: float = -0.35, frames: int = 2) -> bool:
+  phase = 0
+  for index in range(len(values) - frames + 1):
+    window = values[index : index + frames]
+    if np.all(window <= brake):
+      if phase == 2:
+        return True
+      phase = 1
+    elif phase == 1 and np.all(window >= coast):
+      phase = 2
+  return False
+
+
+def _has_propulsion_after_braking(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool:
+  braking = False
+  for index in range(len(values) - frames + 1):
+    window = values[index : index + frames]
+    if np.all(window <= brake):
+      braking = True
+    elif braking and np.all(window >= propulsion):
+      return True
+  return False
+
+
+def _has_propulsion_brake_cycle(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool:
+  phases = []
+  for index in range(len(values) - frames + 1):
+    window = values[index : index + frames]
+    phase = 1 if np.all(window >= propulsion) else -1 if np.all(window <= brake) else 0
+    if phase and (not phases or phase != phases[-1]):
+      phases.append(phase)
+      if len(phases) >= 3 and phases[-1] == phases[-3]:
+        return True
+  return False
+
+
+@pytest.mark.parametrize(
+  ("plant_kwargs", "expect_shadow"),
+  [
+    ({"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, False),
+    ({"e2e": True, "lead_relevancy": False, "speed": 20.0}, True),
+  ],
+  ids=("disengaged", "e2e-shadow"),
+)
+def test_non_actuating_modes_match_clean_base(plant_kwargs, expect_shadow):
+  common = dict(duration=2.0, v_lead=14.0, **plant_kwargs)
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0)
+  np.testing.assert_array_equal(trace.should_stop, baseline.should_stop)
+  np.testing.assert_array_equal(trace.fcw, baseline.fcw)
+  assert trace.source == baseline.source
+  assert not trace.active.any()
+  np.testing.assert_array_equal(trace.shadow_active, np.full_like(trace.active, expect_shadow))
+
+
+def test_disabled_profiles_match_clean_base():
+  common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0)
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+  for trace in traces[1:]:
+    np.testing.assert_allclose(trace.a_target, traces[0].a_target, atol=1e-6, rtol=0.0)
+    np.testing.assert_array_equal(trace.should_stop, traces[0].should_stop)
+    assert trace.source == traces[0].source
+  assert all(np.isinf(trace.effective_accel_max).all() for trace in traces)
+
+
+@pytest.mark.parametrize("lead_relevancy", (False, True), ids=("clear-road", "lead"))
+def test_force_decel_matches_controller_off(lead_relevancy):
+  common = dict(duration=2.0, force_decel=True, lead_relevancy=lead_relevancy, speed=20.0,
+                distance_lead=70.0, v_lead=14.0, profile=0)
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+  np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0)
+  np.testing.assert_array_equal(trace.should_stop, baseline.should_stop)
+  np.testing.assert_array_equal(trace.fcw, baseline.fcw)
+  assert trace.source == baseline.source
+
+
+def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority():
+  plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=0)
+  result = plant.step(v_cruise=15.0)
+  controller = plant.planner.accel_controller_result
+
+  assert controller.mpc_accel_max is not None
+  np.testing.assert_allclose(plant.planner.mpc.params[:, 1], controller.mpc_accel_max)
+  assert np.all((plant.planner.mpc.params[:, 1] >= 0.0) & (plant.planner.mpc.params[:, 1] <= ACCEL_MAX))
+  assert ACCEL_MIN <= result["a_target"] <= get_max_accel(plant.speed)
+  for _ in range(100):
+    result = plant.step(v_cruise=15.0)
+    if plant.speed >= 0.30:
+      break
+  assert plant.speed >= 0.30
+  controller = plant.planner.accel_controller_result
+  assert controller.mpc_accel_max is not None
+  np.testing.assert_allclose(plant.planner.mpc.params[:, 1], controller.mpc_accel_max)
+  assert np.all((plant.planner.mpc.params[:, 1] >= 0.0) & (plant.planner.mpc.params[:, 1] <= ACCEL_MAX))
+
+  lead_plant = Plant(lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(lead_plant, enabled=True, profile=0)
+  lead_plant.step(v_lead=0.0, v_cruise=15.0)
+  controller = lead_plant.planner.accel_controller_result
+  assert controller.target_speed == 0.0
+  np.testing.assert_array_equal(controller.mpc_accel_max, 0.0)
+  np.testing.assert_array_equal(lead_plant.planner.mpc.params[:, 1], 0.0)
+
+
+def test_clear_road_launch_is_immediate_and_profiles_separate():
+  common = dict(
+    duration=6.0,
+    controller_enabled=True,
+    lead_relevancy=False,
+    speed=0.0,
+    v_cruise=15.0,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+
+  for trace in traces:
+    positive = np.flatnonzero(trace.a_target > 0.05)
+    moving = np.flatnonzero(trace.speed > 0.01)
+    assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
+    assert len(moving) and trace.time[moving[0]] <= 1.0
+    assert np.all(trace.effective_accel_max[trace.active] > 0.0)
+    assert not np.any(trace.a_target < -0.05)
+    assert trace.solver_failures == 0
+
+  onset_times = [float(trace.time[np.flatnonzero(trace.a_target > 0.05)[0]]) for trace in traces]
+  assert max(onset_times) - min(onset_times) <= DT_MDL
+
+
+def test_profiles_have_distinct_moving_speed_preshape():
+  traces = [
+    _run(
+      duration=18.0,
+      controller_enabled=True,
+      profile=profile,
+      lead_relevancy=False,
+      speed=0.0,
+      v_cruise=30.0,
+      actuator_delay=0.15,
+      actuator_lag=0.20,
+    )
+    for profile in range(3)
+  ]
+  samples = [np.flatnonzero(trace.speed >= 10.0)[0] for trace in traces]
+  configured = [float(trace.profile_accel_max[index]) for trace, index in zip(traces, samples, strict=True)]
+  effective = [float(trace.effective_accel_max[index]) for trace, index in zip(traces, samples, strict=True)]
+  assert configured[0] < configured[1] < configured[2]
+  assert effective[0] < effective[1] < effective[2]
+  speed_grid = np.linspace(5.0, 16.0, 45)
+  moving_acceleration = [np.interp(speed_grid, trace.speed, trace.a_target) for trace in traces]
+  assert np.all(moving_acceleration[1] - moving_acceleration[0] > 0.10)
+  assert np.all(moving_acceleration[2] - moving_acceleration[1] > 0.05)
+  assert all(trace.solver_failures == 0 for trace in traces)
+
+
+def test_runtime_profile_switch_is_distinct_and_smooth():
+  plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=AccelProfile.sport)
+  while plant.speed < 10.0 and plant.current_time < 15.0:
+    plant.step(v_cruise=30.0)
+  assert plant.speed >= 10.0
+  switch_start = plant.current_time
+  rows = []
+  while plant.current_time < switch_start + 5.0:
+    elapsed = plant.current_time - switch_start
+    profile = AccelProfile.sport if elapsed < 1.0 or elapsed >= 3.0 else AccelProfile.eco
+    plant.planner.accel_personality = profile
+    result = plant.step(v_cruise=30.0)
+    controller = plant.planner.accel_controller_result
+    rows.append((plant.current_time - switch_start, profile, result["a_target"], controller.effective_accel_max,
+                 plant.planner.mpc.last_solution_status, plant.planner.accel_controller_fault_latched))
+
+  data = np.asarray(rows, dtype=float)
+  time_values, profiles, acceleration, effective_max, solver_status, fault_latched = data.T
+  settled_eco = (profiles == AccelProfile.eco) & (time_values >= 2.0) & (time_values < 3.0)
+  settled_sport = (profiles == AccelProfile.sport) & (time_values >= 4.0)
+  assert np.max(effective_max[settled_eco]) < np.min(effective_max[settled_sport])
+  assert np.mean(acceleration[settled_eco]) + 0.15 < np.mean(acceleration[settled_sport])
+  switch_window = ((time_values[1:] >= 0.5) & (time_values[1:] <= 1.5)) | ((time_values[1:] >= 2.5) & (time_values[1:] <= 3.5))
+  assert np.max(np.abs(np.diff(acceleration)[switch_window] / DT_MDL)) < 3.0
+  assert np.min(acceleration) >= -0.05
+  assert not solver_status.any()
+  assert not fault_latched.any()
+
+
+def test_clear_road_acceleration_crosses_lut_without_solver_failure():
+  trace = _run(
+    duration=12.0,
+    controller_enabled=True,
+    profile=1,
+    lead_relevancy=False,
+    speed=0.0,
+    v_cruise=22.352,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  assert np.max(trace.speed) > 10.0
+  assert trace.solver_failures == 0
+  assert np.all(trace.effective_accel_max[trace.active] > 0.0)
+
+
+def test_prius_route_model_launches_without_a_dead_pedal():
+  trace = _run(
+    duration=3.0,
+    controller_enabled=True,
+    profile=1,
+    lead_relevancy=False,
+    speed=0.0,
+    v_cruise=22.352,
+    actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+  )
+  positive = np.flatnonzero(trace.a_target > 0.05)
+  moving = np.flatnonzero(trace.speed > 0.05)
+  assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
+  assert len(moving) and trace.time[moving[0]] <= 1.0
+  assert trace.solver_failures == 0
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag"),
+  [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+def test_stopped_lead_requires_four_departure_frames_and_launches_promptly(actuator_delay, actuator_lag):
+  departure_time = 1.0
+
+  def lead_speed(current_time: float) -> float:
+    return 0.0 if current_time < departure_time else 2.0
+
+  trace = _run(
+    duration=2.5,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=6.0,
+    v_lead=lead_speed,
+    v_cruise=8.0,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+
+  first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9)
+  assert np.max(trace.speed[first_three]) < 1e-3
+  assert not trace.launching[first_three].any()
+  departure_release = np.flatnonzero((trace.time >= departure_time) & trace.launching)
+  assert len(departure_release) and trace.time[departure_release[0]] >= departure_time + 3 * DT_MDL
+  moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
+  assert len(moving) and trace.time[moving[0]] <= departure_time + 4 * DT_MDL + 1.0
+  assert np.min(trace.effective_accel_max[departure_release[0] : moving[0] + 1]) > 1.5
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= departure_time])
+  assert trace.solver_failures == 0
+
+
+def test_creeping_lead_departure_is_prompt_and_does_not_lurch():
+  departure_time = 1.0
+
+  def lead_speed(current_time: float) -> float:
+    if current_time < departure_time:
+      return 0.0
+    if current_time < departure_time + 0.5:
+      return 1.6 * (current_time - departure_time)
+    return min(2.5, 0.8 + 0.7 * (current_time - departure_time - 0.5))
+
+  def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
+
+  common = dict(
+    duration=6.0,
+    profile=0,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=3.6,
+    v_lead=lead_speed,
+    v_cruise=22.352,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+  after_departure = trace.time >= departure_time
+  lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time])
+  baseline_moving = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05))
+  moving = np.flatnonzero(after_departure & (trace.speed > 0.05))
+  assert len(baseline_moving) and len(moving)
+  assert trace.time[moving[0]] <= baseline.time[baseline_moving[0]]
+  assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20)
+  assert not _has_brake_coast_brake(trace.a_target[after_departure])
+  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3
+  assert trace.solver_failures == 0
+
+
+def test_stop_hold_ignores_two_frame_total_lead_dropout():
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    return None if 1.0 <= current_time < 1.1 else truth
+
+  trace = _run(
+    duration=2.0,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=6.0,
+    v_lead=0.0,
+    v_cruise=8.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  assert np.max(trace.speed) < 1e-3
+  assert np.max(trace.effective_accel_max[np.isfinite(trace.effective_accel_max)]) == 0.0
+  assert trace.solver_failures == 0
+
+
+def test_low_speed_stopped_lead_never_accelerates_during_stop_hold():
+  def lead_speed(current_time: float) -> float:
+    return max(0.0, 1.9 - 1.16 * current_time)
+
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if lead_name == "leadTwo":
+      return None
+    moving = lead_speed(current_time) > 0.0
+    return truth | {"vLeadK": truth["vLeadK"] if moving else -0.01, "aLeadK": -1.16 if moving else 0.0, "radarTrackId": 7, "radar": True}
+
+  common = dict(
+    duration=6.0,
+    profile=0,
+    lead_relevancy=True,
+    speed=4.5,
+    distance_lead=18.0,
+    v_lead=lead_speed,
+    v_cruise=23.056,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  urgent_demand = (trace.required_decel >= 0.45) & (trace.speed >= 0.30) & ~trace.should_stop
+  stop_hold = trace.state == int(AccelControllerState.stopHold)
+  assert urgent_demand.any() and stop_hold.any()
+  assert np.max(trace.a_target[urgent_demand]) < 0.0
+  hold_indices = np.flatnonzero(stop_hold)
+  assert np.max(trace.acceleration[stop_hold]) < 0.25
+  assert np.max(trace.speed[stop_hold]) < 0.30
+  assert trace.distance[hold_indices[-1]] - trace.distance[hold_indices[0]] < 0.05
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
+  assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE
+  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE
+  assert not trace.fcw.any()
+  assert trace.solver_failures == 0
+
+
+def test_moving_lead_dropout_and_false_relief_do_not_release_pace():
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if 2.0 <= current_time < 2.1:
+      return None
+    if 3.0 <= current_time < 3.1:
+      return {"dRel": truth["dRel"] + 5.0}
+    return truth
+
+  common = dict(
+    duration=5.0,
+    lead_relevancy=True,
+    speed=22.0,
+    distance_lead=85.0,
+    v_lead=14.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  trace = _run(controller_enabled=True, **common)
+  for start in (2.0, 3.0):
+    before = trace.effective_accel_max[np.flatnonzero(trace.time < start)[-1]]
+    guard = (trace.time >= start) & (trace.time < start + 0.2) & trace.active
+    assert np.all(trace.effective_accel_max[guard] <= before + 0.02)
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
+  assert not _has_propulsion_after_braking(trace.a_target[trace.time >= 1.0])
+  assert np.max(np.abs(_command_jerk(trace, after=1.0))) < 3.0
+  assert trace.solver_failures == 0
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag"),
+  [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+def test_confirmed_finite_relief_transitions_smoothly(actuator_delay, actuator_lag):
+  def lead_speed(current_time: float) -> float:
+    return 8.0 if current_time < 5.0 else min(15.0, 8.0 + 3.5 * (current_time - 5.0))
+
+  common = dict(
+    duration=9.0,
+    profile=1,
+    lead_relevancy=True,
+    speed=12.0,
+    distance_lead=50.0,
+    v_lead=lead_speed,
+    v_cruise=20.0,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+  trace = _run(controller_enabled=True, **common)
+
+  released = np.flatnonzero((trace.time >= 5.0) & (trace.state == int(AccelControllerState.free)))
+  assert len(released)
+  reached_profile = np.flatnonzero((np.arange(len(trace.time)) >= released[0]) &
+                                   (trace.effective_accel_max >= trace.profile_accel_max - 1e-6))
+  assert len(reached_profile)
+  rising = (np.arange(len(trace.time)) >= released[0]) & (np.arange(len(trace.time)) <= reached_profile[0])
+  assert rising.any()
+  assert np.all(np.diff(trace.effective_accel_max[rising]) >= -1e-9)
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 5.0])
+  assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 5.0])
+  assert np.max(np.abs(_command_jerk(trace, after=5.0))) < 3.0
+  assert trace.solver_failures == 0
+
+
+def test_low_speed_far_lead_acquisition_does_not_fault_or_lurch():
+  acquisition_time = 5.0
+
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    return None if current_time < acquisition_time else truth
+
+  common = dict(
+    duration=8.0,
+    profile=0,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=180.0,
+    v_lead=3.0,
+    v_cruise=30.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  trace = _run(controller_enabled=True, **common)
+
+  acquired = (trace.time >= acquisition_time) & (trace.selected_lead >= 0)
+  response = trace.time >= acquisition_time
+  jerk_response = trace.time[1:] >= acquisition_time
+  assert acquired.any()
+  assert not trace.controller_fault[response].any()
+  assert not trace.solver_status.any()
+  assert not trace.controller_fault_latched.any()
+  assert trace.solver_failures == 0
+  assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
+  assert not _has_brake_coast_brake(trace.a_target[response])
+  assert not _has_propulsion_after_braking(trace.a_target[response])
+
+
+def test_alternating_range_glitch_has_bounded_jerk_and_no_reversal():
+  glitch_start = 5.0
+  glitch_end = 5.5
+
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation:
+    if glitch_start <= current_time < glitch_end:
+      frame = round(current_time / DT_MDL)
+      return truth | {"dRel": truth["dRel"] + (5.0 if frame % 2 else 0.0)}
+    return truth
+
+  common = dict(
+    duration=10.0,
+    lead_relevancy=True,
+    speed=8.0,
+    distance_lead=20.0,
+    v_lead=1.5,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  control = _run(controller_enabled=True, **common)
+  baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common)
+  trace = _run(controller_enabled=True, lead_observation_fn=observe, **common)
+  window = (trace.time[1:] >= glitch_start) & (trace.time[1:] < glitch_end)
+  assert np.max(np.abs(np.diff(trace.a_target)[window] / DT_MDL)) < 3.0
+  response = (trace.time >= glitch_start) & (trace.time < glitch_end + 1.0)
+  assert np.max(np.abs((trace.a_target - control.a_target)[response])) < 0.07
+  np.testing.assert_array_equal(trace.should_stop[response], baseline.should_stop[response])
+  np.testing.assert_array_equal(trace.fcw[response], baseline.fcw[response])
+  assert not _has_brake_coast_brake(trace.a_target[response])
+  assert not _has_propulsion_after_braking(trace.a_target[response])
+  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE
+  assert trace.solver_failures == 0
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag"),
+  [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+def test_slow_lead_approach_is_smooth_across_actuator_dynamics(actuator_delay, actuator_lag):
+  lead_speed = 10.0
+  trace = _run(
+    duration=70.0,
+    controller_enabled=True,
+    profile=1,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=100.0,
+    v_lead=lead_speed,
+    v_cruise=30.0,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+  desired_gap = STOP_DISTANCE + get_T_FOLLOW() * lead_speed
+  gap = trace.distance_lead - trace.distance
+  closing_speed = trace.speed - lead_speed
+  closing = closing_speed > 0.1
+  meaningful_closing = closing_speed > 0.3
+  settled = trace.time >= trace.time[-1] - 3.0
+  moving = (trace.time[1:] >= 0.5) & (trace.speed[1:] >= 2.0) & ~trace.should_stop[1:] & ~trace.should_stop[:-1]
+  assert np.max(np.abs(np.diff(trace.a_target)[moving] / DT_MDL)) < 3.0
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
+  assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0])
+  assert np.max(trace.a_target[meaningful_closing]) <= 0.2
+  assert np.percentile(np.abs(_filtered_realized_jerk(trace)), 95) < 0.35
+  assert np.min(trace.a_target) >= -1.1
+  assert np.min(trace.acceleration) >= -1.1
+  assert np.min(gap) >= desired_gap - 1.6
+  assert np.min(gap[closing] / closing_speed[closing]) >= 2.0
+  assert abs(np.median(trace.speed[settled]) - lead_speed) <= 0.5
+  assert desired_gap - 1.6 <= np.median(gap[settled]) <= desired_gap + 6.0
+  assert not trace.fcw.any()
+  assert not trace.should_stop.any()
+  assert not trace.solver_status.any()
+  assert not trace.controller_fault_latched.any()
+  assert trace.solver_failures == 0
+
+
+def test_decelerating_moving_lead_stays_smooth_and_safe():
+  def lead_speed(current_time: float) -> float:
+    if current_time < 2.0:
+      return 15.0
+    progress = min((current_time - 2.0) / 6.0, 1.0)
+    return 15.0 - 5.0 * (3.0 * progress**2 - 2.0 * progress**3)
+
+  common = dict(
+    duration=14.0,
+    profile=1,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=110.0,
+    v_lead=lead_speed,
+    v_cruise=30.0,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+  lead_decelerating = (trace.time >= 2.0) & (trace.time <= 8.0) & trace.active
+  settled = trace.time >= 8.0
+  assert np.any(trace.effective_accel_max[lead_decelerating] < 0.0)
+  assert not trace.should_stop.any()
+  assert np.max(np.abs(_command_jerk(trace, after=1.0))) < 4.25
+  baseline_p95 = np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)
+  trace_p95 = np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)
+  assert trace_p95 <= max(0.20, baseline_p95 + 0.02)
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
+  assert not _has_propulsion_after_braking(trace.a_target[trace.time >= 1.0])
+  assert np.max(trace.a_target[settled]) <= 0.2
+  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE
+  assert not trace.fcw.any()
+  assert trace.solver_failures == 0
+
+
+def test_severe_closing_never_delays_stock_braking_or_reduces_clearance():
+  common = dict(
+    duration=12.0,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=160.0,
+    v_lead=3.5,
+    actuator_delay=0.20,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+  for threshold in (-1.0, -2.0):
+    assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
+  baseline_gap = baseline.distance_lead - baseline.distance
+  controlled_gap = trace.distance_lead - trace.distance
+  assert np.min(controlled_gap) >= np.min(baseline_gap) - 0.02
+  baseline_closing = baseline.speed - 3.5
+  controlled_closing = trace.speed - 3.5
+  baseline_ttc = np.min(baseline_gap[baseline_closing > 0.1] / baseline_closing[baseline_closing > 0.1])
+  controlled_ttc = np.min(controlled_gap[controlled_closing > 0.1] / controlled_closing[controlled_closing > 0.1])
+  assert controlled_ttc >= baseline_ttc - 0.02
+  assert np.min(controlled_gap) > 0.0
+  onset = (trace.time[1:] > 0.5) & (trace.time[1:] < 3.0)
+  assert np.max(np.abs(np.diff(trace.a_target)[onset] / DT_MDL)) < 4.0
+  assert trace.solver_failures == 0
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag"),
+  [(0.10, 0.20), (0.15, 0.25), (0.20, 0.20), (0.25, 0.30), (0.30, 0.35)],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
+def test_far_lead_deceleration_starts_early_and_stays_smooth(profile, actuator_delay, actuator_lag):
+  common = dict(
+    duration=11.0,
+    lead_relevancy=True,
+    speed=25.0,
+    distance_lead=200.0,
+    v_lead=15.0,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, profile=profile, **common)
+  baseline_onset = _sustained_time_below(baseline, -0.10)
+  trace_onset = _sustained_time_below(trace, -0.10)
+  negative_bound = np.isfinite(trace.mpc_accel_max) & (trace.mpc_accel_max < -0.05)
+  assert negative_bound.any()
+  assert trace.time[np.flatnonzero(negative_bound)[0]] <= baseline_onset - 0.5
+  assert trace_onset <= baseline_onset - 0.5
+  assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1
+  trace_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95))
+  assert trace_p95 < 0.45
+  assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 3.0
+  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
+  assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0])
+  assert not trace.fcw.any()
+  assert not trace.solver_status.any()
+  assert not trace.controller_fault_latched.any()
+  assert trace.solver_failures == 0
+
+
+def test_far_lead_profile_order_is_monotonic():
+  traces = [
+    _run(
+      duration=6.0,
+      controller_enabled=True,
+      profile=profile,
+      lead_relevancy=True,
+      speed=25.0,
+      distance_lead=200.0,
+      v_lead=15.0,
+      actuator_delay=0.10,
+      actuator_lag=0.20,
+    )
+    for profile in range(3)
+  ]
+  bound_onsets = [
+    float(trace.time[np.flatnonzero(np.isfinite(trace.mpc_accel_max) & (trace.mpc_accel_max < -0.05))[0]])
+    for trace in traces
+  ]
+  decel_onsets = [_sustained_time_below(trace, -0.10) for trace in traces]
+  assert bound_onsets[0] <= bound_onsets[1] <= bound_onsets[2]
+  assert decel_onsets[0] <= decel_onsets[1] <= decel_onsets[2]
+  assert traces[0].raw_cap[0] < traces[1].raw_cap[0] < traces[2].raw_cap[0]
+
+
+def test_prior_stock_solver_status_does_not_disable_clear_road_controller():
+  plant = Plant(speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=1)
+  plant.step(v_cruise=15.0)
+  assert plant.planner.accel_controller_result.active
+  assert plant.planner.mpc.last_solution_status == 0
+
+  plant.planner.mpc.last_solution_status = 3
+  plant.step(v_cruise=15.0)
+  assert plant.planner.mpc.last_solution_status == 0
+  recovered = plant.planner.accel_controller_result
+  assert recovered.active
+  assert not plant.planner.accel_controller_fault_latched
+  assert np.isfinite(recovered.effective_accel_max)
+
+
+def test_prior_stock_solver_status_does_not_disable_lead_controller():
+  plant = Plant(lead_relevancy=True, speed=25.0, distance_lead=200.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=1)
+  plant.v_lead_prev = 15.0
+  for _ in range(30):
+    plant.step(v_lead=15.0, v_cruise=30.0)
+
+  assert plant.planner.accel_controller_result.effective_accel_max < 0.0
+  assert plant.planner.mpc.last_solution_status == 0
+
+  plant.planner.mpc.last_solution_status = 3
+  result = plant.step(v_lead=15.0, v_cruise=30.0)
+  assert plant.planner.mpc.last_solution_status == 0
+  assert plant.planner.accel_controller_result.active
+  assert not plant.planner.accel_controller_fault_latched
+  assert result["a_target"] <= 0.2
+
+
+@pytest.mark.parametrize(("profile", "speed", "expects_ceiling"), ((0, 10.0, True), (2, 0.0, False)), ids=("ceiling", "seed-only"))
+def test_failed_custom_solve_restores_stock_state_and_counts_fcw_once(profile, speed, expects_ceiling):
+  plant = Plant(lead_relevancy=False, speed=speed, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=profile)
+  saved_a_prev = np.full_like(plant.planner.mpc.a_prev, -0.25)
+  accepted_a_prev = np.full_like(saved_a_prev, 0.15)
+  plant.planner.mpc.a_prev = saved_a_prev.copy()
+  plant.planner.mpc.crash_cnt = 2.0
+  if not expects_ceiling:
+    plant.planner.accel_controller._build_accel_ceiling = lambda *_args: None
+  calls = []
+
+  def update_mpc(_radar_state, _v_cruise, personality, accel_max=None):
+    calls.append((personality, accel_max))
+    if len(calls) == 1:
+      plant.planner.mpc.last_solution_status = plant.planner.mpc.solution_status = 4
+      plant.planner.mpc.a_prev = np.zeros_like(saved_a_prev)
+      plant.planner.mpc.crash_cnt = 0.0
+    else:
+      np.testing.assert_array_equal(plant.planner.mpc.a_prev, saved_a_prev)
+      assert plant.planner.mpc.crash_cnt == 2.0
+      plant.planner.mpc.last_solution_status = plant.planner.mpc.solution_status = 0
+      plant.planner.mpc.a_prev = accepted_a_prev.copy()
+      plant.planner.mpc.crash_cnt += 1.0
+
+  plant.planner.mpc.update = update_mpc
+  result = plant.step(v_cruise=30.0)
+  assert len(calls) == 2
+  assert (calls[0][1] is not None) == expects_ceiling and calls[1][1] is None
+  assert plant.planner.accel_controller_fault_latched
+  assert not plant.planner.accel_controller_result.active
+  assert plant.planner.mpc.crash_cnt == 3.0
+  np.testing.assert_array_equal(plant.planner.mpc.a_prev, accepted_a_prev)
+  assert result["fcw"] == (speed > 0.0)
+
+
+@pytest.mark.parametrize("mode", ("disabled", "e2e"))
+def test_stock_solver_recovery_is_not_warm_seeded_when_controller_cannot_actuate(mode):
+  plant = Plant(lead_relevancy=False, speed=10.0, actuator_delay=0.15, actuator_lag=0.20, e2e=mode == "e2e")
+  _configure_plant(plant, enabled=mode != "disabled", profile=0)
+  plant.planner.mpc.last_solution_status = 3
+  seeds = []
+  plant.planner._seed_mpc_current_state = lambda _target=None: seeds.append(True)
+  plant.step(v_cruise=30.0)
+  assert not seeds
+
+
+@pytest.mark.parametrize("pre_frames", (1, 2))
+@pytest.mark.parametrize("mode", ("disabled", "e2e"))
+def test_early_launch_transition_returns_to_stock_without_solver_fault(pre_frames, mode):
+  plant = Plant(speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=1)
+  for _ in range(pre_frames):
+    plant.step(v_cruise=15.0)
+
+  if mode == "disabled":
+    plant.planner.accel_personality_enabled = False
+    plant.planner._read_accel_controller_params = lambda: None
+  else:
+    plant.e2e = True
+
+  for _ in range(4):
+    plant.step(v_cruise=15.0)
+    controller = plant.planner.accel_controller_result
+    assert not controller.active
+    assert controller.mpc_accel_max is None
+    assert plant.planner.mpc.last_solution_status == 0
+    np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], ACCEL_MAX)
+
+
+@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
+@pytest.mark.parametrize("mode", ("disabled", "e2e"))
+def test_launch_transition_after_crossing_standstill_threshold(profile, mode):
+  plant = Plant(speed=0.29, actuator_delay=0.15, actuator_lag=0.20)
+  _configure_plant(plant, enabled=True, profile=profile)
+  plant.acceleration = 0.5
+  plant.planner.a_desired = 0.5
+  plant.step(v_cruise=15.0)
+  assert plant.speed > 0.30
+
+  if mode == "disabled":
+    plant.planner.accel_personality_enabled = False
+    plant.planner._read_accel_controller_params = lambda: None
+  else:
+    plant.e2e = True
+
+  for _ in range(4):
+    plant.step(v_cruise=15.0)
+    controller = plant.planner.accel_controller_result
+    assert not controller.active
+    assert controller.mpc_accel_max is None
+    assert plant.planner.mpc.last_solution_status == 0
+    np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], ACCEL_MAX)

sunnypilot/sunnylink/params_metadata.json

@@ -1,4 +1,26 @@
 {
+  "AccelPersonality": {
+    "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"
+      }
+    ]
+  },
+  "AccelPersonalityEnabled": {
+    "title": "Enable Accel Controller",
+    "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -519,12 +519,6 @@
                 }
               ]
             },
-            {
-              "key": "RoadEdgeLaneChangeEnabled",
-              "widget": "toggle",
-              "title": "Block Lane Change: Road Edge Detection",
-              "description": "Blocks lane change when the model sees a road edge on the side you signal."
-            },
             {
               "key": "AutoLaneChangeBsmDelay",
               "widget": "toggle",
@@ -629,8 +623,8 @@
             {
               "key": "AccelPersonalityEnabled",
               "widget": "toggle",
-              "title": "Enable Acceleration Profiles",
-              "description": "Enables acceleration profile selection for longitudinal control.",
+              "title": "Enable Accel Controller",
+              "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
               "visibility": [
                 {
                   "type": "capability",
@@ -650,10 +644,10 @@
               "key": "AccelPersonality",
               "widget": "multiple_button",
               "title": "Acceleration Profile",
-              "description": "Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.",
+              "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
               "options": [
                 {
-                  "value": 2,
+                  "value": 0,
                   "label": "Eco"
                 },
                 {
@@ -661,7 +655,7 @@
                   "label": "Normal"
                 },
                 {
-                  "value": 0,
+                  "value": 2,
                   "label": "Sport"
                 }
               ],
@@ -2059,6 +2053,22 @@
                   "equals": true
                 }
               ]
+            },
+            {
+              "key": "PlanplusControl",
+              "widget": "option",
+              "title": "Plan Plus Controls",
+              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
+              "min": 0.0,
+              "max": 2.0,
+              "step": 0.1,
+              "enablement": [
+                {
+                  "type": "param",
+                  "key": "ShowAdvancedControls",
+                  "equals": true
+                }
+              ]
             }
           ]
         },
@@ -2226,6 +2236,50 @@
       "title": "Toyota / Lexus Settings",
       "description": "",
       "items": [
+        {
+          "key": "ToyotaAutoHold",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Prius TSS2 BSM and some tssp",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaTSS2Long",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: custom longitudinal for TSS2",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaDriveMode",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enable drive mode btn link",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
         {
           "key": "ToyotaEnforceStockLongitudinal",
           "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,19 +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: Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.
+    description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
+      and recovers more quickly.
     options:
-    - value: 2
+    - value: 0
       label: Eco
     - value: 1
       label: Normal
-    - value: 0
+    - 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)

sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -272,6 +272,22 @@ class TestKnownPanels:
     nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"}
     assert "EnforceTorqueControl" in nnlc_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):