diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 0728a5eab3b086..62159c94636a30 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -627,8 +627,8 @@ static void ui_draw_vision_event(UIState *s) { const int viz_event_h = (header_h - (bdr_s*1.5)); if (s->scene.decel_for_model && s->scene.engaged) { // draw winding road sign - const int img_turn_size = 160*1.5; - const int img_turn_x = viz_event_x-(img_turn_size/4); + 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); @@ -645,6 +645,7 @@ static void ui_draw_vision_event(UIState *s) { const int img_wheel_size = bg_wheel_size*1.5; const int img_wheel_x = bg_wheel_x-(img_wheel_size/2); const int img_wheel_y = bg_wheel_y-25; + const float img_rotation = s->scene.angleSteers/180*3.141592; float img_wheel_alpha = 0.1f; bool is_engaged = (s->status == STATUS_ENGAGED); bool is_warning = (s->status == STATUS_WARNING); @@ -662,12 +663,16 @@ static void ui_draw_vision_event(UIState *s) { nvgFill(s->vg); img_wheel_alpha = 1.0f; } - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y, - img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); - nvgRect(s->vg, img_wheel_x, img_wheel_y, img_wheel_size, img_wheel_size); - nvgFillPaint(s->vg, imgPaint); + nvgSave(s->vg); + nvgTranslate(s->vg,bg_wheel_x,(bg_wheel_y + (bdr_s*1.5))); + nvgRotate(s->vg,-img_rotation); + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), + img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); + nvgRect(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), img_wheel_size, img_wheel_size); + nvgFillPaint(s->vg, imgPaint); nvgFill(s->vg); + nvgRestore(s->vg); } } @@ -746,6 +751,305 @@ static void ui_draw_vision_header(UIState *s) { ui_draw_vision_event(s); } +//BB START: functions added for the display of various items +static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_uom, const char* bb_label, + int bb_x, int bb_y, int bb_uom_dx, + NVGcolor bb_valueColor, NVGcolor bb_labelColor, NVGcolor bb_uomColor, + int bb_valueFontSize, int bb_labelFontSize, int bb_uomFontSize ) { + const UIScene *scene = &s->scene; + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + int dx = 0; + if (strlen(bb_uom) > 0) { + dx = (int)(bb_uomFontSize*2.5/2); + } + //print value + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, bb_valueFontSize*2.5); + nvgFillColor(s->vg, bb_valueColor); + nvgText(s->vg, bb_x-dx/2, bb_y+ (int)(bb_valueFontSize*2.5)+5, bb_value, NULL); + //print label + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, bb_labelFontSize*2.5); + nvgFillColor(s->vg, bb_labelColor); + nvgText(s->vg, bb_x, bb_y + (int)(bb_valueFontSize*2.5)+5 + (int)(bb_labelFontSize*2.5)+5, bb_label, NULL); + //print uom + if (strlen(bb_uom) > 0) { + nvgSave(s->vg); + int rx =bb_x + bb_uom_dx + bb_valueFontSize -3; + int ry = bb_y + (int)(bb_valueFontSize*2.5/2)+25; + nvgTranslate(s->vg,rx,ry); + nvgRotate(s->vg, -1.5708); //-90deg in radians + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, (int)(bb_uomFontSize*2.5)); + nvgFillColor(s->vg, bb_uomColor); + nvgText(s->vg, 0, 0, bb_uom, NULL); + nvgRestore(s->vg); + } + return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5; +} + +static void bb_ui_draw_measures_left(UIState *s, int bb_x, int bb_y, int bb_w ) { + const UIScene *scene = &s->scene; + int bb_rx = bb_x + (int)(bb_w/2); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = nvgRGBA(255, 255, 255, 200); + NVGcolor uom_color = nvgRGBA(255, 255, 255, 200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + + //add CPU temperature + /* + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if((int)(scene->maxCpuTemp/10) > 80) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(scene->maxCpuTemp/10) > 92) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // temp is alway in C * 10 + snprintf(val_str, sizeof(val_str), "%d°C", (int)(scene->maxCpuTemp/10)); + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "CPU TEMP", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + */ + + //add battery temperature + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (s->scene.pa0 > 50) { + val_color = nvgRGBA(255, 0, 0, 200); + } else if (s->scene.pa0 > 40) { + val_color = nvgRGBA(255, 188, 3, 200); + } + + snprintf(val_str, sizeof(val_str), "%2.0f°C", s->scene.pa0); + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "BAT TEMP", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add grey panda GPS accuracy + /*if (true) { + char val_str[16]; + char uom_str[3]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + //show red/orange if gps accuracy is high + if(scene->gpsAccuracy > 0.59) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(scene->gpsAccuracy > 0.8) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // gps accuracy is always in meters + snprintf(val_str, sizeof(val_str), "%.2f", (s->scene.gpsAccuracy)); + snprintf(uom_str, sizeof(uom_str), "m");; + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "GPS PREC", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + }*/ + + //add free space - from bthaler1 + if (true) { + char val_str[16]; + char uom_str[3]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + + //show red/orange if free space is low + if(scene->freeSpace < 0.4) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(scene->freeSpace < 0.2) { + val_color = nvgRGBA(255, 0, 0, 200); + } + + snprintf(val_str, sizeof(val_str), "%.0f%%", s->scene.freeSpace* 100); + snprintf(uom_str, sizeof(uom_str), ""); + + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "FREE SPACE", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + + +static void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) { + const UIScene *scene = &s->scene; + int bb_rx = bb_x + (int)(bb_w/2); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = nvgRGBA(255, 255, 255, 200); + NVGcolor uom_color = nvgRGBA(255, 255, 255, 200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + + //add visual radar relative distance + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (scene->lead_status) { + //show RED if less than 5 meters + //show orange if less than 15 meters + if((int)(scene->lead_d_rel) < 15) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(scene->lead_d_rel) < 5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative distance is always in meters + snprintf(val_str, sizeof(val_str), "%d", (int)scene->lead_d_rel); + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + snprintf(uom_str, sizeof(uom_str), "m "); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL DIST", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add visual radar relative speed + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (scene->lead_status) { + //show Orange if negative speed (approaching) + //show Orange if negative speed faster than 5mph (approaching fast) + if((int)(scene->lead_v_rel) < 0) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(scene->lead_v_rel) < -5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative speed is always in meters + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 3.6 + 0.5)); + } else { + snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 2.2374144 + 0.5)); + } + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + if (s->is_metric) { + snprintf(uom_str, sizeof(uom_str), "km/h");; + } else { + snprintf(uom_str, sizeof(uom_str), "mph"); + } + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL SPEED", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(scene->angleSteers) < -6) || ((int)(scene->angleSteers) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(scene->angleSteers) < -12) || ((int)(scene->angleSteers) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteers)); + + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REAL STEER", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add desired steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (scene->engaged) { + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(scene->angleSteersDes) < -6) || ((int)(scene->angleSteersDes) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(scene->angleSteersDes) < -12) || ((int)(scene->angleSteersDes) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteersDes)); + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "DESIR STEER", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + +static void bb_ui_draw_UI(UIState *s) +{ + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_s * 2)); + const int bb_dml_y = (box_y + (bdr_s * 1.5)) + 220; + + const int bb_dmr_w = 180; + const int bb_dmr_x = scene->ui_viz_rx + scene->ui_viz_rw - bb_dmr_w - (bdr_s * 2); + const int bb_dmr_y = (box_y + (bdr_s * 1.5)) + 220; + + bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w); + bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w); +} +//BB END: functions added for the display of various items + static void ui_draw_vision_footer(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; @@ -757,8 +1061,9 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_face(s); #ifdef SHOW_SPEEDLIMIT - // ui_draw_vision_map(s); + ui_draw_vision_map(s); #endif + bb_ui_draw_UI(s); } void ui_draw_vision_alert(UIState *s, int va_size, int va_color, diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 6a36f49198b7c9..863d39591060af 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -20,6 +20,7 @@ #include "ui.hpp" #include "sound.hpp" + static int last_brightness = -1; static void set_brightness(UIState *s, int brightness) { if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -112,25 +113,28 @@ static void ui_init(UIState *s) { s->uilayout_sock = SubSocket::create(s->ctx, "uiLayoutState"); s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration"); s->radarstate_sock = SubSocket::create(s->ctx, "radarState"); + s->thermal_sock = SubSocket::create(s->ctx, "thermal"); assert(s->model_sock != NULL); assert(s->controlsstate_sock != NULL); assert(s->uilayout_sock != NULL); assert(s->livecalibration_sock != NULL); assert(s->radarstate_sock != NULL); + assert(s->thermal_sock != NULL); s->poller = Poller::create({ s->model_sock, s->controlsstate_sock, s->uilayout_sock, s->livecalibration_sock, - s->radarstate_sock + s->radarstate_sock, + s->thermal_sock }); #ifdef SHOW_SPEEDLIMIT - s->map_data_sock = SubSock::create(s->ctx, "liveMapData"); + s->map_data_sock = SubSocket::create(s->ctx, "liveMapData"); assert(s->map_data_sock != NULL); - s->poller.registerSocket(s->map_data_sock); + s->poller->registerSocket(s->map_data_sock); #endif s->ipc_fd = -1; @@ -275,6 +279,7 @@ void handle_message(UIState *s, Message * msg) { } s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; + s->scene.angleSteers = datad.angleSteers; s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; s->scene.engageable = datad.engageable; @@ -285,6 +290,9 @@ void handle_message(UIState *s, Message * msg) { s->scene.decel_for_model = datad.decelForModel; + // getting steering related data for dev ui + s->scene.angleSteersDes = datad.angleSteersDes; + if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) { if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { stop_alert_sound(s->alert_sound); @@ -418,6 +426,17 @@ void handle_message(UIState *s, Message * msg) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); s->scene.map_valid = datad.mapValid; + s->scene.speedlimit = datad.speedLimit; + s->scene.speedlimitahead_valid = datad.speedLimitAheadValid; + s->scene.speedlimitaheaddistance = datad.speedLimitAheadDistance; + s->scene.speedlimit_valid = datad.speedLimitValid; + // getting thermal related data for dev ui + } else if (eventd.which == cereal_Event_thermal) { + struct cereal_ThermalData datad; + cereal_read_ThermalData(&datad, eventd.thermal); + + s->scene.pa0 = datad.pa0; + s->scene.freeSpace = datad.freeSpace; } capn_free(&ctx); } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index e520f6eae48d2c..aa6682e0beb468 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -32,7 +32,7 @@ #endif #define UI_BUF_COUNT 4 -//#define SHOW_SPEEDLIMIT 1 +#define SHOW_SPEEDLIMIT 1 //#define DEBUG_TURN const int vwp_w = 1920; @@ -87,6 +87,9 @@ typedef struct UIScene { bool decel_for_model; float speedlimit; + float angleSteers; + float speedlimitaheaddistance; + bool speedlimitahead_valid; bool speedlimit_valid; bool map_valid; @@ -117,6 +120,12 @@ typedef struct UIScene { // Used to show gps planner status bool gps_planner_active; + + // dev ui + float angleSteersDes; + float pa0; + float freeSpace; + } UIScene; typedef struct { @@ -242,6 +251,9 @@ typedef struct UIState { model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; track_vertices_data track_vertices[2]; + + // dev ui + SubSocket *thermal_sock; } UIState; // API