diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 41807f44ac..25fb9cc328 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 from collections import deque from dataclasses import dataclass, field from enum import IntEnum @@ -424,7 +423,7 @@ class AccelController: separation = path.robust_departure_separation(lead_index) if math.isfinite(separation) and path.departure_references[lead_index] is None: path.departure_references[lead_index] = separation - raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED + raw_departure = ((has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_CREEP_SPEED and envelope.departure_cap > STOP_HOLD_CREEP_SPEED) or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames)) departed = self._creep_departure(path, envelope) or raw_departure diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 8dbf0605cf..7314dc3254 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -44,6 +44,7 @@ MATCHED_PACE_DECEL_RATE = 0.50 BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11 MPC_DECEL_JERK_COST_MULTIPLIER = 1.05 MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80 +MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35 MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0 STOP_HOLD_EGO_SPEED = 0.30 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 9dfdec4dd4..d970c01f47 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 @@ -526,6 +526,26 @@ class TestPaceAndLifecycle: assert results[launch_index].departure_launching assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max) + def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + speed_pulse = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877) + distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04) + + for distance, speed in zip(distances, speed_pulse, strict=True): + radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=4887), + make_lead(status=True, d_rel=6.08, v_lead_k=0.0, radar_track_id=4905)) + held = update(controller, radar, base_speed=8.0, v_ego=0.0) + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 and not held.launching + + departing = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=2.0, radar_track_id=4887), + make_lead(status=True, d_rel=6.48, v_lead_k=2.0, radar_track_id=4905)) + results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self): controller = make_controller() enter_stop_hold(controller) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index 79047b60be..0abeb7f13f 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -1,3 +1,4 @@ +from collections import deque import inspect import math from types import SimpleNamespace @@ -7,6 +8,7 @@ import pytest from cereal import custom, log, messaging from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile @@ -40,6 +42,9 @@ def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_ac is_e2e_calls = [] planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e planner._accel_jerk_smoothing_blocked = False + planner._accel_required_decel_samples = deque(maxlen=4) + planner._accel_required_decel_lead = -1 + planner._dt = DT_MDL planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0) planner.update_accel_controller = lambda *_args, **_kwargs: setattr( planner, "accel_controller_result", @@ -272,6 +277,50 @@ def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_e assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER} +def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends(): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18, + ) + _, calls = run_controller_mpc(planner) + routine_result = planner.accel_controller_result + multipliers = [calls[0][1]["jerk_cost_multiplier"]] + for required_decel in (0.20, 0.23, 0.25): + result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel})) + planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result) + _, calls = run_controller_mpc(planner) + multipliers.append(calls[0][1]["jerk_cost_multiplier"]) + + assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0] + + easing_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": 0.20})) + planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", easing_result) + _, calls = run_controller_mpc(planner) + assert calls[0][1] == {"jerk_cost_multiplier": 1.0} + + free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0})) + planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result) + run_controller_mpc(planner) + planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result) + _, calls = run_controller_mpc(planner) + assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER} + + +def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing(): + planner, _ = planner_for_mpc_test( + state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18, + ) + _, calls = run_controller_mpc(planner) + routine_result = planner.accel_controller_result + multipliers = [calls[0][1]["jerk_cost_multiplier"]] + for required_decel in (0.24, 0.19, 0.22): + result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel})) + planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result) + _, calls = run_controller_mpc(planner) + multipliers.append(calls[0][1]["jerk_cost_multiplier"]) + + assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4 + + @pytest.mark.parametrize( ("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"), [ @@ -335,10 +384,11 @@ def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller(): planner._read_accel_controller_params = lambda: None planner.events_sp = SimpleNamespace(clear=lambda: None) dec_freshness = [] - planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh: dec_freshness.append(radar_fresh)) + planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh)) planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None) planner.accel_personality = int(AccelProfile.normal) planner.accel_personality_enabled = True + planner.output_a_target = 0.0 planner.a_desired = 0.0 planner.v_desired_filter = SimpleNamespace(x=10.0) planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise) diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index ab173cf8ad..76d00d2cea 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -29,6 +29,12 @@ class WMACConstants: MODEL_DECEL_START = -0.5 MODEL_DECEL_RANGE = 2.0 + MODEL_DECEL_TREND_FRAMES = 4 + MODEL_DECEL_TREND_ACCEL = -0.075 + MODEL_DECEL_TREND_RATE = 0.35 + MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075 + MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15 + MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02 ENDPOINT_URGENCY_GAIN = 1.3 CRITICAL_ENDPOINT_FACTOR = 0.3 CRITICAL_URGENCY_GAIN = 1.5 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 9fc945a419..36e5999cf7 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -6,12 +6,15 @@ See the LICENSE.md file in the root directory for more details. """ # Version = 2025-6-30 +from collections import deque +import math from typing import Literal from cereal import messaging from numpy import interp from opendbc.car import structs from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants ModeType = Literal['acc', 'blended'] @@ -168,6 +171,10 @@ class DynamicExperimentalController: self._expected_distance = 0.0 self._trajectory_valid = False self._raw_urgency = 0.0 + self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES) + self._model_decel_trending = False + self._model_decel_latched = False + self._planner_accel = math.nan def _read_params(self) -> None: if self._frame % WMACConstants.PARAM_READ_FRAMES == 0: @@ -234,6 +241,7 @@ class DynamicExperimentalController: self._expected_distance = 0.0 self._trajectory_valid = False + self._update_model_decel_trend(md) urgency = self._model_action_urgency(md) position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE @@ -247,6 +255,31 @@ class DynamicExperimentalController: self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency) self._urgency = self._slow_down_tracker.value + def _update_model_decel_trend(self, md) -> None: + try: + desired_accel = float(md.action.desiredAcceleration) + except (AttributeError, OverflowError, TypeError, ValueError): + desired_accel = math.nan + if not math.isfinite(desired_accel): + self._reset_model_decel_trend() + else: + self._model_accel_samples.append(desired_accel) + history = tuple(self._model_accel_samples) + self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen + and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL + and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE + and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True)) + and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2) + if len(history) == self._model_accel_samples.maxlen and all( + accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history + ): + self._model_decel_latched = False + + def _reset_model_decel_trend(self) -> None: + self._model_accel_samples.clear() + self._model_decel_trending = False + self._model_decel_latched = False + def _radar_acc_lead_score(self, lead_one) -> float: radar_track_id = int(getattr(lead_one, 'radarTrackId', -1)) return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0)) @@ -290,11 +323,21 @@ class DynamicExperimentalController: return urgency + def _model_decel_handoff_ready(self) -> bool: + try: + mpc_accel = float(self._mpc.a_solution[1]) + return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL + and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL + and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP) + except (AttributeError, IndexError, OverflowError, TypeError, ValueError): + return False + def _desired_mode(self) -> tuple[ModeType, bool]: standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB if not self._CP.radarUnavailable and self._has_current_radar_acc_lead: + self._reset_model_decel_trend() return 'acc', True radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1 @@ -304,8 +347,14 @@ class DynamicExperimentalController: return 'blended', True if not self._CP.radarUnavailable and self._has_radar_acc_lead: + self._reset_model_decel_trend() return 'acc', True + entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched + self._model_decel_latched |= entering_model_slowdown + if self._model_decel_latched: + return 'blended', entering_model_slowdown + if self._has_mpc_fcw: return 'blended', True @@ -319,15 +368,24 @@ class DynamicExperimentalController: return 'acc', False - def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None: + def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None: self._read_params() self.set_mpc_fcw_crash_cnt() + try: + self._planner_accel = float(planner_accel) + except (OverflowError, TypeError, ValueError): + self._planner_accel = math.nan self._update_calculations(sm, radar_fresh) + self._active = sm['selfdriveState'].experimentalMode and self._enabled + if not self._active: + model_decel_latched = self._model_decel_latched + self._reset_model_decel_trend() + if model_decel_latched: + self._mode_manager.request_mode('acc', immediate=True) mode, immediate = self._desired_mode() self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES, cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead) self._mode_manager.update() - self._active = sm['selfdriveState'].experimentalMode and self._enabled self._frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index ee77f04259..a4bcadb6fe 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -77,6 +77,7 @@ def mock_cp(): def mock_mpc(): class MPC: crash_cnt = 0 + a_solution = [0.0, 0.0] return MPC() @@ -159,6 +160,159 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo assert controller.mode() == "blended" +def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller.mode() == "acc" + + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12) + controller.update(default_sm, planner_accel=0.0) + + assert controller._model_decel_trending + assert not controller._has_slow_down + assert controller.mode() == "blended" + + +def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_trending + assert controller._model_decel_latched + assert controller.mode() == "blended" + + for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller._model_decel_latched + + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['selfdriveState'].experimentalMode = False + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + + default_sm['selfdriveState'].experimentalMode = True + controller.update(default_sm, planner_accel=0.0) + assert not controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller._model_decel_latched + assert controller.mode() == "blended" + + default_sm['selfdriveState'].experimentalMode = False + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + mock_mpc.a_solution[1] = 0.5 + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (0.0, -0.2, -0.4, -0.6): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.05) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.2) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (0.0, -0.2, 0.0, -0.2): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm) + + assert not controller._model_decel_trending + assert controller.mode() == "acc" + + def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 70ddcab1ce..e15caa8982 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -5,6 +5,9 @@ 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. """ +from collections import deque +import math + from cereal import messaging, custom from opendbc.car import structs from openpilot.common.constants import CV @@ -14,7 +17,8 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import ( - MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, + MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, + MPC_DECEL_JERK_MAX_TARGET_REDUCTION, ) from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper @@ -43,6 +47,9 @@ class LongitudinalPlannerSP: self.accel_controller = AccelController(CP, dt=dt) self.accel_controller_result = None self._accel_jerk_smoothing_blocked = False + self._accel_required_decel_samples = deque(maxlen=4) + self._accel_required_decel_lead = -1 + self._dt = dt self._radar_log_mono_time = None self._radar_fresh_this_cycle = True @@ -158,8 +165,18 @@ class LongitudinalPlannerSP: actuating and prev_accel_constraint and result.state == AccelControllerState.restrict and result.selected_lead >= 0 and not result.launching and target_reduction > 1e-6 ) + if not lead_restriction or result.selected_lead != self._accel_required_decel_lead or not math.isfinite(result.required_decel): + self._accel_required_decel_samples.clear() + if lead_restriction and math.isfinite(result.required_decel): + self._accel_required_decel_samples.append(result.required_decel) + self._accel_required_decel_lead = result.selected_lead if lead_restriction else -1 + required_decel_history = tuple(self._accel_required_decel_samples) + tightening_lead = (len(required_decel_history) == self._accel_required_decel_samples.maxlen + and (required_decel_history[-1] - required_decel_history[0]) / + (self._dt * (len(required_decel_history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE + and sum(after > before for before, after in zip(required_decel_history[:-1], required_decel_history[1:], strict=True)) >= 2) smoothing_eligible = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION - and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL) + and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL and not tightening_lead) smoothing_blocked = getattr(self, '_accel_jerk_smoothing_blocked', False) if previous_mpc_failed: smoothing_blocked = True @@ -185,7 +202,7 @@ class LongitudinalPlannerSP: self._radar_fresh_this_cycle = self._update_radar_freshness(sm) self._read_accel_controller_params() self.events_sp.clear() - self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle) + self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target) self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 1cb77f8784..93a99491fc 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -13,7 +13,8 @@ from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROU from openpilot.sunnypilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_sp from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import ( - MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, PACE_TARGET_RESERVE, + MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, PACE_TARGET_RESERVE, + STOP_HOLD_EXIT_FRAMES, ) ACTUATOR_DYNAMICS = ( @@ -387,6 +388,35 @@ def test_dec_retains_acc_through_route_like_radar_marker_dropout(): assert trace.solver_failures == 0 +def test_dec_uses_confirmed_model_slowdown_while_the_handoff_is_still_gentle(): + def model_action(current_time: float, _v_ego: float, _a_ego: float) -> tuple[float, bool]: + if current_time < 1.0: + return 0.0, False + if current_time < 1.75: + return -0.5 * (current_time - 1.0), False + if current_time < 3.25: + return -0.375, False + return -0.375 - 0.5 * (current_time - 3.25), False + + trace = _run( + duration=4.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=False, speed=22.0, + v_cruise=22.0, model_action_fn=model_action, actuator_delay=0.15, actuator_lag=0.20, + ) + mode_changes = np.flatnonzero(np.asarray(trace.dec_mode)[1:] != np.asarray(trace.dec_mode)[:-1]) + 1 + response = trace.time >= 0.5 + + assert len(mode_changes) == 1 + assert trace.dec_mode[mode_changes[0]] == "blended" + assert trace.time[mode_changes[0]] <= 1.20 + 1e-9 + assert -0.10 < trace.a_target[mode_changes[0]] < 0.0 + assert np.all(np.asarray(trace.dec_mode)[(trace.time >= 1.75) & (trace.time < 3.25)] == "blended") + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert not trace.fcw.any() + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + assert trace.solver_failures == 0 + + def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): traces = [ _run( @@ -467,6 +497,43 @@ def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_brakin assert smoothed.solver_failures == 0 +def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch): + event_time = 3.0 + lead_jerk = 1.02 + max_lead_decel = 2.22 + ramp_time = max_lead_decel / lead_jerk + + def lead_speed(current_time: float) -> float: + braking_time = max(current_time - event_time, 0.0) + ramp = min(braking_time, ramp_time) + return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0) + + common = dict( + duration=7.0, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7, + v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20, + ) + stock = _run(controller_enabled=False, **common) + monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf) + always_smoothed = _run(controller_enabled=True, **common) + monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE) + trace = _run(controller_enabled=True, **common) + response = trace.time >= event_time + response_jerk = trace.time[1:] >= event_time + required_decel_rate = (trace.required_decel[3:] - trace.required_decel[:-3]) / (3 * DT_MDL) + gap = trace.distance_lead - trace.distance + always_smoothed_gap = always_smoothed.distance_lead - always_smoothed.distance + + assert np.max(required_decel_rate[trace.required_decel[3:] >= 0.15]) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE + assert _first_time_below(trace, -0.5) <= _first_time_below(stock, -0.5) + 1e-9 + assert _first_time_below(trace, -0.5) <= _first_time_below(always_smoothed, -0.5) - DT_MDL + 1e-9 + assert np.max(np.abs(np.diff(trace.a_target)[response_jerk] / DT_MDL)) < 3.0 + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(gap) >= np.min(always_smoothed_gap) - 1e-6 + assert not stock.fcw.any() and not always_smoothed.fcw.any() and not trace.fcw.any() + assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0 + + def test_prius_route_model_launches_without_a_dead_pedal(): trace = _run( duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0, @@ -496,6 +563,40 @@ def test_stop_hold_survives_short_full_field_dropout(): _assert_no_new_solver_failures(trace, baseline) +def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold(): + pulse_start = 1.0 + departure_time = 2.0 + pulse_speeds = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877) + pulse_distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + result = truth | {"radar": True, "radarTrackId": 4887 if lead_name == "leadOne" else 4905} + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] if lead_name == "leadOne" else 0.0 + distance = pulse_distances[pulse_frame] if lead_name == "leadOne" else 6.08 + result |= {"dRel": distance, "vLead": speed, "vLeadK": speed, "vRel": speed, "aLeadK": 0.0} + return result + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[pulse]) < 0.01 + assert len(launched) and trace.time[launched[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + @pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag): departure_time = 1.0