diff --git a/common/params.py b/common/params.py index 451a9c250..51616c5be 100755 --- a/common/params.py +++ b/common/params.py @@ -88,6 +88,7 @@ keys = { "DragonCachedModel": [TxType.PERSISTENT], "DragonCachedFP": [TxType.PERSISTENT], "DragonCachedVIN": [TxType.PERSISTENT], + "DragonBBUI": [TxType.PERSISTENT], } diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 0234337b2..226d62e20 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -16,6 +16,7 @@ default_conf = { 'DragonCachedModel': '', # for cache car 'DragonCachedFP': '', # for cache car 'DragonCachedVIN': '', # for cache car + 'DragonBBUI': '0', } def dragonpilot_set_params(params): diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 70cb840d3..8b02b67af 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -266,11 +266,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 +664,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 +1495,173 @@ static void ui_draw_infobar(UIState *s) { nvgText(s->vg, rect_x + 720 + sidebar_offset, rect_y + 35, infobar, NULL); } + +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,7 +1675,11 @@ static void ui_draw_vision_footer(UIState *s) { #ifdef SHOW_SPEEDLIMIT ui_draw_vision_map(s); #endif - ui_draw_infobar(s); + if (&s->dragon_bbui) { + ui_draw_bbui(s); + } else { + ui_draw_infobar(s); + } } static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,