From bc498ea6af70f0ad8da77db9672025ec4e52c11c Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 30 Jun 2026 10:03:52 -0700 Subject: [PATCH] refactor(long): cleam up --- .../test_longitudinal_smoothing_wiring.py | 14 +- .../lib/accel_personality/accel_controller.py | 120 ++--------- .../lib/accel_personality/constants.py | 47 +--- .../tests/test_accel_controller.py | 201 ++---------------- .../controls/lib/longitudinal_planner.py | 2 - .../lib/radar_distance/radar_distance.py | 52 +---- .../tests/test_radar_distance.py | 31 +-- 7 files changed, 68 insertions(+), 399 deletions(-) diff --git a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py index 0cb47fe46e..d99035d3ff 100644 --- a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py +++ b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index d7854bdac5..0f71d632ea 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index cc00eb99c5..2854522211 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -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 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 32d2db9686..4d0fd057b0 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 @@ -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(): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 4796bac48e..8d90571159 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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()) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 15e654cb2d..6c3e496b5f 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -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) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 2b88329a56..bc925007e8 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py @@ -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():