fix(dec): react to model braking

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

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)

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):