diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 354674ba0..baf0fc7cc 100644 --- a/selfdrive/controls/radard.py +++ b/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)