ui/model: use height from LiveCalibration instead of hardcoded offset…

… Z (#33802) * add PATH_OFFSET_Z constant * use height from liveCalibration
commaai · Nov 12, 2024 · d050e0c · d050e0c
1 parent 4170534
commit d050e0c
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 2 deletions.
diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc
@@ -25,6 +25,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
   clip_region = surface_rect.adjusted(-CLIP_MARGIN, -CLIP_MARGIN, CLIP_MARGIN, CLIP_MARGIN);
   experimental_mode = sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
   longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
+  path_offset_z = sm["liveCalibration"].getLiveCalibration().getHeight()[0];
 
   painter.save();
 
@@ -55,7 +56,7 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state,
     const auto &lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
     if (lead_data.getStatus()) {
       float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())];
-      mapToScreen(lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &lead_vertices[i]);
+      mapToScreen(lead_data.getDRel(), -lead_data.getYRel(), z + path_offset_z, &lead_vertices[i]);
     }
   }
 }
@@ -87,7 +88,7 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
     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);
-  mapLineToPolygon(model_position, 0.9, 1.22, &track_vertices, max_idx, false);
+  mapLineToPolygon(model_position, 0.9, path_offset_z, &track_vertices, max_idx, false);
 }
 
 void ModelRenderer::drawLaneLines(QPainter &painter) {

diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h
@@ -29,6 +29,7 @@ class ModelRenderer {
   bool prev_allow_throttle = true;
   float lane_line_probs[4] = {};
   float road_edge_stds[2] = {};
+  float path_offset_z = 1.22f;
   QPolygonF track_vertices;
   QPolygonF lane_line_vertices[4] = {};
   QPolygonF road_edge_vertices[2] = {};