Avoid profile jerk by shaping feasible MPC plans

.gitmodules

@@ -4,6 +4,7 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
+  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/commaai/msgq.git

cereal/custom.capnp

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

common/params_keys.h

@@ -179,12 +179,19 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // toyota specific params
+    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -228,6 +235,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // Accel Controller relative-pace governor (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

common/tests/test_params.py

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

selfdrive/car/card.py

@@ -10,7 +10,7 @@ from cereal import car, log, custom
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
-
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
@@ -121,7 +121,13 @@ class Car:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
+    # set alternative experiences from parameters
+    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
+    if sp_toyota_auto_brake_hold:
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
+
+
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)

selfdrive/controls/lib/desire_helper.py

@@ -56,7 +56,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob):
+  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -88,8 +88,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -217,6 +217,7 @@ class LongitudinalMpc:
   def __init__(self, dt=DT_MDL):
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.last_solution_status = 0
     self.reset()
     self.source = LongitudinalPlanSource.cruise
 
@@ -313,7 +314,8 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard,
+             accel_max: float | tuple[float, ...] | np.ndarray | None = None, shape_accel_max_in_cruise: bool = False):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -327,11 +329,25 @@ class LongitudinalMpc:
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
+    custom_accel_max = False
+    accel_max_traj = ACCEL_MAX * np.ones(N + 1)
+    if accel_max is not None:
+      accel_max_input = np.asarray(accel_max, dtype=float)
+      if accel_max_input.ndim == 0:
+        accel_max_input = np.full(N + 1, float(accel_max_input))
+      custom_accel_max = accel_max_input.shape == (N + 1,) and np.all(np.isfinite(accel_max_input))
+      if custom_accel_max:
+        accel_max_traj = np.clip(accel_max_input, 0.0, ACCEL_MAX)
+
     # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
     # when the leads are no factor.
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
+    if custom_accel_max and shape_accel_max_in_cruise:
+      cruise_accel_max_traj = np.minimum(accel_max_traj, CRUISE_MAX_ACCEL)
+      v_upper = v_ego + (np.cumsum(T_DIFFS * cruise_accel_max_traj) * 1.05)
+    else:
+      v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 
@@ -344,7 +360,11 @@ class LongitudinalMpc:
     self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
 
     self.params[:,0] = ACCEL_MIN
-    self.params[:,1] = ACCEL_MAX
+    if custom_accel_max:
+      self.params[:,1] = accel_max_traj
+      self.params[0,1] = max(accel_max_traj[0], self.x0[2])
+    else:
+      self.params[:,1] = ACCEL_MAX
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -364,6 +384,7 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
+    self.last_solution_status = self.solution_status
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
     self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
     self.time_linearization = float(self.solver.get_stats('time_lin')[0])

selfdrive/controls/lib/longitudinal_planner.py

@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
-    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -133,12 +133,24 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # 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)
 
+    # DEC is the sole ACC/e2e authority. Cache its decision once for both the governor and output arbitration.
+    is_e2e = self.is_e2e(sm)
+    v_cruise = LongitudinalPlannerSP.update_accel_controller(
+      self, sm, v_cruise, engaged=not reset_state, cruise_initialized=v_cruise_initialized, acc_selected=not is_e2e,
+      planner_speed=self.v_desired_filter.x, previous_mpc_source=self.mpc.source, previous_should_stop=self.output_should_stop,
+      stock_accel_max=accel_clip[1], planner_accel=self.a_desired, controller_fault=self.mpc.last_solution_status != 0,
+    )
+
     if force_slow_decel:
       v_cruise = 0.0
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    self.mpc.update(
+      sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
+      accel_max=self.accel_controller_result.mpc_accel_max,
+      shape_accel_max_in_cruise=self.accel_controller_result.mpc_shape_cruise,
+    )
 
     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)
@@ -160,7 +172,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.is_e2e(sm):
+    if is_e2e:
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:

selfdrive/selfdrived/selfdrived.py

@@ -321,9 +321,16 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
+      mdv2sp = self.sm['modelDataV2SP']
+
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+            (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
+
+      elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
+           (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
+        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
+
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,5 +1,10 @@
 #!/usr/bin/env python3
+from collections import deque
+from collections.abc import Callable
+import math
 import time
+from typing import Any
+
 import numpy as np
 
 from cereal import log
@@ -11,12 +16,52 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+
+
 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,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+  ):
+    """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)``.
+
+    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.
+    """
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
 
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
@@ -28,10 +73,12 @@ 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
 
     # lead car
     self.lead_relevancy = lead_relevancy
@@ -42,9 +89,14 @@ 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.actuator_delay = actuator_delay
+    self.actuator_lag = actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, actuator_delay is not None, actuator_lag > 0.0))
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1. / self.rate
+    self.ts = 1.0 / self.rate
     time.sleep(0.1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
@@ -52,14 +104,54 @@ 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)
 
+    delay_steps = 0 if self.actuator_delay is None else round(self.actuator_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_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
+      self.acceleration += alpha * (delayed_command - self.acceleration)
+    else:
+      self.acceleration = delayed_command
+    return delayed_command, self.acceleration
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
     # ******** publish a fake model going straight and fake calibration ********
     # note that this is worst case for MPC, since model will delay long mpc by one time step
     radar = messaging.new_message('radarState')
@@ -72,39 +164,48 @@ class Plant:
     car_state_sp = messaging.new_message('carStateSP')
     live_map_data_sp = messaging.new_message('liveMapDataSP')
     gps_data = messaging.new_message('gpsLocation')
-    a_lead = (v_lead - self.v_lead_prev)/self.ts
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
     self.v_lead_prev = v_lead
 
     if self.lead_relevancy:
-      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob_lead > .5:
+      elif prob_lead > 0.5:
         status = True
       else:
         status = False
     else:
-      d_rel = 200.
-      v_rel = 0.
+      d_rel = 200.0
+      v_rel = 0.0
       prob_lead = 0.0
       status = False
 
-    lead = log.RadarState.LeadData.new_message()
-    lead.dRel = float(d_rel)
-    lead.yRel = 0.0
-    lead.vRel = float(v_rel)
-    lead.aRel = float(a_lead - self.acceleration)
-    lead.vLead = float(v_lead)
-    lead.vLeadK = float(v_lead)
-    lead.aLeadK = float(a_lead)
-    # TODO use real radard logic for this
-    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
-    lead.status = status
-    lead.modelProb = float(prob_lead)
-    if not self.only_lead2:
-      radar.radarState.leadOne = lead
-    radar.radarState.leadTwo = lead
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "aRel": float(a_lead - self.acceleration),
+      "vLead": float(v_lead),
+      "dPath": 0.0,
+      "vLat": 0.0,
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "fcw": False,
+      "status": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
 
     # Simulate model predicting slightly faster speed
     # this is to ensure lead policy is effective when model
@@ -112,10 +213,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)]
@@ -127,32 +233,38 @@ class Plant:
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
+    published_a_ego = self.acceleration if self.publish_realized_a_ego else 0.0
+    car_state.carState.aEgo = float(published_a_ego)
     car_state.carState.standstill = bool(self.speed < 0.01)
     car_state.carState.vCruise = float(v_cruise * 3.6)
-    car_control.carControl.orientationNED = [0., float(pitch), 0.]
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
 
     # ******** get controlsState messages for plotting ***
-    sm = {'radarState': radar.radarState,
-          'carState': car_state.carState,
-          'carControl': car_control.carControl,
-          'controlsState': control.controlsState,
-          'selfdriveState': ss.selfdriveState,
-          'liveParameters': lp.liveParameters,
-          'modelV2': model.modelV2,
-          'carStateSP': car_state_sp.carStateSP,
-          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation}
+    sm = {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'liveParameters': lp.liveParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    }
     self.planner.update(sm)
-    self.acceleration = self.planner.output_a_target
+    self.a_target = self.planner.output_a_target
+    self.actuator_command = self.a_target
     if self.planner.output_should_stop:
-      self.acceleration = min(-0.5, self.acceleration)
+      self.actuator_command = min(-0.5, self.actuator_command)
+    delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
     self.speed = self.speed + self.acceleration * self.ts
     self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********
-    #print(self.distance, speed)
+    # print(self.distance, speed)
     if self.speed <= 0:
       self.speed = 0
       self.acceleration = 0
@@ -160,30 +272,51 @@ 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,
+      "actuator_command": self.actuator_command,
+      "delayed_actuator_command": delayed_actuator_command,
+      "published_a_ego": published_a_ego,
       "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "pace_cap": getattr(accel_controller_result, "target_speed", None),
+      "base_target": getattr(accel_controller_result, "base_speed", None),
+      "raw_energy_cap": getattr(accel_controller_result, "raw_energy_cap", None),
+      "live_filtered_cap": getattr(accel_controller_result, "live_filtered_cap", None),
+      "shadow_filtered_cap": getattr(accel_controller_result, "shadow_filtered_cap", None),
+      "accel_controller_selected_lead": getattr(accel_controller_result, "selected_lead", None),
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
     }
 
+
 # simple engage in standalone mode
 def plant_thread():
   plant = Plant()

selfdrive/test/longitudinal_maneuvers/test_plant.py

@@ -0,0 +1,80 @@
+import math
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+
+
+def test_full_lead_observation_is_independent_from_truth():
+  callback_inputs = []
+
+  def observe_lead(current_time, lead_name, truth):
+    callback_inputs.append((current_time, lead_name, truth))
+    if lead_name == "leadOne":
+      return {
+        "dRel": 12.5,
+        "vRel": -4.0,
+        "vLead": 6.0,
+        "vLeadK": 5.5,
+        "aLeadK": -1.25,
+        "aLeadTau": 0.7,
+        "status": True,
+        "modelProb": 0.9,
+        "radarTrackId": 42,
+      }
+    return None
+
+  plant = Plant(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
+  result = plant.step(v_lead=8.0)
+
+  assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
+  assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0)
+  assert result["truth_lead"]["dRel"] == pytest.approx(50.0)
+  assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5)
+  assert result["lead_one_observation"]["radarTrackId"] == 42
+  assert result["lead_two_observation"] is None
+  assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL)
+
+
+def test_model_action_realized_acceleration_and_source_logging():
+  def model_action(current_time, v_ego, a_ego):
+    return -1.25, True
+
+  plant = Plant(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
+  first = plant.step()
+  second = plant.step()
+
+  assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
+  assert first["published_a_ego"] == pytest.approx(0.0)
+  assert second["published_a_ego"] == pytest.approx(first["realized_acceleration"])
+  assert first["acceleration"] == first["realized_acceleration"]
+  assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
+  assert first["mpc_source"] is not None
+  assert first["dec_mode"] in ("acc", "blended")
+  assert "pace_cap" in first
+  assert "raw_energy_cap" in first
+  assert "live_filtered_cap" in first
+  assert first["lead_one_observation"] is not None
+  assert first["truth_lead"] == first["lead_one_observation"]
+
+
+def test_configurable_transport_delay_and_first_order_lag():
+  plant = Plant(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
+
+  assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(2 * DT_MDL)
+  delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
+  assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
+
+  expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
+  assert delayed_commands[2][0] == -1.0
+  assert delayed_commands[2][1] == pytest.approx(expected_acceleration)
+
+
+@pytest.mark.parametrize(
+  ("delay", "lag"),
+  [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
+)
+def test_invalid_actuator_dynamics(delay, lag):
+  with pytest.raises(ValueError):
+    Plant(actuator_delay=delay, actuator_lag=lag)

selfdrive/ui/layouts/settings/toggles.py

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

selfdrive/ui/mici/layouts/main.py

@@ -13,6 +13,7 @@ from openpilot.system.ui.lib.application import gui_app
 
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -118,13 +119,15 @@ class MiciMainLayout(Scroller):
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
       self._onroad_time_delay = None
 
     # When car leaves standstill, pop nav stack and scroll to onroad
     CS = ui_state.sm["carState"]
     if not CS.standstill and self._prev_standstill:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
     self._prev_standstill = CS.standstill
 
   def _on_interactive_timeout(self):

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

@@ -14,6 +14,8 @@ class TogglesLayoutMici(NavScroller):
     super().__init__()
 
     self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
+    self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
+    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
     self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode")
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
@@ -24,6 +26,8 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
+      self._accel_personality_enabled,
+      self._accel_personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
       ldw_toggle,
@@ -36,6 +40,7 @@ class TogglesLayoutMici(NavScroller):
     # Toggle lists
     self._refresh_toggles = (
       ("ExperimentalMode", self._experimental_btn),
+      ("AccelPersonalityEnabled", self._accel_personality_enabled),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -45,6 +50,9 @@ class TogglesLayoutMici(NavScroller):
     )
 
     enable_openpilot.set_enabled(lambda: not ui_state.engaged)
+    self._accel_personality_toggle.set_enabled(
+      lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
+    )
     record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
     record_mic.set_enabled(lambda: not ui_state.engaged)
 
@@ -75,13 +83,18 @@ class TogglesLayoutMici(NavScroller):
       if ui_state.has_longitudinal_control:
         self._experimental_btn.set_visible(True)
         self._personality_toggle.set_visible(True)
+        self._accel_personality_enabled.set_visible(True)
+        self._accel_personality_toggle.set_visible(True)
       else:
         # no long for now
         self._experimental_btn.set_visible(False)
         self._experimental_btn.set_checked(False)
         self._personality_toggle.set_visible(False)
+        self._accel_personality_enabled.set_visible(False)
+        self._accel_personality_toggle.set_visible(False)
         ui_state.params.remove("ExperimentalMode")
 
     # Refresh toggles from params to mirror external changes
     for key, item in self._refresh_toggles:
       item.set_checked(ui_state.params.get_bool(key))
+    self._accel_personality_toggle.refresh()

selfdrive/ui/mici/widgets/button.py

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

selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py

@@ -51,11 +51,17 @@ class LaneChangeSettingsLayout(Widget):
       description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
                              "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
     )
+    self._road_edge_block = toggle_item_sp(
+      param="RoadEdgeLaneChangeEnabled",
+      title=lambda: tr("Block Lane Change: Road Edge Detection"),
+      description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
+    )
 
     items = [
       self._lane_change_timer,
       LineSeparatorSP(40),
       self._bsm_delay,
+      self._road_edge_block,
     ]
 
     return items

selfdrive/ui/sunnypilot/mici/layouts/main.py

@@ -0,0 +1,13 @@
+"""
+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 openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
+
+
+class MiciMainLayoutSP(MiciMainLayout):
+  def _should_auto_scroll_to_onroad(self) -> bool:
+    return not self._onroad_layout.is_on_info_panel()

selfdrive/ui/sunnypilot/mici/layouts/onroad.py

@@ -0,0 +1,63 @@
+"""
+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 pyray as rl
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroller_sp import ScrollerSP
+from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad_info_panel import OnroadInfoPanel
+
+CONFIDENCE_BALL_VISIBLE_RATIO = 0.4
+HORIZONTAL_SETTLE_PX = 5
+HORIZONTAL_RESET_RATIO = 0.5
+
+
+class OnroadViewContainerSP(ScrollerSP):
+  def __init__(self, bookmark_callback=None):
+    super().__init__(horizontal=False, snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
+    self.road_view = AugmentedRoadViewSP(bookmark_callback=bookmark_callback)
+    self.onroad_info_panel = OnroadInfoPanel(bookmark_callback=bookmark_callback)
+
+    self._scroller.add_widgets([
+      self.road_view,
+      self.onroad_info_panel,
+    ])
+    self._scroller.set_reset_scroll_at_show(False)
+    self._scroller.set_scrolling_enabled(lambda: abs(self.rect.x) < HORIZONTAL_SETTLE_PX)
+
+    for child in (self.road_view, self.onroad_info_panel):
+      inner_touch_valid = child._touch_valid_callback
+      child.set_touch_valid_callback(
+        lambda inner=inner_touch_valid: self._touch_valid() and (inner() if inner else True)
+      )
+
+  def set_rect(self, rect: rl.Rectangle):
+    super().set_rect(rect)
+    self.road_view.set_rect(rect)
+    self.onroad_info_panel.set_rect(rect)
+    return self
+
+  def is_swiping_left(self) -> bool:
+    return self.road_view.is_swiping_left() or self.onroad_info_panel.is_swiping_left()
+
+  def set_click_callback(self, callback) -> None:
+    self.road_view.set_click_callback(callback)
+    self.onroad_info_panel.set_click_callback(callback)
+
+  def is_on_info_panel(self) -> bool:
+    """True when scrolled past halfway toward onroad_info_panel (used by main layout
+    to skip auto-pop-back-to-camera while user is reading the info panel)."""
+    return abs(self._scroller.scroll_panel.get_offset()) > self._rect.height / 2
+
+  def _render(self, rect: rl.Rectangle):
+    if abs(self.rect.x) > gui_app.width * HORIZONTAL_RESET_RATIO:
+      self._scroller.scroll_panel.set_offset(0)
+
+    vertical_offset = self._scroller.scroll_panel.get_offset()
+    show_ball = abs(vertical_offset) < rect.height * CONFIDENCE_BALL_VISIBLE_RATIO
+    self.road_view.set_show_confidence_ball(show_ball)
+
+    super()._render(rect)

selfdrive/ui/sunnypilot/mici/layouts/onroad_info_panel.py

@@ -0,0 +1,324 @@
+"""
+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 pyray as rl
+from dataclasses import dataclass
+from openpilot.common.constants import CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.lib.application import MousePos
+from openpilot.system.ui.widgets import Widget
+from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
+from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import BookmarkIcon
+
+METER_TO_KM = 0.001
+METER_TO_MILE = 0.000621371
+
+
+@dataclass(frozen=True)
+class OnroadInfoPanelColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = rl.Color(255, 0, 0, 255)
+  green: rl.Color = rl.Color(0, 255, 0, 255)
+  grey: rl.Color = rl.Color(190, 195, 190, 255)
+  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
+  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
+  bg_dark: rl.Color = rl.Color(0, 0, 0, 255)
+  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
+  badge_bg: rl.Color = rl.Color(60, 60, 60, 255)
+
+
+COLORS = OnroadInfoPanelColors()
+
+
+class OnroadInfoPanel(Widget):
+  def __init__(self, bookmark_callback=None):
+    super().__init__()
+    self.speed_limit: float = 0.0
+    self.speed_limit_valid: bool = False
+    self.speed_limit_offset: float = 0.0
+    self.next_speed_limit: float = 0.0
+    self.next_speed_limit_distance: float = 0.0
+    self.road_name: str = ""
+    self.current_speed: float = 0.0
+    self.set_speed: float = 0.0
+    self.cruise_enabled: bool = False
+
+    self._sign_slide: float = 0.0
+
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
+
+    self._marquee_offset: float = 0.0
+    self._marquee_direction: int = 1
+    self._marquee_pause_timer: float = 0.0
+    self._marquee_speed: float = 40.0
+    self._marquee_pause_duration: float = 1.5
+
+    self._alert_renderer = AlertRenderer()
+    self._alert_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
+
+    self._bookmark_icon = BookmarkIcon(bookmark_callback)
+
+  def is_swiping_left(self) -> bool:
+    return self._bookmark_icon.is_swiping_left()
+
+  def _handle_mouse_release(self, mouse_pos: MousePos) -> None:
+    # Mirror stock AugmentedRoadView: suppress click while bookmark gesture active
+    if not self._bookmark_icon.interacting():
+      super()._handle_mouse_release(mouse_pos)
+
+  def _update_state(self) -> None:
+    sm = ui_state.sm
+    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+
+    if sm.valid["longitudinalPlanSP"]:
+      lp_sp = sm["longitudinalPlanSP"]
+      resolver = lp_sp.speedLimit.resolver
+      self.speed_limit = resolver.speedLimit * speed_conv
+      self.speed_limit_valid = resolver.speedLimitValid
+      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
+
+    if sm.valid["liveMapDataSP"]:
+      lmd = sm["liveMapDataSP"]
+      self.next_speed_limit = lmd.speedLimitAhead * speed_conv
+      self.next_speed_limit_distance = lmd.speedLimitAheadDistance
+      self.road_name = lmd.roadName
+
+    if sm.updated["carState"]:
+      self.current_speed = sm["carState"].vEgo * speed_conv
+
+    if sm.valid["carState"] and sm.valid["controlsState"]:
+      self.cruise_enabled = sm["carState"].cruiseState.enabled
+      v_cruise_cluster = sm["carState"].vCruiseCluster
+      set_speed_kph = sm["controlsState"].vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
+      self.set_speed = set_speed_kph * (METER_TO_MILE / METER_TO_KM) if not ui_state.is_metric else set_speed_kph
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    self._update_state()
+
+    rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
+    margin = 20
+    mid_y = rect.y + rect.height / 2
+
+    left_x = rect.x + margin
+
+    if self.cruise_enabled:
+      unit = tr("MAX")
+      display_speed = self.set_speed
+    else:
+      unit = tr("km/h") if ui_state.is_metric else tr("MPH")
+      display_speed = self.current_speed
+
+    speed_val = str(round(display_speed))
+    if self.speed_limit_valid and display_speed > self.speed_limit:
+      speed_color = COLORS.red
+    else:
+      speed_color = COLORS.white
+
+    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, mid_y - 95), 38, 0, COLORS.grey)
+    rl.draw_text_ex(self._font_bold, speed_val, rl.Vector2(left_x, mid_y - 60), 110, 0, speed_color)
+
+    sign_width = 135
+    sign_height = 135 if ui_state.is_metric else 175
+
+    has_next = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
+    target_slide = 1.0 if has_next else 0.0
+    slide_speed = 3.0 * rl.get_frame_time()
+    if self._sign_slide < target_slide:
+      self._sign_slide = min(self._sign_slide + slide_speed, target_slide)
+    elif self._sign_slide > target_slide:
+      self._sign_slide = max(self._sign_slide - slide_speed, target_slide)
+
+    next_w = int(sign_width * 0.7)
+    next_h = int(sign_height * 0.7)
+    next_peek = int(next_w * 0.85) + 5
+    centered_x = rect.x + rect.width - sign_width - margin
+    shifted_x = rect.x + rect.width - sign_width - margin - next_peek
+    sign_x = centered_x + (shifted_x - centered_x) * self._sign_slide
+    sign_y = rect.y + (rect.height - sign_height) / 2
+
+    road_y = mid_y + 55
+    road_width = sign_x - left_x - margin
+    self._draw_road_name(left_x, road_y, road_width)
+
+    if has_next and self._sign_slide > 0.01:
+      next_val = str(round(self.next_speed_limit))
+      dist_str = self._format_distance(self.next_speed_limit_distance)
+      next_x = sign_x + sign_width - int(next_w * 0.15)
+      next_y = sign_y + (sign_height - next_h) / 2
+
+      next_speed_color = COLORS.black
+      if ui_state.is_metric:
+        self._draw_vienna_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
+      else:
+        self._draw_mutcd_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
+
+      dist_size = measure_text_cached(self._font_medium, dist_str, 24)
+      rl.draw_text_ex(self._font_medium, dist_str, rl.Vector2(next_x + next_w / 2 - dist_size.x / 2, next_y + next_h + 4), 24, 0, COLORS.grey)
+
+    self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
+
+    if self.speed_limit_offset != 0 and self.speed_limit_valid:
+      offset_val = str(abs(round(self.speed_limit_offset)))
+      badge_sz = 42
+      badge_x = sign_x + sign_width - badge_sz * 0.85
+      badge_y = sign_y - badge_sz * 0.25
+
+      if ui_state.is_metric:
+        badge_r = badge_sz / 2
+        badge_cx = badge_x + badge_r
+        badge_cy = badge_y + badge_r
+        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r + 2, COLORS.dark_grey)
+        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r, COLORS.badge_bg)
+        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(badge_cx, badge_cy), COLORS.white)
+      else:
+        mutcd_badge_x = sign_x + sign_width - badge_sz * 0.65
+        mutcd_badge_y = sign_y - badge_sz * 0.50
+        badge_rect = rl.Rectangle(mutcd_badge_x, mutcd_badge_y, badge_sz, badge_sz)
+        rl.draw_rectangle_rounded(badge_rect, 0.25, 10, COLORS.badge_bg)
+        rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.25, 10, 2, COLORS.dark_grey)
+        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(mutcd_badge_x + badge_sz / 2, mutcd_badge_y + badge_sz / 2), COLORS.white)
+
+    # SCC
+    speed_size = measure_text_cached(self._font_bold, speed_val, 110)
+    scc_x = left_x + speed_size.x + 30
+    scc_y = mid_y - 50
+    self._draw_scc_icons(scc_x, scc_y)
+
+    self._bookmark_icon.render(rect)
+
+    if ui_state.started:
+      alert_obj, no_alert = self._alert_renderer.will_render()
+      self._alert_alpha_filter.update(0 if no_alert else 1)
+      alpha = self._alert_alpha_filter.x
+      if alpha > 0.01:
+        rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), rl.Color(0, 0, 0, int(150 * alpha)))
+      self._alert_renderer.render(rect)
+
+  def _draw_scc_icons(self, x: float, y: float) -> None:
+    sm = ui_state.sm
+    if not sm.valid["longitudinalPlanSP"]:
+      return
+    scc = sm["longitudinalPlanSP"].smartCruiseControl
+
+    box_w, box_h = 100, 36
+    gap = 6
+    drawn = 0
+
+    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
+      if not active:
+        continue
+      bx = x
+      by = y + drawn * (box_h + gap)
+      rl.draw_rectangle_rounded(rl.Rectangle(bx, by, box_w, box_h), 0.3, 10, COLORS.green)
+      self._draw_text_centered(self._font_bold, label, 20, rl.Vector2(bx + box_w / 2, by + box_h / 2), COLORS.black)
+      drawn += 1
+
+  def _draw_speed_limit_sign(self, x: float, y: float, sign_width: float, sign_height: float) -> None:
+    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid and self.speed_limit > 0 else "--"
+    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
+
+    if ui_state.is_metric:
+      self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
+    else:
+      self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
+
+  def _draw_road_name(self, x: float, y: float, width: float) -> None:
+    road_display = self.road_name if self.road_name else "--"
+    font_size = 30
+    road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
+    text_width = road_size.x
+
+    if text_width <= width:
+      self._marquee_offset = 0.0
+      self._marquee_direction = 1
+      self._marquee_pause_timer = 0.0
+      rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
+    else:
+      overflow = text_width - width
+      dt = rl.get_frame_time()
+
+      if self._marquee_pause_timer > 0:
+        self._marquee_pause_timer -= dt
+      else:
+        self._marquee_offset += self._marquee_direction * self._marquee_speed * dt
+
+        if self._marquee_offset >= overflow:
+          self._marquee_offset = overflow
+          self._marquee_direction = -1
+          self._marquee_pause_timer = self._marquee_pause_duration
+        elif self._marquee_offset <= 0:
+          self._marquee_offset = 0
+          self._marquee_direction = 1
+          self._marquee_pause_timer = self._marquee_pause_duration
+
+      rl.begin_scissor_mode(int(x), int(y), int(width), int(road_size.y + 4))
+      text_pos = rl.Vector2(x - self._marquee_offset, y)
+      rl.draw_text_ex(self._font_semi_bold, road_display, text_pos, font_size, 0, COLORS.white)
+      rl.end_scissor_mode()
+
+  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
+    center = rl.Vector2(x + width / 2, y + height / 2)
+    outer_radius = min(width, height) / 2
+
+    rl.draw_circle_v(center, outer_radius, COLORS.white)
+    ring_width = outer_radius * 0.18
+    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
+
+    font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
+    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
+    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
+    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
+
+  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
+    sign_rect = rl.Rectangle(x, y, width, height)
+    rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white)
+
+    inset = max(4, width * 0.05)
+    inner_rect = rl.Rectangle(x + inset, y + inset, width - inset * 2, height - inset * 2)
+    outer_radius = 0.35 * width / 2.0
+    inner_radius = outer_radius - inset
+    inner_roundness = inner_radius / (inner_rect.width / 2.0)
+    rl.draw_rectangle_rounded_lines_ex(inner_rect, inner_roundness, 10, 3, COLORS.black)
+
+    mid_x = x + width / 2
+    label_size = max(18, int(width * 0.26))
+    if is_upcoming:
+      self._draw_text_centered(self._font_bold, tr("AHEAD"), label_size, rl.Vector2(mid_x, y + height * 0.27), COLORS.black)
+    else:
+      self._draw_text_centered(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black)
+      self._draw_text_centered(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black)
+
+    speed_font_size = int(width * 0.52) if len(speed_str) >= 3 else int(width * 0.62)
+    self._draw_text_centered(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color)
+
+  def _draw_text_centered(self, font, text, size, pos_center, color):
+    sz = measure_text_cached(font, text, size)
+    rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
+
+  def _format_distance(self, distance: float) -> str:
+    if ui_state.is_metric:
+      if distance < 50:
+        return tr("Near")
+      if distance >= 1000:
+        return f"{distance * METER_TO_KM:.1f}" + tr("km")
+      if distance < 200:
+        rounded = max(10, int(distance / 10) * 10)
+      else:
+        rounded = int(distance / 100) * 100
+      return str(rounded) + tr("m")
+    else:
+      distance_mi = distance * METER_TO_MILE
+      if distance_mi < 0.1:
+        return tr("Near")
+      return f"{distance_mi:.1f}" + tr("mi")

selfdrive/ui/sunnypilot/mici/onroad/augmented_road_view.py

@@ -0,0 +1,30 @@
+"""
+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 pyray as rl
+from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
+
+
+class _SuppressedConfidenceBall:
+  def render(self, *_):
+    pass
+
+
+class AugmentedRoadViewSP(AugmentedRoadView):
+  def __init__(self, **kwargs):
+    super().__init__(**kwargs)
+    self._show_confidence_ball: bool = True
+    self._real_confidence_ball = self._confidence_ball
+    self._confidence_ball = _SuppressedConfidenceBall()
+
+  def set_show_confidence_ball(self, show: bool) -> None:
+    self._show_confidence_ball = show
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    super()._render(rect)
+    if self._show_confidence_ball:
+      self._real_confidence_ball.render(self.rect)

selfdrive/ui/sunnypilot/mici/widgets/scroll_panel_sp.py

@@ -0,0 +1,34 @@
+"""
+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 pyray as rl
+from openpilot.system.ui.lib.application import MouseEvent
+from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState
+
+
+class GuiScrollPanel2SP(GuiScrollPanel2):
+  """Reject orthogonal-dominant drags so nested scrollers (outer horizontal +
+  inner vertical) don't both engage on a slightly diagonal swipe.
+
+  Implemented as a post-super state rollback rather than reimplementing the
+  PRESSED state machine — keeps stock behaviour authoritative."""
+
+  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
+                          content_size: float) -> None:
+    pre_state = self._state
+    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
+
+    if self._state == ScrollState.MANUAL_SCROLL and pre_state == ScrollState.PRESSED and \
+       self._initial_click_event is not None:
+      diff_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
+      diff_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
+      along = diff_x if self._horizontal else diff_y
+      anti = diff_y if self._horizontal else diff_x
+      if anti > along:
+        self._state = ScrollState.STEADY
+        self._velocity = 0.0
+        self._velocity_buffer.clear()

selfdrive/ui/sunnypilot/mici/widgets/scroller_sp.py

@@ -0,0 +1,16 @@
+"""
+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 openpilot.system.ui.widgets.scroller import Scroller
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
+
+
+class ScrollerSP(Scroller):
+  def __init__(self, **kwargs):
+    super().__init__(**kwargs)
+    inner = self._scroller
+    inner.scroll_panel = GuiScrollPanel2SP(inner._horizontal, handle_out_of_bounds=not inner._snap_items)

selfdrive/ui/ui.py

@@ -10,6 +10,9 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
 from openpilot.selfdrive.ui.ui_state import ui_state
 
+if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
+
 BIG_UI = gui_app.big_ui()
 
 

sunnypilot/modeld_v2/modeld.py

@@ -40,6 +40,7 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
 
@@ -329,6 +330,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(DH)
   meta_constants = load_meta_constants()
 
   while True:
@@ -433,7 +435,10 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
+      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
+      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

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,
+  AccelProfile,
+)
+
+__all__ = ["AccelController", "AccelControllerResult", "AccelControllerState", "AccelProfile"]

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

@@ -0,0 +1,585 @@
+#!/usr/bin/env python3
+from collections import deque
+from dataclasses import dataclass, field
+from enum import IntEnum
+import math
+
+import numpy as np
+
+from cereal import log
+from opendbc.car.interfaces import ACCEL_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,
+)
+
+
+class AccelProfile(IntEnum):
+  eco = 0
+  normal = 1
+  sport = 2
+
+
+class AccelControllerState(IntEnum):
+  inactive = 0
+  free = 1
+  restrict = 2
+  hold = 3
+  release = 4
+  stopHold = 5
+
+
+@dataclass(frozen=True)
+class ProfileConfig:
+  comfort_decel: float
+  release_rate: float
+  release_confirm: float
+
+
+PROFILE_CONFIGS = {
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=0.65, release_confirm=0.50),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.335, release_rate=0.85, release_confirm=0.35),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_rate=1.10, release_confirm=0.20),
+}
+
+ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0]
+ACCEL_PROFILE_MAX_V = {
+  AccelProfile.eco: [0.95, 0.70, 0.42, 0.28],
+  AccelProfile.normal: [1.30, 1.00, 0.65, 0.45],
+  AccelProfile.sport: [1.55, 1.15, 0.78, 0.58],
+}
+LAUNCH_DELTA_V = 3.0
+
+CAP_FILTER_FRAMES = 5
+RESTRICT_DEADBAND = 0.15
+RELIEF_DEADBAND = 0.35
+STOP_HOLD_EGO_SPEED = 0.30
+STOP_HOLD_CAP = 0.50
+STOPPED_LEAD_SPEED = 0.30
+STOP_HOLD_EXIT_CAP = 0.80
+STOP_HOLD_EXIT_FRAMES = 4
+CLEAR_ROAD_PROFILE_SPEED = 0.20
+ACCEL_LIMIT_JERK = 1.0
+LAUNCH_ACCEL_JERK = 3.0
+LAUNCH_PACE_RATE = 5.0
+MPC_LAUNCH_BOUND_NODES = 2
+MPC_STOP_WARM_BLEND = 0.0
+MPC_CONFIRM_WARM_BLEND = 0.10
+MPC_DEPART_WARM_BLEND = 0.25
+
+
+@dataclass(frozen=True)
+class EnergyEnvelope:
+  cap: float = math.inf
+  selected_lead: int = -1
+  usable_gap: float = math.inf
+  closing_speed: float = 0.0
+  required_decel: float = 0.0
+  has_nearly_stopped_lead: bool = False
+
+
+@dataclass(frozen=True)
+class AccelControllerResult:
+  target_speed: float
+  enabled: bool
+  active: bool
+  shadow_active: bool
+  launching: bool
+  profile: AccelProfile
+  profile_accel_max: float
+  effective_accel_max: float
+  mpc_accel_max: tuple[float, ...] | None
+  mpc_shape_cruise: bool
+  state: AccelControllerState
+  shadow_state: AccelControllerState
+  base_speed: float
+  raw_energy_cap: float
+  live_filtered_cap: float
+  shadow_filtered_cap: float
+  live_pace: float
+  shadow_pace: float
+  selected_lead: int
+  usable_gap: float
+  closing_speed: float
+  required_decel: float
+
+
+@dataclass
+class _PacePath:
+  cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
+  pace: float | None = None
+  state: AccelControllerState = AccelControllerState.inactive
+  relief_time: float = 0.0
+  departure_frames: int = 0
+  departing_from_stop: bool = False
+  stopped_lead_hold: bool = False
+  accel_limit: float | None = None
+
+  def reset(self) -> None:
+    self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
+    self.pace = None
+    self.state = AccelControllerState.inactive
+    self.relief_time = 0.0
+    self.departure_frames = 0
+    self.departing_from_stop = False
+    self.stopped_lead_hold = False
+    self.accel_limit = None
+
+  def update_filter(self, cap: float) -> float:
+    self.cap_samples.append(cap)
+    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def filtered_cap(self) -> float:
+    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+
+
+class AccelController:
+  """A relative-pace governor with a positive-acceleration comfort ceiling."""
+
+  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.live = _PacePath()
+    self.shadow = _PacePath()
+
+  @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:
+    """Return the profile's positive-acceleration ceiling at the current speed."""
+    if not math.isfinite(v_ego):
+      return math.nan
+
+    profile = cls._profile(profile)
+    return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile]))
+
+  def _delay(self) -> float:
+    try:
+      return float(self.CP.longitudinalActuatorDelay) + DT_MDL
+    except (AttributeError, 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:
+        return -v_ego * v_ego / (2.0 * a_ego) if v_ego > 0.0 else 0.0, 0.0
+
+    return max(v_ego * delay + 0.5 * a_ego * delay * delay, 0.0), max(v_ego + a_ego * delay, 0.0)
+
+  @staticmethod
+  def _valid_lead(lead) -> bool:
+    return bool(lead.status) and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau))
+
+  def calculate_energy_envelope(
+    self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality=log.LongitudinalPersonality.standard
+  ) -> EnergyEnvelope:
+    """Calculate the unfiltered relative-energy speed cap without mutating radar state."""
+    profile = self._profile(profile)
+    config = PROFILE_CONFIGS[profile]
+    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:
+      t_follow = get_T_FOLLOW(follow_personality)
+    except (NotImplementedError, TypeError, ValueError):
+      t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard)
+
+    x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
+    candidates: list[EnergyEnvelope] = []
+    nearly_stopped = False
+
+    for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
+      if not self._valid_lead(lead):
+        continue
+
+      x_lead = float(lead.dRel)
+      v_lead = float(lead.vLeadK)
+      a_lead = np.clip(float(lead.aLeadK), -10.0, 5.0)
+      a_lead_tau = float(lead.aLeadTau)
+      lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+      x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
+      v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
+      nearly_stopped = nearly_stopped or v_lead_delay < STOPPED_LEAD_SPEED
+
+      match_gap = STOP_DISTANCE + t_follow * v_lead_delay
+      usable_gap = max(x_lead_delay - x_ego - match_gap, 0.0)
+      closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
+      if closing_speed == 0.0:
+        required_decel = 0.0
+      elif usable_gap == 0.0:
+        required_decel = math.inf
+      else:
+        required_decel = closing_speed * closing_speed / (2.0 * usable_gap)
+
+      # Relative kinetic energy: the lead keeps moving while ego sheds closing speed.
+      cap = v_lead_delay + math.sqrt(2.0 * config.comfort_decel * usable_gap)
+      candidates.append(EnergyEnvelope(cap, lead_index, usable_gap, closing_speed, required_decel))
+
+    if not candidates:
+      return EnergyEnvelope(has_nearly_stopped_lead=nearly_stopped)
+
+    selected = min(candidates, key=lambda candidate: candidate.cap)
+    return EnergyEnvelope(selected.cap, selected.selected_lead, selected.usable_gap, selected.closing_speed, selected.required_decel, nearly_stopped)
+
+  def reset(self) -> None:
+    self.live.reset()
+    self.shadow.reset()
+
+  @staticmethod
+  def _lead_source(source) -> bool:
+    return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+  def _update_path(
+    self,
+    path: _PacePath,
+    raw_cap: float,
+    base_speed: float,
+    v_ego: float,
+    config: ProfileConfig,
+    previous_mpc_source,
+    planner_speed: float,
+    previous_should_stop: bool,
+    has_nearly_stopped_lead: bool,
+    launch_delta_v: float,
+  ) -> float:
+    filtered_cap = path.update_filter(raw_cap)
+    just_initialized = path.pace is None
+    if just_initialized:
+      path.pace = min(base_speed, v_ego)
+      path.state = AccelControllerState.free
+
+    # A clear-road standstill engagement should request motion immediately. A
+    # stopped/previously-stopping lead still goes through stop-hold confirmation.
+    if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop:
+      path.pace = min(base_speed, v_ego + launch_delta_v)
+      path.state = AccelControllerState.release
+      path.relief_time = config.release_confirm
+      path.departing_from_stop = True
+      return filtered_cap
+
+    # A lower non-controller target is authoritative, and is also the correct seed if it later clears.
+    path.pace = min(path.pace, base_speed)
+    if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace:
+      path.pace = max(planner_speed, 0.0)
+
+    if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead):
+      path.stopped_lead_hold = True
+
+    clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= CLEAR_ROAD_PROFILE_SPEED
+    if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete:
+      path.departing_from_stop = False
+      path.stopped_lead_hold = False
+
+    renewed_stop_evidence = filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead
+    enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or (previous_should_stop and not path.departing_from_stop))
+    if enter_stop_hold and path.state != AccelControllerState.stopHold:
+      path.pace = 0.0
+      path.state = AccelControllerState.stopHold
+      path.relief_time = 0.0
+      path.departure_frames = 0
+      path.departing_from_stop = False
+      return filtered_cap
+
+    if path.state == AccelControllerState.stopHold:
+      # A continuously observed moving lead exits after exactly four raw frames.
+      # Total lead loss still waits for the five-frame median dropout guard first.
+      raw_departure = math.isfinite(raw_cap) and raw_cap > STOP_HOLD_EXIT_CAP and not has_nearly_stopped_lead
+      guarded_lead_loss = not math.isfinite(raw_cap) and filtered_cap > STOP_HOLD_EXIT_CAP
+      if raw_departure or guarded_lead_loss:
+        path.departure_frames += 1
+      else:
+        path.departure_frames = 0
+
+      if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
+        path.pace = 0.0
+        return filtered_cap
+
+      path.state = AccelControllerState.release
+      path.relief_time = config.release_confirm
+      path.departure_frames = 0
+      path.departing_from_stop = True
+      path.pace = min(base_speed, filtered_cap, v_ego + launch_delta_v)
+      return filtered_cap
+
+    ceiling = min(base_speed, filtered_cap)
+    if ceiling <= path.pace - RESTRICT_DEADBAND:
+      path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt)
+      path.state = AccelControllerState.restrict
+      path.relief_time = 0.0
+      path.departing_from_stop = False
+      return filtered_cap
+
+    relief = ceiling - path.pace
+    release_allowed = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND
+    if relief >= RELIEF_DEADBAND and not release_allowed:
+      path.relief_time += self.dt
+      path.state = AccelControllerState.hold
+      release_allowed = path.relief_time >= config.release_confirm
+
+    if release_allowed:
+      pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else config.release_rate
+      path.pace = min(ceiling, path.pace + pace_rate * self.dt)
+      path.state = AccelControllerState.release
+    elif relief <= RELIEF_DEADBAND:
+      path.relief_time = 0.0
+      path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.hold
+
+    return filtered_cap
+
+  def _update_accel_limit(
+    self,
+    path: _PacePath,
+    stock_accel_max: float,
+    planner_accel: float,
+    profile_accel_max: float,
+  ) -> tuple[float, float]:
+    """Return telemetry effective max and the controller's pre-MPC positive bound."""
+    requested_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
+
+    if path.state == AccelControllerState.stopHold:
+      path.accel_limit = 0.0
+      return min(stock_accel_max, 0.0), 0.0
+
+    if path.departing_from_stop:
+      if path.stopped_lead_hold:
+        # A confirmed lead departure opens quickly but continuously from zero.
+        previous_limit = path.accel_limit if path.accel_limit is not None else 0.0
+        path.accel_limit = min(requested_limit, previous_limit + LAUNCH_ACCEL_JERK * self.dt)
+      else:
+        # The MPC stays completely stock for the first few centimeters of a
+        # clear-road launch. Seed the selected table value for a smooth handoff.
+        path.accel_limit = requested_limit
+      return min(stock_accel_max, path.accel_limit), path.accel_limit
+
+    if path.accel_limit is None:
+      # Avoid a discontinuity when enabling around an already-positive command.
+      # The global OP limit bounds this seed; dynamic stock output constraints
+      # still retain their existing output-side enforcement and slew.
+      path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel)))
+    else:
+      max_step = ACCEL_LIMIT_JERK * self.dt
+      path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step))
+
+    effective_limit = min(stock_accel_max, path.accel_limit)
+    return effective_limit, path.accel_limit
+
+  def _build_mpc_accel_max(
+    self,
+    path: _PacePath,
+    envelope: EnergyEnvelope,
+    filtered_cap: float,
+    previous_mpc_source,
+    accel_limit: float,
+  ) -> tuple[float, ...] | None:
+    """Build a short pre-MPC bound while leaving the future horizon stock-warm."""
+    # Stock tip-in removes launch delay and gives every profile the same initial
+    # response. The lookup table becomes active once the car is barely rolling.
+    if path.departing_from_stop and not path.stopped_lead_hold:
+      return None
+
+    # A short total-lead dropout has no obstacle to hold stock MPC at zero.
+    # Bound the whole horizon only while the median guard still says "stopped";
+    # genuine loss transitions to the tapered confirmation path below.
+    if path.state == AccelControllerState.stopHold and envelope.selected_lead < 0 and path.departure_frames == 0:
+      return tuple(0.0 for _ in T_IDXS)
+
+    special_launch_state = path.state == AccelControllerState.stopHold or path.departing_from_stop
+
+    # Ordinary lead following must retain stock MPC constraints and obstacle
+    # behavior. Include filtered and previous-source state so a radar dropout
+    # cannot switch the profile bound on for only one or two frames.
+    lead_guarded = envelope.selected_lead >= 0 or math.isfinite(filtered_cap) or self._lead_source(previous_mpc_source)
+    if not special_launch_state and lead_guarded:
+      return None
+
+    if not math.isfinite(accel_limit):
+      return None
+
+    bounded_limit = float(np.clip(accel_limit, 0.0, ACCEL_MAX))
+    accel_max = np.full(len(T_IDXS), bounded_limit, dtype=float)
+    if special_launch_state:
+      # A hard low bound across the full action-delay horizon cold-soaks the
+      # stop solver. Two bounded nodes plus one tapered warm-up node holds the
+      # vehicle through confirmation while preserving a ready future solution.
+      accel_max[MPC_LAUNCH_BOUND_NODES:] = ACCEL_MAX
+      if len(accel_max) > MPC_LAUNCH_BOUND_NODES:
+        if path.state == AccelControllerState.stopHold:
+          warm_blend = MPC_CONFIRM_WARM_BLEND if path.departure_frames > 0 else MPC_STOP_WARM_BLEND
+        else:
+          warm_blend = MPC_DEPART_WARM_BLEND
+        accel_max[MPC_LAUNCH_BOUND_NODES] = bounded_limit + warm_blend * (ACCEL_MAX - bounded_limit)
+    return tuple(float(value) for value in accel_max)
+
+  @staticmethod
+  def _valid_context(
+    base_speed: float,
+    v_ego: float,
+    a_ego: float,
+    planner_speed: float,
+    stock_accel_max: float,
+    planner_accel: float,
+    delay: float,
+    engaged: bool,
+    cruise_initialized: bool,
+    controller_fault: bool,
+  ) -> bool:
+    return (
+      engaged
+      and cruise_initialized
+      and not controller_fault
+      and base_speed >= 0.0
+      and v_ego >= 0.0
+      and planner_speed >= 0.0
+      and delay >= 0.0
+      and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay))
+    )
+
+  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,
+    previous_mpc_source,
+    planner_speed: float,
+    stock_accel_max: float,
+    planner_accel: float,
+    previous_should_stop: bool,
+    controller_fault: bool = False,
+  ) -> AccelControllerResult:
+    """Update live and shadow acceleration controllers and return the target and additive telemetry."""
+    profile = self._profile(profile)
+    config = PROFILE_CONFIGS[profile]
+    profile_accel_max = self.get_profile_accel_max(profile, v_ego)
+    launch_delta_v = LAUNCH_DELTA_V
+    delay = self._delay()
+    valid_context = self._valid_context(
+      base_speed,
+      v_ego,
+      a_ego,
+      planner_speed,
+      stock_accel_max,
+      planner_accel,
+      delay,
+      engaged,
+      cruise_initialized,
+      controller_fault,
+    )
+
+    envelope = self.calculate_energy_envelope(radar_state, v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
+
+    if valid_context:
+      shadow_filtered_cap = self._update_path(
+        self.shadow,
+        envelope.cap,
+        base_speed,
+        v_ego,
+        config,
+        previous_mpc_source,
+        planner_speed,
+        previous_should_stop,
+        envelope.has_nearly_stopped_lead,
+        launch_delta_v,
+      )
+      self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max)
+      shadow_active = True
+    else:
+      self.shadow.reset()
+      shadow_filtered_cap = math.inf
+      shadow_active = False
+
+    live_active = valid_context and bool(enabled) and bool(acc_selected)
+    if live_active:
+      live_filtered_cap = self._update_path(
+        self.live,
+        envelope.cap,
+        base_speed,
+        v_ego,
+        config,
+        previous_mpc_source,
+        planner_speed,
+        previous_should_stop,
+        envelope.has_nearly_stopped_lead,
+        launch_delta_v,
+      )
+      effective_accel_max, controller_accel_max = self._update_accel_limit(
+        self.live, stock_accel_max, planner_accel, profile_accel_max
+      )
+      # Feed only the controller-owned ceiling into MPC. Stock's speed, turn,
+      # coast, and no-throttle limits remain in their original output clip.
+      mpc_accel_max = self._build_mpc_accel_max(
+        self.live, envelope, live_filtered_cap, previous_mpc_source, controller_accel_max,
+      )
+      mpc_shape_cruise = (
+        mpc_accel_max is not None
+        and self.live.state != AccelControllerState.stopHold
+        and not self.live.departing_from_stop
+      )
+      if mpc_accel_max is None:
+        effective_accel_max = stock_accel_max
+      if self.live.state == AccelControllerState.stopHold:
+        # Bounds provide the dropout/creep guard while the stock cruise target
+        # keeps the solver ready for a confirmed departure.
+        target_speed = base_speed
+      elif self.live.departing_from_stop and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0:
+        # A moving lead keeps stock MPC well-conditioned during a confirmed
+        # departure. Clear-road launches retain the bounded live pace below.
+        target_speed = base_speed
+      else:
+        target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
+    else:
+      self.live.reset()
+      live_filtered_cap = math.inf
+      # Preserve the stock target bit-for-bit on every bypass, including stock's own invalid-value handling.
+      target_speed = base_speed
+      effective_accel_max = math.inf
+      mpc_accel_max = None
+      mpc_shape_cruise = False
+
+    return AccelControllerResult(
+      target_speed=target_speed,
+      enabled=bool(enabled),
+      active=live_active,
+      shadow_active=shadow_active,
+      launching=live_active and self.live.departing_from_stop,
+      profile=profile,
+      profile_accel_max=profile_accel_max if live_active else math.inf,
+      effective_accel_max=effective_accel_max,
+      mpc_accel_max=mpc_accel_max,
+      mpc_shape_cruise=mpc_shape_cruise,
+      state=self.live.state,
+      shadow_state=self.shadow.state,
+      base_speed=base_speed,
+      raw_energy_cap=envelope.cap,
+      live_filtered_cap=live_filtered_cap,
+      shadow_filtered_cap=shadow_filtered_cap,
+      live_pace=self.live.pace if self.live.pace is not None else math.inf,
+      shadow_pace=self.shadow.pace if self.shadow.pace is not None else math.inf,
+      selected_lead=envelope.selected_lead,
+      usable_gap=envelope.usable_gap,
+      closing_speed=envelope.closing_speed,
+      required_decel=envelope.required_decel,
+    )

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

@@ -0,0 +1,734 @@
+#!/usr/bin/env python3
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  ACCEL_LIMIT_JERK,
+  ACCEL_PROFILE_MAX_BP,
+  ACCEL_PROFILE_MAX_V,
+  LAUNCH_ACCEL_JERK,
+  LAUNCH_DELTA_V,
+  MPC_CONFIRM_WARM_BLEND,
+  MPC_DEPART_WARM_BLEND,
+  MPC_LAUNCH_BOUND_NODES,
+  AccelController,
+  AccelControllerState,
+  AccelProfile,
+  PROFILE_CONFIGS,
+)
+
+
+def make_lead(*, status: bool = False, d_rel: float = 0.0, v_lead_k: float = 0.0, a_lead_k: float = 0.0, a_lead_tau: float = 1.5):
+  return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau)
+
+
+def make_radar(lead_one=None, lead_two=None):
+  return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead())
+
+
+def make_governor(delay: float = 0.10):
+  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay))
+
+
+def update(governor, radar_state=None, **overrides):
+  args = {
+    "base_speed": 20.0,
+    "v_ego": 20.0,
+    "a_ego": 0.0,
+    "profile": AccelProfile.normal,
+    "follow_personality": log.LongitudinalPersonality.standard,
+    "enabled": True,
+    "acc_selected": True,
+    "engaged": True,
+    "cruise_initialized": True,
+    "previous_mpc_source": LongitudinalPlanSource.cruise,
+    "planner_speed": 20.0,
+    "stock_accel_max": 2.0,
+    "planner_accel": 0.0,
+    "previous_should_stop": False,
+  }
+  args.update(overrides)
+  return governor.update(radar_state or make_radar(), **args)
+
+
+def assert_profile_trajectory(result, expected: float) -> None:
+  assert result.mpc_accel_max is not None
+  np.testing.assert_array_equal(result.mpc_accel_max, expected)
+
+
+class TestAccelProfileLimits:
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_accel_max_matches_lookup_table(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
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_accel_max_interpolates_and_clamps(self, profile):
+    expected_midpoint = (ACCEL_PROFILE_MAX_V[profile][1] + ACCEL_PROFILE_MAX_V[profile][2]) / 2.0
+
+    assert AccelController.get_profile_accel_max(profile, 17.5) == pytest.approx(expected_midpoint)
+    assert AccelController.get_profile_accel_max(profile, -1.0) == ACCEL_PROFILE_MAX_V[profile][0]
+    assert AccelController.get_profile_accel_max(profile, 50.0) == ACCEL_PROFILE_MAX_V[profile][-1]
+
+  @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
+  def test_profile_accel_max_order_is_distinct(self, speed):
+    limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
+
+    assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport]
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_table_never_exceeds_stock_speed_limit(self, profile):
+    for step in range(161):
+      speed = step * 0.25
+      assert AccelController.get_profile_accel_max(profile, speed) <= get_max_accel(speed)
+
+  def test_active_result_exposes_profile_accel_max(self):
+    governor = make_governor()
+
+    result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5)
+
+    assert result.profile_accel_max == pytest.approx(0.56)
+
+  def test_clear_road_profile_is_a_separate_pre_mpc_trajectory(self):
+    governor = make_governor()
+
+    result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.4)
+
+    assert result.mpc_accel_max is not None
+    assert result.mpc_shape_cruise
+    assert len(result.mpc_accel_max) == N + 1
+    assert_profile_trajectory(result, 1.0)
+
+  def test_ordinary_lead_keeps_stock_mpc_accel_bounds(self):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0))
+
+    result = update(governor, radar_state)
+
+    assert result.active
+    assert result.selected_lead == 0
+    assert result.mpc_accel_max is None
+    assert not result.mpc_shape_cruise
+
+  def test_filtered_lead_history_keeps_stock_mpc_bounds_through_two_dropouts(self):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0))
+    for _ in range(3):
+      update(governor, radar_state)
+
+    dropouts = [update(governor), update(governor)]
+
+    assert all(math.isfinite(result.live_filtered_cap) for result in dropouts)
+    assert all(result.mpc_accel_max is None for result in dropouts)
+
+  def test_stop_hold_warms_only_after_departure_evidence(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+
+    held = update(governor, stopped, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
+    confirming = update(governor, moving, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
+
+    assert held.state == AccelControllerState.stopHold
+    assert held.mpc_accel_max is not None
+    assert not held.mpc_shape_cruise
+    np.testing.assert_array_equal(held.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES + 1], 0.0)
+    assert confirming.state == AccelControllerState.stopHold
+    assert confirming.mpc_accel_max is not None
+    assert not confirming.mpc_shape_cruise
+    np.testing.assert_array_equal(confirming.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES], 0.0)
+    assert confirming.mpc_accel_max[MPC_LAUNCH_BOUND_NODES] == pytest.approx(MPC_CONFIRM_WARM_BLEND * 2.0)
+    np.testing.assert_array_equal(confirming.mpc_accel_max[MPC_LAUNCH_BOUND_NODES + 1:], 2.0)
+
+  def test_normal_active_limits_are_bounded_by_stock_and_profile(self):
+    governor = make_governor()
+
+    result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    assert result.profile_accel_max == 1.0
+    assert result.effective_accel_max == 1.0
+    assert_profile_trajectory(result, 1.0)
+
+  def test_first_enable_seeds_from_positive_planner_accel_within_stock(self):
+    governor = make_governor()
+
+    first = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20
+    )
+    second = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20
+    )
+
+    assert first.effective_accel_max == 1.20
+    assert second.effective_accel_max == pytest.approx(first.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL)
+
+  def test_first_enable_seed_preserves_current_plan_but_effective_limit_stays_stock_bounded(self):
+    governor = make_governor()
+
+    result = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.10, planner_accel=1.80
+    )
+
+    assert result.effective_accel_max == 1.10
+    assert_profile_trajectory(result, 1.80)
+
+  def test_profile_switch_slews_at_one_meter_per_second_cubed(self):
+    governor = make_governor()
+    sport = update(governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0)
+
+    eco = update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0)
+
+    assert sport.effective_accel_max == 1.15
+    assert eco.effective_accel_max == pytest.approx(sport.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL)
+    assert eco.effective_accel_max > eco.profile_accel_max
+
+  def test_dynamic_stock_tightening_does_not_enter_controller_comfort_state(self):
+    governor = make_governor()
+    update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    tightened = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=0.40)
+    released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    assert tightened.effective_accel_max == 0.40
+    assert_profile_trajectory(tightened, 1.0)
+    assert released.effective_accel_max == 1.0
+    assert_profile_trajectory(released, 1.0)
+
+  def test_negative_stock_max_remains_authoritative_outside_the_mpc_profile_bound(self):
+    governor = make_governor()
+
+    result = update(governor, v_ego=10.0, planner_speed=10.0, stock_accel_max=-0.20, planner_accel=1.0)
+
+    assert result.effective_accel_max == -0.20
+    assert_profile_trajectory(result, 1.0)
+
+  def test_profile_tightening_can_converge_below_positive_planner_accel(self):
+    governor = make_governor()
+    update(
+      governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15
+    )
+
+    results = [
+      update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15)
+      for _ in range(10)
+    ]
+
+    assert results[-1].effective_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][1])
+    assert results[-1].effective_accel_max < 1.15
+    assert all(result.mpc_accel_max is not None for result in results)
+
+  @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}])
+  def test_bypass_does_not_expose_an_active_accel_limit(self, bypass):
+    governor = make_governor()
+
+    result = update(governor, **bypass)
+
+    assert math.isinf(result.profile_accel_max)
+    assert math.isinf(result.effective_accel_max)
+    assert result.mpc_accel_max is None
+    assert not result.mpc_shape_cruise
+
+  @pytest.mark.parametrize("invalid", [{"stock_accel_max": math.nan}, {"planner_accel": math.nan}])
+  def test_invalid_accel_input_bypasses_and_resets_limits(self, invalid):
+    governor = make_governor()
+    update(governor)
+
+    result = update(governor, **invalid)
+
+    assert not result.active
+    assert governor.live.accel_limit is None
+    assert math.isinf(result.effective_accel_max)
+    assert not result.mpc_shape_cruise
+
+
+class TestEnergyEnvelope:
+  def test_correct_relative_energy_formula_and_lead_selection(self):
+    governor = make_governor()
+    lead_one = make_lead(status=True, d_rel=60.0, v_lead_k=10.0)
+    lead_two = make_lead(status=True, d_rel=100.0, v_lead_k=15.0)
+    radar_state = make_radar(lead_one, lead_two)
+
+    envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal)
+
+    delay = governor.CP.longitudinalActuatorDelay + DT_MDL
+    x_ego = 20.0 * delay
+    x_lead = lead_one.dRel + lead_one.vLeadK * delay
+    usable_gap = x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW() * lead_one.vLeadK
+    expected = lead_one.vLeadK + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap)
+    incorrect_fixed_target_formula = math.sqrt(lead_one.vLeadK**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap)
+
+    assert envelope.selected_lead == 0
+    assert envelope.usable_gap == pytest.approx(usable_gap)
+    assert envelope.cap == pytest.approx(expected)
+    assert envelope.cap != pytest.approx(incorrect_fixed_target_formula)
+
+  def test_lead_two_can_be_more_restrictive(self):
+    governor = make_governor()
+    lead_one = make_lead(status=True, d_rel=100.0, v_lead_k=18.0)
+    lead_two = make_lead(status=True, d_rel=35.0, v_lead_k=5.0)
+
+    envelope = governor.calculate_energy_envelope(make_radar(lead_one, lead_two), 20.0, 0.0, AccelProfile.normal)
+
+    assert envelope.selected_lead == 1
+    assert envelope.cap < 10.0
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_uses_its_comfort_deceleration(self, profile):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0))
+
+    envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, profile)
+    expected = 10.0 + math.sqrt(2.0 * PROFILE_CONFIGS[profile].comfort_decel * envelope.usable_gap)
+
+    assert envelope.cap == pytest.approx(expected)
+
+  def test_profile_caps_order_eco_normal_sport(self):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0))
+
+    caps = [governor.calculate_energy_envelope(radar_state, 20.0, 0.0, profile).cap for profile in AccelProfile]
+
+    assert caps[AccelProfile.eco] < caps[AccelProfile.normal] < caps[AccelProfile.sport]
+
+  def test_stock_follow_personality_is_independent(self):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0))
+
+    aggressive = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.aggressive)
+    relaxed = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal, log.LongitudinalPersonality.relaxed)
+
+    assert relaxed.usable_gap < aggressive.usable_gap
+    assert relaxed.cap < aggressive.cap
+
+  def test_lead_acceleration_is_clipped_before_extrapolation(self):
+    governor = make_governor(delay=0.30)
+    extreme = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0, a_lead_k=-100.0))
+    clipped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0, a_lead_k=-10.0))
+
+    extreme_envelope = governor.calculate_energy_envelope(extreme, 20.0, 0.0, AccelProfile.normal)
+    clipped_envelope = governor.calculate_energy_envelope(clipped, 20.0, 0.0, AccelProfile.normal)
+
+    assert extreme_envelope == clipped_envelope
+
+  def test_ego_projection_stops_at_zero_velocity(self):
+    x_ego, v_ego = AccelController._project_ego(0.2, -4.0, 0.15)
+
+    assert x_ego == pytest.approx(0.005)
+    assert v_ego == 0.0
+
+  def test_invalid_lead_is_ignored(self):
+    governor = make_governor()
+    radar_state = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=10.0))
+
+    envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, AccelProfile.normal)
+
+    assert math.isinf(envelope.cap)
+    assert envelope.selected_lead == -1
+
+
+class TestAccelControllerState:
+  restrictive_lead = make_lead(status=True, d_rel=40.0, v_lead_k=5.0)
+
+  def test_five_frame_median_requires_three_observations_and_holds_two_dropouts(self):
+    governor = make_governor()
+    restrictive_radar = make_radar(self.restrictive_lead)
+
+    first = update(governor, restrictive_radar)
+    second = update(governor, restrictive_radar)
+    third = update(governor, restrictive_radar)
+
+    assert math.isinf(first.live_filtered_cap)
+    assert math.isinf(second.live_filtered_cap)
+    assert math.isfinite(third.live_filtered_cap)
+
+    dropout_one = update(governor)
+    dropout_two = update(governor)
+    dropout_three = update(governor)
+
+    assert math.isfinite(dropout_one.live_filtered_cap)
+    assert math.isfinite(dropout_two.live_filtered_cap)
+    assert math.isinf(dropout_three.live_filtered_cap)
+
+  def test_restriction_is_limited_by_profile_deceleration(self):
+    governor = make_governor()
+    radar_state = make_radar(self.restrictive_lead)
+
+    update(governor, radar_state)
+    update(governor, radar_state)
+    first_restriction = update(governor, radar_state)
+    next_restriction = update(governor, radar_state)
+
+    expected_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
+    assert first_restriction.live_pace == pytest.approx(20.0 - expected_step)
+    assert next_restriction.live_pace == pytest.approx(first_restriction.live_pace - expected_step)
+    assert next_restriction.state == AccelControllerState.restrict
+
+  def test_release_waits_for_confirmation_then_uses_profile_rate(self):
+    governor = make_governor()
+    radar_state = make_radar(self.restrictive_lead)
+    for _ in range(30):
+      update(governor, radar_state)
+
+    result = update(governor)
+    while math.isfinite(result.live_filtered_cap):
+      result = update(governor)
+
+    held_pace = result.live_pace
+    assert result.state == AccelControllerState.hold
+
+    confirmation_updates = 0
+    while result.state != AccelControllerState.release:
+      assert result.live_pace == held_pace
+      result = update(governor)
+      confirmation_updates += 1
+      assert confirmation_updates < 20
+
+    assert confirmation_updates >= 6
+    expected_rate = PROFILE_CONFIGS[AccelProfile.normal].release_rate
+    assert result.live_pace == pytest.approx(held_pace + expected_rate * DT_MDL)
+
+  def test_live_state_never_adopts_shadow_history(self):
+    governor = make_governor()
+    radar_state = make_radar(self.restrictive_lead)
+    for _ in range(20):
+      active = update(governor, radar_state)
+    assert active.live_pace < 20.0
+
+    shadow_only = update(governor, radar_state, acc_selected=False)
+    assert shadow_only.target_speed == 20.0
+    assert shadow_only.state == AccelControllerState.inactive
+    assert math.isinf(shadow_only.live_pace)
+    assert shadow_only.shadow_pace < active.shadow_pace
+
+    reactivated = update(governor, radar_state)
+    assert reactivated.live_pace == 20.0
+    assert reactivated.target_speed == 20.0
+    assert reactivated.shadow_pace < 20.0
+
+  def test_previous_lead_plan_synchronizes_pace_downward(self):
+    governor = make_governor()
+    update(governor, base_speed=30.0, v_ego=20.0, planner_speed=20.0)
+
+    result = update(governor, base_speed=30.0, v_ego=20.0, planner_speed=15.0, previous_mpc_source=LongitudinalPlanSource.lead0)
+
+    assert result.live_pace == 15.0
+    assert result.target_speed == 15.0
+
+  def test_stop_hold_requires_four_confirmed_departure_frames(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    stop_args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.1}
+
+    for _ in range(3):
+      result = update(governor, stopped, **stop_args)
+      assert result.state == AccelControllerState.stopHold
+      assert result.live_pace == 0.0
+      assert result.target_speed == stop_args["base_speed"]
+      assert result.effective_accel_max == 0.0
+      assert result.mpc_accel_max is not None
+
+    for _ in range(3):
+      result = update(governor, moving, **stop_args)
+      assert result.state == AccelControllerState.stopHold
+      assert not result.launching
+      assert result.live_pace == 0.0
+      assert result.target_speed == stop_args["base_speed"]
+      assert result.effective_accel_max == 0.0
+      assert result.mpc_accel_max is not None
+
+    departed = update(governor, moving, **stop_args)
+    assert departed.state == AccelControllerState.release
+    assert departed.launching
+    assert departed.live_pace == pytest.approx(stop_args["v_ego"] + LAUNCH_DELTA_V)
+    assert departed.target_speed == stop_args["base_speed"]
+    assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
+    np.testing.assert_allclose(departed.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES], departed.effective_accel_max)
+    expected_warm = departed.effective_accel_max + MPC_DEPART_WARM_BLEND * (2.0 - departed.effective_accel_max)
+    assert departed.mpc_accel_max[MPC_LAUNCH_BOUND_NODES] == pytest.approx(expected_warm)
+
+  def test_second_nearly_stopped_lead_blocks_departure_confirmation(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    mixed = make_radar(
+      make_lead(status=True, d_rel=20.0, v_lead_k=5.0),
+      make_lead(status=True, d_rel=200.0, v_lead_k=0.0),
+    )
+    args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0}
+    update(governor, stopped, **args)
+
+    results = [update(governor, mixed, **args) for _ in range(5)]
+
+    assert all(result.raw_energy_cap > 0.8 for result in results)
+    assert all(result.state == AccelControllerState.stopHold for result in results)
+    assert all(not result.launching for result in results)
+    assert governor.live.departure_frames == 0
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_confirmed_departure_launch_is_immediate_bounded_and_profiled(self, profile):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "stock_accel_max": 2.0, "profile": profile}
+
+    for _ in range(3):
+      update(governor, stopped, **args)
+    departure = [update(governor, moving, **args) for _ in range(4)]
+
+    assert [result.target_speed for result in departure[:3]] == [args["base_speed"]] * 3
+    expected_launch_pace = min(args["base_speed"], departure[-1].live_filtered_cap, args["v_ego"] + LAUNCH_DELTA_V)
+    assert departure[-1].live_pace == pytest.approx(expected_launch_pace)
+    assert departure[-1].target_speed == args["base_speed"]
+    assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
+    assert departure[-1].mpc_accel_max is not None
+
+  def test_stopped_lead_departure_releases_while_mpc_source_remains_lead(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    lead_args = {
+      "base_speed": 5.0,
+      "v_ego": 0.1,
+      "planner_speed": 0.0,
+      "previous_mpc_source": LongitudinalPlanSource.lead0,
+    }
+
+    for _ in range(3):
+      update(governor, stopped, **lead_args)
+
+    departure = [update(governor, moving, **lead_args) for _ in range(4)]
+    assert [result.live_pace for result in departure[:3]] == [0.0] * 3
+    assert departure[3].live_pace > 0.0
+    assert [result.target_speed for result in departure[:3]] == [lead_args["base_speed"]] * 3
+    assert departure[-1].target_speed == lead_args["base_speed"]
+    assert len(departure) * DT_MDL < 1.0
+
+    continued_release = update(governor, moving, **lead_args)
+    assert continued_release.state == AccelControllerState.release
+    assert continued_release.live_pace > departure[-1].live_pace
+
+  def test_stale_should_stop_does_not_restart_departure_confirmation(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    stale_stop_args = {
+      "base_speed": 5.0,
+      "v_ego": 0.1,
+      "planner_speed": 0.0,
+      "previous_mpc_source": LongitudinalPlanSource.lead0,
+      "previous_should_stop": True,
+    }
+
+    for _ in range(3):
+      update(governor, stopped, **stale_stop_args)
+
+    departure = [update(governor, moving, **stale_stop_args) for _ in range(4)]
+    assert [result.live_pace for result in departure[:3]] == [0.0] * 3
+    assert departure[3].live_pace > 0.0
+    assert len(departure) * DT_MDL < 1.0
+
+    continued_paces = [update(governor, moving, **stale_stop_args).live_pace for _ in range(60)]
+    assert all(current >= previous for previous, current in zip(continued_paces[:-1], continued_paces[1:], strict=True))
+    assert continued_paces[-1] > departure[-1].live_pace
+
+  def test_renewed_stopped_lead_interrupts_confirmed_departure(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    stale_stop_args = {
+      "base_speed": 5.0,
+      "v_ego": 0.1,
+      "planner_speed": 0.0,
+      "previous_mpc_source": LongitudinalPlanSource.lead0,
+      "previous_should_stop": True,
+    }
+
+    for _ in range(3):
+      update(governor, stopped, **stale_stop_args)
+    for _ in range(8):
+      departing = update(governor, moving, **stale_stop_args)
+    assert departing.target_speed > 0.0
+
+    renewed_stop = update(governor, stopped, **stale_stop_args)
+    assert renewed_stop.state == AccelControllerState.stopHold
+    assert renewed_stop.live_pace == 0.0
+    assert renewed_stop.target_speed == stale_stop_args["base_speed"]
+    assert renewed_stop.mpc_accel_max is not None
+    assert not governor.live.departing_from_stop
+
+  def test_low_speed_moving_lead_never_bypasses_bounded_pace(self):
+    governor = make_governor()
+    noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
+
+    first = update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
+    second = update(governor, noisy_moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
+
+    assert first.selected_lead == 0
+    assert first.live_pace == 0.0
+    assert first.target_speed == first.live_pace
+    assert not governor.live.stopped_lead_hold
+    assert second.target_speed == second.live_pace
+    assert second.target_speed < second.base_speed
+
+  def test_real_stopped_evidence_latches_hold_after_noisy_first_frame(self):
+    governor = make_governor()
+    noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    args = {"base_speed": 5.0, "v_ego": 0.0, "planner_speed": 0.0}
+
+    initial_noise = update(governor, noisy_moving_lead, **args)
+    stopped_evidence = update(governor, stopped, **args)
+    repeated_noise = update(governor, noisy_moving_lead, **args)
+
+    assert initial_noise.target_speed == initial_noise.live_pace
+    assert stopped_evidence.state == AccelControllerState.stopHold
+    assert governor.live.stopped_lead_hold
+    assert stopped_evidence.target_speed == stopped_evidence.base_speed
+    assert repeated_noise.target_speed == args["base_speed"]
+
+  def test_later_continuously_moving_lead_does_not_latch_stopped_hold(self):
+    governor = make_governor()
+    moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
+    update(governor, base_speed=5.0, v_ego=1.0, planner_speed=1.0)
+
+    settled = update(governor, moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
+
+    assert settled.selected_lead == 0
+    assert not governor.live.stopped_lead_hold
+    assert settled.target_speed == settled.live_pace
+    assert settled.target_speed < settled.base_speed
+
+  def test_stop_hold_dropout_pins_target_without_losing_hold_state(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    stop_args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.1}
+    for _ in range(3):
+      update(governor, stopped, **stop_args)
+
+    dropout = update(governor, **stop_args)
+
+    assert dropout.selected_lead == -1
+    assert dropout.state == AccelControllerState.stopHold
+    assert dropout.live_pace == 0.0
+    assert dropout.target_speed == dropout.base_speed
+    np.testing.assert_array_equal(dropout.mpc_accel_max, 0.0)
+    assert governor.live.stopped_lead_hold
+
+  def test_no_lead_start_launches_immediately_with_profile_limit(self):
+    governor = make_governor()
+
+    result = update(governor, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
+
+    assert result.selected_lead == -1
+    assert result.launching
+    assert result.live_pace == pytest.approx(0.1 + LAUNCH_DELTA_V)
+    assert result.target_speed == result.live_pace
+    assert result.effective_accel_max == 2.0
+    assert result.mpc_accel_max is None
+
+  def test_confirmed_departure_has_no_later_pace_jump(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    lead_args = {
+      "base_speed": 5.0,
+      "v_ego": 0.1,
+      "planner_speed": 0.0,
+      "previous_mpc_source": LongitudinalPlanSource.lead0,
+      "previous_should_stop": True,
+    }
+    for _ in range(3):
+      update(governor, stopped, **lead_args)
+    for _ in range(8):
+      departing = update(governor, moving, **lead_args)
+    assert governor.live.departing_from_stop
+
+    handed_back = update(governor, moving, **(lead_args | {"v_ego": 0.31, "planner_speed": 0.31}))
+
+    assert not governor.live.departing_from_stop
+    assert not governor.live.stopped_lead_hold
+    expected_step = PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL
+    assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step)
+    assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap)
+    assert handed_back.target_speed == handed_back.live_pace
+    assert handed_back.mpc_accel_max is None
+
+  @pytest.mark.parametrize(
+    "bypass",
+    [
+      {"enabled": False},
+      {"acc_selected": False},
+      {"engaged": False},
+      {"cruise_initialized": False},
+      {"controller_fault": True},
+      {"a_ego": math.nan},
+    ],
+  )
+  def test_bypass_returns_base_and_resets_live(self, bypass):
+    governor = make_governor()
+    radar_state = make_radar(self.restrictive_lead)
+    for _ in range(5):
+      update(governor, radar_state)
+
+    result = update(governor, radar_state, **bypass)
+
+    assert result.target_speed == 20.0
+    assert not result.active
+    assert result.state == AccelControllerState.inactive
+    assert math.isinf(result.live_pace)
+    assert governor.live.accel_limit is None
+    assert math.isinf(result.effective_accel_max)
+    assert result.mpc_accel_max is None
+    assert not result.mpc_shape_cruise
+
+  def test_disabled_acc_mode_keeps_shadow_running(self):
+    governor = make_governor()
+    radar_state = make_radar(self.restrictive_lead)
+
+    results = [update(governor, radar_state, enabled=False) for _ in range(3)]
+
+    assert all(not result.active for result in results)
+    assert all(result.shadow_active for result in results)
+    assert results[-1].shadow_state == AccelControllerState.restrict
+    assert math.isfinite(results[-1].shadow_filtered_cap)
+
+  def test_invalid_profile_defaults_to_normal(self):
+    governor = make_governor()
+
+    result = update(governor, profile=99)
+
+    assert result.profile == AccelProfile.normal
+
+  def test_invalid_delay_resets_and_bypasses(self):
+    governor = AccelController(SimpleNamespace(longitudinalActuatorDelay=None))
+
+    result = update(governor)
+
+    assert result.target_speed == 20.0
+    assert not result.active
+    assert not result.shadow_active
+
+  def test_nonfinite_base_is_preserved_on_bypass(self):
+    governor = make_governor()
+
+    result = update(governor, base_speed=math.nan)
+
+    assert math.isnan(result.target_speed)
+    assert not result.active
+
+  def test_radar_input_is_not_mutated(self):
+    governor = make_governor()
+    lead = make_lead(status=True, d_rel=50.0, v_lead_k=10.0, a_lead_k=-2.0, a_lead_tau=1.2)
+    radar_state = make_radar(lead)
+    before = (lead.status, lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)
+
+    update(governor, radar_state)
+
+    assert (lead.status, lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau) == before

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

@@ -0,0 +1,157 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import custom, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def test_legacy_profile_enum_keeps_toyota_importable():
+  expected = {"eco": 0, "normal": 1, "sport": 2}
+  legacy_profile = custom.LongitudinalPlanSP.AccelerationPersonality
+
+  assert legacy_profile.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
+
+  from opendbc.car.toyota.carstate import AccelPersonality, CarState
+
+  assert AccelPersonality.schema.enumerants == expected
+  assert CarState.__module__ == "opendbc.car.toyota.carstate"
+
+
+def test_mpc_profile_preshapes_accel_bound_and_reachable_cruise_reference():
+  radar_state = messaging.new_message('radarState').radarState
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  accel_max = np.linspace(0.4, 1.0, N + 1)
+
+  mpc.update(radar_state, 30.0, accel_max=accel_max, shape_accel_max_in_cruise=True)
+  shaped_params = mpc.params.copy()
+  mpc.update(radar_state, 30.0)
+  stock_params = mpc.params.copy()
+
+  np.testing.assert_array_equal(shaped_params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(shaped_params[:, 1], accel_max)
+  assert np.any(shaped_params[:, 2] < stock_params[:, 2])
+  np.testing.assert_array_equal(shaped_params[:, 3:], stock_params[:, 3:])
+  np.testing.assert_array_equal(stock_params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(stock_params[:, 1], ACCEL_MAX)
+
+
+def test_mpc_preshape_keeps_current_accel_feasible_only_at_initial_node():
+  radar_state = messaging.new_message('radarState').radarState
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.8)
+  mpc.run = lambda: None
+
+  mpc.update(radar_state, 30.0, accel_max=np.full(N + 1, 0.3))
+  shaped_params = mpc.params.copy()
+  mpc.update(radar_state, 30.0)
+  stock_params = mpc.params.copy()
+
+  assert shaped_params[0, 1] == pytest.approx(0.8)
+  np.testing.assert_array_equal(shaped_params[1:, 1], 0.3)
+  np.testing.assert_array_equal(shaped_params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(shaped_params[:, 2:], stock_params[:, 2:])
+
+
+def test_mpc_last_solve_failure_survives_internal_solver_reset():
+  mpc = LongitudinalMpc()
+  mpc.last_solution_status = 3
+
+  mpc.reset()
+
+  assert mpc.solution_status == 0
+  assert mpc.last_solution_status == 3
+
+
+@pytest.mark.parametrize("accel_max", [None, np.inf, np.nan, np.ones(N), np.r_[np.ones(N), np.nan]])
+def test_mpc_missing_or_invalid_preshape_is_exact_stock(accel_max):
+  radar_state = messaging.new_message('radarState').radarState
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar_state, 30.0)
+  stock_params = mpc.params.copy()
+
+  mpc.update(radar_state, 30.0, accel_max=accel_max)
+
+  np.testing.assert_array_equal(mpc.params, stock_params)
+
+
+def test_shadow_target_telemetry_publishes_filtered_cap():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.source = LongitudinalPlanSource.cruise
+  planner.output_v_target = 20.0
+  planner.output_a_target = 0.0
+  planner.events_sp = SimpleNamespace(to_msg=list)
+  planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
+  planner.accel_controller_result = SimpleNamespace(
+    enabled=True,
+    active=False,
+    shadow_active=True,
+    launching=False,
+    profile=AccelProfile.normal,
+    state=AccelControllerState.inactive,
+    shadow_state=AccelControllerState.restrict,
+    base_speed=20.0,
+    raw_energy_cap=15.0,
+    live_filtered_cap=99.0,
+    shadow_filtered_cap=12.5,
+    shadow_pace=7.25,
+    selected_lead=1,
+    usable_gap=30.0,
+    closing_speed=5.0,
+    required_decel=0.4,
+    profile_accel_max=1.0,
+    effective_accel_max=0.85,
+    mpc_accel_max=tuple(np.full(N + 1, 0.85)),
+    mpc_shape_cruise=True,
+  )
+  planner.scc = SimpleNamespace(
+    vision=SimpleNamespace(
+      state=0,
+      output_v_target=20.0,
+      output_a_target=0.0,
+      current_lat_acc=0.0,
+      max_pred_lat_acc=0.0,
+      is_enabled=False,
+      is_active=False,
+    ),
+    map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
+  )
+  planner.resolver = SimpleNamespace(
+    speed_limit=0.0,
+    speed_limit_last=0.0,
+    speed_limit_final=0.0,
+    speed_limit_final_last=0.0,
+    speed_limit_valid=False,
+    speed_limit_last_valid=False,
+    speed_limit_offset=0.0,
+    distance=0.0,
+    source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
+  )
+  planner.sla = SimpleNamespace(
+    state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled,
+    is_enabled=False,
+    is_active=False,
+    output_v_target=20.0,
+    output_a_target=0.0,
+  )
+  planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
+
+  sent = {}
+  sm = SimpleNamespace(all_checks=lambda service_list: True)
+  pm = SimpleNamespace(send=lambda service, message: sent.update({service: message}))
+  planner.publish_longitudinal_plan_sp(sm, pm)
+
+  telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
+  assert telemetry.vTargetShadow == pytest.approx(planner.accel_controller_result.shadow_filtered_cap)
+  assert telemetry.vTargetShadow != pytest.approx(planner.accel_controller_result.shadow_pace)
+  assert telemetry.aMaxProfile == pytest.approx(planner.accel_controller_result.profile_accel_max)
+  assert telemetry.aMaxEffective == pytest.approx(planner.accel_controller_result.effective_accel_max)

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

@@ -1,17 +1,46 @@
+from openpilot.common.realtime import DT_MDL
+
+
 class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
+  TRAJECTORY_SIZE = 33
+  PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL)))
 
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
+  EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL)))
+  MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))}
+  ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL)))
+  EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL)))
+  STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL)))
 
-  # Optimized slow down distance curve - smooth and progressive
+  LEAD_PROB = 0.45
+  LEAD_EXIT_PROB = 0.25
+  LEAD_RISE_RATE = 1.0
+  LEAD_FALL_RATE = 0.35
+  RADAR_LEAD_ACC_PROB = 0.5
+  RADAR_LEAD_ACC_EXIT_PROB = 0.4
+  RADAR_LEAD_ACC_RISE_RATE = 1.0
+  RADAR_LEAD_ACC_FALL_RATE = 0.25
+  RADAR_LEAD_ACC_MAX_DREL = 80.0
+  RADAR_LEAD_ACC_MAX_TTC = 6.0
+  RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
+
+  SLOW_DOWN_PROB = 0.5
+  SLOW_DOWN_EXIT_PROB = 0.4
+  SLOW_DOWN_RISE_RATE = 0.65
+  SLOW_DOWN_FALL_RATE = 0.15
   SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
   SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
+  URGENT_SLOW_DOWN_PROB = 0.85
 
-  # Slowness detection parameters
-  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
-  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
-  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset
+  MODEL_DECEL_START = -0.5
+  MODEL_DECEL_RANGE = 2.0
+  ENDPOINT_URGENCY_GAIN = 1.3
+  CRITICAL_ENDPOINT_FACTOR = 0.3
+  CRITICAL_URGENCY_GAIN = 1.5
+  SPEED_URGENCY_MIN = 25.0
+  SPEED_URGENCY_RANGE = 80.0
+
+  SLOWNESS_PROB = 0.55
+  SLOWNESS_EXIT_PROB = 0.45
+  SLOWNESS_RISE_RATE = 0.35
+  SLOWNESS_FALL_RATE = 0.5
+  SLOWNESS_CRUISE_OFFSET = 1.025

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -6,129 +6,116 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
-from cereal import messaging
-from opendbc.car import structs
-from numpy import interp
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 from typing import Literal
 
-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
+from cereal import messaging
+from numpy import interp
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 
-# Define the valid mode types
 ModeType = Literal['acc', 'blended']
 
 
-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
+def clip01(value: float) -> float:
+  return max(0.0, min(1.0, float(value)))
 
-  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
-               alpha=1.0, smoothing_factor=0.85):
-    self.x = initial_value
-    self.P = 1.0
-    self.R = measurement_noise
-    self.Q = process_noise
-    self.alpha = alpha
-    self.smoothing_factor = smoothing_factor
-    self.initialized = False
-    self.history = []
-    self.max_history = 10
-    self.confidence = 0.0
 
-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
+class SmoothedSignal:
+  def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0):
+    self.rise_rate = clip01(rise_rate)
+    self.fall_rate = clip01(fall_rate)
+    self.value = clip01(initial_value)
 
-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
+  def update(self, measurement: float) -> float:
+    measurement = clip01(measurement)
+    rate = self.rise_rate if measurement > self.value else self.fall_rate
+    self.value += (measurement - self.value) * rate
+    return self.value
 
-    self.P = self.alpha * self.P + self.Q
+  def reset(self, value: float = 0.0) -> None:
+    self.value = clip01(value)
 
-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
 
-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
+class HysteresisSignal:
+  def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float):
+    self.enter_threshold = clip01(enter_threshold)
+    self.exit_threshold = clip01(exit_threshold)
+    self.filter = SmoothedSignal(rise_rate, fall_rate)
+    self.active = False
 
-    if abs(innovation) < 0.1:
-      self.confidence = min(1.0, self.confidence + 0.05)
-    else:
-      self.confidence = max(0.1, self.confidence - 0.02)
+  def update(self, measurement: float) -> bool:
+    value = self.filter.update(measurement)
+    threshold = self.exit_threshold if self.active else self.enter_threshold
+    self.active = value > threshold
+    return self.active
 
-  def get_value(self):
-    return self.x if self.initialized else None
+  def reset(self) -> None:
+    self.filter.reset()
+    self.active = False
 
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
+  @property
+  def value(self) -> float:
+    return self.filter.value
 
 
 class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
-
   def __init__(self):
     self.current_mode: ModeType = 'acc'
-    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
-    self.transition_timeout = 0
-    self.min_mode_duration = 10
     self.mode_duration = 0
-    self.emergency_override = False
+    self._pending_mode: ModeType = 'acc'
+    self._pending_count = 0
+    self._blended_hold_frames = 0
 
-  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
-    # Emergency override for critical situations (stops, collisions)
-    if emergency:
-      self.emergency_override = True
-      self.current_mode = mode
-      self.transition_timeout = SET_MODE_TIMEOUT
-      self.mode_duration = 0
+  def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
+    if immediate:
+      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
+      self._pending_mode = mode
+      self._pending_count = 0
+      self._switch_mode(mode)
       return
 
-    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
-    for m in self.mode_confidence:
-      if m != mode:
-        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
+    if cancel_hold and mode == 'acc':
+      self._blended_hold_frames = 0
 
-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
+    if self._blended_hold_frames > 0:
+      mode = 'blended'
+
+    if mode == self.current_mode:
+      self._pending_mode = mode
+      self._pending_count = 0
       return
 
-    # Hysteresis: higher threshold for mode changes
-    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
+    if mode != self._pending_mode:
+      self._pending_mode = mode
+      self._pending_count = 1
+    else:
+      self._pending_count += 1
 
-    if self.mode_confidence[mode] > confidence_threshold:
-      if mode != self.current_mode and self.transition_timeout == 0:
-        self.transition_timeout = SET_MODE_TIMEOUT
-        self.current_mode = mode
-        self.mode_duration = 0
+    if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]:
+      return
 
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
+    required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
+    if self._pending_count >= required_count:
+      self._switch_mode(mode)
+
+  def update(self) -> None:
+    if self._blended_hold_frames > 0:
+      self._blended_hold_frames -= 1
     self.mode_duration += 1
 
-    # Reset emergency override after some time
-    if self.emergency_override and self.mode_duration > 20:
-      self.emergency_override = False
-
-    # Gradual confidence decay
-    for mode in self.mode_confidence:
-      self.mode_confidence[mode] *= 0.98
-
   def get_mode(self) -> ModeType:
     return self.current_mode
 
+  def _switch_mode(self, mode: ModeType) -> None:
+    if mode == self.current_mode:
+      return
+
+    self.current_mode = mode
+    self.mode_duration = 0
+    self._pending_mode = mode
+    self._pending_count = 0
+
 
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
@@ -142,35 +129,33 @@ class DynamicExperimentalController:
 
     self._mode_manager = ModeTransitionManager()
 
-    # Smooth filters for stable decision making with faster response for critical scenarios
-    self._lead_filter = SmoothKalmanFilter(
-      measurement_noise=0.15,
-      process_noise=0.05,
-      alpha=1.02,
-      smoothing_factor=0.8
+    self._lead_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.LEAD_PROB,
+      exit_threshold=WMACConstants.LEAD_EXIT_PROB,
+      rise_rate=WMACConstants.LEAD_RISE_RATE,
+      fall_rate=WMACConstants.LEAD_FALL_RATE,
+    )
+    self._radar_acc_lead_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB,
+      exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB,
+      rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE,
+      fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE,
+    )
+    self._slow_down_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.SLOW_DOWN_PROB,
+      exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
+      rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE,
+      fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE,
+    )
+    self._slowness_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.SLOWNESS_PROB,
+      exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB,
+      rise_rate=WMACConstants.SLOWNESS_RISE_RATE,
+      fall_rate=WMACConstants.SLOWNESS_FALL_RATE,
     )
 
-    self._slow_down_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.1,
-      alpha=1.05,
-      smoothing_factor=0.7
-    )
-
-    self._slowness_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.06,
-      alpha=1.015,
-      smoothing_factor=0.92
-    )
-
-    self._mpc_fcw_filter = SmoothKalmanFilter(
-      measurement_noise=0.2,
-      process_noise=0.1,
-      alpha=1.1,
-      smoothing_factor=0.5
-    )
     self._has_lead_filtered = False
+    self._has_radar_acc_lead = False
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -179,13 +164,14 @@ class DynamicExperimentalController:
     self._has_standstill = False
     self._mpc_fcw_crash_cnt = 0
     self._standstill_count = 0
-    # debug
+
     self._endpoint_x = float('inf')
     self._expected_distance = 0.0
     self._trajectory_valid = False
+    self._raw_urgency = 0.0
 
   def _read_params(self) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
+    if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
@@ -198,7 +184,6 @@ class DynamicExperimentalController:
     return self._active
 
   def set_mpc_fcw_crash_cnt(self) -> None:
-    """Set MPC FCW crash count"""
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
@@ -210,179 +195,109 @@ class DynamicExperimentalController:
     self._v_cruise_kph = car_state.vCruise
     self._has_standstill = car_state.standstill
 
-    # standstill detection
     if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
+      self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, self._standstill_count + 1)
     else:
       self._standstill_count = max(0, self._standstill_count - 1)
 
-    # Lead detection
-    self._lead_filter.add_data(float(lead_one.status))
-    lead_value = self._lead_filter.get_value() or 0.0
-    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
-
-    # MPC FCW detection
-    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
-    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
-    self._has_mpc_fcw = fcw_filtered_value > 0.5
-
-    # Slow down detection
+    self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
+    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one))
+    self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
     self._calculate_slow_down(md)
 
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
+    if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down:
+      self._slowness_tracker.reset()
+      self._has_slowness = False
+    else:
       current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._slowness_filter.add_data(current_slowness)
-      slowness_value = self._slowness_filter.get_value() or 0.0
+      self._has_slowness = self._slowness_tracker.update(current_slowness)
 
-      # Hysteresis for slowness
-      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
-      self._has_slowness = slowness_value > threshold
-
-  def _calculate_slow_down(self, md):
-    """Calculate urgency based on trajectory endpoint vs expected distance."""
-
-    # Reset to safe defaults
-    urgency = 0.0
+  def _calculate_slow_down(self, md) -> None:
     self._endpoint_x = float('inf')
+    self._expected_distance = 0.0
     self._trajectory_valid = False
 
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
+    urgency = self._model_action_urgency(md)
+    position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
 
-    if not (position_valid and orientation_valid):
-      # Invalid trajectory - this itself might indicate a stop scenario
-      # Apply moderate urgency for incomplete trajectories at speed
-      if self._v_ego_kph > 20.0:
-        urgency = 0.3
+    if position_valid:
+      self._trajectory_valid = True
+      self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1]
+      self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST)
+      urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance))
 
-      self._slow_down_filter.add_data(urgency)
-      urgency_filtered = self._slow_down_filter.get_value() or 0.0
-      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
-      self._urgency = urgency_filtered
-      return
+    self._raw_urgency = clip01(urgency)
+    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
+    self._urgency = self._slow_down_tracker.value
 
-    # We have a valid full trajectory
-    self._trajectory_valid = True
+  def _radar_acc_lead_score(self, lead_one) -> float:
+    if not lead_one.status:
+      return 0.0
 
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
+    d_rel = float(getattr(lead_one, 'dRel', float('inf')))
+    v_rel = float(getattr(lead_one, 'vRel', 0.0))
+    if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
+      return 1.0
+    if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
+      return 1.0
+    return 0.0
 
-    # Get expected distance based on current speed using tuned constants
-    expected_distance = interp(self._v_ego_kph,
-                               WMACConstants.SLOW_DOWN_BP,
-                               WMACConstants.SLOW_DOWN_DIST)
-    self._expected_distance = expected_distance
+  def _model_action_urgency(self, md) -> float:
+    action = getattr(md, 'action', None)
+    if action is None:
+      return 0.0
 
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
+    urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0
+    desired_accel = getattr(action, 'desiredAcceleration', 0.0)
+    if desired_accel < WMACConstants.MODEL_DECEL_START:
+      urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE))
+    return urgency
 
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
+  def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float:
+    if endpoint_x >= expected_distance:
+      return 0.0
 
-      # Increase urgency for very short trajectories (imminent stops)
-      critical_distance = expected_distance * 0.3
-      if endpoint_x < critical_distance:
-        urgency = min(1.0, urgency * 2.0)
+    shortage_ratio = (expected_distance - endpoint_x) / expected_distance
+    urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN)
 
-      # Speed-based urgency adjustment
-      if self._v_ego_kph > 25.0:
-        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
-        urgency = min(1.0, urgency * speed_factor)
+    if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR:
+      urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN)
 
-    # Apply filtering but with less smoothing for stops
-    self._slow_down_filter.add_data(urgency)
-    urgency_filtered = self._slow_down_filter.get_value() or 0.0
+    if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN:
+      speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE
+      urgency = min(1.0, urgency * speed_factor)
 
-    # Update state with lower threshold for better stop detection
-    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
-    self._urgency = urgency_filtered
+    return urgency
 
-  def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
+  def _desired_mode(self) -> tuple[ModeType, bool]:
+    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
+      return 'acc', False
 
-    # EMERGENCY: MPC FCW - immediate blended mode
     if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
+      return 'blended', True
 
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
+    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
+    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
 
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
+    if self._CP.radarUnavailable:
+      if standstill or self._has_slow_down:
+        return 'blended', urgent_slow_down
+      return 'acc', False
 
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
+    if standstill or self._has_slow_down:
+      return 'blended', urgent_slow_down
 
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
-
-  def _radar_mode(self) -> None:
-    """Radar mode with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # If lead detected and not in standstill: always use ACC
-    if self._has_lead_filtered and not (self._standstill_count > 3):
-      self._mode_manager.request_mode('acc', confidence=1.0)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.3)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
+    return 'acc', False
 
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
-
     self.set_mpc_fcw_crash_cnt()
-
     self._update_calculations(sm)
 
-    if self._CP.radarUnavailable:
-      self._radarless_mode()
-    else:
-      self._radar_mode()
-
+    mode, immediate = self._desired_mode()
+    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
+                                    cancel_hold=self._has_radar_acc_lead)
     self._mode_manager.update()
+
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -1,94 +0,0 @@
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
-class MockLeadOne:
-  def __init__(self, status=0.0):
-    self.status = status
-
-class MockRadarState:
-  def __init__(self, status=0.0):
-    self.leadOne = MockLeadOne(status=status)
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-class MockModelData:
-  def __init__(self, valid=True):
-    size = 33 if valid else 10  # incomplete if invalid
-    self.position = type("Pos", (), {"x": [0.0] * size})()
-    self.orientation = type("Ori", (), {"x": [0.0] * size})()
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
-
-class MockParams:
-  def get_bool(self, name):
-    return True
-
-@pytest.fixture
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
-
-@pytest.fixture
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
-
-@pytest.fixture
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
-
-# Fake Kalman Filter that always returns a given value
-class FakeKalman:
-  def __init__(self, value=1.0):
-    self.value = value
-  def add_data(self, v): pass
-  def get_value(self): return self.value
-  def get_confidence(self): return 1.0
-  def reset_data(self): pass
-
-def test_initial_mode_is_acc(mock_cp, mock_mpc):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  assert controller.mode() == "acc"
-
-def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['carState'].standstill = True
-  for _ in range(10):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  mock_mpc.crash_cnt = 1  # simulate FCW
-  for _ in range(2):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-
-  # Force conditions to simulate slowdown
-  controller._slow_down_filter = FakeKalman(value=1.0)  # Ensure urgency triggers slowdown
-  controller._v_ego_kph = 35.0
-  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
-
-  for _ in range(3):
-    controller.update(default_sm)
-
-  assert controller.mode() == "blended"

sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -0,0 +1,235 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
+
+
+class MockLeadOne:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
+    self.status = status
+    self.dRel = dRel
+    self.vRel = vRel
+
+
+class MockRadarState:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
+
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+
+class MockAction:
+  def __init__(self, desiredAcceleration=0.0, shouldStop=False):
+    self.desiredAcceleration = desiredAcceleration
+    self.shouldStop = shouldStop
+
+
+class MockModelData:
+  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
+    position_size = 33 if valid else 10
+    orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
+    position_x = [0.0] * position_size
+    if position_x:
+      position_x[-1] = endpoint_x
+    self.position = type("Pos", (), {"x": position_x})()
+    self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
+    self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
+    self.action = MockAction(desired_acceleration, should_stop)
+
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+  return MPC()
+
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['carState'].standstill = True
+  for _ in range(20):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  mock_mpc.crash_cnt = 1
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller.mode() == "blended"
+
+
+def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False)
+
+  controller.update(default_sm)
+
+  assert controller._trajectory_valid
+  assert controller.mode() == "blended"
+
+
+def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0)
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert not controller._trajectory_valid
+  assert not controller._has_slow_down
+  assert controller.mode() == "acc"
+
+
+def test_slowdown_hysteresis_prevents_threshold_chatter():
+  signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0)
+
+  assert signal.update(0.55)
+  assert signal.update(0.45)
+  assert not signal.update(0.35)
+
+
+def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+
+  controller.update(default_sm)
+
+  assert not controller._trajectory_valid
+  assert controller.mode() == "blended"
+
+
+def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller._has_slow_down
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0)
+  for _ in range(20):
+    controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for _ in range(20):
+    controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['carState'].standstill = True
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
+  mock_mpc.crash_cnt = 1
+
+  for _ in range(10):
+    controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller._has_mpc_fcw
+  assert controller.mode() == "acc"
+
+
+def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller.mode() == "acc"

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,7 +8,11 @@ See the LICENSE.md file in the root directory for more details.
 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
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -22,7 +26,8 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
+    self.params = Params()
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
@@ -32,10 +37,26 @@ 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._param_read_frames = max(1, int(round(0.25 / dt)))
+    self._param_frame = 0
+    self.accel_personality_enabled = False
+    self.accel_personality = int(AccelProfile.normal)
 
     self.output_v_target = 0.
     self.output_a_target = 0.
 
+  def _read_accel_controller_params(self) -> None:
+    if self._param_frame % self._param_read_frames == 0:
+      self.accel_personality_enabled = self.params.get_bool("AccelPersonalityEnabled")
+      self.accel_personality = get_sanitize_int_param(
+        "AccelPersonality", int(AccelProfile.eco), int(AccelProfile.sport), self.params,
+      )
+
+    self._param_frame += 1
+
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
     if not self.dec.active():
@@ -73,7 +94,31 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
+                              acc_selected: bool, planner_speed: float, previous_mpc_source, previous_should_stop: bool,
+                              stock_accel_max: float, planner_accel: float, controller_fault: bool = False) -> float:
+    self.accel_controller_result = self.accel_controller.update(
+      sm['radarState'],
+      base_speed=base_speed,
+      v_ego=sm['carState'].vEgo,
+      a_ego=sm['carState'].aEgo,
+      profile=self.accel_personality,
+      follow_personality=sm['selfdriveState'].personality,
+      enabled=self.accel_personality_enabled,
+      acc_selected=acc_selected,
+      engaged=engaged,
+      cruise_initialized=cruise_initialized,
+      previous_mpc_source=previous_mpc_source,
+      planner_speed=planner_speed,
+      stock_accel_max=stock_accel_max,
+      planner_accel=planner_accel,
+      previous_should_stop=previous_should_stop,
+      controller_fault=controller_fault,
+    )
+    return self.accel_controller_result.target_speed
+
   def update(self, sm: messaging.SubMaster) -> None:
+    self._read_accel_controller_params()
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -95,6 +140,26 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    # Accel Controller relative-pace governor
+    if self.accel_controller_result is not None:
+      result = self.accel_controller_result
+      accel_controller = longitudinalPlanSP.accelController
+      accel_controller.enabled = result.enabled
+      accel_controller.active = result.active
+      accel_controller.shadowOnly = result.shadow_active and not result.active
+      accel_controller.profile = int(result.profile)
+      accel_controller.state = int(result.state if result.active else result.shadow_state)
+      accel_controller.vTargetBase = float(result.base_speed)
+      accel_controller.vTargetRaw = float(result.raw_energy_cap)
+      accel_controller.vTargetFiltered = float(result.live_filtered_cap)
+      accel_controller.vTargetShadow = float(result.shadow_filtered_cap)
+      accel_controller.leadIndex = result.selected_lead
+      accel_controller.usableGap = float(result.usable_gap)
+      accel_controller.closingSpeed = float(result.closing_speed)
+      accel_controller.requiredDecel = float(result.required_decel)
+      accel_controller.aMaxProfile = float(result.profile_accel_max)
+      accel_controller.aMaxEffective = float(result.effective_accel_max)
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

sunnypilot/selfdrive/controls/lib/relc.py

@@ -0,0 +1,84 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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 openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+NEARSIDE_PROB = 0.2
+EDGE_PROB = 0.35
+EDGE_REACTION_TIME = 1.0
+EDGE_CLEAR_TIME = 0.3
+MIN_SPEED = 20 * CV.MPH_TO_MS
+
+
+class RoadEdgeLaneChangeController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.params = Params()
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+    self.param_read_counter = 0
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def read_params(self) -> None:
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def reset(self) -> None:
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
+    self.update_params()
+
+    if not self.enabled or v_ego < MIN_SPEED:
+      self.reset()
+      return
+
+    left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
+    right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
+    left_lane_prob = lane_line_probs[0]
+    right_lane_prob = lane_line_probs[3]
+
+    left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
+    right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
+
+    if left_cond:
+      self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.left_clear_timer = 0.0
+      if self.left_edge_timer > EDGE_REACTION_TIME:
+        self.left_edge_detected = True
+    else:
+      self.left_clear_timer += DT_MDL
+      if self.left_clear_timer > EDGE_CLEAR_TIME:
+        self.left_edge_timer = 0.0
+        self.left_edge_detected = False
+
+    if right_cond:
+      self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.right_clear_timer = 0.0
+      if self.right_edge_timer > EDGE_REACTION_TIME:
+        self.right_edge_detected = True
+    else:
+      self.right_clear_timer += DT_MDL
+      if self.right_clear_timer > EDGE_CLEAR_TIME:
+        self.right_edge_timer = 0.0
+        self.right_edge_detected = False

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

@@ -0,0 +1,621 @@
+from collections.abc import Callable
+from dataclasses import dataclass
+
+import numpy as np
+import pytest
+
+from opendbc.car.interfaces import ACCEL_MIN
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import LeadObservation, Plant
+
+
+@dataclass
+class ClosedLoopTrace:
+  time: np.ndarray
+  speed: np.ndarray
+  distance: np.ndarray
+  distance_lead: np.ndarray
+  a_target: np.ndarray
+  acceleration: np.ndarray
+  should_stop: np.ndarray
+  fcw: np.ndarray
+  source: list
+  active: np.ndarray
+  shadow_active: np.ndarray
+  launching: np.ndarray
+  pace: np.ndarray
+  filtered_cap: np.ndarray
+  selected_lead: np.ndarray
+  profile_accel_max: np.ndarray
+  effective_accel_max: np.ndarray
+  controller_fault: np.ndarray
+  solver_failures: int
+
+
+def _set_accel_controller_params(*, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None:
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+  params.put("AccelPersonality", profile, block=True)
+  params.put_bool("DynamicExperimentalControl", dec_enabled, block=True)
+
+
+def _run(
+  *,
+  duration: float,
+  controller_enabled: bool,
+  profile: int = 1,
+  v_lead: float | Callable[[float], float] = 0.0,
+  v_cruise: float = 30.0,
+  dec_enabled: bool = False,
+  **plant_kwargs,
+) -> ClosedLoopTrace:
+  _set_accel_controller_params(enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled)
+  plant = Plant(**plant_kwargs)
+  solver_failures = 0
+  original_mpc_reset = plant.planner.mpc.reset
+
+  def count_failed_solve() -> None:
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    original_mpc_reset()
+
+  plant.planner.mpc.reset = count_failed_solve
+  rows = []
+  sources = []
+  while plant.current_time < duration:
+    lead_speed = float(v_lead) if isinstance(v_lead, (int, float)) else v_lead(plant.current_time)
+    controller_fault = plant.planner.mpc.last_solution_status != 0
+    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
+    controller = plant.planner.accel_controller_result
+    rows.append(
+      (
+        plant.current_time,
+        result["speed"],
+        result["distance"],
+        result["distance_lead"],
+        result["a_target"],
+        result["realized_acceleration"],
+        result["should_stop"],
+        result["fcw"],
+        controller.active,
+        controller.shadow_active,
+        controller.launching,
+        controller.live_pace,
+        controller.live_filtered_cap,
+        controller.selected_lead,
+        controller.profile_accel_max,
+        controller.effective_accel_max,
+        controller_fault,
+      )
+    )
+    sources.append(result["mpc_source"])
+
+  data = np.asarray(rows, dtype=float)
+  return ClosedLoopTrace(
+    time=data[:, 0],
+    speed=data[:, 1],
+    distance=data[:, 2],
+    distance_lead=data[:, 3],
+    a_target=data[:, 4],
+    acceleration=data[:, 5],
+    should_stop=data[:, 6].astype(bool),
+    fcw=data[:, 7].astype(bool),
+    source=sources,
+    active=data[:, 8].astype(bool),
+    shadow_active=data[:, 9].astype(bool),
+    launching=data[:, 10].astype(bool),
+    pace=data[:, 11],
+    filtered_cap=data[:, 12],
+    selected_lead=data[:, 13].astype(int),
+    profile_accel_max=data[:, 14],
+    effective_accel_max=data[:, 15],
+    controller_fault=data[:, 16].astype(bool),
+    solver_failures=solver_failures,
+  )
+
+
+def _first_time_below(trace: ClosedLoopTrace, threshold: float) -> float:
+  indices = np.flatnonzero(trace.a_target <= threshold)
+  assert len(indices), f"never reached {threshold} m/s²"
+  return float(trace.time[indices[0]])
+
+
+def _sustained_time_below(trace: ClosedLoopTrace, threshold: float, *, after: float = 0.5, duration: float = 0.5) -> float:
+  required_frames = round(duration / DT_MDL)
+  below = (trace.time >= after) & (trace.a_target <= threshold)
+  sustained = np.convolve(below.astype(int), np.ones(required_frames, dtype=int), mode="valid") == required_frames
+  indices = np.flatnonzero(sustained)
+  assert len(indices), f"never sustained {threshold} m/s² for {duration} s"
+  return float(trace.time[indices[0]])
+
+
+def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray:
+  indices = np.flatnonzero(trace.time >= after)
+  assert len(indices) >= 2
+  return np.diff(trace.a_target[indices]) / DT_MDL
+
+
+def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0) -> np.ndarray:
+  filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid")
+  jerk = np.diff(filtered_acceleration) / DT_MDL
+  return jerk[trace.time[2:-1] >= after]
+
+
+def _has_propulsion_brake_reversal(trace: ClosedLoopTrace, after: float) -> bool:
+  indices = np.flatnonzero(trace.time >= after)
+  commands = trace.a_target[indices]
+  propulsion_seen = False
+  for command in commands:
+    propulsion_seen = propulsion_seen or command > 0.2
+    if propulsion_seen and command < -0.2:
+      return True
+  return False
+
+
+@pytest.fixture(autouse=True)
+def _restore_controller_defaults():
+  yield
+  _set_accel_controller_params(enabled=False, profile=1, dec_enabled=False)
+
+
+@pytest.mark.parametrize(
+  ("plant_kwargs", "expect_shadow_active"),
+  [
+    ({"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, False),
+    ({"e2e": True, "lead_relevancy": False, "speed": 20.0}, True),
+  ],
+  ids=("disengaged", "e2e-shadow"),
+)
+def test_non_actuating_modes_are_bit_exact(plant_kwargs, expect_shadow_active):
+  common = dict(duration=2.0, v_lead=14.0, **plant_kwargs)
+  disabled = _run(controller_enabled=False, **common)
+  shadow = _run(controller_enabled=True, **common)
+
+  np.testing.assert_allclose(shadow.a_target, disabled.a_target, atol=1e-6, rtol=0.0)
+  np.testing.assert_array_equal(shadow.should_stop, disabled.should_stop)
+  np.testing.assert_array_equal(shadow.fcw, disabled.fcw)
+  assert shadow.source == disabled.source
+  assert not shadow.active.any()
+  if expect_shadow_active:
+    np.testing.assert_array_equal(shadow.shadow_active, ~shadow.controller_fault)
+  else:
+    assert not shadow.shadow_active.any()
+
+
+def test_disabled_profiles_are_bit_exact_in_engaged_acc():
+  common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0)
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+
+  for trace in traces[1:]:
+    np.testing.assert_allclose(trace.a_target, traces[0].a_target, atol=1e-6, rtol=0.0)
+    np.testing.assert_array_equal(trace.should_stop, traces[0].should_stop)
+    np.testing.assert_array_equal(trace.fcw, traces[0].fcw)
+    assert trace.source == traces[0].source
+  assert all(not trace.active.any() for trace in traces)
+  assert all(np.isinf(trace.effective_accel_max).all() for trace in traces)
+
+
+def test_dec_radar_lead_selects_acc_and_standstill_uses_shadow_only():
+  blended = _run(
+    duration=2.0,
+    controller_enabled=True,
+    dec_enabled=True,
+    e2e=True,
+    lead_relevancy=False,
+    speed=0.0,
+  )
+  radar_acc = _run(
+    duration=1.0,
+    controller_enabled=True,
+    dec_enabled=True,
+    e2e=True,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=55.0,
+    v_lead=12.0,
+  )
+
+  assert not blended.active[-10:].any()
+  np.testing.assert_array_equal(blended.shadow_active, ~blended.controller_fault)
+  assert radar_acc.active.all()
+
+
+def test_two_frame_dropout_and_false_relief_do_not_release_pace(record_property):
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if 2.0 <= current_time < 2.1:
+      return None
+    if 3.0 <= current_time < 3.1:
+      return {"dRel": truth["dRel"] + 5.0}
+    return truth
+
+  common = dict(
+    duration=5.0,
+    lead_relevancy=True,
+    speed=22.0,
+    distance_lead=85.0,
+    v_lead=14.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  for start in (2.0, 3.0):
+    before = trace.pace[np.flatnonzero(trace.time < start)[-1]]
+    guard = (trace.time >= start) & (trace.time < start + 0.2)
+    during_and_guard = trace.pace[guard & trace.active]
+    assert np.all(during_and_guard <= before + 1e-9)
+    assert np.isinf(trace.pace[guard & ~trace.active]).all()
+  assert not _has_propulsion_brake_reversal(trace, after=1.0)
+  record_property("clean_base_solver_failures", baseline.solver_failures)
+  record_property("accel_controller_solver_failures", trace.solver_failures)
+  assert trace.solver_failures <= baseline.solver_failures
+  if trace.solver_failures:
+    pytest.xfail("opt-in validation: absolute zero-solver-failure gate is unmet with raw two-frame all-lead dropout")
+
+
+def test_lead_slot_handoff_does_not_resurrect_stale_relief():
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    if current_time < 2.0:
+      return truth if lead_name == "leadOne" else None
+    if current_time < 2.1:
+      return None
+    if lead_name == "leadTwo":
+      return {"dRel": truth["dRel"] + 2.0, "radarTrackId": 38}
+    return None
+
+  trace = _run(
+    duration=4.0,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=80.0,
+    v_lead=14.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+
+  assert np.all(trace.selected_lead[(trace.time >= 0.5) & (trace.time < 2.0)] == 0)
+  assert np.all(trace.selected_lead[trace.time >= 2.2] == 1)
+  pace_before_handoff = trace.pace[np.flatnonzero(trace.time < 2.0)[-1]]
+  handoff_guard = trace.pace[(trace.time >= 2.0) & (trace.time < 2.3)]
+  assert np.all(handoff_guard <= pace_before_handoff + 1e-9)
+  assert not _has_propulsion_brake_reversal(trace, after=1.0)
+
+
+def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal():
+  glitch_start = 5.0
+  glitch_end = 5.5
+
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation:
+    if glitch_start <= current_time < glitch_end:
+      frame = round(current_time / DT_MDL)
+      observed = dict(truth)
+      observed["dRel"] = truth["dRel"] + (5.0 if frame % 2 else 0.0)
+      return observed
+    return truth
+
+  common = dict(
+    duration=10.0,
+    lead_relevancy=True,
+    speed=8.0,
+    distance_lead=20.0,
+    v_lead=1.5,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  control = _run(controller_enabled=True, **common)
+  trace = _run(controller_enabled=True, lead_observation_fn=observe, **common)
+
+  np.testing.assert_array_equal(trace.time, control.time)
+  jerk_window = (trace.time[1:] >= glitch_start) & (trace.time[1:] < glitch_end + 0.5)
+  assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) < 3.0
+
+  # Attribute only the disturbance response: this fixture has a later natural
+  # propulsion-to-brake transition even without the range glitch.
+  response_window = (trace.time >= glitch_start) & (trace.time < glitch_end + 1.0)
+  disturbance = trace.a_target[response_window] - control.a_target[response_window]
+  positive = np.flatnonzero(disturbance > 0.2)
+  if len(positive):
+    assert not np.any(disturbance[positive[0] + 1:] < -0.2)
+
+
+def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal():
+  def lead_speed(current_time: float) -> float:
+    return float(0.1 * (1.0 - np.cos(np.pi * current_time)))
+
+  trace = _run(
+    duration=9.0,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=2.0,
+    distance_lead=10.0,
+    v_lead=lead_speed,
+    v_cruise=8.0,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+
+  settled = trace.time >= 4.0
+  assert trace.active[settled].all()
+  assert np.all(trace.pace[settled] == 0.0)
+  assert np.max(trace.a_target[settled]) <= 0.2
+  assert not _has_propulsion_brake_reversal(trace, after=4.0)
+
+
+def test_severe_closing_never_delays_braking_or_reduces_clearance():
+  common = dict(
+    duration=12.0,
+    lead_relevancy=True,
+    speed=20.0,
+    distance_lead=160.0,
+    v_lead=3.5,
+    actuator_delay=0.20,
+    actuator_lag=0.20,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  controlled = _run(controller_enabled=True, **common)
+
+  for threshold in (-1.0, -2.0):
+    assert _first_time_below(controlled, threshold) <= _first_time_below(baseline, threshold) + 1e-9
+
+  baseline_gap = baseline.distance_lead - baseline.distance
+  controlled_gap = controlled.distance_lead - controlled.distance
+  assert controlled_gap.min() >= baseline_gap.min() - 1e-3
+  assert controlled_gap.min() > 0.4
+  onset = (controlled.time[1:] > 0.5) & (controlled.time[1:] < 3.0)
+  assert np.max(np.abs(np.diff(controlled.a_target)[onset] / DT_MDL)) < 4.0
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag"),
+  [
+    (0.10, 0.20),
+    (0.15, 0.25),
+    (0.20, 0.20),
+    (0.25, 0.30),
+    (0.30, 0.35),
+  ],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+def test_stopped_lead_noise_requires_four_departure_frames_and_launches_within_one_second(
+  actuator_delay, actuator_lag, record_property,
+):
+  departure_time = 1.0
+
+  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:
+    frame = round(current_time / DT_MDL)
+    if current_time < departure_time and frame % 4 == 0:
+      return {
+        "dRel": truth["dRel"] + 4.0,
+        "vRel": 1.5,
+        "vLead": 1.5,
+        "vLeadK": 1.5,
+        "aLeadK": 0.0,
+      }
+    return truth
+
+  common = dict(
+    duration=2.5,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=6.0,
+    v_lead=lead_speed,
+    v_cruise=8.0,
+    lead_observation_fn=observe,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  trace = _run(controller_enabled=True, **common)
+
+  baseline_should_stop_clear = np.flatnonzero((baseline.time >= departure_time) & ~baseline.should_stop)
+  baseline_launched = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05))
+  assert len(baseline_should_stop_clear)
+  assert len(baseline_launched)
+  record_property("clean_base_departure_should_stop_clear_time", float(baseline.time[baseline_should_stop_clear[0]] - departure_time))
+  record_property("clean_base_departure_launch_time", float(baseline.time[baseline_launched[0]] - departure_time))
+
+  before_departure = trace.time < departure_time
+  assert np.max(trace.speed[before_departure]) < 1e-3
+  assert not _has_propulsion_brake_reversal(trace, after=0.3)
+  first_three_departure_frames = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9)
+  record_property("predeparture_peak_command", float(np.max(trace.a_target[before_departure])))
+  record_property("first_three_departure_frames_peak_command", float(np.max(trace.a_target[first_three_departure_frames])))
+  assert np.max(trace.speed[first_three_departure_frames]) < 1e-3
+  assert not trace.launching[first_three_departure_frames].any()
+
+  launched = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
+  assert len(launched)
+  launch_time = float(trace.time[launched[0]] - departure_time)
+  departure_jerk = np.diff(trace.a_target[trace.time >= departure_time]) / DT_MDL
+  peak_departure_jerk = float(np.max(np.abs(departure_jerk)))
+  record_property("departure_launch_time", launch_time)
+  record_property("departure_peak_command_jerk", peak_departure_jerk)
+  assert launch_time <= 1.0
+  assert peak_departure_jerk < 4.0
+  assert trace.solver_failures == 0
+  assert not _has_propulsion_brake_reversal(trace, after=departure_time)
+
+
+def test_stop_hold_two_frame_total_lead_dropout_cannot_launch():
+  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
+    return None if 1.0 <= current_time < 1.1 else truth
+
+  trace = _run(
+    duration=2.0,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=6.0,
+    v_lead=0.0,
+    v_cruise=8.0,
+    lead_observation_fn=observe,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+
+  assert np.max(trace.speed) < 1e-3
+  assert np.max(trace.pace) == 0.0
+  assert trace.solver_failures == 0
+  assert not _has_propulsion_brake_reversal(trace, after=0.5)
+
+
+def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct():
+  common = dict(
+    duration=6.0,
+    controller_enabled=True,
+    lead_relevancy=False,
+    speed=0.0,
+    v_cruise=15.0,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+
+  onset_times = []
+  movement_times = []
+  for trace in traces:
+    positive = np.flatnonzero(trace.a_target > 0.05)
+    moving = np.flatnonzero(trace.speed > 0.01)
+    assert len(positive)
+    assert len(moving)
+    onset_times.append(float(trace.time[positive[0]]))
+    movement_times.append(float(trace.time[moving[0]]))
+    assert trace.solver_failures == 0
+
+  assert max(onset_times) - min(onset_times) <= DT_MDL
+  assert max(onset_times) <= 4 * DT_MDL
+  assert max(movement_times) <= 1.0
+
+  for sample_time in (2.0,):
+    realized = [float(trace.acceleration[np.searchsorted(trace.time, sample_time)]) for trace in traces]
+    assert realized[0] < realized[1] < realized[2], (sample_time, realized)
+  final_speeds = [trace.speed[-1] for trace in traces]
+  assert final_speeds[0] < final_speeds[1] < final_speeds[2]
+  assert final_speeds[1] - final_speeds[0] > 0.5
+  assert final_speeds[2] - final_speeds[1] > 0.4
+
+
+def test_profile_trajectory_is_pre_mpc_and_not_a_custom_output_clamp():
+  _set_accel_controller_params(enabled=True, profile=0)
+  plant = Plant(speed=10.0, actuator_delay=0.15, actuator_lag=0.20)
+  # Start above Eco's table value to verify the controller hands the current
+  # feasible acceleration to MPC and slews down instead of clipping the output.
+  plant.acceleration = 1.30
+  plant.planner.a_desired = 1.30
+
+  result = plant.step(v_cruise=30.0)
+  controller = plant.planner.accel_controller_result
+
+  assert controller.mpc_accel_max is not None
+  assert controller.mpc_shape_cruise
+  np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], controller.mpc_accel_max)
+  assert result["a_target"] > controller.profile_accel_max
+  assert ACCEL_MIN <= result["a_target"] <= get_max_accel(plant.speed)
+
+
+def test_solver_fault_discards_live_state_before_fresh_preshape_seed():
+  _set_accel_controller_params(enabled=True, profile=1)
+  plant = Plant(speed=10.0, actuator_delay=0.15, actuator_lag=0.20)
+  plant.step(v_cruise=30.0)
+  assert plant.planner.accel_controller_result.active
+
+  plant.planner.mpc.last_solution_status = 3
+  plant.planner.mpc.reset()
+  plant.step(v_cruise=30.0)
+  faulted = plant.planner.accel_controller_result
+  assert not faulted.active
+  assert np.isinf(faulted.live_pace)
+  assert faulted.mpc_accel_max is None
+  assert not faulted.mpc_shape_cruise
+
+  # Represent the next successful MPC solve; the controller must seed from
+  # current state rather than resurrecting its discarded pre-fault history.
+  plant.planner.mpc.last_solution_status = 0
+  plant.step(v_cruise=30.0)
+  recovered = plant.planner.accel_controller_result
+  assert recovered.active
+  assert np.isfinite(recovered.live_pace)
+  assert recovered.mpc_accel_max is not None
+  assert recovered.mpc_shape_cruise
+
+
+@pytest.mark.parametrize(
+  ("actuator_delay", "actuator_lag", "current_tn_jerk_p95"),
+  [
+    (0.10, 0.20, 0.0988673),
+    (0.15, 0.25, 0.1010401),
+    (0.20, 0.20, 0.1004875),
+    (0.25, 0.30, 0.0973712),
+    (0.30, 0.35, 0.1050558),
+  ],
+  ids=("toyota", "honda", "gm", "hyundai", "ford"),
+)
+def test_far_lead_deceleration_is_early_across_actuator_dynamics(actuator_delay, actuator_lag, current_tn_jerk_p95, record_property):
+  common = dict(
+    duration=11.0,
+    lead_relevancy=True,
+    speed=25.0,
+    distance_lead=200.0,
+    v_lead=15.0,
+    actuator_delay=actuator_delay,
+    actuator_lag=actuator_lag,
+  )
+  baseline = _run(controller_enabled=False, **common)
+  controlled = _run(controller_enabled=True, profile=1, **common)
+
+  baseline_onset = _sustained_time_below(baseline, -0.10)
+  controlled_onset = _sustained_time_below(controlled, -0.10)
+  assert controlled_onset <= baseline_onset - 0.5
+
+  # The feature moves the event earlier; it must not buy that anticipation with a
+  # harsher routine stop or a noisier physical response.
+  assert controlled.acceleration.min() >= baseline.acceleration.min() - 0.1
+  baseline_jerk = _filtered_realized_jerk(baseline)
+  controlled_jerk = _filtered_realized_jerk(controlled)
+  clean_base_jerk_p95 = float(np.percentile(np.abs(baseline_jerk), 95))
+  controller_jerk_p95 = float(np.percentile(np.abs(controlled_jerk), 95))
+  record_property("clean_base_filtered_realized_jerk_p95", clean_base_jerk_p95)
+  record_property("current_tn_filtered_realized_jerk_p95", current_tn_jerk_p95)
+  record_property("accel_controller_filtered_realized_jerk_p95", controller_jerk_p95)
+  assert np.isfinite(clean_base_jerk_p95)
+  assert np.isfinite(controller_jerk_p95)
+  if controller_jerk_p95 > current_tn_jerk_p95:
+    pytest.xfail("opt-in validation: filtered realized-jerk p95 still exceeds the saved current-tn comparator")
+  assert controller_jerk_p95 <= current_tn_jerk_p95
+
+
+def test_profiles_order_anticipation_and_pace_rates():
+  common = dict(
+    duration=10.0,
+    controller_enabled=True,
+    lead_relevancy=True,
+    speed=25.0,
+    distance_lead=200.0,
+    v_lead=15.0,
+    actuator_delay=0.20,
+    actuator_lag=0.25,
+  )
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+  onsets = []
+  for trace in traces:
+    restricting = np.flatnonzero(np.diff(trace.pace) < -1e-6)
+    assert len(restricting)
+    onsets.append(float(trace.time[restricting[0] + 1]))
+  assert onsets[0] < onsets[1] < onsets[2]
+
+  expected_down_rates = [0.25, 0.335, 0.50]
+  measured_down_rates = []
+  for trace in traces:
+    restricting = np.flatnonzero(np.diff(trace.pace) < -1e-6)
+    measured_down_rates.append(float(np.median(-np.diff(trace.pace)[restricting] / DT_MDL)))
+  np.testing.assert_allclose(measured_down_rates, expected_down_rates, atol=1e-6, rtol=0.0)

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

@@ -5,6 +5,8 @@ from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
+
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
@@ -107,7 +109,11 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
+  relc = RoadEdgeLaneChangeController(dh)
+  relc.enabled = True
   dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
-    dh.update(carstate, lateral_active, lane_change_prob)
+    dh.update(carstate, lateral_active, lane_change_prob,
+              left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
   assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+

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

@@ -0,0 +1,99 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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 pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
+  RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
+)
+
+V_HIGH = MIN_SPEED + 2.0
+V_LOW = MIN_SPEED - 1.0
+
+
+@pytest.fixture
+def relc(mocker):
+  mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
+  mock_params.return_value.get_bool.return_value = True
+  controller = RoadEdgeLaneChangeController(DesireHelper())
+  controller.enabled = True
+  return controller
+
+
+def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
+  for _ in range(int(seconds / DT_MDL) + 1):
+    controller.update(road_edge_stds, lane_line_probs, v_ego)
+
+
+@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
+  ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
+  ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
+])
+def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
+  drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
+  assert getattr(relc, attr)
+
+
+def test_edge_detection_requires_time(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
+  assert not relc.left_edge_detected
+
+
+def test_both_edges_detected(relc):
+  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+  assert relc.right_edge_detected
+
+
+def test_noise_doesnt_clear(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update(*clear, V_HIGH)
+  relc.update(*edge, V_HIGH)
+  assert relc.left_edge_detected
+
+
+def test_clears_after_window(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_low_speed_skips(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_speed_drop_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
+  assert not relc.left_edge_detected
+
+
+def test_param_off_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.params.get_bool.return_value = False
+  relc.read_params()
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected

sunnypilot/selfdrive/selfdrived/events.py

@@ -243,4 +243,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.laneChangeRoadEdge: {
+    ET.WARNING: Alert(
+      "Lane Change Unavailable: Road Edge",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }

sunnypilot/sunnylink/params_metadata.json

@@ -1,4 +1,26 @@
 {
+  "AccelPersonality": {
+    "title": "Acceleration Profile",
+    "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
+    "options": [
+      {
+        "value": 0,
+        "label": "Eco"
+      },
+      {
+        "value": 1,
+        "label": "Normal"
+      },
+      {
+        "value": 2,
+        "label": "Sport"
+      }
+    ]
+  },
+  "AccelPersonalityEnabled": {
+    "title": "Enable Accel Controller",
+    "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""
@@ -1118,6 +1140,10 @@
     "title": "Record Front Lock",
     "description": ""
   },
+  "RoadEdgeLaneChangeEnabled": {
+    "title": "Block Lane Change: Road Edge Detection",
+    "description": ""
+  },
   "RoadName": {
     "title": "Road Name",
     "description": ""
@@ -1323,6 +1349,22 @@
     "max": 5.0,
     "step": 0.1,
     "unit": "m/s\u00b2"
+  },
+    "ToyotaAutoHold": {
+    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+    "description": ""
+  },
+    "ToyotaEnhancedBsm": {
+    "title": "Toyota: Prius TSS2 BSM and some tssp",
+    "description": ""
+  },
+    "ToyotaTSS2Long": {
+    "title": "Toyota: custom longitudinal for TSS2",
+    "description": ""
+  },
+    "ToyotaDriveMode": {
+    "title": "Enable drive mode btn link",
+    "description": ""
   },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",

sunnypilot/sunnylink/settings_ui.json

@@ -620,6 +620,58 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "key": "AccelPersonality",
+              "widget": "multiple_button",
+              "title": "Acceleration Profile",
+              "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                },
+                {
+                  "type": "param",
+                  "key": "AccelPersonalityEnabled",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -2001,6 +2053,22 @@
                   "equals": true
                 }
               ]
+            },
+            {
+              "key": "PlanplusControl",
+              "widget": "option",
+              "title": "Plan Plus Controls",
+              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
+              "min": 0.0,
+              "max": 2.0,
+              "step": 0.1,
+              "enablement": [
+                {
+                  "type": "param",
+                  "key": "ShowAdvancedControls",
+                  "equals": true
+                }
+              ]
             }
           ]
         },
@@ -2168,6 +2236,50 @@
       "title": "Toyota / Lexus Settings",
       "description": "",
       "items": [
+        {
+          "key": "ToyotaAutoHold",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Prius TSS2 BSM and some tssp",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaTSS2Long",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: custom longitudinal for TSS2",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaDriveMode",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enable drive mode btn link",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
         {
           "key": "ToyotaEnforceStockLongitudinal",
           "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,6 +43,32 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Accel Controller
+    description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
+      and stopping authority.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
+      and recovers more quickly.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: AccelPersonalityEnabled
+      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

sunnypilot/sunnylink/settings_ui_src/pages/models.yaml

@@ -51,6 +51,16 @@ sections:
         key: LagdToggle
         equals: true
     - $ref: '#/macros/advanced_only'
+  - key: PlanplusControl
+    widget: option
+    title: Plan Plus Controls
+    description: Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover
+      to lane center; too high and it will ping-pong.
+    min: 0.0
+    max: 2.0
+    step: 0.1
+    enablement:
+    - $ref: '#/macros/advanced_only'
 - id: lateral_control
   title: Lateral Control
   description: Neural network lateral control for supported models

sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -60,6 +60,30 @@ sections:
   title: Toyota / Lexus Settings
   description: ''
   items:
+  - key: ToyotaAutoHold
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaEnhancedBsm
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Prius TSS2 BSM and some tssp'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaTSS2Long
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: custom longitudinal for TSS2'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaDriveMode
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Enable drive mode btn link
+    enablement:
+    - $ref: '#/macros/not_engaged'
   - key: ToyotaEnforceStockLongitudinal
     widget: toggle
     needs_onroad_cycle: true

sunnypilot/sunnylink/tests/test_settings_schema.py

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

system/ui/lib/scroll_panel2.py

@@ -45,8 +45,9 @@ class ScrollState(Enum):
 
 
 class GuiScrollPanel2:
-  def __init__(self, horizontal: bool = True) -> None:
+  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
     self._horizontal = horizontal
+    self._handle_out_of_bounds = handle_out_of_bounds
     self._state = ScrollState.STEADY
     self._offset: rl.Vector2 = rl.Vector2(0, 0)
     self._initial_click_event: MouseEvent | None = None
@@ -98,7 +99,7 @@ class GuiScrollPanel2:
       # simple exponential return if out of bounds
       # out of bounds is handled by snapping, so skip if set
       out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset
-      if out_of_bounds and snap_target is None:
+      if out_of_bounds and snap_target is None and self._handle_out_of_bounds:
         target = max_offset if self.get_offset() > max_offset else min_offset
 
         dt = rl.get_frame_time() or 1e-6

system/ui/widgets/scroller.py

@@ -75,7 +75,6 @@ class _Scroller(Widget):
     self._items: list[Widget] = []
     self._horizontal = horizontal
     self._snap_items = snap_items
-    assert not self._snap_items or self._horizontal, "Snapping is only supported for horizontal scrolling"
     self._spacing = spacing
     self._pad = pad
 
@@ -191,8 +190,12 @@ class _Scroller(Widget):
     snap_target: float | None = None
     if self._snap_items and visible_items and self._scrolling_to[0] is None:
       # TODO: this doesn't handle two small buttons at the edges well
-      center_pos = self._rect.x + self._rect.width / 2
-      closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
+      if self._horizontal:
+        center_pos = self._rect.x + self._rect.width / 2
+        closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
+      else:
+        center_pos = self._rect.y + self._rect.height / 2
+        closest_delta_pos = min((((item.rect.y + item.rect.height / 2) - center_pos) for item in visible_items), key=abs)
       snap_target = self.scroll_panel.get_offset() - closest_delta_pos
 
     return self.scroll_panel.update(self._rect, content_size, snap_target=snap_target)