diff --git a/common/params.py b/common/params.py index 1ba9a0aeb..efcaa756e 100755 --- a/common/params.py +++ b/common/params.py @@ -93,6 +93,7 @@ keys = { "DragonCachedFP": [TxType.PERSISTENT], "DragonCachedVIN": [TxType.PERSISTENT], "DragonAllowGas": [TxType.PERSISTENT], + "DragonBBUI": [TxType.PERSISTENT], } diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index c41bed987..d1f94bda0 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -18,6 +18,7 @@ default_conf = { 'DragonCachedFP': '', # for cache car 'DragonCachedVIN': '', # for cache car 'DragonAllowGas': '0', + 'DragonBBUI': '0', } def dragonpilot_set_params(params): diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 70cb840d3..03e4c0d40 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -69,6 +69,7 @@ const int viz_w = vwp_w-(bdr_s*2); const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; +const int bdr_is = 30; const int UI_FREQ = 30; // Hz @@ -266,11 +267,13 @@ typedef struct UIState { int is_metric_timeout; int longitudinal_control_timeout; int limit_set_speed_timeout; + int dragon_bbui_timeout; int status; bool is_metric; bool longitudinal_control; bool limit_set_speed; + bool dragon_bbui; float speed_lim_off; bool is_ego_over_limit; char alert_type[64]; @@ -662,11 +665,14 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, read_param_bool(&s->is_metric, "IsMetric"); read_param_bool(&s->longitudinal_control, "LongitudinalControl"); read_param_bool(&s->limit_set_speed, "LimitSetSpeed"); + read_param_bool(&s->dragon_bbui, "DragonBBUI"); + // Set offsets so params don't get read at the same time s->longitudinal_control_timeout = UI_FREQ / 3; s->is_metric_timeout = UI_FREQ / 2; s->limit_set_speed_timeout = UI_FREQ; + s->dragon_bbui_timeout = UI_FREQ / 4; } static void ui_draw_transformed_box(UIState *s, uint32_t color) { @@ -1490,6 +1496,209 @@ static void ui_draw_infobar(UIState *s) { nvgText(s->vg, rect_x + 720 + sidebar_offset, rect_y + 35, infobar, NULL); } +//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 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; + } + + //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 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); + //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)); + + 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 ui_draw_bbui(UIState *s) { + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_is * 2)); + const int bb_dml_y = (box_y + (bdr_is * 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_is * 2); + const int bb_dmr_y = (box_y + (bdr_is * 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); +} + static void ui_draw_vision_footer(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; @@ -1503,6 +1712,9 @@ static void ui_draw_vision_footer(UIState *s) { #ifdef SHOW_SPEEDLIMIT ui_draw_vision_map(s); #endif + if (s->dragon_bbui) { + ui_draw_bbui(s); + } ui_draw_infobar(s); } @@ -2404,6 +2616,7 @@ int main(int argc, char* argv[]) { read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout); read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout); read_param_float_timeout(&s->speed_lim_off, "SpeedLimitOffset", &s->limit_set_speed_timeout); + read_param_bool_timeout(&s->dragon_bbui, "DragonBBUI", &s->dragon_bbui_timeout); pthread_mutex_unlock(&s->lock);