fix(long): frame based staleness for jumpguard and lead-smoother

_LeadHold counted its own step() calls to gauge staleness, but
smooth_radarstate() stops calling step() below LOW_SPEED_PASSTHROUGH_V —
so the counter froze indefinitely during any low-speed period (measured
60-80s freezes on real routes) and read as "just a few frames old" on
resume no matter how much real time passed, letting a long-dead hold
resurrect as a phantom lead. Compare against the caller's live frame
counter instead.

cereal/custom.capnp

@@ -194,7 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
-  accelController @8 :AccelController;
+  acceleration @8 :Acceleration;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -298,41 +298,20 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     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;
-    }
+  # Acceleration Personality (Eco / Normal / Sport)
+  struct Acceleration {
+    personality @0 :AccelerationPersonality;
+    enabled @1 :Bool;
+    maxAccel @2 :Float32;     # current speed-indexed accel ceiling
+    brakeNeed @3 :Float32;    # repurposed: follow-gap widen added on top of the stock t_follow (s)
+    decelTarget @4 :Float32;  # repurposed: t_follow handed to the MPC (s)
+    smoothActive @5 :Bool;    # repurposed: follow-gap widen currently active
+    bypassed @6 :Bool;        # unused (input-shaping design has no output post-shaping / bypass)
+    comfortStopActive @7 :Bool;   # low-speed comfort decel-to-stop floor currently governing (behind a near-stopped lead)
+    comfortStopFloor @8 :Float32; # comfort-stop floor commanded (m/s^2, negative; 0 when not engaged)
+    leadUnstable @9 :Bool;        # RadarDistance lead-instability telemetry (bimodal/bouncing radar lead; informational, no control effect yet)
   }
 
-  # Compatibility type for vehicle integrations that map physical drive modes
-  # onto AccelPersonality. New controller telemetry uses AccelController.Profile.
   enum AccelerationPersonality {
     eco @0;
     normal @1;

common/params_keys.h

@@ -4,6 +4,7 @@
 #include <unordered_map>
 
 #include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/custom.capnp.h"
 
 inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}},
@@ -235,9 +236,13 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // Accel Controller relative-pace governor (Eco / Normal / Sport)
+    // Acceleration Personality (Eco / Normal / Sport)
     {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
+
+    // Radar Distance: de-noise the lead the MPC follows (flicker-hold + churn smoother); never reports a
+    // farther/faster lead than reality, so braking stays >= stock
+    {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},

common/tests/test_params.py

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

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -217,9 +217,9 @@ 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
+    self.t_follow_fn = None  # sunnypilot: optional (t_follow, v_ego)->t_follow override; None == byte-stock
 
   def reset(self):
     self.solver.reset()
@@ -268,8 +268,8 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
-    jerk_factor = get_jerk_factor(personality)
+  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_scale=1.0):
+    jerk_factor = get_jerk_factor(personality) * jerk_scale
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
     cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
     constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
@@ -314,10 +314,11 @@ 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,
-             accel_max: float | tuple[float, ...] | np.ndarray | None = None, shape_accel_max_in_cruise: bool = False):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
+    if self.t_follow_fn is not None:
+      t_follow = self.t_follow_fn(t_follow, v_ego)
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
@@ -329,25 +330,11 @@ class LongitudinalMpc:
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
-    custom_accel_max = False
-    accel_max_traj = ACCEL_MAX * np.ones(N + 1)
-    if accel_max is not None:
-      accel_max_input = np.asarray(accel_max, dtype=float)
-      if accel_max_input.ndim == 0:
-        accel_max_input = np.full(N + 1, float(accel_max_input))
-      custom_accel_max = accel_max_input.shape == (N + 1,) and np.all(np.isfinite(accel_max_input))
-      if custom_accel_max:
-        accel_max_traj = np.clip(accel_max_input, 0.0, ACCEL_MAX)
-
     # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
     # when the leads are no factor.
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    if custom_accel_max and shape_accel_max_in_cruise:
-      cruise_accel_max_traj = np.minimum(accel_max_traj, CRUISE_MAX_ACCEL)
-      v_upper = v_ego + (np.cumsum(T_DIFFS * cruise_accel_max_traj) * 1.05)
-    else:
-      v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
+    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 
@@ -360,11 +347,7 @@ class LongitudinalMpc:
     self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
 
     self.params[:,0] = ACCEL_MIN
-    if custom_accel_max:
-      self.params[:,1] = accel_max_traj
-      self.params[0,1] = max(accel_max_traj[0], self.x0[2])
-    else:
-      self.params[:,1] = ACCEL_MAX
+    self.params[:,1] = ACCEL_MAX
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -384,7 +367,6 @@ 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,8 @@ 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, dt=dt)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    self.mpc.t_follow_fn = self.accel.get_t_follow  # Acceleration Personality: add-only follow-gap widen
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -110,7 +111,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+    accel_clip = [ACCEL_MIN, self.accel.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)
 
@@ -118,6 +119,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active
       self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1])
+      self.accel.reset()  # drop any accumulated follow-gap widen so it re-ramps cleanly on re-engage
+      self._e2e_transition_guard.reset()  # drop the tracked baseline so it doesn't apply a stale limit on re-engage
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -133,24 +136,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
 
-    # DEC is the sole ACC/e2e authority. Cache its decision once for both the governor and output arbitration.
-    is_e2e = self.is_e2e(sm)
-    v_cruise = LongitudinalPlannerSP.update_accel_controller(
-      self, sm, v_cruise, engaged=not reset_state, cruise_initialized=v_cruise_initialized, acc_selected=not is_e2e,
-      planner_speed=self.v_desired_filter.x, previous_mpc_source=self.mpc.source, previous_should_stop=self.output_should_stop,
-      stock_accel_max=accel_clip[1], planner_accel=self.a_desired, controller_fault=self.mpc.last_solution_status != 0,
-    )
-
     if force_slow_decel:
       v_cruise = 0.0
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_scale=self.accel.get_jerk_scale(v_ego))
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(
-      sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
-      accel_max=self.accel_controller_result.mpc_accel_max,
-      shape_accel_max_in_cruise=self.accel_controller_result.mpc_shape_cruise,
-    )
+    self.mpc.update(self.smooth_radarstate(sm['radarState']), v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -172,6 +163,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
+    is_e2e = 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
@@ -181,8 +173,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
 
-    for idx in range(2):
-      accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
+    output_a_target = self.smooth_e2e_transition(output_a_target)
+
+    # Acceleration Personality shapes only MPC INPUTS (accel ceiling above + t_follow via mpc.t_follow_fn),
+    # never the output accel -- output_a_target passes through byte-stock so the MPC owns the trajectory.
+
+    # Lower (braking) bound and the ceiling's downward slew stay at the stock rate; only the ceiling's
+    # upward slew is tier-dependent (Acceleration Personality).
+    accel_clip[0] = np.clip(accel_clip[0], self.prev_accel_clip[0] - 0.05, self.prev_accel_clip[0] + 0.05)
+    accel_clip[1] = np.clip(accel_clip[1], self.prev_accel_clip[1] - 0.05, self.prev_accel_clip[1] + self.accel.get_rise_rate(v_ego))
     self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
     self.prev_accel_clip = accel_clip
 

selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py

@@ -0,0 +1,57 @@
+import inspect
+import re
+from pathlib import Path
+
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+
+
+REPO_ROOT = Path(__file__).resolve().parents[3]
+
+
+def test_smoothing_params_default_off():
+  params_keys = (REPO_ROOT / "common/params_keys.h").read_text()
+
+  assert re.search(r'"AccelPersonalityEnabled", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
+  assert re.search(r'"RadarDistance", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
+  # the retired dRel-bias sub-toggles must not return (their features were deleted in the input-shaping rewrite)
+  assert '"StopGapBias"' not in params_keys
+  assert '"LeadDecelAnticipate"' not in params_keys
+
+
+def test_output_is_byte_stock_and_inputs_are_shaped():
+  update_src = inspect.getsource(LongitudinalPlanner.update)
+
+  # INPUT shaping only: the accel ceiling and the radar-conditioning seam are present...
+  assert "self.accel.get_max_accel(v_ego)" in update_src
+  assert "self.mpc.update(self.smooth_radarstate(sm['radarState'])" in update_src
+  # ...and the OUTPUT is never post-shaped (byte-stock output; no accel shaping, no should_stop override).
+  assert "smooth_target_accel" not in update_src
+  assert "sng_should_stop" not in update_src   # reverted: the should_stop hysteresis caused a high-speed under-brake
+
+
+def test_t_follow_hook_wired_and_identity_default():
+  init_src = inspect.getsource(LongitudinalPlanner.__init__)
+  assert "self.mpc.t_follow_fn = self.accel.get_t_follow" in init_src   # planner wires the add-only widen
+
+  mpc_init = inspect.getsource(LongitudinalMpc.__init__)
+  assert "self.t_follow_fn = None" in mpc_init                          # default None == byte-stock identity
+
+  mpc_update = inspect.getsource(LongitudinalMpc.update)
+  assert "if self.t_follow_fn is not None:" in mpc_update               # guarded hook, only fires when set
+
+
+# Tokens for the reverted input-side DEC model-stop-target (capped v_target into the MPC pre-solve). It was
+# superseded by DEC blended-mode and chased a source-fixed radar gate; it must not silently return.
+_DEC_MODEL_STOP_TOKENS = ("apply_model_stop_target", "force_stop_requested", "_update_model_stop", "MODEL_STOP_TARGET_TIME")
+
+
+def test_dec_model_stop_target_not_reintroduced():
+  this_file = Path(__file__).resolve()
+  for sub in ("selfdrive/controls", "sunnypilot/selfdrive/controls"):
+    for path in (REPO_ROOT / sub).rglob("*.py"):
+      if path.resolve() == this_file:
+        continue                                      # this guard names the tokens as strings
+      src = path.read_text()
+      for token in _DEC_MODEL_STOP_TOKENS:
+        assert token not in src, f"reverted DEC model-stop-target ({token}) re-introduced in {path}"

selfdrive/modeld/modeld.py

@@ -28,6 +28,7 @@ from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path,
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -223,6 +224,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(DH)
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -325,7 +327,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, modelv2_send.modelV2.roadEdges)
+      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

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,10 +1,5 @@
 #!/usr/bin/env python3
-from collections import deque
-from collections.abc import Callable
-import math
 import time
-from typing import Any
-
 import numpy as np
 
 from cereal import log
@@ -16,52 +11,22 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
-LeadObservation = dict[str, Any]
-LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
-ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
-
-
 class Plant:
   messaging_initialized = False
 
-  def __init__(
-    self,
-    lead_relevancy=False,
-    speed=0.0,
-    distance_lead=2.0,
-    enabled=True,
-    only_lead2=False,
-    only_radar=False,
-    e2e=False,
-    personality=0,
-    force_decel=False,
-    lead_observation_fn: LeadObservationFn | None = None,
-    model_action_fn: ModelActionFn | None = None,
-    actuator_delay: float | None = None,
-    actuator_lag: float = 0.0,
-  ):
-    """Closed-loop longitudinal planner plant.
-
-    ``lead_observation_fn(time, lead_name, truth)`` may return a complete or partial
-    observed LeadData mapping, or ``None`` for an absent lead. It is called separately
-    for ``leadOne`` and ``leadTwo``. The supplied truth mapping is a copy, and observed
-    values never affect the physical lead trajectory.
-
-    ``model_action_fn(time, v_ego, a_ego)`` returns
-    ``(desired_acceleration, should_stop)``.
-
-    Passing ``actuator_delay`` both overrides ``CP.longitudinalActuatorDelay`` and
-    adds the corresponding command transport delay to the plant. ``None`` keeps the
-    historical Honda planner delay with instantaneous plant response. ``actuator_lag``
-    is an optional first-order acceleration-response time constant. Both defaults keep
-    historical plant dynamics unchanged.
-    """
-    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
-      raise ValueError("actuator_delay must be finite and non-negative")
-    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
-      raise ValueError("actuator_lag must be finite and non-negative")
-
-    self.rate = 1.0 / DT_MDL
+  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_dRel_glitch_fn=None, e2e_accel_fn=None):
+    # lead_dRel_glitch_fn(current_time, d_rel, v_rel) -> (d_rel, v_rel): optional per-step override of the
+    # REPORTED radar lead dRel/vRel, independent of the true physics used to advance distance_lead/speed --
+    # models a sensor/fusion glitch (what radard actually reports can diverge from the real world) without
+    # corrupting the closed-loop ego dynamics the rest of the planner reacts to. None -> no override (default).
+    # e2e_accel_fn(current_time, ego_speed, ego_accel) -> float: optional override of the e2e model's
+    # action.desiredAcceleration (default is a mild self.acceleration+0.1 echo, which can't independently
+    # disagree with the MPC enough to exercise is_e2e()-gated behavior). None -> default echo (unchanged).
+    self.lead_dRel_glitch_fn = lead_dRel_glitch_fn
+    self.e2e_accel_fn = e2e_accel_fn
+    self.rate = 1. / DT_MDL
 
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
@@ -73,12 +38,10 @@ class Plant:
 
     self.v_lead_prev = 0.0
 
-    self.distance = 0.0
+    self.distance = 0.
     self.speed = speed
     self.should_stop = False
     self.acceleration = 0.0
-    self.a_target = 0.0
-    self.actuator_command = 0.0
 
     # lead car
     self.lead_relevancy = lead_relevancy
@@ -89,14 +52,9 @@ class Plant:
     self.e2e = e2e
     self.personality = personality
     self.force_decel = force_decel
-    self.lead_observation_fn = lead_observation_fn
-    self.model_action_fn = model_action_fn
-    self.actuator_delay = actuator_delay
-    self.actuator_lag = actuator_lag
-    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, actuator_delay is not None, actuator_lag > 0.0))
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1.0 / self.rate
+    self.ts = 1. / self.rate
     time.sleep(0.1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
@@ -104,54 +62,14 @@ class Plant:
     from opendbc.car.honda.interface import CarInterface
 
     CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
-    if self.actuator_delay is not None:
-      CP.longitudinalActuatorDelay = self.actuator_delay
     CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
     self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
 
-    delay_steps = 0 if self.actuator_delay is None else round(self.actuator_delay / self.ts)
-    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
-
   @property
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  @staticmethod
-  def _lead_message(observation: LeadObservation):
-    lead = log.RadarState.LeadData.new_message()
-    for field, value in observation.items():
-      setattr(lead, field, value)
-    return lead
-
-  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
-    if self.lead_observation_fn is None:
-      return dict(truth) if present_by_default else None
-
-    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
-    if observed is None:
-      return None
-
-    # Partial overrides are convenient for individual sensor glitches, while copying
-    # from truth ensures every field written to cereal is deterministic.
-    complete_observation = dict(truth)
-    complete_observation.update(observed)
-    return complete_observation
-
-  def _update_actuator(self, command: float) -> tuple[float, float]:
-    if self._actuator_delay_queue:
-      self._actuator_delay_queue.append(command)
-      delayed_command = self._actuator_delay_queue.popleft()
-    else:
-      delayed_command = command
-
-    if self.actuator_lag > 0.0:
-      alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
-      self.acceleration += alpha * (delayed_command - self.acceleration)
-    else:
-      self.acceleration = delayed_command
-    return delayed_command, self.acceleration
-
-  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., 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')
@@ -164,48 +82,42 @@ 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.0, self.distance_lead - self.distance)
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob_lead > 0.5:
+      elif prob_lead > .5:
         status = True
       else:
         status = False
     else:
-      d_rel = 200.0
-      v_rel = 0.0
+      d_rel = 200.
+      v_rel = 0.
       prob_lead = 0.0
       status = False
 
-    truth_lead: LeadObservation = {
-      "dRel": float(d_rel),
-      "yRel": 0.0,
-      "vRel": float(v_rel),
-      "aRel": float(a_lead - self.acceleration),
-      "vLead": float(v_lead),
-      "dPath": 0.0,
-      "vLat": 0.0,
-      "vLeadK": float(v_lead),
-      "aLeadK": float(a_lead),
-      "fcw": False,
-      "status": bool(status),
-      # TODO use real radard logic for this
-      "aLeadTau": float(_LEAD_ACCEL_TAU),
-      "modelProb": float(prob_lead),
-      "radar": bool(self.only_radar),
-      "radarTrackId": -1,
-    }
-    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
-    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
-    if lead_one_observation is not None:
-      radar.radarState.leadOne = self._lead_message(lead_one_observation)
-    if lead_two_observation is not None:
-      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
+    if self.lead_dRel_glitch_fn is not None and self.lead_relevancy:
+      d_rel, v_rel = self.lead_dRel_glitch_fn(self.current_time, d_rel, v_rel)
+
+    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
 
     # Simulate model predicting slightly faster speed
     # this is to ensure lead policy is effective when model
@@ -213,15 +125,13 @@ 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
-    if self.model_action_fn is None:
-      model_acceleration, model_should_stop = self.acceleration + 0.1, False
+    if self.e2e_accel_fn is not None:
+      model.modelV2.action.desiredAcceleration = float(self.e2e_accel_fn(self.current_time, self.speed, self.acceleration))
     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)
+      model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
     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)]
@@ -233,38 +143,32 @@ class Plant:
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
-    published_a_ego = self.acceleration if self.publish_realized_a_ego else 0.0
-    car_state.carState.aEgo = float(published_a_ego)
     car_state.carState.standstill = bool(self.speed < 0.01)
     car_state.carState.vCruise = float(v_cruise * 3.6)
-    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
+    car_control.carControl.orientationNED = [0., float(pitch), 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.a_target = self.planner.output_a_target
-    self.actuator_command = self.a_target
+    self.acceleration = self.planner.output_a_target
     if self.planner.output_should_stop:
-      self.actuator_command = min(-0.5, self.actuator_command)
-    delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
+      self.acceleration = min(-0.5, self.acceleration)
     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
@@ -272,51 +176,30 @@ class Plant:
 
     # *** radar model ***
     if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
     else:
-      d_rel = 200.0
-      v_rel = 0.0
+      d_rel = 200.
+      v_rel = 0.
 
     # print at 5hz
     # if (self.rk.frame % (self.rate // 5)) == 0:
     #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
     #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
 
+
     # ******** update prevs ********
     self.rk.monitor_time()
 
-    accel_controller_result = getattr(self.planner, "accel_controller_result", None)
     return {
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
-      "realized_acceleration": self.acceleration,
-      "a_target": self.a_target,
-      "actuator_command": self.actuator_command,
-      "delayed_actuator_command": delayed_actuator_command,
-      "published_a_ego": published_a_ego,
       "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
-      "mpc_source": self.planner.mpc.source,
-      "dec_mode": self.planner.dec.mode(),
-      "pace_cap": getattr(accel_controller_result, "target_speed", None),
-      "base_target": getattr(accel_controller_result, "base_speed", None),
-      "raw_energy_cap": getattr(accel_controller_result, "raw_energy_cap", None),
-      "live_filtered_cap": getattr(accel_controller_result, "live_filtered_cap", None),
-      "shadow_filtered_cap": getattr(accel_controller_result, "shadow_filtered_cap", None),
-      "accel_controller_selected_lead": getattr(accel_controller_result, "selected_lead", None),
-      "model_action": {
-        "desiredAcceleration": float(model_acceleration),
-        "shouldStop": bool(model_should_stop),
-      },
-      "truth_lead": dict(truth_lead),
-      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
-      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
     }
 
-
 # simple engage in standalone mode
 def plant_thread():
   plant = Plant()

selfdrive/test/longitudinal_maneuvers/test_plant.py

@@ -1,80 +0,0 @@
-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,16 +27,19 @@ 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)."
   ),
+  "AccelPersonalityEnabled": tr_noop("Enable Eco/Normal/Sport acceleration profiles, including early soft braking."),
+  "AccelPersonality": tr_noop(
+    "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. " +
+    "Hard-braking authority is always preserved."
+  ),
+  "RadarDistance": tr_noop(
+    "Hold a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, " +
+    "smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock."
+  ),
   "AlwaysOnDM": tr_noop("Enable driver monitoring even when sunnypilot is not engaged."),
   'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
   "IsMetric": tr_noop("Display speed in km/h instead of mph."),
@@ -70,6 +73,12 @@ class TogglesLayout(Widget):
         "disengage_on_accelerator.png",
         False,
       ),
+      "RadarDistance": (
+        lambda: tr("Radar Distance"),
+        DESCRIPTIONS["RadarDistance"],
+        "speed_limit.png",
+        False,
+      ),
       "IsLdwEnabled": (
         lambda: tr("Enable Lane Departure Warnings"),
         DESCRIPTIONS["IsLdwEnabled"],
@@ -113,7 +122,7 @@ class TogglesLayout(Widget):
     )
 
     self._accel_personality_enabled = toggle_item(
-      lambda: tr("Enable Accel Controller"),
+      lambda: tr("Enable Acceleration Profiles"),
       lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
       self._params.get_bool("AccelPersonalityEnabled"),
       callback=self._set_accel_personality_enabled,
@@ -159,7 +168,7 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality and Accel Controller settings after NDOG toggle
+      # insert longitudinal + acceleration personality after NDOG toggle
       if param == "DisengageOnAccelerator":
         self._toggles["LongitudinalPersonality"] = self._long_personality_setting
         self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
@@ -184,7 +193,6 @@ 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. " +
@@ -204,7 +212,7 @@ class TogglesLayout(Widget):
         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)
+        self._accel_personality_setting.action_item.set_enabled(True)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
@@ -234,10 +242,6 @@ 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:
@@ -288,4 +292,3 @@ class TogglesLayout(Widget):
 
   def _set_accel_personality_enabled(self, state: bool):
     self._params.put_bool("AccelPersonalityEnabled", state, block=True)
-    self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)

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

@@ -14,8 +14,6 @@ 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")
@@ -26,8 +24,6 @@ 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,
@@ -40,7 +36,6 @@ 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),
@@ -50,9 +45,6 @@ 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)
 
@@ -83,18 +75,13 @@ 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,18 +382,13 @@ class BigMultiParamToggle(BigMultiToggle):
     self._load_value()
 
   def _load_value(self):
-    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))])
+    self.set_value(self._options[self._params.get(self._param) or 0])
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     super()._handle_mouse_release(mouse_pos)
     new_idx = self._options.index(self.value)
     self._params.put(self._param, new_idx)
 
-  def refresh(self):
-    self._load_value()
-
 
 class BigParamControl(BigToggle):
   def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):

sunnypilot/modeld_v2/modeld.py

@@ -435,7 +435,7 @@ 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
-      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego, modelv2_send.modelV2.roadEdges)
       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)

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

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

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

@@ -1,585 +1,215 @@
-#!/usr/bin/env python3
-from collections import deque
-from dataclasses import dataclass, field
-from enum import IntEnum
-import math
+"""
+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.
+
+Acceleration Personality (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never the output:
+  * positive-accel ceiling + its per-cycle open-rate -> tier-scaled take-off from a stop;
+  * jerk-cost relaxation -> smooth accel/decel onset near a stop, on a fresh direction change, a
+    hard-braking lead, or a fast-closing gap;
+  * add-only, speed-dependent follow-gap widen on the MPC t_follow.
+Disabled => stock everywhere (byte-stock).
+"""
 
 import numpy as np
 
-from cereal import log
-from opendbc.car.interfaces import ACCEL_MAX
+from cereal import messaging
+from opendbc.car import structs
+from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  LongitudinalMpc,
-  LongitudinalPlanSource,
-  STOP_DISTANCE,
-  T_IDXS,
-  get_T_FOLLOW,
-)
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
+  NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, STOCK_A_CRUISE_MAX_V, \
+  RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, RISE_RATE_LEAD_VREL_GATE, JERK_SCALE_BP, JERK_SCALE_V, \
+  ONSET_DEADBAND, ONSET_RAMP_S, ONSET_FLOOR, RELAX_RAMP_S, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, \
+  CLOSING_VREL_BP, CLOSING_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, \
+  TF_SLEW_PER_S, TF_DECEL_HOLD_A
 
 
-class AccelProfile(IntEnum):
-  eco = 0
-  normal = 1
-  sport = 2
-
-
-class AccelControllerState(IntEnum):
-  inactive = 0
-  free = 1
-  restrict = 2
-  hold = 3
-  release = 4
-  stopHold = 5
-
-
-@dataclass(frozen=True)
-class ProfileConfig:
-  comfort_decel: float
-  release_rate: float
-  release_confirm: float
-
-
-PROFILE_CONFIGS = {
-  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=0.65, release_confirm=0.50),
-  AccelProfile.normal: ProfileConfig(comfort_decel=0.335, release_rate=0.85, release_confirm=0.35),
-  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_rate=1.10, release_confirm=0.20),
-}
-
-ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0]
-ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [0.95, 0.70, 0.42, 0.28],
-  AccelProfile.normal: [1.30, 1.00, 0.65, 0.45],
-  AccelProfile.sport: [1.55, 1.15, 0.78, 0.58],
-}
-LAUNCH_DELTA_V = 3.0
-
-CAP_FILTER_FRAMES = 5
-RESTRICT_DEADBAND = 0.15
-RELIEF_DEADBAND = 0.35
-STOP_HOLD_EGO_SPEED = 0.30
-STOP_HOLD_CAP = 0.50
-STOPPED_LEAD_SPEED = 0.30
-STOP_HOLD_EXIT_CAP = 0.80
-STOP_HOLD_EXIT_FRAMES = 4
-CLEAR_ROAD_PROFILE_SPEED = 0.20
-ACCEL_LIMIT_JERK = 1.0
-LAUNCH_ACCEL_JERK = 3.0
-LAUNCH_PACE_RATE = 5.0
-MPC_LAUNCH_BOUND_NODES = 2
-MPC_STOP_WARM_BLEND = 0.0
-MPC_CONFIRM_WARM_BLEND = 0.10
-MPC_DEPART_WARM_BLEND = 0.25
-
-
-@dataclass(frozen=True)
-class EnergyEnvelope:
-  cap: float = math.inf
-  selected_lead: int = -1
-  usable_gap: float = math.inf
-  closing_speed: float = 0.0
-  required_decel: float = 0.0
-  has_nearly_stopped_lead: bool = False
-
-
-@dataclass(frozen=True)
-class AccelControllerResult:
-  target_speed: float
-  enabled: bool
-  active: bool
-  shadow_active: bool
-  launching: bool
-  profile: AccelProfile
-  profile_accel_max: float
-  effective_accel_max: float
-  mpc_accel_max: tuple[float, ...] | None
-  mpc_shape_cruise: bool
-  state: AccelControllerState
-  shadow_state: AccelControllerState
-  base_speed: float
-  raw_energy_cap: float
-  live_filtered_cap: float
-  shadow_filtered_cap: float
-  live_pace: float
-  shadow_pace: float
-  selected_lead: int
-  usable_gap: float
-  closing_speed: float
-  required_decel: float
-
-
-@dataclass
-class _PacePath:
-  cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
-  pace: float | None = None
-  state: AccelControllerState = AccelControllerState.inactive
-  relief_time: float = 0.0
-  departure_frames: int = 0
-  departing_from_stop: bool = False
-  stopped_lead_hold: bool = False
-  accel_limit: float | None = None
+class _OnsetRelax:
+  # Snaps to a tier-scaled floor on a fresh accel<->decel direction change, eases back to 1.0 over ONSET_RAMP_S.
+  def __init__(self):
+    self._prev_sign = 0
+    self._ramp = 1.0
 
   def reset(self) -> None:
-    self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
-    self.pace = None
-    self.state = AccelControllerState.inactive
-    self.relief_time = 0.0
-    self.departure_frames = 0
-    self.departing_from_stop = False
-    self.stopped_lead_hold = False
-    self.accel_limit = None
+    self._prev_sign = 0
+    self._ramp = 1.0
 
-  def update_filter(self, cap: float) -> float:
-    self.cap_samples.append(cap)
-    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+  def update(self, a_ego: float, floor: float) -> float:
+    sign = 0
+    if a_ego > ONSET_DEADBAND:
+      sign = 1
+    elif a_ego < -ONSET_DEADBAND:
+      sign = -1
 
-  @property
-  def filtered_cap(self) -> float:
-    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+    if sign != 0 and sign != self._prev_sign:
+      self._ramp = floor
+      self._prev_sign = sign
+    else:
+      self._ramp = min(1.0, self._ramp + (1.0 - floor) * (DT_MDL / ONSET_RAMP_S))
+    return self._ramp
+
+
+class _TransientRelax:
+  # Same shape as _OnsetRelax, but triggered by a level instead of a sign flip: snap to a fresh/escalating
+  # factor, then ease back to 1.0 over ramp_s regardless of whether the raw factor is still low. A pinned
+  # floor destabilizes the MPC's re-solve over a sustained episode, so only the onset gets softened.
+  def __init__(self):
+    self._was_active = False
+    self._episode_floor = 1.0
+    self._ramp = 1.0
+
+  def reset(self) -> None:
+    self._was_active = False
+    self._episode_floor = 1.0
+    self._ramp = 1.0
+
+  def update(self, raw_factor: float, ramp_s: float) -> float:
+    active = raw_factor < 1.0 - 1e-3
+    if active and (not self._was_active or raw_factor < self._episode_floor):
+      self._episode_floor = raw_factor
+      self._ramp = raw_factor
+    else:
+      self._ramp = min(1.0, self._ramp + DT_MDL / ramp_s)
+      if not active:
+        self._episode_floor = 1.0
+    self._was_active = active
+    return self._ramp
 
 
 class AccelController:
-  """A relative-pace governor with a positive-acceleration comfort ceiling."""
+  def __init__(self, CP: structs.CarParams, mpc=None, params=None):
+    # CP/mpc accepted for the planner's constructor signature; unused (shapes MPC inputs only).
+    self._params = params or Params()
+    self._frame = 0
+    self._enabled = False
+    self._personality = NORMAL
+    self._v_ego = 0.0
+    self._a_ego = 0.0
+    self._widen = 0.0                     # current slewed follow-gap widen (s), add-only
+    self._t_follow = 0.0                  # last t_follow handed to the MPC (telemetry)
+    self._onset_relax = _OnsetRelax()
+    self._lead_brake_relax = _TransientRelax()
+    self._closing_relax = _TransientRelax()
+    self._onset_factor = 1.0
+    self._lead_brake_factor = 1.0
+    self._closing_factor = 1.0
+    self._closing_on_lead = False
+    self._read_params()
 
-  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")
+  def _read_params(self) -> None:
+    self._enabled = self._params.get_bool("AccelPersonalityEnabled")
+    if not self._enabled:
+      self._personality = NORMAL
+      return
+    self._personality = get_sanitize_int_param("AccelPersonality", PERSONALITY_MIN, PERSONALITY_MAX, self._params)
 
-    self.CP = CP
-    self.dt = dt
-    self.live = _PacePath()
-    self.shadow = _PacePath()
+  def update(self, sm: messaging.SubMaster) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._read_params()
+    self._v_ego = float(sm['carState'].vEgo)
+    self._a_ego = float(sm['carState'].aEgo)
 
-  @staticmethod
-  def _profile(profile: int | AccelProfile) -> AccelProfile:
-    try:
-      return AccelProfile(profile)
-    except (TypeError, ValueError):
-      return AccelProfile.normal
+    if self._enabled:
+      lead = sm['radarState'].leadOne
+      self._onset_factor = self._onset_relax.update(self._a_ego, ONSET_FLOOR[self._personality])
+      self._lead_brake_factor = self._lead_brake_relax.update(self._get_lead_brake_factor(lead), RELAX_RAMP_S)
+      self._closing_factor = self._closing_relax.update(self._get_closing_factor(lead), RELAX_RAMP_S)
+      self._closing_on_lead = lead.status and lead.vRel < RISE_RATE_LEAD_VREL_GATE
+    else:
+      self._onset_relax.reset()
+      self._lead_brake_relax.reset()
+      self._closing_relax.reset()
+      self._onset_factor = 1.0
+      self._lead_brake_factor = 1.0
+      self._closing_factor = 1.0
+      self._closing_on_lead = False
 
-  @classmethod
-  def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float:
-    """Return the profile's positive-acceleration ceiling at the current speed."""
-    if not math.isfinite(v_ego):
-      return math.nan
+    self._frame += 1
 
-    profile = cls._profile(profile)
-    return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile]))
+  def _get_lead_brake_factor(self, lead) -> float:
+    # Raw, uneased tier-scaled factor for THIS cycle -- fed through _TransientRelax in update(), never used
+    # directly as the published factor (see the class docstring for why: a level-pinned floor destabilizes
+    # the MPC's re-solve over a sustained episode).
+    if not lead.status:
+      return 1.0
+    return float(np.interp(lead.aLeadK, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[self._personality]))
 
-  def _delay(self) -> float:
-    try:
-      return float(self.CP.longitudinalActuatorDelay) + DT_MDL
-    except (AttributeError, TypeError, ValueError):
-      return math.nan
-
-  @staticmethod
-  def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
-    if a_ego < 0.0:
-      stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
-      if stop_time <= delay:
-        return -v_ego * v_ego / (2.0 * a_ego) if v_ego > 0.0 else 0.0, 0.0
-
-    return max(v_ego * delay + 0.5 * a_ego * delay * delay, 0.0), max(v_ego + a_ego * delay, 0.0)
-
-  @staticmethod
-  def _valid_lead(lead) -> bool:
-    return bool(lead.status) and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau))
-
-  def calculate_energy_envelope(
-    self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality=log.LongitudinalPersonality.standard
-  ) -> EnergyEnvelope:
-    """Calculate the unfiltered relative-energy speed cap without mutating radar state."""
-    profile = self._profile(profile)
-    config = PROFILE_CONFIGS[profile]
-    delay = self._delay()
-    if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
-      return EnergyEnvelope()
-
-    try:
-      t_follow = get_T_FOLLOW(follow_personality)
-    except (NotImplementedError, TypeError, ValueError):
-      t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard)
-
-    x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
-    candidates: list[EnergyEnvelope] = []
-    nearly_stopped = False
-
-    for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
-      if not self._valid_lead(lead):
-        continue
-
-      x_lead = float(lead.dRel)
-      v_lead = float(lead.vLeadK)
-      a_lead = np.clip(float(lead.aLeadK), -10.0, 5.0)
-      a_lead_tau = float(lead.aLeadTau)
-      lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
-      x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
-      v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
-      nearly_stopped = nearly_stopped or v_lead_delay < STOPPED_LEAD_SPEED
-
-      match_gap = STOP_DISTANCE + t_follow * v_lead_delay
-      usable_gap = max(x_lead_delay - x_ego - match_gap, 0.0)
-      closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
-      if closing_speed == 0.0:
-        required_decel = 0.0
-      elif usable_gap == 0.0:
-        required_decel = math.inf
-      else:
-        required_decel = closing_speed * closing_speed / (2.0 * usable_gap)
-
-      # Relative kinetic energy: the lead keeps moving while ego sheds closing speed.
-      cap = v_lead_delay + math.sqrt(2.0 * config.comfort_decel * usable_gap)
-      candidates.append(EnergyEnvelope(cap, lead_index, usable_gap, closing_speed, required_decel))
-
-    if not candidates:
-      return EnergyEnvelope(has_nearly_stopped_lead=nearly_stopped)
-
-    selected = min(candidates, key=lambda candidate: candidate.cap)
-    return EnergyEnvelope(selected.cap, selected.selected_lead, selected.usable_gap, selected.closing_speed, selected.required_decel, nearly_stopped)
+  def _get_closing_factor(self, lead) -> float:
+    # Raw, uneased tier-scaled factor for THIS cycle -- see _get_lead_brake_factor's note.
+    if not lead.status:
+      return 1.0
+    return float(np.interp(lead.vRel, CLOSING_VREL_BP, CLOSING_FACTOR_V[self._personality]))
 
   def reset(self) -> None:
-    self.live.reset()
-    self.shadow.reset()
+    # Drop the accumulated widen (e.g. on disengage / standstill re-init) so it re-ramps cleanly.
+    self._widen = 0.0
+    self._onset_relax.reset()
+    self._lead_brake_relax.reset()
+    self._closing_relax.reset()
+    self._onset_factor = 1.0
+    self._lead_brake_factor = 1.0
+    self._closing_factor = 1.0
 
-  @staticmethod
-  def _lead_source(source) -> bool:
-    return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+  def get_max_accel(self, v_ego: float) -> float:
+    # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ).
+    table = A_CRUISE_MAX_V[self._personality] if self._enabled else STOCK_A_CRUISE_MAX_V
+    return float(np.interp(v_ego, A_CRUISE_MAX_BP, table))
 
-  def _update_path(
-    self,
-    path: _PacePath,
-    raw_cap: float,
-    base_speed: float,
-    v_ego: float,
-    config: ProfileConfig,
-    previous_mpc_source,
-    planner_speed: float,
-    previous_should_stop: bool,
-    has_nearly_stopped_lead: bool,
-    launch_delta_v: float,
-  ) -> float:
-    filtered_cap = path.update_filter(raw_cap)
-    just_initialized = path.pace is None
-    if just_initialized:
-      path.pace = min(base_speed, v_ego)
-      path.state = AccelControllerState.free
+  def get_rise_rate(self, v_ego: float) -> float:
+    # Disabled, or still closing on a lead -> stock ceiling open-rate.
+    if not self._enabled or self._closing_on_lead:
+      return STOCK_RISE_RATE
+    return float(np.interp(v_ego, RISE_RATE_BP, RISE_RATE_V[self._personality]))
 
-    # A clear-road standstill engagement should request motion immediately. A
-    # stopped/previously-stopping lead still goes through stop-hold confirmation.
-    if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop:
-      path.pace = min(base_speed, v_ego + launch_delta_v)
-      path.state = AccelControllerState.release
-      path.relief_time = config.release_confirm
-      path.departing_from_stop = True
-      return filtered_cap
+  def get_jerk_scale(self, v_ego: float) -> float:
+    # Most-relaxed of: near a stop, a fresh onset, a hard-braking lead, a fast-closing gap. Never > 1.0 (stock).
+    if not self._enabled:
+      return 1.0
+    near_stop = float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality]))
+    return min(near_stop, self._onset_factor, self._lead_brake_factor, self._closing_factor)
 
-    # A lower non-controller target is authoritative, and is also the correct seed if it later clears.
-    path.pace = min(path.pace, base_speed)
-    if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace:
-      path.pace = max(planner_speed, 0.0)
+  def get_t_follow(self, t_follow: float, v_ego: float) -> float:
+    # MPC t_follow hook. Adds a slewed, decel-held, speed-dependent comfort widen on top of the stock
+    # t_follow. Identity when disabled => byte-stock. Add-only => desired distance >= stock => brake >= stock.
+    t_follow = float(t_follow)
+    if not self._enabled:
+      self._widen = 0.0
+      self._t_follow = t_follow
+      return t_follow
 
-    if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead):
-      path.stopped_lead_hold = True
+    target = float(np.interp(v_ego, TF_WIDEN_V_BP, TF_WIDEN_BASE_V)) * TF_WIDEN_TIER[self._personality]
+    target = min(target, TF_WIDEN_MAX)
+    step = TF_SLEW_PER_S * DT_MDL
 
-    clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= CLEAR_ROAD_PROFILE_SPEED
-    if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete:
-      path.departing_from_stop = False
-      path.stopped_lead_hold = False
-
-    renewed_stop_evidence = filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead
-    enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or (previous_should_stop and not path.departing_from_stop))
-    if enter_stop_hold and path.state != AccelControllerState.stopHold:
-      path.pace = 0.0
-      path.state = AccelControllerState.stopHold
-      path.relief_time = 0.0
-      path.departure_frames = 0
-      path.departing_from_stop = False
-      return filtered_cap
-
-    if path.state == AccelControllerState.stopHold:
-      # A continuously observed moving lead exits after exactly four raw frames.
-      # Total lead loss still waits for the five-frame median dropout guard first.
-      raw_departure = math.isfinite(raw_cap) and raw_cap > STOP_HOLD_EXIT_CAP and not has_nearly_stopped_lead
-      guarded_lead_loss = not math.isfinite(raw_cap) and filtered_cap > STOP_HOLD_EXIT_CAP
-      if raw_departure or guarded_lead_loss:
-        path.departure_frames += 1
-      else:
-        path.departure_frames = 0
-
-      if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
-        path.pace = 0.0
-        return filtered_cap
-
-      path.state = AccelControllerState.release
-      path.relief_time = config.release_confirm
-      path.departure_frames = 0
-      path.departing_from_stop = True
-      path.pace = min(base_speed, filtered_cap, v_ego + launch_delta_v)
-      return filtered_cap
-
-    ceiling = min(base_speed, filtered_cap)
-    if ceiling <= path.pace - RESTRICT_DEADBAND:
-      path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt)
-      path.state = AccelControllerState.restrict
-      path.relief_time = 0.0
-      path.departing_from_stop = False
-      return filtered_cap
-
-    relief = ceiling - path.pace
-    release_allowed = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND
-    if relief >= RELIEF_DEADBAND and not release_allowed:
-      path.relief_time += self.dt
-      path.state = AccelControllerState.hold
-      release_allowed = path.relief_time >= config.release_confirm
-
-    if release_allowed:
-      pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else config.release_rate
-      path.pace = min(ceiling, path.pace + pace_rate * self.dt)
-      path.state = AccelControllerState.release
-    elif relief <= RELIEF_DEADBAND:
-      path.relief_time = 0.0
-      path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.hold
-
-    return filtered_cap
-
-  def _update_accel_limit(
-    self,
-    path: _PacePath,
-    stock_accel_max: float,
-    planner_accel: float,
-    profile_accel_max: float,
-  ) -> tuple[float, float]:
-    """Return telemetry effective max and the controller's pre-MPC positive bound."""
-    requested_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
-
-    if path.state == AccelControllerState.stopHold:
-      path.accel_limit = 0.0
-      return min(stock_accel_max, 0.0), 0.0
-
-    if path.departing_from_stop:
-      if path.stopped_lead_hold:
-        # A confirmed lead departure opens quickly but continuously from zero.
-        previous_limit = path.accel_limit if path.accel_limit is not None else 0.0
-        path.accel_limit = min(requested_limit, previous_limit + LAUNCH_ACCEL_JERK * self.dt)
-      else:
-        # The MPC stays completely stock for the first few centimeters of a
-        # clear-road launch. Seed the selected table value for a smooth handoff.
-        path.accel_limit = requested_limit
-      return min(stock_accel_max, path.accel_limit), path.accel_limit
-
-    if path.accel_limit is None:
-      # Avoid a discontinuity when enabling around an already-positive command.
-      # The global OP limit bounds this seed; dynamic stock output constraints
-      # still retain their existing output-side enforcement and slew.
-      path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel)))
+    if self._a_ego <= TF_DECEL_HOLD_A and target < self._widen:
+      pass                                              # decel-hold: don't ease the gap in while braking
+    elif target > self._widen:
+      self._widen = min(target, self._widen + step)     # open the gap, slewed
     else:
-      max_step = ACCEL_LIMIT_JERK * self.dt
-      path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step))
+      self._widen = max(target, self._widen - step)     # close the gap, slewed
 
-    effective_limit = min(stock_accel_max, path.accel_limit)
-    return effective_limit, path.accel_limit
+    self._widen = max(0.0, self._widen)                 # add-only guard
+    self._t_follow = t_follow + self._widen
+    return self._t_follow
 
-  def _build_mpc_accel_max(
-    self,
-    path: _PacePath,
-    envelope: EnergyEnvelope,
-    filtered_cap: float,
-    previous_mpc_source,
-    accel_limit: float,
-  ) -> tuple[float, ...] | None:
-    """Build a short pre-MPC bound while leaving the future horizon stock-warm."""
-    # Stock tip-in removes launch delay and gives every profile the same initial
-    # response. The lookup table becomes active once the car is barely rolling.
-    if path.departing_from_stop and not path.stopped_lead_hold:
-      return None
+  # --- telemetry (published to cereal LongitudinalPlanSP.acceleration; no control effect) ---
+  def enabled(self) -> bool:
+    return self._enabled
 
-    # A short total-lead dropout has no obstacle to hold stock MPC at zero.
-    # Bound the whole horizon only while the median guard still says "stopped";
-    # genuine loss transitions to the tapered confirmation path below.
-    if path.state == AccelControllerState.stopHold and envelope.selected_lead < 0 and path.departure_frames == 0:
-      return tuple(0.0 for _ in T_IDXS)
+  def personality(self):
+    return self._personality
 
-    special_launch_state = path.state == AccelControllerState.stopHold or path.departing_from_stop
+  def max_accel(self) -> float:
+    return self.get_max_accel(self._v_ego)
 
-    # Ordinary lead following must retain stock MPC constraints and obstacle
-    # behavior. Include filtered and previous-source state so a radar dropout
-    # cannot switch the profile bound on for only one or two frames.
-    lead_guarded = envelope.selected_lead >= 0 or math.isfinite(filtered_cap) or self._lead_source(previous_mpc_source)
-    if not special_launch_state and lead_guarded:
-      return None
+  def t_follow(self) -> float:
+    return self._t_follow
 
-    if not math.isfinite(accel_limit):
-      return None
+  def follow_widen(self) -> float:
+    return self._widen
 
-    bounded_limit = float(np.clip(accel_limit, 0.0, ACCEL_MAX))
-    accel_max = np.full(len(T_IDXS), bounded_limit, dtype=float)
-    if special_launch_state:
-      # A hard low bound across the full action-delay horizon cold-soaks the
-      # stop solver. Two bounded nodes plus one tapered warm-up node holds the
-      # vehicle through confirmation while preserving a ready future solution.
-      accel_max[MPC_LAUNCH_BOUND_NODES:] = ACCEL_MAX
-      if len(accel_max) > MPC_LAUNCH_BOUND_NODES:
-        if path.state == AccelControllerState.stopHold:
-          warm_blend = MPC_CONFIRM_WARM_BLEND if path.departure_frames > 0 else MPC_STOP_WARM_BLEND
-        else:
-          warm_blend = MPC_DEPART_WARM_BLEND
-        accel_max[MPC_LAUNCH_BOUND_NODES] = bounded_limit + warm_blend * (ACCEL_MAX - bounded_limit)
-    return tuple(float(value) for value in accel_max)
-
-  @staticmethod
-  def _valid_context(
-    base_speed: float,
-    v_ego: float,
-    a_ego: float,
-    planner_speed: float,
-    stock_accel_max: float,
-    planner_accel: float,
-    delay: float,
-    engaged: bool,
-    cruise_initialized: bool,
-    controller_fault: bool,
-  ) -> bool:
-    return (
-      engaged
-      and cruise_initialized
-      and not controller_fault
-      and base_speed >= 0.0
-      and v_ego >= 0.0
-      and planner_speed >= 0.0
-      and delay >= 0.0
-      and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay))
-    )
-
-  def update(
-    self,
-    radar_state,
-    *,
-    base_speed: float,
-    v_ego: float,
-    a_ego: float,
-    profile: int | AccelProfile,
-    follow_personality,
-    enabled: bool,
-    acc_selected: bool,
-    engaged: bool,
-    cruise_initialized: bool,
-    previous_mpc_source,
-    planner_speed: float,
-    stock_accel_max: float,
-    planner_accel: float,
-    previous_should_stop: bool,
-    controller_fault: bool = False,
-  ) -> AccelControllerResult:
-    """Update live and shadow acceleration controllers and return the target and additive telemetry."""
-    profile = self._profile(profile)
-    config = PROFILE_CONFIGS[profile]
-    profile_accel_max = self.get_profile_accel_max(profile, v_ego)
-    launch_delta_v = LAUNCH_DELTA_V
-    delay = self._delay()
-    valid_context = self._valid_context(
-      base_speed,
-      v_ego,
-      a_ego,
-      planner_speed,
-      stock_accel_max,
-      planner_accel,
-      delay,
-      engaged,
-      cruise_initialized,
-      controller_fault,
-    )
-
-    envelope = self.calculate_energy_envelope(radar_state, v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
-
-    if valid_context:
-      shadow_filtered_cap = self._update_path(
-        self.shadow,
-        envelope.cap,
-        base_speed,
-        v_ego,
-        config,
-        previous_mpc_source,
-        planner_speed,
-        previous_should_stop,
-        envelope.has_nearly_stopped_lead,
-        launch_delta_v,
-      )
-      self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max)
-      shadow_active = True
-    else:
-      self.shadow.reset()
-      shadow_filtered_cap = math.inf
-      shadow_active = False
-
-    live_active = valid_context and bool(enabled) and bool(acc_selected)
-    if live_active:
-      live_filtered_cap = self._update_path(
-        self.live,
-        envelope.cap,
-        base_speed,
-        v_ego,
-        config,
-        previous_mpc_source,
-        planner_speed,
-        previous_should_stop,
-        envelope.has_nearly_stopped_lead,
-        launch_delta_v,
-      )
-      effective_accel_max, controller_accel_max = self._update_accel_limit(
-        self.live, stock_accel_max, planner_accel, profile_accel_max
-      )
-      # Feed only the controller-owned ceiling into MPC. Stock's speed, turn,
-      # coast, and no-throttle limits remain in their original output clip.
-      mpc_accel_max = self._build_mpc_accel_max(
-        self.live, envelope, live_filtered_cap, previous_mpc_source, controller_accel_max,
-      )
-      mpc_shape_cruise = (
-        mpc_accel_max is not None
-        and self.live.state != AccelControllerState.stopHold
-        and not self.live.departing_from_stop
-      )
-      if mpc_accel_max is None:
-        effective_accel_max = stock_accel_max
-      if self.live.state == AccelControllerState.stopHold:
-        # Bounds provide the dropout/creep guard while the stock cruise target
-        # keeps the solver ready for a confirmed departure.
-        target_speed = base_speed
-      elif self.live.departing_from_stop and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0:
-        # A moving lead keeps stock MPC well-conditioned during a confirmed
-        # departure. Clear-road launches retain the bounded live pace below.
-        target_speed = base_speed
-      else:
-        target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
-    else:
-      self.live.reset()
-      live_filtered_cap = math.inf
-      # Preserve the stock target bit-for-bit on every bypass, including stock's own invalid-value handling.
-      target_speed = base_speed
-      effective_accel_max = math.inf
-      mpc_accel_max = None
-      mpc_shape_cruise = False
-
-    return AccelControllerResult(
-      target_speed=target_speed,
-      enabled=bool(enabled),
-      active=live_active,
-      shadow_active=shadow_active,
-      launching=live_active and self.live.departing_from_stop,
-      profile=profile,
-      profile_accel_max=profile_accel_max if live_active else math.inf,
-      effective_accel_max=effective_accel_max,
-      mpc_accel_max=mpc_accel_max,
-      mpc_shape_cruise=mpc_shape_cruise,
-      state=self.live.state,
-      shadow_state=self.shadow.state,
-      base_speed=base_speed,
-      raw_energy_cap=envelope.cap,
-      live_filtered_cap=live_filtered_cap,
-      shadow_filtered_cap=shadow_filtered_cap,
-      live_pace=self.live.pace if self.live.pace is not None else math.inf,
-      shadow_pace=self.shadow.pace if self.shadow.pace is not None else math.inf,
-      selected_lead=envelope.selected_lead,
-      usable_gap=envelope.usable_gap,
-      closing_speed=envelope.closing_speed,
-      required_decel=envelope.required_decel,
-    )
+  def widen_active(self) -> bool:
+    return self._enabled and self._widen > 0.005

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

@@ -0,0 +1,86 @@
+"""
+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.
+
+Acceleration Personality tuning tables. The controller shapes only what the longitudinal MPC CONSUMES
+(the positive-accel ceiling + its open-rate, and an add-only follow-gap widen fed to the MPC's t_follow);
+it never post-shapes the MPC's output accel. Disabled => every getter returns the upstream stock value,
+so off == byte-stock.
+"""
+
+from cereal import custom
+
+AccelerationPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+ECO = AccelerationPersonality.eco
+NORMAL = AccelerationPersonality.normal
+SPORT = AccelerationPersonality.sport
+
+PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values())
+PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values())
+
+# --- Positive-accel ceiling (launch/cruise) + its upward open-rate ---------------------------------------
+# Disabled -> falls back to STOCK_* (byte-stock). ACCEL_MAX (opendbc) hard-caps the ceiling at 2.0 m/s^2.
+A_CRUISE_MAX_BP = [0., 10., 25., 40.]              # m/s (matches upstream A_CRUISE_MAX_BP)
+STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6]        # upstream A_CRUISE_MAX_VALS
+STOCK_RISE_RATE = 0.05                             # upstream ceiling open-rate (m/s^2 per cycle)
+A_CRUISE_MAX_V = {
+  ECO:    [1.55, 0.75, 0.35, 0.20],
+  NORMAL: [2.00, 1.40, 0.95, 0.70],
+  SPORT:  [2.00, 1.70, 1.20, 0.90],
+}
+# Ceiling open-rate (m/s^2 per cycle): fast near v=0 so launch isn't delayed, tapers by v=5.
+RISE_RATE_BP = [0., 5.]                            # m/s
+RISE_RATE_V = {
+  ECO:    [0.80, 0.07],
+  NORMAL: [1.00, 0.16],
+  SPORT:  [1.20, 0.24],
+}
+# The fast near-stop rise-rate above only looks at v_ego, so it also fires while still closing on a lead
+# that hasn't cleared yet, letting the ceiling snap open faster than the actual situation calls for. Fall
+# back to the stock rate whenever still closing (vRel this negative or more).
+RISE_RATE_LEAD_VREL_GATE = -0.5                    # m/s
+
+# --- Launch jerk-cost relaxation (MPC INPUT: scales the core MPC's own jerk_factor) -----------------------
+# Bounded near a stop, ramped back to 1.0 (stock) by cruise speed. v=0 knot closed-loop verified; SPORT tied
+# to NORMAL rather than pushed lower (lower destabilizes the solver at this knot).
+JERK_SCALE_BP = [0., 5.]                           # m/s
+JERK_SCALE_V = {
+  ECO:    [0.60, 1.0],
+  NORMAL: [0.45, 1.0],
+  SPORT:  [0.45, 1.0],
+}
+
+# --- Onset jerk-cost relaxation (MPC INPUT: any accel<->decel direction change, not just launch) ----------
+ONSET_DEADBAND = 0.15          # m/s^2: ignore aEgo noise this small around a zero-crossing
+ONSET_RAMP_S = 0.4             # s: ease back to stock over this long
+ONSET_FLOOR = {ECO: 0.75, NORMAL: 0.65, SPORT: 0.50}
+
+# Ramp shared by the two level-triggered factors below (lead-braking, closing-rate): a sustained pinned
+# floor destabilizes the MPC's re-solve, so both ease back to stock over this window instead.
+RELAX_RAMP_S = 0.4
+
+# --- Lead-braking jerk-cost relaxation (MPC INPUT: react faster to a hard-braking lead) --------------------
+LEAD_BRAKE_ALEAD_BP = [-3.0, -0.5]      # m/s^2, lead's own aLeadK
+LEAD_BRAKE_FACTOR_V = {
+  ECO:    [0.75, 1.0],
+  NORMAL: [0.60, 1.0],
+  SPORT:  [0.45, 1.0],
+}
+
+# --- Closing-rate jerk-cost relaxation (MPC INPUT: react faster to a fast-closing gap, any cause) ----------
+CLOSING_VREL_BP = [-6.0, -1.5]          # m/s, closing rate (negative = closing)
+CLOSING_FACTOR_V = {
+  ECO:    [0.75, 1.0],
+  NORMAL: [0.60, 1.0],
+  SPORT:  [0.45, 1.0],
+}
+
+# --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------
+TF_WIDEN_V_BP = [14.0, 28.0]                       # m/s: widen ramps in across this band, flat above
+TF_WIDEN_BASE_V = [0.0, 0.30]                      # s: base follow-time added at the band ends (pre-tier)
+TF_WIDEN_TIER = {ECO: 1.30, NORMAL: 1.00, SPORT: 0.50}
+TF_WIDEN_MAX = 0.45                                # s: absolute cap on the added gap
+TF_SLEW_PER_S = 0.50                               # s per second: max rate the widen may open/close
+TF_DECEL_HOLD_A = -0.20                            # m/s^2: at/below this a_ego the widen won't shrink

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

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

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

@@ -10,6 +10,7 @@ class WMACConstants:
   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)))
+  TRANSITION_SMOOTH_FRAMES = max(1, int(round(0.5 / DT_MDL)))
 
   LEAD_PROB = 0.45
   LEAD_EXIT_PROB = 0.25

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

@@ -66,13 +66,15 @@ class ModeTransitionManager:
     self._pending_mode: ModeType = 'acc'
     self._pending_count = 0
     self._blended_hold_frames = 0
+    self.frames_since_switch = WMACConstants.TRANSITION_SMOOTH_FRAMES
+    self.last_switch_was_immediate = False
 
   def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
     if immediate:
       self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
       self._pending_mode = mode
       self._pending_count = 0
-      self._switch_mode(mode)
+      self._switch_mode(mode, immediate=True)
       return
 
     if cancel_hold and mode == 'acc':
@@ -97,17 +99,18 @@ class ModeTransitionManager:
 
     required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
     if self._pending_count >= required_count:
-      self._switch_mode(mode)
+      self._switch_mode(mode, immediate=False)
 
   def update(self) -> None:
     if self._blended_hold_frames > 0:
       self._blended_hold_frames -= 1
     self.mode_duration += 1
+    self.frames_since_switch += 1
 
   def get_mode(self) -> ModeType:
     return self.current_mode
 
-  def _switch_mode(self, mode: ModeType) -> None:
+  def _switch_mode(self, mode: ModeType, immediate: bool) -> None:
     if mode == self.current_mode:
       return
 
@@ -115,6 +118,8 @@ class ModeTransitionManager:
     self.mode_duration = 0
     self._pending_mode = mode
     self._pending_count = 0
+    self.frames_since_switch = 0
+    self.last_switch_was_immediate = immediate
 
 
 class DynamicExperimentalController:
@@ -183,6 +188,31 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
+  def has_radar_acc_lead(self) -> bool:
+    # Same criterion _desired_mode() uses to force 'acc' mode: a near or fast-closing radar lead is trusted
+    # enough that the e2e model's own opinion should never blend in over pure MPC. Computed every cycle
+    # regardless of the DEC param/active() state (see _update_calculations, called unconditionally from
+    # update()), so callers can use this as a lead-safety baseline independent of whether DEC itself is on.
+    return not self._CP.radarUnavailable and self._has_radar_acc_lead
+
+  def is_urgent(self) -> bool:
+    # Same "immediate" criterion _desired_mode() uses to decide a mode switch can't wait: an FCW-flagged MPC,
+    # or a model slow-down whose smoothed hysteresis has latched AND whose raw severity clears the urgent
+    # threshold. Public so callers outside the mode-switch decision (e.g. smoothing_transition) can reuse the
+    # identical, already-tuned definition of "this is not a routine moment" instead of inventing a second one.
+    return self._has_mpc_fcw or (self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB)
+
+  def smoothing_transition(self) -> bool:
+    # True continuously while blended and NOT currently urgent -- not just for a short window right after the
+    # switch. The e2e model's own action.desiredAcceleration can otherwise drop sharply frame-to-frame at any
+    # point while steady-state in blended (not only at the switch instant), with zero jerk shaping from
+    # either this fork's MPC-side relax (jerk_scale never touches the e2e path) or the old switch-scoped-only
+    # version of this guard. Checking is_urgent() every cycle (not just "was the switch immediate") also
+    # covers a new emergency that appears AFTER a routine switch, which the old snapshot-at-switch-time
+    # check couldn't. Never true during a genuine emergency at any point, so it never delays/softens a real
+    # stop the model or FCW is trying to make happen sooner.
+    return self._mode_manager.current_mode == 'blended' and not self.is_urgent()
+
   def set_mpc_fcw_crash_cnt(self) -> None:
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 

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

@@ -1,6 +1,7 @@
 import pytest
 
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 
 
 class MockLeadOne:
@@ -98,6 +99,83 @@ def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
   assert controller.mode() == "blended"
 
 
+def test_smoothing_transition_false_while_never_switched(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  assert controller.mode() == "acc"
+  assert not controller.smoothing_transition()
+
+
+def test_smoothing_transition_true_right_after_routine_switch_and_stays_true(mock_cp, mock_mpc, default_sm):
+  # a standstill-driven switch is routine (not FCW/immediate) -- smoothing must engage at the switch and,
+  # since the moment stays non-urgent, keep applying continuously (no more time-boxed lapse) as long as we
+  # stay in blended and nothing urgent shows up -- see test_smoothing_transition_turns_off_when_urgency_rises
+  # for the case that DOES turn it off.
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['carState'].standstill = True
+  saw_switch_with_smoothing = False
+  prev_mode = controller.mode()
+  for _ in range(20):
+    controller.update(default_sm)
+    if controller.mode() == "blended" and prev_mode == "acc":
+      saw_switch_with_smoothing = controller.smoothing_transition()
+      break
+    prev_mode = controller.mode()
+  assert saw_switch_with_smoothing
+  for _ in range(WMACConstants.TRANSITION_SMOOTH_FRAMES + 2):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+  assert controller.smoothing_transition()   # still non-urgent -- no time-based lapse anymore
+
+
+def test_smoothing_transition_turns_off_when_urgency_rises_while_already_blended(mock_cp, mock_mpc, default_sm):
+  # The gap this fixes: entering blended for a ROUTINE reason (standstill) must not permanently exempt a
+  # LATER genuine emergency that appears while already blended -- the old last_switch_was_immediate snapshot
+  # only looked at how we entered, never re-checked. is_urgent() re-checks every cycle instead.
+  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"
+  assert controller.smoothing_transition()
+  assert not controller.is_urgent()
+
+  mock_mpc.crash_cnt = 1   # a new FCW appears, still in blended (standstill keeps it there)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"   # no mode switch needed/observed -- already blended
+  assert controller.is_urgent()
+  assert not controller.smoothing_transition()   # must turn off immediately, same cycle
+
+
+def test_is_urgent_true_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  mock_mpc.crash_cnt = 1
+  controller.update(default_sm)
+  assert controller.is_urgent()
+
+
+def test_is_urgent_false_for_routine_slowdown_below_threshold(mock_cp, mock_mpc, default_sm):
+  # a mild slow-down (not near the URGENT_SLOW_DOWN_PROB severity) must not read as urgent.
+  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, desired_acceleration=-0.6)   # just past MODEL_DECEL_START
+  controller.update(default_sm)
+  assert controller._raw_urgency < WMACConstants.URGENT_SLOW_DOWN_PROB
+  assert not controller.is_urgent()
+
+
+def test_smoothing_transition_false_for_emergency_switch(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"
+  assert not controller.smoothing_transition()   # immediate/emergency switch must never be smoothed
+
+
 def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
   mock_cp.radarUnavailable = True
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
@@ -233,3 +311,42 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau
 
   assert controller._has_lead_filtered
   assert controller.mode() == "acc"
+
+
+def test_has_radar_acc_lead_true_for_near_lead(mock_cp, mock_mpc, default_sm):
+  # within RADAR_LEAD_ACC_MAX_DREL -- available regardless of whether DEC's own param/active() is on, since
+  # _update_calculations runs every cycle unconditionally.
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=40.0, vRel=0.0)
+  controller.update(default_sm)
+  assert controller.has_radar_acc_lead()
+
+
+def test_has_radar_acc_lead_false_for_far_slow_lead(mock_cp, mock_mpc, default_sm):
+  # beyond MAX_DREL and not closing fast enough for the TTC gate -- correctly not trusted as an ACC-safe lead.
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  controller.update(default_sm)
+  assert not controller.has_radar_acc_lead()
+
+
+def test_has_radar_acc_lead_false_when_radar_unavailable(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, dRel=40.0, vRel=0.0)
+  controller.update(default_sm)
+  assert not controller.has_radar_acc_lead()
+
+
+def test_has_radar_acc_lead_independent_of_dec_param(mock_cp, mock_mpc, default_sm):
+  # DEC disabled (param False) must not affect this -- it's a lead-safety baseline other callers rely on
+  # regardless of whether DEC itself is on.
+  class MockParamsOff:
+    def get_bool(self, name):
+      return False
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParamsOff())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=40.0, vRel=0.0)
+  controller.update(default_sm)
+  assert not controller.enabled()
+  assert not controller.active()
+  assert controller.has_radar_acc_lead()

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,11 +8,9 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.sunnypilot import get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
+from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import RadarDistanceController
 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
@@ -24,45 +22,67 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
+# Bounds how fast output_a_target may drop while blended and NOT currently urgent (continuous -- not just a
+# short window right after the switch; see DynamicExperimentalController.smoothing_transition()/is_urgent()).
+# Never active during a genuine emergency (FCW, or a model slow-down past the urgent-severity threshold) at
+# any point, whether that was true at the moment of the switch or only became true later while still blended.
+TRANSITION_MAX_DROP_PER_CYCLE = 0.15   # m/s^2 per cycle
+
+
+class _E2ETransitionGuard:
+  # Without this, the e2e model's own action.desiredAcceleration -- which the core MPC-side jerk_scale never
+  # shapes (it only touches the MPC's own solution, not this raw model path) -- can drop sharply frame-to-
+  # frame at any point while blended, not only at the instant DEC switches into it, producing a discontinuous
+  # brake with zero jerk shaping. Only ever limits a DOWNWARD move; never delays a rise.
+  def __init__(self):
+    self._last = None
+
+  def reset(self) -> None:
+    self._last = None
+
+  def apply(self, output_a_target: float, smoothing_active: bool) -> float:
+    if not smoothing_active or self._last is None:
+      self._last = output_a_target
+      return output_a_target
+    limited = max(output_a_target, self._last - TRANSITION_MAX_DROP_PER_CYCLE)
+    self._last = limited
+    return limited
+
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
-    self.params = Params()
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel = AccelController(CP, mpc)
+    self.radar_distance = RadarDistanceController(CP)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
-    self.accel_controller = AccelController(CP, dt=dt)
-    self.accel_controller_result = None
-
-    self._param_read_frames = max(1, int(round(0.25 / dt)))
-    self._param_frame = 0
-    self.accel_personality_enabled = False
-    self.accel_personality = int(AccelProfile.normal)
 
     self.output_v_target = 0.
     self.output_a_target = 0.
-
-  def _read_accel_controller_params(self) -> None:
-    if self._param_frame % self._param_read_frames == 0:
-      self.accel_personality_enabled = self.params.get_bool("AccelPersonalityEnabled")
-      self.accel_personality = get_sanitize_int_param(
-        "AccelPersonality", int(AccelProfile.eco), int(AccelProfile.sport), self.params,
-      )
-
-    self._param_frame += 1
+    self._e2e_transition_guard = _E2ETransitionGuard()
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.dec.active():
-      return experimental_mode
+    if not experimental_mode:
+      return False
 
-    return experimental_mode and self.dec.mode() == "blended"
+    # A near/fast-closing radar lead always routes to pure MPC, regardless of whether DEC itself is on --
+    # this baseline is not something the DEC toggle should be able to bypass (dec.has_radar_acc_lead() is
+    # computed every cycle independent of dec.active()). DEC's own toggle only gates the OTHER blended
+    # triggers (standstill, slow-down, FCW) below.
+    if self.dec.has_radar_acc_lead():
+      return False
+
+    if not self.dec.active():
+      return True
+
+    return self.dec.mode() == "blended"
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
@@ -94,35 +114,19 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
-  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
-                              acc_selected: bool, planner_speed: float, previous_mpc_source, previous_should_stop: bool,
-                              stock_accel_max: float, planner_accel: float, controller_fault: bool = False) -> float:
-    self.accel_controller_result = self.accel_controller.update(
-      sm['radarState'],
-      base_speed=base_speed,
-      v_ego=sm['carState'].vEgo,
-      a_ego=sm['carState'].aEgo,
-      profile=self.accel_personality,
-      follow_personality=sm['selfdriveState'].personality,
-      enabled=self.accel_personality_enabled,
-      acc_selected=acc_selected,
-      engaged=engaged,
-      cruise_initialized=cruise_initialized,
-      previous_mpc_source=previous_mpc_source,
-      planner_speed=planner_speed,
-      stock_accel_max=stock_accel_max,
-      planner_accel=planner_accel,
-      previous_should_stop=previous_should_stop,
-      controller_fault=controller_fault,
-    )
-    return self.accel_controller_result.target_speed
-
   def update(self, sm: messaging.SubMaster) -> None:
-    self._read_accel_controller_params()
     self.events_sp.clear()
     self.dec.update(sm)
+    self.accel.update(sm)
+    self.radar_distance.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
+  def smooth_radarstate(self, radarstate):
+    return self.radar_distance.smooth_radarstate(radarstate)
+
+  def smooth_e2e_transition(self, output_a_target: float) -> float:
+    return self._e2e_transition_guard.apply(output_a_target, self.dec.smoothing_transition())
+
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
 
@@ -140,26 +144,6 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
-    # Accel Controller relative-pace governor
-    if self.accel_controller_result is not None:
-      result = self.accel_controller_result
-      accel_controller = longitudinalPlanSP.accelController
-      accel_controller.enabled = result.enabled
-      accel_controller.active = result.active
-      accel_controller.shadowOnly = result.shadow_active and not result.active
-      accel_controller.profile = int(result.profile)
-      accel_controller.state = int(result.state if result.active else result.shadow_state)
-      accel_controller.vTargetBase = float(result.base_speed)
-      accel_controller.vTargetRaw = float(result.raw_energy_cap)
-      accel_controller.vTargetFiltered = float(result.live_filtered_cap)
-      accel_controller.vTargetShadow = float(result.shadow_filtered_cap)
-      accel_controller.leadIndex = result.selected_lead
-      accel_controller.usableGap = float(result.usable_gap)
-      accel_controller.closingSpeed = float(result.closing_speed)
-      accel_controller.requiredDecel = float(result.required_decel)
-      accel_controller.aMaxProfile = float(result.profile_accel_max)
-      accel_controller.aMaxEffective = float(result.effective_accel_max)
-
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control
@@ -203,4 +187,17 @@ class LongitudinalPlannerSP:
     e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
     e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert
 
+    # Acceleration Personality (telemetry only; brakeNeed/decelTarget/smoothActive repurposed for the
+    # input-shaping design -- see cereal custom.capnp Acceleration).
+    acceleration = longitudinalPlanSP.acceleration
+    acceleration.personality = self.accel.personality()
+    acceleration.enabled = self.accel.enabled()
+    acceleration.maxAccel = float(self.accel.max_accel())
+    acceleration.brakeNeed = float(self.accel.follow_widen())    # follow-gap widen added on top of stock (s)
+    acceleration.decelTarget = float(self.accel.t_follow())       # t_follow handed to the MPC (s)
+    acceleration.smoothActive = self.accel.widen_active()         # follow-gap widen currently active
+    acceleration.bypassed = False                                 # unused (no output shaping / bypass anymore)
+    acceleration.leadUnstable = bool(self.radar_distance.lead_unstable())
+
+
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py

@@ -0,0 +1,449 @@
+"""
+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.
+
+RadarDistance conditions the lead the longitudinal MPC follows on a noisy (TSS2-class) radar. It NEVER
+reports a farther-or-faster lead than reality, so braking is always >= stock. Four mechanisms:
+  * jump-guard: reject a same-cycle FARTHER dRel jump on a lead that never dropped status (a vision/radar
+    fusion transient during lead acquisition -- e.g. a cut-in whose vision distance estimate briefly
+    disagrees with a solid radar track) by holding the last-trusted, closer reading instead of snapping back
+    out. A closer jump of any size always passes immediately -- this only ever delays relief, never a brake;
+  * flicker-hold: keep a just-dropped, recently-sustained lead alive (dead-reckoned) through a brief radar
+    dropout so the MPC does not lose and re-grab it (which reads as a phantom release then a catch-up brake);
+  * churn/noise smoother: a short EMA on a lead's dRel/vLead/vRel so the MPC stops hunting the gap (removes
+    the follow-jitter that reads as rubber-banding and, on the sensor side, as a lead-detection "lurch").
+    Covers two DISTINCT same-physical-object noise signatures: trackId churn (id flips frame-to-frame but the
+    kinematics stay coherent -- one real lead getting re-labeled) and same-track noise (id stays constant but
+    vLead itself is bimodal/bouncing -- one real lead with a noisy fusion/Doppler velocity read). Both are
+    safe to EMA because the id evidence pins them to a SINGLE physical object; a bimodal vLead WITH the id
+    also changing is left alone (ambiguous -- could be two really-different real objects) so this can never
+    average two real tracks together. dRel is asymmetric -- closer accepted immediately, only farther is
+    EMA-lagged -- so it can't hold a steadily-closing lead farther-than-true; vLead/vRel stay symmetric;
+  * stop-gap: near a (near-)stopped lead at low speed report dRel a touch closer so the MPC's own smooth stop
+    settles farther back (the Prius TSS2 stock crawl creeps in to ~1.5 m). Monotone (closer => brake >= stock).
+    Overridden off by sustained lead motion (even slow creep) so it can't suppress a real, growing gap during
+    a launch. Never runs on a held (jump-guard or flicker-hold) lead, since a hold's vLead/dRel are stale;
+Also publishes a read-only lead-instability flag (telemetry). Disabled => byte-stock passthrough always.
+"""
+
+from collections import deque
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+HOLD_MAX_FRAMES = 20        # ~1.0s flicker-hold cap, since the last sustained lead
+SUSTAIN_FRAMES = 2
+DROPOUT_DREL = 1.0
+FCW_PROB_CAP = 0.9
+MIN_HELD_DREL = 0.5
+
+LOW_SPEED_PASSTHROUGH_V = 5.0   # m/s: below this, no flicker-hold (holding a stale lead near a stop would
+                                # delay the launch); the churn smoother still runs down to CREEP_PASSTHROUGH_V
+CREEP_PASSTHROUGH_V = 1.0       # m/s: below this, full byte-stock passthrough (protect the stock stop distance)
+
+SWITCH_DREL = 4.0               # m, dRel jump = a track switch (used by the instability detector + jump-guard).
+
+# Jump-guard: a same-cycle dRel jump this far FARTHER, on a lead that never dropped status, is treated as a
+# fusion transient (not a real sudden separation) and held at the last-trusted value for a bounded number of
+# frames. Self-heals fast so a genuinely-departing lead is never held stale for long.
+JUMP_GUARD_MAX_HOLD = 10       # frames (~0.5s)
+
+# How many frames a _JumpGuard/_LeadSmoother reference can go without a step()/update() call (below the speed
+# gate that call is skipped entirely) before it's discarded as stale rather than diffed/EMA'd against. Must be
+# small next to a real stop (the bug this guards against ran 3200+ frames/160s) but comfortably bigger than an
+# incidental single skipped cycle from v_ego dithering right at LOW_SPEED_PASSTHROUGH_V/CREEP_PASSTHROUGH_V --
+# a flat ">1" here defeated the same-cycle farther-jump guard on every dithered cycle and let a real jitter
+# glitch leak into the smoother's EMA for up to ~1s.
+REFERENCE_STALE_FRAMES = 20    # ~1.0s
+
+# Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead.
+STABILITY_WINDOW = 5           # frames (~0.25s @ 20Hz)
+VLEAD_SPREAD = 4.0            # m/s, vLead range over the window above which the lead is "unstable"
+ID_CHURN_WINDOW = 10          # frames (~0.5s) for radarTrackId-churn detection (steady lead, flipping ids)
+ID_CHURN = 3                 # trackId switches in the window above which the lead is "unstable" (follow-hunting)
+
+# Churn smoother: during trackId churn the per-track dRel/vRel jitter makes the MPC hunt the follow gap. A
+# short SYMMETRIC EMA on the churning lead removes the jitter so the MPC sees a steady lead. Active ONLY
+# during churn (NOT bimodal vLead -> never averages two real tracks). Bounded symmetric lag ~LEAD_SMOOTH_TAU.
+LEAD_SMOOTH_TAU = 0.5          # s, EMA time constant
+LEAD_SMOOTH_HOLD = 20          # frames (~1s): keep smoothing through brief churn gaps (churn toggles on/off)
+
+# Stop-gap: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_M closer so the MPC's own
+# smooth stop terminates that much farther back (stock Prius crawl-creeps to ~1.5 m). Monotone (only reports
+# closer => brake >= stock). Ramps in below the regime dRel and out as the lead starts moving; releases on
+# launch as ego speed rises past STOP_GAP_VEGO.
+STOP_GAP_M = 2.5               # m: max dRel reduction = added standstill gap
+STOP_GAP_VEGO = 8.0            # m/s: only below this ego speed
+STOP_GAP_VLEAD = 1.5           # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this
+STOP_GAP_REGIME_DREL = 12.0    # m: bias ramps in below this dRel
+STOP_GAP_RAMP_BAND = 2.0       # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND)
+STOP_GAP_MIN_DREL = 2.0        # m: never report a lead closer than this
+
+# Stop-gap creep override: a lead creeping forward slowly can sit under STOP_GAP_VLEAD for many seconds
+# without crossing it, so the bias keeps suppressing a real, growing gap. Sustained motion this long overrides
+# the bias off regardless of how slow. The counter decays (not just holds) on a sub-threshold frame, so it
+# takes sustained motion, not noise straddling the threshold, to reach the cap.
+STOP_GAP_CREEP_V = 0.03        # m/s: a truly-stopped lead reads exactly 0.0; treat anything above this as motion
+STOP_GAP_CREEP_HOLD_S = 1.5    # s: this much sustained motion overrides the bias off
+STOP_GAP_CREEP_HOLD_FRAMES = int(STOP_GAP_CREEP_HOLD_S / DT_MDL)
+
+
+class _BiasedLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, dRel):
+    self.status = src.status
+    self.dRel = dRel
+    self.yRel = src.yRel
+    self.vRel = src.vRel
+    self.vLead = src.vLead
+    self.vLeadK = src.vLeadK
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
+class _SmoothedLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, dRel, vLead, vRel):
+    self.status = src.status
+    self.dRel = dRel
+    self.yRel = src.yRel
+    self.vRel = vRel
+    self.vLead = vLead
+    self.vLeadK = vLead
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
+class _HeldLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, dRel, vRel, vLead, aLeadK, aLeadTau, modelProb):
+    self.status = True
+    self.dRel = dRel
+    self.vRel = vRel
+    self.vLead = vLead
+    self.vLeadK = vLead
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau
+    self.modelProb = modelProb
+    self.yRel = 0.0
+
+
+class _RadarStateProxy:
+  __slots__ = ('leadOne', 'leadTwo')
+
+  def __init__(self, lead_one, lead_two):
+    self.leadOne = lead_one
+    self.leadTwo = lead_two
+
+
+class _LeadSmoother:
+  # EMA on a noisy same-physical-object lead's dRel/vLead/vRel (jitter removal; see _LeadStability for what
+  # qualifies as "same object"). A hold keeps it active through brief noise gaps (the trigger toggles on/off);
+  # passthrough + reset only after the hold lapses. dRel is ASYMMETRIC: a closer raw reading is accepted
+  # immediately (never delay awareness of closer -- the file's own invariant), only a FARTHER raw reading is
+  # EMA-lagged (reject noise in that direction). Without this, a lead that's genuinely closing steadily while
+  # noisy (even briefly) gets held farther-than-true for the full LEAD_SMOOTH_HOLD window, then snaps -- a
+  # false-relief-then-correction that itself becomes a hard brake.
+  def __init__(self):
+    self._d = None
+    self._vl = None
+    self._vr = None
+    self._hold = 0
+    self._last_frame = 0
+
+  def reset(self):
+    self._d = None
+    self._vl = None
+    self._vr = None
+    self._hold = 0
+    self._last_frame = 0
+
+  def update(self, lead, noisy: bool, frame: int):
+    # A held lead's dRel/vLead is a stale extrapolation -- feeding it into the EMA would both hide that from
+    # downstream (wraps it into a _SmoothedLead) and pollute _d/_vl/_vr, lagging the real value's recovery.
+    if isinstance(lead, _HeldLead):
+      self.reset()
+      return lead
+    # update() is only called above CREEP_PASSTHROUGH_V (see smooth_radarstate), so _hold and the EMA state
+    # (_d/_vl/_vr) freeze for the entire duration of any full standstill. Resuming and EMA-ing a real, opening
+    # lead against that frozen _d as if no time had passed lags dRel toward the stale, closer pre-stop value
+    # for up to LEAD_SMOOTH_TAU-ish seconds -- same bug class as _JumpGuard/_LeadHold's frame-based staleness
+    # fixes. Treat a gap since the last call larger than REFERENCE_STALE_FRAMES (not a flat 1) as no state at
+    # all, so an incidental single skipped cycle from v_ego dithering right at the gate doesn't itself wipe the
+    # EMA's short-term jitter-suppression memory.
+    if self._last_frame and frame - self._last_frame > REFERENCE_STALE_FRAMES:
+      self.reset()
+    self._last_frame = frame
+    self._hold = LEAD_SMOOTH_HOLD if noisy else self._hold - 1
+    if self._hold <= 0 or not lead.status:
+      self.reset()
+      return lead
+    if self._d is None:
+      self._d, self._vl, self._vr = lead.dRel, lead.vLead, lead.vRel
+      return lead
+    a = DT_MDL / LEAD_SMOOTH_TAU
+    self._d = lead.dRel if lead.dRel < self._d else self._d + (lead.dRel - self._d) * a
+    self._vl += (lead.vLead - self._vl) * a
+    self._vr += (lead.vRel - self._vr) * a
+    return _SmoothedLead(lead, self._d, self._vl, self._vr)
+
+
+class _JumpGuard:
+  # Rejects a same-cycle FARTHER dRel jump on a lead that never dropped status (a vision/radar fusion
+  # transient, e.g. a cut-in whose vision distance estimate briefly disagrees with a solid radar track before
+  # the match locks on) by holding the last-trusted reading, extrapolated by its own vRel, for a bounded
+  # number of frames. A CLOSER jump of any size always passes through immediately -- this can only ever delay
+  # relief, never a brake -- and it self-heals after JUMP_GUARD_MAX_HOLD frames if the jump was real.
+  # modelProb is capped at FCW_PROB_CAP on the held lead (same as _LeadHold's flicker-hold, below) -- a held
+  # reading is no longer confirmed fresh, so it must not carry enough confidence to trip the stock FCW gate.
+  def __init__(self):
+    self._last = None
+    self._last_frame = 0
+    self._hold = 0
+    self._grace_used = False
+
+  def reset(self):
+    self._last = None
+    self._last_frame = 0
+    self._hold = 0
+    self._grace_used = False
+
+  def step(self, raw, frame):
+    if not raw.status:
+      self.reset()
+      return raw
+
+    # _last is only a valid reference to diff against if it's recent. smooth_radarstate() stops calling step()
+    # below LOW_SPEED_PASSTHROUGH_V (see _LeadHold), so after any low-speed gap (a full stop, a slow zone)
+    # _last can be arbitrarily many real seconds old. Diffing a fresh reading against a stale one as if it were
+    # a same-cycle transient rejects a real, large, entirely legitimate change (e.g. a lead that pulled away
+    # during the gap) as a fusion glitch and holds a phantom lead -- measured causing a hard, unwarranted brake
+    # on a real route (launch from a stop after the lead had long since moved on). Treat a stale _last as no
+    # reference at all: pass raw through and re-baseline. REFERENCE_STALE_FRAMES (not a flat 1) so an
+    # incidental single skipped cycle from v_ego dithering right at the gate doesn't itself defeat the guard.
+    stale = self._last is not None and (frame - self._last_frame) > REFERENCE_STALE_FRAMES
+
+    if not stale and self._last is not None and (raw.dRel - self._last[0]) > SWITCH_DREL and self._hold < JUMP_GUARD_MAX_HOLD:
+      dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
+      self._hold += 1
+      held_dRel = max(MIN_HELD_DREL, dRel0 - max(-vRel0, 0.0) * DT_MDL)
+      self._last = (held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, prob0)
+      self._last_frame = frame
+      return _HeldLead(held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, min(prob0, FCW_PROB_CAP))
+
+    # Hold cap reached on a lead that was closing: self-healing straight onto raw here would adopt a farther
+    # reading than the trajectory already tracked, i.e. report a farther lead than reality for at least one
+    # more cycle. Take exactly one bounded extra cycle at the last-held value first -- never a second, so this
+    # can't turn into an indefinite hold on a lead that genuinely departed.
+    if (not stale and self._hold >= JUMP_GUARD_MAX_HOLD and not self._grace_used and self._last is not None and
+        self._last[1] < 0.0 and (raw.dRel - self._last[0]) > SWITCH_DREL):
+      dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
+      self._grace_used = True
+      self._last_frame = frame
+      return _HeldLead(dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, min(prob0, FCW_PROB_CAP))
+
+    self._hold = 0
+    self._grace_used = False
+    self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb)
+    self._last_frame = frame
+    return raw
+
+
+class _LeadHold:
+  # step() takes the caller's absolute frame counter rather than counting its own calls: below
+  # LOW_SPEED_PASSTHROUGH_V the caller stops calling step() at all (see smooth_radarstate), and a
+  # self-incrementing counter would then stay frozen at whatever it was for however long that lasts -- on
+  # resume it would read as "just a few frames since the last real sighting" no matter how much real time
+  # (a full stop, a slow zone) actually passed, and could hand HOLD_MAX_FRAMES worth of stale credit to a
+  # sighting from arbitrarily long ago. Comparing against the caller's frame counter makes the elapsed-frames
+  # check correct regardless of how many cycles were skipped in between.
+  def __init__(self):
+    self._last = None
+    self._sustained = 0
+    self._real_frame = 0
+    self._armed = False
+    self._held_dRel = 0.0
+    self._holding = False   # true once this hold episode has been reseeded from a real reading
+
+  def reset(self):
+    self.__init__()
+
+  def step(self, raw, frame):
+    if raw.status and raw.dRel > DROPOUT_DREL:
+      self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb)
+      self._sustained += 1
+      if self._sustained >= SUSTAIN_FRAMES:
+        self._real_frame = frame
+        self._armed = True
+      self._holding = False   # back on a real sighting -- the next dropout starts a fresh hold episode
+      return raw
+
+    self._sustained = 0
+    since_real = frame - self._real_frame
+    if self._armed and self._last is not None and since_real <= HOLD_MAX_FRAMES:
+      dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
+      # Reseed _held_dRel from the real last-known value exactly once per hold episode, on whichever call
+      # first starts holding -- NOT on since_real==1: since_real is elapsed REAL frames (see class docstring),
+      # so any low-speed gap in between (step() skipped) makes since_real > 1 on the very first dropout call
+      # actually made, and comparing it to 1 would silently skip the reseed -- leaving _held_dRel at its
+      # stale/init value (0.0), which the next line's floor then clamps to MIN_HELD_DREL: a fabricated
+      # near-bumper phantom lead, not a dead-reckoned extrapolation of the real one.
+      if not self._holding:
+        self._held_dRel = dRel0
+        self._holding = True
+      self._held_dRel = max(MIN_HELD_DREL, self._held_dRel - max(-vRel0, 0.0) * DT_MDL)
+      return _HeldLead(self._held_dRel, vRel0, vLead0, min(aLeadK0, 0.0), aLeadTau0, min(prob0, FCW_PROB_CAP))
+
+    self._armed = False
+    self._holding = False
+    return raw
+
+
+class _LeadStability:
+  # Read-only monitor: flags an unstable leadOne -- bimodal/bouncing vLead, dRel track-switch jumps, or
+  # radarTrackId churn (a steady lead flipping track ids -> vRel jitter -> follow-hunting). Telemetry only.
+  # Also derives same_track_noise: a bimodal/bouncing vLead while radarTrackId sat CONSTANT the whole window
+  # -- i.e. the id evidence pins the noise to one physical object (a Doppler/fusion-noisy velocity read on one
+  # real lead), so it is safe to feed the smoother (see _LeadSmoother). A bimodal vLead WITH the id also
+  # changing stays outside same_track_noise (could be two really-different real objects at different speeds)
+  # and is left unmitigated, same as before. dRel track-jumps are deliberately excluded here: while status
+  # stays True (this class's own precondition), a repeated FARTHER dRel jump is already absorbed by
+  # _JumpGuard upstream (same SWITCH_DREL threshold), so adding it here would just double up on the same
+  # signal rather than covering a real gap.
+  def __init__(self):
+    self._v = deque(maxlen=STABILITY_WINDOW)
+    self._d = deque(maxlen=STABILITY_WINDOW)
+    self._id = deque(maxlen=ID_CHURN_WINDOW)
+    self.unstable = False
+    self.churn = False
+    self.same_track_noise = False
+
+  def reset(self):
+    self._v.clear()
+    self._d.clear()
+    self._id.clear()
+    self.unstable = False
+    self.churn = False
+    self.same_track_noise = False
+
+  def update(self, lead, v_ego: float) -> None:
+    if not lead.status or v_ego < CREEP_PASSTHROUGH_V:
+      self.reset()
+      return
+    self._v.append(float(lead.vLead))
+    self._d.append(float(lead.dRel))
+    self._id.append(int(getattr(lead, 'radarTrackId', -1)))
+    if len(self._v) < STABILITY_WINDOW:
+      self.unstable = False
+      return
+    v_spread = max(self._v) - min(self._v)
+    d_jumps = sum(abs(b - a) > SWITCH_DREL for a, b in zip(self._d, list(self._d)[1:], strict=False))
+    ids = list(self._id)
+    id_churn = sum(1 for a, b in zip(ids, ids[1:], strict=False) if a != b and a > 0 and b > 0)
+    recent_ids = ids[-STABILITY_WINDOW:]
+    same_track = recent_ids[0] > 0 and len(set(recent_ids)) == 1
+    self.churn = id_churn >= ID_CHURN and v_spread <= VLEAD_SPREAD   # steady lead, flipping ids (not bimodal)
+    self.same_track_noise = same_track and v_spread > VLEAD_SPREAD
+    self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2 or self.churn
+
+
+class RadarDistanceController:
+  def __init__(self, CP: structs.CarParams, params=None):
+    # CP accepted for the planner's constructor signature; unused.
+    self._params = params or Params()
+    self._frame = 0
+    self._v_ego = 0.0
+    self._enabled = self._params.get_bool("RadarDistance")
+    self._jump_guard = _JumpGuard()
+    self._one = _LeadHold()
+    self._two = _LeadHold()
+    self._stability = _LeadStability()
+    self._smoother = _LeadSmoother()
+    self._creep_frames = 0
+    self._creep_released = False
+
+  def _read_params(self) -> None:
+    enabled = self._params.get_bool("RadarDistance")
+    if not enabled and self._enabled:
+      self._jump_guard.reset()
+      self._one.reset()
+      self._two.reset()
+      self._smoother.reset()
+      self._creep_frames = 0
+      self._creep_released = False
+    self._enabled = enabled
+
+  def update(self, sm) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._read_params()
+    self._v_ego = float(sm['carState'].vEgo)
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def lead_unstable(self) -> bool:
+    return self._stability.unstable
+
+  def _stop_gap_bias(self, lead):
+    # Report a (near-)stopped lead up to STOP_GAP_M closer at low speed so the MPC's own smooth stop ends
+    # that much farther back. Monotone (only ever reports closer). No-op outside the regime.
+    in_regime = (lead.status and lead.vLead <= STOP_GAP_VLEAD and
+                 self._v_ego <= STOP_GAP_VEGO and lead.dRel > STOP_GAP_MIN_DREL)
+    if not in_regime:
+      self._creep_frames = 0
+      self._creep_released = False
+      return lead
+
+    # A held (stale) lead's near-zero vLead can still satisfy the regime check on a lead that's departed --
+    # skip biasing it, but freeze the creep latch rather than resetting it (an unrelated jump-guard glitch
+    # shouldn't undo motion the lead already earned).
+    if isinstance(lead, _HeldLead):
+      return lead
+
+    if lead.vLead > STOP_GAP_CREEP_V:
+      self._creep_frames = min(self._creep_frames + 1, STOP_GAP_CREEP_HOLD_FRAMES)
+      if self._creep_frames >= STOP_GAP_CREEP_HOLD_FRAMES:
+        self._creep_released = True
+    else:
+      # decay: a sub-threshold frame undoes one frame of "motion" credit, so only SUSTAINED motion (not
+      # cumulative noise straddling the threshold) can reach the cap.
+      self._creep_frames = max(self._creep_frames - 1, 0)
+    if self._creep_released:                          # once latched: sticky -- a single-frame near-zero blip
+      return lead                                      # afterward (e.g. sensor noise mid-creep) can't re-suppress it
+
+    d_ramp = min(max((STOP_GAP_REGIME_DREL - lead.dRel) / STOP_GAP_RAMP_BAND, 0.0), 1.0)
+    v_ramp = min(max((STOP_GAP_VLEAD - lead.vLead) / STOP_GAP_VLEAD, 0.0), 1.0)
+    offset = STOP_GAP_M * d_ramp * v_ramp
+    if offset < 0.05:
+      return lead
+    return _BiasedLead(lead, max(lead.dRel - offset, STOP_GAP_MIN_DREL))
+
+  def smooth_radarstate(self, radarstate):
+    self._frame += 1                                           # step()'s elapsed-frames basis; see _LeadHold
+    self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry, runs every cycle
+    if not self._enabled:
+      return radarstate                                       # off: byte-stock passthrough
+    two = radarstate.leadTwo
+    noisy = self._stability.churn or self._stability.same_track_noise
+    if self._v_ego >= LOW_SPEED_PASSTHROUGH_V:
+      one = self._jump_guard.step(radarstate.leadOne, self._frame)  # reject a same-cycle farther-jump transient ...
+      one = self._one.step(one, self._frame)                  # ... + flicker-hold ...
+      two = self._two.step(radarstate.leadTwo, self._frame)
+      one = self._smoother.update(one, noisy, self._frame)     # ... + same-object de-jitter (anti follow-hunt)
+    elif self._v_ego >= CREEP_PASSTHROUGH_V:
+      # creep band: de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch)
+      one = self._smoother.update(radarstate.leadOne, noisy, self._frame)
+    else:
+      one = radarstate.leadOne                                # full standstill: no hold/smoothing
+    one = self._stop_gap_bias(one)                            # low-speed near-stopped: settle farther back
+    if one is radarstate.leadOne and two is radarstate.leadTwo:
+      return radarstate                                       # nothing changed -> byte-stock object
+    return _RadarStateProxy(one, two)

sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py

@@ -0,0 +1,801 @@
+"""
+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.
+
+RadarDistance is a pure lead DE-NOISER: flicker-hold + churn smoother + instability telemetry, and nothing
+else (no dRel biasing). These tests pin: off / low-speed == byte-stock (stock stop distance); the hold is
+obstacle-monotone (brake >= stock) and bounded; the churn smoother de-jitters only a track-flipping lead;
+and the instability flag is telemetry that runs regardless of the gate.
+"""
+
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
+  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, CREEP_PASSTHROUGH_V, \
+  DROPOUT_DREL, STOP_GAP_MIN_DREL, STOP_GAP_VEGO, STOP_GAP_VLEAD, STOP_GAP_REGIME_DREL, SWITCH_DREL, \
+  JUMP_GUARD_MAX_HOLD, STOP_GAP_CREEP_V, STOP_GAP_CREEP_HOLD_FRAMES
+
+COMFORT_BRAKE = 2.5
+
+
+class FakeParams:
+  def __init__(self, store=None):
+    self.store = dict(store or {})
+
+  def get_bool(self, key):
+    return bool(self.store.get(key, False))
+
+
+def lead(status=True, dRel=40.0, vRel=-2.0, vLead=18.0, aLeadK=0.0, aLeadTau=1.5, modelProb=0.95, radarTrackId=-1):
+  return SimpleNamespace(status=status, dRel=dRel, yRel=0.0, vRel=vRel, vLead=vLead, vLeadK=vLead,
+                         aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb, radarTrackId=radarTrackId)
+
+
+def rs(one, two=None):
+  return SimpleNamespace(leadOne=one, leadTwo=two or lead(status=False, dRel=0.0, modelProb=0.0))
+
+
+def obstacle(ld):
+  return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE)
+
+
+def ctrl(enabled=True, v_ego=10.0):
+  c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
+  c._v_ego = v_ego   # above the low-speed gate so the hold + smoother run
+  return c
+
+
+def churn_frames(n, d_a=40.0, d_b=42.0, vLead=18.0):
+  # a steady lead whose radarTrackId flips every frame (dRel jitters with it) -> the churn detector fires and
+  # the smoother should de-jitter dRel. vLead is steady so it is NOT flagged bimodal (never averages 2 tracks).
+  for i in range(n):
+    even = i % 2 == 0
+    yield lead(dRel=d_a if even else d_b, vLead=vLead, vRel=-1.0, radarTrackId=1 if even else 2)
+
+
+# --- off / low-speed == byte-stock ------------------------------------------------------------------------
+
+def test_disabled_is_identity():
+  c = ctrl(enabled=False)
+  r = rs(lead())
+  assert c.smooth_radarstate(r) is r                 # byte-stock passthrough
+
+
+def test_valid_lead_passthrough():
+  c = ctrl()
+  one = lead(dRel=40.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one                          # clean lead, no churn -> unchanged
+
+
+def test_full_standstill_outside_stopgap_is_passthrough():
+  # Full standstill (< CREEP_PASSTHROUGH_V), lead OUTSIDE the stop-gap regime (far): no hold, no smoothing,
+  # no bias -> the EXACT raw radarstate object (byte-identical). The stop-gap only engages inside its regime.
+  c = ctrl(v_ego=CREEP_PASSTHROUGH_V - 0.5)
+  r = rs(lead(dRel=STOP_GAP_REGIME_DREL + 8.0, vLead=0.5))
+  assert c.smooth_radarstate(r) is r
+
+
+def test_creep_dejitters_churn_but_no_hold():
+  # Creep band [CREEP, LOW_SPEED): the churn smoother runs (de-jitter -> smooth stop-and-go), but the
+  # flicker-hold does NOT (a dropped/departed lead must not be held, or launch would be delayed).
+  # vLead>STOP_GAP_VLEAD so the stop-gap stays out and this isolates the EMA.
+  c = ctrl(v_ego=(CREEP_PASSTHROUGH_V + LOW_SPEED_PASSTHROUGH_V) / 2)
+  out = None
+  for f in churn_frames(30, d_a=6.0, d_b=8.0, vLead=3.0):
+    out = c.smooth_radarstate(rs(f))
+  assert 6.0 < out.leadOne.dRel < 8.0                # jitter smoothed
+  # a dropout in the creep band is NOT held -> raw passes through (no stale lead)
+  drop = rs(lead(status=False, dRel=0.0, modelProb=0.0))
+  assert c.smooth_radarstate(drop) is drop
+
+
+def test_creep_clean_lead_passthrough():
+  # creep band, steady moving lead (no churn, outside stop-gap regime) -> exact raw object (unbiased)
+  c = ctrl(v_ego=(CREEP_PASSTHROUGH_V + LOW_SPEED_PASSTHROUGH_V) / 2)
+  r = rs(lead(dRel=4.0, vLead=2.5, radarTrackId=3))
+  assert c.smooth_radarstate(r) is r
+
+
+# --- stop-gap (settle farther back from a near-stopped lead) ----------------------------------------------
+
+def test_stop_gap_pulls_stopped_lead_closer():
+  c = ctrl(v_ego=2.0)
+  one = lead(dRel=6.0, vLead=0.0, vRel=-1.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne.dRel < 6.0                      # reported closer -> MPC stops farther back
+  assert obstacle(out.leadOne) <= obstacle(one) + 1e-6   # brake >= stock (obstacle never farther)
+
+
+def test_stop_gap_monotone_never_farther():
+  c = ctrl(v_ego=3.0)
+  for d in (4.0, 6.0, 9.0, 11.0):
+    out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.0)))
+    assert out.leadOne.dRel <= d + 1e-6
+
+
+def test_stop_gap_min_floor():
+  c = ctrl(v_ego=2.0)
+  out = c.smooth_radarstate(rs(lead(dRel=STOP_GAP_MIN_DREL + 0.5, vLead=0.0)))
+  assert out.leadOne.dRel >= STOP_GAP_MIN_DREL - 1e-6
+
+
+def test_stop_gap_off_when_disabled():
+  c = ctrl(enabled=False, v_ego=2.0)
+  r = rs(lead(dRel=6.0, vLead=0.0))
+  assert c.smooth_radarstate(r) is r                 # disabled -> stock stop distance
+
+
+def test_stop_gap_moving_lead_no_change():
+  c = ctrl(v_ego=2.0)
+  out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=STOP_GAP_VLEAD + 1.0)))
+  assert out.leadOne.dRel == pytest.approx(6.0)      # lead moving -> not a stop
+
+
+def test_stop_gap_high_speed_no_change():
+  c = ctrl(v_ego=STOP_GAP_VEGO + 2.0)
+  out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=0.0)))
+  assert out.leadOne.dRel == pytest.approx(6.0)      # above the stop regime -> unbiased
+
+
+def test_stop_gap_far_lead_no_change():
+  c = ctrl(v_ego=2.0)
+  d = STOP_GAP_REGIME_DREL + 5.0
+  out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.0)))
+  assert out.leadOne.dRel == pytest.approx(d)        # beyond the ramp-in regime -> unbiased
+
+
+# --- stop-gap creep override (sustained lead motion releases the bias even below STOP_GAP_VLEAD) ----------
+
+def test_stop_gap_creep_releases_after_sustained_motion():
+  # route 550a71ee4c7a7fbe/000004a4--c9c4691959, t~1126-1138: a lead crept forward at 0.3-0.6 m/s (well under
+  # STOP_GAP_VLEAD) for 9+ seconds; without the override the bias suppressed the whole real gap growth the
+  # entire time, producing a 9+ second launch delay. Real dRel grows steadily 4.0 -> 4.5m over this window.
+  c = ctrl(v_ego=0.0)
+  d = 4.0
+  out = None
+  for i in range(STOP_GAP_CREEP_HOLD_FRAMES + 5):
+    d += 0.05
+    out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.4)))
+    if i < STOP_GAP_CREEP_HOLD_FRAMES - 1:
+      assert out.leadOne.dRel < d - 1e-6                # still suppressed while creep hasn't sustained yet
+  assert out.leadOne.dRel == pytest.approx(d)            # released after STOP_GAP_CREEP_HOLD_FRAMES of motion
+
+
+def test_stop_gap_creep_release_is_sticky_through_a_momentary_zero_blip():
+  # A single-frame return to exactly 0.0 (real sensor behavior mid-creep, not just noise) must not re-arm the
+  # bias once sustained creep has already released it -- this was the actual bug: the real route's lead
+  # dipped to vLead=0.00 for one frame mid-launch and the bias briefly re-suppressed the gap right as a result.
+  c = ctrl(v_ego=0.0)
+  d = 4.0
+  for _ in range(STOP_GAP_CREEP_HOLD_FRAMES):
+    d += 0.05
+    c.smooth_radarstate(rs(lead(dRel=d, vLead=0.4)))
+  d += 0.05
+  out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.0)))  # exact-zero blip
+  assert out.leadOne.dRel == pytest.approx(d)             # still released, not re-suppressed
+  d += 0.05
+  out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.3)))  # creep resumes
+  assert out.leadOne.dRel == pytest.approx(d)
+
+
+def test_stop_gap_creep_counter_resets_on_a_genuine_new_stop():
+  # Leaving the bias regime entirely (lead departs, or ego speeds past STOP_GAP_VEGO) must re-arm the
+  # override so a LATER, unrelated near-stop encounter isn't permanently exempted by a stale latch.
+  c = ctrl(v_ego=0.0)
+  d = 4.0
+  for _ in range(STOP_GAP_CREEP_HOLD_FRAMES):
+    d += 0.05
+    c.smooth_radarstate(rs(lead(dRel=d, vLead=0.4)))
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))   # lead lost -> regime exit
+  out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=0.0)))               # fresh near-stop encounter
+  assert out.leadOne.dRel < 6.0                                          # bias re-armed, active again
+
+
+def test_stop_gap_creep_below_threshold_never_releases():
+  c = ctrl(v_ego=0.0)
+  out = None
+  for _ in range(STOP_GAP_CREEP_HOLD_FRAMES + 10):
+    out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=STOP_GAP_CREEP_V * 0.5)))
+  assert out.leadOne.dRel < 6.0 - 1e-6                # vLead never exceeds the creep threshold -> stays biased
+
+
+def test_stop_gap_creep_intermittent_noise_never_releases():
+  # route 550a71ee4c7a7fbe/000004b6--d4a8ac3352, t~678-690s: a genuinely-stopped lead's vLead noise blipped
+  # above STOP_GAP_CREEP_V on roughly half the frames (never below zero motion overall, never sustained) --
+  # the old monotonic-only counter still accumulated those blips to the cap over enough frames and falsely
+  # latched the bias off mid-stop, producing a same-cycle jump in the reported gap. Alternating strictly
+  # above/below the threshold every frame, for far longer than STOP_GAP_CREEP_HOLD_FRAMES, must never release.
+  c = ctrl(v_ego=0.0)
+  out = None
+  for i in range(STOP_GAP_CREEP_HOLD_FRAMES * 4):
+    vLead = STOP_GAP_CREEP_V + 0.05 if i % 2 == 0 else 0.0
+    out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=vLead)))
+  assert out.leadOne.dRel < 6.0 - 1e-6                # never sustained -> never releases, even after 4x the hold
+
+
+def test_stop_gap_creep_sustained_after_intermittent_noise_still_releases():
+  # the decay must not make the override permanently harder to reach -- real sustained motion right after a
+  # noisy patch still releases within the normal HOLD window (decay only undoes noise, doesn't add a penalty).
+  c = ctrl(v_ego=0.0)
+  for i in range(STOP_GAP_CREEP_HOLD_FRAMES):
+    vLead = STOP_GAP_CREEP_V + 0.05 if i % 2 == 0 else 0.0
+    c.smooth_radarstate(rs(lead(dRel=6.0, vLead=vLead)))
+  out = None
+  for _ in range(STOP_GAP_CREEP_HOLD_FRAMES + 5):
+    out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=STOP_GAP_CREEP_V + 0.05)))
+  assert out.leadOne.dRel == pytest.approx(6.0)       # sustained motion still releases in bounded time
+
+
+def test_stop_gap_does_not_double_bias_a_jump_guard_held_lead():
+  # route 550a71ee4c7a7fbe/000004bc--d9e0efd5ac, t~1563.4-1563.9: a lead sitting near-stopped (vLead~0,
+  # inside the stop-gap regime) departs fast enough that a single raw dRel jump exceeds jump-guard's
+  # SWITCH_DREL. Jump-guard holds its OLD, stale (near-zero vLead0) reading rather than the new, fast, real
+  # one -- and that held vLead0 STILL satisfies the stop-gap regime check (it looks near-stopped), so without
+  # the skip, stop-gap piles a SECOND closer-bias on top of an already-stale value. On the real route this
+  # compounding dropped the reported gap far enough (raw ~16m -> ~2-5m) to fool the MPC's own forward-solve
+  # into a spurious FCW during a real launch. Fixed: stop-gap must skip a held (stale) lead entirely.
+  v_ego = (LOW_SPEED_PASSTHROUGH_V + STOP_GAP_VEGO) / 2   # in-band for jump-guard hold AND stop-gap regime
+  c = ctrl(v_ego=v_ego)
+  dRel0, vRel0, vLead0 = 7.5, -0.1, 0.2                  # near-stopped baseline, trusted
+  c.smooth_radarstate(rs(lead(dRel=dRel0, vRel=vRel0, vLead=vLead0)))
+  jumped = c.smooth_radarstate(rs(lead(dRel=dRel0 + SWITCH_DREL + 1.0, vRel=6.5, vLead=6.8)))  # real, fast departure
+  expected_held = dRel0 - max(-vRel0, 0.0) * DT_MDL      # jump-guard's own extrapolation -- no further bias
+  assert jumped.leadOne.dRel == pytest.approx(expected_held, abs=1e-6)
+
+
+def test_smoother_does_not_launder_a_jump_guard_hold_during_churn():
+  # A churn episode (real radarTrackId flapping, steady kinematics) actively engaging the smoother right as a
+  # jump-guard hold begins must not let the smoother wrap the held lead into a _SmoothedLead -- that would hide
+  # "this is stale" from stop-gap's held-lead check and let a second closer-bias stack on top.
+  v_ego = (LOW_SPEED_PASSTHROUGH_V + STOP_GAP_VEGO) / 2   # in-band for jump-guard hold AND stop-gap regime
+  c = ctrl(v_ego=v_ego)
+  d = 9.0
+  for i in range(8):
+    tid = 1 if i % 2 == 0 else 2
+    c.smooth_radarstate(rs(lead(dRel=d, vRel=-6.3, vLead=0.2, radarTrackId=tid)))
+    dRel0 = d
+    d -= 0.15
+  assert c.lead_unstable()                               # churn primed the smoother
+  vRel0 = -6.3
+  jumped = c.smooth_radarstate(rs(lead(dRel=dRel0 + SWITCH_DREL + 1.0, vRel=6.5, vLead=6.8, radarTrackId=1)))
+  expected_held = dRel0 - max(-vRel0, 0.0) * DT_MDL
+  assert type(jumped.leadOne).__name__ == '_HeldLead'
+  assert jumped.leadOne.dRel == pytest.approx(expected_held, abs=1e-6)
+
+
+def test_stop_gap_creep_latch_survives_an_unrelated_jump_guard_hold():
+  # A creep-release already earned (sustained real motion) must not be wiped by an unrelated jump-guard hold
+  # that happens to land on the same lead -- the hold is a one-cycle fusion transient, not evidence the lead
+  # stopped moving again.
+  v_ego = (LOW_SPEED_PASSTHROUGH_V + STOP_GAP_VEGO) / 2   # in-band for jump-guard hold AND stop-gap regime
+  c = ctrl(v_ego=v_ego)
+  d = 4.0
+  for _ in range(STOP_GAP_CREEP_HOLD_FRAMES + 2):
+    d += 0.05
+    c.smooth_radarstate(rs(lead(dRel=d, vRel=-6.0, vLead=0.4)))
+  assert c._creep_released
+  dRel0 = d
+  held = c.smooth_radarstate(rs(lead(dRel=dRel0 + SWITCH_DREL + 1.0, vRel=6.5, vLead=6.8)))
+  assert type(held.leadOne).__name__ == '_HeldLead'       # bias correctly skipped on the held cycle
+  assert c._creep_released                                # but the earned latch must survive the glitch
+  d += 0.05
+  out = c.smooth_radarstate(rs(lead(dRel=d, vRel=-0.4, vLead=0.4)))
+  assert out.leadOne.dRel == pytest.approx(d)              # creep resumes unbiased, no re-suppression
+
+
+def test_low_speed_override_lead_passthrough():
+  # radard low_speed_override emits a real closest-track lead with modelProb=0.0. It must be honored, not
+  # rejected in favor of a stale farther held lead (which would under-brake / stop too close).
+  c = ctrl()
+  one = lead(status=True, dRel=2.5, vRel=0.0, vLead=0.0, modelProb=0.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one
+
+
+# --- jump-guard (reject a same-cycle farther fusion transient) --------------------------------------------
+
+def test_jump_guard_holds_farther_transient():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=27.92, vRel=-5.60, vLead=24.47, radarTrackId=1058)))
+  out = c.smooth_radarstate(rs(lead(dRel=38.88, vRel=-3.19, vLead=26.91, radarTrackId=-1)))
+  assert out.leadOne.dRel < 30.0                     # farther jump rejected, held near the trusted value
+  assert out.leadOne.status is True
+
+
+def test_jump_guard_passes_closer_jump_immediately():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-2.0, vLead=18.0)))
+  out = c.smooth_radarstate(rs(lead(dRel=27.0, vRel=-5.0, vLead=15.0)))   # big CLOSER jump
+  assert out.leadOne.dRel == pytest.approx(27.0)     # closer always passes through -- never delays a brake
+
+
+def test_jump_guard_replays_real_route_whiplash():
+  # route 550a71ee4c7a7fbe/00000498--0704864d6a, t~402.2-402.8: a merging lead's vision distance estimate
+  # whiplashed 27.92 -> 38.88 -> 37.69 -> 37.20 -> 26.84 for ~0.3s while a solid radar track sat at ~27m the
+  # whole time. The guard should smooth the farther excursion into a monotone converge toward the real value.
+  c = ctrl()
+  raw = [
+    (74.18, -4.05, 25.77, -1), (53.21, -3.55, 26.33, -1), (47.42, -3.23, 26.67, -1),
+    (42.64, -3.50, 26.42, -1), (43.22, -3.49, 26.49, -1), (40.03, -3.04, 26.96, -1),
+    (39.50, -3.29, 26.74, -1), (27.92, -5.60, 24.47, 1058), (38.88, -3.19, 26.91, -1),
+    (37.69, -3.09, 27.04, -1), (37.20, -2.77, 27.39, -1), (26.84, -5.80, 24.37, 1058),
+  ]
+  out = None
+  for dRel, vRel, vLead, tid in raw:
+    out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=vLead, radarTrackId=tid)))
+  assert out.leadOne.dRel == pytest.approx(26.84)    # real value recovered exactly once raw resumes reporting it
+  # peak reported dRel during the excursion never revisits the raw 38.88 spike
+  seen = []
+  c = ctrl()
+  for dRel, vRel, vLead, tid in raw:
+    seen.append(c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=vLead, radarTrackId=tid))).leadOne.dRel)
+  assert max(seen[8:11]) < 30.0                      # the 3 farther-jump frames are all held near ~27m, not ~37-39m
+
+
+def test_jump_guard_self_heals_after_cap():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0)))
+  for _ in range(JUMP_GUARD_MAX_HOLD):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
+    assert out.leadOne.dRel < 40.0                   # held while under the cap
+  # cap just reached on a lead that was closing -- one bounded grace cycle before accepting a farther raw value
+  out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
+  assert out.leadOne.dRel < 40.0                     # grace cycle: still held, not yet accepted
+  out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
+  assert out.leadOne.dRel == pytest.approx(40.0)     # grace spent -> accepts the real (departing) value
+
+
+def test_jump_guard_self_heals_immediately_when_not_closing():
+  # The grace cycle only protects a lead that was closing when the cap was hit -- a lead that was already
+  # steady/opening (vRel >= 0) self-heals on the very first cap-exceeding frame, same as before this fix.
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=20.0, vRel=0.5, vLead=19.0)))
+  for _ in range(JUMP_GUARD_MAX_HOLD):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=0.5, vLead=19.0)))
+    assert out.leadOne.dRel < 40.0
+  out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=0.5, vLead=19.0)))
+  assert out.leadOne.dRel == pytest.approx(40.0)     # no grace needed -> heals immediately, unchanged behavior
+
+
+def test_jump_guard_grace_is_used_at_most_once_per_hold_episode():
+  # The grace cycle must be bounded -- a lead that keeps reading farther after the grace is spent must not
+  # get a second grace before genuinely accepting the new value (else a departed lead could be held forever).
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0)))
+  for _ in range(JUMP_GUARD_MAX_HOLD):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
+  c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))    # grace cycle, spent
+  out = c.smooth_radarstate(rs(lead(dRel=70.0, vRel=-1.0, vLead=19.0)))
+  assert out.leadOne.dRel == pytest.approx(70.0)     # grace already spent this episode -> accepts immediately
+
+
+def test_jump_guard_replays_real_route_dropout_catchup():
+  # route 550a71ee4c7a7fbe/000004c6--ed1b6d7f95, t~1337.9-1338.5: a spurious closer misread (31.08 -> 24.94)
+  # passes through immediately (closer always does), poisoning the guard's anchor. The lead's real, continuing
+  # trajectory (~31m, closing) then reads as a farther jump against that bad anchor and gets held for the full
+  # cap. Without the grace cycle, the guard self-healed straight onto a farther transitional misread (56.52)
+  # right as a real dropout began, and _LeadHold then flicker-held THAT value through the whole dropout --
+  # reporting a lead ~2x farther and opening instead of closing, easing the MPC off right before a real
+  # catch-up brake. The grace cycle must keep the held value close to the real trajectory across this handoff.
+  c = ctrl(v_ego=14.4)
+  raw = [
+    (30.62, -0.45, 1), (38.36, -3.33, -1), (38.20, -3.42, -1), (38.08, -3.45, -1), (37.88, -3.53, -1),
+    (37.72, -3.58, -1), (47.53, 0.35, -1), (24.94, -1.90, -1), (31.44, -3.72, -1), (31.20, -3.97, -1),
+    (31.08, -3.95, 2), (74.32, 3.65, 3), (74.52, 3.70, 3), (74.92, 3.85, 3), (75.12, 3.88, 3),
+    (75.28, 3.90, 3), (75.64, 3.95, 3), (75.64, 3.95, 3), (56.52, -2.09, -1),
+  ]
+  out = None
+  for dRel, vRel, tid in raw:
+    out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=10.5, radarTrackId=tid)))
+  assert out.leadOne.dRel < 30.0                     # grace cycle: still held near the real trajectory
+  dropout_held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert dropout_held.status is True
+  assert dropout_held.dRel < 30.0                    # flicker-hold seeds from the grace-held value, not 56.52
+
+
+def test_jump_guard_resets_on_dropout():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0)))
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
+  assert out.leadOne.dRel == pytest.approx(40.0)     # a real dropout in between is not a same-cycle jump
+
+
+def test_jump_guard_off_when_disabled():
+  c = ctrl(enabled=False)
+  c.smooth_radarstate(rs(lead(dRel=27.92, vRel=-5.60, vLead=24.47)))
+  r = rs(lead(dRel=38.88, vRel=-3.19, vLead=26.91))
+  assert c.smooth_radarstate(r) is r                 # disabled -> raw passthrough, no guard
+
+
+def test_jump_guard_replays_real_route_sub_threshold_bounce():
+  # route 550a71ee4c7a7fbe/000004b4--2bd66184db, t~976.08-976.48: dRel bounced 17.70 -> 12.32 -> ... -> 17.15
+  # -> 12.04m across ~0.4s while vRel stayed -0.8 to -2.4 m/s -- physically impossible for one real object
+  # at that closing speed (5m in ~0.1s would need ~50 m/s, not ~1-2). This is the case that motivated
+  # lowering SWITCH_DREL from 8.0 to 4.0: the farther excursion (12.24 -> 17.15, a 4.91m jump) sailed through
+  # unguarded at the old threshold, producing a false-relief-then-correction whipsaw. A closer jump (e.g.
+  # 17.70 -> 12.32) always passes immediately regardless of threshold -- that invariant is untouched here.
+  c = ctrl()
+  raw = [
+    (19.12, -2.32, 9.22, -0.67, -1), (17.95, -2.06, 9.39, -0.58, -1), (18.06, -1.90, 9.49, -0.60, -1),
+    (17.70, -1.84, 9.44, -0.52, -1), (12.32, -1.20, 10.01, -0.02, 2449), (12.12, -1.40, 9.75, -1.60, 2427),
+    (12.56, -1.20, 9.87, -1.45, 2427), (12.24, -1.05, 9.92, -1.29, 2427), (17.15, -2.39, 8.53, -0.85, -1),
+    (12.04, -0.82, 10.02, -0.97, 2427), (12.04, -0.82, 9.94, -0.85, 2427), (11.80, -0.85, 9.81, -0.78, 2427),
+  ]
+  out = None
+  seen = []
+  for dRel, vRel, vLead, aLeadK, tid in raw:
+    out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=vLead, aLeadK=aLeadK, radarTrackId=tid)))
+    seen.append(out.leadOne.dRel)
+  assert seen[4] == pytest.approx(12.32)          # the initial closer jump (17.70->12.32) passes immediately
+  assert seen[8] < 14.0                            # the 12.24->17.15 farther excursion is held, not passed
+  assert out.leadOne.dRel == pytest.approx(11.80)  # recovers exactly once raw resumes reporting close values
+
+
+def test_jump_guard_hold_caps_model_prob_for_fcw():
+  # route 550a71ee4c7a7fbe/000004bc--d9e0efd5ac, t~1563.5: a real, high-confidence (modelProb 0.999) lead
+  # departs and gets held near-stationary by the guard mid-launch. The stock crash_cnt FCW gate fires on
+  # radarState.leadOne.modelProb > 0.9 -- a held (stale, no longer confirmed-fresh) reading must not carry
+  # enough confidence on its own to satisfy that gate, matching _LeadHold's existing flicker-hold cap.
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=15.72, vRel=1.80, vLead=6.79, modelProb=0.999)))
+  held = c.smooth_radarstate(rs(lead(dRel=15.72 + SWITCH_DREL + 1.0, vRel=6.5, vLead=6.8, modelProb=0.999))).leadOne
+  assert held.modelProb <= FCW_PROB_CAP
+
+
+def test_jump_guard_boundary_not_triggered():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0, vLead=18.0)))
+  out = c.smooth_radarstate(rs(lead(dRel=30.0 + SWITCH_DREL - 0.1, vRel=-2.0, vLead=18.0)))
+  assert out.leadOne.dRel == pytest.approx(30.0 + SWITCH_DREL - 0.1)   # under threshold -> passes through
+
+
+def test_jump_guard_does_not_hold_a_stale_reference_after_an_extended_low_speed_gap():
+  # Mirrors _LeadHold's identical bug (see test_hold_does_not_resurrect_a_stale_lead_after_an_extended_low_speed_gap):
+  # _jump_guard.step() is also only called above LOW_SPEED_PASSTHROUGH_V, so its _last reference used to
+  # freeze for the entire duration of any low-speed period with no elapsed-time awareness. Route
+  # 550a71ee4c7a7fbe/000004dc--c8c0867520, t~407.1: a lead tracked at dRel=11.72 while decelerating into a
+  # stop froze there through a ~160s standstill. On relaunch the real lead (now dRel=23.16, opening) was
+  # diffed against that 160s-stale reference as if it were a same-cycle transient, held as a fabricated
+  # closing lead, and fed the MPC a phantom near-collision course that produced a real, unwarranted hard
+  # brake on a real drive.
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  c.smooth_radarstate(rs(lead(dRel=11.72, vRel=-2.33, vLead=2.72)))    # last reading before decelerating to a stop
+  c._v_ego = CREEP_PASSTHROUGH_V - 0.5                                 # full stop: jump-guard.step() never called
+  for _ in range(HOLD_MAX_FRAMES * 10):                                # far longer than any hold cap -- a real gap
+    c.smooth_radarstate(rs(lead(dRel=3.68, vRel=0.0, vLead=0.0)))     # the lead is stopped just ahead
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0                             # relaunch: real lead now far + opening
+  out = c.smooth_radarstate(rs(lead(dRel=23.16, vRel=4.72, vLead=9.77))).leadOne
+  assert out.dRel == pytest.approx(23.16)             # fresh reading passes through, not held as a phantom jump
+  assert out.vRel == pytest.approx(4.72)
+
+
+def test_jump_guard_survives_a_single_incidental_gap_without_losing_protection():
+  # Regression: a flat ">1 frame gap -> stale" threshold discarded the trusted reference (and so skipped the
+  # SWITCH_DREL check entirely) after ANY single skipped call -- not just a real multi-second stop. That
+  # happens on every cycle of ordinary v_ego dithering right at LOW_SPEED_PASSTHROUGH_V (a car crawling near
+  # 5 m/s in stop-and-go traffic), permanently voiding the same-cycle fusion-transient guard for as long as the
+  # dithering continues and letting a real glitch straight through. A single one-frame dip (a momentary v_ego
+  # dip below the gate, then immediately back above it) must NOT be treated as stale -- the original farther-
+  # jump rejection must still fire right after it, same as it would with no gap at all.
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0, vLead=18.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0             # one incidental dip below the gate
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0             # immediately back above it
+  glitch = rs(lead(dRel=30.0 + SWITCH_DREL + 1.0, vRel=6.0, vLead=20.0))   # same-cycle fusion transient
+  out = c.smooth_radarstate(glitch).leadOne
+  assert out.dRel < 32.0                               # still held near ~30, glitch did not leak through
+
+
+# --- flicker-hold -----------------------------------------------------------------------------------------
+
+def test_holds_after_sustained_dropout():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)))
+  held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert held.status is True
+  assert held.dRel < 30.0                            # dead-reckoned closer
+  assert held.dRel == pytest.approx(30.0 - 4.0 * 0.05, abs=1e-6)
+
+
+def test_no_hold_without_sustained_lead():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=30.0)))           # single frame < SUSTAIN_FRAMES
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is False                 # no hold armed
+
+
+def test_hold_does_not_resurrect_a_stale_lead_after_an_extended_low_speed_gap():
+  # Below LOW_SPEED_PASSTHROUGH_V the hold is never stepped at all (see smooth_radarstate), so an elapsed-
+  # frames check must be based on real cycles, not "cycles since step() was last called" -- otherwise resuming
+  # above the gate looks like no time passed no matter how long the low-speed period actually was, and a hold
+  # armed on a real lead long before the gap can resurrect as if it were still fresh.
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-3.0, vLead=5.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0             # below the gate: step() stops being called on the hold
+  for _ in range(HOLD_MAX_FRAMES * 3):
+    c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0             # back above the gate, lead still gone
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is False                  # must not resurrect the old hold
+
+
+def test_hold_survives_a_brief_low_speed_dip_within_the_cap():
+  # A short dip below the gate (well under HOLD_MAX_FRAMES real cycles) is the case flicker-hold exists for --
+  # it must still bridge, same as a same-speed dropout of the same real duration would. dRel must stay close
+  # to the real last-known value (a genuine dead-reckoned extrapolation) -- NOT collapse toward MIN_HELD_DREL,
+  # which is what a broken reseed would produce (see test_hold_reseeds_correctly_after_any_low_speed_gap).
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-3.0, vLead=5.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V - 1.0
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is True
+  assert out.leadOne.dRel < 30.0
+  assert out.leadOne.dRel > 29.0                     # dead-reckoned from 30.0, not collapsed to MIN_HELD_DREL
+
+
+def test_hold_reseeds_correctly_after_any_low_speed_gap():
+  # Regression: _held_dRel used to be reseeded to the real last-known value only when since_real == 1. Once
+  # since_real became elapsed-REAL-frames (not a self-incrementing call counter), any skipped low-speed frame
+  # made since_real > 1 on the very first dropout call actually made, silently skipping the reseed -- leaving
+  # _held_dRel at its stale/init value (0.0), which the very next line's floor clamps to MIN_HELD_DREL: a
+  # fabricated near-bumper phantom lead fed straight to the MPC, not a dead-reckoned extrapolation of the real
+  # one. A single one-frame low-speed dip (the shortest possible gap, ~0.05s) is enough to trigger it.
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=50.0, vRel=-3.0, vLead=15.0)))
+  c._v_ego = CREEP_PASSTHROUGH_V - 0.5
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))   # exactly ONE low-speed frame
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert out.status is True
+  assert out.dRel > 45.0                             # must be near the real ~50m, not a fabricated 0.5m
+
+
+def test_releases_after_hold_cap():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0)))
+  drop = rs(lead(status=False, dRel=0.0, modelProb=0.0))
+  for _ in range(HOLD_MAX_FRAMES):
+    assert c.smooth_radarstate(drop).leadOne.status is True
+  assert c.smooth_radarstate(drop).leadOne.status is False   # released after the cap
+
+
+def test_obstacle_monotone_during_hold():
+  c = ctrl()
+  for _ in range(3):
+    real = lead(dRel=30.0, vRel=-3.0, vLead=15.0)
+    c.smooth_radarstate(rs(real))
+  base = obstacle(real)
+  drop = rs(lead(status=False, dRel=0.0, modelProb=0.0))
+  prev = base
+  for _ in range(HOLD_MAX_FRAMES):
+    held = c.smooth_radarstate(drop).leadOne
+    assert obstacle(held) <= prev + 1e-6             # never reports a farther obstacle -> brake >= stock
+    prev = obstacle(held)
+
+
+def test_fcw_prob_capped_and_aleadk_not_positive():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, aLeadK=1.5, modelProb=0.99)))
+  held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert held.modelProb <= FCW_PROB_CAP
+  assert held.aLeadK <= 0.0
+
+
+def test_flicker_does_not_reset_wall_clock():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0)))
+  # alternating drop/reacquire must not refill the hold budget: after > HOLD_MAX_FRAMES wall time it releases
+  for i in range(HOLD_MAX_FRAMES + 4):
+    frame = rs(lead(status=False, dRel=0.0, modelProb=0.0)) if i % 2 else rs(lead(dRel=0.5))  # dRel<=DROPOUT: not real
+    c.smooth_radarstate(frame)
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is False
+  assert DROPOUT_DREL == 1.0
+
+
+# --- churn smoother ---------------------------------------------------------------------------------------
+
+def test_churn_smoother_removes_jitter():
+  c = ctrl()
+  out = None
+  for f in churn_frames(30):
+    out = c.smooth_radarstate(rs(f))
+  assert c.lead_unstable()                           # churn detected
+  assert 40.0 < out.leadOne.dRel < 42.0              # EMA settled between the two jittering tracks
+  assert out.leadOne.dRel not in (40.0, 42.0)        # not the raw alternating value
+
+
+def test_churn_smoother_off_when_disabled():
+  c = ctrl(enabled=False)
+  out = None
+  for f in churn_frames(30):
+    r = rs(f)
+    out = c.smooth_radarstate(r)
+    assert out is r                                  # disabled -> raw passthrough, no smoothing
+
+
+def test_smoother_inactive_without_churn():
+  c = ctrl()
+  one = lead(dRel=40.0, radarTrackId=7)
+  for _ in range(10):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0, radarTrackId=7)))
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one                          # steady id -> no churn -> exact passthrough
+
+
+def test_churn_smoother_closer_accepted_immediately():
+  # A steadily-closing lead that also briefly churns must never be held farther than the current raw value --
+  # otherwise the EMA lags a real closing lead for the whole LEAD_SMOOTH_HOLD window, then snaps (a false
+  # relief followed by a hard catch-up brake -- route 550a71ee4c7a7fbe/00000499, t~1387, real regression).
+  c = ctrl()
+  d = 82.0
+  for i in range(40):
+    tid = 1 if i % 3 else 2   # enough id-churn to keep the smoother engaged
+    out = c.smooth_radarstate(rs(lead(dRel=d, vRel=-6.0, vLead=24.0, radarTrackId=tid)))
+    assert out.leadOne.dRel <= d + 1e-6                # never farther than the latest raw reading
+    d -= 0.4                                            # steadily closing
+
+
+def test_churn_smoother_replays_real_route_late_acquisition():
+  # route 550a71ee4c7a7fbe/00000499--7f57e1d000, t~1386.9-1388.4: radard toggles between two real candidate
+  # tracks (id 4611 ~110m, id 4609 ~82m closing) while acquiring, then a couple of vision-fallback frames
+  # (id -1) report ~104-109m mid-acquisition. The real dRel (track 4609) closes smoothly 82.0 -> 73.2m the
+  # whole time. Old symmetric EMA held the reported dRel near ~82m (farther than truth) for ~1s after the
+  # brief churn window, then snapped -- this is the false-relief-then-correction pattern being fixed here.
+  c = ctrl()
+  raw = [
+    (110.84, -1.75, 4611), (82.04, -3.78, 4609), (110.60, -1.85, 4611), (81.68, -3.80, 4609),
+    (82.28, -4.13, 4609), (110.40, -2.05, 4611), (110.16, -1.93, 4611), (110.08, -2.00, 4611),
+    (110.00, -2.00, 4611), (80.12, -4.83, 4609), (79.88, -4.95, 4609), (79.64, -5.08, 4609),
+    (79.32, -5.20, 4609), (79.48, -5.38, 4609), (79.08, -5.55, 4609), (78.64, -5.70, 4609),
+    (78.20, -5.88, 4609), (77.84, -6.00, 4609), (77.60, -6.18, 4609), (77.48, -6.30, 4609),
+    (76.96, -6.50, 4609), (76.48, -6.65, 4609), (103.52, -1.75, -1), (76.08, -6.90, 4609),
+    (75.52, -7.05, 4609), (108.97, -2.02, -1), (104.23, -2.15, -1), (103.64, -2.13, -1),
+    (74.16, -7.43, 4609), (73.72, -7.60, 4609), (73.24, -7.70, 4609),
+  ]
+  out = None
+  for dRel, vRel, tid in raw:
+    out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=24.0 + vRel, radarTrackId=tid)))
+  assert out.leadOne.dRel == pytest.approx(73.24, abs=0.5)   # tracks the true closing value, no lag
+  # at no point does the reported dRel sit meaningfully farther than the most recent real (id>0) reading
+  c = ctrl()
+  worst_overshoot = 0.0
+  last_real = None
+  for dRel, vRel, tid in raw:
+    out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=24.0 + vRel, radarTrackId=tid)))
+    if tid > 0:
+      last_real = dRel
+    if last_real is not None:
+      worst_overshoot = max(worst_overshoot, out.leadOne.dRel - last_real)
+  assert worst_overshoot < 1.0                              # old code overshot by ~6-9m for up to ~1s
+
+
+# --- instability telemetry --------------------------------------------------------------------------------
+
+def test_stability_quiet_on_clean_lead():
+  c = ctrl()
+  for _ in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0, radarTrackId=5)))
+  assert not c.lead_unstable()
+
+
+def test_stability_flags_bimodal_lead():
+  c = ctrl()
+  for i in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0, radarTrackId=5)))
+  assert c.lead_unstable()
+
+
+def test_stability_flags_trackid_churn():
+  c = ctrl()
+  for f in churn_frames(20):
+    c.smooth_radarstate(rs(f))
+  assert c.lead_unstable()
+
+
+def test_stability_resets_on_dropout():
+  c = ctrl()
+  for i in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0)))
+  assert c.lead_unstable()
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert not c.lead_unstable()
+
+
+def test_stability_runs_even_when_disabled():
+  c = ctrl(enabled=False)
+  for i in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0)))
+  assert c.lead_unstable()                           # telemetry not gated by the RadarDistance param
+
+
+# --- same-track noise smoother (bimodal vLead / repeated dRel jump on a CONSTANT radarTrackId) -------------
+
+def test_smoother_dejitters_bimodal_vlead_on_same_track():
+  # Same physical object (radarTrackId constant) but a bouncing velocity read (Doppler/fusion noise) -- the
+  # id evidence pins this to ONE real lead, so it's safe to EMA (unlike a bimodal read with a changing id).
+  c = ctrl()
+  out = None
+  for i in range(30):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0, vRel=-1.0, radarTrackId=9)))
+  assert c.lead_unstable()
+  assert 10.0 < out.leadOne.vLead < 18.0             # EMA settled between the two bouncing readings
+  assert out.leadOne.vLead not in (10.0, 18.0)
+
+
+def test_smoother_inactive_on_bimodal_vlead_with_changing_track():
+  # Same bimodal vLead signature, but radarTrackId ALSO changes -- ambiguous (could be two really-different
+  # real objects at different speeds), so this must NOT be smoothed, unlike the same-track case above.
+  c = ctrl()
+  one = lead(dRel=40.0, vLead=18.0, radarTrackId=1)
+  for i in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0, radarTrackId=1 if i % 2 else 2)))
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one                          # exact passthrough -- not averaged across tracks
+
+
+def test_smoother_same_track_noise_ignores_drel_jump():
+  # dRel track-jumps are excluded from same_track_noise on purpose: while status stays True, a repeated
+  # farther jump this large is already absorbed by _JumpGuard upstream, so the smoother never even sees the
+  # raw alternation here -- confirms the two mechanisms don't double up on the same signal.
+  c = ctrl()
+  out = None
+  for i in range(30):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0 if i % 2 == 0 else 55.0, vLead=18.0, vRel=-1.0, radarTrackId=4)))
+  assert out.leadOne.dRel < 45.0                     # held near the trusted value by the jump-guard, not 55
+
+
+def test_smoother_does_not_lag_a_stale_ema_after_an_extended_low_speed_gap():
+  # Mirrors the identical bug already fixed in _JumpGuard/_LeadHold: _smoother.update() is only called above
+  # CREEP_PASSTHROUGH_V (see smooth_radarstate), so its EMA state (_d/_vl/_vr) and _hold freeze for the entire
+  # duration of any full standstill. Resuming and EMA-ing a real, opening lead against that frozen state as if
+  # no time had passed lags dRel toward the stale, closer pre-stop value -- confirmed on the same real route as
+  # the _JumpGuard bug (550a71ee4c7a7fbe/000004dc--c8c0867520): pre-fix this reported 12.86m instead of the
+  # real 23.16m on relaunch.
+  c = ctrl(v_ego=LOW_SPEED_PASSTHROUGH_V + 1.0)
+  for i in range(12):
+    c.smooth_radarstate(rs(lead(dRel=11.72, vLead=2.72, vRel=-2.33, radarTrackId=1 if i % 2 else 2)))  # arms churn
+  c._v_ego = CREEP_PASSTHROUGH_V - 0.5                   # full stop: smoother.update() never called
+  for _ in range(60):
+    c.smooth_radarstate(rs(lead(dRel=3.68, vRel=0.0, vLead=0.0)))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 1.0               # relaunch: real lead now far + opening
+  out = c.smooth_radarstate(rs(lead(dRel=23.16, vRel=4.72, vLead=9.77, radarTrackId=1))).leadOne
+  assert out.dRel == pytest.approx(23.16)                # fresh reading passes through, not EMA-lagged stale
+
+
+def test_single_incidental_gap_during_churn_does_not_leak_a_glitch_into_the_ema():
+  # Regression, deeper than the single-mechanism cases above: a flat ">1 frame gap -> stale" threshold made
+  # _jump_guard treat ANY single skipped call as fully stale and skip the SWITCH_DREL check -- so a same-cycle
+  # fusion-transient glitch right after one incidental low-speed dip passed straight through _jump_guard
+  # unguarded, then got folded into the churn smoother's EMA (which was still live from before the dip),
+  # lagging vLead/dRel toward the glitch's inflated values for ~1s: a farther-and-faster-than-real lead, i.e.
+  # a real violation of this file's own invariant ("NEVER report a farther-or-faster lead than reality").
+  c = ctrl(v_ego=10.0)
+  for i in range(6):                                     # prime churn (real radar trackId-flip signature)
+    tid = 1 if i % 2 == 0 else 2
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-1.0, vLead=15.0, radarTrackId=tid)))
+  c._v_ego = 3.0                                         # one incidental dip into the creep band
+  c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-1.0, vLead=15.0, radarTrackId=1)))
+  c._v_ego = 10.0                                        # immediately back above the full-pipeline gate
+  glitch = lead(dRel=30.0 + SWITCH_DREL + 1.0, vRel=6.0, vLead=20.0, radarTrackId=1)
+  out = c.smooth_radarstate(rs(glitch)).leadOne
+  assert out.dRel < 31.0                                 # held near real ~30, not the glitch's 35.0
+  assert out.vLead < 16.0                                # held near real ~15, not the glitch's 20.0

sunnypilot/selfdrive/controls/lib/relc.py

@@ -10,11 +10,14 @@ from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.params import Params
 
-NEARSIDE_PROB = 0.2
+NEARSIDE_PROB = 0.25
 EDGE_PROB = 0.35
 EDGE_REACTION_TIME = 1.0
 EDGE_CLEAR_TIME = 0.3
 MIN_SPEED = 20 * CV.MPH_TO_MS
+NEAR_EDGE_DISTANCE = 4.5
+LEFT_NEARSIDE_LANE_IDX = 1
+RIGHT_NEARSIDE_LANE_IDX = 2
 
 
 class RoadEdgeLaneChangeController:
@@ -46,7 +49,21 @@ class RoadEdgeLaneChangeController:
     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:
+  @staticmethod
+  def _road_edge_y(road_edges, idx: int) -> float | None:
+    if road_edges is None or len(road_edges) <= idx or len(road_edges[idx].y) == 0:
+      return None
+    return road_edges[idx].y[0]
+
+  @staticmethod
+  def _edge_is_near(edge_y: float | None, left: bool) -> bool:
+    if edge_y is None:
+      return False
+    if left:
+      return bool(-NEAR_EDGE_DISTANCE < edge_y < 0.0)
+    return bool(0.0 < edge_y < NEAR_EDGE_DISTANCE)
+
+  def update(self, road_edge_stds, lane_line_probs, v_ego: float, road_edges=None) -> None:
     self.update_params()
 
     if not self.enabled or v_ego < MIN_SPEED:
@@ -55,11 +72,16 @@ class RoadEdgeLaneChangeController:
 
     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_lane_prob = lane_line_probs[LEFT_NEARSIDE_LANE_IDX]
+    right_lane_prob = lane_line_probs[RIGHT_NEARSIDE_LANE_IDX]
 
-    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
+    left_edge_y = self._road_edge_y(road_edges, 0)
+    right_edge_y = self._road_edge_y(road_edges, 1)
+    left_edge_near = self._edge_is_near(left_edge_y, True)
+    right_edge_near = self._edge_is_near(right_edge_y, False)
+
+    left_cond = left_edge_prob > EDGE_PROB and (left_edge_near or (left_edge_y is None and left_lane_prob < NEARSIDE_PROB))
+    right_cond = right_edge_prob > EDGE_PROB and (right_edge_near or (right_edge_y is None and right_lane_prob < NEARSIDE_PROB))
 
     if left_cond:
       self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -92,8 +92,7 @@ class SpeedLimitAssist:
     self._minus_hold = 0.
     self._last_carstate_ts = 0.
 
-    # TODO-SP: SLA's own output_a_target for planner
-    # Solution functions mapped to respective states
+    # Solution functions mapped to respective states (dispatched from get_a_target_from_control)
     self.acceleration_solutions = {
       SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
       SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
@@ -135,9 +134,14 @@ class SpeedLimitAssist:
     # Fallback
     return V_CRUISE_UNSET
 
-  # TODO-SP: SLA's own output_a_target for planner
   def get_a_target_from_control(self) -> float:
-    return self.a_ego
+    # Dispatches to acceleration_solutions[self.state] -- a real per-state decel/accel estimate (kinematic
+    # distance-to-limit formula while adapting, offset-over-horizon while active/holding) instead of echoing
+    # a_ego. Clipped to LIMIT_MIN_ACC/LIMIT_MAX_ACC: this is a comfort bound on the VALUE HANDED TO THE MPC AS
+    # ITS INITIAL STATE (x0), not a brake cap -- the MPC's own solve is free to command more decel than this
+    # from that starting point, so clipping here can't reduce achievable braking.
+    solution = self.acceleration_solutions.get(self.state, self.get_current_acceleration_as_target)
+    return max(LIMIT_MIN_ACC, min(LIMIT_MAX_ACC, solution()))
 
   def update_params(self) -> None:
     if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -16,12 +16,14 @@ 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_UNSET
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
-  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
+  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES, LIMIT_MIN_ACC, LIMIT_MAX_ACC
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
@@ -243,7 +245,36 @@ class TestSpeedLimitAssist:
 
     self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+    assert self.sla.output_v_target == target_speed
+    # kinematic (target^2 - current^2) / (2*distance) is well past LIMIT_MIN_ACC here -> clipped
+    expected_raw = (target_speed ** 2 - current_speed ** 2) / (2. * distance)
+    assert expected_raw < LIMIT_MIN_ACC
+    assert self.sla.output_a_target == pytest.approx(LIMIT_MIN_ACC)
+
+  def test_active_state_uses_offset_over_horizon_formula(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+    target_speed = SPEED_LIMITS['highway']
+    current_speed = target_speed - 1.0   # small offset, stays within the clip band
+
+    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+    expected = (target_speed - current_speed) / float(ModelConstants.T_IDXS[CONTROL_N])
+    assert LIMIT_MIN_ACC < expected < LIMIT_MAX_ACC   # sanity: this case is meant to land inside the clip band
+    assert self.sla.output_a_target == pytest.approx(expected)
+
+  def test_a_target_clipped_to_max_acc(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    distance = 1.0   # tiny distance -> a large positive kinematic solution -> clipped
+    current_speed = SPEED_LIMITS['city']
+    target_speed = SPEED_LIMITS['highway']
+
+    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
+    expected_raw = (target_speed ** 2 - current_speed ** 2) / (2. * distance)
+    assert expected_raw > LIMIT_MAX_ACC
+    assert self.sla.output_a_target == pytest.approx(LIMIT_MAX_ACC)
 
   def test_long_disengaged_to_disabled(self):
     self.initialize_active_state(self.pcm_long_max_set_speed)

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

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

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

@@ -0,0 +1,185 @@
+"""
+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.
+
+Closed-loop regression tests for sunnypilot's longitudinal comfort/smoothness fixes, built on the real
+LongitudinalPlanner via selfdrive/test/longitudinal_maneuvers/plant.py. Unlike a static log replay (which
+can only recompute an isolated input factor from recorded data), this drives the REAL MPC solver every
+cycle and feeds its output back into ego's own simulated speed/distance -- so it can catch regressions in
+the actual re-solved trajectory, not just in one factor's value. Params are set via the real Params() store;
+pytest's autouse openpilot_function_fixture (root conftest.py) gives each test function a fresh isolated
+prefix, so this is safe to run without touching real device state.
+
+Each test targets a SPECIFIC bug found and fixed this session, and is verified to actually fail if that fix
+is reverted (see the commit history / memory notes referenced in each test's docstring) -- these aren't
+just plausible-looking assertions, they have demonstrated teeth.
+"""
+
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import NORMAL
+
+
+def enable_sunnypilot_longitudinal(params: Params, accel_personality=True, radar_distance=True, dec=False):
+  params.put_bool("AccelPersonalityEnabled", accel_personality)
+  if accel_personality:
+    params.put("AccelPersonality", int(NORMAL))
+  params.put_bool("RadarDistance", radar_distance)
+  params.put_bool("DynamicExperimentalControl", dec)
+
+
+def count_launch_brake_cycles(accel: np.ndarray, speed: np.ndarray, launch_th=0.3, settle_speed=0.5) -> int:
+  # Count distinct "launch" events (acceleration crosses above launch_th while ego is near-stopped) followed
+  # by a return to near-zero/negative acceleration -- the "creep brake creep brake" signature. Only counts
+  # launches while speed stays low (settle_speed) -- a real, sustained departure isn't a cycle.
+  cycles = 0
+  launched = False
+  for a, v in zip(accel, speed, strict=True):
+    if not launched and a > launch_th and v < settle_speed:
+      launched = True
+      cycles += 1
+    elif launched and a < 0.0:
+      launched = False
+  return cycles
+
+
+def test_creep_noise_never_causes_repeated_launch_brake_cycling():
+  # Route 550a71ee4c7a7fbe/000004b6, t~678-690s: a genuinely-stopped lead's vLead sensor noise (small blips
+  # above STOP_GAP_CREEP_V=0.03 m/s, never sustained) previously accumulated in radar_distance.py's
+  # stop-gap creep-override counter (monotonic, never decayed) until it falsely latched the bias off,
+  # producing a same-cycle gap-widening the MPC read as "room to launch" -- see
+  # lead_unstable_gate_revert / routes_04b5_04b6_creep_bug memory notes; fixed in commit 73bea3866f
+  # (counter now decays on sub-threshold frames). This reproduces the noise pattern in closed loop: ego
+  # approaches and settles behind a near-stopped lead, then the lead's speed hovers with intermittent
+  # noise (never sustained motion) for 40s. Must never repeatedly launch-then-brake.
+  params = Params()
+  enable_sunnypilot_longitudinal(params)
+
+  rng = np.random.default_rng(0)
+  breakpoints = [0.0, 8.0]
+  speed_lead_values = [3.0, 0.0]
+  # 40s of intermittent noise: alternating 0.0 / small blip every 0.5s -- never two consecutive "moving" ticks,
+  # so no real sustained motion, matching the real route's noise signature.
+  noise_t = np.arange(8.0, 48.0, 0.5)
+  noise_v = np.where(rng.random(len(noise_t)) > 0.5, 0.08, 0.0)
+  breakpoints += list(noise_t)
+  speed_lead_values += list(noise_v)
+
+  man = Maneuver(
+    'stopped lead with intermittent vLead noise, no sustained motion',
+    duration=48.0,
+    initial_speed=8.0,
+    lead_relevancy=True,
+    initial_distance_lead=30.0,
+    speed_lead_values=speed_lead_values,
+    breakpoints=breakpoints,
+  )
+  valid, logs = man.evaluate()
+  assert valid
+  # logs columns: time, distance, distance_lead, speed, speed_lead, acceleration, d_rel
+  t, speed, accel = logs[:, 0], logs[:, 3], logs[:, 5]
+  settled = t > 10.0  # after the initial approach, while noise is active
+  cycles = count_launch_brake_cycles(accel[settled], speed[settled])
+  assert cycles <= 1, f'expected at most one settle-launch, got {cycles} launch-brake cycles from pure sensor noise'
+
+
+def test_drel_glitch_does_not_whipsaw_accel():
+  # Route 550a71ee4c7a7fbe/000004b4, t~976.1s: raw dRel bounced 17.7->12.3->17.15->12.0m across ~0.3s while
+  # vRel stayed -1 to -2 m/s -- physically impossible for one real object, a fusion glitch. SWITCH_DREL was
+  # 8.0 (too coarse to catch the ~5m bounce); lowered to 4.0 in commit f978c923a4. This reproduces the exact
+  # bounce pattern via Plant's lead_dRel_glitch_fn hook (overrides only dRel, leaving the true physics-based
+  # vRel/speed evolution intact for the closed loop) and checks the MPC's actual commanded accel doesn't
+  # whipsaw in response.
+  params = Params()
+  enable_sunnypilot_longitudinal(params)
+
+  glitch_window = (5.0, 5.5)  # apply the bounce for a short window mid-maneuver
+
+  def glitch_fn(t, d_rel, v_rel):
+    if glitch_window[0] <= t < glitch_window[1]:
+      # alternate between the true (closer) reading and a ~5m-farther bounce, matching the real route
+      phase = int((t - glitch_window[0]) / 0.05) % 2
+      if phase == 1:
+        return d_rel + 5.0, v_rel
+    return d_rel, v_rel
+
+  plant = Plant(lead_relevancy=True, speed=8.0, distance_lead=20.0, e2e=False,
+                lead_dRel_glitch_fn=glitch_fn)
+
+  accels = []
+  while plant.current_time < 10.0:
+    log = plant.step(v_lead=1.5)
+    accels.append(log['acceleration'])
+  accels = np.array(accels)
+
+  jerk = np.diff(accels) / (1.0 / plant.rate)
+  # focus on the glitch window and its immediate aftermath
+  t_arr = np.arange(len(accels)) / plant.rate
+  during = (t_arr[1:] >= glitch_window[0]) & (t_arr[1:] < glitch_window[1] + 0.5)
+  assert during.any()
+  peak_jerk = np.max(np.abs(jerk[during]))
+  assert peak_jerk < 3.0, f'dRel glitch produced a {peak_jerk:.2f} m/s^3 accel whipsaw -- glitch is leaking into the commanded accel'
+
+
+def test_fast_closing_lead_onset_is_ramped_not_snapped():
+  # A severe closing-rate lead (matching route 000004b5's flagship regression episode, vRel to -16.5 m/s)
+  # legitimately requires a large final decel -- that's not a bug (see routes_04b5_04b6_creep_bug memory:
+  # "very firm brake... looks legitimate"). What IS a bug is an instantaneous snap rather than a ramped
+  # onset. This checks onset smoothness (peak jerk during the transition) without asserting the final
+  # magnitude must be small.
+  params = Params()
+  enable_sunnypilot_longitudinal(params)
+
+  man = Maneuver(
+    'severe closing-rate lead, onset must ramp not snap',
+    duration=12.0,
+    initial_speed=20.0,
+    lead_relevancy=True,
+    initial_distance_lead=160.0,
+    speed_lead_values=[3.5, 3.5],
+    breakpoints=[0.0, 12.0],
+  )
+  valid, logs = man.evaluate()
+  assert valid
+  t, accel = logs[:, 0], logs[:, 5]
+  dt = np.diff(t)
+  dt[dt <= 0] = np.nan
+  jerk = np.diff(accel) / dt
+  onset = (t[1:] > 0.5) & (t[1:] < 3.0)  # after the first solve settles, during the initial hard reaction
+  assert onset.any()
+  peak_onset_jerk = np.nanmax(np.abs(jerk[onset]))
+  assert peak_onset_jerk < 4.0, f'onset jerk {peak_onset_jerk:.2f} m/s^3 -- braking snapped instead of ramping'
+  # sanity: this scenario genuinely needs real braking (not asserting it stays small)
+  assert np.min(accel) < -1.0
+
+
+def test_dec_on_off_agree_with_lead_present():
+  # is_e2e() previously only enforced "near/closing radar lead -> pure MPC, never blend e2e" inside DEC's
+  # active()-gated branch -- DEC off silently dropped the whole check, letting the e2e model's opinion
+  # blend in via min() regardless of the lead. Fixed in commit 7ff32eafea (dec.has_radar_acc_lead() checked
+  # unconditionally, before dec.active() is even consulted). This closed-loop test uses an adversarial
+  # e2e_accel_fn (independently opinionated, not the harness's default mild self.acceleration+0.1 echo) so
+  # DEC-on vs DEC-off would visibly diverge if the fix regressed.
+  def adversarial_e2e(t, speed, accel):
+    return -2.0  # e2e model insists on a hard brake, independent of the MPC's own view
+
+  def run(dec_enabled):
+    params = Params()
+    enable_sunnypilot_longitudinal(params, dec=dec_enabled)
+    plant = Plant(lead_relevancy=True, speed=15.0, distance_lead=40.0, e2e=True,
+                  e2e_accel_fn=adversarial_e2e)
+    accels = []
+    while plant.current_time < 5.0:
+      log = plant.step(v_lead=13.0)  # near lead, closing slowly -- within RADAR_LEAD_ACC_MAX_DREL
+      accels.append(log['acceleration'])
+    return np.array(accels)
+
+  accel_dec_on = run(dec_enabled=True)
+  accel_dec_off = run(dec_enabled=False)
+  np.testing.assert_allclose(accel_dec_on, accel_dec_off, atol=0.05,
+                              err_msg='DEC on vs off disagree with an identical near/closing lead present')

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

@@ -0,0 +1,72 @@
+"""
+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.
+
+_E2ETransitionGuard bounds how fast output_a_target may drop right after DEC switches into blended mode for
+a routine reason, so the e2e model's own (previously-hidden) desiredAcceleration can't produce a same-cycle
+discontinuous brake. It must never limit a rise, never limit anything while smoothing is inactive, and never
+apply when the caller signals an urgent/immediate transition.
+"""
+
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import _E2ETransitionGuard, TRANSITION_MAX_DROP_PER_CYCLE
+
+
+def test_inactive_is_identity():
+  g = _E2ETransitionGuard()
+  g.apply(-0.3, False)
+  assert g.apply(-1.1, False) == pytest.approx(-1.1)   # smoothing inactive -> raw passthrough always
+
+
+def test_first_call_seeds_without_limiting():
+  g = _E2ETransitionGuard()
+  assert g.apply(-1.1, True) == pytest.approx(-1.1)    # no prior baseline -> nothing to limit against yet
+
+
+def test_limits_downward_jump_when_active():
+  g = _E2ETransitionGuard()
+  g.apply(-0.3, False)                                 # establish baseline while inactive
+  out = g.apply(-1.1, True)                             # a farther-in-one-cycle jump, smoothing now active
+  assert out == pytest.approx(-0.3 - TRANSITION_MAX_DROP_PER_CYCLE)
+  assert out > -1.1                                     # not the raw discontinuous value
+
+
+def test_never_limits_a_rise():
+  g = _E2ETransitionGuard()
+  g.apply(-1.0, False)
+  out = g.apply(0.5, True)                              # accel rising -- must never be held back
+  assert out == pytest.approx(0.5)
+
+
+def test_converges_to_raw_within_a_few_cycles():
+  g = _E2ETransitionGuard()
+  g.apply(-0.3, False)
+  out = -0.3
+  for _ in range(20):
+    out = g.apply(-1.1, True)
+  assert out == pytest.approx(-1.1, abs=1e-6)            # eventually tracks the sustained raw value exactly
+
+
+def test_replays_real_route_e2e_transition():
+  # route 550a71ee4c7a7fbe/0000049f--71203acd12, t~165.3-166.2: DEC switches acc->blended (routine slow-down,
+  # not FCW) and the raw blended output snaps -0.312 -> -1.109 in one 50ms tick, then continues to ~-1.33.
+  g = _E2ETransitionGuard()
+  raw_acc = [-0.312, -0.312]           # still acc mode
+  raw_blended = [-1.109, -1.112, -1.121, -1.110, -1.118, -1.137, -1.140, -1.133, -1.145, -1.165,
+                 -1.199, -1.227, -1.255, -1.268, -1.287, -1.293, -1.326]
+  for v in raw_acc:
+    g.apply(v, False)
+  guarded = [g.apply(v, i < 10) for i, v in enumerate(raw_blended)]   # smoothing active for the first 10 frames
+  assert guarded[0] == pytest.approx(-0.312 - TRANSITION_MAX_DROP_PER_CYCLE)   # graded, not the -1.109 snap
+  assert min(guarded[:3]) > -1.0        # nowhere near the raw value in the first few frames
+  assert guarded[-1] == pytest.approx(raw_blended[-1])   # long since converged and tracking raw exactly
+
+
+def test_reset_drops_stale_baseline():
+  g = _E2ETransitionGuard()
+  g.apply(-0.3, False)
+  g.reset()
+  assert g.apply(-1.1, True) == pytest.approx(-1.1)      # no stale baseline to limit against after reset

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

@@ -0,0 +1,62 @@
+"""
+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.
+
+LongitudinalPlannerSP.is_e2e() decides whether the e2e model's raw action.desiredAcceleration blends into
+the MPC's solution via min(). A near/fast-closing radar lead must always route to pure MPC regardless of
+whether DEC itself is on -- that baseline previously lived entirely inside DEC's active()-gated branch, so
+turning DEC off silently dropped it (identical lead input, different is_e2e() answer). These tests pin the
+fix: the lead check now runs unconditionally, before DEC's own toggle is even consulted.
+"""
+
+from types import SimpleNamespace
+
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
+
+class FakeDec:
+  def __init__(self, active=False, mode="acc", has_radar_acc_lead=False):
+    self._active = active
+    self._mode = mode
+    self._has_radar_acc_lead = has_radar_acc_lead
+
+  def active(self):
+    return self._active
+
+  def mode(self):
+    return self._mode
+
+  def has_radar_acc_lead(self):
+    return self._has_radar_acc_lead
+
+
+def make_sm(experimental_mode=True):
+  return {'selfdriveState': SimpleNamespace(experimentalMode=experimental_mode)}
+
+
+def is_e2e(dec):
+  # is_e2e only reads self.dec -- no need to construct the full LongitudinalPlannerSP.
+  return LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=dec), make_sm())
+
+
+def test_experimental_mode_off_never_e2e():
+  sm_off = make_sm(experimental_mode=False)
+  assert not LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=FakeDec(active=True, mode="blended")), sm_off)
+  assert not LongitudinalPlannerSP.is_e2e(SimpleNamespace(dec=FakeDec(has_radar_acc_lead=True)), sm_off)
+
+
+def test_lead_present_blocks_e2e_regardless_of_dec_active():
+  # the bug this fixes: identical lead, DEC on vs off must agree.
+  assert not is_e2e(FakeDec(active=True, mode="acc", has_radar_acc_lead=True))
+  assert not is_e2e(FakeDec(active=False, mode="acc", has_radar_acc_lead=True))
+
+
+def test_no_lead_dec_off_falls_back_to_experimental_mode():
+  assert is_e2e(FakeDec(active=False, has_radar_acc_lead=False))
+
+
+def test_no_lead_dec_on_follows_dec_mode():
+  assert is_e2e(FakeDec(active=True, mode="blended", has_radar_acc_lead=False))
+  assert not is_e2e(FakeDec(active=True, mode="acc", has_radar_acc_lead=False))

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

@@ -16,6 +16,11 @@ V_HIGH = MIN_SPEED + 2.0
 V_LOW = MIN_SPEED - 1.0
 
 
+class DummyRoadEdge:
+  def __init__(self, y):
+    self.y = [y]
+
+
 @pytest.fixture
 def relc(mocker):
   mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
@@ -25,14 +30,18 @@ def relc(mocker):
   return controller
 
 
-def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
+def make_road_edges(left_y=-3.0, right_y=3.0):
+  return [DummyRoadEdge(left_y), DummyRoadEdge(right_y)]
+
+
+def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH, road_edges=None):
   for _ in range(int(seconds / DT_MDL) + 1):
-    controller.update(road_edge_stds, lane_line_probs, v_ego)
+    controller.update(road_edge_stds, lane_line_probs, v_ego, road_edges)
 
 
 @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"),
+  ([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], "left_edge_detected"),
+  ([0.9, 0.0], [0.8, 0.8, 0.0, 0.8], "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)
@@ -40,18 +49,18 @@ def test_edge_detection(relc, road_edge_stds, lane_line_probs, 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)
+  drive(relc, [0.0, 0.9], [0.8, 0.0, 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)
+  drive(relc, [0.0, 0.0], [0.8, 0.0, 0.0, 0.8], 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])
+  edge = ([0.0, 0.9], [0.8, 0.0, 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)
@@ -63,7 +72,7 @@ def test_noise_doesnt_clear(relc):
 
 
 def test_clears_after_window(relc):
-  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  edge = ([0.0, 0.9], [0.8, 0.0, 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)
@@ -75,25 +84,50 @@ def test_clears_after_window(relc):
 
 
 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)
+  drive(relc, [0.0, 0.9], [0.8, 0.0, 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)
+  drive(relc, [0.0, 0.9], [0.8, 0.0, 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)
+  relc.update([0.0, 0.9], [0.8, 0.0, 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)
+  drive(relc, [0.0, 0.9], [0.8, 0.0, 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)
+  relc.update([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], V_HIGH)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+@pytest.mark.parametrize("lane_line_probs", [
+  [0.0, 0.8, 0.8, 0.8],
+  [0.8, 0.8, 0.8, 0.0],
+])
+def test_outer_lane_lines_do_not_drive_edge_detection(relc, lane_line_probs):
+  drive(relc, [0.0, 0.0], lane_line_probs, EDGE_REACTION_TIME + 0.1)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+@pytest.mark.parametrize("road_edge_stds,road_edges,attr", [
+  ([0.0, 0.9], make_road_edges(left_y=-3.0, right_y=8.0), "left_edge_detected"),
+  ([0.9, 0.0], make_road_edges(left_y=-8.0, right_y=3.0), "right_edge_detected"),
+])
+def test_near_road_edge_geometry_blocks_with_visible_lane_lines(relc, road_edge_stds, road_edges, attr):
+  drive(relc, road_edge_stds, [0.8, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=road_edges)
+  assert getattr(relc, attr)
+
+
+def test_far_road_edge_geometry_does_not_block(relc):
+  drive(relc, [0.0, 0.0], [0.8, 0.0, 0.0, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=make_road_edges(left_y=-8.0, right_y=8.0))
   assert not relc.left_edge_detected
   assert not relc.right_edge_detected

sunnypilot/sunnylink/params_metadata.json

@@ -1,25 +1,11 @@
 {
   "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"
-      }
-    ]
+    "description": "Eco takes off gently and keeps the roomiest, smoothest following gap; Sport takes off briskly and follows a touch tighter. Hard-braking authority is always preserved."
   },
   "AccelPersonalityEnabled": {
-    "title": "Enable Accel Controller",
-    "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+    "title": "Enable Acceleration Profiles",
+    "description": "Enables Eco/Normal/Sport profiles -- a tier-scaled take-off from a stop and a slightly wider, smoother following gap. Braking is never reduced below stock."
   },
   "AccessToken": {
     "title": "AccessTokenIsNice",
@@ -1120,6 +1106,10 @@
     "title": "Quiet Mode",
     "description": ""
   },
+  "RadarDistance": {
+    "title": "Radar Distance",
+    "description": "De-noises the lead sunnypilot follows -- holds it through brief radar flicker/dropout and smooths a jittery (track-flipping) lead -- so it does not lose and re-grab the lead or hunt the gap. Braking is never reduced below stock."
+  },
   "RainbowMode": {
     "title": "Rainbow Mode",
     "description": ""
@@ -1142,7 +1132,7 @@
   },
   "RoadEdgeLaneChangeEnabled": {
     "title": "Block Lane Change: Road Edge Detection",
-    "description": ""
+    "description": "Blocks lane changes when the model sees a road edge on the signaled side."
   },
   "RoadName": {
     "title": "Road Name",
@@ -1350,19 +1340,11 @@
     "step": 0.1,
     "unit": "m/s\u00b2"
   },
-    "ToyotaAutoHold": {
+  "ToyotaAutoHold": {
     "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
     "description": ""
   },
-    "ToyotaEnhancedBsm": {
-    "title": "Toyota: Prius TSS2 BSM and some tssp",
-    "description": ""
-  },
-    "ToyotaTSS2Long": {
-    "title": "Toyota: custom longitudinal for TSS2",
-    "description": ""
-  },
-    "ToyotaDriveMode": {
+  "ToyotaDriveMode": {
     "title": "Enable drive mode btn link",
     "description": ""
   },
@@ -1370,10 +1352,18 @@
     "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

@@ -537,6 +537,12 @@
                   "value": 0
                 }
               ]
+            },
+            {
+              "key": "RoadEdgeLaneChangeEnabled",
+              "widget": "toggle",
+              "title": "Block Lane Change: Road Edge Detection",
+              "description": "Blocks the lane change if the model sees a road edge on your signaled side."
             }
           ]
         }
@@ -587,6 +593,26 @@
                 }
               ]
             },
+            {
+              "key": "RadarDistance",
+              "widget": "toggle",
+              "title": "Radar Distance",
+              "description": "De-noises the lead sunnypilot follows -- holds it through brief radar flicker/dropout and smooths a jittery (track-flipping) lead -- so it does not lose and re-grab the lead or hunt the gap. Braking is never reduced below stock.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "DisengageOnAccelerator",
               "widget": "toggle",
@@ -623,8 +649,8 @@
             {
               "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.",
+              "title": "Enable Acceleration Profiles",
+              "description": "Enables Eco/Normal/Sport profiles -- a tier-scaled take-off from a stop and a slightly wider, smoother following gap. Braking is never reduced below stock.",
               "visibility": [
                 {
                   "type": "capability",
@@ -644,7 +670,7 @@
               "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.",
+              "description": "Eco takes off gently and keeps the roomiest, smoothest following gap; Sport takes off briskly and follows a touch tighter. Hard-braking authority is always preserved.",
               "options": [
                 {
                   "value": 0,

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -24,6 +24,16 @@ sections:
     - $ref: '#/macros/longitudinal'
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: RadarDistance
+    widget: toggle
+    title: Radar Distance
+    description: De-noises the lead sunnypilot follows -- holds it through brief radar flicker/dropout and
+      smooths a jittery (track-flipping) lead -- so it does not lose and re-grab the lead or hunt the gap.
+      Braking is never reduced below stock.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: DisengageOnAccelerator
     widget: toggle
     title: Disengage Cruise on Accelerator Pedal
@@ -45,9 +55,9 @@ sections:
     - $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.
+    title: Enable Acceleration Profiles
+    description: Enables Eco/Normal/Sport profiles -- a tier-scaled take-off from a stop and a slightly wider,
+      smoother following gap. Braking is never reduced below stock.
     visibility:
     - $ref: '#/macros/longitudinal'
     enablement:
@@ -55,8 +65,8 @@ sections:
   - 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.
+    description: Eco takes off gently and keeps the roomiest, smoothest following gap; Sport takes off briskly
+      and follows a touch tighter. Hard-braking authority is always preserved.
     options:
     - value: 0
       label: Eco

sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml

@@ -255,3 +255,7 @@ sections:
       key: AutoLaneChangeTimer
       op: '>'
       value: 0
+  - key: RoadEdgeLaneChangeEnabled
+    widget: toggle
+    title: 'Block Lane Change: Road Edge Detection'
+    description: Blocks the lane change if the model sees a road edge on your signaled side.

sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -272,22 +272,6 @@ 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):