mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-17 02:02:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(long): stop farther from a stopped lead + gentler eco onset

Browse Source
This commit is contained in:
rav4kumar
2026-07-02 11:52:45 -07:00
parent cb0bc5746d
commit eea9dbcf28
3 changed files with 121 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/constants.py
+5 -5
View File
@@ -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 ->
sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py
+58 -15
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py
+58 -8
View File
@@ -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).