sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
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.
131 Commits
115 Branches
85 Tags
6.2 GiB
 
 
 
 
 
 
Tag: Branch: Tree: f2aa2c9ae7
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'f2aa2c9ae7'
${ noResults }
openpilot_comma/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;
}