From 6c85949da84b333181c3b4e88e3037935e0e9b96 Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Mon, 20 Jul 2026 20:21:27 -0700
Subject: [PATCH] Prevent radar timing skew from resetting longitudinal control

---
 sunnypilot/selfdrive/controls/lib/dec/dec.py  |  3 ++-
 .../lib/dec/tests/test_dynamic_controller.py  | 22 +++++++++++++++++++
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py
index cb06593e04..9fc945a419 100644
--- a/sunnypilot/selfdrive/controls/lib/dec/dec.py
+++ b/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
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 a307960317..ee77f04259 100644
--- a/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py
+++ b/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)