Prevent radar timing skew from resetting longitudinal control

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -297,7 +297,8 @@ class DynamicExperimentalController:
     if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
       return 'acc', True
 
-    if (not self._radar_fresh or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
+    radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
+    if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
       self._radar_acc_lead_frames = 0
       self._has_radar_acc_lead = False
       return 'blended', True

sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -349,6 +349,28 @@ def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, de
   assert controller.mode() == "acc"
 
 
+def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "acc"
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+
+def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  mock_mpc.crash_cnt = 1
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert controller.mode() == "blended"
+
+
 def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
   controller.update(default_sm)