From 52d7dd58a74a18bfd35c3d70f4277b1ca81d5843 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 16 Jul 2026 13:21:22 -0700 Subject: [PATCH] Keep radar lead response predictable under ACC --- .../controls/lib/longitudinal_planner.py | 2 +- .../selfdrive/controls/lib/dec/constants.py | 4 -- sunnypilot/selfdrive/controls/lib/dec/dec.py | 20 ++---- .../lib/dec/tests/test_dynamic_controller.py | 61 ++++++++++--------- 4 files changed, 39 insertions(+), 48 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 39946830c3..8034957a8e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -179,7 +179,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): if output_a_target < output_a_target_mpc: self.mpc.source = LongitudinalPlanSource.e2e else: - output_a_target = output_a_target_mpc + output_a_target = min(output_a_target_mpc, stop_constraint.accel_ceiling_mps2) self.output_should_stop = output_should_stop_mpc self.output_should_stop |= stop_constraint.hold diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index 765bb23757..9471472aa7 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -19,10 +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 SLOW_DOWN_RISE_RATE = 0.65 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index ceb24bc12f..90ea5c263e 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -237,16 +237,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) @@ -276,15 +268,15 @@ class DynamicExperimentalController: return urgency def _desired_mode(self) -> tuple[ModeType, bool]: + if not self._CP.radarUnavailable and self._has_radar_acc_lead: + return 'acc', True + if self._stop_constraint.state == StopIntentState.suppressed: return 'acc', True if self._stop_constraint.active: return 'blended', True - if not self._CP.radarUnavailable and self._has_radar_acc_lead: - return 'acc', False - if self._has_mpc_fcw: return 'blended', True @@ -328,7 +320,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._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 b7d733eb73..59daac6c75 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -5,15 +5,17 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.stop_intent import StopInte 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): + self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId) class MockCarState: @@ -70,7 +72,7 @@ def default_sm(): sm = MockSubMaster({ 'carControl': MockCarControl(), '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), }) @@ -173,7 +175,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): @@ -184,36 +186,31 @@ 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, radarTrackId=7) 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" - - -def test_relevant_radar_lead_smoothly_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=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) - for _ in range(20): - controller.update(default_sm) - assert controller._has_radar_acc_lead assert controller.mode() == "acc" +def test_vision_only_far_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=120.0, vRel=0.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_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, radar=True, radarTrackId=7) default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) for _ in range(20): @@ -225,7 +222,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 @@ -240,7 +237,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) @@ -251,7 +248,7 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau assert controller.mode() == "acc" -def test_consistent_stop_intent_commits_and_survives_radar_acquisition(mock_cp, mock_mpc, default_sm): +def test_radar_acquisition_preserves_committed_stop_and_forces_acc(mock_cp, mock_mpc, default_sm): controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) default_sm['radarState'] = MockRadarState(status=0.0) @@ -263,10 +260,16 @@ def test_consistent_stop_intent_commits_and_survives_radar_acquisition(mock_cp, assert controller.stop_constraint().state == StopIntentState.approach assert controller.mode() == "blended" - default_sm['radarState'] = MockRadarState(status=1.0) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) controller.update(default_sm) - assert controller.stop_constraint().active - assert controller.mode() == "blended" + assert controller.stop_constraint().state == StopIntentState.approach + assert controller.mode() == "acc" + + default_sm['carState'].vEgo = 0.0 + default_sm['carState'].standstill = True + controller.update(default_sm) + assert controller.stop_constraint().state == StopIntentState.hold + assert controller.mode() == "acc" def test_gas_suppresses_committed_stop_and_returns_to_acc(mock_cp, mock_mpc, default_sm):