From 19c7fb308fdc5063fc37004fc1281bb03a663d89 Mon Sep 17 00:00:00 2001 From: Prius <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 12 Dec 2020 13:18:04 -0700 Subject: [PATCH] smart speed. --- selfdrive/ui/paint.cc | 132 +++++++++++++++++++++++++++++++++++++----- selfdrive/ui/ui.cc | 13 ++++- selfdrive/ui/ui.hpp | 14 ++++- 3 files changed, 142 insertions(+), 17 deletions(-) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 632cc2875fd85b..90d649a31e193d 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,3 +1,8 @@ +#include +#include +#include +#include +#include #include "ui.hpp" #include #include @@ -19,6 +24,7 @@ extern "C"{ #include "sidebar.hpp" #include "paint_dp.hpp" +int border_shifter = 20; // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size @@ -101,7 +107,7 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int image, NVGcolor color, float img_alpha, int img_y = 0) { const int img_size = size * 1.5; nvgBeginPath(vg); - nvgCircle(vg, x, y + (bdr_s * 1.5), size); + nvgCircle(vg, x, y + (bdr_is * 1.5), size); nvgFillColor(vg, color); nvgFill(vg); ui_draw_image(vg, x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size, image, img_alpha); @@ -295,25 +301,38 @@ static void ui_draw_vision_maxspeed(UIState *s) { char maxspeed_str[32]; float maxspeed = s->scene.controls_state.getVCruise(); int maxspeed_calc = maxspeed * 0.6225 + 0.5; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; if (s->is_metric) { maxspeed_calc = maxspeed + 0.5; + speedlim_calc = speedlimit * 3.6 + 0.5; } - + int speed_lim_off = speedlim_calc * (1 + s->speed_lim_off / 100.0); bool is_cruise_set = (maxspeed != 0 && maxspeed != SET_SPEED_NA); - + bool is_speedlim_valid = s->scene.speedlimit_valid; + bool is_set_over_limit = is_speedlim_valid && s->scene.controls_state.getEnabled() && + is_cruise_set && maxspeed_calc > (speedlim_calc + speed_lim_off); int viz_maxspeed_w = 184; int viz_maxspeed_h = 202; - int viz_maxspeed_x = s->scene.viz_rect.x + (bdr_s*2); - int viz_maxspeed_y = s->scene.viz_rect.y + (bdr_s*1.5); + int viz_maxspeed_x = (s->video_rect.x + (bdr_is*2)); + int viz_maxspeed_y = (s->video_rect.y + (bdr_is*1.5)); int viz_maxspeed_xo = 180; - viz_maxspeed_xo = 0; + viz_maxspeed_w += viz_maxspeed_xo; + viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2); + //viz_maxspeed_xo = 0; // Draw Background - ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, COLOR_BLACK_ALPHA(100), 30); + ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, + is_set_over_limit ? nvgRGBA(218, 111, 37, 180) : COLOR_BLACK_ALPHA(100), 30); // Draw Border NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_set_over_limit) { + color = COLOR_OCHRE; + } else if (is_speedlim_valid) { + color = s->is_ego_over_limit ? COLOR_WHITE_ALPHA(20) : COLOR_WHITE; + } ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, color, 20, 10); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); @@ -328,6 +347,65 @@ static void ui_draw_vision_maxspeed(UIState *s) { } } +static void ui_draw_vision_speedlimit(UIState *s) { + char speedlim_str[32]; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; + if (s->is_metric) { + speedlim_calc = speedlimit * 3.6 + 0.5; + } + + bool is_speedlim_valid = s->scene.speedlimit_valid; + float hysteresis_offset = 0.5; + if (s->is_ego_over_limit) { + hysteresis_offset = 0.0; + } + s->is_ego_over_limit = is_speedlim_valid && s->scene.controls_state.getVEgo() > (speedlimit + hysteresis_offset); + + int viz_speedlim_w = 180; + int viz_speedlim_h = 202; + int viz_speedlim_x = (s->video_rect.x + (bdr_s*2)); + int viz_speedlim_y = (s->video_rect.y + (bdr_s*1.5)); + if (!is_speedlim_valid) { + viz_speedlim_w -= 5; + viz_speedlim_h -= 10; + viz_speedlim_x += 9; + viz_speedlim_y += 5; + } + // Draw Background + NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_speedlim_valid && s->is_ego_over_limit) { + color = nvgRGBA(218, 111, 37, 180); + } else if (is_speedlim_valid) { + color = COLOR_WHITE; + } + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, color, is_speedlim_valid ? 30 : 15); + + // Draw Border + if (is_speedlim_valid) { + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, + s->is_ego_over_limit ? COLOR_OCHRE : COLOR_WHITE, 20, 10); + } + const float text_x = viz_speedlim_x + viz_speedlim_w / 2; + const float text_y = viz_speedlim_y + (is_speedlim_valid ? 50 : 45); + // Draw "Speed Limit" Text + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + color = is_speedlim_valid && s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y, "SMART", 50, color, s->font_sans_semibold); + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y + 40, "SPEED", 50, color, s->font_sans_semibold); + + // Draw Speed Text + color = s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + if (is_speedlim_valid) { + snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc); + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, 48*2.5, color, s->font_sans_bold); + } else { + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", 42*2.5, color, s->font_sans_semibold); + } +} + + + static void ui_draw_vision_speed(UIState *s) { const Rect &viz_rect = s->scene.viz_rect; float v_ego = s->scene.controls_state.getVEgo(); @@ -377,12 +455,23 @@ static void ui_draw_vision_speed(UIState *s) { static void ui_draw_vision_event(UIState *s) { const int viz_event_w = 220; - const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_s*2); - const int viz_event_y = s->scene.viz_rect.y + (bdr_s*1.5); - if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { + const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_is*2); + const int viz_event_y = s->scene.viz_rect.y + (bdr_is*1.5); + if (s->scene.speedlimitahead_valid && s->scene.speedlimitaheaddistance < 300 && s->scene.controls_state.getEnabled() && s->limit_set_speed) { + const int img_turn_size = 160; + const int img_turn_x = viz_event_x-(img_turn_size/4)+80; + const int img_turn_y = viz_event_y+bdr_s-25; + float img_turn_alpha = 1.0f; + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y, + img_turn_size, img_turn_size, 0, s->img_speed, img_turn_alpha); + nvgRect(s->vg, img_turn_x, img_turn_y, img_turn_size, img_turn_size); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); + } else if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { // draw winding road sign - const int img_turn_size = 160*1.5; - ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); + const int img_turn_size = 160*1.5*0.82; + ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_is - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; @@ -394,9 +483,16 @@ static void ui_draw_vision_event(UIState *s) { } } +static void ui_draw_vision_map(UIState *s) { + const int map_size = 96; + const int map_x = (s->video_rect.x + (map_size * 3) + (bdr_s * 3)); + const int map_y = (s->scene.viz_rect.bottom() + ((footer_h - map_size) / 2)); + ui_draw_circle_image(s->vg, map_x, map_y, map_size, s->img_map, s->scene.map_valid); +} + static void ui_draw_vision_face(UIState *s) { const int face_size = 96; - const int face_x = (s->scene.viz_rect.x + face_size + (bdr_s * 2)); + const int face_x = (s->scene.viz_rect.x + face_size + (bdr_is * 2)); const int face_y = (s->scene.viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected()); } @@ -465,6 +561,7 @@ static void ui_draw_vision_header(UIState *s) { } if (s->scene.dpUiMaxSpeed) { ui_draw_vision_maxspeed(s); + ui_draw_vision_speedlimit(s); } if (s->scene.dpUiSpeed) { ui_draw_vision_speed(s); @@ -477,6 +574,7 @@ static void ui_draw_vision_header(UIState *s) { static void ui_draw_vision_footer(UIState *s) { if (s->scene.dpUiFace) { ui_draw_vision_face(s); + ui_draw_vision_map(s); } if ((int)s->scene.dpDynamicFollow > 0) { ui_draw_df_button(s); @@ -510,8 +608,8 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, color.a *= s->alert_blinking_alpha; int alr_s = alert_size_map[va_size]; - const int alr_x = scene->viz_rect.x - bdr_s + 100; - const int alr_w = scene->viz_rect.w + (bdr_s*2) - 200; + const int alr_x = scene->viz_rect.x - bdr_is + 100; + const int alr_w = scene->viz_rect.w + (bdr_is*2) - 200; const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s) - 100; const int alr_y = s->fb_h-alr_h - 100; @@ -702,6 +800,10 @@ void ui_nvg_init(UIState *s) { s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); assert(s->img_wheel != 0); + s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); + assert(s->img_map != 0); + s->img_speed = nvgCreateImage(s->vg, "../assets/img_trafficSign_speedahead.png", 1); + assert(s->img_speed != 0); s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); assert(s->img_turn != 0); s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2fb22f64f11980..0b2a110bacd8ad 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -25,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", "dragonConf", "carState"}); @@ -204,6 +204,13 @@ void update_sockets(UIState *s) { if (sm.updated("thermal")) { scene.thermal = sm["thermal"].getThermal(); } + if (sm.updated("liveMapData")) { + scene.map_valid = sm["liveMapData"].getLiveMapData().getMapValid(); + scene.speedlimit = sm["liveMapData"].getLiveMapData().getSpeedLimit(); + scene.speedlimit_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitValid(); + scene.speedlimitahead_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadValid(); + scene.speedlimitaheaddistance = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadDistance(); + } if (sm.updated("ubloxGnss")) { auto data = sm["ubloxGnss"].getUbloxGnss(); if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) { @@ -332,6 +339,10 @@ void ui_update(UIState *s) { // Read params if ((s->sm)->frame % (5*UI_FREQ) == 0) { read_param(&s->is_metric, "IsMetric"); + } else if ((s->sm)->frame % (7*UI_FREQ) == 0) { + read_param(&s->speed_lim_off, "SpeedLimitOffset"); + } else if ((s->sm)->frame % (11*UI_FREQ) == 0) { + read_param(&s->limit_set_speed, "LimitSetSpeed"); } else if ((s->sm)->frame % (6*UI_FREQ) == 0) { int param_read = read_param(&s->last_athena_ping, "LastAthenaPingTime"); if (param_read != 0) { // Failed to read param diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index afeb64468f7268..48795f762faa5a 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -34,6 +34,7 @@ #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) #define COLOR_RED_ALPHA(x) nvgRGBA(201, 34, 49, x) +#define COLOR_OCHRE nvgRGBA(218, 111, 37, 255) #define UI_BUF_COUNT 4 @@ -48,7 +49,8 @@ typedef struct Rect { } Rect; const int sbr_w = 300; -const int bdr_s = 30; +const int bdr_s = 10; +const int bdr_is = 30; const int header_h = 420; const int footer_h = 280; const Rect settings_btn = {50, 35, 200, 117}; @@ -106,6 +108,11 @@ typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; + float speedlimit; + bool speedlimit_valid; + float speedlimitaheaddistance; + bool speedlimitahead_valid; + bool map_valid; bool is_rhd; bool frontview; @@ -219,6 +226,8 @@ typedef struct UIState { int img_battery; int img_battery_charging; int img_network[6]; + int img_map; + int img_speed; SubMaster *sm; @@ -250,6 +259,9 @@ typedef struct UIState { bool ignition; bool is_metric; bool longitudinal_control; + bool limit_set_speed; + bool is_ego_over_limit; + float speed_lim_off; uint64_t last_athena_ping; uint64_t started_frame;