feat(long): gas suppression near a lead

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

@@ -5,8 +5,7 @@ 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 (gated OFF by default via COMFORT_STOP_ENABLED -- stops pass
-through stock). SAFETY: a firm/closing brake -- emergency (raw <=
+brake front-load, and a low-speed comfort stop. 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,
@@ -30,7 +29,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
   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_ENABLED, COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
-  COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP
+  COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP, \
+  GAS_SUPPRESS_ENABLED, GAS_SUPPRESS_DREL, GAS_SUPPRESS_VREL, GAS_SUPPRESS_CLOSE, \
+  GAS_SUPPRESS_RECENT_T, GAS_SUPPRESS_BRAKE_THR
 
 _ZERO_ACCEL_EPS = 1e-6
 
@@ -53,7 +54,9 @@ 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._comfort_stop_enabled = COMFORT_STOP_ENABLED
+    self._gas_suppress_enabled = GAS_SUPPRESS_ENABLED
+    self._since_brake_frames = 10 ** 6                   # frames since last brake output (gas-suppress recency)
     self._read_params()
 
   def _read_params(self) -> None:
@@ -117,6 +120,7 @@ class AccelController:
       target = min(raw, self._decel_target)
       if raw > 0.0:
         target = max(target, 0.0)                              # plan wants throttle -> ease the gas early, never fabricate a brake
+    target = self._suppress_gas_near_lead(target, raw)
     slewed = self._slew(target)
     if raw >= 0.0:
       return slewed
@@ -124,6 +128,19 @@ class AccelController:
       return min(slewed, raw)                                  # blended/e2e: the model owns the brake -> strict never-weaker
     return self._onset_spread(slewed, raw)                     # non-emergency brake: bounded onset spread (<= ONSET_SPREAD_MAX weaker)
 
+  def _suppress_gas_near_lead(self, target: float, raw: float) -> float:
+    # Coast instead of accelerating toward a close lead: T1 recent brake + lead not pulling away, or T2 clearly
+    # closing. Only reduces accel, never a brake. Off => no-op.
+    if not self._gas_suppress_enabled or raw <= 0.0 or not self._lead_status:
+      return target
+    if not 0.1 < self._lead_d < GAS_SUPPRESS_DREL:
+      return target
+    closing = self._lead_vlead - self._v_ego
+    recent_brake = self._since_brake_frames * DT_MDL < GAS_SUPPRESS_RECENT_T
+    if (recent_brake and closing < GAS_SUPPRESS_VREL) or closing < GAS_SUPPRESS_CLOSE:
+      return min(target, 0.0)
+    return target
+
   def _onset_spread(self, shaped: float, raw: float) -> float:
     # Scoped softening: on a NON-emergency brake the onset may arrive spread instead of stepping to the plan.
     # The output deepens toward the plan jerk-limited at ONSET_SPREAD_JERK and may lag it by at most
@@ -200,6 +217,7 @@ class AccelController:
   def _finalize(self, target_accel: float) -> float:
     target_accel = self._clean_accel(target_accel)
     self._last_target_accel = target_accel
+    self._since_brake_frames = 0 if target_accel < GAS_SUPPRESS_BRAKE_THR else self._since_brake_frames + 1
     return target_accel
 
   @staticmethod

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

@@ -79,11 +79,7 @@ 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_ENABLED = False      # gated off: final-approach stops pass through stock
 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
@@ -91,3 +87,13 @@ COMFORT_STOP_MAX_DECEL = -1.6     # m/s^2: backstop cap on the held decel (a bri
 COMFORT_STOP_RELEASE_V = 0.3      # m/s: below this, ease the floor out (release rate) -> smooth stock standstill handoff
 COMFORT_STOP_HOLD_GAP = 2.0       # m: within this of the reference gap = final-approach window where the hold applies;
                                   # beyond it the floor eases out (a creeping lead opening the gap -> no phantom brake)
+
+# Gas suppression near a lead: coast instead of accelerating toward a close lead, in two cases (OR) --
+# T1 we braked for it within RECENT_T and it is still not pulling away (closing < VREL); T2 we are clearly
+# gaining on it (closing < CLOSE). Only reduces accel, never a brake; opening/far lead keeps its gas.
+GAS_SUPPRESS_ENABLED = False
+GAS_SUPPRESS_DREL = 60.0          # m: lead within this distance
+GAS_SUPPRESS_VREL = 0.5           # m/s: "not pulling away" bound for the rebound trigger (vLead - vEgo)
+GAS_SUPPRESS_CLOSE = -1.5         # m/s: closing rate below which gas is suppressed outright
+GAS_SUPPRESS_RECENT_T = 3.0       # s: a brake within this long counts as recent
+GAS_SUPPRESS_BRAKE_THR = -0.30    # m/s^2: output below this is a "brake" for the recency latch

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

@@ -39,10 +39,11 @@ 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, comfort_stop=False):
+def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=False, gas_suppress=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._gas_suppress_enabled = gas_suppress   # gas-suppression is gated off in production; opt in per-test
   ctrl.update(make_sm())
   return ctrl
 
@@ -330,6 +331,59 @@ def test_comfort_stop_releases_on_launch():
   assert ctrl._stop_floor == 0.0                               # floor fully released
 
 
+# --- gas suppression near a non-opening lead ---------------------------------
+
+def _gas(ctrl, v_ego, lead_d, lead_vlead, raw=0.4):
+  ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+  return ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+
+def _brake_then(ctrl, v_ego, lead_d, lead_vlead, raw):
+  for _ in range(3):                                           # brake for the lead -> set the recency latch
+    ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+    ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
+  ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+  return ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+
+def test_gas_suppress_T2_closing_lead_coasts():
+  # T2: clearly gaining on the lead (closing -2.5) -> suppress outright (the 0480 t448 case).
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5)
+  assert out <= _EPS                                           # coast, never a fabricated brake
+  assert out >= -0.5 - _EPS
+
+def test_gas_suppress_T1_rebound_after_brake():
+  # T1: braked for a matched lead, then plan wants gas again within RECENT_T -> suppress the rebound (t1302 case).
+  ctrl = make_controller(gas_suppress=True)
+  out = _brake_then(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=20.0, raw=0.3)
+  assert out <= _EPS
+
+def test_gas_suppress_allows_matched_lead_without_recent_brake():
+  # The deliberate narrowing: a steady matched lead we did NOT just brake for keeps its gas (no normal-following drag).
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=20.0)   # closing 0, no recent brake
+  assert out > 0.0
+
+def test_gas_suppress_allows_gas_when_lead_opening():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=24.0)   # lead pulling away +4 -> keep up (neither trigger)
+  assert out > 0.0
+
+def test_gas_suppress_allows_gas_when_lead_far():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=80.0, lead_vlead=20.5)   # closing -2.5 but beyond GAS_SUPPRESS_DREL -> allow
+  assert out > 0.0
+
+def test_gas_suppress_off_by_default_is_stock_gas():
+  ctrl = make_controller()                                     # gas_suppress defaults False (production)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5)
+  assert out > 0.0                                             # stock gas passes through
+
+def test_gas_suppress_does_not_touch_brake():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5, raw=-0.5)   # plan brakes
+  assert out <= 0.0                                            # suppression no-ops on a brake (never weakens it)
+
+
 def test_onset_spread_bounded_and_skipped_for_emergency():
   # Non-emergency brake onset is spread (lagged) but never by more than ONSET_SPREAD_MAX; an emergency brake
   # is instant full depth (no spread).

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

@@ -27,18 +27,13 @@ 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.
+# Speed-damp (B) gated off (caused phantom braking + launch rubber-band before); flicker-hold (A) runs alone.
 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
 
-# Lead-instability detector (telemetry only, no control effect yet): flags a bimodal/bouncing radar lead --
-# the signature behind the residual goal-2 firm brakes (vLead jumping between two tracks, dRel stepping).
-# Validated on routes 047f/0480: fires 0.6-0.9% of following frames, concentrated at the firm-brake events.
+# 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"
 
@@ -132,9 +127,7 @@ class _LeadHold:
 
 
 class _LeadStability:
-  # Read-only lead-quality monitor. Watches raw leadOne for the bimodal/bouncing signature (vLead range over a
-  # short window, or repeated dRel track-switch jumps). Pure telemetry -- it conditions nothing, just reports a
-  # flag so we can size how often the residual firm brakes are radar-instability driven before building a fix.
+  # Read-only monitor: flags a bimodal/bouncing leadOne (vLead range, or repeated dRel track-switches). Telemetry.
   def __init__(self):
     self._v = deque(maxlen=STABILITY_WINDOW)
     self._d = deque(maxlen=STABILITY_WINDOW)
@@ -166,10 +159,10 @@ 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._vlead_damp_enabled = VLEAD_DAMP_ENABLED
     self._one = _LeadHold()
     self._two = _LeadHold()
-    self._stability = _LeadStability()   # lead-instability telemetry (informational, no control effect)
+    self._stability = _LeadStability()
 
   def _read_params(self) -> None:
     enabled = self._params.get_bool("RadarDistance")
@@ -191,7 +184,7 @@ class RadarDistanceController:
     return self._stability.unstable
 
   def smooth_radarstate(self, radarstate):
-    self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry; runs every cycle, even when disabled
+    self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry, runs every cycle
     if not self._enabled:
       return radarstate
     one = self._one.step(radarstate.leadOne)
@@ -199,5 +192,5 @@ class RadarDistanceController:
     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(one, two)                       # flicker-hold (A) only
     return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two))