diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py index 765bb23757..76d00d2cea 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -15,13 +15,9 @@ class WMACConstants: LEAD_EXIT_PROB = 0.25 LEAD_RISE_RATE = 1.0 LEAD_FALL_RATE = 0.35 - RADAR_LEAD_ACC_PROB = 0.5 - RADAR_LEAD_ACC_EXIT_PROB = 0.4 - RADAR_LEAD_ACC_RISE_RATE = 1.0 - RADAR_LEAD_ACC_FALL_RATE = 0.25 - RADAR_LEAD_ACC_MAX_DREL = 80.0 - RADAR_LEAD_ACC_MAX_TTC = 6.0 - RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5 + RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL))) + RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL))) + RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL))) SLOW_DOWN_PROB = 0.5 SLOW_DOWN_EXIT_PROB = 0.4 @@ -33,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/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index f15cd594f9..acb2ab6540 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/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 openpilot.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'] @@ -69,7 +72,7 @@ class ModeTransitionManager: def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None: if immediate: - self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) + self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0 self._pending_mode = mode self._pending_count = 0 self._switch_mode(mode) @@ -135,12 +138,6 @@ class DynamicExperimentalController: rise_rate=WMACConstants.LEAD_RISE_RATE, fall_rate=WMACConstants.LEAD_FALL_RATE, ) - self._radar_acc_lead_tracker = HysteresisSignal( - enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB, - exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB, - rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE, - fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE, - ) self._slow_down_tracker = HysteresisSignal( enter_threshold=WMACConstants.SLOW_DOWN_PROB, exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB, @@ -155,7 +152,12 @@ class DynamicExperimentalController: ) self._has_lead_filtered = False + self._has_any_lead = False + self._has_current_radar_acc_lead = False self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 + self._radar_fresh = True + self._radar_stale_frames = 0 self._has_slow_down = False self._has_slowness = False self._has_mpc_fcw = False @@ -169,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: @@ -186,9 +192,11 @@ class DynamicExperimentalController: def set_mpc_fcw_crash_cnt(self) -> None: self._mpc_fcw_crash_cnt = self._mpc.crash_cnt - def _update_calculations(self, sm: messaging.SubMaster) -> None: + def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None: car_state = sm['carState'] - lead_one = sm['radarState'].leadOne + radar_state = sm['radarState'] + lead_one = radar_state.leadOne + lead_two = radar_state.leadTwo md = sm['modelV2'] self._v_ego_kph = car_state.vEgo * 3.6 @@ -200,8 +208,24 @@ class DynamicExperimentalController: else: self._standstill_count = max(0, self._standstill_count - 1) - self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present)) - self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one)) + self._radar_fresh = bool(radar_fresh) + if self._radar_fresh: + self._radar_stale_frames = 0 + self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present)) + self._has_any_lead = bool(lead_one.present or lead_two.present) + self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two))) + self._update_radar_acc_lead() + else: + self._radar_stale_frames += 1 + self._has_current_radar_acc_lead = False + if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES: + self._update_radar_acc_lead() + else: + self._lead_tracker.reset() + self._has_lead_filtered = False + self._has_any_lead = False + self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0 self._calculate_slow_down(md) @@ -217,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 @@ -230,17 +255,46 @@ class DynamicExperimentalController: self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency) self._urgency = self._slow_down_tracker.value - def _radar_acc_lead_score(self, lead_one) -> float: - if not lead_one.present: - return 0.0 + 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 - d_rel = float(getattr(lead_one, 'dRel', float('inf'))) - v_rel = float(getattr(lead_one, 'vRel', 0.0)) - if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL: - return 1.0 - if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC: - return 1.0 - return 0.0 + 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.present and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0)) + + def _update_radar_acc_lead(self) -> None: + if self._has_current_radar_acc_lead: + self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + self._has_radar_acc_lead = True + return + + if not self._has_any_lead: + self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES) + + self._has_radar_acc_lead = self._radar_acc_lead_frames > 0 + self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1) def _model_action_urgency(self, md) -> float: action = getattr(md, 'action', None) @@ -269,16 +323,41 @@ 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 + if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down): + self._radar_acc_lead_frames = 0 + self._has_radar_acc_lead = False + return 'blended', True + if not self._CP.radarUnavailable and self._has_radar_acc_lead: - return 'acc', False + 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 - standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES - urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB - if self._CP.radarUnavailable: if standstill or self._has_slow_down: return 'blended', urgent_slow_down @@ -289,15 +368,24 @@ class DynamicExperimentalController: return 'acc', False - def update(self, sm: messaging.SubMaster) -> 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() - self._update_calculations(sm) + 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=self._has_radar_acc_lead) + 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/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index d4d057ab1d..c35252f2ec 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -1,18 +1,22 @@ import pytest +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal class MockLeadOne: - def __init__(self, status=0.0, dRel=30.0, vRel=0.0): + def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1): self.present = status self.dRel = dRel self.vRel = vRel + self.radar = radar + self.radarTrackId = radarTrackId class MockRadarState: - def __init__(self, status=0.0, dRel=30.0, vRel=0.0): - self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel) + def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None): + self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId) + self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne() class MockCarState: @@ -55,7 +59,7 @@ class MockParams: def default_sm(): sm = { 'carState': MockCarState(vEgo=10.0, vCruise=20.0), - 'radarState': MockRadarState(status=1.0), + 'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7), 'modelV2': MockModelData(valid=True), 'selfdriveState': MockSelfDriveState(experimentalMode=True), } @@ -73,6 +77,7 @@ def mock_cp(): def mock_mpc(): class MPC: crash_cnt = 0 + a_solution = [0.0, 0.0] return MPC() @@ -155,9 +160,162 @@ 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) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) for _ in range(3): @@ -168,36 +326,94 @@ def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm) assert controller.mode() == "acc" -def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm): +def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True) default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) controller.update(default_sm) assert controller._has_lead_filtered - assert not controller._has_radar_acc_lead - assert controller.mode() == "blended" + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" -def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm): +def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0) + default_sm['radarState'] = MockRadarState(status=0.0) default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) controller.update(default_sm) assert controller.mode() == "blended" - default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True) for _ in range(20): controller.update(default_sm) + assert controller.mode() == "acc" + + +def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) assert controller._has_radar_acc_lead assert controller.mode() == "acc" +def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "blended" + + def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7) default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) for _ in range(20): @@ -209,7 +425,7 @@ def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm): def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['radarState'] = MockRadarState(status=1.0) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) default_sm['carState'].standstill = True default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True) mock_mpc.crash_cnt = 1 @@ -224,12 +440,194 @@ def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['radarState'] = MockRadarState(status=1.0) - controller.update(default_sm) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down default_sm['radarState'] = MockRadarState(status=0.0) - default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) controller.update(default_sm) assert controller._has_lead_filtered assert controller.mode() == "acc" + + +def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(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) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + controller.update(default_sm) + assert not controller._has_radar_acc_lead + assert controller.mode() == "acc" + + for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1): + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_radar_lead_short_dropout_guard_expires_without_any_lead(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) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + + controller.update(default_sm) + assert not controller._has_radar_acc_lead + + +def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert controller._has_radar_acc_lead + assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1 + assert controller._radar_stale_frames == 1 + assert controller.mode() == "acc" + + +def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "acc" + + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "blended" + + +def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + mock_mpc.crash_cnt = 1 + + controller.update(default_sm, radar_fresh=False) + + assert controller.mode() == "blended" + + +def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1): + controller.update(default_sm, radar_fresh=False) + assert controller._has_radar_acc_lead + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert not controller._has_radar_acc_lead + assert not controller._has_any_lead + assert not controller._has_lead_filtered + + +def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + for _ in range(WMACConstants.RADAR_STALE_FRAMES): + controller.update(default_sm, radar_fresh=False) + + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + controller.update(default_sm, radar_fresh=True) + + assert controller._radar_stale_frames == 0 + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"]) +def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + if urgent_source == "fcw": + mock_mpc.crash_cnt = 1 + else: + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + mock_mpc.crash_cnt = 0 + default_sm['modelV2'] = MockModelData(valid=True) + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2): + if frame % 2 == 0: + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0)) + else: + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8)) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(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) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1): + controller.update(default_sm) + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index d66463a923..7233da3eb4 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -121,7 +121,7 @@ class LongitudinalPlannerSP: self._radar_fresh_this_cycle = self._update_radar_freshness(sm) self.accel_controller.update_params() self.events_sp.clear() - self.dec.update(sm) + 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/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index ab8a076ad5..12911323ec 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -394,6 +394,65 @@ def test_e2e_to_radar_acc_handoff_keeps_braking_continuous(): assert active[transition] +def test_dec_retains_acc_through_route_like_radar_marker_dropout(): + dropout_start = 1.0 + reacquisition_time = 1.8 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + frame = round(current_time / DT_MDL) + if current_time < dropout_start: + marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo" + return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1} + if current_time < reacquisition_time: + return truth | {"radar": False, "radarTrackId": -1} + return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1} + + trace = _run( + duration=2.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=True, speed=20.0, + distance_lead=35.0, v_lead=18.0, v_cruise=30.0, lead_observation_fn=observe, + model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20, + ) + response = (trace.time >= dropout_start - DT_MDL) & (trace.time <= reacquisition_time + 0.5) + + assert all(mode == "acc" for mode in trace.dec_mode) + assert all(str(source) != "e2e" for source in trace.source) + assert not trace.fcw.any() + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.max(np.abs(np.diff(trace.a_target[response]) / DT_MDL)) < 3.0 + assert trace.raw_radar_passthrough.all() + assert np.all(trace.mpc_calls == 1) + 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(