diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 1b400403e8..e4c3f60e54 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -177,69 +177,24 @@ static void draw_frame(UIState *s) { glBindVertexArray(0); } -template -static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, - float y_off, float z_off, T *pvd, float max_distance) { - const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); - int max_idx = -1; - vertex_data *v = &pvd->v[0]; - const float margin = 500.0f; - for (int i = 0; ((i < TRAJECTORY_SIZE) and (line_x[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { - v += car_space_to_full_frame(s, line_x[i], -line_y[i] - y_off, -line_z[i] + z_off, &v->x, &v->y, margin); - max_idx = i; - } - for (int i = max_idx; i >= 0; i--) { - v += car_space_to_full_frame(s, line_x[i], -line_y[i] + y_off, -line_z[i] + z_off, &v->x, &v->y, margin); - } - pvd->cnt = v - pvd->v; - assert(pvd->cnt < std::size(pvd->v)); -} - -static void update_model(UIState *s) { - const UIScene *scene = &s->scene; - // update lane lines - const auto lane_lines = scene->model.getLaneLines(); - const auto lane_line_probs = scene->model.getLaneLineProbs(); - for (int i = 0; i < std::size(s->lane_line_vertices); i++) { - update_line_data(s, lane_lines[i], 0.025 * lane_line_probs[i], 1.22, &s->lane_line_vertices[i], scene->max_distance); - } - - // update road edges - const auto road_edges = scene->model.getRoadEdges(); - for (int i = 0; i < std::size(s->road_edge_vertices); i++) { - update_line_data(s, road_edges[i], 0.025, 1.22, &s->road_edge_vertices[i], scene->max_distance); - } - - // update path - const float lead_d = s->sm->updated("radarState") ? scene->lead_data[0].getDRel() * 2. : MAX_DRAW_DISTANCE; - float path_length = (lead_d > 0.) ? lead_d - fmin(lead_d * 0.35, 10.) : MAX_DRAW_DISTANCE; - path_length = fmin(path_length, scene->max_distance); - update_line_data(s, scene->model.getPosition(), 0.5, 0, &s->track_vertices, path_length); -} - static void ui_draw_vision_lane_lines(UIState *s) { - const UIScene *scene = &s->scene; - if (s->sm->updated("modelV2")) { - update_model(s); - } + const UIScene &scene = s->scene; // paint lanelines - const auto lane_line_probs = scene->model.getLaneLineProbs(); - for (int i = 0; i < std::size(s->lane_line_vertices); i++) { - NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, lane_line_probs[i]); - ui_draw_line(s, s->lane_line_vertices[i].v, s->lane_line_vertices[i].cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]); + ui_draw_line(s, scene.lane_line_vertices[i].v, scene.lane_line_vertices[i].cnt, &color, nullptr); } // paint road edges - const auto road_edge_stds = scene->model.getRoadEdgeStds(); - for (int i = 0; i < std::size(s->road_edge_vertices); i++) { - NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0 - road_edge_stds[i], 0.0, 1.0)); - ui_draw_line(s, s->road_edge_vertices[i].v, s->road_edge_vertices[i].cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0)); + ui_draw_line(s, scene.road_edge_vertices[i].v, scene.road_edge_vertices[i].cnt, &color, nullptr); } // paint path NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, COLOR_WHITE, COLOR_WHITE_ALPHA(0)); - ui_draw_line(s, s->track_vertices.v, s->track_vertices.cnt, nullptr, &track_bg); + ui_draw_line(s, scene.track_vertices.v, scene.track_vertices.cnt, nullptr, &track_bg); } // Draw all world space objects. diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3ae909b4d1..ec690eeea0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -16,6 +16,8 @@ #include "ui.hpp" #include "paint.hpp" +extern bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin); + int write_param_float(float param, const char* param_name, bool persistent_param) { char s[16]; int size = snprintf(s, sizeof(s), "%f", param); @@ -104,6 +106,50 @@ destroy: s->vision_connected = false; } +template +static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, + float y_off, float z_off, T *pvd, float max_distance) { + const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); + int max_idx = -1; + vertex_data *v = &pvd->v[0]; + const float margin = 500.0f; + for (int i = 0; ((i < TRAJECTORY_SIZE) and (line_x[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { + v += car_space_to_full_frame(s, line_x[i], -line_y[i] - y_off, -line_z[i] + z_off, &v->x, &v->y, margin); + max_idx = i; + } + for (int i = max_idx; i >= 0; i--) { + v += car_space_to_full_frame(s, line_x[i], -line_y[i] + y_off, -line_z[i] + z_off, &v->x, &v->y, margin); + } + pvd->cnt = v - pvd->v; + assert(pvd->cnt < std::size(pvd->v)); +} + +static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { + UIScene &scene = s->scene; + const float max_distance = fmin(model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + // update lane lines + const auto lane_lines = model.getLaneLines(); + const auto lane_line_probs = model.getLaneLineProbs(); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + scene.lane_line_probs[i] = lane_line_probs[i]; + update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 1.22, &scene.lane_line_vertices[i], max_distance); + } + + // update road edges + const auto road_edges = model.getRoadEdges(); + const auto road_edge_stds = model.getRoadEdgeStds(); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + scene.road_edge_stds[i] = road_edge_stds[i]; + update_line_data(s, road_edges[i], 0.025, 1.22, &scene.road_edge_vertices[i], max_distance); + } + + // update path + const float lead_d = s->sm->updated("radarState") ? scene.lead_data[0].getDRel() * 2. : MAX_DRAW_DISTANCE; + float path_length = (lead_d > 0.) ? lead_d - fmin(lead_d * 0.35, 10.) : MAX_DRAW_DISTANCE; + path_length = fmin(path_length, max_distance); + update_line_data(s, model.getPosition(), 0.5, 0, &scene.track_vertices, path_length); +} + void update_sockets(UIState *s) { UIScene &scene = s->scene; @@ -170,8 +216,7 @@ void update_sockets(UIState *s) { } } if (sm.updated("modelV2")) { - scene.model = sm["modelV2"].getModelV2(); - scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + update_model(s, sm["modelV2"].getModelV2()); } if (sm.updated("uiLayoutState")) { auto data = sm["uiLayoutState"].getUiLayoutState(); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 49fe92f11c..767352ef72 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -86,11 +86,18 @@ static std::map bg_colors = { }; typedef struct { - float x[TRAJECTORY_SIZE]; - float y[TRAJECTORY_SIZE]; - float z[TRAJECTORY_SIZE]; -} line; + float x, y; +} vertex_data; +typedef struct { + vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; + int cnt; +} line_vertices_data; + +typedef struct { + vertex_data v[TRACK_POINTS_MAX_CNT]; + int cnt; +} track_vertices_data; typedef struct UIScene { @@ -117,31 +124,14 @@ typedef struct UIScene { cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; cereal::DMonitoringState::Reader dmonitoring_state; - cereal::ModelDataV2::Reader model; - line path; - line outer_left_lane_line; - line left_lane_line; - line right_lane_line; - line outer_right_lane_line; - line left_road_edge; - line right_road_edge; - float max_distance; -} UIScene; - -typedef struct { - float x, y; -} vertex_data; - -typedef struct { - vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; - int cnt; -} line_vertices_data; - -typedef struct { - vertex_data v[TRACK_POINTS_MAX_CNT]; - int cnt; -} track_vertices_data; + // modelV2 + float lane_line_probs[4]; + float road_edge_stds[2]; + track_vertices_data track_vertices; + line_vertices_data lane_line_vertices[4]; + line_vertices_data road_edge_vertices[2]; +} UIScene; typedef struct UIState { // framebuffer @@ -199,10 +189,6 @@ typedef struct UIState { bool alert_blinked; float alert_blinking_alpha; - track_vertices_data track_vertices; - line_vertices_data lane_line_vertices[4]; - line_vertices_data road_edge_vertices[2]; - Rect video_rect; } UIState;