Keep real radar leads under ACC authority

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

@@ -19,9 +19,6 @@ class WMACConstants:
   RADAR_LEAD_ACC_EXIT_PROB = 0.4
   RADAR_LEAD_ACC_RISE_RATE = 1.0
   RADAR_LEAD_ACC_FALL_RATE = 0.25
-  RADAR_LEAD_ACC_MAX_DREL = 80.0
-  RADAR_LEAD_ACC_MAX_TTC = 6.0
-  RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
 
   SLOW_DOWN_PROB = 0.5
   SLOW_DOWN_EXIT_PROB = 0.4

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

@@ -69,7 +69,7 @@ class ModeTransitionManager:
 
   def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
     if immediate:
-      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
+      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0
       self._pending_mode = mode
       self._pending_count = 0
       self._switch_mode(mode)
@@ -188,7 +188,9 @@ class DynamicExperimentalController:
 
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
     car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
+    radar_state = sm['radarState']
+    lead_one = radar_state.leadOne
+    lead_two = radar_state.leadTwo
     md = sm['modelV2']
 
     self._v_ego_kph = car_state.vEgo * 3.6
@@ -201,7 +203,8 @@ class DynamicExperimentalController:
       self._standstill_count = max(0, self._standstill_count - 1)
 
     self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
-    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one))
+    radar_acc_lead_score = max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two))
+    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(radar_acc_lead_score)
     self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
     self._calculate_slow_down(md)
 
@@ -231,16 +234,8 @@ class DynamicExperimentalController:
     self._urgency = self._slow_down_tracker.value
 
   def _radar_acc_lead_score(self, lead_one) -> float:
-    if not lead_one.status:
-      return 0.0
-
-    d_rel = float(getattr(lead_one, 'dRel', float('inf')))
-    v_rel = float(getattr(lead_one, 'vRel', 0.0))
-    if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
-      return 1.0
-    if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
-      return 1.0
-    return 0.0
+    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
+    return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
 
   def _model_action_urgency(self, md) -> float:
     action = getattr(md, 'action', None)
@@ -271,7 +266,7 @@ class DynamicExperimentalController:
 
   def _desired_mode(self) -> tuple[ModeType, bool]:
     if not self._CP.radarUnavailable and self._has_radar_acc_lead:
-      return 'acc', False
+      return 'acc', True
 
     if self._has_mpc_fcw:
       return 'blended', True
@@ -296,7 +291,7 @@ class DynamicExperimentalController:
 
     mode, immediate = self._desired_mode()
     self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
-                                    cancel_hold=self._has_radar_acc_lead)
+                                    cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
     self._mode_manager.update()
 
     self._active = sm['selfdriveState'].experimentalMode and self._enabled

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

@@ -4,15 +4,18 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen
 
 
 class MockLeadOne:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
     self.status = status
     self.dRel = dRel
     self.vRel = vRel
+    self.radar = radar
+    self.radarTrackId = radarTrackId
 
 
 class MockRadarState:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
-    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
+    self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne()
 
 
 class MockCarState:
@@ -55,7 +58,7 @@ class MockParams:
 def default_sm():
   sm = {
     'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
+    'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
     'modelV2': MockModelData(valid=True),
     'selfdriveState': MockSelfDriveState(experimentalMode=True),
   }
@@ -157,7 +160,7 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo
 
 def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   for _ in range(3):
@@ -168,36 +171,94 @@ def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm)
   assert controller.mode() == "acc"
 
 
-def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm):
+def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   controller.update(default_sm)
 
   assert controller._has_lead_filtered
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
 
 
-def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm):
+def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
+  default_sm['radarState'] = MockRadarState(status=0.0)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
   controller.update(default_sm)
   assert controller.mode() == "blended"
 
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True)
   for _ in range(20):
     controller.update(default_sm)
+  assert controller.mode() == "acc"
+
+
+def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
 
   assert controller._has_radar_acc_lead
   assert controller.mode() == "acc"
 
 
+def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
 def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7)
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
 
   for _ in range(20):
@@ -209,7 +270,7 @@ def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
 
 def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   default_sm['carState'].standstill = True
   default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
   mock_mpc.crash_cnt = 1
@@ -224,7 +285,7 @@ def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default
 
 def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
   controller.update(default_sm)
 
   default_sm['radarState'] = MockRadarState(status=0.0)
@@ -233,3 +294,20 @@ def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, defau
 
   assert controller._has_lead_filtered
   assert controller.mode() == "acc"
+
+
+def test_radar_lead_dropout_guard_expires(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  for _ in range(3):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"