diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index ac944fd872d090..ca0b668531d916 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -67,15 +67,15 @@ 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::RadarState::LeadData::Reader &lead_data, const vertex_data &vd) { +static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, 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.getDRel(); - float v_rel = lead_data.getVRel(); + float d_rel = lead_data.getXyva()[0]; + float v_rel = lead_data.getXyva()[2]; if (d_rel < leadBuff) { fillAlpha = 255*(1.0-(d_rel/leadBuff)); if (v_rel < 0) { @@ -167,13 +167,12 @@ static void ui_draw_world(UIState *s) { // Draw lead indicators if openpilot is handling longitudinal if (s->scene.longitudinal_control) { - auto radar_state = (*s->sm)["radarState"].getRadarState(); - auto lead_one = radar_state.getLeadOne(); - auto lead_two = radar_state.getLeadTwo(); - if (lead_one.getStatus()) { + auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0]; + auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeads()[1]; + if (lead_one.getProb() > .5) { draw_lead(s, lead_one, s->scene.lead_vertices[0]); } - if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { + if (lead_two.getProb() > .5 && (std::abs(lead_one.getXyva()[0] - lead_two.getXyva()[0]) > 3.0)) { draw_lead(s, lead_two, s->scene.lead_vertices[1]); } } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8c1477b23fb265..a7110873dbfd16 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -63,14 +63,11 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line return max_idx; } -static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, std::optional line) { - for (int i = 0; i < 2; ++i) { - auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); - if (lead_data.getStatus()) { - float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getDRel())] : 0.0; - // negative because radarState uses left positive convention - calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]); - } +static void update_leads(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, std::optional line) { + if (lead_data.getProb() > 0.5) { + float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getXyva()[0])] : 0.0; + calib_frame_to_full_frame(s, lead_data.getXyva()[0], lead_data.getXyva()[1], z + 1.22, &s->scene.lead_vertices[0]); + } } @@ -112,9 +109,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { } // update path - auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne(); - if (lead_one.getStatus()) { - const float lead_d = lead_one.getDRel() * 2.; + auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0]; + if (lead_one.getProb() > 0.5) { + const float lead_d = lead_one.getXyva()[0] * 2.; max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance); @@ -134,12 +131,12 @@ static void update_state(UIState *s) { scene.engageable = sm["controlsState"].getControlsState().getEngageable(); scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode(); } - if (sm.updated("radarState") && s->vg) { + if (sm.updated("modelV2") && s->vg) { std::optional line; if (sm.rcv_frame("modelV2") > 0) { line = sm["modelV2"].getModelV2().getPosition(); } - update_leads(s, sm["radarState"].getRadarState(), line); + update_leads(s, sm["modelV2"].getModelV2().getLeads()[0], line); } if (sm.updated("liveCalibration")) { scene.world_objects_visible = true; @@ -273,7 +270,7 @@ static void update_status(UIState *s) { QUIState::QUIState(QObject *parent) : QObject(parent) { ui_state.sm = std::make_unique>({ - "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", + "modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState", "pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", });