fix(long): comfort_stop and vLead

selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py

@@ -0,0 +1,52 @@
+import inspect
+import re
+from pathlib import Path
+
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+
+
+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)
+
+
+def test_longitudinal_smoothing_stays_planner_side():
+  update_src = inspect.getsource(LongitudinalPlanner.update)
+
+  accel_ceiling_idx = update_src.index("self.accel.get_max_accel(v_ego)")
+  radar_smoothing_idx = update_src.index("self.mpc.update(self.smooth_radarstate(sm['radarState'])")
+  accel_smoothing_idx = update_src.index("self.accel.smooth_target_accel(")
+
+  assert accel_ceiling_idx < radar_smoothing_idx
+  assert radar_smoothing_idx < accel_smoothing_idx
+
+
+# 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}"
+
+
+def test_comfort_stop_and_vlead_damp_gated_off():
+  # Strategy invariants (tn @ 2026-06-26): final-approach stop passes through stock (goal 6 stock-met), and the
+  # input-side vLead speed-damp (B) stays off pending on-road proof. Flicker-hold (A) is unaffected by either.
+  from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import COMFORT_STOP_ENABLED
+  from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import VLEAD_DAMP_ENABLED
+
+  assert COMFORT_STOP_ENABLED is False
+  assert VLEAD_DAMP_ENABLED is False

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

@@ -5,7 +5,8 @@ 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: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT), an anticipatory
-brake front-load, and a low-speed comfort stop. SAFETY: a firm/closing brake -- emergency (raw <=
+brake front-load, and a low-speed comfort stop (gated OFF by default via COMFORT_STOP_ENABLED -- stops pass
+through stock). SAFETY: a firm/closing brake -- emergency (raw <=
 HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED), FCW/crash, should_stop, or blended/e2e -- passes
 the plan straight through at full strength and rate, never softened/delayed/rate-limited. Only on the
 NON-emergency comfort path may the onset arrive spread by at most ONSET_SPREAD_MAX (a tightly bounded,
@@ -28,7 +29,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
   BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, MIN_SMOOTH_BRAKE_NEED, \
   HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, OVERBITE_CAP, STOP_PASSTHROUGH_V, \
   STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, ONSET_SPREAD_MAX, ONSET_SPREAD_JERK, \
-  COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
+  COMFORT_STOP_ENABLED, COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
   COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP
 
 _ZERO_ACCEL_EPS = 1e-6
@@ -52,6 +53,7 @@ class AccelController:
     self._lead_d = 0.0
     self._lead_vlead = 0.0
     self._stop_floor = 0.0       # comfort-stop floor latch (monotone within a stop episode, eased on release)
+    self._comfort_stop_enabled = COMFORT_STOP_ENABLED   # gated OFF: stops pass through stock (goal 6 stock-met)
     self._read_params()
 
   def _read_params(self) -> None:
@@ -140,8 +142,8 @@ class AccelController:
     # enforcer demanded a firm ~-1.6 grab; this does not). Outside the window (gap opening as a creeping lead
     # pulls away / lead moving / launch / standstill) the floor eases out at the release rate. min(out, floor)
     # keeps it never weaker than the plan. Off => no-op (off==stock).
-    if reset or not self._enabled:
-      self._stop_floor = 0.0                                   # disengaged/disabled: drop the latch, pure passthrough
+    if reset or not self._enabled or not self._comfort_stop_enabled:
+      self._stop_floor = 0.0                                   # disabled/gated/reset: drop the latch, pure passthrough
       return out
     final_approach = (self._lead_status and self._lead_vlead < COMFORT_STOP_LEAD_V and self._lead_d > 0.1
                       and COMFORT_STOP_RELEASE_V <= self._v_ego < COMFORT_STOP_V

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

@@ -79,6 +79,11 @@ ONSET_SPREAD_JERK = 2.5           # m/s^3: rate the spread output deepens back t
 # (cruising / gap opening as a creeping lead pulls away / lead moving / launch) the floor eases out at the
 # release rate. min(plan, floor) keeps it never weaker than the plan. Replaces the old kinematic v^2/(2*gap)
 # enforcer, which engaged late and demanded a firm ~-1.6 grab to hit a fixed gap. Off => no-op.
+# Gated OFF by default and independently of AccelPersonalityEnabled: the final approach passes through stock,
+# because goal 6 (smooth coming to stop) is already met by the stock/Toyota tune (parks ~4.2m smoothly) and the
+# old stops-too-close complaint traced to the radar_distance modelProb gate, since fixed at source. Re-enable
+# (flip True) only if an on-road roll-in / creep is observed, ideally with a gentler COMFORT_STOP_MAX_DECEL (~-1.2).
+COMFORT_STOP_ENABLED = False
 COMFORT_STOP_V = 4.0              # m/s: only engage at/below this ego speed
 COMFORT_STOP_LEAD_V = 1.0         # m/s: only behind a (near-)stopped lead
 COMFORT_STOP_GAP = 5.0            # m: reference standstill gap (radar dRel) for the final-approach window

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

@@ -39,9 +39,10 @@ def make_sm(v_ego=20.0, lead_status=False, lead_d=0.0, lead_vlead=0.0):
   return {'carState': SimpleNamespace(vEgo=v_ego), 'radarState': SimpleNamespace(leadOne=lead)}
 
 
-def make_controller(enabled=True, personality=NORMAL, crash_cnt=0):
+def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=False):
   store = {"AccelPersonalityEnabled": enabled, "AccelPersonality": int(personality)}
   ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=crash_cnt), params=FakeParams(store))
+  ctrl._comfort_stop_enabled = comfort_stop   # comfort_stop is gated off in production; opt in per-test
   ctrl.update(make_sm())
   return ctrl
 
@@ -232,7 +233,7 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes():
 def test_comfort_stop_holds_through_plan_ease():
   # Plan brakes to a peak then eases off near the stop (the stock creep). The hold keeps the deeper decel so
   # the brake does not ease in (no roll) -- but NEVER firmer than the plan's own peak (no added hard bite).
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   out = 0.0
   for plan in [-0.4, -0.8, -1.1, -1.1, -0.6, -0.3, -0.1]:   # decel to a -1.1 peak, then ease (creep)
     ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
@@ -243,7 +244,7 @@ def test_comfort_stop_holds_through_plan_ease():
 
 def test_comfort_stop_never_firmer_than_plan():
   # The hold can only stop the brake from WEAKENING; it never commands a decel firmer than the plan itself.
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   for plan in [-0.2, -0.5, -0.9, -0.9, -0.9]:           # steady (no ease) -> hold matches plan, adds nothing
     ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
     out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
@@ -252,7 +253,7 @@ def test_comfort_stop_never_firmer_than_plan():
 
 def test_comfort_stop_monotone_no_early_release():
   # While still moving, the comfort floor never WEAKENS frame-to-frame (the old enforcer self-released -> roll).
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   floors = []
   for v in [3.0, 2.6, 2.2, 1.8, 1.4, 1.0, 0.6]:         # decelerating toward the lead
     ctrl.update(make_sm(v_ego=v, lead_status=True, lead_d=max(0.5, 7.0 - (3.0 - v) * 2), lead_vlead=0.0))
@@ -269,9 +270,21 @@ def test_comfort_stop_off_when_disabled():
   assert out == pytest.approx(-0.1, abs=_EPS)
 
 
+def test_comfort_stop_gated_off_is_stock_passthrough():
+  # Production default (COMFORT_STOP_ENABLED off, even with AccelController enabled): the final approach is stock
+  # passthrough -- output follows the easing plan, no anti-creep hold, floor stays 0 (goal 6 met by stock).
+  ctrl = make_controller(personality=ECO)                      # comfort_stop defaults False (production)
+  out = 0.0
+  for plan in [-0.4, -0.8, -1.1, -0.6, -0.1]:                  # decel to a peak then ease (stock creep)
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
+    out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-0.1, abs=_EPS)                  # follows the easing plan -> no hold
+  assert ctrl._stop_floor == 0.0                               # never latched
+
+
 def test_comfort_stop_no_op_moving_lead():
   # Moving lead (vLead high): no comfort stop (only behind a near-stopped lead).
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=5.0))
   out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
   assert out == pytest.approx(-0.1, abs=_EPS)
@@ -279,7 +292,7 @@ def test_comfort_stop_no_op_moving_lead():
 
 def test_comfort_stop_never_weaker():
   # The comfort floor only ever ADDS braking: output never weaker than the plan.
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   for raw in (-0.05, -0.3, -1.0, -2.5):
     ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.0))
     out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
@@ -289,7 +302,7 @@ def test_comfort_stop_never_weaker():
 def test_comfort_stop_weakens_when_gap_opens():
   # Creeping stop-and-go lead (vLead stays < COMFORT_STOP_LEAD_V) that pulls away: once the gap opens well past
   # the target the floor must WEAKEN, not hold a phantom brake into an opening gap.
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   for _ in range(15):                                          # approach close -> deep floor (final-approach hold)
     ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.3))
     ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
@@ -304,7 +317,7 @@ def test_comfort_stop_weakens_when_gap_opens():
 def test_comfort_stop_releases_on_launch():
   # Stop-and-go GO: after holding a comfort floor at a stop, once the lead moves and the plan wants throttle the
   # floor must release (track the plan up) and not hold the output below the natural plan -> the car launches.
-  ctrl = make_controller(personality=ECO)
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
   for _ in range(20):                                          # hold the plan's -1.0 decel approaching a stopped lead
     ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
     ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)

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

@@ -25,6 +25,11 @@ MIN_HELD_DREL = 0.5
 
 LOW_SPEED_PASSTHROUGH_V = 5.0   # m/s
 
+# Speed-damp (B) is gated OFF: it lags a speeding-up lead (input-side) and a prior version caused phantom
+# braking + a launch rubber-band (lead measured up to ~11 m/s slower than real). Flicker-hold (A) runs alone.
+# Flip True only behind an on-road check of a lead accelerating away above LOW_SPEED_PASSTHROUGH_V; if a
+# failure-to-release appears, prefer an output-side accel slew over re-enabling this input-side lag.
+VLEAD_DAMP_ENABLED = False
 VLEAD_TAU = 0.4                 # s, lag on a speeding-up lead
 _VLEAD_ALPHA = DT_MDL / VLEAD_TAU
 SWITCH_DREL = 8.0              # m, dRel jump that means the radar switched to a different track -> reset the filter
@@ -125,6 +130,7 @@ class RadarDistanceController:
     self._frame = 0
     self._v_ego = 0.0
     self._enabled = self._params.get_bool("RadarDistance")
+    self._vlead_damp_enabled = VLEAD_DAMP_ENABLED   # speed-damp (B) gated off; flicker-hold (A) runs alone
     self._one = _LeadHold()
     self._two = _LeadHold()
 
@@ -151,4 +157,6 @@ class RadarDistanceController:
     two = self._two.step(radarstate.leadTwo)
     if self._v_ego < LOW_SPEED_PASSTHROUGH_V:
       return radarstate
+    if not self._vlead_damp_enabled:
+      return _RadarStateProxy(one, two)                       # flicker-hold (A) only; speed-damp (B) gated off
     return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two))

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

@@ -36,9 +36,10 @@ def obstacle(ld):
   return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE)
 
 
-def ctrl(enabled=True):
+def ctrl(enabled=True, vlead_damp=False):
   c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
   c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0  # default above the gate so hold-logic tests exercise the flicker-hold
+  c._vlead_damp_enabled = vlead_damp         # speed-damp (B) is gated off in production; opt in per-test
   return c
 
 
@@ -108,7 +109,7 @@ def test_low_speed_passthrough_but_hold_warmed_for_highway():
 
 
 def test_vlead_lags_rise_instant_fall():
-  c = ctrl()
+  c = ctrl(vlead_damp=True)                                     # speed-damp (B) under test; gated off in production
   c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0)))           # seed at 15
   rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
   assert 15.0 <= rising.vLead < 25.0                            # rise lagged (<= real -> never faster than real)
@@ -119,13 +120,22 @@ def test_vlead_lags_rise_instant_fall():
 def test_vlead_resets_on_track_switch_no_phantom_slow():
   # the old bug: a slow lead's filtered speed carried across a switch to a fast farther track, reporting it
   # near-stopped. A dRel jump (track switch) now resets the filter -> the new track's real speed is reported.
-  c = ctrl()
+  c = ctrl(vlead_damp=True)                                     # speed-damp (B) under test; gated off in production
   for _ in range(3):
     c.smooth_radarstate(rs(lead(dRel=12.0, vLead=0.5)))         # slow close lead
   switched = c.smooth_radarstate(rs(lead(dRel=80.0, vLead=18.0))).leadOne  # different, far, fast track
   assert switched.vLead == pytest.approx(18.0, abs=1e-6)        # real speed, not the stale ~0.5
 
 
+def test_vlead_damp_gated_off_reports_real_speed():
+  # Production default (VLEAD_DAMP_ENABLED off): a speeding-up lead is NOT lagged -> real vLead passes through
+  # (flicker-hold A only). This is the on-by-default behavior; B is opt-in pending on-road proof.
+  c = ctrl()                                                    # vlead_damp defaults off (production)
+  c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0)))
+  rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
+  assert rising.vLead == pytest.approx(25.0, abs=1e-6)          # no damp -> real speed
+
+
 def test_obstacle_monotone_during_hold():
   c = ctrl()
   for _ in range(3):