From c7efa7fa207fa403542e7bb0a47e5a048a626dd8 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sat, 13 Jun 2026 20:42:50 -0500
Subject: [PATCH] Two is not better than one

---
 selfdrive/controls/radard.py           | 38 ++++++++++++++++
 selfdrive/controls/tests/test_leads.py | 60 +++++++++++++++++++++++++-
 2 files changed, 97 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 4b7fa3d00..cedf80dac 100644
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -196,6 +196,39 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
   }
 
 
+VISION_DUPLICATE_LEAD_MAX_DREL_DIFF = 0.75
+VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF = 0.5
+VISION_DUPLICATE_LEAD_MAX_YREL_DIFF = 0.4
+VISION_DUPLICATE_LEAD_MIN_MODEL_PROB = 0.7
+
+
+def leads_are_duplicate(lead_one: dict[str, Any], lead_two: dict[str, Any]) -> bool:
+  if not lead_one.get("status", False) or not lead_two.get("status", False):
+    return False
+
+  lead_one_radar = bool(lead_one.get("radar", False))
+  lead_two_radar = bool(lead_two.get("radar", False))
+
+  if lead_one_radar and lead_two_radar:
+    lead_one_track_id = int(lead_one.get("radarTrackId", -1))
+    lead_two_track_id = int(lead_two.get("radarTrackId", -1))
+    return lead_one_track_id != -1 and lead_one_track_id == lead_two_track_id
+
+  if lead_one_radar or lead_two_radar:
+    return False
+
+  lead_one_prob = float(lead_one.get("modelProb", 0.0))
+  lead_two_prob = float(lead_two.get("modelProb", 0.0))
+  if min(lead_one_prob, lead_two_prob) < VISION_DUPLICATE_LEAD_MIN_MODEL_PROB:
+    return False
+
+  return (
+    abs(float(lead_one.get("dRel", 0.0)) - float(lead_two.get("dRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_DREL_DIFF and
+    abs(float(lead_one.get("vLead", 0.0)) - float(lead_two.get("vLead", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF and
+    abs(float(lead_one.get("yRel", 0.0)) - float(lead_two.get("yRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_YREL_DIFF
+  )
+
+
 def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
              model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool,
              starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace,
@@ -314,6 +347,11 @@ class RadarD:
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'],
                                           sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False,
                                           g90_radar_filter=self.g90_radar_filter)
+      # The model exposes two lead slots, but both can occasionally fuse to the
+      # same radar object. Publishing that as two separate leads makes MPC churn
+      # between lead0/lead1 even though the scene only has one physical target.
+      if leads_are_duplicate(self.radar_state.leadOne, self.radar_state.leadTwo):
+        self.radar_state.leadTwo = {'status': False}
 
     if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes):
       self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py
index 31590abea..09fce2ab4 100644
--- a/selfdrive/controls/tests/test_leads.py
+++ b/selfdrive/controls/tests/test_leads.py
@@ -4,7 +4,7 @@ import cereal.messaging as messaging
 
 from opendbc.car.toyota.values import CAR as TOYOTA
 from openpilot.selfdrive.test.process_replay import replay_process_with_name
-from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane
+from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane, leads_are_duplicate
 
 
 class TestLeads:
@@ -22,6 +22,64 @@ class TestLeads:
     assert g90_low_speed_radar_lead_sane(centered_track, 2.0)
     assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5)
 
+  def test_duplicate_radar_leads_share_track(self):
+    lead_one = {
+      "status": True,
+      "radar": True,
+      "radarTrackId": 20293,
+    }
+    lead_two = {
+      "status": True,
+      "radar": True,
+      "radarTrackId": 20293,
+    }
+    different_track = {
+      "status": True,
+      "radar": True,
+      "radarTrackId": 20326,
+    }
+    vision_only = {
+      "status": True,
+      "radar": False,
+      "radarTrackId": -1,
+    }
+
+    assert leads_are_duplicate(lead_one, lead_two)
+    assert not leads_are_duplicate(lead_one, different_track)
+    assert not leads_are_duplicate(lead_one, vision_only)
+
+  def test_duplicate_vision_leads_are_deduped(self):
+    lead_one = {
+      "status": True,
+      "radar": False,
+      "radarTrackId": -1,
+      "dRel": 48.3,
+      "vLead": 18.9,
+      "yRel": 0.14,
+      "modelProb": 0.99,
+    }
+    lead_two = {
+      "status": True,
+      "radar": False,
+      "radarTrackId": -1,
+      "dRel": 48.4,
+      "vLead": 19.0,
+      "yRel": 0.14,
+      "modelProb": 1.00,
+    }
+    distinct_lead = {
+      "status": True,
+      "radar": False,
+      "radarTrackId": -1,
+      "dRel": 48.3,
+      "vLead": 18.9,
+      "yRel": 1.10,
+      "modelProb": 0.99,
+    }
+
+    assert leads_are_duplicate(lead_one, lead_two)
+    assert not leads_are_duplicate(lead_one, distinct_lead)
+
   def test_radar_fault(self):
     # if there's no radar-related can traffic, radard should either not respond or respond with an error
     # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check