feat(long): fix accelersonality rubbernband

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

@@ -11,10 +11,12 @@ Three independent, per-tier levers keyed by the cereal AccelerationPersonality o
   2. Accel rise rate - get_rise_rate(), slews the accel ceiling upward.
   3. Early soft braking - smooth_target_accel(), front-loads a gentle decel BEFORE the plan brakes,
      never commanding less braking than the plan (never-weaken invariant), with hard-brake / FCW /
-     should_stop / closing-lead / e2e bypass back to the stock plan.
+     should_stop / closing-lead / e2e bypass back to the stock plan. Engagement is hysteretic and
+     suppressed when a lead is pulling away (anti rubber-band), and a stop-hold latch prevents the
+     stop -> creep -> stop "double stop" on small lead twitches.
 
 Disabled or Normal == stock by construction: Normal tier uses the stock ceiling/rise literals, and a
-disabled controller forces Normal and passes the target through untouched.
+disabled controller forces Normal, passes the target through untouched, and never latches stop-hold.
 """
 
 from collections.abc import Sequence
@@ -29,7 +31,9 @@ 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, RISE_RATE, SMOOTH_DECEL_BP, \
   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, CLOSING_LEAD_VREL, CLOSING_LEAD_TTC
+  MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, CLOSING_LEAD_VREL, CLOSING_LEAD_TTC, \
+  SMOOTH_ENTER, SMOOTH_EXIT, EARLY_BRAKE_PULLAWAY_VREL, EARLY_BRAKE_SPEED_BP, EARLY_BRAKE_SPEED_V, \
+  STOP_HOLD_EGO_V, STOP_HOLD_LEAD_V, STOP_HOLD_RELEASE_LEAD_V, STOP_HOLD_RELEASE_DREL, STOP_HOLD_ACCEL
 
 _ZERO_ACCEL_EPS = 1e-6
 
@@ -43,12 +47,19 @@ class AccelController:
     self._enabled: bool = self._params.get_bool("AccelPersonalityEnabled")
     self._personality = NORMAL  # cereal AccelerationPersonality ordinal
     self._v_ego = 0.0
+    self._lead_status = False
+    self._v_rel = 0.0
+    self._v_lead = 0.0
+    self._d_rel = 999.0
     self._lead_closing = False
     self._last_target_accel = 0.0
     self._brake_need = 0.0
     self._decel_target = 0.0
     self._smooth_active = False
+    self._smooth_latched = False
     self._bypassed = False
+    self._stop_held = False
+    self._stop_d_rel = 0.0
     self._read_params()
 
   def _read_params(self) -> None:
@@ -63,19 +74,35 @@ class AccelController:
     if self._frame % int(1. / DT_MDL) == 0:
       self._read_params()
     self._v_ego = sm['carState'].vEgo
-    self._lead_closing = self._compute_lead_closing(sm)
+    lead = sm['radarState'].leadOne
+    self._lead_status = bool(lead.status)
+    self._v_rel = float(lead.vRel) if lead.status else 0.0
+    self._v_lead = float(lead.vLead) if lead.status else 0.0
+    self._d_rel = float(lead.dRel) if lead.status else 999.0
+    self._lead_closing = self._compute_lead_closing()
+    self._update_stop_hold()
     self._frame += 1
 
-  @staticmethod
-  def _compute_lead_closing(sm: messaging.SubMaster) -> bool:
-    lead = sm['radarState'].leadOne
-    if not lead.status:
+  def _compute_lead_closing(self) -> bool:
+    if not self._lead_status or self._v_rel >= 0.0:
       return False
-    v_rel = float(lead.vRel)
-    if v_rel >= 0.0:
-      return False
-    ttc = float(lead.dRel) / max(-v_rel, 1e-3)
-    return v_rel <= CLOSING_LEAD_VREL or ttc <= CLOSING_LEAD_TTC
+    ttc = self._d_rel / max(-self._v_rel, 1e-3)
+    return self._v_rel <= CLOSING_LEAD_VREL or ttc <= CLOSING_LEAD_TTC
+
+  def _update_stop_hold(self) -> None:
+    # Latch a stop behind a near-stopped lead; release only on a real departure (not a 1m twitch).
+    if not self._enabled:
+      self._stop_held = False
+      return
+
+    if not self._stop_held:
+      if self._v_ego < STOP_HOLD_EGO_V and self._lead_status and self._v_lead < STOP_HOLD_LEAD_V:
+        self._stop_held = True
+        self._stop_d_rel = self._d_rel
+    else:
+      departed = (self._v_lead > STOP_HOLD_RELEASE_LEAD_V) or (self._d_rel - self._stop_d_rel > STOP_HOLD_RELEASE_DREL)
+      if departed or self._v_ego > 1.5 or not self._lead_status:
+        self._stop_held = False
 
   # --- positive accel levers ---
 
@@ -95,23 +122,44 @@ class AccelController:
     raw_target_accel = float(raw_target_accel)
     self._brake_need = self._compute_brake_need(raw_target_accel, accel_trajectory, t_idxs)
     self._decel_target = 0.0
+    out = self._smooth_core(raw_target_accel, should_stop, reset, stock_brake)
+    return self._apply_stop_hold(out)
 
+  def _smooth_core(self, raw_target_accel: float, should_stop: bool, reset: bool, stock_brake: bool) -> float:
     if reset or not self._enabled:
       self._bypassed = False
+      self._smooth_latched = False
       return self._passthrough(raw_target_accel)
 
     # e2e/blended path: never reshape braking (the planner already min-blends e2e/mpc, and vision stops
     # are the model's job per the lead->ACC policy).
     if stock_brake and (raw_target_accel < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED):
       self._bypassed = False
+      self._smooth_latched = False
       return self._passthrough(raw_target_accel)
 
     self._bypassed = self._emergency_bypass(raw_target_accel, should_stop)
     if self._bypassed:
+      self._smooth_latched = False
       return self._passthrough(raw_target_accel)
 
-    if self._brake_need < MIN_SMOOTH_BRAKE_NEED:
-      # no decel predicted: jerk-limit the (positive) accel, but never weaken an active brake
+    # A present lead pulling away makes a predicted decel spurious in a following context -> suppress
+    # the anticipatory brake (kills the low-speed rubber-band). The plan's real brake still passes below.
+    eff_brake_need = self._brake_need
+    if self._lead_status and self._v_rel > EARLY_BRAKE_PULLAWAY_VREL:
+      eff_brake_need = 0.0
+    # Taper the anticipatory brake out at low speed (stop-and-go rubber-band zone).
+    eff_brake_need *= float(np.interp(self._v_ego, EARLY_BRAKE_SPEED_BP, EARLY_BRAKE_SPEED_V))
+
+    # hysteresis: engage only on a clear predicted decel, hold until it clearly clears (no toggling)
+    if self._smooth_latched:
+      if eff_brake_need < SMOOTH_EXIT:
+        self._smooth_latched = False
+    elif eff_brake_need >= SMOOTH_ENTER:
+      self._smooth_latched = True
+
+    if not self._smooth_latched:
+      # no anticipatory brake: jerk-limit the (positive) accel, but never weaken an active brake
       self._smooth_active = False
       slewed = self._slew(raw_target_accel)
       out = min(slewed, raw_target_accel) if raw_target_accel < 0.0 else slewed
@@ -119,11 +167,18 @@ class AccelController:
 
     # decel predicted: front-load a gentle EARLY target, but NEVER weaker than the plan.
     self._smooth_active = True
-    self._decel_target = self.get_decel_target(self._brake_need)
+    self._decel_target = self.get_decel_target(eff_brake_need)
     commanded = min(raw_target_accel, self._decel_target)        # early-soft onset, can only brake >= plan
     slewed = self._slew(commanded)
     return self._finalize(min(slewed, raw_target_accel))         # post-slew clamp: never weaker than plan
 
+  def _apply_stop_hold(self, out: float) -> float:
+    # Latched stop: hold gently (no creep), but never weaken a deeper plan brake.
+    if self._stop_held:
+      out = self._clean_accel(min(out, STOP_HOLD_ACCEL))
+      self._last_target_accel = out
+    return out
+
   def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float:
     min_accel = float(raw_target_accel)
     for accel, t in zip(accel_trajectory, t_idxs, strict=False):
@@ -193,3 +248,6 @@ class AccelController:
 
   def bypassed(self) -> bool:
     return self._bypassed
+
+  def stop_held(self) -> bool:
+    return self._stop_held

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

@@ -44,7 +44,7 @@ RISE_RATE = {
 # Gentle, human-like progression: lead the brake early and softly rather than late and hard.
 SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4]
 SMOOTH_DECEL_V = {
-  ECO:    [0.00, -0.10, -0.24, -0.44, -0.68, -0.92, -1.15],
+  ECO:    [0.00, -0.08, -0.20, -0.38, -0.60, -0.82, -1.05],
   NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40],
   SPORT:  [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65],
 }
@@ -53,7 +53,7 @@ SMOOTH_DECEL_V = {
 # Deepening only shapes the EARLY front-loaded brake; the never-weaken clamp lets a real plan brake
 # through immediately, so a soft deepening rate never delays genuine braking.
 BRAKE_DEEPENING_JERK = {
-  ECO:    0.6,
+  ECO:    0.5,
   NORMAL: 0.8,
   SPORT:  1.0,
 }
@@ -80,3 +80,23 @@ HARD_BRAKE_NEED = 2.6
 # accel thresholds above (mirrors the route 000003da lesson - shaping must yield to closing dynamics).
 CLOSING_LEAD_VREL = -8.0   # m/s, lead approaching faster than this
 CLOSING_LEAD_TTC = 4.0     # s, time-to-collision below this
+
+# --- early-soft-braking consistency (anti rubber-band, route 00000423 finding) ---
+# Hysteresis on engagement so the anticipatory brake doesn't toggle on transient predicted-decel dips.
+SMOOTH_ENTER = 0.40   # brake_need to ENGAGE early-soft braking
+SMOOTH_EXIT = 0.15    # brake_need to DISENGAGE once engaged
+# A present lead pulling away makes the model's predicted decel spurious in a following context;
+# suppress the anticipatory brake then (the plan's real brake still passes via never-weaken).
+EARLY_BRAKE_PULLAWAY_VREL = 0.5   # m/s, lead opening faster than this -> no anticipatory brake
+# Taper the anticipatory brake out at low speed: in stop-and-go it only adds rubber-band and the stops
+# are owned by stop-hold / should_stop anyway (route 00000423: the flips lived < 7 m/s). The plan's real
+# brake still passes via never-weaken, so no braking is lost - only the spurious early shaping.
+EARLY_BRAKE_SPEED_BP = [6.0, 9.0]   # m/s
+EARLY_BRAKE_SPEED_V = [0.0, 1.0]    # anticipatory-brake gain
+
+# --- stop-hold / anti-creep (no double-stop / roll, route 00000423 finding) ---
+STOP_HOLD_EGO_V = 0.5             # ego considered stopped below this (m/s)
+STOP_HOLD_LEAD_V = 1.0            # lead considered stopped below this (m/s)
+STOP_HOLD_RELEASE_LEAD_V = 1.5    # lead clearly departing -> release latch (m/s); above a 1m twitch
+STOP_HOLD_RELEASE_DREL = 2.0      # ... or the gap opened this much from the stop (m)
+STOP_HOLD_ACCEL = -0.3            # gentle hold while latched (no creep); never weakens a deeper plan brake

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

@@ -185,6 +185,80 @@ def test_e2e_brake_passthrough():
   assert not ctrl.smooth_active()
 
 
+# --- rubber-band fixes: pull-away suppression + hysteresis ---
+
+def test_lead_pullaway_suppresses_early_brake():
+  # lead pulling away (vRel>0.5) -> model-predicted decel is spurious -> no anticipatory brake
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=10.0, lead_status=True, v_rel=1.5, d_rel=20.0))
+  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False)
+  assert not ctrl.smooth_active()
+  assert out >= -_EPS
+
+
+def test_low_speed_tapers_anticipatory_brake():
+  # at low speed the anticipatory brake is tapered out (rubber-band zone); plan brake still passes
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=3.0))   # < 6 m/s -> gain 0
+  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False)
+  assert not ctrl.smooth_active()
+  assert out >= -_EPS
+  # same predicted decel at speed engages
+  ctrl.update(make_sm(v_ego=15.0))
+  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False)
+  assert ctrl.smooth_active()
+
+
+def test_smooth_hysteresis_no_toggle():
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0))
+  # brake_need 0.25 (between EXIT 0.15 and ENTER 0.40): does NOT engage from idle
+  ctrl.smooth_target_accel(0.0, flat_traj(-0.25), T_IDXS, should_stop=False)
+  assert not ctrl.smooth_active()
+  # clear decel engages
+  ctrl.smooth_target_accel(0.0, flat_traj(-0.6), T_IDXS, should_stop=False)
+  assert ctrl.smooth_active()
+  # drops to 0.25 (still > EXIT): stays engaged (hysteresis, no toggle)
+  ctrl.smooth_target_accel(0.0, flat_traj(-0.25), T_IDXS, should_stop=False)
+  assert ctrl.smooth_active()
+
+
+# --- stop-hold / anti-creep (double-stop fix) ---
+
+def test_stop_hold_prevents_creep():
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0))  # stopped behind stopped lead
+  assert ctrl.stop_held()
+  out = ctrl.smooth_target_accel(0.3, flat_traj(0.3), T_IDXS, should_stop=False)  # plan wants to creep
+  assert out <= 0.0  # creep suppressed
+
+
+def test_stop_hold_ignores_lead_twitch():
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0))
+  assert ctrl.stop_held()
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=1.0, d_rel=5.8))  # twitch: vLead 1.0<1.5, dRel +0.8<2.0
+  assert ctrl.stop_held()
+
+
+def test_stop_hold_releases_on_real_departure():
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0))
+  assert ctrl.stop_held()
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=2.0, d_rel=7.5))  # lead clearly departing (vLead 2.0)
+  assert not ctrl.stop_held()
+  out = ctrl.smooth_target_accel(0.5, flat_traj(0.5), T_IDXS, should_stop=False)
+  assert out > 0.0  # launch allowed
+
+
+def test_stop_hold_disabled_is_stock():
+  ctrl = make_controller(enabled=False)
+  ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0))
+  assert not ctrl.stop_held()  # never latches when disabled (off == stock)
+  out = ctrl.smooth_target_accel(0.3, flat_traj(0.3), T_IDXS, should_stop=False)
+  assert out == pytest.approx(0.3, abs=_EPS)
+
+
 # --- param sanitation ---
 
 def test_out_of_range_personality_clamps():