From df54f8e0831c600c1fb205dec2116f28ce819a46 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 27 Jun 2026 11:21:46 -0700 Subject: [PATCH] feat(long): gas suppression near a lead --- .../lib/accel_personality/accel_controller.py | 26 +++++++-- .../lib/accel_personality/constants.py | 16 ++++-- .../tests/test_accel_controller.py | 56 ++++++++++++++++++- .../lib/radar_distance/radar_distance.py | 21 +++---- 4 files changed, 95 insertions(+), 24 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 10cf133d8f..c14ad131ce 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 8a3bb838c5..02e320b6a7 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 60c2c5f81c..6e792535c7 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/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). diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 348e32c7bc..b6f73a875e 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/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))