draw leads detected from radar since it has a low speed override

sshane · Nov 14, 2021 · 4679fa5 · 4679fa5
1 parent 5ffb3e4
commit 4679fa5
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
@@ -64,15 +64,13 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
   ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
 }
 
-static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
+static void draw_lead(UIState *s, float d_rel, float v_rel, const vertex_data &vd) {
   // Draw lead car indicator
   auto [x, y] = vd;
 
   float fillAlpha = 0;
   float speedBuff = 10.;
   float leadBuff = 40.;
-  float d_rel = lead_data.getX()[0];
-  float v_rel = lead_data.getV()[0];
   if (d_rel < leadBuff) {
     fillAlpha = 255*(1.0-(d_rel/leadBuff));
     if (v_rel < 0) {
@@ -139,13 +137,17 @@ static void ui_draw_world(UIState *s) {
 
   // Draw lead indicators if openpilot is handling longitudinal
   if (s->scene.longitudinal_control) {
+    const float v_ego = (*s->sm)["carState"].getCarState().getVEgo();
+    auto lead_one_radar = (*s->sm)["radarState"].getRadarState().getLeadOne();
     auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
     auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
     if (lead_one.getProb() > .5) {
-      draw_lead(s, lead_one, s->scene.lead_vertices[0]);
+      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 detected from low speed override
+      draw_lead(s, lead_one_radar.getDRel(), 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, s->scene.lead_vertices[1]);
+      draw_lead(s, lead_two.getX()[0], lead_two.getV()[0], s->scene.lead_vertices[1]);
     }
   }
   nvgResetScissor(s->vg);

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
@@ -224,7 +224,7 @@ static void update_status(UIState *s) {
 
 QUIState::QUIState(QObject *parent) : QObject(parent) {
   ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
-    "modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
+    "modelV2", "radarState", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
     "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
   });