From 9f20c56b2f8937ee8be910494d9267237e829598 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 17 Jun 2026 11:11:53 -0700 Subject: [PATCH] feat(long): roomier stops + smoother decel --- .../lib/accel_personality/accel_controller.py | 10 +++++++++- .../controls/lib/accel_personality/constants.py | 11 ++++++++--- .../tests/test_accel_controller.py | 13 +++++++++++++ 3 files changed, 30 insertions(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 3cffb50828..184043deef 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -17,7 +17,7 @@ from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \ SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \ - MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, \ + MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, STOP_APPROACH_VEGO, \ ONSET_JERK0, ONSET_JERK_GAIN, ONSET_GAP_SOFT, ONSET_GAP_GAIN, ONSET_JERK_MAX, ONSET_HANDBACK_JERK, \ SOFT_ONSET_MAX_BRAKE_NEED, SOFT_ONSET_MAX_INSTANT_ACCEL, SOFT_ONSET_REARM_FRAMES @@ -90,6 +90,14 @@ class AccelController: self._reset_onset() return self._passthrough(raw_target_accel) + # Low-speed creep-to-stop: stand down (incl. the brake_need-driven front-load, which softens even + # when raw~0). Softening the final approach to a stopped lead makes the car brake less -> coast + # farther -> halt too close (~1.3 m). Hand full stock decel through so it stops at the proper gap. + # Onset shaping still applies above STOP_APPROACH_VEGO. + if self._v_ego < STOP_APPROACH_VEGO: + self._reset_onset() + return self._passthrough(raw_target_accel) + if self._brake_need < MIN_SMOOTH_BRAKE_NEED: self._smooth_active = False slewed = self._slew(raw_target_accel) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 12471c7056..a49bb69463 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -47,6 +47,11 @@ MIN_SMOOTH_BRAKE_NEED = 0.2 HARD_BRAKE_TARGET_ACCEL = -1.5 HARD_BRAKE_NEED = 2.6 +# Below this ego speed the shaper stands down (full stock decel). Softening the creep-to-stop makes the +# car brake less -> coast farther -> halt too close to a stopped lead (~1.3 m). Stock decel below this +# speed stops at the proper gap; sub-3 m/s braking is gentle anyway. Onset shaping applies above it. +STOP_APPROACH_VEGO = 3.0 # m/s + # --- Convex brake-onset shaper (param-gated; ECO/SPORT only, NORMAL = stock passthrough) --- # The grabby bite is the raw MPC plan: stock deepening uses a CONSTANT jerk (integrates to a LINEAR # accel ramp) and min(slewed,raw) lets the deep raw plan win, so the bite passes through untouched. @@ -58,7 +63,7 @@ HARD_BRAKE_NEED = 2.6 # No velocity-debt feedback: it carried stale state across closely-spaced stop-and-go brakes and # over-braked the next onset (verified). NORMAL omitted -> shaper never runs. ONSET_JERK0 = {ECO: 0.15, SPORT: 0.25} # m/s^3 initial gentle jerk at the bite (target band 0.15-0.25) -ONSET_JERK_GAIN = {ECO: 1.2, SPORT: 2.0} # 1/s depth-proportional growth rate k (convex; bigger -> faster) +ONSET_JERK_GAIN = {ECO: 0.9, SPORT: 1.5} # 1/s depth-proportional growth rate k (lowered: gentler jerk-build = smoother decel, less "jerky") # Bounded softening: the gentle bite lags the plan (brakes shallower) at the very start. To keep the # softening modest (so it never feels like "no brakes"), an INSTANTANEOUS-gap catch-up adds jerk when @@ -67,11 +72,11 @@ ONSET_JERK_GAIN = {ECO: 1.2, SPORT: 2.0} # 1/s depth-proportional growth # softening then settles near ONSET_GAP_SOFT; the hard cap keeps the catch from ever being a grab. ONSET_GAP_SOFT = {ECO: 0.30, SPORT: 0.25} # m/s^2 tolerated shallower-than-plan gap before catch-up ONSET_GAP_GAIN = {ECO: 4.0, SPORT: 5.0} # 1/s extra jerk per m/s^2 of gap beyond ONSET_GAP_SOFT -ONSET_JERK_MAX = {ECO: 1.4, SPORT: 1.8} # m/s^3 hard ceiling on convex-path jerk while still armed/gentle +ONSET_JERK_MAX = {ECO: 1.1, SPORT: 1.4} # m/s^3 hard ceiling on convex-path jerk (lowered: smoother catch-up) # Fast hand-back: once the plan leaves the gentle zone (no longer armed) but a soft gap is still open, # close it at this FIRM jerk so the output catches the plan BEFORE braking gets firm -> no late-brake lag # into the [-1.5,-1.0] band. Jerk-limited (not a snap), and never deeper than the plan, so not a grab. -ONSET_HANDBACK_JERK = {ECO: 3.0, SPORT: 4.0} # m/s^3 gap-close rate when the plan has firmed past the gentle zone +ONSET_HANDBACK_JERK = {ECO: 2.2, SPORT: 3.0} # m/s^3 gap-close rate (lowered: gentler hand-back = less jounce/jerk) # Arm gates (conservative). Only shape genuinely gentle onsets; firm/deep onsets fall to the stock # never-weaker slew (they SHOULD bite). Two independent safety layers against late braking: (1) the 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 f3b68dfa4c..1a08b2084f 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 @@ -107,6 +107,19 @@ def test_early_soft_braking_brakes_before_plan(): assert ctrl.brake_need() == pytest.approx(1.0) +def test_low_speed_stop_approach_stands_down(): + # Below STOP_APPROACH_VEGO the shaper must NOT soften the creep-to-stop (softening -> coast farther -> + # halt too close). Full stock decel passes through; onset shaping re-engages above the threshold. + ctrl = make_controller(personality=ECO) + ctrl.update({'carState': SimpleNamespace(vEgo=2.0)}) + out = ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False) + assert not ctrl.smooth_active() + assert out == pytest.approx(-0.1, abs=_EPS) # stock passthrough, no front-load softening + ctrl.update({'carState': SimpleNamespace(vEgo=8.0)}) + ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False) + assert ctrl.smooth_active() # onset shaping still active above the threshold + + @pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) def test_never_weaker_than_plan_sustained_closing(personality): # NORMAL/off: strict never-weaker (route 000003da regression guard). ECO/SPORT: the convex onset may