From 768ba97405de602d4e7acc6fb7f5506582cc955d Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 25 Jun 2026 11:49:38 -0700 Subject: [PATCH] DEC model stop target --- .../selfdrive/controls/lib/dec/constants.py | 7 +++ sunnypilot/selfdrive/controls/lib/dec/dec.py | 34 ++++++++++- .../lib/dec/tests/test_dynamic_controller.py | 57 +++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 1 + 4 files changed, 98 insertions(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index 765bb23757..a003410cf1 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -33,6 +33,13 @@ class WMACConstants: MODEL_DECEL_START = -0.5 MODEL_DECEL_RANGE = 2.0 + MODEL_STOP_TIME = 3.0 + MODEL_STOP_MIN_DISTANCE = 4.0 + MODEL_STOP_TARGET_TIME = 3.0 + MODEL_STOP_PROB = 0.55 + MODEL_STOP_EXIT_PROB = 0.35 + MODEL_STOP_RISE_RATE = 0.8 + MODEL_STOP_FALL_RATE = 0.25 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 a10a8fc916..99ee8d00b7 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -153,12 +153,20 @@ class DynamicExperimentalController: rise_rate=WMACConstants.SLOWNESS_RISE_RATE, fall_rate=WMACConstants.SLOWNESS_FALL_RATE, ) + self._model_stop_tracker = HysteresisSignal( + enter_threshold=WMACConstants.MODEL_STOP_PROB, + exit_threshold=WMACConstants.MODEL_STOP_EXIT_PROB, + rise_rate=WMACConstants.MODEL_STOP_RISE_RATE, + fall_rate=WMACConstants.MODEL_STOP_FALL_RATE, + ) self._has_lead_filtered = False self._has_radar_acc_lead = False self._has_slow_down = False self._has_slowness = False self._has_mpc_fcw = False + self._has_model_should_stop = False + self._has_model_stop = False self._v_ego_kph = 0.0 self._v_cruise_kph = 0.0 self._has_standstill = False @@ -183,6 +191,18 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active + def force_stop_requested(self) -> bool: + return self._active and self._trajectory_valid and self._has_model_stop and not self._has_lead_filtered + + def apply_model_stop_target(self, v_target: float) -> float: + if not self.force_stop_requested(): + return v_target + if self._has_standstill: + return 0.0 + + stop_target = max(0.0, self._endpoint_x) / WMACConstants.MODEL_STOP_TARGET_TIME + return min(v_target, stop_target) + def set_mpc_fcw_crash_cnt(self) -> None: self._mpc_fcw_crash_cnt = self._mpc.crash_cnt @@ -216,6 +236,7 @@ class DynamicExperimentalController: self._endpoint_x = float('inf') self._expected_distance = 0.0 self._trajectory_valid = False + self._has_model_should_stop = False urgency = self._model_action_urgency(md) position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE @@ -229,6 +250,7 @@ class DynamicExperimentalController: self._raw_urgency = clip01(urgency) self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency) self._urgency = self._slow_down_tracker.value + self._update_model_stop() def _radar_acc_lead_score(self, lead_one) -> float: if not lead_one.status: @@ -247,12 +269,22 @@ class DynamicExperimentalController: if action is None: return 0.0 - urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0 + self._has_model_should_stop = bool(getattr(action, 'shouldStop', False)) + urgency = 1.0 if self._has_model_should_stop else 0.0 desired_accel = getattr(action, 'desiredAcceleration', 0.0) if desired_accel < WMACConstants.MODEL_DECEL_START: urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE)) return urgency + def _update_model_stop(self) -> None: + stop_score = 0.0 + if self._has_model_should_stop and self._trajectory_valid and not self._has_lead_filtered: + v_ego = self._v_ego_kph / 3.6 + stop_window = max(WMACConstants.MODEL_STOP_MIN_DISTANCE, v_ego * WMACConstants.MODEL_STOP_TIME) + stop_score = float(0.0 < self._endpoint_x <= stop_window) + + self._has_model_stop = self._model_stop_tracker.update(stop_score) + def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float: if endpoint_x >= expected_distance: return 0.0 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..10e6891c7d 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -153,6 +153,63 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo assert not controller._trajectory_valid assert controller.mode() == "blended" + assert not controller.force_stop_requested() + assert controller.apply_model_stop_target(20.0) == 20.0 + + +def test_model_should_stop_clamps_target_for_confirmed_near_stop(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=12.0, should_stop=True) + + controller.update(default_sm) + + assert controller.mode() == "blended" + assert controller.force_stop_requested() + assert controller.apply_model_stop_target(20.0) == pytest.approx(4.0) + + +def test_far_model_should_stop_does_not_force_stop_target(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=200.0, should_stop=True) + + controller.update(default_sm) + + assert controller.mode() == "blended" + assert not controller.force_stop_requested() + assert controller.apply_model_stop_target(20.0) == 20.0 + + +def test_invalid_trajectory_suppresses_stale_model_force_stop(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=12.0, should_stop=True) + controller.update(default_sm) + assert controller.force_stop_requested() + + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + controller.update(default_sm) + + assert not controller.force_stop_requested() + assert controller.apply_model_stop_target(20.0) == 20.0 + + +def test_radar_lead_suppresses_model_force_stop(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=20.0, vRel=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=12.0, should_stop=True) + + for _ in range(3): + controller.update(default_sm) + + assert controller._has_lead_filtered + assert controller.mode() == "acc" + assert not controller.force_stop_requested() + assert controller.apply_model_stop_target(20.0) == 20.0 def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index c0b988bf8c..75942ad127 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -75,6 +75,7 @@ class LongitudinalPlannerSP: self.source = min(targets, key=lambda k: targets[k][0]) self.output_v_target, self.output_a_target = targets[self.source] + self.output_v_target = self.dec.apply_model_stop_target(self.output_v_target) return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: