From f9e7974e0237b5f90c8d8daa0a9848cfc8c01c92 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 24 Jun 2026 10:51:41 -0700 Subject: [PATCH] feat(long): comfort stop --- cereal/custom.capnp | 2 + .../lib/accel_personality/accel_controller.py | 88 +++++++++---- .../lib/accel_personality/constants.py | 49 +++++--- .../tests/test_accel_controller.py | 118 ++++++++++++++---- .../controls/lib/longitudinal_planner.py | 2 + 5 files changed, 195 insertions(+), 64 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8fb6c81d73..3d0314d63b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -307,6 +307,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { decelTarget @4 :Float32; # early-soft comfort decel target (m/s^2, negative) smoothActive @5 :Bool; # early-soft braking currently shaping the target bypassed @6 :Bool; # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e) + comfortStopActive @7 :Bool; # low-speed comfort decel-to-stop floor currently governing (behind a near-stopped lead) + comfortStopFloor @8 :Float32; # comfort-stop floor commanded (m/s^2, negative; 0 when not engaged) } enum AccelerationPersonality { diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 2327f6946f..f1a3b26aa4 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -4,11 +4,13 @@ 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) plus an -anticipatory brake front-load. SAFETY INVARIANT: on the brake side the output is NEVER WEAKER than the -plan -- it can only be EQUAL or DEEPER (front-load). It never softens, delays, or rate-limits a brake, -so it can never under-brake a closing lead. Hard brakes, stops and low speed pass the plan straight -through (stock). Disabled => byte-stock. +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. """ from collections.abc import Sequence @@ -25,8 +27,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp STOCK_A_CRUISE_MAX_V, STOCK_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, OVERBITE_CAP, STOP_PASSTHROUGH_V, \ - STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \ - STOP_ENFORCE_V, STOP_ENFORCE_DIST, STOP_ENFORCE_RANGE, STOP_ENFORCE_LEAD_V, STOP_ENFORCE_MAX_DECEL, STOP_ENFORCE_MIN_GAP + 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 _ZERO_ACCEL_EPS = 1e-6 @@ -48,6 +51,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._read_params() def _read_params(self) -> None: @@ -88,11 +92,11 @@ class AccelController: self._smooth_active = False self._bypassed = False - out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs) - out = self._stop_enforce(out) # never-weaker low-speed floor: no creep inside the target stop gap + 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) return self._finalize(out) - def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs) -> float: + def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs, stock_brake: bool) -> float: # --- Full stock passthroughs (output is exactly the plan, no shaping) --- if reset or not self._enabled: return raw # disabled / reset @@ -100,29 +104,61 @@ class AccelController: return raw # stop/creep regime: braking is stock (no coast-in) self._bypassed = self._emergency_bypass(raw, should_stop) if self._bypassed or self._stop_imminent(speed_trajectory, t_idxs): - return raw # hard brake / coming stop: full strength, no delay + return raw # emergency / coming stop: full strength, no delay # Anticipatory front-load, capped at OVERBITE_CAP below the live plan (avoids an abrupt over-bite on a - # cut-in brake_need spike). min(., raw) keeps the output never weaker than the plan -> never under-brakes. + # cut-in brake_need spike). target = raw if self._brake_need >= MIN_SMOOTH_BRAKE_NEED: self._smooth_active = True self._decel_target = max(self.get_decel_target(self._brake_need), raw - OVERBITE_CAP) 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 slewed = self._slew(target) - return min(slewed, raw) if raw < 0.0 else slewed + if raw >= 0.0: + return slewed + if stock_brake: + 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 _stop_enforce(self, out: float) -> float: - # Never-weaker low-speed floor: bring the car to rest at STOP_ENFORCE_DIST behind a near-stopped lead, - # so the stock MPC's crawl-creep cannot park us inside the target gap. Disabled => no-op (off==stock). - if not (self._enabled and self._lead_status and 0.1 < self._v_ego < STOP_ENFORCE_V - and self._lead_vlead < STOP_ENFORCE_LEAD_V - and 0.1 < self._lead_d < STOP_ENFORCE_DIST + STOP_ENFORCE_RANGE): # only the final-approach creep zone + 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. + # The output deepens toward the plan jerk-limited at ONSET_SPREAD_JERK and may lag it by at most + # ONSET_SPREAD_MAX -- a tightly bounded, transient weaker-than-plan window that smooths the felt onset. + # Emergency brakes never reach here (raw passthrough in _shape), so a genuine hard brake is never softened. + # The front-load still wins when it is deeper (anticipation preserved). + spread = max(raw, self._last_target_accel - ONSET_SPREAD_JERK * DT_MDL) # deepen toward the plan, jerk-limited + 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 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. + if reset or not self._enabled: + self._stop_floor = 0.0 # disengaged/disabled: drop the latch, pure passthrough return out - gap = self._lead_d - STOP_ENFORCE_DIST # distance left before reaching the target gap - floor = -(self._v_ego ** 2) / (2.0 * max(gap, STOP_ENFORCE_MIN_GAP)) # gentle decel to stop at the target - floor = max(floor, STOP_ENFORCE_MAX_DECEL) # cap -> gentle hold, never a grab - return min(out, floor) # never weaker than the plan + 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) + 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). + 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). @@ -193,3 +229,9 @@ 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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index a745ca3ec2..5c11c13af9 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -28,20 +28,21 @@ A_CRUISE_MAX_V = { RISE_RATE = {ECO: 0.10, NORMAL: 0.15, SPORT: 0.22} # ceiling open-rate: all >> stock 0.05 for fast take-off # Anticipatory front-load: predicted brake need (m/s^2) -> early decel target (m/s^2). Starts a gentle -# decel early when a brake is predicted, so it arrives spread out, not as one late firm onset. One-sided -# (never weaker than the plan). -SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] +# decel early when a brake is predicted, so it arrives spread out, not as one late firm onset. The first +# knot sits AT the MIN_SMOOTH_BRAKE_NEED gate (0.00 there): below the gate there is no front-load, so there +# is no dead [0, gate) anchor and no step at the gate (the old [0.0 -> 0.00] knot was never evaluated). +SMOOTH_DECEL_BP = [0.4, 0.8, 1.2, 1.6, 2.0, 2.4] SMOOTH_DECEL_V = { - ECO: [0.00, -0.08, -0.20, -0.35, -0.55, -0.78, -1.00], - NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40], - SPORT: [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65], + ECO: [0.00, -0.20, -0.35, -0.55, -0.78, -1.00], + NORMAL: [0.00, -0.30, -0.55, -0.84, -1.12, -1.40], + SPORT: [0.00, -0.40, -0.72, -1.05, -1.35, -1.65], } BRAKE_DEEPENING_JERK = {ECO: 0.5, NORMAL: 0.8, SPORT: 1.0} BRAKE_RELEASE_JERK = 2.0 ACCEL_RISE_JERK = {ECO: 1.0, NORMAL: 1.5, SPORT: 2.2} # accel-onset jerk: higher = snappier take-off, stepped per tier SMOOTH_DECEL_LOOKAHEAD_T = 3.0 -MIN_SMOOTH_BRAKE_NEED = 0.2 +MIN_SMOOTH_BRAKE_NEED = 0.4 # below this no front-load (kills the faint low-brake_need drag + the gate-crossing toggle) # Cap how much DEEPER than the live plan the front-load may bite -> no abrupt over-bite on a cut-in # brake_need spike (binds only when the plan still wants throttle; once it brakes, the table wins). @@ -59,15 +60,29 @@ HARD_BRAKE_NEED = 2.6 STOP_IMMINENT_VEGO = 1.0 # m/s plan-predicted speed below this within the lookahead == stop coming STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s -# Below this ego speed the brake side is stock passthrough, so stop distance is byte-identical to off. +# Below this ego speed the brake side is stock passthrough (the comfort stop below adds the only low-speed +# shaping); the bounded onset-spread does not run here, so a stock stop is not rate-limited. STOP_PASSTHROUGH_V = 5.0 # m/s -# Low-speed stop-distance enforcer. The stock MPC loses gap-cost leverage at crawl and creeps inside -# STOP_DISTANCE behind a stopped lead. This is a never-weaker floor: command the gentle decel that brings -# the car to rest at STOP_ENFORCE_DIST and take min(plan, floor) -> only ever adds braking, self-targeting. -STOP_ENFORCE_V = 5.0 # m/s: only enforce at/below this ego speed -STOP_ENFORCE_DIST = 5.5 # m: target standstill gap (under STOP_DISTANCE=6 for the radar rear-of-lead offset) -STOP_ENFORCE_RANGE = 3.0 # m: only within DIST+RANGE of the lead (final-approach creep zone) -STOP_ENFORCE_LEAD_V = 1.5 # m/s: only behind a near-stopped lead -STOP_ENFORCE_MAX_DECEL = -1.8 # m/s^2: cap -> always a gentle hold, never a grab -STOP_ENFORCE_MIN_GAP = 0.5 # m: kinematic denominator floor +# Scoped onset-spread -- the ONLY place the output may be transiently WEAKER than the plan. On a NON-emergency +# brake the onset may arrive spread over a bounded ramp instead of stepping straight to the plan: the output +# may lag the plan by at most ONSET_SPREAD_MAX, deepening toward it at ONSET_SPREAD_JERK. A firm/closing brake +# (raw <= HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED, FCW/crash, should_stop, blended/e2e) skips +# this entirely (raw passthrough), so a real hard brake is never softened or delayed. +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. +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 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 a9af884ba3..1efabe98bd 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, AccelerationPersonality + STOP_PASSTHROUGH_V, ONSET_SPREAD_MAX, COMFORT_STOP_MAX_DECEL, 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 @@ -95,12 +95,15 @@ def test_rise_rate_ordering(): @pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) def test_never_weaker_than_plan_sustained(personality): - # Core safety invariant: on the brake side the output is NEVER weaker than the plan (only equal or deeper). + # Safety: an EMERGENCY brake is never weaker than the plan (strict). A non-emergency brake may lag the plan + # by at most ONSET_SPREAD_MAX (the bounded onset-spread) and no more. ctrl = make_controller(personality=personality) for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5, -2.0] + [-2.0] * 20: out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - if raw < 0.0: - assert out <= raw + _EPS + if raw <= HARD_BRAKE_TARGET_ACCEL: + assert out <= raw + _EPS # emergency: strict never-weaker + elif raw < 0.0: + assert out <= raw + ONSET_SPREAD_MAX + _EPS # non-emergency: bounded onset-spread only @pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) @@ -112,7 +115,7 @@ def test_never_weaker_random_walk(personality): traj = flat_traj(raw - float(rng.uniform(0.0, 0.6))) out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False) if raw < 0.0: - assert out <= raw + _EPS + assert out <= raw + ONSET_SPREAD_MAX + _EPS # never more than the bounded onset-spread weaker @pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) @@ -226,42 +229,109 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes(): assert ctrl.smooth_active() -def test_stop_enforce_brakes_when_mpc_creeps_inside_target(): - # Crawl behind a stopped lead inside the target gap: the stock plan eases off (~ -0.1), but the enforcer - # holds a gentle decel to stop at the target -> output is DEEPER than the easing plan (no creep-in). +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). ctrl = make_controller(personality=ECO) - ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=4.0, lead_vlead=0.0)) # inside 5.5m target, moving - out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False) - assert out < -0.1 - _EPS # enforcer added braking vs the easing plan - assert out >= -2.0 - _EPS # but gentle (capped), never a grab + 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 -def test_stop_enforce_off_when_disabled(): - # Disabled controller: enforcer is a no-op (off == stock). +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). + 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 + + +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) + 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=1.5, lead_status=True, lead_d=4.0, lead_vlead=0.0)) + 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_stop_enforce_no_op_past_target_and_moving_lead(): - # Past the target gap (lead far): no enforcement. Moving lead: no enforcement (only near-stopped leads). +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) - ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=12.0, lead_vlead=0.0)) # far -> no floor - assert ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False) == pytest.approx(-0.1, abs=_EPS) - ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=4.0, lead_vlead=4.0)) # moving lead -> no floor - assert ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False) == pytest.approx(-0.1, abs=_EPS) + 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_stop_enforce_never_weaker(): - # The enforcer only ever ADDS braking: output is never weaker than the plan. +def test_comfort_stop_never_weaker(): + # The comfort floor only ever ADDS braking: output never weaker than the plan. ctrl = make_controller(personality=ECO) - ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=4.5, lead_vlead=0.0)) 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) + 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) + for _ in range(20): # build a deep comfort floor 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 + 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 + + +def test_onset_spread_bounded_and_skipped_for_emergency(): + # Non-emergency brake onset is spread (lagged) but never by more than ONSET_SPREAD_MAX; an emergency brake + # is instant full depth (no spread). + ctrl = make_controller(personality=ECO) + for _ in range(3): + ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False) # non-emergency step + assert out > -1.0 + _EPS # lagged (spread), not an instant step + assert out <= -1.0 + ONSET_SPREAD_MAX + _EPS # but bounded + ctrl2 = make_controller(personality=ECO) + for _ in range(3): + ctrl2.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) + out2 = ctrl2.smooth_target_accel(-2.0, flat_traj(-2.0), T_IDXS, should_stop=False) # emergency (<= -1.5) + assert out2 == pytest.approx(-2.0, abs=_EPS) + + def test_disabled_hard_brake_is_instant_stock(): ctrl = make_controller(enabled=False, personality=ECO) out = ctrl.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 97f3907930..c0b988bf8c 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -156,6 +156,8 @@ 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()) pm.send('longitudinalPlanSP', plan_sp_send)