Prevent radar timing skew from resetting longitudinal control

Planner replan jitter leaks into desired_lateral_jerk on straights and
clears the friction deadzone, injecting torque chatter with no steering
need behind it. Off by default via FrictionJerkDeadzoneEnabled.

.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 profiles (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -272,6 +283,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     // Torque lateral control custom params
     {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}},
     {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}},
+    {"FrictionJerkDeadzoneEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}},
     {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}},
     {"TorqueControlTune", {PERSISTENT | BACKUP, FLOAT, "0.0"}},

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/latcontrol_torque.py

@@ -84,11 +84,12 @@ class LatControlTorque(LatControl):
     lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
     raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
     desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    friction_jerk = self.extension.get_friction_jerk(desired_lateral_jerk)
     gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
     ff = gravity_adjusted_future_lateral_accel
     # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
     ff -= self.torque_params.latAccelOffset
-    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+    ff += get_friction(error + JERK_GAIN * friction_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
     if not active:
       output_torque = 0.0

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

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

selfdrive/controls/lib/longitudinal_planner.py

@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
-    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -113,6 +113,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
     accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+    profile_accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, [ACCEL_MIN, ACCEL_MAX], self.CP)
 
     if reset_state:
       self.v_desired_filter.x = v_ego
@@ -129,16 +130,21 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast = max(accel_coast, accel_clip[0])
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
+    controller_accel_max = profile_accel_clip[1] if self.allow_throttle else 0.0
 
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
+    previous_output_a_target = self.output_a_target
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+    base_v_cruise = v_cruise
 
     if force_slow_decel:
       v_cruise = 0.0
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    is_e2e = LongitudinalPlannerSP.update_accel_controller_mpc(
+      self, sm, base_v_cruise, v_cruise, prev_accel_constraint, reset_state=reset_state,
+      cruise_initialized=v_cruise_initialized, planner_accel=self.a_desired, previous_output_accel=previous_output_a_target,
+      available_accel_max=controller_accel_max, previous_should_stop=self.output_should_stop, force_decel=force_slow_decel,
+    )
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -154,13 +160,14 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
-    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
-                                                                        action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
+    action_t = self.CP.longitudinalActuatorDelay + DT_MDL
+    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
+      self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping,
+    )
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.is_e2e(sm):
+    if is_e2e:
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:

selfdrive/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,11 @@
 #!/usr/bin/env python3
+from collections import deque
+from collections.abc import Callable
+from dataclasses import dataclass
+import math
 import time
+from typing import Any
+
 import numpy as np
 
 from cereal import log
@@ -11,12 +17,105 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
+
+
+@dataclass(frozen=True)
+class ActuatorModel:
+  planner_delay: float
+  transport_delay: float
+  actuator_lag: float
+  command_rate_limit: float
+  stopping_acceleration: float
+  standstill_breakaway_acceleration: float
+  standstill_breakaway_time: float
+
+  def __post_init__(self):
+    nonnegative_fields = {
+      "planner_delay": self.planner_delay,
+      "transport_delay": self.transport_delay,
+      "actuator_lag": self.actuator_lag,
+      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
+      "standstill_breakaway_time": self.standstill_breakaway_time,
+    }
+    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
+      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
+    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
+      raise ValueError("command_rate_limit must be finite and positive")
+    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
+      raise ValueError("stopping_acceleration must be finite and non-positive")
+
+
+# Route-derived conservative Prius TSS2 stress model for the acceleration-controller
+# regression suite. The 1.0 m/s² gate represents prompt takeoffs, not a universal
+# physical threshold: the supplied routes also contain low-command creep departures.
+# This models vehicle response only and does not emulate Toyota's CAN controller.
+PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
+  planner_delay=0.05,
+  transport_delay=0.0,
+  actuator_lag=0.20,
+  command_rate_limit=4.0,
+  stopping_acceleration=-2.0,
+  standstill_breakaway_acceleration=1.0,
+  standstill_breakaway_time=0.05,
+)
+
+
 class Plant:
   messaging_initialized = False
 
-  def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
-               enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False):
-    self.rate = 1. / DT_MDL
+  def __init__(
+    self,
+    lead_relevancy=False,
+    speed=0.0,
+    distance_lead=2.0,
+    enabled=True,
+    only_lead2=False,
+    only_radar=False,
+    e2e=False,
+    personality=0,
+    force_decel=False,
+    lead_observation_fn: LeadObservationFn | None = None,
+    model_action_fn: ModelActionFn | None = None,
+    ego_observation_fn: EgoObservationFn | None = None,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+    actuator_model: ActuatorModel | None = None,
+  ):
+    """Closed-loop longitudinal planner plant.
+
+    ``lead_observation_fn(time, lead_name, truth)`` may return a complete or partial
+    observed LeadData mapping, or ``None`` for an absent lead. It is called separately
+    for ``leadOne`` and ``leadTwo``. The supplied truth mapping is a copy, and observed
+    values never affect the physical lead trajectory.
+
+    ``model_action_fn(time, v_ego, a_ego)`` returns
+    ``(desired_acceleration, should_stop)``.
+
+    ``ego_observation_fn(time, true_v_ego, true_a_ego)`` returns the observed
+    ``(v_ego, a_ego)`` published in ``carState``. It can inject measurement noise
+    without changing the physical plant state.
+
+    Passing ``actuator_delay`` both overrides ``CP.longitudinalActuatorDelay`` and
+    adds the corresponding command transport delay to the plant. ``None`` keeps the
+    historical Honda planner delay with instantaneous plant response. ``actuator_lag``
+    is an optional first-order acceleration-response time constant. Both defaults keep
+    historical plant dynamics unchanged.
+
+    ``actuator_model`` opts into a staged vehicle-response model. Its planner delay
+    is used by MPC, while its independent transport delay is used by the command
+    queue before rate limiting, standstill breakaway confirmation, and first-order
+    lag. Leaving it unset preserves the historical actuator path.
+    """
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
 
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
@@ -28,10 +127,15 @@ class Plant:
 
     self.v_lead_prev = 0.0
 
-    self.distance = 0.
+    self.distance = 0.0
     self.speed = speed
     self.should_stop = False
     self.acceleration = 0.0
+    self.a_target = 0.0
+    self.actuator_command = 0.0
+    self.applied_actuator_command = 0.0
+    self.breakaway_confirmed = False
+    self._breakaway_timer = 0.0
 
     # lead car
     self.lead_relevancy = lead_relevancy
@@ -42,9 +146,18 @@ class Plant:
     self.e2e = e2e
     self.personality = personality
     self.force_decel = force_decel
+    self.lead_observation_fn = lead_observation_fn
+    self.model_action_fn = model_action_fn
+    self.ego_observation_fn = ego_observation_fn
+    self.actuator_model = actuator_model
+    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
+    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
+    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
+                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None))
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1. / self.rate
+    self.ts = 1.0 / self.rate
     time.sleep(0.1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
@@ -52,14 +165,86 @@ class Plant:
     from opendbc.car.honda.interface import CarInterface
 
     CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    if self.actuator_delay is not None:
+      CP.longitudinalActuatorDelay = self.actuator_delay
     CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
     self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
 
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
   @property
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0):
+  @staticmethod
+  def _lead_message(observation: LeadObservation):
+    lead = log.RadarState.LeadData.new_message()
+    for field, value in observation.items():
+      setattr(lead, field, value)
+    return lead
+
+  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
+    if self.lead_observation_fn is None:
+      return dict(truth) if present_by_default else None
+
+    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
+    if observed is None:
+      return None
+
+    # Partial overrides are convenient for individual sensor glitches, while copying
+    # from truth ensures every field written to cereal is deterministic.
+    complete_observation = dict(truth)
+    complete_observation.update(observed)
+    return complete_observation
+
+  def _update_actuator(self, command: float) -> tuple[float, float]:
+    if self._actuator_delay_queue:
+      self._actuator_delay_queue.append(command)
+      delayed_command = self._actuator_delay_queue.popleft()
+    else:
+      delayed_command = command
+
+    if self.actuator_model is not None:
+      max_command_delta = self.actuator_model.command_rate_limit * self.ts
+      self.applied_actuator_command = float(np.clip(delayed_command,
+                                                   self.applied_actuator_command - max_command_delta,
+                                                   self.applied_actuator_command + max_command_delta))
+
+      if self.speed < 0.01:
+        if self.applied_actuator_command <= 0.0:
+          self.breakaway_confirmed = False
+          self._breakaway_timer = 0.0
+        elif not self.breakaway_confirmed:
+          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
+          if breakaway_ready:
+            self._breakaway_timer += self.ts
+          else:
+            self._breakaway_timer = 0.0
+
+          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
+        if not self.breakaway_confirmed:
+          self.acceleration = 0.0
+          return delayed_command, self.acceleration
+      else:
+        self.breakaway_confirmed = True
+
+      response_command = self.applied_actuator_command
+    else:
+      # Preserve the historical response path exactly when no staged model is used.
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
     # ******** publish a fake model going straight and fake calibration ********
     # note that this is worst case for MPC, since model will delay long mpc by one time step
     radar = messaging.new_message('radarState')
@@ -72,39 +257,48 @@ class Plant:
     car_state_sp = messaging.new_message('carStateSP')
     live_map_data_sp = messaging.new_message('liveMapDataSP')
     gps_data = messaging.new_message('gpsLocation')
-    a_lead = (v_lead - self.v_lead_prev)/self.ts
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
     self.v_lead_prev = v_lead
 
     if self.lead_relevancy:
-      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob_lead > .5:
+      elif prob_lead > 0.5:
         status = True
       else:
         status = False
     else:
-      d_rel = 200.
-      v_rel = 0.
+      d_rel = 200.0
+      v_rel = 0.0
       prob_lead = 0.0
       status = False
 
-    lead = log.RadarState.LeadData.new_message()
-    lead.dRel = float(d_rel)
-    lead.yRel = 0.0
-    lead.vRel = float(v_rel)
-    lead.aRel = float(a_lead - self.acceleration)
-    lead.vLead = float(v_lead)
-    lead.vLeadK = float(v_lead)
-    lead.aLeadK = float(a_lead)
-    # TODO use real radard logic for this
-    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
-    lead.status = status
-    lead.modelProb = float(prob_lead)
-    if not self.only_lead2:
-      radar.radarState.leadOne = lead
-    radar.radarState.leadTwo = lead
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "aRel": float(a_lead - self.acceleration),
+      "vLead": float(v_lead),
+      "dPath": 0.0,
+      "vLat": 0.0,
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "fcw": False,
+      "status": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
 
     # Simulate model predicting slightly faster speed
     # this is to ensure lead policy is effective when model
@@ -112,10 +306,15 @@ class Plant:
     position = log.XYZTData.new_message()
     position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
     model.modelV2.position = position
-    model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
+    if self.model_action_fn is None:
+      model_acceleration, model_should_stop = self.acceleration + 0.1, False
+    else:
+      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
+    model.modelV2.action.desiredAcceleration = float(model_acceleration)
+    model.modelV2.action.shouldStop = bool(model_should_stop)
     velocity = log.XYZTData.new_message()
     velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
-    velocity.x[0] = float(self.speed) # always start at current speed
+    velocity.x[0] = float(self.speed)  # always start at current speed
     model.modelV2.velocity = velocity
     acceleration = log.XYZTData.new_message()
     acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
@@ -126,33 +325,45 @@ class Plant:
     ss.selfdriveState.experimentalMode = self.e2e
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
-    car_state.carState.vEgo = float(self.speed)
+    true_v_ego = self.speed
+    true_a_ego = self.acceleration
+    published_v_ego = true_v_ego
+    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
+    if self.ego_observation_fn is not None:
+      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
+    car_state.carState.vEgo = float(published_v_ego)
+    car_state.carState.aEgo = float(published_a_ego)
     car_state.carState.standstill = bool(self.speed < 0.01)
     car_state.carState.vCruise = float(v_cruise * 3.6)
-    car_control.carControl.orientationNED = [0., float(pitch), 0.]
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
 
     # ******** get controlsState messages for plotting ***
-    sm = {'radarState': radar.radarState,
-          'carState': car_state.carState,
-          'carControl': car_control.carControl,
-          'controlsState': control.controlsState,
-          'selfdriveState': ss.selfdriveState,
-          'liveParameters': lp.liveParameters,
-          'modelV2': model.modelV2,
-          'carStateSP': car_state_sp.carStateSP,
-          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation}
+    sm = {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'liveParameters': lp.liveParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    }
     self.planner.update(sm)
-    self.acceleration = self.planner.output_a_target
+    self.a_target = self.planner.output_a_target
+    self.actuator_command = self.a_target
     if self.planner.output_should_stop:
-      self.acceleration = min(-0.5, self.acceleration)
+      stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
+      self.actuator_command = min(stopping_acceleration, self.actuator_command)
+    delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
     self.speed = self.speed + self.acceleration * self.ts
     self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********
-    #print(self.distance, speed)
+    # print(self.distance, speed)
     if self.speed <= 0:
       self.speed = 0
       self.acceleration = 0
@@ -160,30 +371,64 @@ class Plant:
 
     # *** radar model ***
     if self.lead_relevancy:
-      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
     else:
-      d_rel = 200.
-      v_rel = 0.
+      d_rel = 200.0
+      v_rel = 0.0
 
     # print at 5hz
     # if (self.rk.frame % (self.rate // 5)) == 0:
     #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
     #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
 
-
     # ******** update prevs ********
     self.rk.monitor_time()
 
+    accel_controller_result = getattr(self.planner, "accel_controller_result", None)
     return {
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "planner_acceleration": self.a_target,
+      "actuator_command": self.actuator_command,
+      "stop_clamped_actuator_command": self.actuator_command,
+      "delayed_actuator_command": delayed_actuator_command,
+      "applied_actuator_command": self.applied_actuator_command,
+      "vehicle_actuator_command": self.applied_actuator_command,
+      "true_v_ego": true_v_ego,
+      "true_a_ego": true_a_ego,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "observed_a_ego": published_a_ego,
+      "observed_v_ego": published_v_ego,
+      "planner_delay": self.actuator_delay,
+      "transport_delay": self.transport_delay,
+      "breakaway_confirmed": self.breakaway_confirmed,
+      "breakaway_time": self._breakaway_timer,
       "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "pace_cap": getattr(accel_controller_result, "target_speed", None),
+      "base_target": getattr(accel_controller_result, "base_speed", None),
+      "raw_energy_cap": getattr(accel_controller_result, "raw_energy_cap", None),
+      "live_filtered_cap": getattr(accel_controller_result, "live_filtered_cap", None),
+      "shadow_filtered_cap": getattr(accel_controller_result, "shadow_filtered_cap", None),
+      "accel_controller_selected_lead": getattr(accel_controller_result, "selected_lead", None),
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
     }
 
+
 # simple engage in standalone mode
 def plant_thread():
   plant = Plant()

selfdrive/test/longitudinal_maneuvers/test_plant.py

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

selfdrive/ui/layouts/settings/toggles.py

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

selfdrive/ui/mici/layouts/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,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import AccelProfile
+
+__all__ = ["AccelController", "AccelControllerResult", "AccelControllerState", "AccelProfile"]

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

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

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

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

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

@@ -0,0 +1,456 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import log
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE, T_IDXS, LongitudinalMpc, get_T_FOLLOW
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  ACCEL_PROFILE_MAX_BP,
+  ACCEL_PROFILE_MAX_V,
+  CAP_FILTER_FRAMES,
+  HORIZON_SPEED_BUDGET,
+  POSITIVE_MPC_HEADROOM,
+  PROFILE_CONFIGS,
+  PROFILE_TRANSITION_JERK,
+  RADAR_STALE_TIMEOUT,
+  RELIEF_CONFIRM_FRAMES,
+  SHALLOW_BRAKE_BOUND,
+  STOP_GAP_RESERVE,
+  STOP_HOLD_CREEP_ABORT_FRAMES,
+  STOP_HOLD_EXIT_FRAMES,
+  AccelController,
+  AccelControllerState,
+  AccelProfile,
+)
+
+
+def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5):
+  return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau)
+
+
+def make_radar(lead_one=None, lead_two=None):
+  return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead())
+
+
+def make_controller(delay=0.10):
+  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay))
+
+
+def update(controller, radar_state=None, **overrides):
+  args = {
+    "base_speed": 25.0,
+    "v_ego": 10.0,
+    "a_ego": 0.0,
+    "profile": AccelProfile.normal,
+    "follow_personality": log.LongitudinalPersonality.standard,
+    "enabled": True,
+    "acc_selected": True,
+    "engaged": True,
+    "cruise_initialized": True,
+    "planner_accel": 0.0,
+    "action_accel": 0.0,
+    "stock_accel_max": ACCEL_MAX,
+    "previous_should_stop": False,
+  }
+  args.update(overrides)
+  return controller.update(radar_state or make_radar(), **args)
+
+
+def restrictive_radar():
+  return make_radar(make_lead(status=True, d_rel=25.0, v_lead_k=8.0, a_lead_k=-0.5))
+
+
+class TestProfiles:
+  def test_lookup_table_is_explicit_and_tunable(self):
+    assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
+    assert ACCEL_PROFILE_MAX_V == {
+      AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16],
+      AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30],
+      AccelProfile.sport: [2.00, 1.90, 1.55, 0.80, 0.50],
+    }
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
+    for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
+      assert AccelController.get_profile_accel_max(profile, speed) == expected
+    limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)]
+    assert all(0.0 <= limit <= ACCEL_MAX for limit in limits)
+    post_launch_limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(3.0, 40.0, 149)]
+    assert np.all(np.diff(post_launch_limits) <= 0.0)
+
+  @pytest.mark.parametrize("speed", [0.0, 3.0, 10.0, 25.0, 40.0])
+  def test_profile_order_is_distinct(self, speed):
+    limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
+    assert limits[0] < limits[1] < limits[2]
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_clear_road_applies_profile_immediately(self, profile):
+    result = update(make_controller(), v_ego=0.0, profile=profile)
+    expected = ACCEL_PROFILE_MAX_V[profile][0]
+    assert result.active and result.state == AccelControllerState.free
+    assert result.target_speed == result.base_speed == 25.0
+    assert result.positive_accel_max == expected
+    assert result.effective_accel_max == expected
+    if expected == ACCEL_MAX:
+      assert result.mpc_accel_max is None
+    else:
+      np.testing.assert_array_equal(result.mpc_accel_max, min(expected + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
+
+  @pytest.mark.parametrize(("profile", "expected"), [
+    (AccelProfile.eco, 1.25), (AccelProfile.normal, 1.40), (AccelProfile.sport, 1.90),
+  ])
+  def test_launch_strength_is_preserved_through_three_meters_per_second(self, profile, expected):
+    result = update(make_controller(), v_ego=3.0, profile=profile)
+    assert result.profile_accel_max == expected
+    assert result.positive_accel_max == expected
+    assert result.effective_accel_max == expected
+
+  def test_turn_or_throttle_limit_intersects_profile(self):
+    result = update(make_controller(), profile=AccelProfile.sport, stock_accel_max=0.0)
+    assert result.positive_accel_max == 0.0
+    assert result.effective_accel_max == 0.0
+    np.testing.assert_array_equal(result.mpc_accel_max, 0.0)
+
+  def test_profile_switch_changes_ceiling_without_a_step(self):
+    controller = make_controller()
+    sport = update(controller, profile=AccelProfile.sport, v_ego=10.0)
+    eco = update(controller, profile=AccelProfile.eco, v_ego=10.0)
+    assert sport.effective_accel_max > eco.effective_accel_max > eco.positive_accel_max
+    assert sport.effective_accel_max - eco.effective_accel_max == pytest.approx(PROFILE_TRANSITION_JERK * DT_MDL)
+
+  def test_invalid_profile_defaults_to_normal(self):
+    result = update(make_controller(), profile=999)
+    assert result.profile == AccelProfile.normal
+
+
+class TestEnergyEnvelope:
+  def test_relative_pace_energy_formula(self):
+    controller = make_controller()
+    lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
+    envelope = controller.calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    delay = controller._delay()
+    lead_xv = LongitudinalMpc.extrapolate_lead(lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)
+    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
+    v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
+    x_ego, _ = controller._project_ego(10.0, 0.0, delay)
+    gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
+    expected = v_lead + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * gap)
+    assert envelope.cap == pytest.approx(expected)
+    assert envelope.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * gap))
+
+  def test_profile_order_controls_approach_timing(self):
+    controller = make_controller()
+    radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
+    caps = [controller.calculate_energy_envelope(radar, 10.0, 0.0, profile).cap for profile in AccelProfile]
+    assert caps[0] < caps[1] < caps[2]
+
+  def test_stopped_lead_reserve_only_reduces_comfort_gap(self):
+    controller = make_controller()
+    lead = make_lead(status=True, d_rel=20.0, v_lead_k=0.0)
+    envelope = controller.calculate_energy_envelope(make_radar(lead), 2.0, 0.0, AccelProfile.normal)
+    expected = math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * envelope.usable_gap)
+
+    assert envelope.required_decel < 0.30
+    assert envelope.safety_usable_gap - envelope.usable_gap == pytest.approx(STOP_GAP_RESERVE)
+    assert envelope.cap == pytest.approx(expected)
+    assert envelope.departure_cap > envelope.cap
+
+  def test_stop_reserve_fades_out_of_urgent_braking(self):
+    controller = make_controller()
+    lead = make_lead(status=True, d_rel=20.0, v_lead_k=0.0)
+    envelope = controller.calculate_energy_envelope(make_radar(lead), 20.0, 0.0, AccelProfile.normal)
+
+    assert envelope.required_decel > 0.80
+    assert envelope.usable_gap == envelope.safety_usable_gap
+    assert envelope.cap == envelope.departure_cap
+    assert controller._ttc(envelope) == pytest.approx(envelope.safety_usable_gap / envelope.closing_speed)
+
+  def test_more_restrictive_lead_is_selected(self):
+    radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
+    envelope = make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == 1
+
+  @pytest.mark.parametrize("field,value", [("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0)])
+  def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
+    setattr(lead, field, value)
+    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == 0
+    assert math.isfinite(envelope.cap)
+
+  @pytest.mark.parametrize("field,value", [("dRel", math.nan), ("dRel", -1.0), ("vLeadK", math.nan), ("vLeadK", -2.0)])
+  def test_invalid_geometry_is_not_used(self, field, value):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
+    setattr(lead, field, value)
+    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert envelope.selected_lead == -1
+    assert envelope.lead_status
+
+  def test_raw_radar_is_never_mutated(self):
+    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan)
+    before = vars(lead).copy()
+    make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
+    assert vars(lead) == before
+
+
+class TestAccelControllerState:
+  def test_five_frame_median_needs_three_restrictive_samples(self):
+    controller = make_controller()
+    results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES)]
+    assert math.isinf(results[1].live_filtered_cap)
+    assert math.isfinite(results[2].live_filtered_cap)
+
+  def test_routine_approach_builds_safe_finite_horizon_ceiling(self):
+    controller = make_controller()
+    result = None
+    for _ in range(CAP_FILTER_FRAMES):
+      result = update(controller, restrictive_radar())
+    assert result is not None and result.state == AccelControllerState.restrict
+    ceiling = np.asarray(result.mpc_accel_max)
+    assert ceiling.shape == T_IDXS.shape
+    assert np.all(np.isfinite(ceiling))
+    assert np.all((ceiling >= ACCEL_MIN) & (ceiling <= ACCEL_MAX))
+    assert ceiling[0] >= 0.0
+    assert np.min(ceiling) < -0.05 and ceiling[-1] == pytest.approx(0.0)
+    assert np.trapezoid(-np.minimum(ceiling, 0.0), T_IDXS) <= HORIZON_SPEED_BUDGET * 10.0 + 1e-9
+
+  def test_ongoing_mpc_braking_does_not_ratchet_the_controller(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      previous = update(controller, restrictive_radar())
+    result = update(controller, restrictive_radar(), action_accel=-1.2, planner_accel=-1.0)
+    assert result.effective_accel_max >= previous.effective_accel_max - 0.60 * DT_MDL - 1e-9
+
+  def test_two_dropouts_cannot_release_restriction(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      restricted = update(controller, restrictive_radar())
+    results = [update(controller) for _ in range(2)]
+    assert all(result.active and result.effective_accel_max <= 0.0 for result in results)
+    assert all(result.effective_accel_max >= restricted.effective_accel_max for result in results)
+
+  def test_relief_requires_consecutive_confirmation(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      update(controller, restrictive_radar())
+    moving_away = make_radar(make_lead(status=True, d_rel=45.0, v_lead_k=13.0))
+    early = [update(controller, moving_away) for _ in range(RELIEF_CONFIRM_FRAMES - 1)]
+    assert all(result.state == AccelControllerState.hold and result.effective_accel_max <= 0.0 for result in early)
+    released = update(controller, moving_away)
+    assert released.state == AccelControllerState.free
+    assert released.effective_accel_max <= 0.0
+    accelerating = update(controller, moving_away)
+    assert released.effective_accel_max < accelerating.effective_accel_max <= accelerating.positive_accel_max
+
+  def test_shallow_brake_relief_uses_long_confirmation_without_delaying_tightening(self):
+    controller = make_controller()
+    controller.live.state = AccelControllerState.restrict
+    controller.live.bound = SHALLOW_BRAKE_BOUND + 0.05
+    matched = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=10.0))
+
+    held = [update(controller, matched) for _ in range(controller.shallow_brake_relief_frames - 1)]
+    assert all(result.effective_accel_max == pytest.approx(SHALLOW_BRAKE_BOUND + 0.05) for result in held)
+    relaxed = update(controller, matched)
+    assert relaxed.effective_accel_max > held[-1].effective_accel_max
+
+    for _ in range(CAP_FILTER_FRAMES):
+      tightened = update(controller, restrictive_radar())
+    assert tightened.effective_accel_max < relaxed.effective_accel_max
+
+  def test_strong_brake_relief_keeps_existing_confirmation(self):
+    controller = make_controller()
+    controller.live.state = AccelControllerState.restrict
+    controller.live.bound = SHALLOW_BRAKE_BOUND - 0.25
+    matched = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=10.0))
+
+    held = [update(controller, matched) for _ in range(RELIEF_CONFIRM_FRAMES - 1)]
+    assert all(result.effective_accel_max == pytest.approx(SHALLOW_BRAKE_BOUND - 0.25) for result in held)
+    relaxed = update(controller, matched)
+    assert relaxed.effective_accel_max > held[-1].effective_accel_max
+    continuing = [update(controller, matched) for _ in range(8)]
+    assert all(current.effective_accel_max > previous.effective_accel_max
+               for previous, current in zip([relaxed, *continuing[:-1]], continuing, strict=True))
+
+  def test_urgent_frame_uses_exact_stock_path(self):
+    urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
+    result = update(make_controller(), urgent, v_ego=20.0)
+    assert result.active and result.stock_mode
+    assert result.mpc_accel_max is None
+    assert math.isinf(result.effective_accel_max)
+
+  def test_urgent_relief_stays_stock_until_braking_has_recovered(self):
+    controller = make_controller()
+    urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
+    update(controller, urgent, v_ego=20.0)
+    result = update(controller, action_accel=-1.5, planner_accel=-1.2, v_ego=19.8)
+    assert result.stock_mode
+    assert result.mpc_accel_max is None
+    recovered = [update(controller, action_accel=0.0, planner_accel=0.0, v_ego=19.8) for _ in range(RELIEF_CONFIRM_FRAMES)]
+    assert all(sample.stock_mode for sample in recovered[:-1])
+    assert recovered[-1].state == AccelControllerState.free
+
+  def test_stop_hold_needs_four_departure_frames(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    held = update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+    assert held.state == AccelControllerState.stopHold
+    np.testing.assert_array_equal(held.mpc_accel_max, 0.0)
+
+    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
+    confirmation = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_EXIT_FRAMES)]
+    assert all(result.effective_accel_max == 0.0 for result in confirmation[:-1])
+    launched = confirmation[-1]
+    assert launched.launching and launched.state == AccelControllerState.free
+    assert launched.effective_accel_max == launched.positive_accel_max
+
+  def test_false_creep_speed_without_range_gain_stays_held(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+
+    false_creep = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.30))
+    held = [update(controller, false_creep, base_speed=8.0, v_ego=0.1) for _ in range(20)]
+    assert all(result.state == AccelControllerState.stopHold and not result.launching for result in held)
+
+  def test_invalid_lead_geometry_cannot_confirm_departure(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+
+    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=0.30))
+    held = [update(controller, invalid, base_speed=8.0, v_ego=0.1) for _ in range(2 * STOP_HOLD_EXIT_FRAMES)]
+    assert all(result.state == AccelControllerState.stopHold and not result.launching for result in held)
+
+  def test_short_range_drop_and_restore_cannot_fake_creep_departure(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+
+    low_range = make_radar(make_lead(status=True, d_rel=5.0, v_lead_k=0.0))
+    for _ in range(2):
+      update(controller, low_range, base_speed=8.0, v_ego=0.1)
+    restored = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.30))
+    results = [update(controller, restored, base_speed=8.0, v_ego=0.1) for _ in range(8)]
+
+    assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results)
+
+  def test_slow_creep_with_confirmed_range_gain_releases(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+
+    results = []
+    for frame in range(20):
+      creep = make_radar(make_lead(status=True, d_rel=6.0 + 0.05 * frame, v_lead_k=0.30))
+      results.append(update(controller, creep, base_speed=8.0, v_ego=0.1))
+
+    launched = [frame for frame, result in enumerate(results) if result.launching]
+    assert launched and launched[0] >= STOP_HOLD_EXIT_FRAMES
+    assert all(result.state == AccelControllerState.stopHold for result in results[:launched[0]])
+
+  def test_slow_creep_survives_lead_slot_switching(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0), make_lead(status=True, d_rel=6.1))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+
+    results = []
+    for frame in range(24):
+      distance = 6.0 + 0.04 * frame
+      offset = 0.05 if frame % 2 else 0.0
+      lead_one = make_lead(status=True, d_rel=distance + offset, v_lead_k=0.30)
+      lead_two = make_lead(status=True, d_rel=distance + 0.05 - offset, v_lead_k=0.30)
+      results.append(update(controller, make_radar(lead_one, lead_two), base_speed=8.0, v_ego=0.1))
+
+    assert any(result.launching for result in results)
+
+  def test_departure_that_stops_again_returns_to_hold(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
+    for _ in range(STOP_HOLD_EXIT_FRAMES):
+      launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
+    assert launched.launching
+
+    stalled = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.11))
+    settling = [update(controller, stalled, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_CREEP_ABORT_FRAMES)]
+    assert all(result.launching for result in settling[:-1])
+    assert settling[-1].state == AccelControllerState.stopHold and not settling[-1].launching
+
+  def test_invalid_geometry_after_departure_returns_to_hold(self):
+    controller = make_controller()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
+    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
+    for _ in range(STOP_HOLD_EXIT_FRAMES):
+      launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
+    assert launched.launching
+
+    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=0.30))
+    settling = [update(controller, invalid, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_CREEP_ABORT_FRAMES)]
+    assert settling[-1].state == AccelControllerState.stopHold and not settling[-1].launching
+
+  def test_stale_radar_freezes_then_discards_live_state(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      restricted = update(controller, restrictive_radar())
+    hold_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) - 1
+    frozen = [update(controller, radar_fresh=False) for _ in range(hold_frames)]
+    assert all(result.active and result.effective_accel_max == restricted.effective_accel_max for result in frozen)
+    timed_out = update(controller, radar_fresh=False)
+    assert not timed_out.active and timed_out.mpc_accel_max is None
+
+  def test_stale_radar_preserves_urgent_stock_passthrough_until_timeout(self):
+    controller = make_controller()
+    urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
+    result = update(controller, urgent, v_ego=20.0)
+    assert result.active and result.shadow_active and result.stock_mode
+
+    hold_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) - 1
+    frozen = [update(controller, radar_fresh=False, v_ego=20.0) for _ in range(hold_frames)]
+    assert all(sample.active and sample.shadow_active and sample.stock_mode for sample in frozen)
+    assert all(sample.state == AccelControllerState.hold and sample.mpc_accel_max is None for sample in frozen)
+
+    timed_out = update(controller, radar_fresh=False, v_ego=20.0)
+    assert not timed_out.active and not timed_out.shadow_active and not timed_out.stock_mode
+
+  @pytest.mark.parametrize("override", [
+    {"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"controller_fault": True},
+  ])
+  def test_bypass_never_actuates(self, override):
+    result = update(make_controller(), restrictive_radar(), **override)
+    assert not result.active
+    assert result.target_speed == result.base_speed
+    assert result.mpc_accel_max is None
+    assert math.isinf(result.effective_accel_max)
+
+  def test_shadow_history_never_enters_live_actuation(self):
+    controller = make_controller()
+    for _ in range(CAP_FILTER_FRAMES):
+      shadow = update(controller, restrictive_radar(), enabled=False)
+    assert shadow.shadow_state == AccelControllerState.restrict
+    live = update(controller)
+    assert live.state == AccelControllerState.free
+    assert live.effective_accel_max > 0.0
+
+
+@pytest.mark.parametrize("v_ego", [0.0, 0.2, 0.5, 1.0, 2.0, 10.0, 40.0])
+@pytest.mark.parametrize("bound", [-3.5, -2.0, -1.0, -0.1, 0.0, 0.8, 2.0])
+def test_accel_ceiling_properties(v_ego, bound):
+  result = AccelController._build_accel_ceiling(bound, v_ego, planner_accel=0.3, action_time=0.25)
+  if bound >= ACCEL_MAX:
+    assert result is None
+    return
+  ceiling = np.asarray(result)
+  assert ceiling.shape == T_IDXS.shape
+  assert np.all(np.isfinite(ceiling))
+  assert np.all((ceiling >= ACCEL_MIN) & (ceiling <= ACCEL_MAX))
+  assert ceiling[0] >= 0.3 - 1e-9
+  if bound > 0.0:
+    np.testing.assert_array_equal(ceiling, min(bound + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
+  if bound < 0.0:
+    assert np.trapezoid(-np.minimum(ceiling, 0.0), T_IDXS) <= HORIZON_SPEED_BUDGET * v_ego + 1e-9

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

@@ -0,0 +1,300 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import custom, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def radar_state():
+  return messaging.new_message('radarState').radarState
+
+
+class PlannerSM(dict):
+  def __init__(self, radar_log_mono_time, *, radar_updated=False):
+    super().__init__(
+      radarState=radar_state(), carState=SimpleNamespace(vEgo=10.0, aEgo=0.0),
+      selfdriveState=SimpleNamespace(personality=0),
+    )
+    self.updated = {'radarState': radar_updated}
+    self.valid = {'radarState': True}
+    self.alive = {'radarState': True}
+    self.logMonoTime = {'radarState': radar_log_mono_time}
+
+
+def test_legacy_profile_enum_keeps_toyota_importable():
+  expected = {"eco": 0, "normal": 1, "sport": 2}
+  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
+  from opendbc.car.toyota.carstate import AccelPersonality, CarState
+  assert AccelPersonality.schema.enumerants == expected
+  assert CarState.__module__ == "opendbc.car.toyota.carstate"
+
+
+def test_stock_mpc_parameters_are_unchanged():
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar_state(), 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+
+def test_positive_scalar_changes_only_acceleration_ceiling():
+  radar = radar_state()
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  stock_source = mpc.source
+  mpc.update(radar, 30.0, accel_max=0.8)
+  np.testing.assert_array_equal(mpc.params[:, 0], stock[:, 0])
+  np.testing.assert_array_equal(mpc.params[:, 1], 0.8)
+  np.testing.assert_array_equal(mpc.params[:, 2:], stock[:, 2:])
+  assert mpc.source == stock_source
+
+
+def test_negative_finite_horizon_ceiling_is_applied_exactly():
+  ceiling = np.linspace(0.2, -0.8, N + 1)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.2)
+  mpc.run = lambda: None
+  mpc.update(radar_state(), 30.0, accel_max=ceiling)
+  np.testing.assert_allclose(mpc.params[:, 1], ceiling)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+
+
+@pytest.mark.parametrize("accel_max", [np.inf, np.nan, -0.4, ACCEL_MIN, np.ones(N), "invalid"])
+def test_invalid_or_negative_scalar_limit_is_exact_stock(accel_max):
+  radar = radar_state()
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  mpc.update(radar, 30.0, accel_max=accel_max)
+  np.testing.assert_array_equal(mpc.params, stock)
+
+
+def test_custom_ceiling_keeps_raw_lead_obstacle_and_source_authoritative():
+  radar = radar_state()
+  radar.leadOne.status = True
+  radar.leadOne.dRel = 30.0
+  radar.leadOne.vLead = 5.0
+  radar.leadOne.aLeadK = 0.0
+  radar.leadOne.aLeadTau = 1.0
+  before = (radar.leadOne.dRel, radar.leadOne.vLead, radar.leadOne.aLeadK)
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(20.0, 0.0)
+  mpc.run = lambda: None
+  mpc.update(radar, 30.0)
+  stock = mpc.params.copy()
+  stock_source = mpc.source
+  mpc.update(radar, 30.0, accel_max=np.linspace(0.0, -0.5, N + 1))
+  np.testing.assert_array_equal(mpc.params[:, 0], stock[:, 0])
+  np.testing.assert_array_equal(mpc.params[:, 2:], stock[:, 2:])
+  assert mpc.source == stock_source
+  assert (radar.leadOne.dRel, radar.leadOne.vLead, radar.leadOne.aLeadK) == before
+
+
+def test_retry_seed_is_bounded_and_nonnegative_in_speed():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.mpc = LongitudinalMpc()
+  states = []
+  planner.mpc.solver = SimpleNamespace(set=lambda _stage, field, value: states.append(np.asarray(value)) if field == 'x' else None)
+  planner.mpc.set_cur_state(0.0, ACCEL_MIN)
+  planner._seed_mpc_current_state()
+  states = np.asarray(states)
+  assert len(states) == N + 1
+  assert np.all(np.diff(states[:, 0]) >= 0.0)
+  assert np.all(states[:, 1] >= 0.0)
+  assert np.all((states[:, 2] >= ACCEL_MIN) & (states[:, 2] <= ACCEL_MAX))
+
+
+def test_radar_freshness_is_cached_once_per_planner_cycle():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._radar_log_mono_time = None
+  planner._radar_fresh_this_cycle = True
+  planner._read_accel_controller_params = lambda: None
+  planner.events_sp = SimpleNamespace(clear=lambda: None)
+  dec_freshness = []
+  planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh: dec_freshness.append(radar_fresh))
+  planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
+  controller_freshness = []
+
+  def update_controller(*_args, **kwargs):
+    controller_freshness.append(kwargs['radar_fresh'])
+    return SimpleNamespace(target_speed=20.0)
+
+  planner.accel_controller = SimpleNamespace(update=update_controller)
+  planner.accel_personality = int(AccelProfile.normal)
+  planner.accel_personality_enabled = True
+
+  sm = PlannerSM(100, radar_updated=False)
+  planner.update(sm)
+  planner.update_accel_controller(sm, 20.0, True, True, True, 0.0, 0.0, ACCEL_MAX, False)
+  assert dec_freshness[-1] is True
+  assert controller_freshness[-1] is True
+
+  planner.update(sm)
+  sm.logMonoTime['radarState'] = 101
+  planner.update_accel_controller(sm, 20.0, True, True, True, 0.0, 0.0, ACCEL_MAX, False)
+  assert dec_freshness[-1] is False
+  assert controller_freshness[-1] is False
+
+  planner.update(sm)
+  planner.update_accel_controller(sm, 20.0, True, True, True, 0.0, 0.0, ACCEL_MAX, False)
+  assert dec_freshness[-1] is True
+  assert controller_freshness[-1] is True
+
+
+def test_radar_freshness_preserves_minimal_mock_compatibility():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  assert planner._update_radar_freshness(SimpleNamespace()) is True
+
+
+def test_last_solve_failure_survives_internal_reset():
+  mpc = LongitudinalMpc()
+  mpc.last_solution_status = 3
+  mpc.reset()
+  assert mpc.solution_status == 0
+  assert mpc.last_solution_status == 3
+
+
+@pytest.mark.parametrize(("controller_active", "expected_accel"), [(True, -1.2), (False, None)])
+def test_e2e_to_acc_handoff_preserves_braking_only_when_controller_is_active(controller_active, expected_accel):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._previous_is_e2e = True
+  planner.accel_personality_enabled = controller_active
+  planner.accel_controller_fault_latched = False
+  planner.is_e2e = lambda _sm: False
+  planner.accel_controller = SimpleNamespace(reset=lambda: None)
+  planner.mpc = SimpleNamespace(
+    a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
+    j_solution=np.zeros(N), last_solution_status=0,
+  )
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
+    planner, "accel_controller_result",
+    SimpleNamespace(enabled=controller_active, active=controller_active, shadow_active=controller_active,
+                    stock_mode=not controller_active, target_speed=20.0, mpc_accel_max=None,
+                    state=AccelControllerState.free, effective_accel_max=0.8 if controller_active else np.inf),
+  )
+  calls = []
+  planner._run_mpc = lambda *_args, **kwargs: calls.append(kwargs)
+
+  is_e2e = planner.update_accel_controller_mpc(
+    {}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=0.1,
+    previous_output_accel=-1.2, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
+  )
+
+  assert not is_e2e
+  assert calls[0]["current_accel"] == expected_accel
+  assert calls[0]["seed_target"] is None
+
+
+def test_failed_e2e_to_acc_handoff_retries_without_custom_state():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._previous_is_e2e = True
+  planner.accel_personality_enabled = True
+  planner.accel_controller_fault_latched = False
+  planner.is_e2e = lambda _sm: False
+  reset_calls = []
+  planner.accel_controller = SimpleNamespace(reset=lambda: reset_calls.append(True))
+  planner.mpc = SimpleNamespace(
+    a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
+    j_solution=np.zeros(N), last_solution_status=0,
+  )
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
+    planner, "accel_controller_result",
+    SimpleNamespace(enabled=True, active=True, shadow_active=True, stock_mode=True, target_speed=15.0, mpc_accel_max=None,
+                    state=AccelControllerState.hold, effective_accel_max=np.inf),
+  )
+  calls = []
+  positional_calls = []
+
+  def run_mpc(*args, **kwargs):
+    positional_calls.append(args)
+    calls.append(kwargs)
+    planner.mpc.last_solution_status = 1 if len(calls) == 1 else 0
+
+  planner._run_mpc = run_mpc
+  planner.update_accel_controller_mpc(
+    {}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=0.1,
+    previous_output_accel=-1.2, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
+  )
+
+  assert [call.get("current_accel") for call in calls] == [-1.2, None]
+  assert [args[1] for args in positional_calls] == [15.0, 20.0]
+  assert len(positional_calls[1]) == 3
+  assert calls[0]["seed_target"] is None
+  assert calls[1]["seed"]
+  assert "current_accel" not in calls[1]
+  assert "retry_state" in calls[1]
+  assert reset_calls == [True]
+  assert planner.accel_controller_fault_latched
+
+
+def test_e2e_to_acc_handoff_never_turns_braking_into_acceleration():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._previous_is_e2e = True
+  planner.accel_personality_enabled = True
+  planner.accel_controller_fault_latched = False
+  planner.is_e2e = lambda _sm: False
+  planner.accel_controller = SimpleNamespace(reset=lambda: None)
+  planner.mpc = SimpleNamespace(
+    a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
+    j_solution=np.zeros(N), last_solution_status=0,
+  )
+  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
+    planner, "accel_controller_result",
+    SimpleNamespace(enabled=True, active=True, shadow_active=True, stock_mode=True, target_speed=20.0, mpc_accel_max=None,
+                    state=AccelControllerState.free, effective_accel_max=np.inf),
+  )
+  calls = []
+  planner._run_mpc = lambda *_args, **kwargs: calls.append(kwargs)
+
+  planner.update_accel_controller_mpc(
+    {}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=-0.7,
+    previous_output_accel=0.3, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
+  )
+
+  assert calls[0]["current_accel"] == -0.7
+
+
+def test_shadow_telemetry_publishes_controller_fields():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.source = LongitudinalPlanSource.cruise
+  planner.output_v_target = 20.0
+  planner.output_a_target = 0.0
+  planner.events_sp = SimpleNamespace(to_msg=list)
+  planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
+  planner.accel_controller_result = SimpleNamespace(
+    enabled=True, active=False, shadow_active=True, profile=AccelProfile.normal, state=AccelControllerState.inactive,
+    shadow_state=AccelControllerState.restrict, base_speed=20.0, raw_energy_cap=15.0, live_filtered_cap=np.inf,
+    shadow_filtered_cap=12.5, selected_lead=1, usable_gap=30.0, closing_speed=5.0, required_decel=0.4,
+    profile_accel_max=np.inf, effective_accel_max=np.inf,
+  )
+  planner.scc = SimpleNamespace(
+    vision=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0, max_pred_lat_acc=0.0,
+                           is_enabled=False, is_active=False),
+    map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
+  )
+  planner.resolver = SimpleNamespace(speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
+                                     speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0,
+                                     distance=0.0, source=custom.LongitudinalPlanSP.SpeedLimit.Source.none)
+  planner.sla = SimpleNamespace(state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False, is_active=False,
+                                output_v_target=20.0, output_a_target=0.0)
+  planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
+  sent = {}
+  planner.publish_longitudinal_plan_sp(SimpleNamespace(all_checks=lambda service_list: True),
+                                       SimpleNamespace(send=lambda service, message: sent.update({service: message})))
+  telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
+  assert telemetry.vTargetShadow == pytest.approx(12.5)
+  assert telemetry.aMaxProfile == math.inf
+  assert telemetry.aMaxEffective == math.inf

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

@@ -1,17 +1,42 @@
+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_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL)))
+  RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL)))
+  RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+
+  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) if mode == 'blended' else 0
+      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,32 @@ 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._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_any_lead = False
+    self._has_current_radar_acc_lead = False
+    self._has_radar_acc_lead = False
+    self._radar_acc_lead_frames = 0
+    self._radar_fresh = True
+    self._radar_stale_frames = 0
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -179,13 +163,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,191 +183,151 @@ 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:
+  def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None:
     car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
+    radar_state = sm['radarState']
+    lead_one = radar_state.leadOne
+    lead_two = radar_state.leadTwo
     md = sm['modelV2']
 
     self._v_ego_kph = car_state.vEgo * 3.6
     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._radar_fresh = bool(radar_fresh)
+    if self._radar_fresh:
+      self._radar_stale_frames = 0
+      self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
+      self._has_any_lead = bool(lead_one.status or lead_two.status)
+      self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)))
+      self._update_radar_acc_lead()
+    else:
+      self._radar_stale_frames += 1
+      self._has_current_radar_acc_lead = False
+      if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES:
+        self._update_radar_acc_lead()
+      else:
+        self._lead_tracker.reset()
+        self._has_lead_filtered = False
+        self._has_any_lead = False
+        self._has_radar_acc_lead = False
+        self._radar_acc_lead_frames = 0
+    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
+    self._raw_urgency = clip01(urgency)
+    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
+    self._urgency = self._slow_down_tracker.value
+
+  def _radar_acc_lead_score(self, lead_one) -> float:
+    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
+    return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
+
+  def _update_radar_acc_lead(self) -> None:
+    if self._has_current_radar_acc_lead:
+      self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES
+      self._has_radar_acc_lead = True
       return
 
-    # We have a valid full trajectory
-    self._trajectory_valid = True
+    if not self._has_any_lead:
+      self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
 
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
+    self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
+    self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
 
-    # 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]:
+    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
+    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
+
+    if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
+      return 'acc', True
+
+    radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
+    if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
+      self._radar_acc_lead_frames = 0
+      self._has_radar_acc_lead = False
+      return 'blended', True
+
+    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
+      return 'acc', True
 
-    # EMERGENCY: MPC FCW - immediate blended mode
     if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      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.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      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)
-
-  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)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    self._read_params()
-
-    self.set_mpc_fcw_crash_cnt()
-
-    self._update_calculations(sm)
+      return 'blended', True
 
     if self._CP.radarUnavailable:
-      self._radarless_mode()
-    else:
-      self._radar_mode()
+      if standstill or self._has_slow_down:
+        return 'blended', urgent_slow_down
+      return 'acc', False
 
+    if standstill or self._has_slow_down:
+      return 'blended', urgent_slow_down
+
+    return 'acc', False
+
+  def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None:
+    self._read_params()
+    self.set_mpc_fcw_crash_cnt()
+    self._update_calculations(sm, radar_fresh)
+
+    mode, immediate = self._desired_mode()
+    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
+                                    cancel_hold=not self._CP.radarUnavailable and 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,479 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+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, radar=False, radarTrackId=-1):
+    self.status = status
+    self.dRel = dRel
+    self.vRel = vRel
+    self.radar = radar
+    self.radarTrackId = radarTrackId
+
+
+class MockRadarState:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
+    self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne()
+
+
+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, radar=True, radarTrackId=7),
+    '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, radar=True, radarTrackId=7)
+  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_always_uses_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, radar=True)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm):
+  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"
+
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True)
+  for _ in range(20):
+    controller.update(default_sm)
+  assert controller.mode() == "acc"
+
+
+def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+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, radarTrackId=7)
+  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, radar=True, radarTrackId=7)
+  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, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller.mode() == "acc"
+
+
+def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+
+
+def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert controller._has_radar_acc_lead
+  assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1
+  assert controller._radar_stale_frames == 1
+  assert controller.mode() == "acc"
+
+
+def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "acc"
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+
+def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  mock_mpc.crash_cnt = 1
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert controller.mode() == "blended"
+
+
+def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1):
+    controller.update(default_sm, radar_fresh=False)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert not controller._has_radar_acc_lead
+  assert not controller._has_any_lead
+  assert not controller._has_lead_filtered
+
+
+def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES):
+    controller.update(default_sm, radar_fresh=False)
+
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  controller.update(default_sm, radar_fresh=True)
+
+  assert controller._radar_stale_frames == 0
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"])
+def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  if urgent_source == "fcw":
+    mock_mpc.crash_cnt = 1
+  else:
+    default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  mock_mpc.crash_cnt = 0
+  default_sm['modelV2'] = MockModelData(valid=True)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2):
+    if frame % 2 == 0:
+      default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0))
+    else:
+      default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8))
+    controller.update(default_sm)
+
+    assert controller._has_current_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1):
+    controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two)
+  controller.update(default_sm)
+
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py

@@ -5,8 +5,19 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 
+import math
+
 from openpilot.common.params import Params
 
+# Planner-replan jitter shows up as small nonzero desired_lateral_jerk even on a straight
+# road with a near-zero lateral accel error. Fed into get_friction() that noise still
+# clears the (much smaller, sensor-deadzone-sized) lateral_accel_deadzone and comes out
+# as a real friction-compensation torque command, i.e. chatter with no steering need
+# behind it. Deadbanding the jerk itself (not the combined error) kills that noise floor
+# while leaving real jerk (an actual curve entry or lane change, an order of magnitude
+# larger) essentially unaffected.
+FRICTION_JERK_DEADZONE = 0.3  # m/s^3
+
 
 class LatControlTorqueExtOverride:
   def __init__(self, CP):
@@ -14,8 +25,15 @@ class LatControlTorqueExtOverride:
     self.params = Params()
     self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl")  # only during init
     self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+    self.friction_jerk_deadzone_enabled = self.params.get_bool("FrictionJerkDeadzoneEnabled")
     self.frame = -1
 
+  def get_friction_jerk(self, desired_lateral_jerk: float) -> float:
+    if not self.friction_jerk_deadzone_enabled:
+      return desired_lateral_jerk
+
+    return math.copysign(max(abs(desired_lateral_jerk) - FRICTION_JERK_DEADZONE, 0.0), desired_lateral_jerk)
+
   def update_override_torque_params(self, torque_params) -> bool:
     if not self.enforce_torque_control_toggle:
       return False

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,10 +5,19 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 
+import numpy as np
+
 from cereal import messaging, custom
 from opendbc.car import structs
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, T_IDXS
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import MPC_SEED_RISE_RATE
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -22,7 +31,8 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
+    self.params = Params()
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
@@ -32,10 +42,30 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self.accel_controller = AccelController(CP, dt=dt)
+    self.accel_controller_result = None
+    self.accel_controller_fault_latched = False
+    self._previous_is_e2e = False
+    self._radar_log_mono_time = None
+    self._radar_fresh_this_cycle = True
+
+    self._param_read_frames = max(1, int(round(0.25 / dt)))
+    self._param_frame = 0
+    self.accel_personality_enabled = False
+    self.accel_personality = int(AccelProfile.normal)
 
     self.output_v_target = 0.
     self.output_a_target = 0.
 
+  def _read_accel_controller_params(self) -> None:
+    if self._param_frame % self._param_read_frames == 0:
+      self.accel_personality_enabled = self.params.get_bool("AccelPersonalityEnabled")
+      self.accel_personality = get_sanitize_int_param(
+        "AccelPersonality", int(AccelProfile.eco), int(AccelProfile.sport), self.params,
+      )
+
+    self._param_frame += 1
+
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
     if not self.dec.active():
@@ -73,9 +103,118 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
+    try:
+      radar_log_mono_time = int(sm.logMonoTime['radarState'])
+      radar_healthy = bool(sm.valid['radarState'] and sm.alive['radarState'])
+    except (AttributeError, KeyError, TypeError, ValueError):
+      return True
+
+    previous_log_mono_time = getattr(self, '_radar_log_mono_time', None)
+    radar_advanced = previous_log_mono_time is None or radar_log_mono_time > previous_log_mono_time
+    if radar_advanced:
+      self._radar_log_mono_time = radar_log_mono_time
+    return radar_healthy and radar_advanced
+
+  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
+                              acc_selected: bool, planner_accel: float, action_accel: float, stock_accel_max: float,
+                              previous_should_stop: bool, controller_fault: bool = False) -> float:
+    self.accel_controller_result = self.accel_controller.update(
+      sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
+      profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality,
+      enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
+      planner_accel=planner_accel, action_accel=action_accel, stock_accel_max=stock_accel_max,
+      previous_should_stop=previous_should_stop, controller_fault=controller_fault,
+      radar_fresh=getattr(self, '_radar_fresh_this_cycle', True),
+    )
+    return self.accel_controller_result.target_speed
+
+  def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, seed=False,
+               seed_target=None, seed_rise_rate=MPC_SEED_RISE_RATE, retry_state=None, current_accel=None) -> None:
+    if retry_state is not None:
+      self.mpc.a_prev = retry_state[0].copy()
+      self.mpc.crash_cnt = retry_state[1]
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    mpc_accel = self.a_desired if current_accel is None else float(np.clip(current_accel, ACCEL_MIN, ACCEL_MAX))
+    self.mpc.set_cur_state(self.v_desired_filter.x, mpc_accel)
+    if seed or seed_target is not None:
+      self._seed_mpc_current_state(seed_target, seed_rise_rate)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=accel_max)
+
+  def _seed_mpc_current_state(self, accel_target=None, rise_rate=MPC_SEED_RISE_RATE) -> None:
+    target = float(np.clip(self.mpc.x0[2] if accel_target is None else accel_target, ACCEL_MIN, ACCEL_MAX))
+    desired_accel = target * np.ones(N + 1) if accel_target is None else np.minimum(self.mpc.x0[2] + rise_rate * T_IDXS, target)
+    acceleration = np.zeros(N + 1)
+    velocity = np.zeros(N + 1)
+    position = np.zeros(N + 1)
+    jerk = np.zeros(N)
+    acceleration[0] = self.mpc.x0[2]
+    velocity[0] = max(self.mpc.x0[1], 0.0)
+    position[0] = self.mpc.x0[0]
+    for idx in range(1, N + 1):
+      dt = T_IDXS[idx] - T_IDXS[idx - 1]
+      min_accel = 0.0 if velocity[idx - 1] <= 1e-3 and acceleration[idx - 1] < 0.0 else -2.0 * velocity[idx - 1] / dt - acceleration[idx - 1]
+      acceleration[idx] = np.clip(max(desired_accel[idx], min_accel), ACCEL_MIN, ACCEL_MAX)
+      jerk[idx - 1] = (acceleration[idx] - acceleration[idx - 1]) / dt
+      position[idx] = max(position[idx - 1], position[idx - 1] + velocity[idx - 1] * dt + 0.5 * acceleration[idx - 1] * dt**2
+                          + jerk[idx - 1] * dt**3 / 6.0)
+      velocity[idx] = max(0.0, velocity[idx - 1] + 0.5 * (acceleration[idx - 1] + acceleration[idx]) * dt)
+    for idx in range(N + 1):
+      self.mpc.solver.set(idx, 'x', np.array([position[idx], velocity[idx], acceleration[idx]]))
+    for idx in range(N):
+      self.mpc.solver.set(idx, 'u', np.array([jerk[idx]]))
+
+  def update_accel_controller_mpc(self, sm: messaging.SubMaster, base_v_cruise: float, mpc_v_cruise: float,
+                                  prev_accel_constraint: bool, *, reset_state: bool, cruise_initialized: bool,
+                                  planner_accel: float, previous_output_accel: float, available_accel_max: float,
+                                  previous_should_stop: bool, force_decel: bool):
+    is_e2e = self.is_e2e(sm)
+    was_e2e = self._previous_is_e2e
+    if reset_state or not self.accel_personality_enabled:
+      self.accel_controller_fault_latched = False
+
+    self.update_accel_controller(
+      sm, base_v_cruise, engaged=not reset_state and not force_decel, cruise_initialized=cruise_initialized,
+      acc_selected=not is_e2e, planner_accel=planner_accel, action_accel=previous_output_accel,
+      stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop,
+      controller_fault=self.accel_controller_fault_latched,
+    )
+    result = self.accel_controller_result
+    handoff_context = result.enabled and result.shadow_active and not force_decel and not self.accel_controller_fault_latched
+    transition_from_e2e = handoff_context and was_e2e and not is_e2e and result.active
+    handoff_accel = (min(planner_accel, previous_output_accel)
+                     if transition_from_e2e and result.active and np.isfinite(previous_output_accel) else None)
+    self._previous_is_e2e = is_e2e and handoff_context
+    controller_actuating = result.active and not result.stock_mode and not force_decel
+    accel_max = result.mpc_accel_max if controller_actuating else None
+    free_profile_limit = controller_actuating and result.state == AccelControllerState.free and result.effective_accel_max > 0.0
+    seed_target = result.effective_accel_max if free_profile_limit and handoff_accel is None else None
+    custom_mpc = handoff_accel is not None or (controller_actuating and (accel_max is not None or seed_target is not None))
+    retry_state = (self.mpc.a_prev.copy(), self.mpc.crash_cnt)
+    controller_v_cruise = min(mpc_v_cruise, result.target_speed)
+    self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, seed_target=seed_target, current_accel=handoff_accel)
+
+    finite_solution = all(np.all(np.isfinite(solution)) for solution in (self.mpc.v_solution, self.mpc.a_solution, self.mpc.j_solution))
+    custom_failed = custom_mpc and (self.mpc.last_solution_status != 0 or not finite_solution)
+    if custom_failed:
+      self.accel_controller_fault_latched = True
+      self.accel_controller.reset()
+      self._run_mpc(sm, mpc_v_cruise, prev_accel_constraint, seed=True, retry_state=retry_state)
+      self.update_accel_controller(
+        sm, base_v_cruise, engaged=not reset_state and not force_decel, cruise_initialized=cruise_initialized,
+        acc_selected=not is_e2e, planner_accel=planner_accel, action_accel=previous_output_accel,
+        stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop, controller_fault=True,
+      )
+    if custom_failed and self.mpc.last_solution_status != 0:
+      self.mpc.a_prev, self.mpc.crash_cnt = retry_state
+
+    return is_e2e
+
   def update(self, sm: messaging.SubMaster) -> None:
+    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
+    self._read_accel_controller_params()
     self.events_sp.clear()
-    self.dec.update(sm)
+    self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -95,6 +234,25 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    if self.accel_controller_result is not None:
+      result = self.accel_controller_result
+      accel_controller = longitudinalPlanSP.accelController
+      accel_controller.enabled = result.enabled
+      accel_controller.active = result.active
+      accel_controller.shadowOnly = result.shadow_active and not result.active
+      accel_controller.profile = int(result.profile)
+      accel_controller.state = int(result.state if result.active else result.shadow_state)
+      accel_controller.vTargetBase = float(result.base_speed)
+      accel_controller.vTargetRaw = float(result.raw_energy_cap)
+      accel_controller.vTargetFiltered = float(result.live_filtered_cap)
+      accel_controller.vTargetShadow = float(result.shadow_filtered_cap)
+      accel_controller.leadIndex = result.selected_lead
+      accel_controller.usableGap = float(result.usable_gap)
+      accel_controller.closingSpeed = float(result.closing_speed)
+      accel_controller.requiredDecel = float(result.required_decel)
+      accel_controller.aMaxProfile = float(result.profile_accel_max)
+      accel_controller.aMaxEffective = float(result.effective_accel_max)
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

sunnypilot/selfdrive/controls/lib/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/smart_cruise_control/tests/test_vision_controller.py

@@ -4,6 +4,8 @@ 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 types import SimpleNamespace
+
 import numpy as np
 import pytest
 
@@ -13,8 +15,12 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import (
+  _A_LAT_REG_MAX, _BELOW_EGO_TARGET_RELEASE_RATE, _ENTERING_PRED_LAT_ACC_TH, _MIN_ACTIVATION_SPEED,
+  _RELIEF_CONFIRMATION_FRAMES, _TARGET_RELEASE_RATE, SmartCruiseControlVision,
+)
 
 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
 
@@ -118,6 +124,21 @@ class TestSmartCruiseControlVision:
   def reset_params(self):
     self.params.put_bool("SmartCruiseControlVision", True, block=True)
 
+  def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20., model_speed: float = 20.) -> None:
+    self.sm['controlsState'].curvature = current / v_ego**2
+    self.sm['modelV2'].velocity.x = [model_speed] * len(ModelConstants.T_IDXS)
+    self.sm['modelV2'].orientationRate.z = [predicted / model_speed] * len(ModelConstants.T_IDXS)
+
+  def update_lat_accels(self, current: float, predicted: float, cruise: float = 30., a_ego: float = 0.,
+                        v_ego: float = 20., model_speed: float = 20.) -> None:
+    self.set_lat_accels(current, predicted, v_ego, model_speed)
+    self.scc_v.update(self.sm, True, False, v_ego, a_ego, cruise)
+
+  def enter_curve(self, predicted: float = 2.2) -> None:
+    self.update_lat_accels(0.5, predicted)
+    self.update_lat_accels(0.5, predicted)
+    assert self.scc_v.state == VisionState.entering
+
   def test_initial_state(self):
     assert self.scc_v.state == VisionState.disabled
     assert not self.scc_v.is_active
@@ -143,6 +164,253 @@ class TestSmartCruiseControlVision:
       self.scc_v.update(self.sm, True, False, 0., 0., 0.)
     assert self.scc_v.state == VisionState.enabled
 
+  def test_unconfirmed_leaving_and_reentry_only_shape_speed(self):
+    self.enter_curve()
+    targets = [self.scc_v.output_v_target]
+
+    self.update_lat_accels(2., 2.2, a_ego=-0.8)
+    assert self.scc_v.state == VisionState.turning
+    assert self.scc_v.output_a_target == -0.8
+    targets.append(self.scc_v.output_v_target)
+
+    self.update_lat_accels(1.2, 1.2, a_ego=0.3)
+    assert self.scc_v.state == VisionState.leaving
+    assert self.scc_v.output_a_target == 0.3
+    targets.append(self.scc_v.output_v_target)
+
+    self.update_lat_accels(1., 3., a_ego=-1.2)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_a_target == -1.2
+    targets.append(self.scc_v.output_v_target)
+
+    entering, turning, leaving, reentering = targets
+    assert turning == pytest.approx(entering)
+    assert 0. < leaving - turning <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    assert reentering < leaving
+
+  def test_new_curve_interrupts_confirmed_release_immediately(self):
+    self.enter_curve()
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1):
+      self.update_lat_accels(0.8, 0.8)
+    releasing_v_target = self.scc_v.output_v_target
+    assert self.scc_v.state == VisionState.leaving
+
+    self.update_lat_accels(0.8, 3., a_ego=-0.7)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_v_target < releasing_v_target
+    assert self.scc_v.output_a_target == -0.7
+
+  @pytest.mark.parametrize("planner_accel", (-2., -0.5, 0., 0.8))
+  def test_planner_acceleration_passes_through_exactly(self, planner_accel):
+    self.enter_curve()
+    self.update_lat_accels(0.5, 2.2, a_ego=planner_accel)
+    assert self.scc_v.output_a_target == planner_accel
+
+  def test_planner_acceleration_passes_through_all_states(self):
+    cases = (
+      (False, False, 0.5, 2.2, -0.2, VisionState.disabled),
+      (True, False, 0.5, 0.8, 0.1, VisionState.enabled),
+      (True, False, 0.5, 2.2, -0.4, VisionState.entering),
+      (True, False, 2., 2.2, -0.8, VisionState.turning),
+      (True, False, 1.2, 1.2, 0.3, VisionState.leaving),
+      (True, True, 1.2, 1.2, 0.6, VisionState.overriding),
+    )
+    for long_enabled, override, current, predicted, planner_accel, state in cases:
+      self.set_lat_accels(current, predicted)
+      self.scc_v.update(self.sm, long_enabled, override, 20., planner_accel, 30.)
+      assert self.scc_v.state == state
+      assert self.scc_v.output_a_target == planner_accel
+
+  def test_jitter_requires_confirmed_relief_then_releases_smoothly(self):
+    self.enter_curve()
+    previous_v_target = self.scc_v.output_v_target
+
+    for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2):
+      self.update_lat_accels(1., 1.05 if frame % 2 == 0 else 1.15)
+      assert self.scc_v.state == VisionState.entering
+      assert self.scc_v.output_v_target >= previous_v_target
+      assert self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES):
+      self.update_lat_accels(1.15, 0.8)
+      assert self.scc_v.state == VisionState.entering
+      assert 0. <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    release_cruise = 30.
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      assert self.scc_v.state == VisionState.entering
+      assert 0. <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    active_v_targets = [previous_v_target]
+    for _ in range(int((release_cruise - previous_v_target) / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      if not self.scc_v.is_active:
+        break
+      assert self.scc_v.state == VisionState.leaving
+      assert self.scc_v.output_v_target != V_CRUISE_UNSET
+      active_v_targets.append(self.scc_v.output_v_target)
+
+    assert self.scc_v.state == VisionState.enabled
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    assert active_v_targets[-1] == pytest.approx(release_cruise)
+    assert np.all((np.diff(active_v_targets) >= 0.) &
+                  (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
+
+  def test_target_release_slows_after_reaching_ego_speed(self):
+    self.enter_curve()
+
+    for _ in range(100):
+      previous_v_target = self.scc_v.output_v_target
+      self.update_lat_accels(0.8, 0.8)
+      if previous_v_target >= self.scc_v.v_ego:
+        rise = self.scc_v.output_v_target - previous_v_target
+        assert 0. < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
+        break
+    else:
+      pytest.fail("curve target did not release to ego speed")
+
+  def test_curve_target_is_independent_of_ego_speed(self):
+    model_speed = 24.
+    predicted_yaw_rate = 0.12
+    predicted_lat_accel = model_speed * predicted_yaw_rate
+    expected_v_target = (_A_LAT_REG_MAX / (predicted_yaw_rate / model_speed)) ** 0.5
+    targets = []
+
+    for v_ego in (18., 28.):
+      controller = SmartCruiseControlVision()
+      self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed)
+      controller.update(self.sm, True, False, v_ego, 0., 30.)
+      controller.update(self.sm, True, False, v_ego, 0., 30.)
+      assert controller.state == VisionState.entering
+      targets.append(controller.v_target)
+
+    assert targets[0] == pytest.approx(expected_v_target)
+    assert targets[1] == pytest.approx(expected_v_target)
+
+  def test_curve_target_respects_minimum_speed_floor(self):
+    model_speed = 10.
+    predicted_yaw_rate = 2.
+    self.set_lat_accels(0.5, model_speed * predicted_yaw_rate, model_speed=model_speed)
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.v_target < MIN_V
+    assert self.scc_v.output_v_target == pytest.approx(MIN_V)
+
+  @pytest.mark.parametrize(
+    ("velocities", "yaw_rates"),
+    [([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.] * 5, [0.1] * 3)],
+    ids=("empty", "nonfinite", "mismatched"),
+  )
+  def test_model_vector_edges_remain_finite(self, velocities, yaw_rates):
+    self.sm['modelV2'].velocity.x = velocities
+    self.sm['modelV2'].orientationRate.z = yaw_rates
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+
+    assert all(np.isfinite(value) for value in (
+      self.scc_v.current_lat_acc, self.scc_v.max_pred_lat_acc, self.scc_v.v_target,
+      self.scc_v.output_v_target, self.scc_v.output_a_target,
+    ))
+
+  @pytest.mark.parametrize("launch_speed", (5.75, 9.9, _MIN_ACTIVATION_SPEED))
+  def test_vision_control_does_not_steal_launch(self, launch_speed):
+    self.set_lat_accels(0.5, 3., launch_speed)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
+
+    assert launch_speed <= _MIN_ACTIVATION_SPEED
+    assert self.scc_v.state == VisionState.enabled
+    assert not self.scc_v.is_active
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+
+  def test_vision_control_can_activate_above_launch_range(self):
+    speed = _MIN_ACTIVATION_SPEED + 0.01
+    self.set_lat_accels(0.5, 3., speed)
+    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
+    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.is_active
+
+  def test_sequential_curve_tightens_immediately_and_releases_bounded(self):
+    self.enter_curve(3.)
+    for _ in range(20):
+      self.update_lat_accels(0.5, 3.)
+    restrictive_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 1.4, a_ego=0.4)
+    first_relief_v_target = self.scc_v.output_v_target
+    assert self.scc_v.state == VisionState.entering
+    assert 0. < first_relief_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    assert self.scc_v.output_a_target == 0.4
+
+    self.update_lat_accels(0.5, 1.4)
+    assert 0. <= self.scc_v.output_v_target - first_relief_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+
+    self.update_lat_accels(0.5, 3., a_ego=-0.6)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_v_target == pytest.approx(restrictive_v_target)
+    assert self.scc_v.output_a_target == -0.6
+
+    for _ in range(4):
+      self.update_lat_accels(0.5, 1.4)
+      assert 0. < self.scc_v.output_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      self.update_lat_accels(0.5, 3.)
+      assert self.scc_v.output_v_target == pytest.approx(restrictive_v_target)
+
+  def test_acceleration_is_continuous_through_planner_arbitration(self):
+    car_control = messaging.new_message('carControl')
+    car_control.carControl.enabled = True
+    car_control.carControl.cruiseControl.override = False
+    self.sm['carControl'] = car_control.carControl
+    self.sm['carState'].vCruiseCluster = 108.
+
+    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    planner.scc = SimpleNamespace(
+      vision=self.scc_v,
+      map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.),
+      update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(
+        sm, enabled, override, v_ego, a_ego, v_cruise),
+    )
+    planner.resolver = SimpleNamespace(
+      speed_limit_valid=False, speed_limit_last_valid=False, speed_limit=0., speed_limit_final_last=0., distance=0.,
+      update=lambda _v_ego, _sm: None,
+    )
+    planner.sla = SimpleNamespace(
+      output_v_target=V_CRUISE_UNSET, output_a_target=0., update=lambda *_args: None,
+    )
+    planner.events_sp = SimpleNamespace()
+
+    self.set_lat_accels(0.5, 2.2)
+    planner.update_targets(self.sm, 20., -0.8, 30.)
+    planner.update_targets(self.sm, 20., -0.8, 30.)
+    assert planner.source == LongitudinalPlanSource.sccVision
+    assert planner.output_a_target == -0.8
+
+    for planner_accel in (-2., 0.5, -0.2):
+      planner.update_targets(self.sm, 20., planner_accel, 30.)
+      assert planner.source == LongitudinalPlanSource.sccVision
+      assert planner.output_a_target == planner_accel
+
+    self.set_lat_accels(0.8, 0.8)
+    for _ in range(int(30. / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
+      planner.update_targets(self.sm, 20., 0.4, 30.)
+      assert planner.output_a_target == 0.4
+      if planner.source == LongitudinalPlanSource.cruise:
+        break
+    else:
+      pytest.fail("SCC Vision did not release to cruise")
+
+    planner.update_targets(self.sm, 20., 0.4, 30.)
+    assert self.scc_v.state == VisionState.enabled
+    assert planner.source == LongitudinalPlanSource.cruise
+
   @pytest.mark.parametrize(
     "case, should_enter",
     [

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

@@ -0,0 +1,82 @@
+"""
+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 gc
+
+import numpy as np
+
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX
+
+
+def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.) -> dict[str, np.ndarray]:
+  gc.collect()
+  curvature = 0.005
+  plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20)
+  planner = plant.planner
+  planner.accel_personality_enabled = False
+  planner._read_accel_controller_params = lambda: None
+  planner.dec._enabled = False
+  planner.dec._read_params = lambda: None
+  planner.scc.map.enabled = False
+  planner.scc.map.update_params = lambda: None
+  planner.scc.vision.enabled = scc_enabled
+  planner.scc.vision._update_params = lambda: None
+
+  if scc_enabled:
+    original_update_calculations = planner.scc.vision._update_calculations
+
+    def inject_constant_curvature(sm):
+      velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float)
+      sm['modelV2'].orientationRate.z = (curvature * velocities).tolist()
+      sm['controlsState'].curvature = curvature
+      original_update_calculations(sm)
+
+    planner.scc.vision._update_calculations = inject_constant_curvature
+
+  original_update = planner.update
+
+  def enable_longitudinal(sm):
+    sm['carControl'].enabled = True
+    sm['carControl'].longActive = True
+    original_update(sm)
+
+  planner.update = enable_longitudinal
+  rows = []
+  while plant.current_time < duration:
+    output = plant.step(v_cruise=cruise)
+    rows.append((
+      plant.current_time, output['speed'], planner.mpc.last_solution_status, output['should_stop'],
+      planner.scc.vision.is_active, planner.source == LongitudinalPlanSource.sccVision,
+      planner.scc.vision.output_v_target,
+    ))
+
+  data = np.asarray(rows, dtype=float)
+  gc.collect()
+  return {
+    'time': data[:, 0], 'speed': data[:, 1], 'solver_status': data[:, 2], 'should_stop': data[:, 3],
+    'active': data[:, 4], 'scc_source': data[:, 5], 'target': data[:, 6],
+  }
+
+
+def test_constant_curve_recovers_like_stock_speed_cap():
+  target = (_A_LAT_REG_MAX / 0.005) ** 0.5
+  scc = _run_constant_curve(scc_enabled=True, cruise=30.)
+  stock = _run_constant_curve(scc_enabled=False, cruise=target)
+  scc_final = scc['speed'][scc['time'] >= 60.]
+  stock_final = stock['speed'][stock['time'] >= 60.]
+
+  assert not scc['solver_status'].any()
+  assert not stock['solver_status'].any()
+  assert not scc['should_stop'].any()
+  assert np.all(scc['active'][scc['time'] >= 60.])
+  assert np.all(scc['scc_source'][scc['time'] >= 60.])
+  assert np.allclose(scc['target'][scc['time'] >= 60.], target)
+  assert scc_final.min() >= target - 1.
+  assert abs(scc_final.mean() - stock_final.mean()) < 0.5
+  assert abs(scc_final.min() - stock_final.min()) < 1.
+  assert abs(scc_final.max() - stock_final.max()) < 1.

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -29,19 +29,11 @@ _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cyc
 
 _A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
 
-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
+_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+_TARGET_RELEASE_RATE = 1.  # m/s^2
+_BELOW_EGO_TARGET_RELEASE_RATE = 3.  # m/s^2
+_MIN_PRED_SPEED = 1.  # m/s
+_MIN_ACTIVATION_SPEED = 10.  # m/s
 
 
 class SmartCruiseControlVision:
@@ -65,13 +57,26 @@ class SmartCruiseControlVision:
     self.state = VisionState.disabled
     self.current_lat_acc = 0.
     self.max_pred_lat_acc = 0.
+    self.relief_frames = 0
+
+  def _v_demand(self) -> float:
+    return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
+
+  def _released_v_target(self) -> float:
+    demand = self._v_demand()
+    if demand < self.output_v_target:
+      return demand
+    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if self.output_v_target < min(self.v_ego, demand) else _TARGET_RELEASE_RATE
+    return min(demand, self.output_v_target + release_rate * DT_MDL)
 
   def get_a_target_from_control(self) -> float:
-    return self.a_target
+    return self.a_ego
 
   def get_v_target_from_control(self) -> float:
     if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+      if self.output_v_target == V_CRUISE_UNSET:
+        return self._v_demand()
+      return self._released_v_target()
 
     return V_CRUISE_UNSET
 
@@ -82,25 +87,27 @@ class SmartCruiseControlVision:
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
     if not self.long_enabled:
       return
-    else:
-      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
-      vel_plan = np.array(sm['modelV2'].velocity.x)
 
-      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+    rate_plan = np.asarray(np.abs(sm['modelV2'].orientationRate.z), dtype=float)
+    vel_plan = np.asarray(sm['modelV2'].velocity.x, dtype=float)
+    size = min(len(rate_plan), len(vel_plan))
+    rate_plan, vel_plan = rate_plan[:size], vel_plan[:size]
+    valid = np.isfinite(rate_plan) & np.isfinite(vel_plan) & (vel_plan >= _MIN_PRED_SPEED)
 
-      # get the maximum lat accel from the model
-      predicted_lat_accels = rate_plan * vel_plan
-      self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
-
-      # get the maximum curve based on the current velocity
-      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
-      max_curve = self.max_pred_lat_acc / (v_ego**2)
-
-      # Get the target velocity for the maximum curve
-      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+    self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+    self.max_pred_lat_acc = 0.
+    self.v_target = V_CRUISE_UNSET
+    if np.any(valid):
+      self.max_pred_lat_acc = float(np.percentile(rate_plan[valid] * vel_plan[valid], 97))
+      max_pred_curvature = float(np.percentile(rate_plan[valid] / vel_plan[valid], 97))
+      if max_pred_curvature > 0.:
+        self.v_target = min(float((_A_LAT_REG_MAX / max_pred_curvature) ** 0.5), V_CRUISE_UNSET)
 
   def _update_state_machine(self) -> tuple[bool, bool]:
     # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
+    relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH
+    self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0
+
     if self.state != VisionState.disabled:
       # longitudinal and feature disable always have priority in a non-disabled state
       if not self.long_enabled or not self.enabled:
@@ -112,7 +119,7 @@ class SmartCruiseControlVision:
         # ENABLED
         if self.state == VisionState.enabled:
           # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= MIN_V:
+          if self.v_ego <= _MIN_ACTIVATION_SPEED:
             pass
           # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
           elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
@@ -128,23 +135,26 @@ class SmartCruiseControlVision:
           # Transition to Turning if current lateral acceleration is over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Abort if the predicted lateral acceleration drops
-          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.enabled
+          # Begin releasing only after both current and predicted lateral acceleration stay clear.
+          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
+            self.state = VisionState.leaving
 
         # TURNING
         elif self.state == VisionState.turning:
-          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          # Transition out of Turning if current lateral acceleration drops below a threshold.
           if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.leaving
+            self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving
 
         # LEAVING
         elif self.state == VisionState.leaving:
           # Transition back to Turning if current lateral acceleration goes back over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Finish if current lateral acceleration goes below a threshold.
-          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+          # Start a new turn cycle immediately if another curve is predicted.
+          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.entering
+          # Finish after confirmed relief and a gradual release to the cruise setpoint.
+          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint:
             self.state = VisionState.enabled
 
     # DISABLED
@@ -157,32 +167,11 @@ class SmartCruiseControlVision:
 
     enabled = self.state in ENABLED_STATES
     active = self.state in ACTIVE_STATES
+    if not active:
+      self.relief_frames = 0
 
     return enabled, active
 
-  def _update_solution(self) -> float:
-    # DISABLED, ENABLED, OVERRIDING
-    if self.state not in ACTIVE_STATES:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-      # the smooth deceleration.
-      a_target = self.a_ego
-    # ENTERING
-    elif self.state == VisionState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-    # TURNING
-    elif self.state == VisionState.turning:
-      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
-      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionState.leaving:
-      # When leaving, we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-    else:
-      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
-
-    return a_target
-
   def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
              v_cruise_setpoint: float) -> None:
     self.long_enabled = long_enabled
@@ -195,7 +184,7 @@ class SmartCruiseControlVision:
     self._update_calculations(sm)
 
     self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self._update_solution()
+    self.a_target = self.a_ego
 
     self.output_v_target = self.get_v_target_from_control()
     self.output_a_target = self.get_a_target_from_control()

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_latcontrol_torque_ext_override.py

@@ -0,0 +1,42 @@
+"""
+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.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import (
+  FRICTION_JERK_DEADZONE, LatControlTorqueExtOverride,
+)
+
+
+class TestLatControlTorqueExtOverrideFrictionJerkDeadzone:
+
+  def setup_method(self):
+    self.override = LatControlTorqueExtOverride(None)
+
+  def test_disabled_by_default(self):
+    assert self.override.friction_jerk_deadzone_enabled is False
+
+  def test_disabled_is_passthrough(self):
+    self.override.friction_jerk_deadzone_enabled = False
+
+    for jerk in (0.0, 0.05, -0.05, FRICTION_JERK_DEADZONE, -FRICTION_JERK_DEADZONE, 2.0, -2.0):
+      assert self.override.get_friction_jerk(jerk) == jerk
+
+  def test_enabled_kills_jerk_below_deadzone(self):
+    self.override.friction_jerk_deadzone_enabled = True
+
+    for jerk in (0.0, 0.05, -0.05, 0.29, -0.29):
+      assert self.override.get_friction_jerk(jerk) == 0.0
+
+  def test_enabled_boundary_at_deadzone(self):
+    self.override.friction_jerk_deadzone_enabled = True
+
+    assert self.override.get_friction_jerk(FRICTION_JERK_DEADZONE) == 0.0
+    assert self.override.get_friction_jerk(-FRICTION_JERK_DEADZONE) == 0.0
+
+  def test_enabled_shifts_jerk_above_deadzone_preserving_sign(self):
+    self.override.friction_jerk_deadzone_enabled = True
+
+    assert self.override.get_friction_jerk(2.0) == 2.0 - FRICTION_JERK_DEADZONE
+    assert self.override.get_friction_jerk(-2.0) == -(2.0 - FRICTION_JERK_DEADZONE)

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": ""
@@ -310,6 +332,10 @@
     "title": "Force Power Down",
     "description": ""
   },
+  "FrictionJerkDeadzoneEnabled": {
+    "title": "Reduce Friction Torque Chatter",
+    "description": "Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing steering chatter on straight roads. Does not change torque during real turns or lane changes."
+  },
   "GitBranch": {
     "title": "Git Branch",
     "description": ""
@@ -1118,6 +1144,10 @@
     "title": "Record Front Lock",
     "description": ""
   },
+  "RoadEdgeLaneChangeEnabled": {
+    "title": "Block Lane Change: Road Edge Detection",
+    "description": ""
+  },
   "RoadName": {
     "title": "Road Name",
     "description": ""
@@ -1324,14 +1354,30 @@
     "step": 0.1,
     "unit": "m/s\u00b2"
   },
+  "ToyotaAutoHold": {
+    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+    "description": ""
+  },
+  "ToyotaDriveMode": {
+    "title": "Enable drive mode btn link",
+    "description": ""
+  },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",
     "description": "When enabled, sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used."
   },
+  "ToyotaEnhancedBsm": {
+    "title": "Toyota: Prius TSS2 BSM and some tssp",
+    "description": ""
+  },
   "ToyotaStopAndGoHack": {
     "title": "Toyota: Stop and Go Hack (Alpha)",
     "description": "sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk."
   },
+  "ToyotaTSS2Long": {
+    "title": "Toyota: custom longitudinal for TSS2",
+    "description": ""
+  },
   "TrainingVersion": {
     "title": "Training Version",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -310,6 +310,37 @@
                   "equals": false
                 }
               ]
+            },
+            {
+              "key": "FrictionJerkDeadzoneEnabled",
+              "widget": "toggle",
+              "title": "Reduce Friction Torque Chatter",
+              "description": "Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing steering chatter on straight roads. Does not change torque during real turns or lane changes.",
+              "visibility": [
+                {
+                  "type": "not",
+                  "condition": {
+                    "type": "capability",
+                    "field": "steer_control_type",
+                    "equals": "angle"
+                  }
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "offroad_only"
+                },
+                {
+                  "type": "capability",
+                  "field": "torque_allowed",
+                  "equals": true
+                },
+                {
+                  "type": "param",
+                  "key": "NeuralNetworkLateralControl",
+                  "equals": false
+                }
+              ]
             }
           ],
           "sub_panels": [
@@ -620,6 +651,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 +2084,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 +2267,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/steering.yaml

@@ -118,6 +118,25 @@ sections:
     - type: param
       key: NeuralNetworkLateralControl
       equals: false
+  - key: FrictionJerkDeadzoneEnabled
+    widget: toggle
+    title: Reduce Friction Torque Chatter
+    description: Ignores small, short-lived steering jerk from planner noise when computing friction compensation, reducing
+      steering chatter on straight roads. Does not change torque during real turns or lane changes.
+    visibility:
+    - type: not
+      condition:
+        type: capability
+        field: steer_control_type
+        equals: angle
+    enablement:
+    - $ref: '#/macros/offroad'
+    - type: capability
+      field: torque_allowed
+      equals: true
+    - type: param
+      key: NeuralNetworkLateralControl
+      equals: false
   sub_panels:
   - id: torque_settings
     label: Torque Settings

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)