sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
open source driving agent
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
121 Commits
109 Branches
34 Tags
1.6 GiB
 
 
 
 
 
 
Tag: Branch: Tree: 41f868c92a
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '41f868c92a'
${ noResults }
jyoung/selfdrive/ui/ui.c

1727 lines
48 KiB
Raw Blame History

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <cutils/properties.h>
#include <GLES3/gl3.h>
#include <EGL/eglext.h>
#include <json.h>
#include <czmq.h>
#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/modeldata.h"
#include "common/params.h"
#include "cereal/gen/c/log.capnp.h"
// Calibration status values from controlsd.py
#define CALIBRATION_UNCALIBRATED 0
#define CALIBRATION_CALIBRATED 1
#define CALIBRATION_INVALID 2
#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 UI_BUF_COUNT 4
const int box_x = 330;
const int box_y = 30;
const int box_width = 1560;
const int box_height = 1020;
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, 0x80},
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0x80},
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0x80},
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0x80},
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0x80},
};
typedef struct UIScene {
int frontview;
uint8_t *bgr_ptr;
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 curvature;
int engaged;
int lead_status;
float lead_d_rel, lead_y_rel, lead_v_rel;
uint8_t *bgr_front_ptr;
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 awareness_status;
uint64_t started_ts;
// Used to display calibration progress
int cal_status;
int cal_perc;
// Used to show gps planner status
bool gps_planner_active;
} UIScene;
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;
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;
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_tex;
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;
mat4 rgb_transform;
unsigned int rgb_front_width, rgb_front_height;
GLuint frame_front_tex;
bool intrinsic_matrix_loaded;
mat3 intrinsic_matrix;
UIScene scene;
bool awake;
int awake_timeout;
int status;
bool is_metric;
bool passive;
float light_sensor;
} UIState;
static int last_brightness = -1;
static void set_brightness(int brightness) {
if (last_brightness != brightness) {
//printf("setting brightness %d\n", brightness);
// can't hurt
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");
framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF);
}
}
}
volatile int do_exit = 0;
static void set_do_exit(int sig) {
do_exit = 1;
}
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 zoon
static const mat4 frame_transform = {{
2*(4./3.)/((float)box_width/box_height), 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,
}};
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->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->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);
// 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);
}
}
}
// If the intrinsics are in the params entry, this copies them to
// intrinsic_matrix and returns true. Otherwise returns false.
static bool try_load_intrinsics(mat3 *intrinsic_matrix) {
char *value;
const int result = read_db_value(NULL, "CloudCalibration", &value, NULL);
if (result == 0) {
JsonNode* calibration_json = json_decode(value);
free(value);
JsonNode *intrinsic_json =
json_find_member(calibration_json, "intrinsic_matrix");
if (intrinsic_json == NULL || intrinsic_json->tag != JSON_ARRAY) {
json_delete(calibration_json);
return false;
}
int i = 0;
JsonNode* json_num;
json_foreach(json_num, intrinsic_json) {
intrinsic_matrix->v[i++] = json_num->number_;
}
json_delete(calibration_json);
return true;
} else {
return false;
}
}
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 = 0,
.cal_status = CALIBRATION_CALIBRATED,
.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_front_width = front_bufs.width;
s->rgb_front_height = front_bufs.height;
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,
}};
char *value;
const int result = read_db_value(NULL, "IsMetric", &value, NULL);
if (result == 0) {
s->is_metric = value[0] == '1';
free(value);
}
}
static bool ui_alert_active(UIState *s) {
return (nanos_since_boot() - s->scene.alert_ts) < 20000000000ULL &&
strlen(s->scene.alert_text1) > 0 &&
s->scene.alert_size == cereal_Live100Data_AlertSize_full;
}
static void ui_update_frame(UIState *s) {
assert(glGetError() == GL_NO_ERROR);
UIScene *scene = &s->scene;
if (scene->frontview && scene->bgr_front_ptr) {
// load front frame texture
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, s->frame_front_tex);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0,
s->rgb_front_width, s->rgb_front_height,
GL_RGB, GL_UNSIGNED_BYTE, scene->bgr_front_ptr);
} else if (!scene->frontview && scene->bgr_ptr) {
// load frame texture
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, s->frame_tex);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0,
s->rgb_width, s->rgb_height,
GL_RGB, GL_UNSIGNED_BYTE, scene->bgr_ptr);
}
assert(glGetError() == GL_NO_ERROR);
}
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; i<ARRAYSIZE(verts); i++) {
verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2];
verts[i].pos.v[1] = s->rgb_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(s->intrinsic_matrix, Ep);
// Project.
const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}};
return p_image;
}
// TODO: refactor with draw_path
static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
// path coords are worked out in rgb-box space
nvgTranslate(s->vg, 240.0f, 0.0);
// zooom in 2x
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);
nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, 5);
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);
// scale with distance
// x_in = 0 -> sz = 30 (max)
// x_in = 90 -> sz = 15 (min)
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];
if (x >= 0 && y >= 0.) {
nvgMoveTo(s->vg, x-sz, y);
nvgLineTo(s->vg, x+sz, y);
nvgMoveTo(s->vg, x, y-sz);
nvgLineTo(s->vg, x, y+sz);
nvgStroke(s->vg);
}
nvgRestore(s->vg);
}
static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, size_t num_points,
NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
// path coords are worked out in rgb-box space
nvgTranslate(s->vg, 240.0f, 0.0);
// zooom in 2x
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);
nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, 2);
bool started = false;
for (int i=0; i<num_points; i++) {
float px = x_coords[i];
float py = y_coords[i];
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
}
nvgStroke(s->vg);
nvgRestore(s->vg);
}
static void draw_path(UIState *s, const float *points, float off,
NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
// path coords are worked out in rgb-box space
nvgTranslate(s->vg, 240.0f, 0.0);
// zooom in 2x
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);
nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, 5);
bool started = false;
for (int i=0; i<50; i++) {
float px = (float)i;
float py = 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);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
}
nvgStroke(s->vg);
nvgRestore(s->vg);
}
static void draw_model_path(UIState *s, const PathData path, NVGcolor color) {
float var = min(path.std, 0.7);
draw_path(s, path.points, 0.0, color);
color.a /= 4;
draw_path(s, path.points, -var, color);
draw_path(s, path.points, var, color);
}
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;
}
draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128));
}
static void draw_frame(UIState *s) {
// draw frame texture
const UIScene *scene = &s->scene;
mat4 out_mat;
float x1, x2, y1, y2;
if (s->scene.frontview) {
out_mat = device_transform; // full 16/9
// flip horizontally so it looks like a mirror
x2 = (float)scene->front_box_x / s->rgb_front_width;
x1 = (float)(scene->front_box_x + scene->front_box_width) / s->rgb_front_width;
y1 = (float)scene->front_box_y / s->rgb_front_height;
y2 = (float)(scene->front_box_y + scene->front_box_height) / s->rgb_front_height;
} else {
out_mat = matmul(device_transform, frame_transform);
x1 = 0.0;
x2 = 1.0;
y1 = 0.0;
y2 = 1.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
};
glActiveTexture(GL_TEXTURE0);
if (s->scene.frontview) {
glBindTexture(GL_TEXTURE_2D, s->frame_front_tex);
} else {
glBindTexture(GL_TEXTURE_2D, s->frame_tex);
}
glUseProgram(s->frame_program);
glUniform1i(s->frame_texture_loc, 0);
glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat.v);
glEnableVertexAttribArray(s->frame_pos_loc);
glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), frame_coords);
glEnableVertexAttribArray(s->frame_texcoord_loc);
glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), &frame_coords[0][2]);
assert(glGetError() == GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, &frame_indicies[0]);
}
/*
* Draw a rect at specific position with specific dimensions
*/
static void ui_draw_rounded_rect(
NVGcontext* c,
int x,
int y,
int width,
int height,
int radius,
NVGcolor color
) {
int bottom_x = x + width;
int bottom_y = y + height;
nvgBeginPath(c);
// Position the rect
nvgRoundedRect(c, x, y, bottom_x, bottom_y, radius);
// Color the rect
nvgFillColor(c, color);
// Draw the rect
nvgFill(c);
// Draw white border around rect
nvgStrokeColor(c, nvgRGBA(255,255,255,200));
nvgStroke(c);
}
// Draw all world space objects.
static void ui_draw_world(UIState *s) {
const UIScene *scene = &s->scene;
if (!scene->world_objects_visible) {
return;
}
//draw_steering(s, scene->curvature);
if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) {
int left_lane_color = (int)(255 * scene->model.left_lane.prob);
int right_lane_color = (int)(255 * scene->model.right_lane.prob);
draw_model_path(
s, scene->model.left_lane,
nvgRGBA(left_lane_color, left_lane_color, left_lane_color, 128));
draw_model_path(
s, scene->model.right_lane,
nvgRGBA(right_lane_color, right_lane_color, right_lane_color, 128));
// draw paths
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255));
// draw MPC only if engaged
if (scene->engaged) {
draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 19, nvgRGBA(255, 0, 0, 255));
}
}
}
static void ui_draw_vision(UIState *s) {
const UIScene *scene = &s->scene;
glClearColor(0.0, 0.0, 0.0, 0.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
// hack for eon ui
glEnable(GL_SCISSOR_TEST);
glScissor(box_x, s->fb_h-(box_y+box_height), box_width, box_height);
glViewport(box_x, s->fb_h-(box_y+box_height), box_width, box_height);
draw_frame(s);
glViewport(0, 0, s->fb_w, s->fb_h);
glDisable(GL_SCISSOR_TEST);
// nvg drawings
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// glEnable(GL_CULL_FACE);
glClear(GL_STENCIL_BUFFER_BIT);
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
nvgSave(s->vg);
// hack for eon ui
const int inner_height = box_width*9/16;
nvgScissor(s->vg, box_x, box_y, box_width, box_height);
nvgTranslate(s->vg, box_x, box_y + (box_height-inner_height)/2.0);
nvgScale(s->vg, (float)box_width / s->fb_w, (float)inner_height / s->fb_h);
if (!scene->frontview) {
// ui_draw_transformed_box(s, 0xFF00FF00);
ui_draw_world(s);
if (scene->lead_status) {
draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30,
nvgRGBA(255, 0, 0, 128));
}
const float label_size = 65.0f;
nvgFontFace(s->vg, "courbd");
if (scene->awareness_status > 0) {
nvgBeginPath(s->vg);
int bar_height = scene->awareness_status * 700;
nvgRect(s->vg, 100, 300 + (700 - bar_height), 50, bar_height);
nvgFillColor(s->vg, nvgRGBA(255 * (1 - scene->awareness_status),
255 * scene->awareness_status, 0, 128));
nvgFill(s->vg);
}
// Draw calibration progress (if needed)
if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
int rec_width = 1120;
int x_pos = 500;
nvgBeginPath(s->vg);
nvgStrokeWidth(s->vg, 14);
nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
nvgStroke(s->vg);
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
nvgFill(s->vg);
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-semibold");
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
char calib_status_str[64];
snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc);
nvgText(s->vg, x_pos, 1010, calib_status_str, NULL);
if (s->is_metric) {
nvgText(s->vg, x_pos + 120, 1110, "Drive above 72 km/h", NULL);
} else {
nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL);
}
} else if (scene->gps_planner_active) {
int rec_width = 1120;
int x_pos = 500;
nvgBeginPath(s->vg);
nvgStrokeWidth(s->vg, 14);
nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
nvgStroke(s->vg);
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
nvgFill(s->vg);
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-semibold");
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL);
}
}
nvgRestore(s->vg);
if (!ui_alert_active(s) && !scene->frontview) {
// draw top bar
const int bar_x = box_x;
const int bar_y = box_y;
const int bar_width = box_width;
const int bar_height = 250 - box_y;
assert(s->status < ARRAYSIZE(bg_colors));
const uint8_t *color = bg_colors[s->status];
nvgBeginPath(s->vg);
nvgRect(s->vg, bar_x, bar_y, bar_width, bar_height);
nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3]));
nvgFill(s->vg);
const int message_y = box_y;
const int message_height = bar_height;
const int message_width = 800;
const int message_x = box_x + box_width / 2 - message_width / 2;
// message background
nvgBeginPath(s->vg);
NVGpaint bg = nvgLinearGradient(s->vg, message_x, message_y, message_x, message_y+message_height,
nvgRGBAf(0.0, 0.0, 0.0, 0.0), nvgRGBAf(0.0, 0.0, 0.0, 0.1));
nvgFillPaint(s->vg, bg);
nvgRect(s->vg, message_x, message_y, message_width, message_height);
nvgFill(s->vg);
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
if (s->passive) {
if (s->scene.started_ts > 0) {
// draw drive time when passive
uint64_t dt = nanos_since_boot() - s->scene.started_ts;
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
char time_str[64];
if (dt > 60*60*1000000000ULL) {
// hours
snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d:%02d",
(int)(dt/(60*60*1000000000ULL)),
(int)((dt%(60*60*1000000000ULL))/(60*1000000000ULL)),
(int)(dt%(60*1000000000ULL)/1000000000ULL));
} else {
snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d",
(int)(dt/(60*1000000000ULL)),
(int)(dt%(60*1000000000ULL)/1000000000ULL));
}
nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, time_str, NULL);
}
} else {
// status text
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 48*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
if (s->scene.alert_size == cereal_Live100Data_AlertSize_small) {
nvgFontSize(s->vg, 40*2.5);
nvgText(s->vg, message_x+message_width/2, 115, s->scene.alert_text1, NULL);
nvgFontSize(s->vg, 26*2.5);
nvgText(s->vg, message_x+message_width/2, 185, s->scene.alert_text2, NULL);
} else if (s->status == STATUS_DISENGAGED) {
nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "DISENGAGED", NULL);
} else if (s->status == STATUS_ENGAGED) {
nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "ENGAGED", NULL);
}
}
// set speed
const int left_x = bar_x;
const int left_y = bar_y;
const int left_width = (bar_width - message_width) / 2;
const int left_height = bar_height;
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
if (scene->v_cruise != 255 && scene->v_cruise != 0) {
char speed_str[16];
if (s->is_metric) {
snprintf(speed_str, sizeof(speed_str), "%3d kph",
(int)(scene->v_cruise + 0.5));
} else {
/* Convert KPH to MPH. Using an approximated mph to kph
conversion factor of 1.609 because this is what the Honda
hud seems to be using */
snprintf(speed_str, sizeof(speed_str), "%3d mph",
(int)(scene->v_cruise * 0.621504 + 0.5));
}
nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL);
} else {
nvgText(s->vg, left_x+left_width/2, 115, "N/A", NULL);
}
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 26*2.5);
nvgText(s->vg, left_x+left_width/2, 185, "SET SPEED", NULL);
// lead car
const int right_y = bar_y;
const int right_width = (bar_width - message_width) / 2;
const int right_x = bar_x+bar_width-right_width;
const int right_height = bar_height;
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
if (scene->lead_status) {
char radar_str[16];
// lead car is always in meters
if (s->is_metric || true) {
snprintf(radar_str, sizeof(radar_str), "%d m", (int)scene->lead_d_rel);
} else {
snprintf(radar_str, sizeof(radar_str), "%d ft", (int)(scene->lead_d_rel * 3.28084));
}
nvgText(s->vg, right_x+right_width/2, 115, radar_str, NULL);
} else {
nvgText(s->vg, right_x+right_width/2, 115, "N/A", NULL);
}
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 26*2.5);
nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL);
}
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
// glDisable(GL_CULL_FACE);
}
static void ui_draw_alerts(UIState *s) {
const UIScene *scene = &s->scene;
if (!ui_alert_active(s)) return;
assert(s->status < ARRAYSIZE(alert_colors));
const uint8_t *color = alert_colors[s->status];
char alert_text1_upper[1024] = {0};
for (int i=0; scene->alert_text1[i] && i < sizeof(alert_text1_upper)-1; i++) {
alert_text1_upper[i] = toupper(scene->alert_text1[i]);
}
nvgBeginPath(s->vg);
nvgRect(s->vg, box_x, box_y, box_width, box_height);
nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3]));
nvgFill(s->vg);
nvgFontFace(s->vg, "sans-semibold");
if (strlen(alert_text1_upper) > 15) {
nvgFontSize(s->vg, 72.0*2.5);
} else {
nvgFontSize(s->vg, 96.0*2.5);
}
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
nvgTextBox(s->vg, box_x + 50, box_y + 287, box_width - 50, alert_text1_upper, NULL);
if (strlen(scene->alert_text2) > 0) {
nvgFontFace(s->vg, "sans-regular");
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgFontSize(s->vg, 44.0*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
nvgTextBox(s->vg, box_x + 50, box_y + box_height - 250, box_width - 50, scene->alert_text2, NULL);
}
}
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_aside(UIState *s) {
char speed_str[32];
float speed;
bool is_cruise_set = (s->scene.v_cruise != 0 && s->scene.v_cruise != 255);
unsigned long last_cruise_update_dt = (nanos_since_boot() - s->scene.v_cruise_update_ts);
bool should_draw_cruise_speed = is_cruise_set && last_cruise_update_dt < 2000000000ULL;
if (should_draw_cruise_speed) {
speed = s->scene.v_cruise / 3.6;
nvgFillColor(s->vg, nvgRGBA(0xFF, 0xD8, 0xAC, 0xFF));
} else {
speed = s->scene.v_ego;
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
}
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 110);
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.2374144 + 0.5));
}
nvgText(s->vg, 150, 762, speed_str, NULL);
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 70);
if (s->is_metric) {
nvgText(s->vg, 150, 817, "kph", NULL);
} else {
nvgText(s->vg, 150, 817, "mph", NULL);
}
}
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);
if (s->vision_connected) {
ui_draw_aside(s);
}
ui_draw_alerts(s);
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
}
eglSwapBuffers(s->display, s->surface);
assert(glGetError() == GL_NO_ERROR);
}
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->intrinsic_matrix_loaded) {
s->intrinsic_matrix_loaded = try_load_intrinsics(&s->intrinsic_matrix);
}
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
// setup frame texture
glDeleteTextures(1, &s->frame_tex); //silently ignores a 0 texture
glGenTextures(1, &s->frame_tex);
glBindTexture(GL_TEXTURE_2D, s->frame_tex);
glTexStorage2D(GL_TEXTURE_2D, 1, GL_RGB8, s->rgb_width, s->rgb_height);
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);
// front
glDeleteTextures(1, &s->frame_front_tex);
glGenTextures(1, &s->frame_front_tex);
glBindTexture(GL_TEXTURE_2D, s->frame_front_tex);
glTexStorage2D(GL_TEXTURE_2D, 1, GL_RGB8, s->rgb_front_width, s->rgb_front_height);
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);
s->vision_connect_firstrun = false;
}
// poll for events
while (true) {
zmq_pollitem_t polls[8] = {{0}};
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->plus_sock_raw;
polls[6].events = ZMQ_POLLIN;
int num_polls = 7;
if (s->vision_connected) {
assert(s->ipc_fd >= 0);
polls[7].fd = s->ipc_fd;
polls[7].events = ZMQ_POLLIN;
num_polls++;
}
int ret = zmq_poll(polls, num_polls, 0);
if (ret < 0) {
LOGW("poll failed (%d)", ret);
break;
}
if (ret == 0) {
break;
}
if (polls[0].revents || polls[1].revents || polls[2].revents ||
polls[3].revents || polls[4].revents) {
// awake on any (old) activity
set_awake(s, true);
}
if (s->vision_connected && polls[7].revents) {
// 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;
continue;
}
if (rp.type == VIPC_STREAM_ACQUIRE) {
bool front = rp.d.stream_acq.type == VISION_STREAM_UI_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;
s->scene.bgr_front_ptr = s->front_bufs[idx].addr;
} else {
assert(idx < UI_BUF_COUNT);
s->cur_vision_idx = idx;
s->scene.bgr_ptr = s->bufs[idx].addr;
// printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]);
}
if (front == s->scene.frontview) {
ui_update_frame(s);
}
} else {
assert(false);
}
} else if (polls[6].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<6; i++) {
if (polls[i].revents) {
which = polls[i].socket;
break;
}
}
if (which == NULL) {
continue;
}
zmq_msg_t msg;
err = zmq_msg_init(&msg);
assert(err == 0);
err = zmq_msg_recv(&msg, which, 0);
assert(err >= 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);
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.gps_planner_active = datad.gpsPlannerActive;
// printf("recv %f\n", datad.vEgo);
s->scene.frontview = datad.rearViewCam;
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.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);
}
} 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;
} else if (eventd.which == cereal_Event_liveCalibration) {
s->scene.world_objects_visible = s->intrinsic_matrix_loaded;
struct cereal_LiveCalibrationData datad;
cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
s->scene.cal_status = datad.calStatus;
s->scene.cal_perc = datad.calPerc;
// 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);
} 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));
}
} 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;
}
capn_free(&ctx);
zmq_msg_close(&msg);
}
}
}
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 p1 = {
.type = VIPC_STREAM_SUBSCRIBE,
.d = { .stream_sub = { .type = VISION_STREAM_UI_BACK, .tbuffer = true, }, },
};
err = vipc_send(fd, &p1);
if (err < 0) {
close(fd);
continue;
}
VisionPacket p2 = {
.type = VIPC_STREAM_SUBSCRIBE,
.d = { .stream_sub = { .type = VISION_STREAM_UI_FRONT, .tbuffer = true, }, },
};
err = vipc_send(fd, &p2);
if (err < 0) {
close(fd);
continue;
}
// printf("init recv\n");
VisionPacket back_rp;
err = vipc_recv(fd, &back_rp);
if (err <= 0) {
close(fd);
continue;
}
assert(back_rp.type == VIPC_STREAM_BUFS);
VisionPacket front_rp;
err = vipc_recv(fd, &front_rp);
if (err <= 0) {
close(fd);
continue;
}
assert(front_rp.type == VIPC_STREAM_BUFS);
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 <hardware/sensors.h>
#include <utils/Timers.h>
#define SENSOR_LIGHT 7
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);
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;
//printf("new light sensor value: %f\n", s->light_sensor);
}
}
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);
bool first = true;
while(!do_exit) {
pthread_mutex_lock(&s->lock);
if (first) {
first = false;
} else {
pthread_cond_wait(&s->bg_cond, &s->lock);
}
assert(s->status < ARRAYSIZE(bg_colors));
const uint8_t *color = bg_colors[s->status];
pthread_mutex_unlock(&s->lock);
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 main() {
int err;
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);
// light sensor scaling params
#define LIGHT_SENSOR_M 1.3
#define LIGHT_SENSOR_B 5.0
#define NEO_BRIGHTNESS 100
float smooth_light_sensor = LIGHT_SENSOR_B;
const int EON = (access("/EON", F_OK) != -1);
while (!do_exit) {
pthread_mutex_lock(&s->lock);
if (EON) {
// light sensor is only exposed on EONs
float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B;
if (clipped_light_sensor > 255) clipped_light_sensor = 255;
smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99;
set_brightness((int)smooth_light_sensor);
} else {
// compromise for bright and dark envs
set_brightness(NEO_BRIGHTNESS);
}
ui_update(s);
if (s->awake) {
ui_draw(s);
}
// awake on any touch
int touch_x = -1, touch_y = -1;
int touched = touch_poll(&touch, &touch_x, &touch_y);
if (touched == 1) {
// touch event will still happen :(
set_awake(s, true);
}
// manage wakefulness
if (s->awake_timeout > 0) {
s->awake_timeout--;
} else {
set_awake(s, false);
}
pthread_mutex_unlock(&s->lock);
// no simple way to do 30fps vsync with surfaceflinger...
usleep(30000);
}
set_awake(s, true);
// 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;
}