From eea9dbcf2862fe2192c43984d07dc639d75c8e5c Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 2 Jul 2026 11:52:45 -0700 Subject: [PATCH] feat(long): stop farther from a stopped lead + gentler eco onset --- .../lib/accel_personality/constants.py | 10 +-- .../lib/radar_distance/radar_distance.py | 73 +++++++++++++++---- .../tests/test_radar_distance.py | 66 +++++++++++++++-- 3 files changed, 121 insertions(+), 28 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 8500b83616..61c3bd717e 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -28,14 +28,14 @@ A_CRUISE_MAX_BP = [0., 10., 25., 40.] # m/s (matches upstream A_CRU STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6] # upstream A_CRUISE_MAX_VALS -> off == byte-stock ceiling STOCK_RISE_RATE = 0.05 # upstream ceiling open-rate (m/s^2 per cycle) A_CRUISE_MAX_V = { - ECO: [1.80, 1.10, 0.70, 0.50], # gentle-but-prompt launch, efficient cruise + ECO: [1.55, 0.95, 0.45, 0.30], # responsive off the line, LAZY at highway speed (mileage) NORMAL: [2.00, 1.40, 0.95, 0.70], # brisk launch, balanced cruise SPORT: [2.00, 1.70, 1.20, 0.90], # strong launch (ACCEL_MAX caps the 0 m/s knot), assertive cruise } -# Ceiling open-rate: how fast the accel ceiling may rise per cycle. All >> stock 0.05 so the permitted -# ceiling opens quickly off the line -> fast take-off from a stop. The MPC's own jerk/a_change cost still -# smooths the actual accel, so a higher open-rate cannot make the launch jerky. -RISE_RATE = {ECO: 0.10, NORMAL: 0.16, SPORT: 0.24} +# Ceiling open-rate: how fast the accel ceiling may rise per cycle. Above stock 0.05 so the permitted ceiling +# opens promptly off the line; ECO stays close to stock for the gentlest onset. The MPC's own jerk/a_change +# cost still smooths the actual accel. +RISE_RATE = {ECO: 0.07, NORMAL: 0.16, SPORT: 0.24} # --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------ # Add a small speed-dependent widen to the stock t_follow (the driver's gap-button value). Wider gap -> diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 4b55c5c048..627dd38ad1 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -4,15 +4,15 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. 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. -RadarDistance de-noises the lead the longitudinal MPC follows on a noisy (TSS2-class) radar. It NEVER -reports a farther-or-faster lead than reality, so braking is always >= stock, and it changes nothing about -the desired gap -- that is the AccelController's job (t_follow). Two mechanisms only: +RadarDistance conditions the lead the longitudinal MPC follows on a noisy (TSS2-class) radar. It NEVER +reports a farther-or-faster lead than reality, so braking is always >= stock. Three mechanisms: * flicker-hold: keep a just-dropped, recently-sustained lead alive (dead-reckoned) through a brief radar dropout so the MPC does not lose and re-grab it (which reads as a phantom release then a catch-up brake); * churn smoother: a short SYMMETRIC EMA on a trackId-churning lead's dRel/vLead/vRel so the MPC stops - hunting the gap (removes the follow-jitter that reads as rubber-banding). -Also publishes a read-only lead-instability flag (telemetry). Below LOW_SPEED_PASSTHROUGH_V it is a -byte-stock passthrough (stop distance stays exactly stock). Disabled => byte-stock passthrough always. + hunting the gap (removes the follow-jitter that reads as rubber-banding); + * stop-gap: near a (near-)stopped lead at low speed report dRel a touch closer so the MPC's own smooth stop + settles farther back (the Prius TSS2 stock crawl creeps in to ~1.5 m). Monotone (closer => brake >= stock). +Also publishes a read-only lead-instability flag (telemetry). Disabled => byte-stock passthrough always. """ from collections import deque @@ -45,6 +45,32 @@ ID_CHURN = 3 # trackId switches in the window above which the le LEAD_SMOOTH_TAU = 0.5 # s, EMA time constant LEAD_SMOOTH_HOLD = 20 # frames (~1s): keep smoothing through brief churn gaps (churn toggles on/off) +# Stop-gap: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_M closer so the MPC's own +# smooth stop terminates that much farther back (stock Prius crawl-creeps to ~1.5 m). Monotone (only reports +# closer => brake >= stock). Ramps in below the regime dRel and out as the lead starts moving; releases on +# launch as ego speed rises past STOP_GAP_VEGO. +STOP_GAP_M = 2.5 # m: max dRel reduction = added standstill gap +STOP_GAP_VEGO = 8.0 # m/s: only below this ego speed +STOP_GAP_VLEAD = 1.5 # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this +STOP_GAP_REGIME_DREL = 12.0 # m: bias ramps in below this dRel +STOP_GAP_RAMP_BAND = 2.0 # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND) +STOP_GAP_MIN_DREL = 2.0 # m: never report a lead closer than this + + +class _BiasedLead: + __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') + + def __init__(self, src, dRel): + self.status = src.status + self.dRel = dRel + 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 _SmoothedLead: __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') @@ -214,16 +240,33 @@ class RadarDistanceController: def lead_unstable(self) -> bool: return self._stability.unstable + def _stop_gap_bias(self, lead): + # Report a (near-)stopped lead up to STOP_GAP_M closer at low speed so the MPC's own smooth stop ends + # that much farther back. Monotone (only ever reports closer). No-op outside the regime. + if not lead.status or lead.vLead > STOP_GAP_VLEAD or self._v_ego > STOP_GAP_VEGO or lead.dRel <= STOP_GAP_MIN_DREL: + return lead + d_ramp = min(max((STOP_GAP_REGIME_DREL - lead.dRel) / STOP_GAP_RAMP_BAND, 0.0), 1.0) + v_ramp = min(max((STOP_GAP_VLEAD - lead.vLead) / STOP_GAP_VLEAD, 0.0), 1.0) + offset = STOP_GAP_M * d_ramp * v_ramp + if offset < 0.05: + return lead + return _BiasedLead(lead, max(lead.dRel - offset, STOP_GAP_MIN_DREL)) + def smooth_radarstate(self, radarstate): self._stability.update(radarstate.leadOne, self._v_ego) # telemetry, runs every cycle - if not self._enabled or self._v_ego < CREEP_PASSTHROUGH_V: - return radarstate # off / full standstill: byte-stock (stock stop) - if self._v_ego < LOW_SPEED_PASSTHROUGH_V: - # creep band: churn de-jitter ONLY (symmetric EMA, mean-preserving), no flicker-hold. Smooths the - # radar jitter that makes stop-and-go feel like gas-brake-gas-brake, without holding a stale lead. + if not self._enabled: + return radarstate # off: byte-stock passthrough + two = radarstate.leadTwo + if self._v_ego >= LOW_SPEED_PASSTHROUGH_V: + one = self._one.step(radarstate.leadOne) # flicker-hold ... + two = self._two.step(radarstate.leadTwo) + one = self._smoother.update(one, self._stability.churn) # ... + churn de-jitter (anti follow-hunt) + elif self._v_ego >= CREEP_PASSTHROUGH_V: + # creep band: churn de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch) one = self._smoother.update(radarstate.leadOne, self._stability.churn) - return radarstate if one is radarstate.leadOne else _RadarStateProxy(one, radarstate.leadTwo) - one = self._one.step(radarstate.leadOne) # >= LOW_SPEED: flicker-hold ... - two = self._two.step(radarstate.leadTwo) - one = self._smoother.update(one, self._stability.churn) # ... + churn de-jitter (anti follow-hunt) + else: + one = radarstate.leadOne # full standstill: no hold/smoothing + one = self._stop_gap_bias(one) # low-speed near-stopped: settle farther back + if one is radarstate.leadOne and two is radarstate.leadTwo: + return radarstate # nothing changed -> byte-stock object return _RadarStateProxy(one, two) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 07447b72bb..9dafd2eee2 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py @@ -15,7 +15,8 @@ from types import SimpleNamespace import pytest from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \ - RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, CREEP_PASSTHROUGH_V, DROPOUT_DREL + RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, CREEP_PASSTHROUGH_V, \ + DROPOUT_DREL, STOP_GAP_MIN_DREL, STOP_GAP_VEGO, STOP_GAP_VLEAD, STOP_GAP_REGIME_DREL COMFORT_BRAKE = 2.5 @@ -70,20 +71,21 @@ def test_valid_lead_passthrough(): assert out.leadOne is one # clean lead, no churn -> unchanged -def test_full_standstill_returns_raw_object(): - # Full standstill (< CREEP_PASSTHROUGH_V): ENABLED returns the EXACT raw radarstate object (byte-identical - # to OFF) so the stock stop distance is preserved and no stale lead is held near a stop. +def test_full_standstill_outside_stopgap_is_passthrough(): + # Full standstill (< CREEP_PASSTHROUGH_V), lead OUTSIDE the stop-gap regime (far): no hold, no smoothing, + # no bias -> the EXACT raw radarstate object (byte-identical). The stop-gap only engages inside its regime. c = ctrl(v_ego=CREEP_PASSTHROUGH_V - 0.5) - r = rs(lead(dRel=3.0, vLead=0.5)) + r = rs(lead(dRel=STOP_GAP_REGIME_DREL + 8.0, vLead=0.5)) assert c.smooth_radarstate(r) is r def test_creep_dejitters_churn_but_no_hold(): # Creep band [CREEP, LOW_SPEED): the churn smoother runs (de-jitter -> smooth stop-and-go), but the # flicker-hold does NOT (a dropped/departed lead must not be held, or launch would be delayed). + # vLead>STOP_GAP_VLEAD so the stop-gap stays out and this isolates the EMA. c = ctrl(v_ego=(CREEP_PASSTHROUGH_V + LOW_SPEED_PASSTHROUGH_V) / 2) out = None - for f in churn_frames(30, d_a=6.0, d_b=8.0, vLead=1.0): + for f in churn_frames(30, d_a=6.0, d_b=8.0, vLead=3.0): out = c.smooth_radarstate(rs(f)) assert 6.0 < out.leadOne.dRel < 8.0 # jitter smoothed # a dropout in the creep band is NOT held -> raw passes through (no stale lead) @@ -92,12 +94,60 @@ def test_creep_dejitters_churn_but_no_hold(): def test_creep_clean_lead_passthrough(): - # creep band, steady single lead (no churn) -> smoother inert -> exact raw object (stop distance unbiased) + # creep band, steady moving lead (no churn, outside stop-gap regime) -> exact raw object (unbiased) c = ctrl(v_ego=(CREEP_PASSTHROUGH_V + LOW_SPEED_PASSTHROUGH_V) / 2) - r = rs(lead(dRel=4.0, vLead=1.5, radarTrackId=3)) + r = rs(lead(dRel=4.0, vLead=2.5, radarTrackId=3)) assert c.smooth_radarstate(r) is r +# --- stop-gap (settle farther back from a near-stopped lead) ---------------------------------------------- + +def test_stop_gap_pulls_stopped_lead_closer(): + c = ctrl(v_ego=2.0) + one = lead(dRel=6.0, vLead=0.0, vRel=-1.0) + out = c.smooth_radarstate(rs(one)) + assert out.leadOne.dRel < 6.0 # reported closer -> MPC stops farther back + assert obstacle(out.leadOne) <= obstacle(one) + 1e-6 # brake >= stock (obstacle never farther) + + +def test_stop_gap_monotone_never_farther(): + c = ctrl(v_ego=3.0) + for d in (4.0, 6.0, 9.0, 11.0): + out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.0))) + assert out.leadOne.dRel <= d + 1e-6 + + +def test_stop_gap_min_floor(): + c = ctrl(v_ego=2.0) + out = c.smooth_radarstate(rs(lead(dRel=STOP_GAP_MIN_DREL + 0.5, vLead=0.0))) + assert out.leadOne.dRel >= STOP_GAP_MIN_DREL - 1e-6 + + +def test_stop_gap_off_when_disabled(): + c = ctrl(enabled=False, v_ego=2.0) + r = rs(lead(dRel=6.0, vLead=0.0)) + assert c.smooth_radarstate(r) is r # disabled -> stock stop distance + + +def test_stop_gap_moving_lead_no_change(): + c = ctrl(v_ego=2.0) + out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=STOP_GAP_VLEAD + 1.0))) + assert out.leadOne.dRel == pytest.approx(6.0) # lead moving -> not a stop + + +def test_stop_gap_high_speed_no_change(): + c = ctrl(v_ego=STOP_GAP_VEGO + 2.0) + out = c.smooth_radarstate(rs(lead(dRel=6.0, vLead=0.0))) + assert out.leadOne.dRel == pytest.approx(6.0) # above the stop regime -> unbiased + + +def test_stop_gap_far_lead_no_change(): + c = ctrl(v_ego=2.0) + d = STOP_GAP_REGIME_DREL + 5.0 + out = c.smooth_radarstate(rs(lead(dRel=d, vLead=0.0))) + assert out.leadOne.dRel == pytest.approx(d) # beyond the ramp-in regime -> unbiased + + def test_low_speed_override_lead_passthrough(): # radard low_speed_override emits a real closest-track lead with modelProb=0.0. It must be honored, not # rejected in favor of a stale farther held lead (which would under-brake / stop too close).