2020-01-17 12:05:23 -07:00
|
|
|
#include <stdio.h>
|
2020-11-11 21:31:46 -07:00
|
|
|
#include <cmath>
|
2020-01-17 12:05:23 -07:00
|
|
|
#include <stdlib.h>
|
|
|
|
#include <stdbool.h>
|
2020-09-03 17:32:55 -06:00
|
|
|
#include <signal.h>
|
2020-01-17 12:05:23 -07:00
|
|
|
#include <unistd.h>
|
|
|
|
#include <assert.h>
|
2020-10-18 20:03:41 -06:00
|
|
|
#include <math.h>
|
2020-09-03 17:32:55 -06:00
|
|
|
#include <poll.h>
|
2020-01-17 12:05:23 -07:00
|
|
|
#include <sys/mman.h>
|
2020-08-20 09:16:44 -06:00
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
#include "common/util.h"
|
|
|
|
#include "common/swaglog.h"
|
|
|
|
#include "common/visionimg.h"
|
2020-06-08 18:24:55 -06:00
|
|
|
#include "common/utilpp.h"
|
2020-05-21 17:04:33 -06:00
|
|
|
#include "ui.hpp"
|
2020-08-20 09:16:44 -06:00
|
|
|
#include "paint.hpp"
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-08-20 09:16:44 -06:00
|
|
|
extern volatile sig_atomic_t do_exit;
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-08-20 09:16:44 -06:00
|
|
|
int write_param_float(float param, const char* param_name, bool persistent_param) {
|
2020-05-08 20:39:18 -06:00
|
|
|
char s[16];
|
|
|
|
int size = snprintf(s, sizeof(s), "%f", param);
|
2020-10-13 08:23:23 -06:00
|
|
|
return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s));
|
2020-05-08 20:39:18 -06:00
|
|
|
}
|
|
|
|
|
2020-08-20 09:16:44 -06:00
|
|
|
void ui_init(UIState *s) {
|
2020-11-11 21:31:46 -07:00
|
|
|
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
|
2020-09-28 15:49:07 -06:00
|
|
|
"health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"});
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-06-23 16:16:55 -06:00
|
|
|
s->started = false;
|
2020-09-03 17:32:55 -06:00
|
|
|
s->status = STATUS_OFFROAD;
|
|
|
|
s->scene.satelliteCount = -1;
|
|
|
|
read_param(&s->is_metric, "IsMetric");
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-04-09 21:55:54 -06:00
|
|
|
s->fb = framebuffer_init("ui", 0, true, &s->fb_w, &s->fb_h);
|
2020-01-17 12:05:23 -07:00
|
|
|
assert(s->fb);
|
|
|
|
|
|
|
|
ui_nvg_init(s);
|
|
|
|
}
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
static void ui_init_vision(UIState *s) {
|
|
|
|
// Invisible until we receive a calibration message.
|
|
|
|
s->scene.world_objects_visible = false;
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
for (int i = 0; i < UI_BUF_COUNT; i++) {
|
|
|
|
if (s->khr[i] != 0) {
|
|
|
|
visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]);
|
|
|
|
glDeleteTextures(1, &s->frame_texs[i]);
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
VisionImg img = {
|
|
|
|
.fd = s->stream.bufs[i].fd,
|
|
|
|
.format = VISIONIMG_FORMAT_RGB24,
|
|
|
|
.width = s->stream.bufs_info.width,
|
|
|
|
.height = s->stream.bufs_info.height,
|
|
|
|
.stride = s->stream.bufs_info.stride,
|
|
|
|
.bpp = 3,
|
|
|
|
.size = s->stream.bufs_info.buf_len,
|
|
|
|
};
|
|
|
|
#ifndef QCOM
|
|
|
|
s->priv_hnds[i] = s->stream.bufs[i].addr;
|
|
|
|
#endif
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
assert(glGetError() == GL_NO_ERROR);
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
void ui_update_vision(UIState *s) {
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
if (!s->vision_connected && s->started) {
|
|
|
|
const VisionStreamType type = s->scene.frontview ? VISION_STREAM_RGB_FRONT : VISION_STREAM_RGB_BACK;
|
|
|
|
int err = visionstream_init(&s->stream, type, true, nullptr);
|
|
|
|
if (err == 0) {
|
|
|
|
ui_init_vision(s);
|
|
|
|
s->vision_connected = true;
|
|
|
|
}
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
if (s->vision_connected) {
|
|
|
|
if (!s->started) goto destroy;
|
|
|
|
|
|
|
|
// poll for a new frame
|
|
|
|
struct pollfd fds[1] = {{
|
|
|
|
.fd = s->stream.ipc_fd,
|
|
|
|
.events = POLLOUT,
|
|
|
|
}};
|
|
|
|
int ret = poll(fds, 1, 100);
|
|
|
|
if (ret > 0) {
|
|
|
|
if (!visionstream_get(&s->stream, nullptr)) goto destroy;
|
|
|
|
}
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
return;
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
destroy:
|
|
|
|
visionstream_destroy(&s->stream);
|
|
|
|
s->vision_connected = false;
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
void update_sockets(UIState *s) {
|
|
|
|
|
2020-05-06 09:38:26 -06:00
|
|
|
UIScene &scene = s->scene;
|
2020-09-03 17:32:55 -06:00
|
|
|
SubMaster &sm = *(s->sm);
|
|
|
|
|
|
|
|
if (sm.update(0) == 0){
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2020-08-31 05:16:30 -06:00
|
|
|
if (s->started && sm.updated("controlsState")) {
|
2020-05-21 17:04:33 -06:00
|
|
|
auto event = sm["controlsState"];
|
2020-06-05 16:33:45 -06:00
|
|
|
scene.controls_state = event.getControlsState();
|
2020-05-06 09:38:26 -06:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
// TODO: the alert stuff shouldn't be handled here
|
2020-06-05 16:33:45 -06:00
|
|
|
auto alert_sound = scene.controls_state.getAlertSound();
|
2020-06-23 11:46:33 -06:00
|
|
|
if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) {
|
2020-06-15 16:26:38 -06:00
|
|
|
if (alert_sound == AudibleAlert::NONE) {
|
2020-09-09 11:04:28 -06:00
|
|
|
s->sound->stop();
|
2020-06-15 16:26:38 -06:00
|
|
|
} else {
|
2020-09-09 11:04:28 -06:00
|
|
|
s->sound->play(alert_sound);
|
2020-05-06 09:38:26 -06:00
|
|
|
}
|
|
|
|
}
|
2020-06-05 16:33:45 -06:00
|
|
|
scene.alert_text1 = scene.controls_state.getAlertText1();
|
|
|
|
scene.alert_text2 = scene.controls_state.getAlertText2();
|
|
|
|
scene.alert_size = scene.controls_state.getAlertSize();
|
2020-06-23 11:46:33 -06:00
|
|
|
scene.alert_type = scene.controls_state.getAlertType();
|
2020-06-05 16:33:45 -06:00
|
|
|
auto alertStatus = scene.controls_state.getAlertStatus();
|
2020-05-06 09:38:26 -06:00
|
|
|
if (alertStatus == cereal::ControlsState::AlertStatus::USER_PROMPT) {
|
2020-09-03 17:32:55 -06:00
|
|
|
s->status = STATUS_WARNING;
|
2020-05-06 09:38:26 -06:00
|
|
|
} else if (alertStatus == cereal::ControlsState::AlertStatus::CRITICAL) {
|
2020-09-03 17:32:55 -06:00
|
|
|
s->status = STATUS_ALERT;
|
2020-05-06 09:38:26 -06:00
|
|
|
} else{
|
2020-09-03 17:32:55 -06:00
|
|
|
s->status = scene.controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-06-05 16:33:45 -06:00
|
|
|
float alert_blinkingrate = scene.controls_state.getAlertBlinkingRate();
|
|
|
|
if (alert_blinkingrate > 0.) {
|
2020-01-17 12:05:23 -07:00
|
|
|
if (s->alert_blinked) {
|
|
|
|
if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) {
|
2020-06-05 16:33:45 -06:00
|
|
|
s->alert_blinking_alpha += (0.05*alert_blinkingrate);
|
2020-01-17 12:05:23 -07:00
|
|
|
} else {
|
|
|
|
s->alert_blinked = false;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (s->alert_blinking_alpha > 0.25) {
|
2020-06-05 16:33:45 -06:00
|
|
|
s->alert_blinking_alpha -= (0.05*alert_blinkingrate);
|
2020-01-17 12:05:23 -07:00
|
|
|
} else {
|
|
|
|
s->alert_blinking_alpha += 0.25;
|
|
|
|
s->alert_blinked = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("radarState")) {
|
|
|
|
auto data = sm["radarState"].getRadarState();
|
2020-06-05 16:33:45 -06:00
|
|
|
scene.lead_data[0] = data.getLeadOne();
|
|
|
|
scene.lead_data[1] = data.getLeadTwo();
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("liveCalibration")) {
|
2020-05-06 09:38:26 -06:00
|
|
|
scene.world_objects_visible = true;
|
2020-05-21 17:04:33 -06:00
|
|
|
auto extrinsicl = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
|
2020-01-17 12:05:23 -07:00
|
|
|
for (int i = 0; i < 3 * 4; i++) {
|
2020-05-06 09:38:26 -06:00
|
|
|
scene.extrinsic_matrix.v[i] = extrinsicl[i];
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
2020-11-11 21:31:46 -07:00
|
|
|
if (sm.updated("modelV2")) {
|
|
|
|
scene.model = sm["modelV2"].getModelV2();
|
|
|
|
scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE);
|
|
|
|
for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
|
|
|
|
if (scene.model.getLaneLineProbs().size() > ll_idx) {
|
|
|
|
scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx];
|
|
|
|
} else {
|
|
|
|
scene.lane_line_probs[ll_idx] = 0.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
for (int re_idx = 0; re_idx < 2; re_idx++) {
|
|
|
|
if (scene.model.getRoadEdgeStds().size() > re_idx) {
|
|
|
|
scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx];
|
|
|
|
} else {
|
|
|
|
scene.road_edge_stds[re_idx] = 1.0;
|
|
|
|
}
|
|
|
|
}
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("uiLayoutState")) {
|
|
|
|
auto data = sm["uiLayoutState"].getUiLayoutState();
|
2020-05-06 09:38:26 -06:00
|
|
|
s->active_app = data.getActiveApp();
|
|
|
|
scene.uilayout_sidebarcollapsed = data.getSidebarCollapsed();
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("thermal")) {
|
2020-06-05 16:33:45 -06:00
|
|
|
scene.thermal = sm["thermal"].getThermal();
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("ubloxGnss")) {
|
|
|
|
auto data = sm["ubloxGnss"].getUbloxGnss();
|
2020-05-06 09:38:26 -06:00
|
|
|
if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) {
|
|
|
|
scene.satelliteCount = data.getMeasurementReport().getNumMeas();
|
|
|
|
}
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("health")) {
|
2020-09-07 05:13:10 -06:00
|
|
|
auto health = sm["health"].getHealth();
|
|
|
|
scene.hwType = health.getHwType();
|
|
|
|
s->ignition = health.getIgnitionLine() || health.getIgnitionCan();
|
2020-09-03 17:32:55 -06:00
|
|
|
} else if ((s->sm->frame - s->sm->rcv_frame("health")) > 5*UI_FREQ) {
|
|
|
|
scene.hwType = cereal::HealthData::HwType::UNKNOWN;
|
2020-08-27 12:57:31 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("carParams")) {
|
|
|
|
s->longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("driverState")) {
|
2020-06-05 16:33:45 -06:00
|
|
|
scene.driver_state = sm["driverState"].getDriverState();
|
2020-05-21 17:04:33 -06:00
|
|
|
}
|
|
|
|
if (sm.updated("dMonitoringState")) {
|
2020-08-01 18:26:35 -06:00
|
|
|
scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState();
|
2020-08-02 11:55:24 -06:00
|
|
|
scene.is_rhd = scene.dmonitoring_state.getIsRHD();
|
|
|
|
scene.frontview = scene.dmonitoring_state.getIsPreview();
|
2020-09-03 17:32:55 -06:00
|
|
|
} else if ((sm.frame - sm.rcv_frame("dMonitoringState")) > UI_FREQ/2) {
|
2020-08-01 18:26:35 -06:00
|
|
|
scene.frontview = false;
|
|
|
|
}
|
2020-09-28 15:49:07 -06:00
|
|
|
if (sm.updated("sensorEvents")) {
|
|
|
|
for (auto sensor : sm["sensorEvents"].getSensorEvents()) {
|
|
|
|
if (sensor.which() == cereal::SensorEventData::LIGHT) {
|
|
|
|
s->light_sensor = sensor.getLight();
|
2020-10-18 20:03:41 -06:00
|
|
|
} else if (!s->started && sensor.which() == cereal::SensorEventData::ACCELERATION) {
|
|
|
|
s->accel_sensor = sensor.getAcceleration().getV()[2];
|
|
|
|
} else if (!s->started && sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
|
|
|
|
s->gyro_sensor = sensor.getGyroUncalibrated().getV()[1];
|
2020-09-28 15:49:07 -06:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-08-01 18:26:35 -06:00
|
|
|
s->started = scene.thermal.getStarted() || scene.frontview;
|
2020-08-20 09:16:44 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
void ui_update(UIState *s) {
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
update_sockets(s);
|
|
|
|
ui_update_vision(s);
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
// Handle onroad/offroad transition
|
|
|
|
if (!s->started && s->status != STATUS_OFFROAD) {
|
|
|
|
s->status = STATUS_OFFROAD;
|
|
|
|
s->active_app = cereal::UiLayoutState::App::HOME;
|
|
|
|
s->scene.uilayout_sidebarcollapsed = false;
|
2020-10-27 15:05:13 -06:00
|
|
|
s->sound->stop();
|
2020-09-03 17:32:55 -06:00
|
|
|
} else if (s->started && s->status == STATUS_OFFROAD) {
|
|
|
|
s->status = STATUS_DISENGAGED;
|
|
|
|
s->started_frame = s->sm->frame;
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
s->active_app = cereal::UiLayoutState::App::NONE;
|
2020-03-09 20:13:32 -06:00
|
|
|
s->scene.uilayout_sidebarcollapsed = true;
|
2020-01-17 12:05:23 -07:00
|
|
|
s->alert_blinked = false;
|
2020-09-03 17:32:55 -06:00
|
|
|
s->alert_blinking_alpha = 1.0;
|
|
|
|
s->scene.alert_size = cereal::ControlsState::AlertSize::NONE;
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
// Handle controls timeout
|
2020-09-09 14:18:50 -06:00
|
|
|
if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 5*UI_FREQ) {
|
2020-09-03 17:32:55 -06:00
|
|
|
if ((s->sm)->rcv_frame("controlsState") < s->started_frame) {
|
|
|
|
// car is started, but controlsState hasn't been seen at all
|
|
|
|
s->scene.alert_text1 = "openpilot Unavailable";
|
|
|
|
s->scene.alert_text2 = "Waiting for controls to start";
|
|
|
|
s->scene.alert_size = cereal::ControlsState::AlertSize::MID;
|
2020-09-09 14:18:50 -06:00
|
|
|
} else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 5*UI_FREQ) {
|
2020-09-03 17:32:55 -06:00
|
|
|
// car is started, but controls is lagging or died
|
|
|
|
if (s->scene.alert_text2 != "Controls Unresponsive") {
|
2020-09-09 11:04:28 -06:00
|
|
|
s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT);
|
2020-09-03 17:32:55 -06:00
|
|
|
LOGE("Controls unresponsive");
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
s->scene.alert_text1 = "TAKE CONTROL IMMEDIATELY";
|
|
|
|
s->scene.alert_text2 = "Controls Unresponsive";
|
|
|
|
s->scene.alert_size = cereal::ControlsState::AlertSize::FULL;
|
|
|
|
s->status = STATUS_ALERT;
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
// Read params
|
|
|
|
if ((s->sm)->frame % (5*UI_FREQ) == 0) {
|
|
|
|
read_param(&s->is_metric, "IsMetric");
|
|
|
|
} else if ((s->sm)->frame % (6*UI_FREQ) == 0) {
|
|
|
|
int param_read = read_param(&s->last_athena_ping, "LastAthenaPingTime");
|
|
|
|
if (param_read != 0) { // Failed to read param
|
|
|
|
s->scene.athenaStatus = NET_DISCONNECTED;
|
|
|
|
} else if (nanos_since_boot() - s->last_athena_ping < 70e9) {
|
|
|
|
s->scene.athenaStatus = NET_CONNECTED;
|
|
|
|
} else {
|
|
|
|
s->scene.athenaStatus = NET_ERROR;
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|