From e09bc59ea3fe71faa747e4d28debf71bf2add699 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 24 Jun 2026 12:39:37 -0700 Subject: [PATCH] fix(long): comfort stop --- .../lib/accel_personality/accel_controller.py | 40 +++++++++---------- .../lib/accel_personality/constants.py | 24 +++++------ .../tests/test_accel_controller.py | 35 ++++++++-------- 3 files changed, 48 insertions(+), 51 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index f1a3b26aa4..2b3e369308 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 5c11c13af9..0b37d53b2a 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -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) 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 1efabe98bd..36fe125bba 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 @@ -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))