#include #include #include #include #include #include #include #include #include #include #include #include #include "nanovg.h" #define NANOVG_GLES3_IMPLEMENTATION #include "nanovg_gl.h" #include "nanovg_gl_utils.h" #include "common/timing.h" #include "common/util.h" #include "common/swaglog.h" #include "common/mat.h" #include "common/glutil.h" #include "common/touch.h" #include "common/framebuffer.h" #include "common/visionipc.h" #include "common/visionimg.h" #include "common/modeldata.h" #include "common/params.h" #include "cereal/gen/c/log.capnp.h" #include "slplay.h" #define STATUS_STOPPED 0 #define STATUS_DISENGAGED 1 #define STATUS_ENGAGED 2 #define STATUS_WARNING 3 #define STATUS_ALERT 4 #define STATUS_MAX 5 #define ALERTSIZE_NONE 0 #define ALERTSIZE_SMALL 1 #define ALERTSIZE_MID 2 #define ALERTSIZE_FULL 3 #define UI_BUF_COUNT 4 //#define DEBUG_TURN //#define DEBUG_FPS const int vwp_w = 1920; const int vwp_h = 1080; const int nav_w = 640; const int nav_ww= 760; const int sbr_w = 300; const int bdr_s = 30; const int box_x = sbr_w+bdr_s; const int box_y = bdr_s; const int box_w = vwp_w-sbr_w-(bdr_s*2); const int box_h = vwp_h-(bdr_s*2); 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 UI_FREQ = 30; // Hz const int MODEL_PATH_MAX_VERTICES_CNT = 98; const int MODEL_LANE_PATH_CNT = 3; const int TRACK_POINTS_MAX_CNT = 50 * 2; const uint8_t bg_colors[][4] = { [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff}, [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff}, [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff}, [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff}, }; const uint8_t alert_colors[][4] = { [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1}, [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8}, [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, }; const int alert_sizes[] = { [ALERTSIZE_NONE] = 0, [ALERTSIZE_SMALL] = 241, [ALERTSIZE_MID] = 390, [ALERTSIZE_FULL] = vwp_h, }; const int SET_SPEED_NA = 255; // TODO: this is also hardcoded in common/transformations/camera.py const mat3 intrinsic_matrix = (mat3){{ 910., 0., 582., 0., 910., 437., 0., 0., 1. }}; typedef struct UIScene { int frontview; int fullview; int transformed_width, transformed_height; uint64_t model_ts; ModelData model; float mpc_x[50]; float mpc_y[50]; bool world_objects_visible; mat3 warp_matrix; // transformed box -> frame. mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. float v_cruise; uint64_t v_cruise_update_ts; float v_ego; float v_curvature; bool decel_for_turn; float speedlimit; bool speedlimit_valid; bool map_valid; float curvature; int engaged; bool engageable; bool monitoring_active; bool uilayout_sidebarcollapsed; bool uilayout_mapenabled; // responsive layout int ui_viz_rx; int ui_viz_rw; int ui_viz_ro; int lead_status; float lead_d_rel, lead_y_rel, lead_v_rel; int front_box_x, front_box_y, front_box_width, front_box_height; uint64_t alert_ts; char alert_text1[1024]; char alert_text2[1024]; uint8_t alert_size; float alert_blinkingrate; float awareness_status; uint64_t started_ts; // Used to show gps planner status bool gps_planner_active; bool is_playing_alert; } UIScene; typedef struct { float x, y; }vertex_data; typedef struct { vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; int cnt; } model_path_vertices_data; typedef struct { vertex_data v[TRACK_POINTS_MAX_CNT]; int cnt; } track_vertices_data; typedef struct UIState { pthread_mutex_t lock; pthread_cond_t bg_cond; FramebufferState *fb; int fb_w, fb_h; EGLDisplay display; EGLSurface surface; NVGcontext *vg; int font_courbd; int font_sans_regular; int font_sans_semibold; int font_sans_bold; int img_wheel; int img_turn; int img_face; int img_map; zsock_t *thermal_sock; void *thermal_sock_raw; zsock_t *model_sock; void *model_sock_raw; zsock_t *live100_sock; void *live100_sock_raw; zsock_t *livecalibration_sock; void *livecalibration_sock_raw; zsock_t *live20_sock; void *live20_sock_raw; zsock_t *livempc_sock; void *livempc_sock_raw; zsock_t *plus_sock; void *plus_sock_raw; zsock_t *map_data_sock; void *map_data_sock_raw; zsock_t *uilayout_sock; void *uilayout_sock_raw; int plus_state; // vision state bool vision_connected; bool vision_connect_firstrun; int ipc_fd; VIPCBuf bufs[UI_BUF_COUNT]; VIPCBuf front_bufs[UI_BUF_COUNT]; int cur_vision_idx; int cur_vision_front_idx; GLuint frame_program; GLuint frame_texs[UI_BUF_COUNT]; EGLImageKHR khr[UI_BUF_COUNT]; void *priv_hnds[UI_BUF_COUNT]; GLuint frame_front_texs[UI_BUF_COUNT]; EGLImageKHR khr_front[UI_BUF_COUNT]; void *priv_hnds_front[UI_BUF_COUNT]; GLint frame_pos_loc, frame_texcoord_loc; GLint frame_texture_loc, frame_transform_loc; GLuint line_program; GLint line_pos_loc, line_color_loc; GLint line_transform_loc; unsigned int rgb_width, rgb_height, rgb_stride; size_t rgb_buf_len; mat4 rgb_transform; unsigned int rgb_front_width, rgb_front_height, rgb_front_stride; size_t rgb_front_buf_len; UIScene scene; bool awake; int awake_timeout; int volume_timeout; int speed_lim_off_timeout; int is_metric_timeout; int limit_set_speed_timeout; int status; bool is_metric; bool limit_set_speed; float speed_lim_off; bool is_ego_over_limit; bool passive; char alert_type[64]; char alert_sound[64]; int alert_size; float alert_blinking_alpha; bool alert_blinked; float light_sensor; int touch_fd; // Hints for re-calculations and redrawing bool model_changed; bool livempc_or_live20_changed; GLuint frame_vao[2], frame_vbo[2], frame_ibo[2]; mat4 rear_frame_mat, front_frame_mat; model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; track_vertices_data track_vertices[2]; } UIState; static int last_brightness = -1; static void set_brightness(UIState *s, int brightness) { if (last_brightness != brightness && (s->awake || brightness == 0)) { FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); if (f != NULL) { fprintf(f, "%d", brightness); fclose(f); last_brightness = brightness; } } } static void set_awake(UIState *s, bool awake) { if (awake) { // 30 second timeout at 30 fps s->awake_timeout = 30*30; } if (s->awake != awake) { s->awake = awake; if (awake) { LOG("awake normal"); framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); } else { LOG("awake off"); set_brightness(s, 0); framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); } } } static void set_volume(UIState *s, int volume) { char volume_change_cmd[64]; sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1", volume); // 5 second timeout at 60fps s->volume_timeout = 5 * UI_FREQ; int volume_changed = system(volume_change_cmd); } volatile int do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; } static void read_speed_lim_off(UIState *s) { char *speed_lim_off = NULL; read_db_value(NULL, "SpeedLimitOffset", &speed_lim_off, NULL); s->speed_lim_off = 0.; if (speed_lim_off) { s->speed_lim_off = strtod(speed_lim_off, NULL); free(speed_lim_off); } s->speed_lim_off_timeout = 2 * UI_FREQ; // 0.5Hz } static void read_is_metric(UIState *s) { char *is_metric; const int result = read_db_value(NULL, "IsMetric", &is_metric, NULL); if (result == 0) { s->is_metric = is_metric[0] == '1'; free(is_metric); } s->is_metric_timeout = 2 * UI_FREQ; // 0.5Hz } static void read_limit_set_speed(UIState *s) { char *limit_set_speed; const int result = read_db_value(NULL, "LimitSetSpeed", &limit_set_speed, NULL); if (result == 0) { s->limit_set_speed = limit_set_speed[0] == '1'; free(limit_set_speed); } s->limit_set_speed_timeout = 2 * UI_FREQ; // 0.2Hz } static const char frame_vertex_shader[] = "attribute vec4 aPosition;\n" "attribute vec4 aTexCoord;\n" "uniform mat4 uTransform;\n" "varying vec4 vTexCoord;\n" "void main() {\n" " gl_Position = uTransform * aPosition;\n" " vTexCoord = aTexCoord;\n" "}\n"; static const char frame_fragment_shader[] = "precision mediump float;\n" "uniform sampler2D uTexture;\n" "varying vec4 vTexCoord;\n" "void main() {\n" " gl_FragColor = texture2D(uTexture, vTexCoord.xy);\n" "}\n"; static const char line_vertex_shader[] = "attribute vec4 aPosition;\n" "attribute vec4 aColor;\n" "uniform mat4 uTransform;\n" "varying vec4 vColor;\n" "void main() {\n" " gl_Position = uTransform * aPosition;\n" " vColor = aColor;\n" "}\n"; static const char line_fragment_shader[] = "precision mediump float;\n" "uniform sampler2D uTexture;\n" "varying vec4 vColor;\n" "void main() {\n" " gl_FragColor = vColor;\n" "}\n"; static const mat4 device_transform = {{ 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; // frame from 4/3 to box size with a 2x zoom static const mat4 frame_transform = {{ 2*(4./3.)/((float)viz_w/box_h), 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; // frame from 4/3 to 16/9 display static const mat4 full_to_wide_frame_transform = {{ .75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; typedef struct { const char* name; const char* uri; bool loop; } sound_file; sound_file sound_table[] = { { "chimeDisengage", "../assets/sounds/disengaged.wav", false }, { "chimeEngage", "../assets/sounds/engaged.wav", false }, { "chimeWarning1", "../assets/sounds/warning_1.wav", false }, { "chimeWarning2", "../assets/sounds/warning_2.wav", false }, { "chimeWarningRepeat", "../assets/sounds/warning_2.wav", true }, { "chimeError", "../assets/sounds/error.wav", false }, { "chimePrompt", "../assets/sounds/error.wav", false }, { NULL, NULL, false }, }; sound_file* get_sound_file_by_name(const char* name) { for (sound_file *s = sound_table; s->name != NULL; s++) { if (strcmp(s->name, name) == 0) { return s; } } return NULL; } void ui_sound_init(char **error) { slplay_setup(error); if (*error) return; for (sound_file *s = sound_table; s->name != NULL; s++) { slplay_create_player_for_uri(s->uri, error); if (*error) return; } } static void ui_init(UIState *s) { memset(s, 0, sizeof(UIState)); pthread_mutex_init(&s->lock, NULL); pthread_cond_init(&s->bg_cond, NULL); // init connections s->thermal_sock = zsock_new_sub(">tcp://127.0.0.1:8005", ""); assert(s->thermal_sock); s->thermal_sock_raw = zsock_resolve(s->thermal_sock); s->model_sock = zsock_new_sub(">tcp://127.0.0.1:8009", ""); assert(s->model_sock); s->model_sock_raw = zsock_resolve(s->model_sock); s->live100_sock = zsock_new_sub(">tcp://127.0.0.1:8007", ""); assert(s->live100_sock); s->live100_sock_raw = zsock_resolve(s->live100_sock); s->uilayout_sock = zsock_new_sub(">tcp://127.0.0.1:8060", ""); assert(s->uilayout_sock); s->uilayout_sock_raw = zsock_resolve(s->uilayout_sock); s->livecalibration_sock = zsock_new_sub(">tcp://127.0.0.1:8019", ""); assert(s->livecalibration_sock); s->livecalibration_sock_raw = zsock_resolve(s->livecalibration_sock); s->live20_sock = zsock_new_sub(">tcp://127.0.0.1:8012", ""); assert(s->live20_sock); s->live20_sock_raw = zsock_resolve(s->live20_sock); s->livempc_sock = zsock_new_sub(">tcp://127.0.0.1:8035", ""); assert(s->livempc_sock); s->livempc_sock_raw = zsock_resolve(s->livempc_sock); s->plus_sock = zsock_new_sub(">tcp://127.0.0.1:8037", ""); assert(s->plus_sock); s->plus_sock_raw = zsock_resolve(s->plus_sock); s->map_data_sock = zsock_new_sub(">tcp://127.0.0.1:8065", ""); assert(s->map_data_sock); s->map_data_sock_raw = zsock_resolve(s->map_data_sock); s->ipc_fd = -1; // init display s->fb = framebuffer_init("ui", 0x00010000, true, &s->display, &s->surface, &s->fb_w, &s->fb_h); assert(s->fb); set_awake(s, true); // init drawing s->vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(s->vg); s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/courbd.ttf"); assert(s->font_courbd >= 0); s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/OpenSans-Regular.ttf"); assert(s->font_sans_regular >= 0); s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf"); assert(s->font_sans_semibold >= 0); s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/OpenSans-Bold.ttf"); assert(s->font_sans_bold >= 0); assert(s->img_wheel >= 0); s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); assert(s->img_turn >= 0); s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); assert(s->img_face >= 0); s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); assert(s->img_map >= 0); s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); assert(s->frame_program); s->frame_pos_loc = glGetAttribLocation(s->frame_program, "aPosition"); s->frame_texcoord_loc = glGetAttribLocation(s->frame_program, "aTexCoord"); s->frame_texture_loc = glGetUniformLocation(s->frame_program, "uTexture"); s->frame_transform_loc = glGetUniformLocation(s->frame_program, "uTransform"); s->line_program = load_program(line_vertex_shader, line_fragment_shader); assert(s->line_program); s->line_pos_loc = glGetAttribLocation(s->line_program, "aPosition"); s->line_color_loc = glGetAttribLocation(s->line_program, "aColor"); s->line_transform_loc = glGetUniformLocation(s->line_program, "uTransform"); glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_DEPTH_TEST); assert(glGetError() == GL_NO_ERROR); { char *value; const int result = read_db_value(NULL, "Passive", &value, NULL); if (result == 0) { s->passive = value[0] == '1'; free(value); } } for(int i = 0; i < 2; i++) { float x1, x2, y1, y2; if (i == 1) { // flip horizontally so it looks like a mirror x1 = 0.0; x2 = 1.0; y1 = 1.0; y2 = 0.0; } else { x1 = 1.0; x2 = 0.0; y1 = 1.0; y2 = 0.0; } const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; const float frame_coords[4][4] = { {-1.0, -1.0, x2, y1}, //bl {-1.0, 1.0, x2, y2}, //tl { 1.0, 1.0, x1, y2}, //tr { 1.0, -1.0, x1, y1}, //br }; glGenVertexArrays(1,&s->frame_vao[i]); glBindVertexArray(s->frame_vao[i]); glGenBuffers(1, &s->frame_vbo[i]); glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo[i]); glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); glEnableVertexAttribArray(s->frame_pos_loc); glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(frame_coords[0]), (const void *)0); glEnableVertexAttribArray(s->frame_texcoord_loc); glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); glGenBuffers(1, &s->frame_ibo[i]); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo[i]); glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER,0); glBindVertexArray(0); } s->model_changed = false; s->livempc_or_live20_changed = false; s->front_frame_mat = matmul(device_transform, full_to_wide_frame_transform); s->rear_frame_mat = matmul(device_transform, frame_transform); for(int i = 0;i < UI_BUF_COUNT; i++) { s->khr[i] = NULL; s->priv_hnds[i] = NULL; s->khr_front[i] = NULL; s->priv_hnds_front[i] = NULL; } } static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, int num_back_fds, const int *back_fds, const VisionStreamBufs front_bufs, int num_front_fds, const int *front_fds) { const VisionUIInfo ui_info = back_bufs.buf_info.ui_info; assert(num_back_fds == UI_BUF_COUNT); assert(num_front_fds == UI_BUF_COUNT); vipc_bufs_load(s->bufs, &back_bufs, num_back_fds, back_fds); vipc_bufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds); s->cur_vision_idx = -1; s->cur_vision_front_idx = -1; s->scene = (UIScene){ .frontview = getenv("FRONTVIEW") != NULL, .fullview = getenv("FULLVIEW") != NULL, .transformed_width = ui_info.transformed_width, .transformed_height = ui_info.transformed_height, .front_box_x = ui_info.front_box_x, .front_box_y = ui_info.front_box_y, .front_box_width = ui_info.front_box_width, .front_box_height = ui_info.front_box_height, .world_objects_visible = false, // Invisible until we receive a calibration message. .gps_planner_active = false, }; s->rgb_width = back_bufs.width; s->rgb_height = back_bufs.height; s->rgb_stride = back_bufs.stride; s->rgb_buf_len = back_bufs.buf_len; s->rgb_front_width = front_bufs.width; s->rgb_front_height = front_bufs.height; s->rgb_front_stride = front_bufs.stride; s->rgb_front_buf_len = front_bufs.buf_len; s->rgb_transform = (mat4){{ 2.0/s->rgb_width, 0.0, 0.0, -1.0, 0.0, 2.0/s->rgb_height, 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; read_speed_lim_off(s); read_is_metric(s); read_limit_set_speed(s); s->is_metric_timeout = UI_FREQ / 2; // offset so values isn't read together with limit offset s->limit_set_speed_timeout = UI_FREQ; // offset so values isn't read together with limit offset } static void ui_draw_transformed_box(UIState *s, uint32_t color) { const UIScene *scene = &s->scene; const mat3 bbt = scene->warp_matrix; struct { vec3 pos; uint32_t color; } verts[] = { {matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color}, {matvecmul3(bbt, (vec3){{scene->transformed_width, 0.0, 1.0,}}), color}, {matvecmul3(bbt, (vec3){{scene->transformed_width, scene->transformed_height, 1.0,}}), color}, {matvecmul3(bbt, (vec3){{0.0, scene->transformed_height, 1.0,}}), color}, {matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color}, }; for (int i=0; irgb_height - verts[i].pos.v[1] / verts[i].pos.v[2]; } glUseProgram(s->line_program); mat4 out_mat = matmul(device_transform, matmul(frame_transform, s->rgb_transform)); glUniformMatrix4fv(s->line_transform_loc, 1, GL_TRUE, out_mat.v); glEnableVertexAttribArray(s->line_pos_loc); glVertexAttribPointer(s->line_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(verts[0]), &verts[0].pos.v[0]); glEnableVertexAttribArray(s->line_color_loc); glVertexAttribPointer(s->line_color_loc, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(verts[0]), &verts[0].color); assert(glGetError() == GL_NO_ERROR); glDrawArrays(GL_LINE_STRIP, 0, ARRAYSIZE(verts)); } // Projects a point in car to space to the corresponding point in full frame // image space. vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { const UIScene *scene = &s->scene; // We'll call the car space point p. // First project into normalized image coordinates with the extrinsics matrix. const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective); // The last entry is zero because of how we store E (to use matvecmul). const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); // Project. const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}}; return p_image; } // Calculate an interpolation between two numbers at a specific increment static float lerp(float v0, float v1, float t) { return (1 - t) * v0 + t * v1; } static void draw_chevron(UIState *s, float x_in, float y_in, float sz, NVGcolor fillColor, NVGcolor glowColor) { const UIScene *scene = &s->scene; nvgSave(s->vg); nvgTranslate(s->vg, 240.0f, 0.0); nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); nvgScale(s->vg, 2.0, 2.0); nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); sz *= 30; sz /= (x_in / 3 + 30); if (sz > 30) sz = 30; if (sz < 15) sz = 15; float x = p_full_frame.v[0]; float y = p_full_frame.v[1]; // glow nvgBeginPath(s->vg); float g_xo = sz/5; float g_yo = sz/10; if (x >= 0 && y >= 0.) { nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); nvgLineTo(s->vg, x, y-g_xo); nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo); nvgLineTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); nvgClosePath(s->vg); } nvgFillColor(s->vg, glowColor); nvgFill(s->vg); // chevron nvgBeginPath(s->vg); if (x >= 0 && y >= 0.) { nvgMoveTo(s->vg, x+(sz*1.25), y+sz); nvgLineTo(s->vg, x, y); nvgLineTo(s->vg, x-(sz*1.25), y+sz); nvgLineTo(s->vg, x+(sz*1.25), y+sz); nvgClosePath(s->vg); } nvgFillColor(s->vg, fillColor); nvgFill(s->vg); nvgRestore(s->vg); } static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { const UIScene *scene = &s->scene; nvgSave(s->vg); nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x nvgScale(s->vg, 2.0, 2.0); nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); nvgBeginPath(s->vg); bool started = false; for (int i=0; icnt; i++) { if (pvd->v[i].x < 0 || pvd->v[i].y < 0.) { continue; } if (!started) { nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); started = true; } else { nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); } } nvgClosePath(s->vg); nvgFillColor(s->vg, color); nvgFill(s->vg); nvgRestore(s->vg); } static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) { const UIScene *scene = &s->scene; const PathData path = scene->model.path; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; bool started = false; float off = is_mpc?0.3:0.5; float lead_d = scene->lead_d_rel*2.; float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20. :(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.; pvd->cnt = 0; // left side up for (int i=0; i<=path_height; i++) { float px, py, mpx; if (is_mpc) { mpx = i==0?0.0:mpc_x_coords[i]; px = lerp(mpx+1.0, mpx, i/100.0); py = mpc_y_coords[i] - off; } else { px = lerp(i+1.0, i, i/100.0); py = path.points[i] - off; } vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { continue; } pvd->v[pvd->cnt].x = p_full_frame.v[0]; pvd->v[pvd->cnt].y = p_full_frame.v[1]; pvd->cnt += 1; } // right side down for (int i=path_height; i>=0; i--) { float px, py, mpx; if (is_mpc) { mpx = i==0?0.0:mpc_x_coords[i]; px = lerp(mpx+1.0, mpx, i/100.0); py = mpc_y_coords[i] + off; } else { px = lerp(i+1.0, i, i/100.0); py = path.points[i] + off; } vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); pvd->v[pvd->cnt].x = p_full_frame.v[0]; pvd->v[pvd->cnt].y = p_full_frame.v[1]; pvd->cnt += 1; } } static void update_all_track_data(UIState *s) { const UIScene *scene = &s->scene; // Draw vision path update_track_data(s, false, &s->track_vertices[0]); if (scene->engaged) { // Draw MPC path when engaged update_track_data(s, true, &s->track_vertices[1]); } } static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { const UIScene *scene = &s->scene; const PathData path = scene->model.path; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; nvgSave(s->vg); nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x nvgScale(s->vg, 2.0, 2.0); nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); nvgBeginPath(s->vg); bool started = false; float off = is_mpc?0.3:0.5; float lead_d = scene->lead_d_rel*2.; float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20. :(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.; int vi = 0; for(int i = 0;i < pvd->cnt;i++) { if (pvd->v[i].x < 0 || pvd->v[i].y < 0) { continue; } if (!started) { nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); started = true; } else { nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); } } nvgClosePath(s->vg); NVGpaint track_bg; if (is_mpc) { // Draw colored MPC track const uint8_t *clr = bg_colors[s->status]; track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4, nvgRGBA(clr[0], clr[1], clr[2], 255), nvgRGBA(clr[0], clr[1], clr[2], 255/2)); } else { // Draw white vision track track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4, nvgRGBA(255, 255, 255, 255), nvgRGBA(255, 255, 255, 0)); } nvgFillPaint(s->vg, track_bg); nvgFill(s->vg); nvgRestore(s->vg); } static void draw_steering(UIState *s, float curvature) { float points[50]; for (int i = 0; i < 50; i++) { float y_actual = i * tan(asin(clamp(i * curvature, -0.999, 0.999)) / 2.); points[i] = y_actual; } // ui_draw_lane_edge(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5); } static void draw_frame(UIState *s) { const UIScene *scene = &s->scene; float x1, x2, y1, y2; if (s->scene.frontview) { glBindVertexArray(s->frame_vao[1]); } else { glBindVertexArray(s->frame_vao[0]); } mat4 *out_mat; if (s->scene.frontview || s->scene.fullview) { out_mat = &s->front_frame_mat; } else { out_mat = &s->rear_frame_mat; } glActiveTexture(GL_TEXTURE0); if (s->scene.frontview && s->cur_vision_front_idx >= 0) { glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); } else if (!scene->frontview && s->cur_vision_idx >= 0) { glBindTexture(GL_TEXTURE_2D, s->frame_texs[s->cur_vision_idx]); } glUseProgram(s->frame_program); glUniform1i(s->frame_texture_loc, 0); glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat->v); assert(glGetError() == GL_NO_ERROR); glEnableVertexAttribArray(0); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void*)0); glDisableVertexAttribArray(0); glBindVertexArray(0); } static inline bool valid_frame_pt(UIState *s, float x, float y) { return x >= 0 && x <= s->rgb_width && y >= 0 && y <= s->rgb_height; } static void update_lane_line_data(UIState *s, const float *points, float off, bool is_ghost, model_path_vertices_data *pvd) { pvd->cnt = 0; for (int i = 0; i < MODEL_PATH_MAX_VERTICES_CNT / 2; i++) { float px = (float)i; float py = points[i] - off; const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) continue; pvd->v[pvd->cnt].x = p_full_frame.v[0]; pvd->v[pvd->cnt].y = p_full_frame.v[1]; pvd->cnt += 1; } for (int i = MODEL_PATH_MAX_VERTICES_CNT / 2; i > 0; i--) { float px = (float)i; float py = is_ghost?(points[i]-off):(points[i]+off); const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) continue; pvd->v[pvd->cnt].x = p_full_frame.v[0]; pvd->v[pvd->cnt].y = p_full_frame.v[1]; pvd->cnt += 1; } } static void update_all_lane_lines_data(UIState *s, const PathData path, model_path_vertices_data *pstart) { update_lane_line_data(s, path.points, 0.025*path.prob, false, pstart); float var = min(path.std, 0.7); update_lane_line_data(s, path.points, -var, true, pstart + 1); update_lane_line_data(s, path.points, var, true, pstart + 2); } static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_data *pstart, NVGcolor color) { ui_draw_lane_line(s, pstart, color); float var = min(path->std, 0.7); color.a /= 4; ui_draw_lane_line(s, pstart + 1, color); ui_draw_lane_line(s, pstart + 2, color); } static void ui_draw_vision_lanes(UIState *s) { const UIScene *scene = &s->scene; model_path_vertices_data *pvd = &s->model_path_vertices[0]; if(s->model_changed) { update_all_lane_lines_data(s, scene->model.left_lane, pvd); update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT); s->model_changed = false; } // Draw left lane edge ui_draw_lane( s, &scene->model.left_lane, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); // Draw right lane edge ui_draw_lane( s, &scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); if(s->livempc_or_live20_changed) { update_all_track_data(s); s->livempc_or_live20_changed = false; } // Draw vision path ui_draw_track(s, false, &s->track_vertices[0]); if (scene->engaged) { // Draw MPC path when engaged ui_draw_track(s, true, &s->track_vertices[1]); } } // Draw all world space objects. static void ui_draw_world(UIState *s) { const UIScene *scene = &s->scene; if (!scene->world_objects_visible) { return; } if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { // Draw lane edges and vision/mpc tracks ui_draw_vision_lanes(s); } if (scene->lead_status) { // Draw lead car indicator float fillAlpha = 0; float speedBuff = 10.; float leadBuff = 40.; if (scene->lead_d_rel < leadBuff) { fillAlpha = 255*(1.0-(scene->lead_d_rel/leadBuff)); if (scene->lead_v_rel < 0) { fillAlpha += 255*(-1*(scene->lead_v_rel/speedBuff)); } fillAlpha = (int)(min(fillAlpha, 255)); } draw_chevron(s, scene->lead_d_rel+2.7, scene->lead_y_rel, 25, nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); } } static void ui_draw_vision_maxspeed(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; char maxspeed_str[32]; float maxspeed = s->scene.v_cruise; int maxspeed_calc = maxspeed * 0.6225 + 0.5; float speedlimit = s->scene.speedlimit; int speedlim_calc = speedlimit * 2.2369363 + 0.5; int speed_lim_off = s->speed_lim_off * 2.2369363 + 0.5; if (s->is_metric) { maxspeed_calc = maxspeed + 0.5; speedlim_calc = speedlimit * 3.6 + 0.5; speed_lim_off = s->speed_lim_off * 3.6 + 0.5; } 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.engaged && is_cruise_set && maxspeed_calc > (speedlim_calc + speed_lim_off); int viz_maxspeed_w = 184; int viz_maxspeed_h = 202; int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2)); int viz_maxspeed_y = (box_y + (bdr_s*1.5)); int viz_maxspeed_xo = 180; viz_maxspeed_w += viz_maxspeed_xo; viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2); // Draw Background nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 30); if (is_set_over_limit) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); } else { nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100)); } nvgFill(s->vg); // Draw Border nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20); if (is_set_over_limit) { nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); } else if (is_speedlim_valid && !s->is_ego_over_limit) { nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); } else if (is_speedlim_valid && s->is_ego_over_limit) { nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 20)); } else { nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 100)); } nvgStrokeWidth(s->vg, 10); nvgStroke(s->vg); // Draw "MAX" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgFontFace(s->vg, "sans-regular"); nvgFontSize(s->vg, 26*2.5); if (is_cruise_set) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); } else { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); } nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "MAX", NULL); // Draw Speed Text nvgFontFace(s->vg, "sans-bold"); nvgFontSize(s->vg, 48*2.5); if (is_cruise_set) { snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL); } else { nvgFontFace(s->vg, "sans-semibold"); nvgFontSize(s->vg, 42*2.5); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL); } #ifdef DEBUG_TURN if (s->scene.decel_for_turn && s->scene.engaged){ int v_curvature = s->scene.v_curvature * 2.2369363 + 0.5; snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", v_curvature); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgFontSize(s->vg, 25*2.5); nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "TURN", NULL); nvgFontSize(s->vg, 50*2.5); nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL); } #endif } static void ui_draw_vision_speedlimit(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; 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.v_ego > (speedlimit + s->speed_lim_off + hysteresis_offset); int viz_speedlim_w = 180; int viz_speedlim_h = 202; int viz_speedlim_x = (ui_viz_rx + (bdr_s*2)); int viz_speedlim_y = (box_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; } int viz_speedlim_bdr = is_speedlim_valid ? 30 : 15; // Draw Background nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, viz_speedlim_bdr); if (is_speedlim_valid && s->is_ego_over_limit) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); } else if (is_speedlim_valid) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); } else { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); } nvgFill(s->vg); // Draw Border if (is_speedlim_valid) { nvgStrokeWidth(s->vg, 10); nvgStroke(s->vg); nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, 20); if (s->is_ego_over_limit) { nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); } else if (is_speedlim_valid) { nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); } } // Draw "Speed Limit" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgFontFace(s->vg, "sans-semibold"); nvgFontSize(s->vg, 50); nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); if (is_speedlim_valid && s->is_ego_over_limit) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); } nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SPEED", NULL); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "LIMIT", NULL); // Draw Speed Text nvgFontFace(s->vg, "sans-bold"); nvgFontSize(s->vg, 48*2.5); if (s->is_ego_over_limit) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); } else { nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); } if (is_speedlim_valid) { snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, NULL); } else { nvgFontFace(s->vg, "sans-semibold"); nvgFontSize(s->vg, 42*2.5); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", NULL); } } static void ui_draw_vision_speed(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; float speed = s->scene.v_ego; const int viz_speed_w = 280; const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2)); char speed_str[32]; nvgBeginPath(s->vg); nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); if (s->is_metric) { snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); } else { snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5)); } nvgFontFace(s->vg, "sans-bold"); nvgFontSize(s->vg, 96*2.5); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); nvgFontFace(s->vg, "sans-regular"); nvgFontSize(s->vg, 36*2.5); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); if (s->is_metric) { nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL); } else { nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL); } } static void ui_draw_vision_event(UIState *s) { const UIScene *scene = &s->scene; const int ui_viz_rx = scene->ui_viz_rx; const int ui_viz_rw = scene->ui_viz_rw; const int viz_event_w = 220; const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2))); const int viz_event_y = (box_y + (bdr_s*1.5)); const int viz_event_h = (header_h - (bdr_s*1.5)); if (s->scene.decel_for_turn && s->scene.engaged && s->limit_set_speed) { // 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_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_turn, 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 { // draw steering wheel const int bg_wheel_size = 96; const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); const int bg_wheel_y = viz_event_y + (bg_wheel_size/2); 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; float img_wheel_alpha = 0.1f; bool is_engaged = (s->status == STATUS_ENGAGED); bool is_warning = (s->status == STATUS_WARNING); bool is_engageable = scene->engageable; if (is_engaged || is_warning || is_engageable) { nvgBeginPath(s->vg); nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size); if (is_engaged) { nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); } else if (is_warning) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); } else if (is_engageable) { nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255)); } 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); nvgFill(s->vg); } } static void ui_draw_vision_map(UIState *s) { const UIScene *scene = &s->scene; const int map_size = 96; const int map_x = (scene->ui_viz_rx + (map_size * 3) + (bdr_s * 3)); const int map_y = (footer_y + ((footer_h - map_size) / 2)); const int map_img_size = (map_size * 1.5); const int map_img_x = (map_x - (map_img_size / 2)); const int map_img_y = (map_y - (map_size / 4)); bool map_valid = s->scene.map_valid; float map_img_alpha = map_valid ? 1.0f : 0.15f; float map_bg_alpha = map_valid ? 0.3f : 0.1f; NVGcolor map_bg = nvgRGBA(0, 0, 0, (255 * map_bg_alpha)); NVGpaint map_img = nvgImagePattern(s->vg, map_img_x, map_img_y, map_img_size, map_img_size, 0, s->img_map, map_img_alpha); nvgBeginPath(s->vg); nvgCircle(s->vg, map_x, (map_y + (bdr_s * 1.5)), map_size); nvgFillColor(s->vg, map_bg); nvgFill(s->vg); nvgBeginPath(s->vg); nvgRect(s->vg, map_img_x, map_img_y, map_img_size, map_img_size); nvgFillPaint(s->vg, map_img); nvgFill(s->vg); } static void ui_draw_vision_face(UIState *s) { const UIScene *scene = &s->scene; const int face_size = 96; const int face_x = (scene->ui_viz_rx + face_size + (bdr_s * 2)); const int face_y = (footer_y + ((footer_h - face_size) / 2)); const int face_img_size = (face_size * 1.5); const int face_img_x = (face_x - (face_img_size / 2)); const int face_img_y = (face_y - (face_size / 4)); float face_img_alpha = scene->monitoring_active ? 1.0f : 0.15f; float face_bg_alpha = scene->monitoring_active ? 0.3f : 0.1f; NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha)); NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, 0, s->img_face, face_img_alpha); nvgBeginPath(s->vg); nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size); nvgFillColor(s->vg, face_bg); nvgFill(s->vg); nvgBeginPath(s->vg); nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size); nvgFillPaint(s->vg, face_img); nvgFill(s->vg); } static void ui_draw_vision_header(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; nvgBeginPath(s->vg); NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx, (box_y+(header_h-(header_h/2.5))), ui_viz_rx, box_y+header_h, nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0)); nvgFillPaint(s->vg, gradient); nvgRect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h); nvgFill(s->vg); ui_draw_vision_maxspeed(s); ui_draw_vision_speedlimit(s); ui_draw_vision_speed(s); ui_draw_vision_event(s); } static void ui_draw_vision_footer(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; nvgBeginPath(s->vg); nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h); ui_draw_vision_face(s); ui_draw_vision_map(s); } static void ui_draw_vision_alert(UIState *s, int va_size, int va_color, const char* va_text1, const char* va_text2) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; bool mapEnabled = s->scene.uilayout_mapenabled; bool longAlert1 = strlen(va_text1) > 15; const uint8_t *color = alert_colors[va_color]; const int alr_s = alert_sizes[va_size]; const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)-bdr_s; const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)+(bdr_s*2); const int alr_h = alr_s+(va_size==ALERTSIZE_NONE?0:bdr_s); const int alr_y = vwp_h-alr_h; nvgBeginPath(s->vg); nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); nvgFill(s->vg); nvgBeginPath(s->vg); NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); nvgFillPaint(s->vg, gradient); nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); nvgFill(s->vg); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); if (va_size == ALERTSIZE_SMALL) { nvgFontFace(s->vg, "sans-semibold"); nvgFontSize(s->vg, 40*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, NULL); } else if (va_size== ALERTSIZE_MID) { nvgFontFace(s->vg, "sans-bold"); nvgFontSize(s->vg, 48*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, NULL); nvgFontFace(s->vg, "sans-regular"); nvgFontSize(s->vg, 36*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, NULL); } else if (va_size== ALERTSIZE_FULL) { nvgFontSize(s->vg, (longAlert1?72:96)*2.5); nvgFontFace(s->vg, "sans-bold"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, va_text1, NULL); nvgFontSize(s->vg, 48*2.5); nvgFontFace(s->vg, "sans-regular"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL); } } static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; int ui_viz_ro = scene->ui_viz_ro; glClearColor(0.0, 0.0, 0.0, 0.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); // Draw video frames glEnable(GL_SCISSOR_TEST); glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h); glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); draw_frame(s); glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_SCISSOR_TEST); glClear(GL_STENCIL_BUFFER_BIT); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgSave(s->vg); // Draw augmented elements const int inner_height = viz_w*9/16; nvgScissor(s->vg, ui_viz_rx, box_y, ui_viz_rw, box_h); nvgTranslate(s->vg, ui_viz_rx+ui_viz_ro, box_y + (box_h-inner_height)/2.0); nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h); if (!scene->frontview && !scene->fullview) { ui_draw_world(s); } nvgRestore(s->vg); // Set Speed, Current Speed, Status/Events ui_draw_vision_header(s); if (s->scene.alert_size != ALERTSIZE_NONE) { // Controls Alerts ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2); } else { ui_draw_vision_footer(s); } nvgEndFrame(s->vg); glDisable(GL_BLEND); } static void ui_draw_blank(UIState *s) { glClearColor(0.0, 0.0, 0.0, 0.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } static void ui_draw(UIState *s) { if (s->vision_connected && s->plus_state == 0) { ui_draw_vision(s); } else { ui_draw_blank(s); } { glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glClear(GL_STENCIL_BUFFER_BIT); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgEndFrame(s->vg); glDisable(GL_BLEND); } } static PathData read_path(cereal_ModelData_PathData_ptr pathp) { PathData ret = {0}; struct cereal_ModelData_PathData pathd; cereal_read_ModelData_PathData(&pathd, pathp); ret.prob = pathd.prob; ret.std = pathd.std; capn_list32 pointl = pathd.points; capn_resolve(&pointl.p); for (int i = 0; i < 50; i++) { ret.points[i] = capn_to_f32(capn_get32(pointl, i)); } return ret; } static ModelData read_model(cereal_ModelData_ptr modelp) { struct cereal_ModelData modeld; cereal_read_ModelData(&modeld, modelp); ModelData d = {0}; d.path = read_path(modeld.path); d.left_lane = read_path(modeld.leftLane); d.right_lane = read_path(modeld.rightLane); struct cereal_ModelData_LeadData leadd; cereal_read_ModelData_LeadData(&leadd, modeld.lead); d.lead = (LeadData){ .dist = leadd.dist, .prob = leadd.prob, .std = leadd.std, }; return d; } static void update_status(UIState *s, int status) { if (s->status != status) { s->status = status; // wake up bg thread to change pthread_cond_signal(&s->bg_cond); } } static void ui_update(UIState *s) { int err; if (s->vision_connect_firstrun) { // cant run this in connector thread because opengl. // do this here for now in lieu of a run_on_main_thread event for (int i=0; ikhr[i] != NULL) { visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]); glDeleteTextures(1, &s->frame_texs[i]); } VisionImg img = { .fd = s->bufs[i].fd, .format = VISIONIMG_FORMAT_RGB24, .width = s->rgb_width, .height = s->rgb_height, .stride = s->rgb_stride, .bpp = 3, .size = s->rgb_buf_len, }; s->frame_texs[i] = visionimg_to_gl(&img, &s->khr[i], &s->priv_hnds[i]); glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); // BGR glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); } for (int i=0; ikhr_front[i] != NULL) { visionimg_destroy_gl(s->khr_front[i], s->priv_hnds_front[i]); glDeleteTextures(1, &s->frame_front_texs[i]); } VisionImg img = { .fd = s->front_bufs[i].fd, .format = VISIONIMG_FORMAT_RGB24, .width = s->rgb_front_width, .height = s->rgb_front_height, .stride = s->rgb_front_stride, .bpp = 3, .size = s->rgb_front_buf_len, }; s->frame_front_texs[i] = visionimg_to_gl(&img, &s->khr_front[i], &s->priv_hnds_front[i]); glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[i]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); // BGR glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); } assert(glGetError() == GL_NO_ERROR); // Default UI Measurements (Assumes sidebar collapsed) s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_ro = 0; s->vision_connect_firstrun = false; s->alert_blinking_alpha = 1.0; s->alert_blinked = false; } zmq_pollitem_t polls[9] = {{0}}; // Wait for next rgb image from visiond while(true) { assert(s->ipc_fd >= 0); polls[0].fd = s->ipc_fd; polls[0].events = ZMQ_POLLIN; int ret = zmq_poll(polls, 1, 1000); if (ret < 0) { LOGW("poll failed (%d)", ret); close(s->ipc_fd); s->ipc_fd = -1; s->vision_connected = false; return; } else if (ret == 0) continue; // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); if (err <= 0) { LOGW("vision disconnected"); close(s->ipc_fd); s->ipc_fd = -1; s->vision_connected = false; return; } if (rp.type == VIPC_STREAM_ACQUIRE) { bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT; int idx = rp.d.stream_acq.idx; int release_idx; if (front) { release_idx = s->cur_vision_front_idx; } else { release_idx = s->cur_vision_idx; } if (release_idx >= 0) { VisionPacket rep = { .type = VIPC_STREAM_RELEASE, .d = { .stream_rel = { .type = rp.d.stream_acq.type, .idx = release_idx, }}, }; vipc_send(s->ipc_fd, &rep); } if (front) { assert(idx < UI_BUF_COUNT); s->cur_vision_front_idx = idx; } else { assert(idx < UI_BUF_COUNT); s->cur_vision_idx = idx; // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]); } } else { assert(false); } break; } // peek and consume all events in the zmq queue, then return. while(true) { polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; polls[1].events = ZMQ_POLLIN; polls[2].socket = s->model_sock_raw; polls[2].events = ZMQ_POLLIN; polls[3].socket = s->live20_sock_raw; polls[3].events = ZMQ_POLLIN; polls[4].socket = s->livempc_sock_raw; polls[4].events = ZMQ_POLLIN; polls[5].socket = s->thermal_sock_raw; polls[5].events = ZMQ_POLLIN; polls[6].socket = s->uilayout_sock_raw; polls[6].events = ZMQ_POLLIN; polls[7].socket = s->map_data_sock_raw; polls[7].events = ZMQ_POLLIN; polls[8].socket = s->plus_sock_raw; // plus_sock should be last polls[8].events = ZMQ_POLLIN; int num_polls = 9; int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); return; } if (ret == 0) { return; } if (polls[0].revents || polls[1].revents || polls[2].revents || polls[3].revents || polls[4].revents || polls[6].revents || polls[7].revents || polls[8].revents) { // awake on any (old) activity set_awake(s, true); } if (polls[8].revents) { // plus socket zmq_msg_t msg; err = zmq_msg_init(&msg); assert(err == 0); err = zmq_msg_recv(&msg, s->plus_sock_raw, 0); assert(err >= 0); assert(zmq_msg_size(&msg) == 1); s->plus_state = ((char*)zmq_msg_data(&msg))[0]; zmq_msg_close(&msg); } else { // zmq messages void* which = NULL; for (int i=0; i= 0); struct capn ctx; capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); cereal_Event_ptr eventp; eventp.p = capn_getp(capn_root(&ctx), 0, 1); struct cereal_Event eventd; cereal_read_Event(&eventd, eventp); double t = millis_since_boot(); if (eventd.which == cereal_Event_live100) { struct cereal_Live100Data datad; cereal_read_Live100Data(&datad, eventd.live100); if (datad.vCruise != s->scene.v_cruise) { s->scene.v_cruise_update_ts = eventd.logMonoTime; } s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; s->scene.engageable = datad.engageable; s->scene.gps_planner_active = datad.gpsPlannerActive; s->scene.monitoring_active = datad.driverMonitoringOn; s->scene.frontview = datad.rearViewCam; s->scene.v_curvature = datad.vCurvature; s->scene.decel_for_turn = datad.decelForTurn; if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) { char* error = NULL; if (s->alert_sound[0] != '\0') { sound_file* active_sound = get_sound_file_by_name(s->alert_sound); slplay_stop_uri(active_sound->uri, &error); if (error) { LOGW("error stopping active sound %s", error); } } sound_file* sound = get_sound_file_by_name(datad.alertSound.str); slplay_play(sound->uri, sound->loop, &error); if(error) { LOGW("error playing sound: %s", error); } snprintf(s->alert_sound, sizeof(s->alert_sound), "%s", datad.alertSound.str); snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str); } else if ((!datad.alertSound.str || datad.alertSound.str[0] == '\0') && s->alert_sound[0] != '\0') { sound_file* sound = get_sound_file_by_name(s->alert_sound); char* error = NULL; slplay_stop_uri(sound->uri, &error); if(error) { LOGW("error stopping sound: %s", error); } s->alert_type[0] = '\0'; s->alert_sound[0] = '\0'; } if (datad.alertText1.str) { snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", datad.alertText1.str); } else { s->scene.alert_text1[0] = '\0'; } if (datad.alertText2.str) { snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", datad.alertText2.str); } else { s->scene.alert_text2[0] = '\0'; } s->scene.awareness_status = datad.awarenessStatus; s->scene.alert_ts = eventd.logMonoTime; s->scene.alert_size = datad.alertSize; if (datad.alertSize == cereal_Live100Data_AlertSize_none) { s->alert_size = ALERTSIZE_NONE; } else if (datad.alertSize == cereal_Live100Data_AlertSize_small) { s->alert_size = ALERTSIZE_SMALL; } else if (datad.alertSize == cereal_Live100Data_AlertSize_mid) { s->alert_size = ALERTSIZE_MID; } else if (datad.alertSize == cereal_Live100Data_AlertSize_full) { s->alert_size = ALERTSIZE_FULL; } if (datad.alertStatus == cereal_Live100Data_AlertStatus_userPrompt) { update_status(s, STATUS_WARNING); } else if (datad.alertStatus == cereal_Live100Data_AlertStatus_critical) { update_status(s, STATUS_ALERT); } else if (datad.enabled) { update_status(s, STATUS_ENGAGED); } else { update_status(s, STATUS_DISENGAGED); } s->scene.alert_blinkingrate = datad.alertBlinkingRate; if (datad.alertBlinkingRate > 0.) { if (s->alert_blinked) { if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) { s->alert_blinking_alpha += (0.05*datad.alertBlinkingRate); } else { s->alert_blinked = false; } } else { if (s->alert_blinking_alpha > 0.25) { s->alert_blinking_alpha -= (0.05*datad.alertBlinkingRate); } else { s->alert_blinking_alpha += 0.25; s->alert_blinked = true; } } } } else if (eventd.which == cereal_Event_live20) { struct cereal_Live20Data datad; cereal_read_Live20Data(&datad, eventd.live20); struct cereal_Live20Data_LeadData leaddatad; cereal_read_Live20Data_LeadData(&leaddatad, datad.leadOne); s->scene.lead_status = leaddatad.status; s->scene.lead_d_rel = leaddatad.dRel; s->scene.lead_y_rel = leaddatad.yRel; s->scene.lead_v_rel = leaddatad.vRel; s->livempc_or_live20_changed = true; } else if (eventd.which == cereal_Event_liveCalibration) { s->scene.world_objects_visible = true; struct cereal_LiveCalibrationData datad; cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration); // should we still even have this? capn_list32 warpl = datad.warpMatrix2; capn_resolve(&warpl.p); // is this a bug? for (int i = 0; i < 3 * 3; i++) { s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i)); } capn_list32 extrinsicl = datad.extrinsicMatrix; capn_resolve(&extrinsicl.p); // is this a bug? for (int i = 0; i < 3 * 4; i++) { s->scene.extrinsic_matrix.v[i] = capn_to_f32(capn_get32(extrinsicl, i)); } } else if (eventd.which == cereal_Event_model) { s->scene.model_ts = eventd.logMonoTime; s->scene.model = read_model(eventd.model); s->model_changed = true; } else if (eventd.which == cereal_Event_liveMpc) { struct cereal_LiveMpcData datad; cereal_read_LiveMpcData(&datad, eventd.liveMpc); capn_list32 x_list = datad.x; capn_resolve(&x_list.p); for (int i = 0; i < 50; i++){ s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i)); } capn_list32 y_list = datad.y; capn_resolve(&y_list.p); for (int i = 0; i < 50; i++){ s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); } s->livempc_or_live20_changed = true; } else if (eventd.which == cereal_Event_thermal) { struct cereal_ThermalData datad; cereal_read_ThermalData(&datad, eventd.thermal); if (!datad.started) { update_status(s, STATUS_STOPPED); } else if (s->status == STATUS_STOPPED) { // car is started but controls doesn't have fingerprint yet update_status(s, STATUS_DISENGAGED); } s->scene.started_ts = datad.startedTs; } else if (eventd.which == cereal_Event_uiLayoutState) { struct cereal_UiLayoutState datad; cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; s->scene.uilayout_mapenabled = datad.mapEnabled; bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; bool mapEnabled = s->scene.uilayout_mapenabled; if (mapEnabled) { s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4)); s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4)); s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s); } else { s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0; } } else if (eventd.which == cereal_Event_liveMapData) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); s->scene.speedlimit = datad.speedLimit; s->scene.speedlimit_valid = datad.speedLimitValid; s->scene.map_valid = datad.mapValid; } capn_free(&ctx); zmq_msg_close(&msg); } } } static int vision_subscribe(int fd, VisionPacket *rp, int type) { int err; LOGW("vision_subscribe type:%d", type); VisionPacket p1 = { .type = VIPC_STREAM_SUBSCRIBE, .d = { .stream_sub = { .type = type, .tbuffer = true, }, }, }; err = vipc_send(fd, &p1); if (err < 0) { close(fd); return 0; } do { err = vipc_recv(fd, rp); if (err <= 0) { close(fd); return 0; } // release what we aren't ready for yet if (rp->type == VIPC_STREAM_ACQUIRE) { VisionPacket rep = { .type = VIPC_STREAM_RELEASE, .d = { .stream_rel = { .type = rp->d.stream_acq.type, .idx = rp->d.stream_acq.idx, }}, }; vipc_send(fd, &rep); } } while (rp->type != VIPC_STREAM_BUFS || rp->d.stream_bufs.type != type); return 1; } static void* vision_connect_thread(void *args) { int err; UIState *s = args; while (!do_exit) { usleep(100000); pthread_mutex_lock(&s->lock); bool connected = s->vision_connected; pthread_mutex_unlock(&s->lock); if (connected) continue; int fd = vipc_connect(); if (fd < 0) continue; VisionPacket back_rp, front_rp; if (!vision_subscribe(fd, &back_rp, VISION_STREAM_RGB_BACK)) continue; if (!vision_subscribe(fd, &front_rp, VISION_STREAM_RGB_FRONT)) continue; pthread_mutex_lock(&s->lock); assert(!s->vision_connected); s->ipc_fd = fd; ui_init_vision(s, back_rp.d.stream_bufs, back_rp.num_fds, back_rp.fds, front_rp.d.stream_bufs, front_rp.num_fds, front_rp.fds); s->vision_connected = true; s->vision_connect_firstrun = true; pthread_mutex_unlock(&s->lock); } return NULL; } #include #include static void* light_sensor_thread(void *args) { int err; UIState *s = args; s->light_sensor = 0.0; struct sensors_poll_device_t* device; struct sensors_module_t* module; hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); sensors_open(&module->common, &device); // need to do this struct sensor_t const* list; int count = module->get_sensors_list(module, &list); int SENSOR_LIGHT = 7; device->activate(device, SENSOR_LIGHT, 0); device->activate(device, SENSOR_LIGHT, 1); device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); while (!do_exit) { static const size_t numEvents = 1; sensors_event_t buffer[numEvents]; int n = device->poll(device, buffer, numEvents); if (n < 0) { LOG_100("light_sensor_poll failed: %d", n); } if (n > 0) { s->light_sensor = buffer[0].light; } } return NULL; } static void* bg_thread(void* args) { UIState *s = args; EGLDisplay bg_display; EGLSurface bg_surface; FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, &bg_display, &bg_surface, NULL, NULL); assert(bg_fb); int bg_status = -1; while(!do_exit) { pthread_mutex_lock(&s->lock); if (bg_status == s->status) { // will always be signaled if it changes? pthread_cond_wait(&s->bg_cond, &s->lock); } bg_status = s->status; pthread_mutex_unlock(&s->lock); assert(bg_status < ARRAYSIZE(bg_colors)); const uint8_t *color = bg_colors[bg_status]; glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0); glClear(GL_COLOR_BUFFER_BIT); eglSwapBuffers(bg_display, bg_surface); assert(glGetError() == GL_NO_ERROR); } return NULL; } int is_leon() { #define MAXCHAR 1000 FILE *fp; char str[MAXCHAR]; char* filename = "/proc/cmdline"; fp = fopen(filename, "r"); if (fp == NULL){ printf("Could not open file %s",filename); return 0; } fgets(str, MAXCHAR, fp); fclose(fp); return strstr(str, "letv") != NULL; } int main() { int err; setpriority(PRIO_PROCESS, 0, -14); zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); UIState uistate; UIState *s = &uistate; ui_init(s); pthread_t connect_thread_handle; err = pthread_create(&connect_thread_handle, NULL, vision_connect_thread, s); assert(err == 0); pthread_t light_sensor_thread_handle; err = pthread_create(&light_sensor_thread_handle, NULL, light_sensor_thread, s); assert(err == 0); pthread_t bg_thread_handle; err = pthread_create(&bg_thread_handle, NULL, bg_thread, s); assert(err == 0); TouchState touch = {0}; touch_init(&touch); s->touch_fd = touch.fd; char* error = NULL; ui_sound_init(&error); if (error) { LOGW(error); exit(1); } // light sensor scaling params const int EON = (access("/EON", F_OK) != -1); const int LEON = is_leon(); const float BRIGHTNESS_B = LEON? 10.0 : 5.0; const float BRIGHTNESS_M = LEON? 2.6 : 1.3; #define NEO_BRIGHTNESS 100 float smooth_brightness = BRIGHTNESS_B; set_volume(s, 0); #ifdef DEBUG_FPS vipc_t1 = millis_since_boot(); double t1 = millis_since_boot(); int draws = 0, old_draws = 0; #endif //DEBUG_FPS while (!do_exit) { bool should_swap = false; if (!s->vision_connected) { // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen. // Don't hold the lock while sleeping, so that vision_connect_thread have chances to get the lock. usleep(30 * 1000); } pthread_mutex_lock(&s->lock); if (EON) { // light sensor is only exposed on EONs float clipped_brightness = (s->light_sensor*BRIGHTNESS_M) + BRIGHTNESS_B; if (clipped_brightness > 255) clipped_brightness = 255; smooth_brightness = clipped_brightness * 0.01 + smooth_brightness * 0.99; set_brightness(s, (int)smooth_brightness); } else { // compromise for bright and dark envs set_brightness(s, NEO_BRIGHTNESS); } if (!s->vision_connected) { // Car is not started, keep in idle state and awake on touch events zmq_pollitem_t polls[1] = {{0}}; polls[0].fd = s->touch_fd; polls[0].events = ZMQ_POLLIN; int ret = zmq_poll(polls, 1, 0); if (ret < 0) LOGW("poll failed (%d)", ret); else if (ret > 0) { // awake on any touch int touch_x = -1, touch_y = -1; int touched = touch_read(&touch, &touch_x, &touch_y); if (touched == 1) { set_awake(s, true); } } } else { // Car started, fetch a new rgb image from ipc and peek for zmq events. ui_update(s); if(!s->vision_connected) { // Visiond process is just stopped, force a redraw to make screen blank again. ui_draw(s); glFinish(); should_swap = true; } } // manage wakefulness if (s->awake_timeout > 0) { s->awake_timeout--; } else { set_awake(s, false); } // Don't waste resources on drawing in case screen is off or car is not started. if (s->awake && s->vision_connected) { ui_draw(s); glFinish(); should_swap = true; #ifdef DEBUG_FPS draws++; double t2 = millis_since_boot(); const double interval = 30.; if(t2 - t1 >= interval * 1000.) { printf("ui draw fps: %.2f\n",((double)(draws - old_draws)) / interval) ; t1 = t2; old_draws = draws; } #endif } if (s->volume_timeout > 0) { s->volume_timeout--; } else { int volume = min(13, 11 + s->scene.v_ego / 15); // up one notch every 15 m/s, starting at 11 set_volume(s, volume); } if (s->speed_lim_off_timeout > 0) { s->speed_lim_off_timeout--; } else { read_speed_lim_off(s); } if (s->is_metric_timeout > 0) { s->is_metric_timeout--; } else { read_is_metric(s); } if (s->limit_set_speed_timeout > 0) { s->limit_set_speed_timeout--; } else { read_limit_set_speed(s); } pthread_mutex_unlock(&s->lock); // the bg thread needs to be scheduled, so the main thread needs time without the lock // safe to do this outside the lock? if (should_swap) { eglSwapBuffers(s->display, s->surface); } } set_awake(s, true); slplay_destroy(); // wake up bg thread to exit pthread_mutex_lock(&s->lock); pthread_cond_signal(&s->bg_cond); pthread_mutex_unlock(&s->lock); err = pthread_join(bg_thread_handle, NULL); assert(err == 0); err = pthread_join(connect_thread_handle, NULL); assert(err == 0); return 0; }