mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-22 08:23:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ui: update lead's z position if modelV2 has been updated after startup. (#23355)

Browse Source 
* model frame >= started_frame

* remove optional
This commit is contained in:
Dean Lee
2022-01-04 06:37:36 +08:00
committed by GitHub
parent 760104068b
commit 2a99bc6dde
1 changed files with 4 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
selfdrive/ui/ui.cc
View File

@@ -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<cereal::ModelDataV2::XYZTData::Reader> 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<cereal::ModelDataV2::XYZTData::Reader> 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();