diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 643ec3e6a36832..3866dffc097ea3 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -144,7 +144,7 @@ static void ui_draw_world(UIState *s) { if (lead_one.getProb() > .5) { draw_lead(s, lead_one.getX()[0], lead_one.getV()[0], s->scene.lead_vertices[0]); } else if (v_ego < 4. && lead_one_radar.getStatus()) { // draw leads from low speed override - draw_lead(s, lead_one_radar.getDRel(), lead_one_radar.getVRel(), s->scene.lead_vertices[0]); + draw_lead(s, lead_one_radar.getDRel(), v_ego + lead_one_radar.getVRel(), s->scene.lead_vertices[0]); } if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) { draw_lead(s, lead_two.getX()[0], lead_two.getV()[0], s->scene.lead_vertices[1]); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 54658604ca1def..f737d91117abb0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -41,12 +41,16 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line } static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) { + const float v_ego = (*s->sm)["carState"].getCarState().getVEgo(); + auto lead_one_radar = (*s->sm)["radarState"].getRadarState().getLeadOne(); auto leads = model.getLeadsV3(); auto model_position = model.getPosition(); for (int i = 0; i < 2; ++i) { if (leads[i].getProb() > 0.5) { float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])]; calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[i]); + } else if (i == 0 && v_ego < 4. && lead_one_radar.getStatus()) { + calib_frame_to_full_frame(s, lead_one_radar.getDRel(), leads[i].getYRel(), 1.22, &s->scene.lead_vertices[i]); } } }