DEC model stop target

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

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

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

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: