diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index 765bb23757..be3aab5ebc 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -19,9 +19,6 @@ class WMACConstants: 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 SLOW_DOWN_PROB = 0.5 SLOW_DOWN_EXIT_PROB = 0.4 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index a10a8fc916..0f881c21dd 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -69,7 +69,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) @@ -188,7 +188,9 @@ class DynamicExperimentalController: def _update_calculations(self, sm: messaging.SubMaster) -> 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 @@ -201,7 +203,8 @@ class DynamicExperimentalController: self._standstill_count = max(0, self._standstill_count - 1) self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status)) - self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one)) + radar_acc_lead_score = max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)) + self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(radar_acc_lead_score) self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0 self._calculate_slow_down(md) @@ -231,16 +234,8 @@ class DynamicExperimentalController: self._urgency = self._slow_down_tracker.value def _radar_acc_lead_score(self, lead_one) -> float: - if not lead_one.status: - return 0.0 - - 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 + 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)) def _model_action_urgency(self, md) -> float: action = getattr(md, 'action', None) @@ -271,7 +266,7 @@ class DynamicExperimentalController: def _desired_mode(self) -> tuple[ModeType, bool]: if not self._CP.radarUnavailable and self._has_radar_acc_lead: - return 'acc', False + return 'acc', True if self._has_mpc_fcw: return 'blended', True @@ -296,7 +291,7 @@ class DynamicExperimentalController: 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 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 d4fa748f7a..00d803c7f3 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -4,15 +4,18 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen 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.status = 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 +58,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), } @@ -157,7 +160,7 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo 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 +171,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 +270,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,7 +285,7 @@ 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) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) controller.update(default_sm) default_sm['radarState'] = MockRadarState(status=0.0) @@ -233,3 +294,20 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau assert controller._has_lead_filtered assert controller.mode() == "acc" + + +def test_radar_lead_dropout_guard_expires(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) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + for _ in range(3): + 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() == "blended"