fill lead_verticies

commaai · Nov 14, 2021 · 5cdf265 · 5cdf265
1 parent 24a8a3f
commit 5cdf265
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git 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
@@ -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]);
     }
   }
 }