diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index cb1753a04d18a9..af147697d3a01d 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -45,7 +45,10 @@ keys = { b"LastAthenaPingTime": [TxType.PERSISTENT], b"LastUpdateTime": [TxType.PERSISTENT], b"LastUpdateException": [TxType.PERSISTENT], + b"LimitSetSpeed": [TxType.PERSISTENT], + b"LimitSetSpeedNeural": [TxType.PERSISTENT], b"LiveParameters": [TxType.PERSISTENT], + b"LongitudinalControl": [TxType.PERSISTENT], b"OpenpilotEnabledToggle": [TxType.PERSISTENT], b"LaneChangeEnabled": [TxType.PERSISTENT], b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], @@ -56,6 +59,7 @@ keys = { b"ReleaseNotes": [TxType.PERSISTENT], b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], b"SubscriberInfo": [TxType.PERSISTENT], + b"SpeedLimitOffset": [TxType.PERSISTENT], b"TermsVersion": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], diff --git a/selfdrive/assets/img_trafficSign_speedahead.png b/selfdrive/assets/img_trafficSign_speedahead.png new file mode 100644 index 00000000000000..8357ff8a9112fc Binary files /dev/null and b/selfdrive/assets/img_trafficSign_speedahead.png differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 9f1b1efdab2387..50f4975120b284 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -580,6 +580,10 @@ def main(): ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), + ("SpeedLimitOffset", "0"), + ("LongitudinalControl", "1"), + ("LimitSetSpeed", "1"), + ("LimitSetSpeedNeural", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 632cc2875fd85b..c0ee6f989f75cc 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,8 @@ 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 @@ -295,36 +302,105 @@ 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); const int text_x = viz_maxspeed_x + (viz_maxspeed_xo / 2) + (viz_maxspeed_w / 2); - ui_draw_text(s->vg, text_x, 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); + ui_draw_text(s->vg, text_x, 148-border_shifter, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); if (is_cruise_set) { snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); - ui_draw_text(s->vg, text_x, 242, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, text_x, 242-border_shifter, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); } else { - ui_draw_text(s->vg, text_x, 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + ui_draw_text(s->vg, text_x, 242-border_shifter, "-", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + } +} + +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_is*2)); + int viz_speedlim_y = (s->video_rect.y + (bdr_is*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); } } @@ -377,12 +453,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 +481,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 +559,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 +572,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,9 +606,9 @@ 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_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s) - 100; + 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_is) - 100; const int alr_y = s->fb_h-alr_h - 100; ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color, 20); @@ -702,6 +798,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/paint_dp.cc b/selfdrive/ui/paint_dp.cc index 1a0515699a617e..4fbf7c413ab687 100644 --- a/selfdrive/ui/paint_dp.cc +++ b/selfdrive/ui/paint_dp.cc @@ -354,12 +354,12 @@ void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) { void ui_draw_bbui(UIState *s) { const int bb_dml_w = 184; - const int bb_dml_x = s->scene.viz_rect.x + (bdr_s*2); - const int bb_dml_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dml_x = s->scene.viz_rect.x + (bdr_is*2); + const int bb_dml_y = s->scene.viz_rect.y + (bdr_is*1.5) + 220; const int bb_dmr_w = 184; - const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_s * 2); - const int bb_dmr_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_is * 2); + const int bb_dmr_y = s->scene.viz_rect.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); -} \ No newline at end of file +} 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..18a743559861ac 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -34,6 +34,8 @@ #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 COLOR_OCHRE_ALPHA(x) nvgRGBA(218, 111, 37, x) #define UI_BUF_COUNT 4 @@ -48,7 +50,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}; @@ -107,6 +110,12 @@ 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; bool uilayout_sidebarcollapsed; @@ -219,6 +228,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 +261,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;