diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 1bbbb8379cd4c10..71613cbb82bd4b8 100644 --- 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 index d0c3c04879c9e78..54658604ca1defa 100644 --- 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>({ - "modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState", + "modelV2", "radarState", "controlsState", "liveCalibration", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", });