Prevent false stop departures without damping takeoff

This reverts commit 9e68801db0.

cereal/custom.capnp

@@ -301,9 +301,19 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   struct AccelController {
     enabled @0 :Bool;
     active @1 :Bool;
-    shadowOnlyDEPRECATED @2 :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;
@@ -321,6 +331,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     }
   }
 
+  # Compatibility type for vehicle integrations that map physical drive modes
+  # onto AccelPersonality. New controller telemetry uses AccelController.Profile.
   enum AccelerationPersonality {
     eco @0;
     normal @1;

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -9,7 +9,6 @@ from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
 
 if __name__ == '__main__':  # generating code
   from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -214,11 +213,11 @@ def gen_long_ocp():
   return ocp
 
 
-class LongitudinalMpc(LongitudinalMpcSP):
+class LongitudinalMpc:
   def __init__(self, dt=DT_MDL):
-    LongitudinalMpcSP.__init__(self)
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.last_solution_status = 0
     self.reset()
     self.source = LongitudinalPlanSource.cruise
 
@@ -269,11 +268,11 @@ class LongitudinalMpc(LongitudinalMpcSP):
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
+  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, *, jerk_cost_multiplier=1.0):
     jerk_factor = get_jerk_factor(personality)
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
     cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost,
-                    LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)]
+                    jerk_factor * J_EGO_COST * jerk_cost_multiplier]
     constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
@@ -316,7 +315,7 @@ class LongitudinalMpc(LongitudinalMpcSP):
     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=None):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -348,7 +347,14 @@ class LongitudinalMpc(LongitudinalMpcSP):
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
-    LongitudinalMpcSP.apply_accel_limits(self)
+    if accel_max is not None:
+      try:
+        accel_max_trajectory = np.asarray(accel_max, dtype=float)
+      except (OverflowError, TypeError, ValueError):
+        accel_max_trajectory = np.empty(0)
+      if accel_max_trajectory.shape == (N + 1,) and np.all(np.isfinite(accel_max_trajectory)):
+        self.params[:,1] = np.clip(accel_max_trajectory, 0.0, 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
@@ -368,7 +374,7 @@ class LongitudinalMpc(LongitudinalMpcSP):
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
-    LongitudinalMpcSP.save_solution_status(self)
+    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

@@ -131,10 +131,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+    base_v_cruise = v_cruise
+
     if force_slow_decel:
       v_cruise = 0.0
 
-    is_e2e = LongitudinalPlannerSP.update_mpc(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state)
+    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, available_accel_max=accel_clip[1] if self.allow_throttle else 0.0,
+      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)
@@ -165,7 +171,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
-    self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop)
+    self.output_should_stop = self.accel_controller_should_stop(self.output_should_stop, is_e2e)
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

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,20 +17,105 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
-class PlannerSM(dict):
-  def __init__(self, radar_frame: int, services: dict):
-    super().__init__(services)
-    self.logMonoTime = {"radarState": radar_frame}
-    self.valid = {"radarState": True}
-    self.alive = {"radarState": True}
+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')
@@ -36,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
@@ -50,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'])
 
@@ -60,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')
@@ -80,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
@@ -120,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)]
@@ -134,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 = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
-          'carState': car_state.carState,
-          'carControl': car_control.carControl,
-          'controlsState': control.controlsState,
-          'selfdriveState': ss.selfdriveState,
-          'liveParameters': lp.liveParameters,
-          'modelV2': model.modelV2,
-          'carStateSP': car_state_sp.carStateSP,
-          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation})
+    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
@@ -168,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)

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

@@ -1,384 +0,0 @@
-import math
-
-import numpy as np
-
-from opendbc.car.interfaces import ACCEL_MAX
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
-from openpilot.sunnypilot import get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  CAP_FILTER_FRAMES, COMFORT_DECEL, DEPARTURE_MOTION_NOISE_FLOOR, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW,
-  LEAD_BRAKING_ACCEL_THRESHOLD, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM,
-  MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
-  MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES, SPEED_RELIEF_DEADBAND, SPEED_RESTRICT_DEADBAND, TARGET_SPEED_ARM_MARGIN,
-  TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED,
-  STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, STOP_HOLD_MAX_LEAD_DISTANCE, VEGO_NOISE_TOLERANCE, PARAM_READ_INTERVAL, AccelProfile,
-  profile_accel_max, sanitize_profile,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan, has_radar_lead, is_lead_source
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.state import AccelControllerState, TargetState
-
-
-class AccelController:
-  def __init__(self, CP, dt: float = DT_MDL):
-    if not math.isfinite(dt) or dt <= 0.0:
-      raise ValueError("dt must be finite and positive")
-
-    self.dt = dt
-    self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
-    self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
-    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
-    self.params = Params()
-    self.available = bool(CP.openpilotLongitudinalControl)
-    self.enabled = False
-    self.profile = AccelProfile.normal
-    self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / dt)))
-    self._param_frame = 0
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples: list[float] = []
-    self._required_decel_lead = -1
-    self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-    self.target_state = TargetState()
-    self._held_lead_plan: LeadPlan | None = None
-    self.is_active = self.launching = self.departure_launching = False
-    self.output_v_target = 0.0
-    self.mpc_accel_max: tuple[float, ...] | None = None
-    self.state = AccelControllerState.inactive
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0
-
-  @property
-  def is_enabled(self) -> bool:
-    return self.available and self.enabled
-
-  def update_params(self) -> None:
-    if self._param_frame % self._param_read_frames == 0:
-      self.enabled = self.params.get_bool("AccelPersonalityEnabled")
-      self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
-    self._param_frame += 1
-
-  @staticmethod
-  def _profile(profile: int) -> int:
-    return sanitize_profile(profile)
-
-  @staticmethod
-  def get_profile_accel_max(profile: int, v_ego: float) -> float:
-    return profile_accel_max(profile, v_ego)
-
-  def _update_target(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, profile: int, profile_max_accel: float,
-                     previous_should_stop: bool, previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
-    state = self.target_state
-    lead_filter_ready = state.update_samples(lead_plan, self.dt)
-    state.active_frames += 1
-    has_lead = lead_plan.selected_lead >= 0
-    filtered_cap = state.filtered_cap
-    slot_changed = has_lead and state.selected_lead >= 0 and lead_plan.selected_lead != state.selected_lead
-    track_changed = (has_lead and state.selected_lead >= 0 and lead_plan.selected_lead == state.selected_lead
-      and lead_plan.selected_lead_track_id != state.selected_lead_track_id
-      and (state.selected_lead_track_id >= 0 or lead_plan.selected_lead_track_id >= 0))
-    false_relief = has_lead and math.isfinite(filtered_cap) and lead_plan.cap >= filtered_cap + SPEED_RELIEF_DEADBAND
-    if (slot_changed or track_changed) and false_relief and state.lead_switch_guard_frames == 0 and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD:
-      state.lead_switch_guard_frames = self.lead_loss_hold_frames
-    elif state.lead_switch_guard_frames > 0:
-      state.lead_switch_guard_frames -= 1
-    if slot_changed or track_changed:
-      state.matched_lead = False
-      state.matched_accel_limit = None
-    if has_lead:
-      state.selected_lead = lead_plan.selected_lead
-      state.selected_lead_track_id = lead_plan.selected_lead_track_id
-    elif state.lead_loss_frames >= self.lead_loss_hold_frames:
-      state.lead_switch_guard_frames = 0
-      state.selected_lead = state.selected_lead_track_id = -1
-    departure_separation = (lead_plan.departure_lead_separations[lead_plan.departure_lead_index]
-                            if lead_plan.departure_lead_index >= 0 else math.inf)
-    stopped_lead_hold = (has_lead and lead_plan.has_nearly_stopped_lead
-      and (lead_plan.departure_cap < 0.50 or (state.lead_braking and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)))
-    invalid_lead = lead_plan.lead_status and not has_lead
-    prior_lead_context = is_lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or state.lead_braking
-    previous_stop = previous_should_stop and prior_lead_context and (not has_lead or lead_plan.departure_lead_speed < STOP_HOLD_EXIT_SPEED)
-    stop_evidence = stopped_lead_hold or lead_plan.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not state.launching) or invalid_lead
-    departure_motion_confirmed = (state.launching and state.departure_launch and has_lead
-      and (state.departure.progress(lead_plan, DEPARTURE_MOTION_NOISE_FLOOR) or state.departure.recent_motion()))
-    if state.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap) and has_lead and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD:
-      state.lead_braking = True
-    elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
-      state.lead_braking = False
-
-    if state.target_speed is None:
-      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
-      seed_from_ego = has_lead and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD and not e2e_handoff
-      state.target_speed = min(base_speed, v_ego) if seed_from_ego else base_speed
-      state.e2e_braking_handoff = e2e_handoff and planner_accel < 0.0
-      state.state = AccelControllerState.free
-      if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
-        state.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-        state.state = AccelControllerState.release
-        state.launching = True
-        state.departure_launch = False
-    elif state.e2e_braking_handoff and planner_accel >= 0.0:
-      state.e2e_braking_handoff = False
-
-    state.target_speed = min(state.target_speed, base_speed)
-    if v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not departure_motion_confirmed and state.state != AccelControllerState.stopHold:
-      state.enter_stop_hold(lead_plan)
-      return state.target_speed
-
-    if state.state == AccelControllerState.stopHold:
-      state.departure.backfill_references()
-      fast_departure = (has_lead and min(lead_plan.selected_lead_speed, lead_plan.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
-        and lead_plan.departure_cap > STOP_HOLD_EXIT_SPEED)
-      raw_departure = fast_departure or not lead_plan.lead_status and state.lead_loss_frames >= self.lead_loss_hold_frames
-      departed = state.departure.progress(lead_plan, STOP_HOLD_CREEP_DISTANCE) or raw_departure
-      if fast_departure and state.departure_frames == 0:
-        state.departure.keep_latest_motion_sample()
-      state.departure_frames = state.departure_frames + 1 if departed else 0
-      state.target_speed = 0.0
-      fast_departure_confirmed = fast_departure and state.departure.recent_motion()
-      if state.departure_frames < STOP_HOLD_EXIT_FRAMES or fast_departure and not fast_departure_confirmed:
-        return state.target_speed
-      state.target_speed = base_speed
-      state.state = AccelControllerState.release
-      state.departure_frames = 0
-      state.launching = True
-      state.departure_launch = has_lead
-      return state.target_speed
-
-    if state.launching:
-      renewed_stop = (has_lead and not departure_motion_confirmed
-        and (lead_plan.cap < STOP_HOLD_EXIT_SPEED
-             or (lead_plan.has_nearly_stopped_lead and lead_plan.departure_cap < STOP_HOLD_EXIT_SPEED)))
-      guarded_departure_loss = state.departure_launch and not lead_plan.lead_status and state.lead_loss_frames < self.lead_loss_hold_frames
-      if invalid_lead:
-        state.launching = state.departure_launch = False
-        if v_ego < STOP_HOLD_EGO_SPEED:
-          state.enter_stop_hold(lead_plan)
-          return state.target_speed
-        state.state = AccelControllerState.hold
-        return state.target_speed
-      if guarded_departure_loss:
-        state.state = AccelControllerState.hold
-        return state.target_speed
-      if state.departure_launch and not has_lead:
-        state.departure_launch = False
-      if renewed_stop:
-        state.launching = state.departure_launch = False
-        if v_ego < STOP_HOLD_EGO_SPEED:
-          state.enter_stop_hold(lead_plan)
-          return state.target_speed
-      if state.launching:
-        if state.departure_launch:
-          state.target_speed = base_speed
-        else:
-          launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-          state.target_speed = min(base_speed, max(state.target_speed, launch_target) + LAUNCH_TARGET_SLEW * self.dt)
-        if v_ego >= LAUNCH_END_SPEED:
-          state.launching = state.departure_launch = False
-
-    comfort_decel = COMFORT_DECEL[profile]
-    if (has_lead and not state.launching and state.state == AccelControllerState.restrict
-      and lead_plan.closing_speed <= 0.0 and v_ego >= state.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
-      state.matched_lead = True
-    elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
-      state.matched_lead = False
-
-    lost_lead_source = is_lead_source(previous_mpc_source) and not has_lead and planner_speed < state.target_speed
-    if not has_lead and (state.matched_lead or lost_lead_source):
-      if lost_lead_source:
-        state.target_speed = max(planner_speed, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
-      state.state = AccelControllerState.hold
-      return state.target_speed
-
-    if state.matched_lead:
-      if math.isfinite(state.filtered_lead_speed):
-        recovery_speed = min(base_speed, state.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * lead_plan.usable_gap))
-        desired_accel_limit = min(profile_max_accel, max(recovery_speed - v_ego, 0.0))
-      else:
-        desired_accel_limit = 0.0
-      if state.filtered_lead_accel < LEAD_BRAKING_ACCEL_THRESHOLD:
-        desired_accel_limit = profile_max_accel
-      if state.matched_accel_limit is None:
-        state.matched_accel_limit = profile_max_accel
-      if state.lead_switch_guard_frames > 0:
-        desired_accel_limit = min(desired_accel_limit, state.matched_accel_limit)
-      state.matched_accel_limit = min(profile_max_accel, float(np.clip(
-        desired_accel_limit, state.matched_accel_limit - LEAD_MATCH_ACCEL_SLEW * self.dt,
-        state.matched_accel_limit + LEAD_MATCH_ACCEL_SLEW * self.dt,
-      )))
-      matched_ceiling = min(base_speed, filtered_cap)
-      if matched_ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND:
-        state.target_speed = max(matched_ceiling, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
-        state.state = AccelControllerState.restrict
-      elif state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND:
-        state.target_speed = min(matched_ceiling, state.target_speed + profile_max_accel * self.dt)
-        state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
-      else:
-        state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
-      return state.target_speed
-    state.matched_accel_limit = None
-
-    ceiling = min(base_speed, filtered_cap)
-    if lead_filter_ready and state.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not state.launching and planner_speed < state.target_speed:
-      state.target_speed = max(planner_speed, state.target_speed - comfort_decel * self.dt)
-
-    if ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND or (state.state == AccelControllerState.restrict and ceiling < state.target_speed):
-      state.target_speed = max(ceiling, state.target_speed - comfort_decel * self.dt)
-      state.state = AccelControllerState.restrict
-      return state.target_speed
-
-    filter_warmup = has_lead and not math.isfinite(filtered_cap)
-    guarded_lead_loss = not has_lead and state.lead_loss_frames < self.lead_loss_hold_frames
-    if (filter_warmup or guarded_lead_loss) and state.target_speed < base_speed - SPEED_RESTRICT_DEADBAND:
-      state.state = AccelControllerState.hold
-      return state.target_speed
-
-    confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
-    relief = not has_lead or lead_plan.closing_speed <= 0.0
-    if relief and (ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > state.target_speed)):
-      if state.lead_switch_guard_frames == 0:
-        state.target_speed = ceiling
-      state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
-    else:
-      state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
-    return state.target_speed
-
-  def _update_freshness(self, radar_fresh: bool) -> None:
-    self.target_state.stale_frames = 0 if radar_fresh else self.target_state.stale_frames + 1
-    if self.target_state.stale_frames >= self.radar_stale_frames:
-      self.target_state = TargetState()
-
-  def reset(self) -> None:
-    self.target_state = TargetState()
-    self._held_lead_plan = None
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples.clear()
-    self._required_decel_lead = self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-    self.is_active = self.launching = self.departure_launching = False
-    self.output_v_target = 0.0
-    self.mpc_accel_max = None
-    self.state = AccelControllerState.inactive
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0
-
-  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool,
-             engaged: bool, cruise_initialized: bool, stock_accel_max: float, previous_should_stop: bool, radar_fresh: bool = True,
-             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> None:
-    self.profile = self._profile(self.profile)
-    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
-    profile_max_accel = self.get_profile_accel_max(self.profile, sanitized_v_ego)
-    stock_accel_max = float(stock_accel_max)
-    positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX))
-                          if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan)
-    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
-    valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay,
-                                     engaged, cruise_initialized)
-    enabled_context = valid_context and self.is_enabled and bool(acc_selected)
-    if enabled_context and radar_fresh:
-      lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality)
-      self._held_lead_plan = lead_plan
-    elif enabled_context and self._held_lead_plan is not None:
-      lead_plan = self._held_lead_plan
-    else:
-      lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
-      self._held_lead_plan = None
-
-    if enabled_context:
-      self._update_freshness(radar_fresh)
-    active = enabled_context and (radar_fresh or self.target_state.target_speed is not None)
-    if active and radar_fresh:
-      target_speed = self._update_target(
-        lead_plan, base_speed, sanitized_v_ego, self.profile, profile_max_accel, previous_should_stop,
-        previous_mpc_source, planner_speed, planner_accel,
-      )
-    elif active:
-      target_speed = self.target_state.target_speed
-    else:
-      self.target_state = TargetState()
-      target_speed = base_speed
-
-    if not radar_fresh and not active:
-      self._held_lead_plan = None
-      lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
-
-    state = self.target_state
-    stop_hold_active = active and state.state == AccelControllerState.stopHold
-    matched_limit_active = active and state.matched_lead and state.matched_accel_limit is not None and not state.e2e_braking_handoff
-    lead_accel_request = active and lead_plan.selected_lead >= 0 and lead_plan.closing_speed <= 0.0 and planner_accel >= 0.0
-    profile_limit_active = active and not stop_hold_active and (state.launching or not lead_plan.lead_status or lead_accel_request)
-    if matched_limit_active:
-      effective_accel_max = min(positive_accel_max, state.matched_accel_limit)
-    elif profile_limit_active:
-      effective_accel_max = positive_accel_max
-    else:
-      effective_accel_max = math.inf
-    mpc_accel_max = build_accel_ceiling(effective_accel_max, planner_accel) if matched_limit_active or profile_limit_active else None
-    guarded_lead_loss = not lead_plan.lead_status and state.selected_lead >= 0 and state.lead_loss_frames < self.lead_loss_hold_frames
-    lead_context = lead_plan.lead_status or math.isfinite(state.filtered_cap) or guarded_lead_loss
-    reserve_eligible = (active and lead_context and not stop_hold_active and state.lead_switch_guard_frames == 0 and not state.launching
-      and not state.e2e_braking_handoff)
-    if not lead_context:
-      state.speed_reserve_armed = False
-    elif (reserve_eligible and not state.speed_reserve_armed and math.isfinite(state.filtered_cap)
-      and state.filtered_cap <= target_speed + TARGET_SPEED_ARM_MARGIN):
-      state.speed_reserve_armed = True
-
-    output_target = 0.0 if stop_hold_active else target_speed
-    if reserve_eligible and state.speed_reserve_armed:
-      output_target = max(0.0, output_target - TARGET_SPEED_RESERVE)
-
-    self.is_active = active
-    self.launching = active and state.launching
-    self.departure_launching = self.launching and state.departure_launch
-    self.output_v_target = output_target
-    self.mpc_accel_max = mpc_accel_max
-    self.state = state.state
-    self.selected_lead = lead_plan.selected_lead
-    self.selected_lead_track_id = lead_plan.selected_lead_track_id
-    self.required_decel = lead_plan.required_decel
-
-  def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float, previous_mpc_failed: bool) -> float:
-    lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0
-      and not self.launching and target_reduction > 1e-6)
-    same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id
-    lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead
-    if lead_changed:
-      self._lead_trend_warmup = True
-    elif not lead_restriction:
-      self._lead_trend_warmup = False
-    if not lead_restriction or not same_lead or not math.isfinite(self.required_decel):
-      self._required_decel_samples.clear()
-    if lead_restriction and math.isfinite(self.required_decel):
-      self._required_decel_samples.append(self.required_decel)
-      if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES:
-        self._required_decel_samples.pop(0)
-    self._required_decel_lead = self.selected_lead if lead_restriction else -1
-    self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1
-
-    history = self._required_decel_samples
-    history_ready = len(history) == MPC_DECEL_TREND_FRAMES
-    tightening_lead = (history_ready
-      and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
-      and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
-    modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
-      and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
-    smoothing_eligible = modest_decel and (not self._lead_trend_warmup or history_ready) and not tightening_lead
-    if history_ready:
-      self._lead_trend_warmup = False
-    if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked and (not modest_decel or tightening_lead)):
-      self._jerk_smoothing_blocked = True
-    elif not lead_restriction:
-      self._jerk_smoothing_blocked = False
-    return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0
-
-  def update_should_stop(self, should_stop: bool) -> bool:
-    if not self.is_active:
-      return should_stop
-    if self.departure_launching:
-      return False
-    return should_stop or self.state == AccelControllerState.stopHold

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

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

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

@@ -1,147 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import math
-from typing import NamedTuple
-
-import numpy as np
-
-from cereal import log
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  LongitudinalMpc, LongitudinalPlanSource, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, get_stopped_equivalence_factor,
-)
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
-  STOP_GAP_RESERVE_LEAD_SPEED, STOPPED_LEAD_SPEED, sanitize_profile,
-)
-
-
-class LeadPlan(NamedTuple):
-  cap: float = math.inf
-  selected_lead: int = -1
-  selected_lead_track_id: int = -1
-  selected_lead_speed: float = math.inf
-  selected_lead_accel: float = 0.0
-  departure_lead_index: int = -1
-  departure_lead_speed: float = math.inf
-  departure_cap: float = math.inf
-  departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
-  departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf)
-  departure_lead_track_ids: tuple[int, int] = (-1, -1)
-  departure_lead_separations: tuple[float, float] = (-math.inf, -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
-
-
-def is_lead_source(source) -> bool:
-  return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
-
-
-def has_radar_lead(radar_state) -> bool:
-  return bool(radar_state.leadOne.status or radar_state.leadTwo.status)
-
-
-def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
-  if a_ego < 0.0:
-    stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
-    if stop_time <= delay:
-      distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
-      return distance, 0.0
-  return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
-
-
-def _lead_values(lead) -> tuple[float, float, float, float] | None:
-  if not lead.status:
-    return None
-  d_rel, v_lead = float(lead.dRel), float(lead.vLeadK)
-  if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
-    return None
-
-  a_lead = float(lead.aLeadK)
-  if not math.isfinite(a_lead):
-    a_lead = 0.0
-  a_lead_tau = float(lead.aLeadTau)
-  if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
-    a_lead_tau = _LEAD_ACCEL_TAU
-  return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
-
-
-def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
-                        follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
-  if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
-    return LeadPlan()
-
-  leads = (radar_state.leadOne, radar_state.leadTwo)
-  lead_status = any(lead.status for lead in leads)
-  t_follow = get_T_FOLLOW(follow_personality)
-  if not math.isfinite(t_follow) or t_follow < 0.0:
-    return LeadPlan(lead_status=lead_status)
-
-  profile = sanitize_profile(profile)
-  x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay)
-  comfort_decel = COMFORT_DECEL[profile]
-  candidates: list[LeadPlan] = []
-  departure_candidates: list[tuple[float, int]] = []
-  departure_speeds = [math.inf, math.inf]
-  departure_distances = [-math.inf, -math.inf]
-  departure_track_ids = [-1, -1]
-  departure_separations = [-math.inf, -math.inf]
-  departure_caps = [math.inf, math.inf]
-
-  for lead_index, lead in enumerate(leads):
-    values = _lead_values(lead)
-    if values is None:
-      continue
-
-    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]))
-    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
-    closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
-    required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
-    reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
-    reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
-    usable_gap = max(safety_gap - reserve * reserve_scale, 0.0)
-    cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
-    departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
-    separation = x_lead - x_ego
-    departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
-
-    finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_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 or not math.isfinite(separation)):
-      continue
-
-    track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
-    candidates.append(LeadPlan(
-      cap=cap, selected_lead=lead_index, selected_lead_track_id=track_id, selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
-      usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
-    ))
-    departure_candidates.append((departure_distance, lead_index))
-    departure_speeds[lead_index] = v_lead_delay
-    departure_distances[lead_index] = d_rel
-    departure_track_ids[lead_index] = track_id
-    departure_separations[lead_index] = separation
-    departure_caps[lead_index] = departure_cap
-
-  if not candidates:
-    return LeadPlan(lead_status=lead_status)
-
-  selected = min(candidates, key=lambda candidate: candidate.cap)
-  departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
-  departure_lead_speed = departure_speeds[departure_lead_index]
-  return selected._replace(
-    departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
-    departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
-    departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
-    departure_lead_separations=tuple(departure_separations), has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED,
-  )

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

@@ -1,139 +0,0 @@
-import math
-from statistics import median
-
-import numpy as np
-
-from cereal import custom
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
-
-
-AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
-
-
-class DepartureTracker:
-  def __init__(self) -> None:
-    self.samples: list[list[float]] = [[], []]
-    self.motion_samples: list[float] = []
-    self.references: list[float | None] = [None, None]
-    self.track_ids = [-1, -1]
-
-  def separation(self, lead_index: int) -> float:
-    samples = self.samples[lead_index]
-    return float(median(samples)) if samples else -math.inf
-
-  def update(self, lead_plan: LeadPlan, dt: float) -> None:
-    for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
-      if not math.isfinite(distance):
-        continue
-      samples = self.samples[lead_index]
-      track_id = lead_plan.departure_lead_track_ids[lead_index]
-      identity_changed = bool(samples) and track_id != self.track_ids[lead_index] and (track_id >= 0 or self.track_ids[lead_index] >= 0)
-      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speeds[lead_index] * dt)
-      geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
-      if identity_changed or geometry_jump:
-        samples.clear()
-        self.references[lead_index] = distance
-      samples.append(distance)
-      if len(samples) > CAP_FILTER_FRAMES:
-        samples.pop(0)
-      self.track_ids[lead_index] = track_id
-    lead_index = lead_plan.departure_lead_index
-    if lead_index >= 0:
-      distance = lead_plan.departure_lead_distances[lead_index]
-      samples = self.motion_samples
-      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speed * dt)
-      if samples and abs(distance - samples[-1]) > max_distance_step:
-        samples.clear()
-      samples.append(distance)
-      if len(samples) > CAP_FILTER_FRAMES:
-        samples.pop(0)
-
-  def seed(self, lead_plan: LeadPlan) -> None:
-    self.samples = [[], []]
-    self.motion_samples = []
-    self.references = [None, None]
-    self.track_ids = list(lead_plan.departure_lead_track_ids)
-    for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
-      if math.isfinite(distance):
-        self.samples[lead_index].append(distance)
-        self.references[lead_index] = distance
-    if lead_plan.departure_lead_index >= 0:
-      self.motion_samples.append(lead_plan.departure_lead_distances[lead_plan.departure_lead_index])
-
-  def progress(self, lead_plan: LeadPlan, minimum_distance: float) -> bool:
-    lead_index = lead_plan.departure_lead_index
-    if lead_index < 0 or lead_plan.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
-      return False
-    reference = self.references[lead_index]
-    distance = self.separation(lead_index)
-    return reference is not None and distance - reference >= minimum_distance
-
-  def recent_motion(self) -> bool:
-    samples = self.motion_samples[-STOP_HOLD_EXIT_FRAMES:]
-    if len(samples) < STOP_HOLD_EXIT_FRAMES:
-      return False
-    deltas = np.diff(samples)
-    return bool(samples[-1] - samples[0] >= DEPARTURE_MOTION_NOISE_FLOOR and np.count_nonzero(deltas > DEPARTURE_MOTION_STEP_MIN) >= 2)
-
-  def backfill_references(self) -> None:
-    for lead_index in range(len(self.references)):
-      separation = self.separation(lead_index)
-      if math.isfinite(separation) and self.references[lead_index] is None:
-        self.references[lead_index] = separation
-
-  def keep_latest_motion_sample(self) -> None:
-    if self.motion_samples:
-      self.motion_samples = self.motion_samples[-1:]
-
-
-class TargetState:
-  def __init__(self) -> None:
-    self.cap_samples = [math.inf] * CAP_FILTER_FRAMES
-    self.lead_speed_samples = [math.inf] * CAP_FILTER_FRAMES
-    self.lead_accel_samples = [0.0] * CAP_FILTER_FRAMES
-    self.departure = DepartureTracker()
-    self.target_speed: float | None = None
-    self.state = AccelControllerState.inactive
-    self.departure_frames = self.active_frames = self.lead_loss_frames = 0
-    self.lead_switch_guard_frames = self.stale_frames = 0
-    self.selected_lead = self.selected_lead_track_id = -1
-    self.launching = self.departure_launch = self.matched_lead = False
-    self.lead_braking = self.e2e_braking_handoff = self.speed_reserve_armed = False
-    self.matched_accel_limit: float | None = None
-
-  @property
-  def filtered_cap(self) -> float:
-    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
-
-  @property
-  def filtered_lead_speed(self) -> float:
-    return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2]
-
-  @property
-  def filtered_lead_accel(self) -> float:
-    return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2]
-
-  def update_samples(self, lead_plan: LeadPlan, dt: float) -> bool:
-    had_filtered_lead = math.isfinite(self.filtered_cap)
-    has_lead = lead_plan.selected_lead >= 0
-    self.cap_samples.append(lead_plan.cap if has_lead else math.inf)
-    self.lead_speed_samples.append(lead_plan.selected_lead_speed if has_lead else math.inf)
-    self.lead_accel_samples.append(lead_plan.selected_lead_accel if has_lead else 0.0)
-    self.cap_samples.pop(0)
-    self.lead_speed_samples.pop(0)
-    self.lead_accel_samples.pop(0)
-    self.lead_loss_frames = 0 if has_lead else self.lead_loss_frames + 1
-    self.departure.update(lead_plan, dt)
-    return not had_filtered_lead and math.isfinite(self.filtered_cap)
-
-  def enter_stop_hold(self, lead_plan: LeadPlan) -> None:
-    self.departure.seed(lead_plan)
-    self.target_speed = 0.0
-    self.state = AccelControllerState.stopHold
-    self.departure_frames = 0
-    self.launching = self.departure_launch = False
-    self.matched_lead = self.speed_reserve_armed = False
-    self.matched_accel_limit = 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,734 @@
+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, LongitudinalPlanSource, 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_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD, CAP_FILTER_FRAMES,
+  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW,
+  LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_TAPER_GAIN, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU,
+  MIN_LEAD_SPEED, PACE_RELIEF_DEADBAND, PACE_TARGET_ARM_MARGIN, PACE_TARGET_RESERVE,
+  PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
+  STOP_GAP_RESERVE_LEAD_SPEED,
+  STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
+  STOP_HOLD_FAST_DEPARTURE_DISTANCE, STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, 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_track_id: int = -1
+  selected_lead_speed: float = math.inf
+  selected_lead_accel: float = 0.0
+  departure_lead_index: int = -1
+  departure_lead_speed: float = math.inf
+  departure_cap: float = math.inf
+  departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
+  departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf)
+  departure_lead_track_ids: tuple[int, int] = (-1, -1)
+  departure_lead_separations: tuple[float, float] = (-math.inf, -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
+  departure_launching: 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))
+  lead_speed_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
+  lead_accel_samples: deque[float] = field(default_factory=lambda: deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
+  departure_samples: tuple[deque[float], deque[float]] = field(
+    default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)),
+  )
+  departure_motion_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
+  departure_references: list[float | None] = field(default_factory=lambda: [None, None])
+  departure_track_ids: list[int] = field(default_factory=lambda: [-1, -1])
+  pace: float | None = None
+  state: AccelControllerState = AccelControllerState.inactive
+  departure_frames: int = 0
+  active_frames: int = 0
+  lead_loss_frames: int = 0
+  lead_switch_guard_frames: int = 0
+  selected_lead: int = -1
+  selected_lead_track_id: int = -1
+  stale_frames: int = 0
+  launching: bool = False
+  departure_launch: bool = False
+  matched_lead: bool = False
+  braking_limited: bool = False
+  braking_handoff: bool = False
+  pace_reserve_armed: bool = False
+  matched_accel_limit: float | None = None
+
+  @property
+  def filtered_cap(self) -> float:
+    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def filtered_lead_speed(self) -> float:
+    return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def filtered_lead_accel(self) -> float:
+    return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2]
+
+  def robust_departure_separation(self, lead_index: int) -> float:
+    samples = self.departure_samples[lead_index]
+    return float(np.median(samples)) if samples else -math.inf
+
+  def reset(self) -> None:
+    self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
+    self.lead_speed_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
+    self.lead_accel_samples = deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
+    self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
+    self.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES)
+    self.departure_references = [None, None]
+    self.departure_track_ids = [-1, -1]
+    self.pace = None
+    self.state = AccelControllerState.inactive
+    self.departure_frames = 0
+    self.active_frames = 0
+    self.lead_loss_frames = 0
+    self.lead_switch_guard_frames = 0
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.stale_frames = 0
+    self.launching = False
+    self.departure_launch = False
+    self.matched_lead = False
+    self.braking_limited = False
+    self.braking_handoff = False
+    self.pace_reserve_armed = False
+    self.matched_accel_limit = None
+
+
+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.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
+    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
+    self.live = _ControllerPath()
+    self.shadow = _ControllerPath()
+    self._held_envelope: EnergyEnvelope | None = None
+
+  @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, v_lead = float(lead.dRel), 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
+
+  @staticmethod
+  def _lead_track_id(lead) -> int:
+    try:
+      return max(int(lead.radarTrackId), -1)
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      return -1
+
+  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, int]] = []
+    departure_speeds = [math.inf, math.inf]
+    departure_distances = [-math.inf, -math.inf]
+    departure_track_ids = [-1, -1]
+    departure_separations = [-math.inf, -math.inf]
+    departure_caps = [math.inf, math.inf]
+
+    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]))
+        safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
+        closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
+        required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
+        reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
+        reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
+        usable_gap = max(safety_gap - reserve * reserve_scale, 0.0)
+        cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
+        departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
+        separation = x_lead - x_ego
+        departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
+      except (FloatingPointError, OverflowError, TypeError, ValueError):
+        continue
+
+      finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_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
+      if not math.isfinite(separation):
+        continue
+
+      candidates.append(EnergyEnvelope(
+        cap=cap, selected_lead=lead_index, selected_lead_track_id=self._lead_track_id(lead),
+        selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
+        usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
+      ))
+      departure_candidates.append((departure_distance, lead_index))
+      departure_speeds[lead_index] = v_lead_delay
+      departure_distances[lead_index] = d_rel
+      departure_track_ids[lead_index] = self._lead_track_id(lead)
+      departure_separations[lead_index] = separation
+      departure_caps[lead_index] = departure_cap
+
+    if not candidates:
+      return EnergyEnvelope(lead_status=lead_status)
+
+    selected = min(candidates, key=lambda candidate: candidate.cap)
+    departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
+    departure_lead_speed = departure_speeds[departure_lead_index]
+    return EnergyEnvelope(
+      cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_track_id=selected.selected_lead_track_id,
+      selected_lead_speed=selected.selected_lead_speed,
+      selected_lead_accel=selected.selected_lead_accel,
+      departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
+      departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
+      departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
+      departure_lead_separations=tuple(departure_separations),
+      usable_gap=selected.usable_gap, closing_speed=selected.closing_speed, required_decel=selected.required_decel,
+      has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, lead_status=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 _lead_source(source) -> bool:
+    return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+  def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
+    had_filtered_lead = math.isfinite(path.filtered_cap)
+    has_lead = envelope.selected_lead >= 0
+    path.cap_samples.append(envelope.cap if has_lead else math.inf)
+    path.lead_speed_samples.append(envelope.selected_lead_speed if has_lead else math.inf)
+    path.lead_accel_samples.append(envelope.selected_lead_accel if has_lead else 0.0)
+    path.lead_loss_frames = 0 if has_lead else path.lead_loss_frames + 1
+    for lead_index, distance in enumerate(envelope.departure_lead_distances):
+      if not math.isfinite(distance):
+        continue
+      samples = path.departure_samples[lead_index]
+      track_id = envelope.departure_lead_track_ids[lead_index]
+      identity_changed = bool(samples) and track_id != path.departure_track_ids[lead_index] and (track_id >= 0 or path.departure_track_ids[lead_index] >= 0)
+      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speeds[lead_index] * self.dt)
+      geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
+      if identity_changed or geometry_jump:
+        samples.clear()
+        path.departure_references[lead_index] = distance
+      samples.append(distance)
+      path.departure_track_ids[lead_index] = track_id
+    lead_index = envelope.departure_lead_index
+    if lead_index >= 0:
+      distance = envelope.departure_lead_distances[lead_index]
+      samples = path.departure_motion_samples
+      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speed * self.dt)
+      if samples and abs(distance - samples[-1]) > max_distance_step:
+        samples.clear()
+      samples.append(distance)
+    return not had_filtered_lead and math.isfinite(path.filtered_cap)
+
+  @staticmethod
+  def _seed_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None:
+    path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
+    path.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES)
+    path.departure_references = [None, None]
+    path.departure_track_ids = list(envelope.departure_lead_track_ids)
+    for lead_index, distance in enumerate(envelope.departure_lead_distances):
+      if math.isfinite(distance):
+        path.departure_samples[lead_index].append(distance)
+        path.departure_references[lead_index] = distance
+    if envelope.departure_lead_index >= 0:
+      path.departure_motion_samples.append(envelope.departure_lead_distances[envelope.departure_lead_index])
+    path.departure_frames = 0
+
+  @staticmethod
+  def _departure_progress(path: _ControllerPath, envelope: EnergyEnvelope, minimum_distance: float, *, robust: bool = True) -> bool:
+    lead_index = envelope.departure_lead_index
+    if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
+      return False
+    reference = path.departure_references[lead_index]
+    samples = path.departure_samples[lead_index]
+    distance = path.robust_departure_separation(lead_index) if robust else samples[-1] if samples else -math.inf
+    return reference is not None and distance - reference >= minimum_distance
+
+  @classmethod
+  def _creep_departure(cls, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
+    return cls._departure_progress(path, envelope, STOP_HOLD_CREEP_DISTANCE)
+
+  @staticmethod
+  def _recent_departure_motion(path: _ControllerPath) -> bool:
+    samples = tuple(path.departure_motion_samples)[-STOP_HOLD_EXIT_FRAMES:]
+    if len(samples) < STOP_HOLD_EXIT_FRAMES:
+      return False
+    deltas = np.diff(samples)
+    return samples[-1] - samples[0] >= STOP_HOLD_FAST_DEPARTURE_DISTANCE and np.count_nonzero(deltas > 0.005) >= 2
+
+  def _enter_stop_hold(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
+    if path.state != AccelControllerState.stopHold:
+      self._seed_departure_tracking(path, envelope)
+    path.pace = 0.0
+    path.state = AccelControllerState.stopHold
+    path.departure_frames = 0
+    path.launching = False
+    path.departure_launch = False
+    path.matched_lead = False
+    path.pace_reserve_armed = False
+    path.matched_accel_limit = None
+
+  def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float,
+                   profile: AccelProfile, profile_accel_max: float, previous_should_stop: bool,
+                   previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
+    confirmed_lead = self._update_samples(path, envelope)
+    path.active_frames += 1
+    has_lead = envelope.selected_lead >= 0
+    filtered_cap = path.filtered_cap
+    slot_changed = has_lead and path.selected_lead >= 0 and envelope.selected_lead != path.selected_lead
+    track_changed = (has_lead and path.selected_lead >= 0 and envelope.selected_lead == path.selected_lead
+                     and envelope.selected_lead_track_id != path.selected_lead_track_id
+                     and (path.selected_lead_track_id >= 0 or envelope.selected_lead_track_id >= 0))
+    false_relief = (has_lead and math.isfinite(filtered_cap)
+                    and envelope.cap >= filtered_cap + PACE_RELIEF_DEADBAND)
+    if (slot_changed or track_changed) and false_relief and path.lead_switch_guard_frames == 0 and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD:
+      path.lead_switch_guard_frames = self.lead_loss_hold_frames
+    elif path.lead_switch_guard_frames > 0:
+      path.lead_switch_guard_frames -= 1
+    if slot_changed or track_changed:
+      path.matched_lead = False
+      path.matched_accel_limit = None
+    if has_lead:
+      path.selected_lead = envelope.selected_lead
+      path.selected_lead_track_id = envelope.selected_lead_track_id
+    elif path.lead_loss_frames >= self.lead_loss_hold_frames:
+      path.lead_switch_guard_frames = 0
+      path.selected_lead = -1
+      path.selected_lead_track_id = -1
+    departure_separation = (envelope.departure_lead_separations[envelope.departure_lead_index]
+                            if envelope.departure_lead_index >= 0 else math.inf)
+    stopped_lead_hold = (has_lead and envelope.has_nearly_stopped_lead
+                         and (envelope.departure_cap < 0.50
+                              or (path.braking_limited and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)))
+    invalid_lead = envelope.lead_status and not has_lead
+    prior_lead_context = self._lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or path.braking_limited
+    previous_stop = (previous_should_stop and prior_lead_context
+                     and (not has_lead or envelope.departure_lead_speed < STOP_HOLD_EXIT_SPEED))
+    stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50
+                     or (previous_stop and not path.launching) or invalid_lead)
+    confirmed_creep_departure = (path.launching and path.departure_launch and has_lead
+                                 and (self._departure_progress(path, envelope, STOP_HOLD_FAST_DEPARTURE_DISTANCE)
+                                      or self._recent_departure_motion(path)))
+    if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap)
+        and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD):
+      path.braking_limited = True
+    elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames:
+      path.braking_limited = False
+
+    if path.pace is None:
+      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
+      seed_from_ego = has_lead and planner_accel > BRAKING_ACCEL_LIMIT_THRESHOLD and not e2e_handoff
+      path.pace = min(base_speed, v_ego) if seed_from_ego else base_speed
+      path.braking_handoff = e2e_handoff and planner_accel < 0.0
+      path.state = AccelControllerState.free
+      if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
+        path.pace = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+        path.state = AccelControllerState.release
+        path.launching = True
+        path.departure_launch = False
+    elif path.braking_handoff and planner_accel >= 0.0:
+      path.braking_handoff = False
+
+    path.pace = min(path.pace, base_speed)
+    if (v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not confirmed_creep_departure
+        and path.state != AccelControllerState.stopHold):
+      self._enter_stop_hold(path, envelope)
+      return path.pace
+
+    if path.state == AccelControllerState.stopHold:
+      for lead_index in range(len(path.departure_references)):
+        separation = path.robust_departure_separation(lead_index)
+        if math.isfinite(separation) and path.departure_references[lead_index] is None:
+          path.departure_references[lead_index] = separation
+      fast_departure = (has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
+                        and envelope.departure_cap > STOP_HOLD_EXIT_SPEED)
+      raw_departure = (fast_departure
+                       or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
+      departed = self._creep_departure(path, envelope) or raw_departure
+      if fast_departure and path.departure_frames == 0 and path.departure_motion_samples:
+        path.departure_motion_samples = deque([path.departure_motion_samples[-1]], maxlen=CAP_FILTER_FRAMES)
+      path.departure_frames = path.departure_frames + 1 if departed else 0
+      path.pace = 0.0
+      fast_departure_confirmed = fast_departure and self._recent_departure_motion(path)
+      if path.departure_frames < STOP_HOLD_EXIT_FRAMES or (fast_departure and not fast_departure_confirmed):
+        return path.pace
+      path.pace = base_speed
+      path.state = AccelControllerState.release
+      path.departure_frames = 0
+      path.launching = True
+      path.departure_launch = has_lead
+      return path.pace
+
+    if path.launching:
+      invalid_lead = envelope.lead_status and not has_lead
+      renewed_stop = (has_lead and not confirmed_creep_departure
+                      and (envelope.cap < STOP_HOLD_EXIT_SPEED
+                           or (envelope.has_nearly_stopped_lead and envelope.departure_cap < STOP_HOLD_EXIT_SPEED)))
+      guarded_departure_loss = path.departure_launch and not envelope.lead_status and path.lead_loss_frames < self.lead_loss_hold_frames
+      if invalid_lead:
+        path.launching = False
+        path.departure_launch = False
+        if v_ego < STOP_HOLD_EGO_SPEED:
+          self._enter_stop_hold(path, envelope)
+          return path.pace
+        path.state = AccelControllerState.hold
+        return path.pace
+      if guarded_departure_loss:
+        path.state = AccelControllerState.hold
+        return path.pace
+      if path.departure_launch and not has_lead:
+        path.departure_launch = False
+      if renewed_stop:
+        path.launching = False
+        path.departure_launch = False
+        if v_ego < STOP_HOLD_EGO_SPEED:
+          self._enter_stop_hold(path, envelope)
+          return path.pace
+      if path.departure_launch:
+        path.pace = base_speed
+      else:
+        launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+        path.pace = min(base_speed, max(path.pace, launch_target) + LAUNCH_TARGET_SLEW * self.dt)
+      if v_ego >= LAUNCH_END_SPEED:
+        path.launching = False
+        path.departure_launch = False
+
+    comfort_decel = PROFILE_CONFIGS[profile].comfort_decel
+    if (has_lead and not path.launching and path.state == AccelControllerState.restrict
+        and envelope.closing_speed <= 0.0
+        and v_ego >= path.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
+      path.matched_lead = True
+    elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames:
+      path.matched_lead = False
+
+    if path.matched_lead:
+      if not has_lead:
+        if self._lead_source(previous_mpc_source) and planner_speed < path.pace:
+          path.pace = max(planner_speed, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
+        path.state = AccelControllerState.hold
+        return path.pace
+      if math.isfinite(path.filtered_lead_speed):
+        recovery_speed = min(base_speed, path.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * envelope.usable_gap))
+        desired_accel_limit = min(profile_accel_max, LEAD_MATCH_TAPER_GAIN * max(recovery_speed - v_ego, 0.0))
+      else:
+        desired_accel_limit = 0.0
+      if path.filtered_lead_accel < BRAKING_ACCEL_LIMIT_THRESHOLD:
+        desired_accel_limit = profile_accel_max
+      if path.matched_accel_limit is None:
+        path.matched_accel_limit = profile_accel_max
+      if path.lead_switch_guard_frames > 0:
+        desired_accel_limit = min(desired_accel_limit, path.matched_accel_limit)
+      path.matched_accel_limit = min(profile_accel_max,
+                                     self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt))
+      matched_ceiling = min(base_speed, filtered_cap)
+      if matched_ceiling <= path.pace - PACE_RESTRICT_DEADBAND:
+        path.pace = max(matched_ceiling, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
+        path.state = AccelControllerState.restrict
+      elif path.lead_switch_guard_frames == 0 and matched_ceiling >= path.pace + PACE_RELIEF_DEADBAND:
+        path.pace = min(matched_ceiling, path.pace + profile_accel_max * self.dt)
+        path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
+      else:
+        path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
+      return path.pace
+    path.matched_accel_limit = None
+
+    ceiling = min(base_speed, filtered_cap)
+    if (confirmed_lead and path.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not path.launching
+        and planner_speed < path.pace):
+      path.pace = max(planner_speed, path.pace - comfort_decel * self.dt)
+
+    if self._lead_source(previous_mpc_source) and not has_lead and planner_speed < path.pace:
+      path.pace = max(planner_speed, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
+      path.state = AccelControllerState.hold
+      return path.pace
+
+    if ceiling <= path.pace - PACE_RESTRICT_DEADBAND or (path.state == AccelControllerState.restrict and ceiling < path.pace):
+      path.pace = max(ceiling, path.pace - comfort_decel * self.dt)
+      path.state = AccelControllerState.restrict
+      return path.pace
+
+    filter_warmup = has_lead and not math.isfinite(filtered_cap)
+    guarded_lead_loss = not has_lead and path.lead_loss_frames < self.lead_loss_hold_frames
+    if (filter_warmup or guarded_lead_loss) and path.pace < base_speed - PACE_RESTRICT_DEADBAND:
+      path.state = AccelControllerState.hold
+      return path.pace
+
+    confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
+    relief = not has_lead or envelope.closing_speed <= 0.0
+    if relief and (ceiling >= path.pace + PACE_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > path.pace)):
+      if path.lead_switch_guard_frames == 0:
+        path.pace = ceiling
+      path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
+    else:
+      path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
+    return path.pace
+
+  @staticmethod
+  def _valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float,
+                     delay: float, engaged: bool, cruise_initialized: bool) -> bool:
+    values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay)
+    return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
+            and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values))
+
+  def _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:
+      path.reset()
+    return False
+
+  @staticmethod
+  def _build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None:
+    if limit >= ACCEL_MAX - 1e-9:
+      return None
+    a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
+    ceiling = np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS)
+    ceiling = np.clip(ceiling, 0.0, ACCEL_MAX)
+    ceiling[0] = max(ceiling[0], a0)
+    return tuple(float(value) for value in ceiling)
+
+  def reset(self) -> None:
+    self.live.reset()
+    self.shadow.reset()
+    self._held_envelope = None
+
+  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, stock_accel_max: float,
+             previous_should_stop: bool, radar_fresh: bool = True,
+             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> 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)
+    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
+    valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self._delay(),
+                                        engaged, cruise_initialized)
+    feature_context = valid_context and bool(enabled)
+    if feature_context and radar_fresh:
+      envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality)
+      self._held_envelope = envelope
+    elif feature_context and self._held_envelope is not None:
+      envelope = self._held_envelope
+    else:
+      envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
+      if not feature_context:
+        self._held_envelope = None
+
+    shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if feature_context else False
+    if feature_context and radar_fresh:
+      self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, previous_should_stop,
+                        previous_mpc_source, planner_speed, planner_accel)
+      shadow_active = True
+    elif feature_context and not shadow_fresh and self.shadow.pace is not None:
+      shadow_active = True
+    else:
+      self.shadow.reset()
+      shadow_active = False
+
+    live_context = feature_context and bool(acc_selected)
+    live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False
+    if live_context and radar_fresh:
+      pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max,
+                                      previous_should_stop,
+                                      previous_mpc_source, planner_speed, planner_accel)
+      live_active = True
+    elif live_context and not live_fresh and self.live.pace is not None:
+      pace_target = self.live.pace
+      live_active = True
+    else:
+      self.live.reset()
+      pace_target = base_speed
+      live_active = False
+
+    if not radar_fresh and not shadow_active and not live_active:
+      self._held_envelope = None
+      envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
+
+    stop_hold_active = live_active and self.live.state == AccelControllerState.stopHold
+    matched_limit_active = (live_active and self.live.matched_lead and self.live.matched_accel_limit is not None
+                            and not self.live.braking_handoff)
+    lead_accel_request = (live_active and envelope.selected_lead >= 0
+                          and envelope.closing_speed <= 0.0 and planner_accel >= 0.0)
+    profile_limit_active = live_active and not stop_hold_active and (self.live.launching or not envelope.lead_status or lead_accel_request)
+    if matched_limit_active:
+      effective_accel_max = min(positive_accel_max, self.live.matched_accel_limit)
+    elif profile_limit_active:
+      effective_accel_max = positive_accel_max
+    else:
+      effective_accel_max = math.inf
+    if matched_limit_active or profile_limit_active:
+      mpc_accel_max = self._build_accel_ceiling(effective_accel_max, planner_accel)
+    else:
+      mpc_accel_max = None
+    guarded_lead_loss = (not envelope.lead_status and self.live.selected_lead >= 0
+                         and self.live.lead_loss_frames < self.lead_loss_hold_frames)
+    lead_context = envelope.lead_status or math.isfinite(self.live.filtered_cap) or guarded_lead_loss
+    reserve_eligible = (live_active and lead_context and not stop_hold_active and self.live.lead_switch_guard_frames == 0
+                        and not self.live.launching and not self.live.braking_handoff)
+    if not lead_context:
+      self.live.pace_reserve_armed = False
+    elif (reserve_eligible and not self.live.pace_reserve_armed and math.isfinite(self.live.filtered_cap)
+          and self.live.filtered_cap <= pace_target + PACE_TARGET_ARM_MARGIN):
+      self.live.pace_reserve_armed = True
+
+    target_speed = 0.0 if stop_hold_active else pace_target
+    if reserve_eligible and self.live.pace_reserve_armed:
+      target_speed = max(0.0, target_speed - PACE_TARGET_RESERVE)
+
+    return AccelControllerResult(
+      target_speed=target_speed,
+      enabled=bool(enabled), active=live_active, shadow_active=shadow_active, launching=live_active and self.live.launching,
+      departure_launching=live_active and self.live.launching and self.live.departure_launch,
+      profile=selected_profile, profile_accel_max=profile_accel_max if live_active else math.inf,
+      positive_accel_max=positive_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max,
+      mpc_accel_max=mpc_accel_max, state=self.live.state,
+      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

@@ -1,17 +1,22 @@
-import math
-
-import numpy as np
-
-from cereal import custom
+from dataclasses import dataclass
+from enum import IntEnum
 
 
-AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
-ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
+class AccelProfile(IntEnum):
+  eco = 0
+  normal = 1
+  sport = 2
 
-COMFORT_DECEL = {
-  AccelProfile.eco: 0.25,
-  AccelProfile.normal: 0.30,
-  AccelProfile.sport: 0.35,
+
+@dataclass(frozen=True)
+class ProfileConfig:
+  comfort_decel: float
+
+
+PROFILE_CONFIGS = {
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.30),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.35),
 }
 
 ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
@@ -23,25 +28,24 @@ ACCEL_PROFILE_MAX_V = {
 
 CAP_FILTER_FRAMES = 5
 LEAD_LOSS_HOLD_TIME = 0.50
-SPEED_RESTRICT_DEADBAND = 0.15
-SPEED_RELIEF_DEADBAND = 0.35
-TARGET_SPEED_ARM_MARGIN = 1.0
-TARGET_SPEED_RESERVE = 0.10
+PACE_RESTRICT_DEADBAND = 0.15
+PACE_RELIEF_DEADBAND = 0.35
+PACE_TARGET_ARM_MARGIN = 1.0
+PACE_TARGET_RESERVE = 0.10
 LAUNCH_TARGET_HEADROOM = 3.0
 LAUNCH_TARGET_SLEW = 8.75
 LAUNCH_END_SPEED = 3.0
 ACCEL_LIMIT_HORIZON_JERK = 1.0
 LEAD_MATCH_GAP_GAIN = 0.04
 LEAD_MATCH_SPEED_HEADROOM = 1.25
+LEAD_MATCH_TAPER_GAIN = 1.00
 LEAD_MATCH_ACCEL_SLEW = 0.25
-MATCHED_SPEED_DECEL_RATE = 0.50
-PLANNER_BRAKING_ACCEL_THRESHOLD = -0.11
-LEAD_BRAKING_ACCEL_THRESHOLD = -0.11
+MATCHED_PACE_DECEL_RATE = 0.50
+BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
 MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
 MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
 MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
 MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
-MPC_DECEL_TREND_FRAMES = 4
 
 STOP_HOLD_EGO_SPEED = 0.30
 STOPPED_LEAD_SPEED = 0.30
@@ -49,8 +53,7 @@ STOP_HOLD_EXIT_SPEED = 0.80
 STOP_HOLD_EXIT_FRAMES = 4
 STOP_HOLD_CREEP_SPEED = 0.15
 STOP_HOLD_CREEP_DISTANCE = 0.30
-DEPARTURE_MOTION_NOISE_FLOOR = 0.03
-DEPARTURE_MOTION_STEP_MIN = 0.005
+STOP_HOLD_FAST_DEPARTURE_DISTANCE = 0.03
 STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
 STOP_GAP_RESERVE = 0.75
 STOP_GAP_RESERVE_LEAD_SPEED = 2.0
@@ -60,14 +63,3 @@ RADAR_STALE_TIMEOUT = 0.50
 MAX_LEAD_ACCEL_TAU = 10.0
 MIN_LEAD_SPEED = -1.0
 VEGO_NOISE_TOLERANCE = 0.10
-PARAM_READ_INTERVAL = 0.25
-
-
-def sanitize_profile(profile: int) -> int:
-  return profile if profile in ACCEL_PROFILES else AccelProfile.normal
-
-
-def profile_accel_max(profile: int, v_ego: float) -> float:
-  if not math.isfinite(v_ego):
-    return math.nan
-  return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)]))

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

@@ -10,14 +10,12 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
   STOP_DISTANCE, T_IDXS, LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW,
 )
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
-  COMFORT_DECEL, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_SPEED_DECEL_RATE,
-  MPC_DECEL_JERK_COST_MULTIPLIER, TARGET_SPEED_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
+  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
+  LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS,
+  PACE_TARGET_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES,
 )
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan
 
 
 def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
@@ -30,11 +28,7 @@ def make_radar(lead_one=None, lead_two=None):
 
 
 def make_controller(delay=0.10):
-  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True))
-
-
-def get_lead_plan(controller, radar_state, v_ego: float, a_ego: float, profile: int):
-  return calculate_lead_plan(radar_state, v_ego, a_ego, controller.delay, profile)
+  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay))
 
 
 def update(controller, radar_state=None, **overrides):
@@ -52,18 +46,7 @@ def update(controller, radar_state=None, **overrides):
     "previous_should_stop": False,
   }
   args.update(overrides)
-  controller.profile = args.pop("profile")
-  controller.enabled = args.pop("enabled")
-  controller.update(radar_state or make_radar(), **args)
-  return SimpleNamespace(
-    target_speed=controller.output_v_target, active=controller.is_active, launching=controller.launching,
-    departure_launching=controller.departure_launching, mpc_accel_max=controller.mpc_accel_max, state=controller.state,
-    selected_lead=controller.selected_lead, required_decel=controller.required_decel,
-  )
-
-
-def effective_accel_max(result):
-  return math.inf if result.mpc_accel_max is None else min(result.mpc_accel_max)
+  return controller.update(radar_state or make_radar(), **args)
 
 
 def restrictive_radar():
@@ -84,7 +67,7 @@ class TestProfiles:
       AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
     }
 
-  @pytest.mark.parametrize("profile", ACCEL_PROFILES)
+  @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
@@ -95,19 +78,20 @@ class TestProfiles:
 
   @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
   def test_profile_order_is_distinct(self, speed):
-    eco, normal, sport = [AccelController.get_profile_accel_max(profile, speed) for profile in ACCEL_PROFILES]
+    eco, normal, sport = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
     assert eco < normal < sport
 
   def test_invalid_profile_defaults_to_normal(self):
-    assert AccelController._profile(999) == AccelProfile.normal
+    assert update(make_controller(), profile=999).profile == AccelProfile.normal
 
   def test_stock_limit_intersects_profile_before_mpc(self):
     controller = make_controller()
     results = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
                for _ in range(controller.lead_loss_hold_frames)]
     result = results[-1]
-    assert AccelController.get_profile_accel_max(AccelProfile.sport, 10.0) == pytest.approx(1.15)
-    assert effective_accel_max(result) == pytest.approx(0.30)
+    assert result.profile_accel_max == pytest.approx(1.15)
+    assert result.positive_accel_max == pytest.approx(0.30)
+    assert result.effective_accel_max == pytest.approx(0.30)
     assert all(sample.mpc_accel_max is not None for sample in results)
     assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results)
 
@@ -117,8 +101,8 @@ class TestProfiles:
              for _ in range(controller.lead_loss_hold_frames)][-1]
     eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
 
-    assert effective_accel_max(sport) == pytest.approx(1.15)
-    assert effective_accel_max(eco) == pytest.approx(0.72)
+    assert sport.effective_accel_max == pytest.approx(1.15)
+    assert eco.effective_accel_max == pytest.approx(0.72)
 
   def test_matched_lead_waits_until_ego_catches_the_lead(self):
     radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
@@ -130,8 +114,8 @@ class TestProfiles:
     update(slow_controller, radar, v_ego=3.0, planner_accel=-0.2)
     update(caught_controller, radar, v_ego=8.0, planner_accel=-0.2)
 
-    assert not slow_controller.target_state.matched_lead
-    assert caught_controller.target_state.matched_lead
+    assert not slow_controller.live.matched_lead
+    assert caught_controller.live.matched_lead
 
   def test_stock_limit_reduction_applies_immediately(self):
     controller = make_controller()
@@ -139,7 +123,7 @@ class TestProfiles:
       update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
 
     reduced = update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
-    assert effective_accel_max(reduced) == pytest.approx(0.30)
+    assert reduced.effective_accel_max == pytest.approx(0.30)
     assert reduced.mpc_accel_max is not None
     assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9
 
@@ -153,8 +137,8 @@ class TestProfiles:
     clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
     recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
 
-    assert effective_accel_max(limited) == 0.0
-    assert effective_accel_max(recovered) == pytest.approx(effective_accel_max(clean))
+    assert limited.effective_accel_max == 0.0
+    assert recovered.effective_accel_max == pytest.approx(clean.effective_accel_max)
 
   @pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
   def test_matched_lead_ceiling_obeys_current_stock_limit(self, radar_fresh):
@@ -164,16 +148,16 @@ class TestProfiles:
       update(controller, radar, v_ego=10.0, planner_accel=-0.2)
     for _ in range(20):
       update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.target_state.matched_lead
+    assert controller.live.matched_lead
 
     limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh)
-    assert effective_accel_max(limited) == 0.0
+    assert limited.effective_accel_max == 0.0
     assert limited.mpc_accel_max is not None
     assert max(limited.mpc_accel_max) == 0.0
 
   def test_exact_global_max_uses_stock_ceiling(self):
     result = update(make_controller(), base_speed=8.0, v_ego=0.0, profile=AccelProfile.sport)
-    assert AccelController.get_profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX
+    assert result.positive_accel_max == ACCEL_MAX
     assert result.mpc_accel_max is None
 
 
@@ -181,7 +165,7 @@ class TestMpcCeiling:
   @pytest.mark.parametrize("planner_accel", (-1.0, 0.0, 1.2, ACCEL_MAX))
   def test_ceiling_is_finite_feasible_and_jerk_bounded(self, planner_accel):
     limit = 0.50
-    ceiling = np.asarray(build_accel_ceiling(limit, planner_accel))
+    ceiling = np.asarray(AccelController._build_accel_ceiling(limit, planner_accel))
     a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
 
     assert ceiling.shape == T_IDXS.shape
@@ -193,7 +177,7 @@ class TestMpcCeiling:
     assert np.all(-np.diff(ceiling) <= ACCEL_LIMIT_HORIZON_JERK * np.diff(T_IDXS) + 1e-9)
 
   def test_zero_limit_remains_feasible_for_positive_x0(self):
-    ceiling = np.asarray(build_accel_ceiling(0.0, 0.8))
+    ceiling = np.asarray(AccelController._build_accel_ceiling(0.0, 0.8))
     assert ceiling[0] == pytest.approx(0.8)
     assert ceiling[-1] == pytest.approx(0.0)
     assert np.all(ceiling >= 0.0)
@@ -202,9 +186,10 @@ class TestMpcCeiling:
     controller = make_controller()
     result = update(controller, enabled=False)
     assert not result.active
+    assert not result.shadow_active
     assert result.mpc_accel_max is None
-    assert math.isinf(effective_accel_max(result))
-    assert controller.target_state.target_speed is None
+    assert math.isinf(result.effective_accel_max)
+    assert controller.live.pace is None and controller.shadow.pace is None
 
   def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
     controller = make_controller()
@@ -212,11 +197,11 @@ class TestMpcCeiling:
     warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)]
 
     assert all(sample.mpc_accel_max is None for sample in warmup)
-    assert controller.target_state.lead_braking
+    assert controller.live.braking_limited
 
     bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
     assert not bypassed.active and bypassed.mpc_accel_max is None
-    assert not controller.target_state.lead_braking
+    assert not controller.live.braking_limited
 
   def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
     controller = make_controller()
@@ -227,7 +212,7 @@ class TestMpcCeiling:
     result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2)
 
     assert result.state == AccelControllerState.restrict
-    assert effective_accel_max(result) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 10.0))
+    assert result.effective_accel_max == pytest.approx(result.positive_accel_max)
     assert result.mpc_accel_max is not None
 
   def test_matched_lead_terminal_taper_changes_smoothly(self):
@@ -237,16 +222,15 @@ class TestMpcCeiling:
       update(controller, radar, v_ego=10.0, planner_accel=-0.2)
 
     braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    braking_limit = controller.target_state.matched_accel_limit
+    braking_limit = controller.live.matched_accel_limit
     accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2)
 
-    assert controller.target_state.matched_lead
+    assert controller.live.matched_lead
     assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None
     assert braking_limit is not None
-    assert abs(controller.target_state.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
-    profile_accel_max = AccelController.get_profile_accel_max(AccelProfile.normal, 8.0)
-    assert effective_accel_max(braking) <= profile_accel_max
-    assert effective_accel_max(accelerating) <= profile_accel_max
+    assert abs(controller.live.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
+    assert braking.effective_accel_max <= braking.positive_accel_max
+    assert accelerating.effective_accel_max <= accelerating.positive_accel_max
 
   def test_matched_lead_ignores_two_frame_speed_jump(self):
     clean_controller, noisy_controller = make_controller(), make_controller()
@@ -261,7 +245,7 @@ class TestMpcCeiling:
     for _ in range(2):
       clean = update(clean_controller, radar, v_ego=8.0)
       noisy = update(noisy_controller, speed_jump, v_ego=8.0)
-      assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
+      assert noisy.effective_accel_max == pytest.approx(clean.effective_accel_max)
       assert noisy.target_speed == pytest.approx(clean.target_speed)
 
   def test_matched_lead_ignores_two_frame_acceleration_jump(self):
@@ -277,44 +261,44 @@ class TestMpcCeiling:
     for _ in range(2):
       clean = update(clean_controller, steady, v_ego=8.0)
       noisy = update(noisy_controller, braking_jump, v_ego=8.0)
-      assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
+      assert noisy.effective_accel_max == pytest.approx(clean.effective_accel_max)
       assert noisy.target_speed == pytest.approx(clean.target_speed)
 
 
-class TestLead:
-  def test_cap_matches_stopping_energy_formula(self):
+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)
-    result = get_lead_plan(controller, make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    delay = controller.delay
+    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, _ = _project_ego(10.0, 0.0, delay)
+    x_ego, _ = controller._project_ego(10.0, 0.0, delay)
     safety_gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
-    expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_gap)
+    expected = v_lead + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * safety_gap)
 
-    assert result.cap == pytest.approx(expected)
-    assert result.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_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 * safety_gap))
 
   def test_profile_order_controls_approach_timing(self):
     radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
-    caps = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).cap for profile in ACCEL_PROFILES]
+    caps = [make_controller().calculate_energy_envelope(radar, 10.0, 0.0, profile).cap for profile in AccelProfile]
     assert caps[0] < caps[1] < caps[2]
 
   def test_stopped_lead_reserve_only_reduces_comfort_gap(self):
-    lead = get_lead_plan(make_controller(),
+    envelope = make_controller().calculate_energy_envelope(
       make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)), 5.0, 0.0, AccelProfile.normal,
     )
-    comfort_decel = COMFORT_DECEL[AccelProfile.normal]
-    safety_gap = (lead.departure_cap - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
-    assert lead.required_decel < 0.30
-    assert safety_gap - lead.usable_gap == pytest.approx(STOP_GAP_RESERVE)
-    assert lead.departure_cap > lead.cap
+    comfort_decel = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel
+    safety_gap = (envelope.departure_cap - envelope.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
+    assert envelope.required_decel < 0.30
+    assert safety_gap - envelope.usable_gap == pytest.approx(STOP_GAP_RESERVE)
+    assert envelope.departure_cap > envelope.cap
 
   def test_more_restrictive_lead_is_selected(self):
     radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
-    assert get_lead_plan(make_controller(), radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
+    assert make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
 
   @pytest.mark.parametrize("field,value", [
     ("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan),
@@ -322,50 +306,47 @@ class TestLead:
   def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
     lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
     setattr(lead, field, value)
-    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert result.selected_lead == 0
-    assert math.isfinite(result.cap)
+    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)
-    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert result.selected_lead == -1
-    assert result.lead_status
-    assert math.isinf(result.cap)
+    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == -1
+    assert envelope.lead_status
+    assert math.isinf(envelope.cap)
 
   def test_raw_radar_is_never_mutated(self):
     lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan)
     before = vars(lead).copy()
-    get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
     assert vars(lead) == before
 
 
-class TestTargetLifecycle:
+class TestPaceAndLifecycle:
   def test_five_frame_median_needs_three_restrictive_samples(self):
     controller = make_controller()
-    filtered_caps = []
-    for _ in range(CAP_FILTER_FRAMES):
-      update(controller, restrictive_radar())
-      filtered_caps.append(controller.target_state.filtered_cap)
-    assert math.isinf(filtered_caps[1])
-    assert math.isfinite(filtered_caps[2])
+    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_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self):
     controller = make_controller()
     results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)]
     targets = np.asarray([result.target_speed for result in results])
-    max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
+    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
     target_steps = -np.diff(targets)
     assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
     assert results[-1].state == AccelControllerState.restrict
     assert results[-1].target_speed < results[0].target_speed
 
   @pytest.mark.parametrize("clear_frames", (1, 2, CAP_FILTER_FRAMES + 1))
-  def test_lead_acquired_after_clear_road_cannot_step_speed_to_planner(self, clear_frames):
+  def test_lead_acquired_after_clear_road_cannot_step_pace_to_planner(self, clear_frames):
     controller = make_controller()
     for _ in range(clear_frames):
       update(controller, base_speed=25.0, v_ego=20.0, planner_speed=25.0)
@@ -373,11 +354,11 @@ class TestTargetLifecycle:
     results = [update(controller, restrictive_radar(), base_speed=25.0, v_ego=20.0, planner_speed=20.0)
                for _ in range(CAP_FILTER_FRAMES)]
     targets = np.asarray([25.0, *(result.target_speed for result in results)])
-    max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
+    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
     target_steps = -np.diff(targets)
     assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
 
   def test_lead_slot_is_forgotten_before_reacquisition(self):
     controller = make_controller()
@@ -387,36 +368,36 @@ class TestTargetLifecycle:
 
     for _ in range(controller.lead_loss_hold_frames):
       before = update(controller, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
-    assert controller.target_state.selected_lead == -1
+    assert controller.live.selected_lead == -1
 
     lead_two = make_radar(lead_two=make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
     results = [update(controller, lead_two, base_speed=25.0, v_ego=20.0, planner_speed=5.0, planner_accel=-0.2)
                for _ in range(CAP_FILTER_FRAMES)]
     targets = np.asarray([before.target_speed, *(result.target_speed for result in results)])
-    max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
+    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
 
     target_steps = -np.diff(targets)
     assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
+    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
 
   @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
-  def test_false_relief_track_replacement_freezes_bounded_speed_release(self, replacement_track_id):
+  def test_false_relief_track_replacement_freezes_bounded_pace_release(self, replacement_track_id):
     controller = make_controller()
     original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
     for _ in range(CAP_FILTER_FRAMES + 10):
       update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
     for _ in range(20):
       before = update(controller, original, base_speed=25.0, v_ego=8.0, planner_speed=8.0, planner_accel=-0.2)
-    assert controller.target_state.matched_lead
+    assert controller.live.matched_lead
 
     replacement = make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=12.0, radar_track_id=replacement_track_id))
     switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2)
 
     target_drop = before.target_speed - switched.target_speed
-    assert -TARGET_SPEED_RESERVE - 1e-9 <= target_drop <= MATCHED_SPEED_DECEL_RATE * DT_MDL + 1e-9
-    assert effective_accel_max(switched) <= AccelController.get_profile_accel_max(AccelProfile.normal, 8.0) + 1e-9
-    assert switched.target_speed < 25.0
-    assert controller.target_state.lead_switch_guard_frames == controller.lead_loss_hold_frames
+    assert -PACE_TARGET_RESERVE - 1e-9 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
+    assert switched.effective_accel_max <= switched.positive_accel_max + 1e-9
+    assert switched.target_speed < switched.base_speed
+    assert controller.live.lead_switch_guard_frames == controller.lead_loss_hold_frames
 
   def test_track_id_churn_without_false_relief_does_not_arm_guard(self):
     controller = make_controller()
@@ -427,7 +408,7 @@ class TestTargetLifecycle:
     replacement = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=200))
     update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
 
-    assert controller.target_state.lead_switch_guard_frames == 0
+    assert controller.live.lead_switch_guard_frames == 0
 
   def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self):
     controller = make_controller()
@@ -438,7 +419,7 @@ class TestTargetLifecycle:
     assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
 
     released = update(controller)
-    assert released.target_speed == 25.0
+    assert released.target_speed == released.base_speed
 
   def test_previous_lead_source_synchronizes_down_to_planner(self):
     controller = make_controller()
@@ -446,7 +427,7 @@ class TestTargetLifecycle:
       restricted = update(controller, restrictive_radar())
     planner_speed = restricted.target_speed - 2.0
     synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
-    assert restricted.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
+    assert restricted.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
     assert synchronized.state == AccelControllerState.hold
 
   def test_matched_lead_dropout_synchronizes_down_to_planner(self):
@@ -456,11 +437,11 @@ class TestTargetLifecycle:
       update(controller, radar, v_ego=10.0, planner_accel=-0.2)
     for _ in range(20):
       matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.target_state.matched_lead
+    assert controller.live.matched_lead
 
     planner_speed = matched.target_speed - 2.0
     synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
-    assert matched.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
+    assert matched.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
     assert synchronized.state == AccelControllerState.hold
 
   def test_reused_radar_holds_matched_lead_until_a_fresh_dropout(self):
@@ -470,7 +451,7 @@ class TestTargetLifecycle:
       update(controller, radar, v_ego=10.0, planner_accel=-0.2)
     for _ in range(20):
       matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.target_state.matched_lead
+    assert controller.live.matched_lead
 
     planner_speed = matched.target_speed - 2.0
     held = update(controller, radar, previous_mpc_source=LongitudinalPlanSource.lead0,
@@ -479,7 +460,7 @@ class TestTargetLifecycle:
 
     assert held.target_speed == pytest.approx(matched.target_speed)
     assert held.state == matched.state
-    assert held.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
+    assert held.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
     assert synchronized.state == AccelControllerState.hold
 
   def test_clear_road_launch_has_immediate_headroom_and_bounded_target_slew(self):
@@ -502,22 +483,12 @@ class TestTargetLifecycle:
     results = [update(controller, far_stopped, base_speed=12.0, v_ego=0.0) for _ in range(4)]
     assert all(result.state != AccelControllerState.stopHold for result in results)
 
-  def test_renewed_stop_above_stop_hold_speed_does_not_apply_extra_launch_ramp_step(self):
-    controller = make_controller()
-    ramp = [update(controller, base_speed=12.0, v_ego=v_ego, profile=AccelProfile.normal) for v_ego in (0.0, 0.5)]
-    assert all(result.launching for result in ramp)
-
-    stopped_lead = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.0))
-    renewed = update(controller, stopped_lead, base_speed=12.0, v_ego=0.5, profile=AccelProfile.normal)
-    assert not renewed.launching
-    assert renewed.target_speed <= ramp[-1].target_speed + 1e-9
-
   def test_far_stopped_lead_does_not_use_sticky_braking_history_as_stop_evidence(self):
     controller = make_controller()
     far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
     for _ in range(controller.lead_loss_hold_frames):
       update(controller, far_stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
-    assert controller.target_state.lead_braking
+    assert controller.live.braking_limited
 
     result = update(controller, far_stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
     assert result.state != AccelControllerState.stopHold
@@ -528,37 +499,35 @@ class TestTargetLifecycle:
     stopped = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=0.0))
     for _ in range(controller.lead_loss_hold_frames):
       update(controller, stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
-    assert controller.target_state.lead_braking
+    assert controller.live.braking_limited
 
     result = update(controller, stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
     assert result.state == AccelControllerState.stopHold
-    assert controller.target_state.target_speed == 0.0
+    assert controller.live.pace == 0.0
     assert result.target_speed == 0.0
-    assert math.isinf(effective_accel_max(result))
+    assert math.isinf(result.effective_accel_max)
     assert result.mpc_accel_max is None
 
     stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0)
-    assert math.isinf(effective_accel_max(stock_limited))
+    assert math.isinf(stock_limited.effective_accel_max)
     assert stock_limited.mpc_accel_max is None
 
   def test_stop_hold_needs_four_confirmed_departure_frames(self):
     controller = make_controller()
     held = enter_stop_hold(controller)
-    assert controller.target_state.target_speed == 0.0
+    assert controller.live.pace == 0.0
     results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
                       base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
     launch_index = next(index for index, result in enumerate(results) if result.launching)
 
     assert held.state == AccelControllerState.stopHold
-    assert held.target_speed == 0.0 and math.isinf(effective_accel_max(held))
+    assert held.target_speed == 0.0 and math.isinf(held.effective_accel_max)
     assert held.mpc_accel_max is None
     assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index])
     assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
     assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM
     assert results[launch_index].departure_launching
-    assert effective_accel_max(results[launch_index]) == pytest.approx(
-      AccelController.get_profile_accel_max(AccelProfile.normal, 0.1),
-    )
+    assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max)
 
   def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self):
     controller = make_controller()
@@ -641,10 +610,10 @@ class TestTargetLifecycle:
     update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
     replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101),
                              make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
-    lead = get_lead_plan(controller, replacement, 0.0, 0.0, AccelProfile.normal)
+    envelope = controller.calculate_energy_envelope(replacement, 0.0, 0.0, AccelProfile.normal)
     results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
 
-    assert lead.selected_lead == 1 and lead.departure_lead_index == 0
+    assert envelope.selected_lead == 1 and envelope.departure_lead_index == 0
     assert all(result.state == AccelControllerState.stopHold for result in results)
     assert all(result.target_speed == 0.0 and not result.launching for result in results)
 
@@ -699,16 +668,16 @@ class TestTargetLifecycle:
       assert held.target_speed == pytest.approx(fresh.target_speed)
       assert held.state == fresh.state
       assert held.selected_lead == fresh.selected_lead == 0
-      assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh))
+      assert held.effective_accel_max == pytest.approx(fresh.effective_accel_max)
       if frame < STOP_HOLD_EXIT_FRAMES - 1:
         assert fresh.state == AccelControllerState.stopHold
-        assert math.isinf(effective_accel_max(fresh))
+        assert math.isinf(fresh.effective_accel_max)
         assert fresh.mpc_accel_max is None
 
     assert fresh.launching and held.launching
     assert fresh.departure_launching and held.departure_launching
     assert fresh.target_speed == held.target_speed == 8.0
-    assert effective_accel_max(fresh) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 0.1))
+    assert fresh.effective_accel_max == pytest.approx(fresh.positive_accel_max)
 
   def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self):
     controller = make_controller()
@@ -721,8 +690,8 @@ class TestTargetLifecycle:
 
     assert warm.state == held.state == AccelControllerState.stopHold
     assert not warm.launching and not held.launching
-    assert math.isinf(effective_accel_max(warm)) and warm.mpc_accel_max is None
-    assert math.isinf(effective_accel_max(held)) and held.mpc_accel_max is None
+    assert math.isinf(warm.effective_accel_max) and warm.mpc_accel_max is None
+    assert math.isinf(held.effective_accel_max) and held.mpc_accel_max is None
     assert held.target_speed == 0.0
 
   def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self):
@@ -742,7 +711,7 @@ class TestTargetLifecycle:
 
     assert result.state == AccelControllerState.stopHold
     assert result.target_speed == 0.0
-    assert math.isinf(effective_accel_max(result))
+    assert math.isinf(result.effective_accel_max)
     assert result.mpc_accel_max is None
 
   def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self):
@@ -752,7 +721,7 @@ class TestTargetLifecycle:
 
     assert missing.state == AccelControllerState.stopHold
     assert missing.target_speed == 0.0
-    assert math.isinf(effective_accel_max(missing))
+    assert math.isinf(missing.effective_accel_max)
     assert missing.mpc_accel_max is None
 
   def test_confirmed_creep_departure_does_not_reenter_stop_hold(self):
@@ -795,7 +764,7 @@ class TestTargetLifecycle:
     assert guarded.state == AccelControllerState.stopHold
     assert guarded.target_speed == 0.0
 
-  def test_stale_timeout_fully_resets_live_state(self):
+  def test_stale_timeout_fully_resets_live_and_shadow(self):
     controller = make_controller()
     for _ in range(CAP_FILTER_FRAMES + 10):
       restricted = update(controller, restrictive_radar())
@@ -804,11 +773,11 @@ class TestTargetLifecycle:
     timed_out = update(controller, radar_fresh=False)
 
     assert all(result.active and result.target_speed == pytest.approx(restricted.target_speed) for result in held)
-    assert not timed_out.active
-    assert timed_out.target_speed == 25.0
+    assert not timed_out.active and not timed_out.shadow_active
+    assert timed_out.target_speed == timed_out.base_speed
     assert timed_out.mpc_accel_max is None
-    assert timed_out.selected_lead == -1 and controller._held_lead_plan is None
-    assert controller.target_state.target_speed is None
+    assert timed_out.selected_lead == -1 and math.isinf(timed_out.raw_energy_cap)
+    assert controller.live.pace is None and controller.shadow.pace is None
 
   @pytest.mark.parametrize("override", [{"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"a_ego": math.inf}])
   def test_bypass_or_invalid_context_resets_live_state(self, override):
@@ -818,65 +787,33 @@ class TestTargetLifecycle:
     result = update(controller, restrictive_radar(), **override)
 
     assert not result.active
-    assert result.target_speed == 25.0
+    assert result.target_speed == result.base_speed
     assert result.mpc_accel_max is None
-    assert controller.target_state.target_speed is None
+    assert controller.live.pace is None
 
-  def test_acc_bypass_does_not_retain_state_for_live_actuation(self):
+  def test_shadow_history_never_steps_into_live_actuation(self):
     controller = make_controller()
     for _ in range(CAP_FILTER_FRAMES + 20):
-      bypassed = update(controller, restrictive_radar(), acc_selected=False)
-      assert not bypassed.active
-      assert controller.target_state.target_speed is None
-      assert controller._held_lead_plan is None
+      shadow = update(controller, restrictive_radar(), acc_selected=False)
     live = update(controller)
 
-    assert live.active and live.target_speed == 25.0
-    assert math.isinf(controller.target_state.filtered_cap)
+    assert shadow.shadow_active and not shadow.active
+    assert shadow.shadow_filtered_cap < math.inf
+    assert live.active and live.target_speed == live.base_speed
+    assert math.isinf(live.live_filtered_cap)
 
-  def test_explicit_reset_clears_target_state(self):
+  def test_explicit_reset_clears_every_path_field(self):
     controller = make_controller()
     for _ in range(CAP_FILTER_FRAMES + 10):
       update(controller, restrictive_radar())
-    controller._jerk_smoothing_blocked = True
-    controller._required_decel_samples = [0.2]
-    controller._required_decel_lead = controller._required_decel_lead_track_id = 1
-    controller._lead_trend_warmup = True
     controller.reset()
 
-    assert controller._held_lead_plan is None
-    assert not controller._jerk_smoothing_blocked
-    assert controller._required_decel_samples == []
-    assert controller._required_decel_lead == controller._required_decel_lead_track_id == -1
-    assert not controller._lead_trend_warmup
-    target_state = controller.target_state
-    assert target_state.target_speed is None and target_state.matched_accel_limit is None
-    assert target_state.state == AccelControllerState.inactive
-    assert target_state.departure_frames == target_state.active_frames == target_state.lead_loss_frames == target_state.stale_frames == 0
-    assert target_state.lead_switch_guard_frames == 0
-    assert target_state.selected_lead == target_state.selected_lead_track_id == -1
-    assert target_state.cap_samples == [math.inf] * CAP_FILTER_FRAMES
-    assert target_state.lead_speed_samples == [math.inf] * CAP_FILTER_FRAMES
-    assert target_state.lead_accel_samples == [0.0] * CAP_FILTER_FRAMES
-    assert target_state.departure.samples == [[], []] and target_state.departure.motion_samples == []
-    assert target_state.departure.references == [None, None] and target_state.departure.track_ids == [-1, -1]
-    assert not target_state.launching and not target_state.departure_launch and not target_state.matched_lead
-    assert not target_state.lead_braking and not target_state.e2e_braking_handoff and not target_state.speed_reserve_armed
-    assert math.isinf(target_state.filtered_cap) and math.isinf(target_state.filtered_lead_speed) and target_state.filtered_lead_accel == 0.0
-
-  @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
-  def test_track_id_change_requires_new_history_before_jerk_smoothing(self, replacement_track_id):
-    controller = make_controller()
-    controller.state = AccelControllerState.restrict
-    controller.launching = False
-    controller.selected_lead = 0
-    controller.selected_lead_track_id = 100
-    controller.required_decel = 0.2
-    original = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
-
-    controller.selected_lead_track_id = replacement_track_id
-    replacement = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
-
-    assert original == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
-    assert replacement == [1.0, 1.0, 1.0, MPC_DECEL_JERK_COST_MULTIPLIER]
-    assert controller._required_decel_samples == [0.2] * 4
+    assert controller._held_envelope is None
+    for path in (controller.live, controller.shadow):
+      assert path.pace is None and path.matched_accel_limit is None
+      assert path.state == AccelControllerState.inactive
+      assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0
+      assert not path.departure_motion_samples
+      assert not path.launching and not path.departure_launch and not path.matched_lead
+      assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed
+      assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0

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

@@ -1,3 +1,4 @@
+from collections import deque
 import inspect
 import math
 from types import SimpleNamespace
@@ -10,11 +11,10 @@ from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile,
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
+  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
 )
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 
 
@@ -26,58 +26,14 @@ class PlannerSM(dict):
   def __init__(self, radar_log_mono_time: int):
     super().__init__(
       radarState=radar_state(),
-      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0),
+      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0),
       selfdriveState=SimpleNamespace(personality=0),
-      controlsState=SimpleNamespace(forceDecel=False),
     )
     self.valid = {"radarState": True}
     self.alive = {"radarState": True}
     self.logMonoTime = {"radarState": radar_log_mono_time}
 
 
-class ControllerStub:
-  def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, state=AccelControllerState.free, selected_lead=-1,
-               selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0):
-    self.available = self.enabled = True
-    self.profile = AccelProfile.normal
-    self.output_v_target = target_speed
-    self.is_active = active
-    self.mpc_accel_max = mpc_accel_max
-    self.state = state
-    self.selected_lead = selected_lead
-    self.selected_lead_track_id = selected_lead_track_id
-    self.launching = launching
-    self.departure_launching = departure_launching
-    self.required_decel = required_decel
-    self.dt = DT_MDL
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples = []
-    self._required_decel_lead = -1
-    self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-    self.update_kwargs = None
-    self.reset_calls = 0
-
-  def update(self, _radar_state, **kwargs):
-    self.update_kwargs = kwargs
-
-  @property
-  def is_enabled(self):
-    return self.available and self.enabled
-
-  def update_params(self):
-    pass
-
-  def reset(self):
-    self.reset_calls += 1
-
-  def get_jerk_cost_multiplier(self, *args):
-    return AccelController.get_jerk_cost_multiplier(self, *args)
-
-  def update_should_stop(self, should_stop):
-    return AccelController.update_should_stop(self, should_stop)
-
-
 def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None,
                          state=AccelControllerState.free, selected_lead=-1, launching=False,
                          departure_launching=False, required_decel=0.0,
@@ -85,75 +41,45 @@ def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_ac
   planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
   is_e2e_calls = []
   planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
-  planner.output_v_target = 20.0
-  planner.output_should_stop = False
-  planner.allow_throttle = True
-  planner.a_desired = 0.0
-  planner.v_desired_filter = SimpleNamespace(x=10.0)
-  planner._radar_fresh_this_cycle = True
+  planner._accel_jerk_smoothing_blocked = False
+  planner._accel_required_decel_samples = deque(maxlen=4)
+  planner._accel_required_decel_lead = -1
+  planner._dt = DT_MDL
   planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
-  planner.accel_controller = ControllerStub(
-    target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching,
-    departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max,
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
+    planner, "accel_controller_result",
+    SimpleNamespace(
+      target_speed=target_speed, active=active, state=state, selected_lead=selected_lead,
+      launching=launching, departure_launching=departure_launching,
+      required_decel=required_decel, mpc_accel_max=mpc_accel_max,
+    ),
   )
   return planner, is_e2e_calls
 
 
 def run_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False):
   calls = []
-  planner._run_mpc = lambda _sm, *args, **kwargs: calls.append((({}, *args), kwargs))
-  sm = {
-    "radarState": radar_state(),
-    "controlsState": SimpleNamespace(forceDecel=force_decel),
-    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
-    "selfdriveState": SimpleNamespace(personality=0),
-  }
-  is_e2e = planner.update_mpc(sm, mpc_v_cruise, True, ACCEL_MAX, False)
+  planner._run_mpc = lambda *args, **kwargs: calls.append((args, kwargs))
+  is_e2e = planner.update_accel_controller_mpc(
+    {}, 20.0, mpc_v_cruise, True, reset_state=False, cruise_initialized=True,
+    available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=force_decel,
+  )
   return is_e2e, calls
 
 
-def test_accel_controller_schema_contract():
+def test_profile_enum_keeps_toyota_importable():
   expected = {"eco": 0, "normal": 1, "sport": 2}
-  state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5}
-  accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"]
-  fields = custom.LongitudinalPlanSP.AccelController.schema.fields
-
-  assert accel_controller.proto.ordinal.explicit == 8
-  assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == {
-    "enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4,
-  }
-  assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool"
   assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
   assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
-  assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state
-
-
-def test_accel_controller_schema_round_trip_and_toyota_compatibility():
-  message = custom.LongitudinalPlanSP.new_message()
-  message.accelController.enabled = True
-  message.accelController.active = True
-  message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
-  message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release
-
-  with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
-    assert reader.accelController.enabled and reader.accelController.active
-    assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport
-    assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release
-
   from opendbc.car.toyota.carstate import AccelPersonality, CarState
 
-  assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2}
+  assert AccelPersonality.schema.enumerants == expected
   assert CarState.__module__ == "opendbc.car.toyota.carstate"
+  assert "Params()" not in inspect.getsource(CarState.update)
 
 
-def test_longitudinal_planner_sp_owns_accel_controller_integration():
-  assert "update_mpc" in LongitudinalPlannerSP.__dict__
-  assert "update_should_stop" in LongitudinalPlannerSP.__dict__
-
-
-def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds():
-  assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,)
-  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality")
+def test_mpc_accepts_optional_acceleration_ceiling_without_changing_stock_bounds():
+  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality", "accel_max")
   mpc = LongitudinalMpc()
   radar = radar_state()
   mpc.run = lambda: None
@@ -163,37 +89,28 @@ def test_mpc_inherits_accel_controller_extension_without_changing_stock_signatur
   np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
   np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
 
-  requested_ceiling = tuple(np.full(N + 1, 0.4))
-  mpc.set_accel_controller_params(requested_ceiling, 1.0)
-  mpc.update(radar, 30.0)
+  requested_ceiling = np.full(N + 1, 0.4)
+  mpc.update(radar, 30.0, accel_max=requested_ceiling)
   np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
   assert mpc.params[0, 1] == pytest.approx(0.8)
   np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:])
 
   for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)):
-    mpc.set_accel_controller_params(malformed_ceiling, 1.0)
-    mpc.update(radar, 30.0)
+    mpc.update(radar, 30.0, accel_max=malformed_ceiling)
     np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
     np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
 
-  mpc.set_accel_controller_params(None, 1.0)
-  mpc.update(radar, 30.0)
-  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
-
 
 def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs():
   mpc = LongitudinalMpc.__new__(LongitudinalMpc)
-  LongitudinalMpcSP.__init__(mpc)
   captured = []
   mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints)))
 
   mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
   default_costs, default_constraints = captured[-1]
-  mpc.set_accel_controller_params(None, 1.0)
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard, jerk_cost_multiplier=1.0)
   explicit_costs, explicit_constraints = captured[-1]
-  mpc.set_accel_controller_params(None, 1.2)
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard, jerk_cost_multiplier=1.2)
   smoothed_costs, smoothed_constraints = captured[-1]
 
   np.testing.assert_array_equal(explicit_costs, default_costs)
@@ -202,7 +119,7 @@ def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_oth
   assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2)
   np.testing.assert_array_equal(smoothed_constraints, default_constraints)
 
-  mpc.set_weights(False, personality=log.LongitudinalPersonality.standard)
+  mpc.set_weights(False, personality=log.LongitudinalPersonality.standard, jerk_cost_multiplier=1.2)
   assert captured[-1][0][-2] == 0.0
   assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
 
@@ -214,12 +131,13 @@ def test_inherited_planner_uses_real_state_raw_radar_and_one_mpc_solve():
   planner.v_desired_filter = SimpleNamespace(x=12.0)
   calls = []
 
-  def update_mpc(radar_arg, target, *, personality):
-    calls.append(("update", radar_arg, target, personality))
+  def update_mpc(radar_arg, target, *, personality, accel_max):
+    calls.append(("update", radar_arg, target, personality, accel_max))
 
   planner.mpc = SimpleNamespace(
-    set_accel_controller_params=lambda accel_max, multiplier: calls.append(("configure", accel_max, multiplier)),
-    set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)),
+    set_weights=lambda constraint, personality, jerk_cost_multiplier: calls.append(
+      ("weights", constraint, personality, jerk_cost_multiplier),
+    ),
     set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
     update=update_mpc,
   )
@@ -228,10 +146,9 @@ def test_inherited_planner_uses_real_state_raw_radar_and_one_mpc_solve():
   planner._run_mpc(sm, 17.5, True, ceiling, jerk_cost_multiplier=1.2)
 
   assert calls == [
-    ("configure", ceiling, 1.2),
-    ("weights", True, 2),
+    ("weights", True, 2, 1.2),
     ("state", 12.0, -0.2),
-    ("update", radar, 17.5, 2),
+    ("update", radar, 17.5, 2, ceiling),
   ]
   assert calls[-1][1] is radar
 
@@ -265,18 +182,22 @@ def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling()
 
 
 @pytest.mark.parametrize(
-  ("active", "departure_launching", "expected"),
+  ("active", "departure_launching", "is_e2e", "expected"),
   [
-    (True, True, False),
-    (True, False, True),
-    (False, True, True),
+    (True, True, False, False),
+    (True, False, False, True),
+    (False, True, False, True),
+    (True, True, True, True),
   ],
 )
-def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected):
+def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, is_e2e, expected):
   planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold)
-  assert planner.update_should_stop(True) is expected
-  assert planner.update_should_stop(False) is (active and not departure_launching)
+  planner.accel_controller_result = SimpleNamespace(
+    active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold,
+  )
+  assert planner.accel_controller_should_stop(True, is_e2e) is expected
+  expected_hold = active and not departure_launching and not is_e2e
+  assert planner.accel_controller_should_stop(False, is_e2e) is expected_hold
 
 
 @pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])
@@ -302,17 +223,18 @@ def test_force_decel_target_remains_authoritative_and_disables_ceiling():
 def test_previous_mpc_failure_gets_one_stock_recovery_cycle():
   ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
   planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
-  controller = planner.accel_controller
-  planner.mpc.last_solution_status = 4
+  resets = []
+  planner.accel_controller = SimpleNamespace(reset=lambda: resets.append(True))
+  planner.mpc = SimpleNamespace(last_solution_status=4)
 
   _, failed_recovery_calls = run_controller_mpc(planner)
-  assert controller.reset_calls == 1
+  assert resets == [True]
   assert len(mode_calls) == 1
   assert failed_recovery_calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
 
   planner.mpc.last_solution_status = 0
   _, recovered_calls = run_controller_mpc(planner)
-  assert controller.reset_calls == 1
+  assert resets == [True]
   assert len(mode_calls) == 2
   assert recovered_calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
 
@@ -336,22 +258,22 @@ def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_e
     state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
   )
   _, initial_calls = run_controller_mpc(planner)
-  controller = planner.accel_controller
+  routine_result = planner.accel_controller_result
   assert initial_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
 
-  controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL
+  ineligible_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": MPC_DECEL_JERK_MAX_REQUIRED_DECEL}))
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", ineligible_result)
   _, ineligible_calls = run_controller_mpc(planner)
   assert ineligible_calls[0][1] == {"jerk_cost_multiplier": 1.0}
 
-  controller.required_decel = 0.30
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
   _, flicker_calls = run_controller_mpc(planner)
   assert flicker_calls[0][1] == {"jerk_cost_multiplier": 1.0}
 
-  controller.state = AccelControllerState.free
-  controller.output_v_target = 20.0
+  free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0}))
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result)
   run_controller_mpc(planner)
-  controller.state = AccelControllerState.restrict
-  controller.output_v_target = 15.0
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
   _, rearmed_calls = run_controller_mpc(planner)
   assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
 
@@ -361,25 +283,25 @@ def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_e
     state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
   )
   _, calls = run_controller_mpc(planner)
-  controller = planner.accel_controller
+  routine_result = planner.accel_controller_result
   multipliers = [calls[0][1]["jerk_cost_multiplier"]]
   for required_decel in (0.20, 0.23, 0.25):
-    controller.required_decel = required_decel
+    result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
+    planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
     _, calls = run_controller_mpc(planner)
     multipliers.append(calls[0][1]["jerk_cost_multiplier"])
 
   assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
 
-  controller.required_decel = 0.20
+  easing_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": 0.20}))
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", easing_result)
   _, calls = run_controller_mpc(planner)
   assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
 
-  controller.state = AccelControllerState.free
-  controller.output_v_target = 20.0
+  free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0}))
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result)
   run_controller_mpc(planner)
-  controller.state = AccelControllerState.restrict
-  controller.output_v_target = 15.0
-  controller.required_decel = 0.18
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
   _, calls = run_controller_mpc(planner)
   assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
 
@@ -389,10 +311,11 @@ def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
     state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
   )
   _, calls = run_controller_mpc(planner)
-  controller = planner.accel_controller
+  routine_result = planner.accel_controller_result
   multipliers = [calls[0][1]["jerk_cost_multiplier"]]
   for required_decel in (0.24, 0.19, 0.22):
-    controller.required_decel = required_decel
+    result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
+    planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
     _, calls = run_controller_mpc(planner)
     multipliers.append(calls[0][1]["jerk_cost_multiplier"])
 
@@ -430,12 +353,25 @@ def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
 
 
 def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
-  planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
+  radar = radar_state()
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_personality = int(AccelProfile.normal)
+  planner.accel_personality_enabled = True
+  planner.accel_personality_available = True
   planner._radar_fresh_this_cycle = True
   planner.a_desired = -0.4
   planner.v_desired_filter = SimpleNamespace(x=9.5)
-  run_controller_mpc(planner)
-  received = planner.accel_controller.update_kwargs
+  planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
+  received = {}
+  planner.accel_controller = SimpleNamespace(
+    update=lambda *_args, **kwargs: received.update(kwargs) or SimpleNamespace(target_speed=12.0),
+  )
+  sm = {
+    "radarState": radar,
+    "carState": SimpleNamespace(vEgo=10.0, aEgo=-0.2),
+    "selfdriveState": SimpleNamespace(personality=0),
+  }
+  planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
 
   assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
   assert received["planner_speed"] == 9.5
@@ -444,50 +380,71 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
 
 
 def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
-  controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False))
-  controller.enabled = True
-  assert not controller.is_enabled
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_personality = int(AccelProfile.normal)
+  planner.accel_personality_enabled = True
+  planner.accel_personality_available = False
+  planner._radar_fresh_this_cycle = True
+  planner.accel_controller = SimpleNamespace(
+    update=lambda *_args, **kwargs: SimpleNamespace(target_speed=20.0, received_enabled=kwargs["enabled"]),
+  )
+  planner.a_desired = 0.0
+  planner.v_desired_filter = SimpleNamespace(x=10.0)
+  planner.mpc = SimpleNamespace(source=MpcLongitudinalPlanSource.cruise)
+  sm = {"radarState": radar_state(), "carState": SimpleNamespace(vEgo=10.0, aEgo=0.0), "selfdriveState": SimpleNamespace(personality=0)}
+
+  planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
+
+  assert not planner.accel_controller_result.received_enabled
 
 
 def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
   planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
   planner._radar_log_mono_time = None
   planner._radar_fresh_this_cycle = True
+  planner._read_accel_controller_params = lambda: None
   planner.events_sp = SimpleNamespace(clear=lambda: None)
   dec_freshness = []
   planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
   planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
+  planner.accel_personality = int(AccelProfile.normal)
+  planner.accel_personality_enabled = True
+  planner.accel_personality_available = True
   planner.output_a_target = 0.0
-  planner.output_v_target = 20.0
-  planner.output_should_stop = False
-  planner.allow_throttle = True
   planner.a_desired = 0.0
   planner.v_desired_filter = SimpleNamespace(x=10.0)
-  planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0)
-  planner.is_e2e = lambda _sm: False
-  planner._run_mpc = lambda *_args, **_kwargs: None
-  planner.accel_controller = ControllerStub(target_speed=20.0, active=False)
+  planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise)
+  controller_freshness = []
+  planner.accel_controller = SimpleNamespace(
+    update=lambda *_args, **kwargs: controller_freshness.append(kwargs["radar_fresh"]) or SimpleNamespace(target_speed=20.0),
+  )
 
   sm = PlannerSM(100)
   for expected in (True, False):
     planner.update(sm)
-    planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False)
-    assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected
+    planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
+    assert dec_freshness[-1] is expected and controller_freshness[-1] is expected
 
   sm.logMonoTime["radarState"] = 101
   planner.update(sm)
-  planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False)
-  assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True
+  planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
+  assert dec_freshness[-1] is True and controller_freshness[-1] is True
 
 
-def test_accel_controller_status_publishes_minimal_fields():
+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 = ControllerStub(active=False, state=AccelControllerState.restrict)
+  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=math.inf, shadow_filtered_cap=12.5,
+    selected_lead=1, usable_gap=30.0, closing_speed=5.0, required_decel=0.4,
+    profile_accel_max=math.inf, effective_accel_max=math.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),
@@ -509,7 +466,15 @@ def test_accel_controller_status_publishes_minimal_fields():
   )
 
   telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
-  assert telemetry.enabled and not telemetry.active
+  assert telemetry.enabled and not telemetry.active and telemetry.shadowOnly
   assert telemetry.profile == int(AccelProfile.normal)
   assert telemetry.state == int(AccelControllerState.restrict)
-  assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}
+  assert telemetry.vTargetBase == pytest.approx(20.0)
+  assert telemetry.vTargetRaw == pytest.approx(15.0)
+  assert telemetry.vTargetShadow == pytest.approx(12.5)
+  assert telemetry.leadIndex == 1
+  assert telemetry.usableGap == pytest.approx(30.0)
+  assert telemetry.closingSpeed == pytest.approx(5.0)
+  assert telemetry.requiredDecel == pytest.approx(0.4)
+  assert telemetry.aMaxProfile == math.inf
+  assert telemetry.aMaxEffective == math.inf

sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py

@@ -33,23 +33,6 @@ LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
 FRICTION_THRESHOLD = 0.3
 VERSION = 0
 
-PRIUS_TSS2 = "TOYOTA_PRIUS_TSS2"
-PRIUS_KP = 0.8
-PRIUS_KI = 0.15
-PRIUS_LAT_ACCEL_FACTOR = 1.65
-PRIUS_LAT_ACCEL_OFFSET = -0.25
-PRIUS_FRICTION = 0.168
-PRIUS_TORQUE_RATE_UP = 1.0
-PRIUS_TORQUE_RATE_DOWN = 5.0 / 3.0
-PRIUS_UNWIND_I_DECAY = 0.99
-PRIUS_UNWIND_JERK_THRESHOLD = 0.1
-
-
-def limit_torque_rate(desired_torque, last_torque, rate_up, rate_down, dt):
-  increasing_magnitude = desired_torque * last_torque >= 0.0 and abs(desired_torque) > abs(last_torque)
-  max_delta = (rate_up if increasing_magnitude else rate_down) * dt
-  return float(np.clip(desired_torque, last_torque - max_delta, last_torque + max_delta))
-
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CP_SP, CI, dt):
@@ -57,19 +40,8 @@ class LatControlTorque(LatControl):
     self.torque_params = CP.lateralTuning.torque.as_builder()
     self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
     self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
-    self.prius_smooth_tune = CP.carFingerprint == PRIUS_TSS2
-    if self.prius_smooth_tune:
-      self._set_prius_torque_params()
-
-    kp_interp = KP_INTERP.copy()
-    ki = KI
-    if self.prius_smooth_tune:
-      kp_interp[-1] = PRIUS_KP
-      ki = PRIUS_KI
-
-    self.pid = PIDController([INTERP_SPEEDS, kp_interp], ki, KD, rate=1/self.dt)
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt)
     self.update_limits()
-    self.output_torque_last = 0.0
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
     self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
     self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
@@ -78,18 +50,10 @@ class LatControlTorque(LatControl):
 
     self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
 
-  def _set_prius_torque_params(self):
-    self.torque_params.latAccelFactor = PRIUS_LAT_ACCEL_FACTOR
-    self.torque_params.latAccelOffset = PRIUS_LAT_ACCEL_OFFSET
-    self.torque_params.friction = PRIUS_FRICTION
-
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
-    if self.prius_smooth_tune:
-      self._set_prius_torque_params()
-    else:
-      self.torque_params.latAccelFactor = latAccelFactor
-      self.torque_params.latAccelOffset = latAccelOffset
-      self.torque_params.friction = friction
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
     self.update_limits()
 
   def update_limits(self):
@@ -99,15 +63,12 @@ class LatControlTorque(LatControl):
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
     # Override torque params from extension
     if self.extension.update_override_torque_params(self.torque_params):
-      if self.prius_smooth_tune:
-        self._set_prius_torque_params()
       self.update_limits()
 
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     pid_log.version = VERSION
     if not active:
       output_torque = 0.0
-      self.output_torque_last = 0.0
       pid_log.active = False
     else:
       measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
@@ -138,12 +99,7 @@ class LatControlTorque(LatControl):
       # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
       ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
-      unwinding = ((abs(desired_lateral_jerk) > PRIUS_UNWIND_JERK_THRESHOLD and setpoint * desired_lateral_jerk <= 0.0) or
-                   setpoint * measurement < 0.0)
-      if self.prius_smooth_tune and unwinding:
-        self.pid.i *= PRIUS_UNWIND_I_DECAY
-
-      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 or (self.prius_smooth_tune and unwinding)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error,
                                        -measurement_rate,
                                         feedforward=ff,
@@ -157,10 +113,6 @@ class LatControlTorque(LatControl):
                                                      future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
                                                      desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
 
-      if self.prius_smooth_tune:
-        output_torque = limit_torque_rate(output_torque, self.output_torque_last, PRIUS_TORQUE_RATE_UP, PRIUS_TORQUE_RATE_DOWN, self.dt)
-      self.output_torque_last = output_torque
-
       pid_log.active = True
       pid_log.p = float(self.pid.p)
       pid_log.i = float(self.pid.i)

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

@@ -1,38 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import numpy as np
-
-from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
-
-
-class LongitudinalMpcSP:
-  def __init__(self) -> None:
-    self._accel_max_trajectory: tuple[float, ...] | None = None
-    self._jerk_cost_multiplier = 1.0
-    self.last_solution_status = 0
-
-  def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float) -> None:
-    self._accel_max_trajectory = accel_max
-    self._jerk_cost_multiplier = jerk_cost_multiplier
-
-  def scale_jerk_cost(self, jerk_cost: float) -> float:
-    return jerk_cost * self._jerk_cost_multiplier
-
-  def apply_accel_limits(self) -> None:
-    if self._accel_max_trajectory is None:
-      return
-
-    accel_max = np.asarray(self._accel_max_trajectory)
-    if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)):
-      return
-
-    self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX)
-    self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
-
-  def save_solution_status(self) -> None:
-    self.last_solution_status = self.solution_status

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,12 +5,21 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 
-from cereal import custom, messaging
+from collections import deque
+import math
+
+from cereal import messaging, custom
 from opendbc.car import structs
 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, V_CRUISE_UNSET
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+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_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
+  MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
+)
 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
@@ -25,9 +34,9 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
-    self.mpc = mpc
-    self.accel_controller = AccelController(CP, dt=dt)
+    self.params = Params()
     self.events_sp = EventsSP()
+    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -35,12 +44,33 @@ 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_personality_available = bool(CP.openpilotLongitudinalControl)
+    self._accel_jerk_smoothing_blocked = False
+    self._accel_required_decel_samples = deque(maxlen=4)
+    self._accel_required_decel_lead = -1
+    self._dt = dt
     self._radar_log_mono_time = None
     self._radar_fresh_this_cycle = True
 
+    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():
@@ -48,41 +78,6 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
-  def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, jerk_cost_multiplier: float = 1.0) -> None:
-    self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier)
-    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)
-
-  def update_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, stock_accel_max: float, reset_state: bool) -> bool:
-    is_e2e = self.is_e2e(sm)
-    force_decel = sm['controlsState'].forceDecel
-    previous_mpc_failed = self.mpc.last_solution_status != 0
-    if previous_mpc_failed:
-      self.accel_controller.reset()
-
-    self.accel_controller.update(
-      sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
-      follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e and not previous_mpc_failed,
-      engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET,
-      stock_accel_max=stock_accel_max if self.allow_throttle else 0.0, previous_should_stop=self.output_should_stop,
-      radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x,
-      planner_accel=self.a_desired,
-    )
-    controller = self.accel_controller
-    actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed
-    valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0
-    controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise
-    accel_max = controller.mpc_accel_max if actuating else None
-    jerk_cost_multiplier = controller.get_jerk_cost_multiplier(
-      actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed,
-    )
-    self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, jerk_cost_multiplier=jerk_cost_multiplier)
-    return is_e2e
-
-  def update_should_stop(self, should_stop: bool) -> bool:
-    return self.accel_controller.update_should_stop(should_stop)
-
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -114,16 +109,100 @@ class LongitudinalPlannerSP:
     return self.output_v_target, self.output_a_target
 
   def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
-    radar_log_mono_time = sm.logMonoTime['radarState']
-    radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
-    radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
+    try:
+      radar_log_mono_time = int(sm.logMonoTime['radarState'])
+      radar_healthy = bool(sm.valid['radarState'] and sm.alive['radarState'])
+    except (AttributeError, KeyError, TypeError, ValueError):
+      return True
+
+    previous_log_mono_time = getattr(self, '_radar_log_mono_time', None)
+    radar_advanced = previous_log_mono_time is None or radar_log_mono_time > previous_log_mono_time
     if radar_advanced:
       self._radar_log_mono_time = radar_log_mono_time
     return radar_healthy and radar_advanced
 
+  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
+                              acc_selected: bool, stock_accel_max: float, previous_should_stop: bool) -> 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 and self.accel_personality_available,
+      acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
+      stock_accel_max=stock_accel_max, previous_should_stop=previous_should_stop,
+      radar_fresh=getattr(self, '_radar_fresh_this_cycle', True),
+      previous_mpc_source=getattr(getattr(self, 'mpc', None), 'source', None),
+      planner_speed=getattr(getattr(self, 'v_desired_filter', None), 'x', sm['carState'].vEgo),
+      planner_accel=getattr(self, 'a_desired', sm['carState'].aEgo),
+    )
+    return self.accel_controller_result.target_speed
+
+  def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None,
+               *, jerk_cost_multiplier: float = 1.0) -> None:
+    self.mpc.set_weights(
+      prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_cost_multiplier=jerk_cost_multiplier,
+    )
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=accel_max)
+
+  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,
+                                  available_accel_max: float, previous_should_stop: bool, force_decel: bool):
+    is_e2e = self.is_e2e(sm)
+    previous_mpc_failed = getattr(getattr(self, 'mpc', None), 'last_solution_status', 0) != 0
+    if previous_mpc_failed and hasattr(self, 'accel_controller'):
+      self.accel_controller.reset()
+
+    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 and not previous_mpc_failed, stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop,
+    )
+    result = self.accel_controller_result
+    actuating = result.active and not is_e2e and not force_decel and not previous_mpc_failed
+    valid_lead_stop_hold = (actuating and result.state == AccelControllerState.stopHold
+                            and result.selected_lead >= 0)
+    controller_v_cruise = mpc_v_cruise if valid_lead_stop_hold else min(mpc_v_cruise, result.target_speed) if actuating else mpc_v_cruise
+    accel_max = result.mpc_accel_max if actuating else None
+    target_reduction = mpc_v_cruise - controller_v_cruise
+    lead_restriction = (
+      actuating and prev_accel_constraint and result.state == AccelControllerState.restrict and result.selected_lead >= 0
+      and not result.launching and target_reduction > 1e-6
+    )
+    if not lead_restriction or result.selected_lead != self._accel_required_decel_lead or not math.isfinite(result.required_decel):
+      self._accel_required_decel_samples.clear()
+    if lead_restriction and math.isfinite(result.required_decel):
+      self._accel_required_decel_samples.append(result.required_decel)
+    self._accel_required_decel_lead = result.selected_lead if lead_restriction else -1
+    required_decel_history = tuple(self._accel_required_decel_samples)
+    tightening_lead = (len(required_decel_history) == self._accel_required_decel_samples.maxlen
+                       and (required_decel_history[-1] - required_decel_history[0]) /
+                       (self._dt * (len(required_decel_history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
+                       and sum(after > before for before, after in zip(required_decel_history[:-1], required_decel_history[1:], strict=True)) >= 2)
+    smoothing_eligible = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
+                          and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL and not tightening_lead)
+    smoothing_blocked = getattr(self, '_accel_jerk_smoothing_blocked', False)
+    if previous_mpc_failed:
+      smoothing_blocked = True
+    elif not lead_restriction:
+      smoothing_blocked = False
+    elif not smoothing_blocked and not smoothing_eligible:
+      smoothing_blocked = True
+    self._accel_jerk_smoothing_blocked = smoothing_blocked
+    jerk_cost_multiplier = MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not smoothing_blocked else 1.0
+    self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, jerk_cost_multiplier=jerk_cost_multiplier)
+
+    return is_e2e
+
+  def accel_controller_should_stop(self, should_stop: bool, is_e2e: bool) -> bool:
+    result = self.accel_controller_result
+    if result is None or not result.active or is_e2e:
+      return should_stop
+    if result.departure_launching:
+      return False
+    return should_stop or result.state == AccelControllerState.stopHold
+
   def update(self, sm: messaging.SubMaster) -> None:
     self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
-    self.accel_controller.update_params()
+    self._read_accel_controller_params()
     self.events_sp.clear()
     self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -145,11 +224,24 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
-    accelController = longitudinalPlanSP.accelController
-    accelController.enabled = self.accel_controller.is_enabled
-    accelController.active = self.accel_controller.is_active
-    accelController.profile = self.accel_controller.profile
-    accelController.state = self.accel_controller.state
+    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

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py

@@ -18,8 +18,8 @@ def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 7
   curvature = 0.005
   plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20)
   planner = plant.planner
-  planner.accel_controller.enabled = False
-  planner.accel_controller.update_params = lambda: None
+  planner.accel_personality_enabled = False
+  planner._read_accel_controller_params = lambda: None
   planner.dec._enabled = False
   planner.dec._read_params = lambda: None
   planner.scc.map.enabled = False

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

@@ -1,7 +1,6 @@
 from collections.abc import Callable
 from dataclasses import dataclass
 import gc
-import math
 
 import numpy as np
 import pytest
@@ -10,12 +9,12 @@ 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 LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW
 from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, PlantSP as Plant
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL,
-  MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_TREND_FRAMES, TARGET_SPEED_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
+from openpilot.sunnypilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_sp
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
+  MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, PACE_TARGET_RESERVE,
+  STOP_HOLD_EXIT_FRAMES,
 )
 
 ACTUATOR_DYNAMICS = (
@@ -45,21 +44,24 @@ class ClosedLoopTrace:
   source: list
   dec_mode: list[str]
   active: np.ndarray
+  shadow_active: np.ndarray
   launching: np.ndarray
-  departure_launching: np.ndarray
   target_speed: np.ndarray
   raw_cap: np.ndarray
+  filtered_cap: np.ndarray
   selected_lead: np.ndarray
   profile_accel_max: np.ndarray
-  accel_ceiling_active: np.ndarray
+  effective_accel_max: np.ndarray
   state: np.ndarray
   required_decel: np.ndarray
   planner_seed_accel: np.ndarray
   mpc_seed_accel: np.ndarray
   mpc_upper_first: np.ndarray
   mpc_upper_min: np.ndarray
+  mpc_upper_max: np.ndarray
   stock_bounds_valid: np.ndarray
   raw_radar_passthrough: np.ndarray
+  actuator_command: np.ndarray
   solver_status: np.ndarray
   mpc_calls: np.ndarray
   solver_failures: int
@@ -67,9 +69,9 @@ class ClosedLoopTrace:
 
 
 def _configure_plant(plant: Plant, *, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None:
-  plant.planner.accel_controller.enabled = enabled
-  plant.planner.accel_controller.profile = profile
-  plant.planner.accel_controller.update_params = lambda: 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
 
@@ -147,7 +149,7 @@ def _run(
       radar_checks_before = len(radar_passthrough)
       seed_calls_before = len(seed_calls)
       result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
-      controller = plant.planner.accel_controller
+      controller = plant.planner.accel_controller_result
       calls_this_frame = mpc_call_count - calls_before
       passthrough_this_frame = (len(radar_passthrough) > radar_checks_before
                                 and all(radar_passthrough[radar_checks_before:]))
@@ -157,20 +159,16 @@ def _run(
         planner_seed_accel = mpc_seed_accel = np.nan
       lower = plant.planner.mpc.params[:, 0]
       upper = plant.planner.mpc.params[:, 1]
-      lead = plant.planner.accel_controller._held_lead_plan
-      raw_cap = lead.cap if lead is not None else math.inf
-      profile_accel_max = (plant.planner.accel_controller.get_profile_accel_max(
-        controller.profile, result["published_v_ego"],
-      ) if controller.is_active else math.inf)
       bounds_valid = (np.allclose(lower, ACCEL_MIN) and np.all(np.isfinite(upper))
                       and np.all(upper >= lower) and np.all(upper <= ACCEL_MAX + 1e-9))
       rows.append((
         plant.current_time, result["speed"], result["distance"], result["distance_lead"], result["a_target"],
-        result["realized_acceleration"], result["should_stop"], result["fcw"], controller.is_active,
-        controller.launching, controller.departure_launching, controller.output_v_target, raw_cap, controller.selected_lead,
-        profile_accel_max, controller.mpc_accel_max is not None, controller.state, controller.required_decel, planner_seed_accel,
-        mpc_seed_accel, upper[0], np.min(upper), bounds_valid, passthrough_this_frame,
-        plant.planner.mpc.last_solution_status, calls_this_frame,
+        result["realized_acceleration"], result["should_stop"], result["fcw"], controller.active,
+        controller.shadow_active, controller.launching, controller.target_speed, controller.raw_energy_cap,
+        controller.live_filtered_cap, controller.selected_lead, controller.profile_accel_max,
+        controller.effective_accel_max, controller.state, controller.required_decel, planner_seed_accel,
+        mpc_seed_accel, upper[0], np.min(upper), np.max(upper), bounds_valid, passthrough_this_frame,
+        result["actuator_command"], plant.planner.mpc.last_solution_status, calls_this_frame,
       ))
       sources.append(result["mpc_source"])
       dec_modes.append(result["dec_mode"])
@@ -184,12 +182,12 @@ def _run(
   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, dec_mode=dec_modes,
-    active=data[:, 8].astype(bool), launching=data[:, 9].astype(bool), departure_launching=data[:, 10].astype(bool),
-    target_speed=data[:, 11], raw_cap=data[:, 12], selected_lead=data[:, 13].astype(int), profile_accel_max=data[:, 14],
-    accel_ceiling_active=data[:, 15].astype(bool), state=data[:, 16].astype(int), required_decel=data[:, 17], planner_seed_accel=data[:, 18],
-    mpc_seed_accel=data[:, 19], mpc_upper_first=data[:, 20], mpc_upper_min=data[:, 21],
-    stock_bounds_valid=data[:, 22].astype(bool), raw_radar_passthrough=data[:, 23].astype(bool),
-    solver_status=data[:, 24].astype(int), mpc_calls=data[:, 25].astype(int), solver_failures=solver_failures,
+    active=data[:, 8].astype(bool), shadow_active=data[:, 9].astype(bool), launching=data[:, 10].astype(bool), target_speed=data[:, 11],
+    raw_cap=data[:, 12], filtered_cap=data[:, 13], selected_lead=data[:, 14].astype(int), profile_accel_max=data[:, 15],
+    effective_accel_max=data[:, 16], state=data[:, 17].astype(int), required_decel=data[:, 18], planner_seed_accel=data[:, 19],
+    mpc_seed_accel=data[:, 20], mpc_upper_first=data[:, 21], mpc_upper_min=data[:, 22], mpc_upper_max=data[:, 23],
+    stock_bounds_valid=data[:, 24].astype(bool), raw_radar_passthrough=data[:, 25].astype(bool), actuator_command=data[:, 26],
+    solver_status=data[:, 27].astype(int), mpc_calls=data[:, 28].astype(int), solver_failures=solver_failures,
     solver_failure_times=solver_failure_times,
   )
   gc.collect()
@@ -274,20 +272,21 @@ def _assert_no_new_solver_failures(trace: ClosedLoopTrace, baseline: ClosedLoopT
 
 
 @pytest.mark.parametrize(
-  "plant_kwargs",
+  ("plant_kwargs", "expect_shadow"),
   [
-    {"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0},
-    {"e2e": True, "lead_relevancy": False, "speed": 20.0},
+    ({"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"),
+  ids=("disengaged", "e2e-shadow"),
 )
-def test_non_actuating_modes_match_stock(plant_kwargs):
+def test_non_actuating_modes_match_stock(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)
 
   _assert_non_actuating_matches_stock(trace, baseline)
   assert not trace.active.any()
+  np.testing.assert_array_equal(trace.shadow_active, np.full_like(trace.active, expect_shadow))
   np.testing.assert_allclose(trace.mpc_upper_min, baseline.mpc_upper_min)
   assert trace.raw_radar_passthrough.all()
   assert np.all(trace.mpc_calls == 1)
@@ -341,7 +340,7 @@ def test_e2e_to_radar_acc_handoff_keeps_braking_continuous():
       plant.e2e = plant.current_time < 2.0
       result = plant.step(v_lead=8.0, v_cruise=20.0)
       rows.append((plant.current_time, result["a_target"], plant.planner.mpc.last_solution_status,
-                   plant.planner.accel_controller.is_active))
+                   plant.planner.accel_controller_result.active))
     return np.asarray(rows, dtype=float).T
 
   baseline_time, baseline_accel, baseline_status, _ = run_handoff(False)
@@ -458,14 +457,14 @@ def test_decel_smoothing_does_not_change_clear_road_acceleration_at_representati
     duration=3.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False,
     speed=speed, v_cruise=v_cruise, actuator_delay=0.15, actuator_lag=0.20,
   )
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
   stock_weight = _run(**common)
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
   smoothed = _run(**common)
 
   np.testing.assert_allclose(smoothed.a_target, stock_weight.a_target, atol=1e-9, rtol=0.0)
   np.testing.assert_allclose(smoothed.speed, stock_weight.speed, atol=1e-9, rtol=0.0)
-  np.testing.assert_allclose(smoothed.mpc_upper_min, stock_weight.mpc_upper_min, atol=1e-9, rtol=0.0)
+  np.testing.assert_allclose(smoothed.effective_accel_max, stock_weight.effective_accel_max, atol=1e-9, rtol=0.0)
   assert smoothed.solver_failures == stock_weight.solver_failures == 0
 
 
@@ -477,9 +476,9 @@ def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_brakin
     duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=29.0,
     distance_lead=80.0, v_lead=lead_speed, v_cruise=33.528, actuator_delay=0.15, actuator_lag=0.20,
   )
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
   baseline = _run(**common)
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
   smoothed = _run(**common)
   response = smoothed.time >= 0.5
   baseline_gap = baseline.distance_lead - baseline.distance
@@ -514,9 +513,9 @@ def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch):
     v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20,
   )
   stock = _run(controller_enabled=False, **common)
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf)
   always_smoothed = _run(controller_enabled=True, **common)
-  monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE)
+  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE)
   trace = _run(controller_enabled=True, **common)
   response = trace.time >= event_time
   response_jerk = trace.time[1:] >= event_time
@@ -535,65 +534,6 @@ def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch):
   assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0
 
 
-def test_same_slot_track_replacement_never_delays_tightening_lead_braking():
-  event_time = 3.0
-  lead_jerk = 1.02
-  max_lead_decel = 2.22
-  ramp_time = max_lead_decel / lead_jerk
-
-  def lead_speed(current_time: float) -> float:
-    braking_time = max(current_time - event_time, 0.0)
-    ramp = min(braking_time, ramp_time)
-    return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0)
-
-  def observe(switch_time: float | None):
-    def observation(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-      if lead_name == "leadTwo":
-        return None
-      track_id = 200 if switch_time is not None and current_time >= switch_time else 100
-      return truth | {"radar": True, "radarTrackId": track_id}
-    return observation
-
-  common = dict(
-    duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7,
-    v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  baseline = _run(lead_observation_fn=observe(None), **common)
-  history = MPC_DECEL_TREND_FRAMES - 1
-  rate = np.full_like(baseline.required_decel, -math.inf)
-  rate[history:] = (baseline.required_decel[history:] - baseline.required_decel[:-history]) / (history * DT_MDL)
-  candidates = np.flatnonzero(
-    (baseline.time >= event_time)
-    & (baseline.state == int(AccelControllerState.restrict))
-    & (baseline.required_decel > 0.0)
-    & (baseline.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
-    & (rate > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE)
-    & (baseline.a_target > -1.0)
-  )
-  assert len(candidates)
-  switch_time = float(baseline.time[candidates[0]])
-  switched = _run(lead_observation_fn=observe(switch_time), **common)
-  response = switched.time >= switch_time
-  baseline_gap = baseline.distance_lead - baseline.distance
-  switched_gap = switched.distance_lead - switched.distance
-
-  def minimum_ttc(trace: ClosedLoopTrace, gap: np.ndarray) -> float:
-    closing_speed = trace.speed - np.asarray([lead_speed(current_time) for current_time in trace.time])
-    closing = closing_speed > 0.1
-    assert closing.any()
-    return float(np.min(gap[closing] / closing_speed[closing]))
-
-  assert np.all(baseline.selected_lead[response] == 0)
-  assert np.all(switched.selected_lead[response] == 0)
-  for threshold in (-1.0, -2.0):
-    assert _first_time_below(switched, threshold) <= _first_time_below(baseline, threshold) + 1e-9
-  assert np.min(switched_gap) >= np.min(baseline_gap) - 0.02
-  assert minimum_ttc(switched, switched_gap) >= minimum_ttc(baseline, baseline_gap) - 0.02
-  assert np.min(switched_gap) > 0.0
-  assert not switched.fcw.any()
-  assert baseline.solver_failures == switched.solver_failures == 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,
@@ -773,83 +713,6 @@ def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_sec
   assert trace.solver_failures == 0
 
 
-def test_stop_hold_departure_survives_radar_staleness():
-  departure_time = 1.0
-  dropout_start_time = 1.3
-  dropout_len_frames = round(0.8 / DT_MDL)
-  dropout_start_frame = round(dropout_start_time / DT_MDL)
-
-  def lead_speed(current_time: float) -> float:
-    return 0.0 if current_time < departure_time else 2.0
-
-  def radar_fresh_fn(frame: int) -> bool:
-    return not (dropout_start_frame <= frame < dropout_start_frame + dropout_len_frames)
-
-  common = dict(
-    duration=4.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=lead_speed, v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25,
-  )
-  baseline = _run(**common)
-  trace = _run(radar_fresh_fn=radar_fresh_fn, **common)
-  just_before_dropout = (trace.time >= dropout_start_time - 2 * DT_MDL) & (trace.time < dropout_start_time)
-  after_recovery = trace.time >= dropout_start_time + dropout_len_frames * DT_MDL
-  gap = trace.distance_lead - trace.distance
-  baseline_gap = baseline.distance_lead - baseline.distance
-
-  assert trace.departure_launching[just_before_dropout].all()
-  assert not _has_propulsion_brake_cycle(trace.a_target)
-  assert not _has_brake_coast_brake(trace.a_target)
-  assert np.max(np.abs(np.diff(trace.a_target) / DT_MDL)) < 3.0
-  assert np.min(gap) >= np.min(baseline_gap) - DROPOUT_GAP_TOLERANCE
-  assert not trace.fcw.any()
-  _assert_no_new_solver_failures(trace, baseline)
-  assert trace.solver_failures == 0
-  assert trace.launching[after_recovery].any()
-  assert trace.time[np.flatnonzero(after_recovery & trace.launching)[0]] <= dropout_start_time + dropout_len_frames * DT_MDL + 1.0
-
-
-def test_confirmed_departure_full_field_dropout_does_not_worsen_stock_response():
-  departure_time = 1.0
-  dropout_start = 1.3
-  dropout_end = 1.45
-  dropped: list[tuple[int, str]] = []
-
-  def lead_speed(current_time: float) -> float:
-    return 0.0 if current_time < departure_time else 2.0
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    if dropout_start <= current_time < dropout_end:
-      dropped.append((round(current_time / DT_MDL), lead_name))
-      return None
-    return truth
-
-  common = dict(
-    duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed,
-    v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, lead_observation_fn=observe,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  dropped.clear()
-  trace = _run(controller_enabled=True, **common)
-  before_dropout = (trace.time >= dropout_start - 2 * DT_MDL) & (trace.time < dropout_start)
-  dropout = (trace.time > dropout_start) & (trace.time <= dropout_end)
-  jerk_window = (trace.time[1:] > dropout_start) & (trace.time[1:] <= dropout_end + 0.25)
-  dropped_by_frame: dict[int, set[str]] = {}
-  for frame, lead_name in dropped:
-    dropped_by_frame.setdefault(frame, set()).add(lead_name)
-
-  assert trace.departure_launching[before_dropout].all()
-  assert len(dropped_by_frame) == round((dropout_end - dropout_start) / DT_MDL)
-  assert all(lead_names == {"leadOne", "leadTwo"} for lead_names in dropped_by_frame.values())
-  assert np.all(trace.selected_lead[dropout] == -1) and np.all(np.isinf(trace.raw_cap[dropout]))
-  assert not np.any(trace.state[dropout] == int(AccelControllerState.stopHold))
-  assert not _has_propulsion_brake_cycle(trace.a_target) and not _has_brake_coast_brake(trace.a_target)
-  assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) <= np.max(np.abs(np.diff(baseline.a_target)[jerk_window] / DT_MDL)) + 0.01
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE
-  assert not trace.fcw.any()
-  np.testing.assert_array_equal(trace.solver_status, baseline.solver_status)
-  assert trace.solver_failure_times == baseline.solver_failure_times
-
-
 def test_reused_radar_frames_do_not_pulse_stop_state_during_departure():
   departure_time = 1.0
   trace = _run(
@@ -982,7 +845,7 @@ def test_invalid_departure_geometry_aborts_launch_until_reconfirmed():
   assert trace.solver_failures == 0
 
 
-def test_moving_full_field_dropout_never_releases_speed_or_adds_solver_failures():
+def test_moving_full_field_dropout_never_releases_pace_or_adds_solver_failures():
   dropout_start = 2.0
   dropout_end = 2.15
 
@@ -1070,7 +933,7 @@ def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, a
   assert not _has_propulsion_brake_cycle(trace.a_target[response])
   assert not _has_brake_coast_brake(trace.a_target[response])
   assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
-  assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(TARGET_SPEED_RESERVE, MATCHED_SPEED_DECEL_RATE * DT_MDL) + 1e-9
+  assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(PACE_TARGET_RESERVE, MATCHED_PACE_DECEL_RATE * DT_MDL) + 1e-9
   assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE
   assert not trace.fcw.any()
   assert trace.solver_failures == 0
@@ -1079,7 +942,7 @@ def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, a
 
 
 @pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-def test_profile_ceiling_and_speed_stay_smooth_through_slot_switch_noise(profile):
+def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile):
   glitch_start = 24.0
   glitch_end = 28.0
 
@@ -1110,16 +973,15 @@ def test_profile_ceiling_and_speed_stay_smooth_through_slot_switch_noise(profile
   trace = _run(lead_observation_fn=observe, **common)
   glitch = (trace.time >= glitch_start) & (trace.time < glitch_end)
   response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0)
-  applied_accel_max = trace.mpc_upper_min[response]
+  effective_accel_max = trace.effective_accel_max[response]
   selected_leads = trace.selected_lead[glitch]
-  controller_limited = trace.accel_ceiling_active[response]
+  finite_limits = np.isfinite(effective_accel_max)
   stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]])
 
   assert set(selected_leads) == {0, 1}
   assert np.count_nonzero(np.diff(selected_leads)) > 20
-  assert controller_limited.any()
-  assert np.all(applied_accel_max[controller_limited] <= trace.profile_accel_max[response][controller_limited] + 1e-6)
-  assert np.all(applied_accel_max[controller_limited] <= stock_accel_max[controller_limited] + 1e-6)
+  assert np.all(effective_accel_max[finite_limits] <= trace.profile_accel_max[response][finite_limits] + 1e-9)
+  assert np.all(effective_accel_max[finite_limits] <= stock_accel_max[finite_limits] + 1e-9)
   assert np.max(trace.a_target[response]) > 0.2
   assert not _has_propulsion_brake_cycle(trace.a_target[response])
   assert not _has_brake_coast_brake(trace.a_target[response])
@@ -1148,7 +1010,7 @@ def test_matched_lead_dropout_keeps_the_profile_acceleration_ceiling(profile):
   dropout = (trace.time >= dropout_start) & (trace.time <= dropout_end)
   response = (trace.time >= dropout_start - 0.5) & (trace.time <= dropout_end + 0.75)
 
-  assert trace.accel_ceiling_active[dropout].all()
+  assert np.all(np.isfinite(trace.effective_accel_max[dropout]))
   assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.08
   assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
   assert not _has_propulsion_brake_cycle(trace.a_target[response])
@@ -1333,8 +1195,7 @@ def test_matched_lead_slowdown_stays_smooth_without_a_second_braking_stage():
   desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed
 
   assert abs(np.mean(trace.speed[matched]) - 10.0) < 0.5
-  assert trace.accel_ceiling_active[matched].all()
-  np.testing.assert_allclose(trace.mpc_upper_min[matched], trace.profile_accel_max[matched], atol=1e-6)
+  np.testing.assert_allclose(trace.effective_accel_max[matched], trace.profile_accel_max[matched], atol=1e-9)
   assert not np.any(trace.state[response] == int(AccelControllerState.stopHold))
   assert not trace.launching[response].any()
   assert not _has_brake_coast_brake(trace.a_target[response])

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

@@ -1,76 +0,0 @@
-import pytest
-
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.toyota.values import CAR as TOYOTA
-from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.car.helpers import convert_to_capnp
-from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
-from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import (
-  KI,
-  KP,
-  PRIUS_FRICTION,
-  PRIUS_KI,
-  PRIUS_KP,
-  PRIUS_LAT_ACCEL_FACTOR,
-  PRIUS_LAT_ACCEL_OFFSET,
-  LatControlTorque,
-  limit_torque_rate,
-)
-
-
-DT = 0.01
-RATE_UP = 1.0
-RATE_DOWN = 5.0 / 3.0
-
-
-def get_controller(car_name):
-  CarInterface = interfaces[car_name]
-  CP = CarInterface.get_non_essential_params(car_name)
-  CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
-  CI = CarInterface(CP, CP_SP)
-  sunnypilot_interfaces.setup_interfaces(CI)
-  return LatControlTorque(CP.as_reader(), convert_to_capnp(CP_SP).as_reader(), CI, DT_CTRL)
-
-
-@pytest.mark.parametrize("target", [-1.0, 1.0])
-def test_torque_rate_limit_windup(target):
-  last = 0.0
-  for _ in range(25):
-    output = limit_torque_rate(target, last, RATE_UP, RATE_DOWN, DT)
-    assert abs(output - last) <= RATE_UP * DT + 1e-9
-    last = output
-
-
-@pytest.mark.parametrize("initial", [-0.5, 0.5])
-def test_torque_rate_limit_unwind(initial):
-  output = limit_torque_rate(0.0, initial, RATE_UP, RATE_DOWN, DT)
-  assert abs(output - initial) == pytest.approx(RATE_DOWN * DT)
-  assert abs(output) < abs(initial)
-
-
-def test_torque_rate_limit_reversal():
-  last = 0.2
-  outputs = []
-  for _ in range(20):
-    last = limit_torque_rate(-1.0, last, RATE_UP, RATE_DOWN, DT)
-    outputs.append(last)
-
-  assert all(abs(current - previous) <= RATE_DOWN * DT + 1e-9 for previous, current in zip([0.2] + outputs[:-1], outputs, strict=True))
-  assert outputs[-1] < 0.0
-
-
-def test_prius_tune_is_stable_and_scoped():
-  prius = get_controller(TOYOTA.TOYOTA_PRIUS_TSS2)
-  assert prius.prius_smooth_tune
-  assert prius.pid._k_p[1][-1] == pytest.approx(PRIUS_KP)
-  assert prius.pid._k_i[1][-1] == pytest.approx(PRIUS_KI)
-
-  prius.update_live_torque_params(1.2, -0.5, 0.3)
-  assert prius.torque_params.latAccelFactor == pytest.approx(PRIUS_LAT_ACCEL_FACTOR)
-  assert prius.torque_params.latAccelOffset == pytest.approx(PRIUS_LAT_ACCEL_OFFSET)
-  assert prius.torque_params.friction == pytest.approx(PRIUS_FRICTION)
-
-  rav4 = get_controller(TOYOTA.TOYOTA_RAV4_TSS2)
-  assert not rav4.prius_smooth_tune
-  assert rav4.pid._k_p[1][-1] == pytest.approx(KP)
-  assert rav4.pid._k_i[1][-1] == pytest.approx(KI)

sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,402 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-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
-import cereal.messaging as messaging
-from openpilot.common.realtime import DT_MDL, Ratekeeper
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
-from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
-
-
-LeadObservation = dict[str, Any]
-LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
-ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
-EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
-
-
-@dataclass(frozen=True)
-class ActuatorModel:
-  planner_delay: float
-  transport_delay: float
-  actuator_lag: float
-  command_rate_limit: float
-  stopping_acceleration: float
-  standstill_breakaway_acceleration: float
-  standstill_breakaway_time: float
-
-  def __post_init__(self):
-    nonnegative_fields = {
-      "planner_delay": self.planner_delay,
-      "transport_delay": self.transport_delay,
-      "actuator_lag": self.actuator_lag,
-      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
-      "standstill_breakaway_time": self.standstill_breakaway_time,
-    }
-    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
-      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
-    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
-      raise ValueError("command_rate_limit must be finite and positive")
-    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
-      raise ValueError("stopping_acceleration must be finite and non-positive")
-
-
-# Conservative Prius TSS2 actuator model.
-PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
-  planner_delay=0.05,
-  transport_delay=0.0,
-  actuator_lag=0.20,
-  command_rate_limit=4.0,
-  stopping_acceleration=-2.0,
-  standstill_breakaway_acceleration=1.0,
-  standstill_breakaway_time=0.05,
-)
-
-
-class PlantSP(Plant):
-  """Closed-loop plant with configurable observations and actuator response."""
-
-  def __init__(
-    self,
-    lead_relevancy=False,
-    speed=0.0,
-    distance_lead=2.0,
-    enabled=True,
-    only_lead2=False,
-    only_radar=False,
-    e2e=False,
-    personality=0,
-    force_decel=False,
-    lead_observation_fn: LeadObservationFn | None = None,
-    model_action_fn: ModelActionFn | None = None,
-    ego_observation_fn: EgoObservationFn | None = None,
-    actuator_delay: float | None = None,
-    actuator_lag: float = 0.0,
-    actuator_model: ActuatorModel | None = None,
-  ):
-    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
-      raise ValueError("actuator_delay must be finite and non-negative")
-    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
-      raise ValueError("actuator_lag must be finite and non-negative")
-
-    self.rate = 1.0 / DT_MDL
-
-    if not Plant.messaging_initialized:
-      Plant.radar = messaging.pub_sock('radarState')
-      Plant.controls_state = messaging.pub_sock('controlsState')
-      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
-      Plant.car_state = messaging.pub_sock('carState')
-      Plant.plan = messaging.sub_sock('longitudinalPlan')
-      Plant.messaging_initialized = True
-
-    self.v_lead_prev = 0.0
-
-    self.distance = 0.0
-    self.speed = speed
-    self.should_stop = False
-    self.acceleration = 0.0
-    self.a_target = 0.0
-    self.actuator_command = 0.0
-    self.applied_actuator_command = 0.0
-    self.breakaway_confirmed = False
-    self._breakaway_timer = 0.0
-
-    # lead car
-    self.lead_relevancy = lead_relevancy
-    self.distance_lead = distance_lead
-    self.enabled = enabled
-    self.only_lead2 = only_lead2
-    self.only_radar = only_radar
-    self.e2e = e2e
-    self.personality = personality
-    self.force_decel = force_decel
-    self.lead_observation_fn = lead_observation_fn
-    self.model_action_fn = model_action_fn
-    self.ego_observation_fn = ego_observation_fn
-    self.actuator_model = actuator_model
-    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
-    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
-    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
-    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
-                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None))
-
-    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1.0 / self.rate
-    time.sleep(0.1)
-    self.sm = messaging.SubMaster(['longitudinalPlan'])
-
-    from opendbc.car.honda.values import CAR
-    from opendbc.car.honda.interface import CarInterface
-
-    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
-    if self.actuator_delay is not None:
-      CP.longitudinalActuatorDelay = self.actuator_delay
-    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
-    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
-
-    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)
-
-  @staticmethod
-  def _lead_message(observation: LeadObservation):
-    lead = log.RadarState.LeadData.new_message()
-    for field, value in observation.items():
-      setattr(lead, field, value)
-    return lead
-
-  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
-    if self.lead_observation_fn is None:
-      return dict(truth) if present_by_default else None
-
-    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
-    if observed is None:
-      return None
-
-    complete_observation = dict(truth)
-    complete_observation.update(observed)
-    return complete_observation
-
-  def _update_actuator(self, command: float) -> tuple[float, float]:
-    if self._actuator_delay_queue:
-      self._actuator_delay_queue.append(command)
-      delayed_command = self._actuator_delay_queue.popleft()
-    else:
-      delayed_command = command
-
-    if self.actuator_model is not None:
-      max_command_delta = self.actuator_model.command_rate_limit * self.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:
-      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')
-    control = messaging.new_message('controlsState')
-    ss = messaging.new_message('selfdriveState')
-    car_state = messaging.new_message('carState')
-    lp = messaging.new_message('liveParameters')
-    car_control = messaging.new_message('carControl')
-    model = messaging.new_message('modelV2')
-    car_state_sp = messaging.new_message('carStateSP')
-    live_map_data_sp = messaging.new_message('liveMapDataSP')
-    gps_data = messaging.new_message('gpsLocation')
-    a_lead = (v_lead - self.v_lead_prev) / self.ts
-    self.v_lead_prev = v_lead
-
-    if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
-      v_rel = v_lead - self.speed
-      if self.only_radar:
-        status = True
-      elif prob_lead > 0.5:
-        status = True
-      else:
-        status = False
-    else:
-      d_rel = 200.0
-      v_rel = 0.0
-      prob_lead = 0.0
-      status = False
-
-    truth_lead: LeadObservation = {
-      "dRel": float(d_rel),
-      "yRel": 0.0,
-      "vRel": float(v_rel),
-      "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
-    # does not predict slowdown in e2e mode
-    position = log.XYZTData.new_message()
-    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
-    model.modelV2.position = position
-    if self.model_action_fn is None:
-      model_acceleration, model_should_stop = self.acceleration + 0.1, False
-    else:
-      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
-    model.modelV2.action.desiredAcceleration = float(model_acceleration)
-    model.modelV2.action.shouldStop = bool(model_should_stop)
-    velocity = log.XYZTData.new_message()
-    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
-    velocity.x[0] = float(self.speed)  # always start at current speed
-    model.modelV2.velocity = velocity
-    acceleration = log.XYZTData.new_message()
-    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
-    model.modelV2.acceleration = acceleration
-    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
-
-    control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
-    ss.selfdriveState.experimentalMode = self.e2e
-    ss.selfdriveState.personality = self.personality
-    control.controlsState.forceDecel = self.force_decel
-    true_v_ego = self.speed
-    true_a_ego = self.acceleration
-    published_v_ego = true_v_ego
-    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
-    if self.ego_observation_fn is not None:
-      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
-    car_state.carState.vEgo = float(published_v_ego)
-    car_state.carState.aEgo = float(published_a_ego)
-    car_state.carState.standstill = bool(self.speed < 0.01)
-    car_state.carState.vCruise = float(v_cruise * 3.6)
-    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
-
-    # ******** get controlsState messages for plotting ***
-    sm = PlannerSM(self.rk.frame, {
-      'radarState': radar.radarState,
-      'carState': car_state.carState,
-      'carControl': car_control.carControl,
-      'controlsState': control.controlsState,
-      'selfdriveState': ss.selfdriveState,
-      'liveParameters': lp.liveParameters,
-      'modelV2': model.modelV2,
-      'carStateSP': car_state_sp.carStateSP,
-      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-      'gpsLocation': gps_data.gpsLocation,
-    })
-    self.planner.update(sm)
-    self.a_target = self.planner.output_a_target
-    self.actuator_command = self.a_target
-    if self.planner.output_should_stop:
-      stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
-      self.actuator_command = min(stopping_acceleration, self.actuator_command)
-    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)
-    if self.speed <= 0:
-      self.speed = 0
-      self.acceleration = 0
-    self.distance = self.distance + self.speed * self.ts
-
-    # *** radar model ***
-    if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
-      v_rel = v_lead - self.speed
-    else:
-      d_rel = 200.0
-      v_rel = 0.0
-
-    # print at 5hz
-    # if (self.rk.frame % (self.rate // 5)) == 0:
-    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
-    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
-
-    # ******** update prevs ********
-    self.rk.monitor_time()
-
-    accel_controller = self.planner.accel_controller
-    lead_plan = accel_controller._held_lead_plan
-    target_state = accel_controller.target_state
-    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(),
-      "controller_target": accel_controller.output_v_target,
-      "base_target": self.planner.output_v_target,
-      "raw_energy_cap": lead_plan.cap if lead_plan is not None else math.inf,
-      "live_filtered_cap": target_state.filtered_cap,
-      "accel_controller_selected_lead": accel_controller.selected_lead,
-      "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),
-    }

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

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