diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8f82250fae1a7e..5c67d5f474ce69 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -42,11 +42,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) { +static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &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; + float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]); } } @@ -110,12 +110,8 @@ static void update_state(UIState *s) { if (sm.updated("modelV2")) { update_model(s, sm["modelV2"].getModelV2()); } - if (sm.updated("radarState")) { - std::optional line; - if (sm.rcv_frame("modelV2") > 0) { - line = sm["modelV2"].getModelV2().getPosition(); - } - update_leads(s, sm["radarState"].getRadarState(), line); + if (sm.updated("radarState") && sm.rcv_frame("modelV2") >= s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); } if (sm.updated("liveCalibration")) { auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib();