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

refactor(long): cleam up

Browse Source
This commit is contained in:
rav4kumar
2026-06-30 10:03:52 -07:00
parent c0e08181df
commit bc498ea6af
7 changed files with 68 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py
+8 -6
View File
@@ -43,10 +43,12 @@ def test_dec_model_stop_target_not_reintroduced():
def test_long_feature_gates():
# comfort_stop OFF: keep the stock smooth taper (flat-hold firms the end); farther-stop comes from the MPC
# stop-target shift instead. vLead speed-damp (B) stays OFF pending on-road proof.
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import COMFORT_STOP_ENABLED
from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import VLEAD_DAMP_ENABLED
# The surviving opt-in long features all default OFF (byte-stock until enabled + on-road verified):
# AccelController jerk-limiter, RadarDistance lead-smoother + stop-gap bias.
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import JERK_LIMIT_ENABLED
from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
LEAD_SMOOTH_ENABLED, STOP_GAP_BIAS_ENABLED
assert COMFORT_STOP_ENABLED is False
assert VLEAD_DAMP_ENABLED is False
assert JERK_LIMIT_ENABLED is False
assert LEAD_SMOOTH_ENABLED is False
assert STOP_GAP_BIAS_ENABLED is False
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+23 -97
View File
@@ -4,13 +4,12 @@ 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.
Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT), an anticipatory
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,
transient lag) so a gentle brake does not land as a step. The front-load and comfort stop only ever ADD
braking (min(., plan)). Disabled => byte-stock.
Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT), an anticipatory brake
front-load, and a TTC-scaled brake-jerk limiter (smooth decel). 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, never softened. Only on the NON-emergency path may the onset arrive spread
by at most ONSET_SPREAD_MAX (a bounded transient lag). The front-load only ever ADDS braking (min(., plan)),
and the jerk limiter caps the deepening RATE only (never the magnitude). Disabled => byte-stock.
"""
from collections.abc import Sequence
@@ -28,12 +27,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, MIN_SMOOTH_BRAKE_NEED, \
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, \
GAS_SUPPRESS_ENABLED, GAS_SUPPRESS_DREL, GAS_SUPPRESS_VREL, GAS_SUPPRESS_CLOSE, \
GAS_SUPPRESS_RECENT_T, GAS_SUPPRESS_BRAKE_THR, \
PHYSICS_CAP_ENABLED, PHYS_CAP_MIN_TTC, PHYS_CAP_MIN_DREL, PHYS_CAP_TGAP, PHYS_CAP_MIN_GAP, PHYS_CAP_VREL_MARGIN, \
PHYS_CAP_FORGET_T, PHYS_CAP_MIN_A
JERK_LIMIT_ENABLED, BRAKE_JERK_SMOOTH, BRAKE_JERK_URGENT, BRAKE_JERK_TTC_HI, BRAKE_JERK_TTC_LO
_ZERO_ACCEL_EPS = 1e-6
@@ -55,14 +49,7 @@ class AccelController:
self._lead_status = False
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
self._gas_suppress_enabled = GAS_SUPPRESS_ENABLED
self._physics_cap_enabled = PHYSICS_CAP_ENABLED
self._cap_vrel = 0.0 # held worst-case (most-closing) lead for the physics cap
self._cap_dRel = 1e9
self._cap_vlead = 0.0
self._since_brake_frames = 10 ** 6 # frames since last brake output (gas-suppress recency)
self._jerk_limit_enabled = JERK_LIMIT_ENABLED
self._read_params()
def _read_params(self) -> None:
@@ -104,8 +91,7 @@ class AccelController:
self._bypassed = False
out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs, stock_brake)
out = self._comfort_stop(out, reset) # low-speed monotone comfort decel-to-stop (replaces the self-releasing enforcer)
out = self._physics_decel_cap(out, reset) # don't over-brake a closing lead that has room (brakes < stock)
out = self._brake_jerk_limit(out, reset) # TTC-scaled deepening-jerk cap: smooth onset where there is room
return self._finalize(out)
def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs, stock_brake: bool) -> float:
@@ -127,7 +113,6 @@ 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
@@ -135,49 +120,21 @@ 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 _physics_decel_cap(self, out: float, reset: bool) -> float:
# On a closing lead with genuine room, cap the brake at the kinematic decel needed to settle at a comfortable
# gap -- the stock MPC over-brakes a slower lead at speed. Only softens (max(out, a_phys)). Uses a HELD
# worst-case closing (decaying ~PHYS_CAP_FORGET_T) so a benign lead-flicker frame cannot relax it, and only
# acts when the closing itself warrants a real brake (a_phys <= PHYS_CAP_MIN_A) so it never softens a brake
# meant for another cause (curve / vision / a closer lead). Guarded to TTC + distance, pessimistic vRel
# margin, self-disengaging as room shrinks (full stock brake returns).
if reset or not self._lead_status:
self._cap_vrel, self._cap_dRel, self._cap_vlead = 0.0, 1e9, 0.0
else:
vrel = self._lead_vlead - self._v_ego
if vrel < self._cap_vrel: # adopt a more-closing lead immediately
self._cap_vrel, self._cap_dRel, self._cap_vlead = vrel, self._lead_d, self._lead_vlead
else: # forget an old threat slowly
f = DT_MDL / PHYS_CAP_FORGET_T
self._cap_vrel += (vrel - self._cap_vrel) * f
self._cap_dRel += (self._lead_d - self._cap_dRel) * f
self._cap_vlead += (self._lead_vlead - self._cap_vlead) * f
if not self._enabled or not self._physics_cap_enabled or out >= 0.0 or not self._lead_status:
return out
hv, hd, hl = self._cap_vrel, self._cap_dRel, self._cap_vlead
if hv >= -0.5 or hd < PHYS_CAP_MIN_DREL or hd / -hv < PHYS_CAP_MIN_TTC:
return out
room = hd - max(PHYS_CAP_MIN_GAP, PHYS_CAP_TGAP * hl)
if room <= 1.0:
return out
a_phys = -((hv - PHYS_CAP_VREL_MARGIN) ** 2) / (2.0 * room)
if a_phys > PHYS_CAP_MIN_A: # lead-closing alone does not warrant a real brake
return out
return max(out, a_phys) # only ever softens; never below the needed decel
def _lead_ttc(self) -> float:
if not self._lead_status:
return 1e3 # no lead -> no urgency
vrel = self._lead_vlead - self._v_ego
if vrel >= -0.1:
return 1e3 # not closing -> no urgency
return self._lead_d / -vrel
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 _brake_jerk_limit(self, out: float, reset: bool) -> float:
# Cap how fast the brake DEEPENS, scaled by TTC: roomy -> gentle (smooth onset), urgent -> fast (no delay).
# Rate only -- never changes the magnitude, so it can never under-brake; an emergency just ramps quickly.
if reset or not self._enabled or not self._jerk_limit_enabled or out >= self._last_target_accel:
return out # releasing/steady or off: nothing to limit
jmax = float(np.interp(self._lead_ttc(), [BRAKE_JERK_TTC_LO, BRAKE_JERK_TTC_HI], [BRAKE_JERK_URGENT, BRAKE_JERK_SMOOTH]))
return max(out, self._last_target_accel - jmax * DT_MDL)
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.
@@ -189,30 +146,6 @@ class AccelController:
spread = min(spread, raw + ONSET_SPREAD_MAX) # never more than the bounded lag weaker
return min(shaped, spread)
def _comfort_stop(self, out: float, reset: bool) -> float:
# Low-speed ANTI-CREEP HOLD behind a near-stopped lead. In the final-approach window it HOLDS the deepest
# decel the PLAN itself commanded this episode (gentle-capped at COMFORT_STOP_MAX_DECEL), so the brake does
# not ease off / creep in before the car is stopped (no roll, slightly roomier). It is NEVER firmer than the
# plan -- it only stops the brake from WEAKENING -- so it can never add a hard bite (the old kinematic
# enforcer demanded a firm ~-1.6 grab; this does not). Outside the window (gap opening as a creeping lead
# pulls away / lead moving / launch / standstill) the floor eases out at the release rate. min(out, floor)
# keeps it never weaker than the plan. Off => no-op (off==stock).
if reset or not self._enabled or not self._comfort_stop_enabled:
self._stop_floor = 0.0 # disabled/gated/reset: drop the latch, pure passthrough
return out
final_approach = (self._lead_status and self._lead_vlead < COMFORT_STOP_LEAD_V and self._lead_d > 0.1
and COMFORT_STOP_RELEASE_V <= self._v_ego < COMFORT_STOP_V
and self._lead_d - COMFORT_STOP_GAP <= COMFORT_STOP_HOLD_GAP)
if final_approach:
plan_hold = max(out, COMFORT_STOP_MAX_DECEL) # the plan's own decel, gentle-capped (never firmer)
self._stop_floor = min(plan_hold, self._stop_floor) # latch the deepest -> hold through the plan's ease
else:
# Not final approach (cruise / gap opening / lead moving / launch / standstill): ease the floor toward 0 at
# the release rate. Matches _shape's own _slew_up rate, so the floor decays in lockstep with the natural
# output -> no launch drag, no release-direction snap, no phantom brake into an opening gap.
self._stop_floor = min(0.0, self._stop_floor + BRAKE_RELEASE_JERK * DT_MDL)
return min(out, self._stop_floor) if self._stop_floor < 0.0 else out
def _stop_imminent(self, speed_trajectory: Sequence[float] | None, t_idxs: Sequence[float]) -> bool:
# plan predicts a near-stop within the lookahead -> a stop is coming (lead or light/sign).
if speed_trajectory is None:
@@ -255,7 +188,6 @@ 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
@@ -283,9 +215,3 @@ class AccelController:
def bypassed(self) -> bool:
return self._bypassed
def comfort_stop_floor(self) -> float:
return self._stop_floor
def comfort_stop_active(self) -> bool:
return self._stop_floor < 0.0
sunnypilot/selfdrive/controls/lib/accel_personality/constants.py
+8 -39
View File
@@ -72,42 +72,11 @@ STOP_PASSTHROUGH_V = 5.0 # m/s
ONSET_SPREAD_MAX = 0.25 # m/s^2: max the output may lag (be weaker than) the live plan, non-emergency only
ONSET_SPREAD_JERK = 2.5 # m/s^3: rate the spread output deepens back toward the plan
# Low-speed comfort stop = ANTI-CREEP HOLD (not a brake adder). In the final approach behind a (near-)stopped
# lead it HOLDS the deepest decel the PLAN itself has commanded (gentle-capped), so the brake does not ease
# off / creep in before the car is stopped (no roll, slightly roomier). It is NEVER firmer than the plan, so
# it can never add a hard bite -- the stop stays as gentle as the plan's own decel. Outside the final approach
# (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.
COMFORT_STOP_ENABLED = False # off: keeps the stock smooth taper (flat-hold firms the end). Farther-stop is via the MPC stop-target shift, not this.
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
COMFORT_STOP_MAX_DECEL = -1.6 # m/s^2: backstop cap on the held decel (a brief plan spike is not held firmer than this)
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.
# Physics decel cap: on a closing lead with genuine room, don't brake HARDER than kinematics require to settle
# at a comfortable gap (stock MPC over-brakes a slower lead at high speed via its big comfort-distance cost).
# The ONLY feature that brakes LESS than stock -- guarded to roomy situations (TTC + distance), pessimistic
# vRel margin, self-disengaging as room shrinks (full stock brake returns). Gated OFF by default.
PHYSICS_CAP_ENABLED = False
PHYS_CAP_MIN_TTC = 4.0 # s: only cap when TTC above this (room to brake gently)
PHYS_CAP_MIN_DREL = 30.0 # m: only cap when the lead is farther than this
PHYS_CAP_TGAP = 1.6 # s: target time-gap to settle at
PHYS_CAP_MIN_GAP = 20.0 # m: floor on the target gap
PHYS_CAP_VREL_MARGIN = 1.5 # m/s: treat the lead as closing this much faster (pessimistic -> firmer cap)
PHYS_CAP_FORGET_T = 1.0 # s: decay of the held worst-case closing (a benign flicker frame cannot relax the cap)
PHYS_CAP_MIN_A = -0.5 # m/s^2: only cap if the closing lead itself warrants at least this brake
# (else the brake is for another cause -- curve / vision / a closer lead -- leave it)
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
# TTC-scaled brake-jerk limiter: cap how fast the brake DEEPENS, scaled by urgency. Roomy (high TTC) -> gentle
# onset (smooth decel, no jerk); urgent (low TTC) -> fast (no delay on a real emergency). Caps the RATE only,
# never the magnitude, so it cannot under-brake -- only smooths the onset where there is room. Gated OFF.
JERK_LIMIT_ENABLED = False
BRAKE_JERK_SMOOTH = 1.5 # m/s^3: deepening-jerk cap when roomy (TTC >= BRAKE_JERK_TTC_HI)
BRAKE_JERK_URGENT = 8.0 # m/s^3: deepening-jerk cap when urgent (TTC <= BRAKE_JERK_TTC_LO; ~unconstrained)
BRAKE_JERK_TTC_HI = 6.0 # s: at/above this TTC -> full smooth limit
BRAKE_JERK_TTC_LO = 2.5 # s: at/below this TTC -> full fast limit
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py
+22 -179
View File
@@ -39,12 +39,10 @@ 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, gas_suppress=False, physics_cap=False):
def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, jerk_limit=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._physics_cap_enabled = physics_cap # physics decel cap is gated off in production; opt in per-test
ctrl._jerk_limit_enabled = jerk_limit # brake-jerk limiter is gated off in production; opt in per-test
ctrl.update(make_sm())
return ctrl
@@ -232,190 +230,35 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes():
assert ctrl.smooth_active()
def test_comfort_stop_holds_through_plan_ease():
# Plan brakes to a peak then eases off near the stop (the stock creep). The hold keeps the deeper decel so
# the brake does not ease in (no roll) -- but NEVER firmer than the plan's own peak (no added hard bite).
ctrl = make_controller(personality=ECO, comfort_stop=True)
out = 0.0
for plan in [-0.4, -0.8, -1.1, -1.1, -0.6, -0.3, -0.1]: # decel to a -1.1 peak, then ease (creep)
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
assert out < -0.3 - _EPS # held deeper than the easing plan (-0.1) -> no creep-in
assert out >= -1.1 - _EPS # but never firmer than the plan's own peak (no -1.6 bite)
# --- TTC-scaled brake-jerk limiter (smooth decel) ----------------------------
def test_comfort_stop_never_firmer_than_plan():
# The hold can only stop the brake from WEAKENING; it never commands a decel firmer than the plan itself.
ctrl = make_controller(personality=ECO, comfort_stop=True)
for plan in [-0.2, -0.5, -0.9, -0.9, -0.9]: # steady (no ease) -> hold matches plan, adds nothing
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
assert out == pytest.approx(plan, abs=_EPS) # never firmer than the (non-easing) plan -> no bite/grab
def test_comfort_stop_monotone_no_early_release():
# While still moving, the comfort floor never WEAKENS frame-to-frame (the old enforcer self-released -> roll).
ctrl = make_controller(personality=ECO, comfort_stop=True)
floors = []
for v in [3.0, 2.6, 2.2, 1.8, 1.4, 1.0, 0.6]: # decelerating toward the lead
ctrl.update(make_sm(v_ego=v, lead_status=True, lead_d=max(0.5, 7.0 - (3.0 - v) * 2), lead_vlead=0.0))
ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
floors.append(ctrl._stop_floor)
for a, b in zip(floors, floors[1:], strict=False):
assert b <= a + _EPS # monotone non-weakening while approaching
def test_comfort_stop_off_when_disabled():
ctrl = make_controller(enabled=False, personality=ECO)
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=4.0, lead_vlead=0.0))
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
assert out == pytest.approx(-0.1, abs=_EPS)
def test_comfort_stop_gated_off_is_stock_passthrough():
# Production default (COMFORT_STOP_ENABLED off, even with AccelController enabled): the final approach is stock
# passthrough -- output follows the easing plan, no anti-creep hold, floor stays 0 (goal 6 met by stock).
ctrl = make_controller(personality=ECO) # comfort_stop defaults False (production)
out = 0.0
for plan in [-0.4, -0.8, -1.1, -0.6, -0.1]: # decel to a peak then ease (stock creep)
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
assert out == pytest.approx(-0.1, abs=_EPS) # follows the easing plan -> no hold
assert ctrl._stop_floor == 0.0 # never latched
def test_comfort_stop_no_op_moving_lead():
# Moving lead (vLead high): no comfort stop (only behind a near-stopped lead).
ctrl = make_controller(personality=ECO, comfort_stop=True)
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=5.0))
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
assert out == pytest.approx(-0.1, abs=_EPS)
def test_comfort_stop_never_weaker():
# The comfort floor only ever ADDS braking: output never weaker than the plan.
ctrl = make_controller(personality=ECO, comfort_stop=True)
for raw in (-0.05, -0.3, -1.0, -2.5):
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.0))
out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
assert out <= raw + _EPS
def test_comfort_stop_weakens_when_gap_opens():
# Creeping stop-and-go lead (vLead stays < COMFORT_STOP_LEAD_V) that pulls away: once the gap opens well past
# the target the floor must WEAKEN, not hold a phantom brake into an opening gap.
ctrl = make_controller(personality=ECO, comfort_stop=True)
for _ in range(15): # approach close -> deep floor (final-approach hold)
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.3))
ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
deep = ctrl._stop_floor
assert deep < -0.3
for _ in range(25): # lead creeps away (still vLead<1): gap opens wide
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=12.0, lead_vlead=0.5))
ctrl.smooth_target_accel(-0.05, flat_traj(-0.05), T_IDXS, should_stop=False)
assert ctrl._stop_floor > deep + 0.3 # floor weakened as the gap opened (no phantom brake)
def test_comfort_stop_releases_on_launch():
# Stop-and-go GO: after holding a comfort floor at a stop, once the lead moves and the plan wants throttle the
# floor must release (track the plan up) and not hold the output below the natural plan -> the car launches.
ctrl = make_controller(personality=ECO, comfort_stop=True)
for _ in range(20): # hold the plan's -1.0 decel approaching a stopped lead
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)
assert ctrl._stop_floor < -0.5 # floor holds the plan's decel (engaged/deep)
out = 0.0
for _ in range(30): # lead launches, plan wants throttle
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=8.0, lead_vlead=4.0))
out = ctrl.smooth_target_accel(0.8, flat_traj(0.8), T_IDXS, should_stop=False)
assert out > 0.0 # launches (floor did not hold it back)
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):
def _jl(ctrl, v_ego, lead_d, lead_vlead, raw=-2.8):
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_jerk_limit_smooths_roomy_onset():
out = _jl(make_controller(jerk_limit=True), 34.7, 84.0, 23.7) # TTC 7.6s -> gentle 1.5 m/s^3
assert -0.2 < out < 0.0 # first frame rate-limited, not slammed to -2.8
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_jerk_limit_lets_urgent_through_fast():
out = _jl(make_controller(jerk_limit=True), 16.0, 30.0, 0.0) # vRel -16, TTC 1.9s -> fast 8 m/s^3
assert out < -0.3 # ramps fast, far firmer than the roomy case
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)
# --- physics decel cap (no over-brake on a roomy closing lead) ---------------
def _closing(ctrl, v_ego, lead_d, lead_vlead, raw):
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_physics_cap_softens_roomy_overbrake():
# the 0488 t600 case: lead 84m, 23.7 m/s, ego 34.7 (vRel -11, TTC 7.6s). -2.8 over-brakes; physics ~ -1.7.
out = _closing(make_controller(physics_cap=True), 34.7, 84.0, 23.7, -2.8)
assert -2.1 < out < -1.2
def test_physics_cap_never_touches_close_lead():
out = _closing(make_controller(physics_cap=True), 20.0, 18.0, 9.0, -2.8) # dRel < MIN_DREL
def test_jerk_limit_off_passthrough():
out = _jl(make_controller(), 34.7, 84.0, 23.7)
assert out == pytest.approx(-2.8, abs=_EPS)
def test_physics_cap_never_touches_low_ttc():
out = _closing(make_controller(physics_cap=True), 30.0, 35.0, 5.0, -2.8) # vRel -25 -> TTC ~1.4s
assert out == pytest.approx(-2.8, abs=_EPS)
def test_jerk_limit_no_lead_gentle():
ctrl = make_controller(jerk_limit=True)
ctrl.update(make_sm(v_ego=20.0)) # no lead -> no urgency
out = ctrl.smooth_target_accel(-2.8, flat_traj(-2.8), T_IDXS, should_stop=False)
assert -0.2 < out < 0.0
def test_physics_cap_off_passthrough():
out = _closing(make_controller(), 34.7, 84.0, 23.7, -2.8) # default off
assert out == pytest.approx(-2.8, abs=_EPS)
def test_physics_cap_only_softens_never_hardens():
out = _closing(make_controller(physics_cap=True), 34.7, 84.0, 23.7, -0.5) # already gentler than physics
assert out > -1.5 # cap does not deepen it
def test_physics_cap_skips_brake_not_from_lead():
# barely-closing far lead (vRel -1) but a real -1.5 brake (curve/vision/closer lead): a_phys ~ 0 -> don't cap.
out = _closing(make_controller(physics_cap=True), 34.0, 100.0, 33.0, -1.5)
assert out == pytest.approx(-1.5, abs=_EPS)
def test_jerk_limit_does_not_limit_release():
ctrl = make_controller(jerk_limit=True)
ctrl._last_target_accel = -2.0
ctrl.update(make_sm(v_ego=20.0, lead_status=True, lead_d=40.0, lead_vlead=18.0))
assert ctrl._brake_jerk_limit(-0.5, reset=False) == pytest.approx(-0.5, abs=_EPS) # releasing -> not limited
def test_onset_spread_bounded_and_skipped_for_emergency():
sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
-2
View File
@@ -156,8 +156,6 @@ class LongitudinalPlannerSP:
acceleration.decelTarget = float(self.accel.decel_target())
acceleration.smoothActive = self.accel.smooth_active()
acceleration.bypassed = bool(self.accel.bypassed())
acceleration.comfortStopActive = bool(self.accel.comfort_stop_active())
acceleration.comfortStopFloor = float(self.accel.comfort_stop_floor())
acceleration.leadUnstable = bool(self.radar_distance.lead_unstable())
sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py
+6 -46
View File
@@ -4,13 +4,12 @@ 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 smooths the lead the longitudinal MPC follows on a noisy radar, never reporting a
RadarDistance conditions the lead the longitudinal MPC follows on a noisy radar, never reporting a
farther-or-faster lead than reality, so braking is always >= stock:
- flicker-hold: keep a just-dropped, recently-sustained lead alive through a radar dropout.
- speed damp: lag the lead speeding up (instant on slow-down) to damp the catch-up surge / rubber-band,
reset on a track switch so it never carries a stale-slow value across a different track.
Active only above LOW_SPEED_PASSTHROUGH_V; at/below it returns the raw radarstate (byte-stock stops).
Default off => stock passthrough.
- lead-jitter smoother: de-jitter a churning (trackId-flipping) lead so the MPC does not hunt the gap.
- stop-gap bias: report a near-stopped lead slightly closer so the MPC stops a touch farther back.
Also publishes a read-only lead-instability flag (telemetry). Default off => stock passthrough.
"""
from collections import deque
@@ -27,11 +26,7 @@ MIN_HELD_DREL = 0.5
LOW_SPEED_PASSTHROUGH_V = 5.0 # m/s
# 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
SWITCH_DREL = 8.0 # m, dRel jump = a track switch (used by the instability detector)
# Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead.
STABILITY_WINDOW = 5 # frames (~0.25s @ 20Hz)
@@ -59,21 +54,6 @@ STOP_BIAS_RAMP_BAND = 2.0 # m: ramp-in band (full offset below REGIME_DREL -
STOP_BIAS_MIN_DREL = 2.0 # m: never report a lead closer than this
class _LeadView:
__slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
def __init__(self, src, vlead):
self.status = src.status
self.dRel = src.dRel
self.yRel = src.yRel
self.vRel = src.vRel
self.vLead = vlead
self.vLeadK = vlead
self.aLeadK = src.aLeadK
self.aLeadTau = src.aLeadTau
self.modelProb = src.modelProb
class _BiasedLead:
__slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
@@ -164,8 +144,6 @@ class _LeadHold:
self._since_real = 0
self._armed = False
self._held_dRel = 0.0
self._vlead_f = None
self._last_dRel = None
def reset(self):
self.__init__()
@@ -191,21 +169,6 @@ class _LeadHold:
self._armed = False
return raw
def smooth(self, lead):
if not lead.status:
self._vlead_f = None
self._last_dRel = None
return lead
if self._last_dRel is None or abs(lead.dRel - self._last_dRel) > SWITCH_DREL:
self._vlead_f = lead.vLead
self._last_dRel = lead.dRel
v = float(lead.vLead)
if self._vlead_f is None or v <= self._vlead_f:
self._vlead_f = v
return lead
self._vlead_f += (v - self._vlead_f) * _VLEAD_ALPHA
return _LeadView(lead, self._vlead_f)
class _LeadStability:
# Read-only monitor: flags an unstable leadOne -- bimodal/bouncing vLead, dRel track-switch jumps, or
@@ -249,7 +212,6 @@ class RadarDistanceController:
self._frame = 0
self._v_ego = 0.0
self._enabled = self._params.get_bool("RadarDistance")
self._vlead_damp_enabled = VLEAD_DAMP_ENABLED
self._stop_gap_bias_enabled = STOP_GAP_BIAS_ENABLED
self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED
self._one = _LeadHold()
@@ -302,6 +264,4 @@ class RadarDistanceController:
one = self._stop_gap_bias(one)
if self._lead_smooth_enabled:
one = self._smoother.update(one, self._stability.churn) # de-jitter a churning lead (anti follow-hunt)
if not self._vlead_damp_enabled:
return _RadarStateProxy(one, two) # flicker-hold (A) only
return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two))
return _RadarStateProxy(one, two)
sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py
+1 -30
View File
@@ -36,10 +36,9 @@ def obstacle(ld):
return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE)
def ctrl(enabled=True, vlead_damp=False):
def ctrl(enabled=True):
c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0 # default above the gate so hold-logic tests exercise the flicker-hold
c._vlead_damp_enabled = vlead_damp # speed-damp (B) is gated off in production; opt in per-test
return c
@@ -108,34 +107,6 @@ def test_low_speed_passthrough_but_hold_warmed_for_highway():
assert out.leadOne.status is True
def test_vlead_lags_rise_instant_fall():
c = ctrl(vlead_damp=True) # speed-damp (B) under test; gated off in production
c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0))) # seed at 15
rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
assert 15.0 <= rising.vLead < 25.0 # rise lagged (<= real -> never faster than real)
falling = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=8.0))).leadOne
assert falling.vLead == pytest.approx(8.0, abs=1e-6) # slow-down instant
def test_vlead_resets_on_track_switch_no_phantom_slow():
# the old bug: a slow lead's filtered speed carried across a switch to a fast farther track, reporting it
# near-stopped. A dRel jump (track switch) now resets the filter -> the new track's real speed is reported.
c = ctrl(vlead_damp=True) # speed-damp (B) under test; gated off in production
for _ in range(3):
c.smooth_radarstate(rs(lead(dRel=12.0, vLead=0.5))) # slow close lead
switched = c.smooth_radarstate(rs(lead(dRel=80.0, vLead=18.0))).leadOne # different, far, fast track
assert switched.vLead == pytest.approx(18.0, abs=1e-6) # real speed, not the stale ~0.5
def test_vlead_damp_gated_off_reports_real_speed():
# Production default (VLEAD_DAMP_ENABLED off): a speeding-up lead is NOT lagged -> real vLead passes through
# (flicker-hold A only). This is the on-by-default behavior; B is opt-in pending on-road proof.
c = ctrl() # vlead_damp defaults off (production)
c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0)))
rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
assert rising.vLead == pytest.approx(25.0, abs=1e-6) # no damp -> real speed
# --- lead-instability detector (telemetry) -----------------------------------
def test_stability_quiet_on_clean_lead():