diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 401d0492b..e94f7baa6 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.frogpilot.common.frogpilot_variables import get_frogpilot_toggles +from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles # Default lead acceleration decay set to 50% at 1s @@ -67,6 +67,8 @@ class Track: self.leadTrackID = 0 + self.radarfulFilter = FirstOrderFilter(0, 1, 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 self.dRel = d_rel # LONG_DIST @@ -136,6 +138,17 @@ class Track: else: return self.leadRight + def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader): + 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 and self.dRel < model_data.position.x[-1] and self.vLeadK > 1: + self.radarfulFilter.update(1) + return True + else: + self.radarfulFilter.update(0) + return False + def laplacian_pdf(x: float, mu: float, b: float): b = max(b, 1e-4) @@ -186,7 +199,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, + model_v_ego: float, model_data: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens @@ -210,6 +223,12 @@ 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 low_speed_override and not lead_dict['status'] and len(tracks) > 0: + far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(lead_msg, 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() + # FrogPilot variables for track in tracks.values(): track.leadTrackID = lead_dict.get('radarTrackId', -1) @@ -291,8 +310,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], self.frogpilot_toggles, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], self.frogpilot_toggles, low_speed_override=False) # FrogPilot variables if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking): diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 66be97157..93dec9c2b 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -39,7 +39,11 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { update_leads(radar_state, model.getPosition()); const auto &lead_two = radar_state.getLeadTwo(); if (lead_one.getStatus()) { - drawLead(painter, lead_one, lead_vertices[0], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString())); + if (lead_one.getModelProb() >= frogpilot_toggles.value("lead_detection_probability").toDouble()) { + drawLead(painter, lead_one, lead_vertices[0], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString())); + } else { + drawLead(painter, lead_one, lead_vertices[0], surface_rect, frogpilot_nvg->whiteColor()); + } } else { // FrogPilot variables if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {