From 2a99bc6ddefaa050bf4f82640e3097ef230102e8 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 4 Jan 2022 06:37:36 +0800 Subject: [PATCH] ui: update lead's z position if modelV2 has been updated after startup. (#23355) * model frame >= started_frame * remove optional --- selfdrive/ui/ui.cc | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) 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();