374 lines
13 KiB
C++
374 lines
13 KiB
C++
#include "selfdrive/ui/ui.h"
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
|
|
#include <cmath>
|
|
|
|
#include "selfdrive/common/swaglog.h"
|
|
#include "selfdrive/common/util.h"
|
|
#include "selfdrive/common/visionimg.h"
|
|
#include "selfdrive/common/watchdog.h"
|
|
#include "selfdrive/hardware/hw.h"
|
|
#include "selfdrive/ui/paint.h"
|
|
#include "selfdrive/ui/qt/qt_window.h"
|
|
|
|
#define BACKLIGHT_DT 0.25
|
|
#define BACKLIGHT_TS 2.00
|
|
#define BACKLIGHT_OFFROAD 50
|
|
|
|
|
|
// Projects a point in car to space to the corresponding point in full frame
|
|
// image space.
|
|
static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out) {
|
|
const float margin = 500.0f;
|
|
const vec3 pt = (vec3){{in_x, in_y, in_z}};
|
|
const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt);
|
|
const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep);
|
|
|
|
// Project.
|
|
float x = KEp.v[0] / KEp.v[2];
|
|
float y = KEp.v[1] / KEp.v[2];
|
|
|
|
nvgTransformPoint(&out->x, &out->y, s->car_space_transform, x, y);
|
|
return out->x >= -margin && out->x <= s->fb_w + margin && out->y >= -margin && out->y <= s->fb_h + margin;
|
|
}
|
|
|
|
static void ui_init_vision(UIState *s) {
|
|
// Invisible until we receive a calibration message.
|
|
s->scene.world_objects_visible = false;
|
|
|
|
for (int i = 0; i < s->vipc_client->num_buffers; i++) {
|
|
s->texture[i].reset(new EGLImageTexture(&s->vipc_client->buffers[i]));
|
|
|
|
glBindTexture(GL_TEXTURE_2D, s->texture[i]->frame_tex);
|
|
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);
|
|
}
|
|
|
|
static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) {
|
|
const auto line_x = line.getX();
|
|
int max_idx = 0;
|
|
for (int i = 0; i < TRAJECTORY_SIZE && line_x[i] < path_height; ++i) {
|
|
max_idx = i;
|
|
}
|
|
return max_idx;
|
|
}
|
|
|
|
static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
|
|
for (int i = 0; i < 2; ++i) {
|
|
auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
|
|
if (lead_data.getStatus()) {
|
|
float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getDRel())] : 0.0;
|
|
// negative because radarState uses left positive convention
|
|
calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
|
|
float y_off, float z_off, line_vertices_data *pvd, int max_idx) {
|
|
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
|
|
vertex_data *v = &pvd->v[0];
|
|
for (int i = 0; i <= max_idx; i++) {
|
|
v += calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, v);
|
|
}
|
|
for (int i = max_idx; i >= 0; i--) {
|
|
v += calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, v);
|
|
}
|
|
pvd->cnt = v - pvd->v;
|
|
assert(pvd->cnt < std::size(pvd->v));
|
|
}
|
|
|
|
static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
|
|
UIScene &scene = s->scene;
|
|
auto model_position = model.getPosition();
|
|
float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1],
|
|
MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
|
|
|
|
// update lane lines
|
|
const auto lane_lines = model.getLaneLines();
|
|
const auto lane_line_probs = model.getLaneLineProbs();
|
|
int max_idx = get_path_length_idx(lane_lines[0], max_distance);
|
|
for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
|
|
scene.lane_line_probs[i] = lane_line_probs[i];
|
|
update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_idx);
|
|
}
|
|
|
|
// update road edges
|
|
const auto road_edges = model.getRoadEdges();
|
|
const auto road_edge_stds = model.getRoadEdgeStds();
|
|
for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
|
|
scene.road_edge_stds[i] = road_edge_stds[i];
|
|
update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx);
|
|
}
|
|
|
|
// update path
|
|
auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
|
|
if (lead_one.getStatus()) {
|
|
const float lead_d = lead_one.getDRel() * 2.;
|
|
max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
|
|
}
|
|
max_idx = get_path_length_idx(model_position, max_distance);
|
|
update_line_data(s, model_position, 0.5, 1.22, &scene.track_vertices, max_idx);
|
|
}
|
|
|
|
static void update_sockets(UIState *s){
|
|
s->sm->update(0);
|
|
}
|
|
|
|
static void update_state(UIState *s) {
|
|
SubMaster &sm = *(s->sm);
|
|
UIScene &scene = s->scene;
|
|
|
|
if (sm.updated("radarState")) {
|
|
std::optional<cereal::ModelDataV2::XYZTData::Reader> line;
|
|
if (sm.rcv_frame("modelV2") > 0) {
|
|
line = sm["modelV2"].getModelV2().getPosition();
|
|
}
|
|
update_leads(s, sm["radarState"].getRadarState(), line);
|
|
}
|
|
if (sm.updated("liveCalibration")) {
|
|
scene.world_objects_visible = true;
|
|
auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
|
|
Eigen::Vector3d rpy;
|
|
rpy << rpy_list[0], rpy_list[1], rpy_list[2];
|
|
Eigen::Matrix3d device_from_calib = euler2rot(rpy);
|
|
Eigen::Matrix3d view_from_device;
|
|
view_from_device << 0,1,0,
|
|
0,0,1,
|
|
1,0,0;
|
|
Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib;
|
|
for (int i = 0; i < 3; i++) {
|
|
for (int j = 0; j < 3; j++) {
|
|
scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j);
|
|
}
|
|
}
|
|
}
|
|
if (sm.updated("modelV2")) {
|
|
update_model(s, sm["modelV2"].getModelV2());
|
|
}
|
|
if (sm.updated("pandaState")) {
|
|
auto pandaState = sm["pandaState"].getPandaState();
|
|
scene.pandaType = pandaState.getPandaType();
|
|
scene.ignition = pandaState.getIgnitionLine() || pandaState.getIgnitionCan();
|
|
} else if ((s->sm->frame - s->sm->rcv_frame("pandaState")) > 5*UI_FREQ) {
|
|
scene.pandaType = cereal::PandaState::PandaType::UNKNOWN;
|
|
}
|
|
if (sm.updated("ubloxGnss")) {
|
|
auto data = sm["ubloxGnss"].getUbloxGnss();
|
|
if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) {
|
|
scene.satelliteCount = data.getMeasurementReport().getNumMeas();
|
|
}
|
|
}
|
|
if (sm.updated("carParams")) {
|
|
scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
|
|
}
|
|
if (sm.updated("sensorEvents")) {
|
|
for (auto sensor : sm["sensorEvents"].getSensorEvents()) {
|
|
if (!Hardware::TICI() && sensor.which() == cereal::SensorEventData::LIGHT) {
|
|
scene.light_sensor = sensor.getLight();
|
|
}
|
|
if (!scene.started && sensor.which() == cereal::SensorEventData::ACCELERATION) {
|
|
auto accel = sensor.getAcceleration().getV();
|
|
if (accel.totalSize().wordCount){ // TODO: sometimes empty lists are received. Figure out why
|
|
scene.accel_sensor = accel[2];
|
|
}
|
|
} else if (!scene.started && sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
|
|
auto gyro = sensor.getGyroUncalibrated().getV();
|
|
if (gyro.totalSize().wordCount){
|
|
scene.gyro_sensor = gyro[1];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (Hardware::TICI() && sm.updated("roadCameraState")) {
|
|
auto camera_state = sm["roadCameraState"].getRoadCameraState();
|
|
float gain = camera_state.getGainFrac() * (camera_state.getGlobalGain() > 100 ? 2.5 : 1.0) / 10.0;
|
|
scene.light_sensor = std::clamp<float>((1023.0 / 1757.0) * (1757.0 - camera_state.getIntegLines()) * (1.0 - gain), 0.0, 1023.0);
|
|
}
|
|
scene.started = sm["deviceState"].getDeviceState().getStarted() || scene.driver_view;
|
|
}
|
|
|
|
static void update_params(UIState *s) {
|
|
const uint64_t frame = s->sm->frame;
|
|
UIScene &scene = s->scene;
|
|
if (frame % (5*UI_FREQ) == 0) {
|
|
scene.is_metric = Params().getBool("IsMetric");
|
|
}
|
|
}
|
|
|
|
static void update_vision(UIState *s) {
|
|
if (!s->vipc_client->connected && s->scene.started) {
|
|
if (s->vipc_client->connect(false)){
|
|
ui_init_vision(s);
|
|
}
|
|
}
|
|
|
|
if (s->vipc_client->connected){
|
|
VisionBuf * buf = s->vipc_client->recv();
|
|
if (buf != nullptr){
|
|
s->last_frame = buf;
|
|
} else if (!Hardware::PC()) {
|
|
LOGE("visionIPC receive timeout");
|
|
}
|
|
}
|
|
}
|
|
|
|
static void update_status(UIState *s) {
|
|
if (s->scene.started && s->sm->updated("controlsState")) {
|
|
auto controls_state = (*s->sm)["controlsState"].getControlsState();
|
|
auto alert_status = controls_state.getAlertStatus();
|
|
if (alert_status == cereal::ControlsState::AlertStatus::USER_PROMPT) {
|
|
s->status = STATUS_WARNING;
|
|
} else if (alert_status == cereal::ControlsState::AlertStatus::CRITICAL) {
|
|
s->status = STATUS_ALERT;
|
|
} else {
|
|
s->status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
|
|
}
|
|
}
|
|
|
|
// Handle onroad/offroad transition
|
|
static bool started_prev = false;
|
|
if (s->scene.started != started_prev) {
|
|
if (s->scene.started) {
|
|
s->status = STATUS_DISENGAGED;
|
|
s->scene.started_frame = s->sm->frame;
|
|
|
|
s->scene.is_rhd = Params().getBool("IsRHD");
|
|
s->scene.end_to_end = Params().getBool("EndToEndToggle");
|
|
s->wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
|
|
|
|
// Update intrinsics matrix after possible wide camera toggle change
|
|
ui_resize(s, s->fb_w, s->fb_h);
|
|
|
|
// Choose vision ipc client
|
|
if (s->scene.driver_view) {
|
|
s->vipc_client = s->vipc_client_front;
|
|
} else if (s->wide_camera){
|
|
s->vipc_client = s->vipc_client_wide;
|
|
} else {
|
|
s->vipc_client = s->vipc_client_rear;
|
|
}
|
|
} else {
|
|
s->vipc_client->connected = false;
|
|
}
|
|
}
|
|
started_prev = s->scene.started;
|
|
}
|
|
|
|
|
|
QUIState::QUIState(QObject *parent) : QObject(parent) {
|
|
ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
|
|
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "liveLocationKalman",
|
|
"pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss",
|
|
#ifdef QCOM2
|
|
"roadCameraState",
|
|
#endif
|
|
});
|
|
|
|
ui_state.fb_w = vwp_w;
|
|
ui_state.fb_h = vwp_h;
|
|
ui_state.scene.started = false;
|
|
ui_state.last_frame = nullptr;
|
|
ui_state.wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
|
|
|
|
ui_state.vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true);
|
|
ui_state.vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true);
|
|
ui_state.vipc_client_wide = new VisionIpcClient("camerad", VISION_STREAM_RGB_WIDE, true);
|
|
|
|
ui_state.vipc_client = ui_state.vipc_client_rear;
|
|
|
|
// update timer
|
|
timer = new QTimer(this);
|
|
QObject::connect(timer, &QTimer::timeout, this, &QUIState::update);
|
|
timer->start(0);
|
|
}
|
|
|
|
void QUIState::update() {
|
|
update_params(&ui_state);
|
|
update_sockets(&ui_state);
|
|
update_state(&ui_state);
|
|
update_status(&ui_state);
|
|
update_vision(&ui_state);
|
|
|
|
if (ui_state.scene.started != started_prev || ui_state.sm->frame == 1) {
|
|
started_prev = ui_state.scene.started;
|
|
emit offroadTransition(!ui_state.scene.started);
|
|
|
|
// Change timeout to 0 when onroad, this will call update continously.
|
|
// This puts visionIPC in charge of update frequency, reducing video latency
|
|
timer->start(ui_state.scene.started ? 0 : 1000 / UI_FREQ);
|
|
}
|
|
|
|
watchdog_kick();
|
|
emit uiUpdate(ui_state);
|
|
}
|
|
|
|
Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) {
|
|
brightness_b = Params(true).get<float>("BRIGHTNESS_B").value_or(10.0);
|
|
brightness_m = Params(true).get<float>("BRIGHTNESS_M").value_or(0.1);
|
|
}
|
|
|
|
void Device::update(const UIState &s) {
|
|
updateBrightness(s);
|
|
updateWakefulness(s);
|
|
|
|
// TODO: remove from UIState and use signals
|
|
QUIState::ui_state.awake = awake;
|
|
}
|
|
|
|
void Device::setAwake(bool on, bool reset) {
|
|
if (on != awake) {
|
|
awake = on;
|
|
Hardware::set_display_power(awake);
|
|
LOGD("setting display power %d", awake);
|
|
emit displayPowerChanged(awake);
|
|
}
|
|
|
|
if (reset) {
|
|
awake_timeout = 30 * UI_FREQ;
|
|
}
|
|
}
|
|
|
|
void Device::updateBrightness(const UIState &s) {
|
|
float clipped_brightness = std::min(100.0f, (s.scene.light_sensor * brightness_m) + brightness_b);
|
|
if (Hardware::TICI() && !s.scene.started) {
|
|
clipped_brightness = BACKLIGHT_OFFROAD;
|
|
}
|
|
|
|
int brightness = brightness_filter.update(clipped_brightness);
|
|
if (!awake) {
|
|
brightness = 0;
|
|
}
|
|
|
|
if (brightness != last_brightness) {
|
|
std::thread{Hardware::set_brightness, brightness}.detach();
|
|
}
|
|
last_brightness = brightness;
|
|
}
|
|
|
|
void Device::updateWakefulness(const UIState &s) {
|
|
awake_timeout = std::max(awake_timeout - 1, 0);
|
|
|
|
bool should_wake = s.scene.started || s.scene.ignition;
|
|
if (!should_wake) {
|
|
// tap detection while display is off
|
|
bool accel_trigger = abs(s.scene.accel_sensor - accel_prev) > 0.2;
|
|
bool gyro_trigger = abs(s.scene.gyro_sensor - gyro_prev) > 0.15;
|
|
should_wake = accel_trigger && gyro_trigger;
|
|
gyro_prev = s.scene.gyro_sensor;
|
|
accel_prev = (accel_prev * (accel_samples - 1) + s.scene.accel_sensor) / accel_samples;
|
|
}
|
|
|
|
setAwake(awake_timeout, should_wake);
|
|
}
|