Radar Only Lead Delete

selfdrive/controls/radard.py

@@ -13,7 +13,7 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.simple_kalman import KF1D
 from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection, LaneChangeState
-from openpilot.starpilot.common.starpilot_variables import THRESHOLD, get_starpilot_toggles
+from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
 
 
 # Default lead acceleration decay set to 50% at 1s
@@ -59,7 +59,6 @@ class Track:
     self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
 
     self.leadTrackID = 0
-    self.radarfulFilter = FirstOrderFilter(0, 0.5, self.K_A[0][1])
 
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
@@ -110,19 +109,6 @@ class Track:
     right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
     return -self.yRel > right_lane
 
-  def potential_far_lead(self, standstill: bool, model_data: capnp._DynamicStructReader):
-    if standstill or self.vLead < 1 or abs(self.yRel) > 1:
-      return False
-
-    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
-    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
-
-    if left_lane < -self.yRel < right_lane:
-      self.radarfulFilter.update(1)
-      return True
-    self.radarfulFilter.update(0)
-    return False
-
   def potential_low_speed_lead(self, v_ego: float):
     # stop for stuff in front of you and low speed, even without model confirmation
     # Radar points closer than 0.75, are almost always glitches on toyota radars
@@ -218,12 +204,6 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
       if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
         lead_dict = closest_track.get_RadarState()
 
-    if not lead_dict['status'] and len(tracks) > 0:
-      far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD]
-      if len(far_lead_tracks) > 0:
-        closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
-        lead_dict = closest_track.get_RadarState()
-
   for track in tracks.values():
     track.leadTrackID = lead_dict.get('radarTrackId', -1)