diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py index e8afd79e7a..9494c7cd8c 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -17,3 +17,9 @@ class WMACConstants: SLOWNESS_WINDOW_SIZE = 10 # Stable slowness detection SLOWNESS_PROB = 0.55 # Clear threshold for slowness SLOWNESS_CRUISE_OFFSET = 1.025 # Conservative cruise speed offset + + # Model action-head deceleration urgency (independent of the trajectory-endpoint + # heuristic above) - catches a vision-only stop the model is already braking for + # before the trajectory shortens enough to trip SLOW_DOWN_BP/DIST. + MODEL_DECEL_URGENCY_ZERO = -0.2 # m/s^2, model decel at/above this contributes no urgency + MODEL_DECEL_URGENCY_FULL = -1.5 # m/s^2, model decel at/below this is full urgency diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index 60840453d2..f5f6e6d298 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -294,6 +294,16 @@ class DynamicExperimentalController: self._endpoint_x = float('inf') self._trajectory_valid = False + # Model action-head deceleration intent - an early, independent signal that doesn't + # wait on the trajectory-endpoint heuristic below to notice the same stop. + action = md.action + if action.shouldStop: + urgency = 1.0 + elif action.desiredAcceleration < WMACConstants.MODEL_DECEL_URGENCY_ZERO: + decel_ratio = ((WMACConstants.MODEL_DECEL_URGENCY_ZERO - action.desiredAcceleration) / + (WMACConstants.MODEL_DECEL_URGENCY_ZERO - WMACConstants.MODEL_DECEL_URGENCY_FULL)) + urgency = max(urgency, min(1.0, decel_ratio)) + #Require exact trajectory size position_valid = len(md.position.x) == TRAJECTORY_SIZE orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE @@ -302,7 +312,7 @@ class DynamicExperimentalController: # Invalid trajectory - this itself might indicate a stop scenario # Apply moderate urgency for incomplete trajectories at speed if self._v_ego_kph > 20.0: - urgency = 0.3 + urgency = max(urgency, 0.3) self._slow_down_filter.add_data(urgency) urgency_filtered = self._slow_down_filter.get_value() or 0.0 @@ -329,17 +339,19 @@ class DynamicExperimentalController: shortage_ratio = shortage / expected_distance # Base urgency on shortage ratio - urgency = min(1.0, shortage_ratio * 2.0) + trajectory_urgency = min(1.0, shortage_ratio * 2.0) # Increase urgency for very short trajectories (imminent stops) critical_distance = expected_distance * 0.3 if endpoint_x < critical_distance: - urgency = min(1.0, urgency * 2.0) + trajectory_urgency = min(1.0, trajectory_urgency * 2.0) # Speed-based urgency adjustment if self._v_ego_kph > 25.0: speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0 - urgency = min(1.0, urgency * speed_factor) + trajectory_urgency = min(1.0, trajectory_urgency * speed_factor) + + urgency = max(urgency, trajectory_urgency) # Apply filtering but with less smoothing for stops self._slow_down_filter.add_data(urgency) 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 8b86880690..e62108fa93 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 @@ -18,10 +18,11 @@ class MockCarState: self.standstill = standstill class MockModelData: - def __init__(self, valid=True): + def __init__(self, valid=True, desired_accel=1.0, should_stop=False, endpoint_x=0.0): size = 33 if valid else 10 # incomplete if invalid - self.position = type("Pos", (), {"x": [0.0] * size})() + self.position = type("Pos", (), {"x": ([0.0] * (size - 1)) + [endpoint_x]})() self.orientation = type("Ori", (), {"x": [0.0] * size})() + self.action = type("Action", (), {"desiredAcceleration": desired_accel, "shouldStop": should_stop})() class MockSelfDriveState: def __init__(self, experimentalMode=False): @@ -92,6 +93,37 @@ class TestDynamicExperimentalController(OpenpilotTestCase): assert controller.mode() == "blended" + def test_model_should_stop_triggers_immediate_blended_without_lead(self, mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'].leadOne.present = 0.0 + # endpoint far past any SLOW_DOWN_DIST breakpoint, isolating the model action-head + # urgency channel from the trajectory-endpoint heuristic. + default_sm['modelV2'] = MockModelData(valid=True, should_stop=True, endpoint_x=200.0) + + controller.update(default_sm) + + assert controller.mode() == "blended" + + def test_model_decel_intent_triggers_blended_without_lead(self, mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'].leadOne.present = 0.0 + default_sm['modelV2'] = MockModelData(valid=True, desired_accel=-0.7, endpoint_x=200.0) + + for _ in range(30): + controller.update(default_sm) + + assert controller.mode() == "blended" + + def test_mild_model_decel_without_lead_stays_acc(self, mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'].leadOne.present = 0.0 + default_sm['modelV2'] = MockModelData(valid=True, desired_accel=-0.05, endpoint_x=200.0) + + for _ in range(30): + controller.update(default_sm) + + assert controller.mode() == "acc" + class TestModelAccelTransition(OpenpilotTestCase): def test_blended_entry_is_rate_bounded(self):