long refactor

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

@@ -18,7 +18,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
   NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \
   STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, \
   SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \
-  MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, HARD_BRAKE_ONSET_JERK, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \
+  MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, HARD_BRAKE_ONSET_JERK, OVERBITE_CAP, \
+  STOP_PASSTHROUGH_V, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \
   ONSET_JERK0, ONSET_JERK_GAIN, ONSET_GAP_SOFT, ONSET_GAP_GAIN, ONSET_JERK_MAX, ONSET_HANDBACK_JERK, \
   SOFT_ONSET_MAX_BRAKE_NEED, SOFT_ONSET_MAX_INSTANT_ACCEL, SOFT_ONSET_REARM_FRAMES
 
@@ -39,6 +40,7 @@ class AccelController:
     self._decel_target = 0.0
     self._smooth_active = False
     self._bypassed = False
+    self._no_soften = False      # blended/e2e: anticipate (front-load) but never soften the onset
     # convex brake-onset shaper state
     self._onset_latched = False # sticky: True once an onset goes firm -> no re-soften until sustained release
     self._onset_release = 0     # consecutive non-deepening frames (sticky re-arm debounce)
@@ -79,37 +81,45 @@ class AccelController:
     self._decel_target = 0.0
     self._smooth_active = False
     self._soft_active = False
+    self._bypassed = False
+    # Blended/e2e (stock_brake): the model owns the brake, so never SOFTEN it (no convex soft-onset),
+    # but still allow the never-weaker front-load so blended anticipates like ACC and brakes enough.
+    self._no_soften = bool(stock_brake)
 
-    # The convex onset shaper runs ONLY for ECO/SPORT (NORMAL and disabled are stock). Reset its state
-    # whenever it cannot run so nothing leaks across a personality toggle or a passthrough interlude.
-    if not (self._enabled and self._personality != NORMAL):
+    # The convex soft-onset runs ONLY for an enabled non-NORMAL, non-no-soften personality. Reset its
+    # state whenever it cannot run so nothing leaks across a toggle or a passthrough interlude.
+    if not (self._enabled and self._personality != NORMAL and not self._no_soften):
       self._reset_onset()
 
-    # Passthroughs (hand the plan straight through, no shaping):
-    if reset or not self._enabled or (stock_brake and (raw < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED)):
-      self._bypassed = False                                  # disabled / reset / blended-e2e braking
-      return self._passthrough(raw)
+    # --- Full stock passthroughs (no shaping at all) ---
+    if reset or not self._enabled:
+      return self._passthrough(raw)                           # disabled / reset
+    if self._v_ego < STOP_PASSTHROUGH_V and raw <= 0.0:
+      # Stop/creep regime: braking is stock so the stop distance matches OFF exactly (no softened crawl /
+      # coast-in). Launch (positive accel) is unaffected. Mirrors radar_distance's low-speed neutrality.
+      return self._stand_down(raw)
+
+    # --- Hard brake / stop: never soften the DEPTH (onset rate-limited, full plan depth always reached) ---
     self._bypassed = self._emergency_bypass(raw, should_stop)
     if self._bypassed:
-      # A hard brake's DEPTH is never softened. True emergencies (FCW / crash imminent) pass straight
-      # through; other firm brakes get a deepening-only onset rate cap so the firm brake arrives smoothly
-      # instead of as a raw stock grab (full plan depth still reached -> never weaker or later in size).
-      if self._mpc.crash_cnt > 0:
+      if self._mpc.crash_cnt > 0:                             # true emergency / FCW -> pure passthrough
         return self._stand_down(raw)
       return self._stand_down_jerk_limited(raw)
     if self._stop_imminent(speed_trajectory, t_idxs):         # stop coming -> stock decel, no coast/creep
       return self._stand_down_jerk_limited(raw)
 
-    # Front-load a gentle early brake when a deeper brake is predicted ahead. The convex shaper owns the
-    # output when it governed this frame (soft_active); otherwise never weaker than the plan.
+    # --- Smooth shaping. min(slewed, raw) keeps the output NEVER WEAKER than the plan; the only softener
+    #     is the convex soft-onset, which is gated off above for no-soften (blended) mode. ---
     if self._brake_need >= MIN_SMOOTH_BRAKE_NEED:
       self._smooth_active = True
-      self._decel_target = self.get_decel_target(self._brake_need)
+      # Front-load a gentle early brake when a deeper brake is predicted ahead, but never bite more than
+      # OVERBITE_CAP below the LIVE plan (a cut-in/merge spikes brake_need while the plan still wants
+      # throttle -> abrupt over-bite). Once the plan itself brakes, the table wins -> anticipation preserved.
+      self._decel_target = max(self.get_decel_target(self._brake_need), raw - OVERBITE_CAP)
       slewed = self._slew(min(raw, self._decel_target))
       return self._finalize(slewed if self._soft_active else min(slewed, raw))
 
-    # Below the smooth-brake threshold: track the plan, never weaker than it while braking.
-    slewed = self._slew(raw)
+    slewed = self._slew(raw)                                  # below the smooth-brake threshold: track the plan
     if self._soft_active or raw >= 0.0:
       return self._finalize(slewed)
     return self._finalize(min(slewed, raw))
@@ -153,9 +163,10 @@ class AccelController:
 
   def _onset_soft_armed(self, target_accel: float) -> bool:
     # Gentle non-emergency onset. Armed from the FIRST deepening tick (no brake_need lower gate, so the
-    # gentle bite lands on the actual onset, not after the plan has already deepened). Two upper gates
-    # keep it off firm/deep braking: the 3s-lookahead brake_need ceiling AND the instantaneous raw depth.
-    return (self._enabled and self._personality != NORMAL and
+    # gentle bite lands on the actual onset, not after the plan has already deepened). Off in no-soften
+    # (blended/e2e) mode. Two upper gates keep it off firm/deep braking: the 3s-lookahead brake_need
+    # ceiling AND the instantaneous raw depth.
+    return (self._enabled and self._personality != NORMAL and not self._no_soften and
             0.0 < self._brake_need < SOFT_ONSET_MAX_BRAKE_NEED and
             target_accel > SOFT_ONSET_MAX_INSTANT_ACCEL)
 

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

@@ -48,6 +48,13 @@ ACCEL_RISE_JERK = {ECO: 1.0, NORMAL: 1.5, SPORT: 2.2}   # accel-onset jerk: high
 
 SMOOTH_DECEL_LOOKAHEAD_T = 3.0
 MIN_SMOOTH_BRAKE_NEED = 0.2
+
+# Front-load over-bite cap. The SMOOTH_DECEL front-load is allowed to brake at most this much DEEPER than
+# the live raw plan. On a cut-in/merge the 3s brake_need spikes and the table would front-load a firm brake
+# while the plan still wants throttle/coast -> an abrupt early bite (the "worse on merge" feel; routes
+# 45e/460). This binds only in that contradictory case; once the plan itself brakes, raw-OVERBITE_CAP sits
+# below the table value so the table wins and the anticipatory early brake (route 456 fix) is preserved.
+OVERBITE_CAP = 0.30   # m/s^2 max front-load depth below the live plan
 HARD_BRAKE_TARGET_ACCEL = -1.5
 HARD_BRAKE_NEED = 2.6
 
@@ -69,6 +76,12 @@ HARD_BRAKE_ONSET_JERK = 2.0   # m/s^3, deepening-only onset rate cap on firm (no
 STOP_IMMINENT_VEGO = 1.0          # m/s  plan-predicted speed below this within the lookahead == stop coming
 STOP_IMMINENT_LOOKAHEAD_T = 3.0   # s
 
+# Stop/creep stop-neutrality. Below this ego speed, the brake side hands the plan straight through (stock),
+# so the controller cannot soften the final crawl and let the car coast in closer than stock. Matches the
+# radar_distance low-speed stop-neutrality so ON == OFF near stops. Positive-accel (launch) shaping is
+# unaffected (the launch profiles still apply via the accel ceiling).
+STOP_PASSTHROUGH_V = 5.0          # m/s ego speed below which braking is stock passthrough
+
 # --- Convex brake-onset shaper (param-gated; ECO/SPORT only, NORMAL = stock passthrough) ---
 # The grabby bite is the raw MPC plan: stock deepening uses a CONSTANT jerk (integrates to a LINEAR
 # accel ramp) and min(slewed,raw) lets the deep raw plan win, so the bite passes through untouched.

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

@@ -15,7 +15,8 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController as _AC  # noqa: F401
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
   ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \
-  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_ONSET_JERK, AccelerationPersonality, \
+  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_ONSET_JERK, OVERBITE_CAP, \
+  STOP_PASSTHROUGH_V, AccelerationPersonality, \
   BRAKE_DEEPENING_JERK, ONSET_JERK0, ONSET_GAP_SOFT, ONSET_HANDBACK_JERK
 
 # The convex onset brakes shallower than the plan during the bite, but the instantaneous-gap catch-up
@@ -107,6 +108,48 @@ def test_early_soft_braking_brakes_before_plan():
   assert ctrl.brake_need() == pytest.approx(1.0)
 
 
+def test_low_speed_brake_is_stock_passthrough():
+  # Stop/creep regime (vEgo < STOP_PASSTHROUGH_V): braking is handed through unshaped (stock) so the
+  # controller cannot soften the crawl and let the car coast in closer than stock.
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1))
+  for raw in (-0.3, -1.0):                                   # a braking plan that would normally front-load
+    out = ctrl.smooth_target_accel(raw, flat_traj(-1.5), T_IDXS, should_stop=False)
+    assert out == pytest.approx(raw, abs=_EPS)              # exact stock passthrough, no front-load
+    assert not ctrl.smooth_active()
+
+
+def test_low_speed_launch_still_shapes():
+  # The low-speed brake passthrough must NOT neutralize positive-accel (launch) shaping.
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1))
+  ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)   # seed
+  out = ctrl.smooth_target_accel(1.5, flat_traj(1.5), T_IDXS, should_stop=False)
+  assert out < 1.5                                           # rise-rate limited (shaped), not raw passthrough
+
+
+def test_overbite_cap_limits_frontload_vs_live_plan():
+  # Cut-in/merge: raw plan still wants throttle (+0.5) while a deep brake is predicted ahead (brake_need
+  # high). The front-load must not settle more than OVERBITE_CAP below the LIVE plan -> no abrupt early grab.
+  ctrl = make_controller(personality=ECO)
+  traj = [0.5, 0.3, 0.0, -0.5, -1.5, -2.0] + [-2.0] * (len(T_IDXS) - 6)   # throttle now, hard brake later
+  out = 0.0
+  for _ in range(8):                                       # let the accel rise-rate slew settle
+    out = ctrl.smooth_target_accel(0.5, traj, T_IDXS, should_stop=False)
+  assert ctrl.smooth_active()
+  assert out == pytest.approx(0.5 - OVERBITE_CAP, abs=1e-3)  # front-load clamped to exactly cap below plan
+
+
+def test_overbite_cap_preserves_anticipation_when_plan_braking():
+  # Once the live plan is itself braking, raw-OVERBITE_CAP sits below the table, so the cap does NOT bind
+  # and the anticipatory front-load (route 456 fix) is preserved (output deeper than the shallow raw).
+  ctrl = make_controller(personality=ECO)
+  out = 0.0
+  for _ in range(4):
+    out = ctrl.smooth_target_accel(-0.2, flat_traj(-1.5), T_IDXS, should_stop=False)
+  assert out < -0.2 - _EPS                                 # still front-loads below the live -0.2 plan
+
+
 def test_stop_imminent_stands_down_but_moving_follow_shapes():
   # Stop coming (plan speed -> ~0): stand down to stock decel so the gentle bite can't coast into the
   # stop (creep). Slowing to a MOVING follow (plan stays > STOP_IMMINENT_VEGO): gentle onset stays active
@@ -273,11 +316,14 @@ def test_fcw_crash_cnt_bypass():
   assert ctrl.bypassed()
 
 
-def test_e2e_brake_passthrough():
+def test_blended_never_softens_brake():
+  # Blended/e2e (stock_brake): the brake is NEVER softened -> output is never weaker than the plan and
+  # the convex soft-onset never engages. (It may still anticipate via the never-weaker front-load.)
   ctrl = make_controller(personality=ECO)
-  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False, stock_brake=True)
-  assert out == pytest.approx(-1.0, abs=_EPS)
-  assert not ctrl.smooth_active()
+  for raw in [0.0, -0.3, -0.6, -0.9, -1.0, -1.0, -1.0]:
+    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False, stock_brake=True)
+    assert out <= raw + _EPS                 # never weaker than the plan (no softening)
+    assert not ctrl._soft_active             # convex soft-onset disabled in blended
 
 
 def test_out_of_range_personality_clamps():

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

@@ -7,6 +7,10 @@ See the LICENSE.md file in the root directory for more details.
 Radar Distance: hold a just-dropped, recently-sustained lead alive through radar flicker so the MPC does
 not lose+regain it. Obstacle-monotone (held obstacle <= last real <= stock) -> braking is always >= stock,
 never less. Wall-clock bounded, flicker-proof. Default off => stock passthrough.
+
+Stop-neutral: at/below LOW_SPEED_PASSTHROUGH_V (the stop/creep regime) it returns the RAW radarstate
+unchanged so the stop distance is byte-identical to stock (RadarDistance OFF). The flicker-hold only
+governs above that speed, where lose+regain actually matters.
 """
 
 from opendbc.car import structs
@@ -19,12 +23,13 @@ DROPOUT_DREL = 1.0
 FCW_PROB_CAP = 0.9       # held lead can't reach the FCW gate (>0.9) -> no false FCW
 MIN_HELD_DREL = 0.5
 
-# Roomier stops. Stock long MPC targets STOP_DISTANCE (6m) but soft-coasts in to ~3.5-4m on a real stop,
-# which feels close. We pull the reported lead in by STOP_MARGIN as the car slows so the MPC settles that
-# much farther back. Scoped to the stop regime by a v_ego ramp (full at/below STOP_MARGIN_V[0], zero at/above
-# STOP_MARGIN_V[1]) so it never touches mid/high-speed following. Set STOP_MARGIN=0 to disable.
-STOP_MARGIN = 1.5        # m extra standstill/low-speed gap
-STOP_MARGIN_V = (1.0, 4.0)  # m/s ramp: full <=1.0, none >=4.0
+# Stop-neutrality gate. At/below this speed we are in the stop/creep regime, where the car's stop
+# distance must match stock openpilot exactly (and radard's low_speed_override already supplies a robust
+# closest-track lead). So below this speed smooth_radarstate returns the RAW radarstate unchanged --
+# byte-identical to RadarDistance OFF -- so neither the flicker-hold's dead-reckoned dRel nor any other
+# transform can move the lead the MPC sees near a stop. The hold is still stepped to keep its state warm
+# for when speed rises back above the gate. Above this speed the highway flicker-hold runs (its purpose).
+LOW_SPEED_PASSTHROUGH_V = 5.0  # m/s (~18 km/h): covers stop + creep, below highway following
 
 
 class _HeldLead:
@@ -50,23 +55,6 @@ class _RadarStateProxy:
     self.leadTwo = lead_two
 
 
-class _LeadView:
-  # mirror the fields the MPC reads from a lead, with dRel pulled in by `pull` (>=0 m) so the car keeps that
-  # much extra gap. Only used in the low-speed stop regime (see STOP_MARGIN); high-speed leads pass through.
-  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
-
-  def __init__(self, src, pull):
-    self.status = src.status
-    self.dRel = max(MIN_HELD_DREL, src.dRel - pull)
-    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 _LeadHold:
   def __init__(self):
     self._last = None
@@ -130,23 +118,14 @@ class RadarDistanceController:
   def enabled(self) -> bool:
     return self._enabled
 
-  def _stop_margin(self) -> float:
-    if STOP_MARGIN <= 0.0:
-      return 0.0
-    lo, hi = STOP_MARGIN_V
-    frac = (hi - self._v_ego) / (hi - lo)
-    frac = 0.0 if frac < 0.0 else (1.0 if frac > 1.0 else frac)
-    return STOP_MARGIN * frac
-
   def smooth_radarstate(self, radarstate):
     if not self._enabled:
       return radarstate
+    # Step the holds every frame to keep their flicker state warm, but in the stop/creep regime return the
+    # RAW radarstate unchanged so the lead the MPC sees -- and therefore the stop distance -- is byte-stock
+    # (identical to RadarDistance OFF). Above the gate the highway flicker-hold governs (its real purpose).
     one = self._one.step(radarstate.leadOne)
     two = self._two.step(radarstate.leadTwo)
-    pull = self._stop_margin()
-    if pull > 0.0:
-      if one.status:
-        one = _LeadView(one, pull)
-      if two.status:
-        two = _LeadView(two, pull)
+    if self._v_ego < LOW_SPEED_PASSTHROUGH_V:
+      return radarstate
     return _RadarStateProxy(one, two)

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

@@ -10,7 +10,7 @@ from types import SimpleNamespace
 import pytest
 
 from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
-  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, STOP_MARGIN, STOP_MARGIN_V
+  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V
 
 COMFORT_BRAKE = 2.5
 
@@ -38,7 +38,7 @@ def obstacle(ld):
 
 def ctrl(enabled=True):
   c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
-  c._v_ego = STOP_MARGIN_V[1] + 10.0  # default to a no-margin (cruise) speed so hold-logic tests are isolated
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0  # default above the gate so hold-logic tests exercise the flicker-hold
   return c
 
 
@@ -85,21 +85,26 @@ def test_low_speed_override_lead_arms_hold():
   assert held.status is True                         # armed off the prob=0 lead, holds through dropout
 
 
-def test_stop_margin_pulls_lead_in_near_stop():
-  # at standstill the reported lead is pulled in by STOP_MARGIN so the MPC keeps extra real gap (roomier stop)
+def test_low_speed_returns_raw_object():
+  # Stop/creep regime: ENABLED returns the EXACT raw radarstate object (byte-identical to OFF), so the
+  # lead the MPC sees -- and thus the stop distance -- is stock. This is the core stop-neutrality guarantee.
   c = ctrl()
-  c._v_ego = STOP_MARGIN_V[0]            # full-margin regime
-  out = c.smooth_radarstate(rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0)))
-  assert out.leadOne.dRel == pytest.approx(6.0 - STOP_MARGIN, abs=1e-6)
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1
+  r = rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0))
+  assert c.smooth_radarstate(r) is r                 # object identity == stock
 
 
-def test_stop_margin_off_at_speed():
-  # no margin at cruise speed -> untouched passthrough (full-fidelity real lead to the MPC)
+def test_low_speed_passthrough_but_hold_warmed_for_highway():
+  # At low speed the raw radarstate is returned, but the hold is still stepped (state kept warm) so the
+  # flicker-hold engages the moment speed rises above the gate.
   c = ctrl()
-  c._v_ego = STOP_MARGIN_V[1] + 5.0
-  one = lead(status=True, dRel=40.0)
-  out = c.smooth_radarstate(rs(one))
-  assert out.leadOne is one
+  for _ in range(3):                                 # sustain a real lead while in the low-speed regime
+    c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1
+    r = rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0))
+    assert c.smooth_radarstate(r) is r               # returned object stays raw at low speed
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0          # rise above the gate -> dropout now held (proxy, not raw)
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is True
 
 
 def test_obstacle_monotone_during_hold():