diff --git a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py new file mode 100644 index 0000000000..34d4f17c40 --- /dev/null +++ b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py @@ -0,0 +1,52 @@ +import inspect +import re +from pathlib import Path + +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner + + +REPO_ROOT = Path(__file__).resolve().parents[3] + + +def test_smoothing_params_default_off(): + params_keys = (REPO_ROOT / "common/params_keys.h").read_text() + + assert re.search(r'"AccelPersonalityEnabled", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) + assert re.search(r'"RadarDistance", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) + + +def test_longitudinal_smoothing_stays_planner_side(): + update_src = inspect.getsource(LongitudinalPlanner.update) + + accel_ceiling_idx = update_src.index("self.accel.get_max_accel(v_ego)") + radar_smoothing_idx = update_src.index("self.mpc.update(self.smooth_radarstate(sm['radarState'])") + accel_smoothing_idx = update_src.index("self.accel.smooth_target_accel(") + + assert accel_ceiling_idx < radar_smoothing_idx + assert radar_smoothing_idx < accel_smoothing_idx + + +# Tokens for the reverted input-side DEC model-stop-target (capped v_target into the MPC pre-solve). It was +# superseded by DEC blended-mode and chased a source-fixed radar gate; it must not silently return. +_DEC_MODEL_STOP_TOKENS = ("apply_model_stop_target", "force_stop_requested", "_update_model_stop", "MODEL_STOP_TARGET_TIME") + + +def test_dec_model_stop_target_not_reintroduced(): + this_file = Path(__file__).resolve() + for sub in ("selfdrive/controls", "sunnypilot/selfdrive/controls"): + for path in (REPO_ROOT / sub).rglob("*.py"): + if path.resolve() == this_file: + continue # this guard names the tokens as strings + src = path.read_text() + for token in _DEC_MODEL_STOP_TOKENS: + assert token not in src, f"reverted DEC model-stop-target ({token}) re-introduced in {path}" + + +def test_comfort_stop_and_vlead_damp_gated_off(): + # Strategy invariants (tn @ 2026-06-26): final-approach stop passes through stock (goal 6 stock-met), and the + # input-side vLead speed-damp (B) stays off pending on-road proof. Flicker-hold (A) is unaffected by either. + 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 + + assert COMFORT_STOP_ENABLED is False + assert VLEAD_DAMP_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 2b3e369308..10cf133d8f 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -5,7 +5,8 @@ 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 <= +brake front-load, and a low-speed comfort stop (gated OFF by default via COMFORT_STOP_ENABLED -- stops pass +through stock). 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, @@ -28,7 +29,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_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \ + 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 _ZERO_ACCEL_EPS = 1e-6 @@ -52,6 +53,7 @@ class AccelController: 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 # gated OFF: stops pass through stock (goal 6 stock-met) self._read_params() def _read_params(self) -> None: @@ -140,8 +142,8 @@ class AccelController: # 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 + 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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 0b37d53b2a..8a3bb838c5 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -79,6 +79,11 @@ ONSET_SPREAD_JERK = 2.5 # m/s^3: rate the spread output deepens back t # (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. +# Gated OFF by default and independently of AccelPersonalityEnabled: the final approach passes through stock, +# because goal 6 (smooth coming to stop) is already met by the stock/Toyota tune (parks ~4.2m smoothly) and the +# old stops-too-close complaint traced to the radar_distance modelProb gate, since fixed at source. Re-enable +# (flip True) only if an on-road roll-in / creep is observed, ideally with a gentler COMFORT_STOP_MAX_DECEL (~-1.2). +COMFORT_STOP_ENABLED = False 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 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 36fe125bba..60c2c5f81c 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,9 +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): +def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=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.update(make_sm()) return ctrl @@ -232,7 +233,7 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes(): 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 = 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)) @@ -243,7 +244,7 @@ def test_comfort_stop_holds_through_plan_ease(): 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 = 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) @@ -252,7 +253,7 @@ def test_comfort_stop_never_firmer_than_plan(): 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) + 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)) @@ -269,9 +270,21 @@ def test_comfort_stop_off_when_disabled(): 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) + 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) @@ -279,7 +292,7 @@ def test_comfort_stop_no_op_moving_lead(): 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 = 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) @@ -289,7 +302,7 @@ def test_comfort_stop_never_weaker(): 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) + 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) @@ -304,7 +317,7 @@ def test_comfort_stop_weakens_when_gap_opens(): 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) + 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) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 5e89d165c3..c7b410338b 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -25,6 +25,11 @@ MIN_HELD_DREL = 0.5 LOW_SPEED_PASSTHROUGH_V = 5.0 # m/s +# Speed-damp (B) is gated OFF: it lags a speeding-up lead (input-side) and a prior version caused phantom +# braking + a launch rubber-band (lead measured up to ~11 m/s slower than real). Flicker-hold (A) runs alone. +# Flip True only behind an on-road check of a lead accelerating away above LOW_SPEED_PASSTHROUGH_V; if a +# failure-to-release appears, prefer an output-side accel slew over re-enabling this input-side lag. +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 @@ -125,6 +130,7 @@ class RadarDistanceController: self._frame = 0 self._v_ego = 0.0 self._enabled = self._params.get_bool("RadarDistance") + self._vlead_damp_enabled = VLEAD_DAMP_ENABLED # speed-damp (B) gated off; flicker-hold (A) runs alone self._one = _LeadHold() self._two = _LeadHold() @@ -151,4 +157,6 @@ class RadarDistanceController: two = self._two.step(radarstate.leadTwo) if self._v_ego < LOW_SPEED_PASSTHROUGH_V: return radarstate + if not self._vlead_damp_enabled: + return _RadarStateProxy(one, two) # flicker-hold (A) only; speed-damp (B) gated off return _RadarStateProxy(self._one.smooth(one), self._two.smooth(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 0bdeac4844..295b18d422 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,9 +36,10 @@ def obstacle(ld): return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE) -def ctrl(enabled=True): +def ctrl(enabled=True, vlead_damp=False): 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,7 +109,7 @@ def test_low_speed_passthrough_but_hold_warmed_for_highway(): def test_vlead_lags_rise_instant_fall(): - c = ctrl() + 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) @@ -119,13 +120,22 @@ def test_vlead_lags_rise_instant_fall(): 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() + 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 + + def test_obstacle_monotone_during_hold(): c = ctrl() for _ in range(3):