From 9da0b6694e5f3b143a3d84fa61434f8a9ed37318 Mon Sep 17 00:00:00 2001 From: deanlee Date: Sun, 2 Jan 2022 18:35:45 +0800 Subject: [PATCH 1/2] model frame >= started_frame --- selfdrive/ui/ui.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8f82250fae1a7e..2c6813d76294f8 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -112,7 +112,7 @@ static void update_state(UIState *s) { } if (sm.updated("radarState")) { std::optional line; - if (sm.rcv_frame("modelV2") > 0) { + if (sm.rcv_frame("modelV2") >= s->scene.started_frame) { line = sm["modelV2"].getModelV2().getPosition(); } update_leads(s, sm["radarState"].getRadarState(), line); From 935ecefa9acee2da7baf00321bf953217e440878 Mon Sep 17 00:00:00 2001 From: deanlee Date: Mon, 3 Jan 2022 13:16:03 +0800 Subject: [PATCH 2/2] 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 2c6813d76294f8..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") >= s->scene.started_frame) { - 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();