mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-25 14:12:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(long): comfort stop

Browse Source
This commit is contained in:
rav4kumar
2026-06-24 12:39:37 -07:00
parent f9e7974e02
commit e09bc59ea3
3 changed files with 48 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+18 -22
View File
@@ -28,8 +28,8 @@ 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_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, COMFORT_STOP_MIN_GAP, \
COMFORT_STOP_MAX_DECEL, COMFORT_STOP_JERK, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP
COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP
_ZERO_ACCEL_EPS = 1e-6
@@ -133,30 +133,26 @@ class AccelController:
return min(shaped, spread)
def _comfort_stop(self, out: float, reset: bool) -> float:
# Low-speed comfort decel-to-stop behind a near-stopped lead. Unlike the old enforcer it slews IN (no entry
# grab) and low-passes raw-radar dRel (deepening rate-limited). It tracks the kinematic decel a_req both ways
# while approaching, BUT holds strictly monotone (never weakens) inside the final-approach window so it cannot
# self-release into a roll; outside that window it may weaken at the release rate when a creeping lead pulls
# away (no phantom brake into an opening gap). min(out, floor) keeps it never weaker than the plan. Off => no-op.
# 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:
self._stop_floor = 0.0 # disengaged/disabled: drop the latch, pure passthrough
return out
engaged = (self._lead_status and self._lead_vlead < COMFORT_STOP_LEAD_V
and self._lead_d > 0.1 and self._v_ego < COMFORT_STOP_V)
if engaged and self._v_ego >= COMFORT_STOP_RELEASE_V:
gap = self._lead_d - COMFORT_STOP_GAP
a_req = max(-(self._v_ego ** 2) / (2.0 * max(gap, COMFORT_STOP_MIN_GAP)), COMFORT_STOP_MAX_DECEL)
lo = self._stop_floor - COMFORT_STOP_JERK * DT_MDL # deepest allowed this frame (slew-in, no grab)
hi = self._stop_floor + BRAKE_RELEASE_JERK * DT_MDL # shallowest allowed this frame (release rate)
tracked = min(hi, max(lo, a_req)) # track a_req, rate-limited both directions
if gap > COMFORT_STOP_HOLD_GAP:
self._stop_floor = min(0.0, tracked) # gap still open -> may weaken if the lead pulls away
else:
self._stop_floor = min(tracked, self._stop_floor) # final approach -> strict monotone hold (no roll)
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:
# Stop episode over (lead moving / launched / standstill handoff): ease the floor toward 0 at the release
# jerk. This matches _shape's own _slew_up release rate (BRAKE_RELEASE_JERK), so the floor decays in
# lockstep with the natural output -> no added launch drag, and no release-direction step (no snap).
# 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
sunnypilot/selfdrive/controls/lib/accel_personality/constants.py
+12 -12
View File
@@ -72,17 +72,17 @@ 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. Behind a (near-)stopped lead, bring the car to rest at COMFORT_STOP_GAP with a
# MONOTONE decel that slews IN (no entry grab) and never self-releases early (so the car does not roll the
# final metre). min(plan, floor) keeps it never weaker than the plan; the monotone + slew-in also low-passes
# raw-radar dRel steps (a farther/noisier dRel can only be ignored, never injected as a deeper grab). Replaces
# the old self-releasing v^2/(2*gap) enforcer, which grabbed at v~3 then released into a roll. Off => no-op.
# 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_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: target standstill gap (radar dRel); roomier than the stock crawl-in (~3.7-4.4m)
COMFORT_STOP_MIN_GAP = 1.0 # m: kinematic denominator floor (gentle; no 1/x blow-up near the target)
COMFORT_STOP_MAX_DECEL = -1.6 # m/s^2: gentle cap -> never a grab
COMFORT_STOP_JERK = 1.0 # m/s^3: slew-IN / deepen rate of the comfort floor (no step on engage)
COMFORT_STOP_RELEASE_V = 0.3 # m/s: below this, ease the floor out (release jerk) -> smooth stock standstill handoff
COMFORT_STOP_HOLD_GAP = 2.0 # m: within this of the target gap = final approach -> strict monotone hold (no roll);
# beyond it the floor may WEAKEN at the release rate if a creeping lead pulls away
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)
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py
+18 -17
View File
@@ -14,7 +14,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \
STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, OVERBITE_CAP, \
STOP_PASSTHROUGH_V, ONSET_SPREAD_MAX, COMFORT_STOP_MAX_DECEL, AccelerationPersonality
STOP_PASSTHROUGH_V, ONSET_SPREAD_MAX, AccelerationPersonality
T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0]
_EPS = 1e-6
@@ -229,24 +229,25 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes():
assert ctrl.smooth_active()
def test_comfort_stop_brakes_approaching_stopped_lead():
# Approaching a near-stopped lead at low speed: the comfort floor adds a gentle decel below the easing plan
# so the car stops cleanly instead of crawling in, and stays within the gentle cap (never a grab).
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)
ctrl.update(make_sm(v_ego=2.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
out = 0.0
for _ in range(30):
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
assert out < -0.1 - _EPS # deeper than the easing plan (no creep-in)
assert out >= COMFORT_STOP_MAX_DECEL - _EPS # but gentle (capped), never a grab
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)
def test_comfort_stop_slews_in_no_grab():
# First engaged frame must NOT step to the cap -- the floor slews in from 0 (no entry grab / jerk).
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)
ctrl.update(make_sm(v_ego=2.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
first = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
assert first > -0.2 # ~ -0.1 plan + a tiny slewed-in floor, not -1.6
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():
@@ -304,10 +305,10 @@ 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)
for _ in range(20): # build a deep comfort floor approaching a stopped lead
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(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
assert ctrl._stop_floor < -0.2 # floor is engaged/deep
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))