Brian Hanson
6e7117b177
Cruise warning sign appears above speed limit sign when cruise set speed is too far from the speed limit: - Red (over): cruise >= limit + 10 (if limit >= 50) or + 5 (if < 50) - Green (under): cruise <= limit - 5 - Only when cruise active (not paused/disabled) and limit >= 20 - Nightrider mode: colored text/border on black background Speed limit sign enlarged 5%. 20px gap between signs. Bench mode gains cruiseactive command (0=disabled, 1=active, 2=paused). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
1552 lines
59 KiB
C++
Executable File
1552 lines
59 KiB
C++
Executable File
#include "selfdrive/ui/qt/onroad.h"
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <map>
|
||
#include <memory>
|
||
#include <sstream>
|
||
|
||
#include <QApplication>
|
||
#include <QDateTime>
|
||
#include <QDebug>
|
||
#include <QMouseEvent>
|
||
|
||
#include "common/swaglog.h"
|
||
#include "common/timing.h"
|
||
#include "selfdrive/ui/qt/util.h"
|
||
|
||
// static void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity, const int angle = 0) {
|
||
// p.setRenderHint(QPainter::Antialiasing);
|
||
// p.setOpacity(1.0); // bg dictates opacity of ellipse
|
||
// p.setPen(Qt::NoPen);
|
||
// p.setBrush(bg);
|
||
// p.drawEllipse(center, btn_size / 2, btn_size / 2);
|
||
// p.save();
|
||
// p.translate(center);
|
||
// p.rotate(-angle);
|
||
// p.setOpacity(opacity);
|
||
// p.drawPixmap(-QPoint(img.width() / 2, img.height() / 2), img);
|
||
// p.setOpacity(1.0);
|
||
// p.restore();
|
||
// }
|
||
|
||
bool alert_is_visible = false;
|
||
|
||
OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->scene) {
|
||
QVBoxLayout *main_layout = new QVBoxLayout(this);
|
||
main_layout->setMargin(UI_BORDER_SIZE);
|
||
QStackedLayout *stacked_layout = new QStackedLayout;
|
||
stacked_layout->setStackingMode(QStackedLayout::StackAll);
|
||
main_layout->addLayout(stacked_layout);
|
||
|
||
nvg = new AnnotatedCameraWidget(VISION_STREAM_ROAD, this);
|
||
|
||
QWidget * split_wrapper = new QWidget;
|
||
split = new QHBoxLayout(split_wrapper);
|
||
split->setContentsMargins(0, 0, 0, 0);
|
||
split->setSpacing(0);
|
||
split->addWidget(nvg);
|
||
|
||
stacked_layout->addWidget(split_wrapper);
|
||
|
||
alerts = new OnroadAlerts(this);
|
||
alerts->setAttribute(Qt::WA_TransparentForMouseEvents, true);
|
||
stacked_layout->addWidget(alerts);
|
||
|
||
// setup stacking order
|
||
alerts->raise();
|
||
|
||
setAttribute(Qt::WA_OpaquePaintEvent);
|
||
QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState);
|
||
QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition);
|
||
|
||
QObject::connect(&clickTimer, &QTimer::timeout, this, [this]() {
|
||
clickTimer.stop();
|
||
QMouseEvent *event = new QMouseEvent(QEvent::MouseButtonPress, timeoutPoint, Qt::LeftButton, Qt::LeftButton, Qt::NoModifier);
|
||
QApplication::postEvent(this, event);
|
||
});
|
||
}
|
||
|
||
void OnroadWindow::updateState(const UIState &s) {
|
||
if (!s.scene.started) {
|
||
return;
|
||
}
|
||
|
||
Alert alert = Alert::get(*(s.sm), s.scene.started_frame);
|
||
alerts->updateAlert(alert);
|
||
|
||
nvg->updateState(s);
|
||
nvg->update(); // CLEARPILOT: force repaint every frame for HUD elements
|
||
|
||
QColor bgColor = bg_colors[s.status];
|
||
if (paramsMemory.getBool("no_lat_lane_change") == 1) {
|
||
bgColor = bg_colors[STATUS_DISENGAGED];
|
||
}
|
||
|
||
if (bg != bgColor) {
|
||
bg = bgColor;
|
||
nvg->updateLaneEdgeColor(bg);
|
||
update();
|
||
}
|
||
}
|
||
|
||
void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
||
// FrogPilot clickable widgets
|
||
bool widgetClicked = false;
|
||
|
||
// // Change cruise control increments button
|
||
// QRect maxSpeedRect(7, 25, 225, 225);
|
||
// bool isMaxSpeedClicked = maxSpeedRect.contains(e->pos()) && scene.reverse_cruise_ui;
|
||
|
||
// // Hide speed button
|
||
// QRect hideSpeedRect(rect().center().x() - 175, 50, 350, 350);
|
||
// bool isSpeedClicked = hideSpeedRect.contains(e->pos()) && scene.hide_speed_ui;
|
||
|
||
// // Speed limit confirmation buttons
|
||
// QSize size = this->size();
|
||
// QRect leftRect(0, 0, size.width() / 2, size.height());
|
||
// QRect rightRect = leftRect.translated(size.width() / 2, 0);
|
||
|
||
// bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
|
||
// bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
|
||
|
||
// // Speed limit offset button
|
||
// QRect speedLimitRect(7, 250, 225, 225);
|
||
// bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()) && scene.show_slc_offset_ui;
|
||
|
||
// if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
|
||
// if (isMaxSpeedClicked) {
|
||
// std::thread([this]() {
|
||
// bool currentReverseCruise = scene.reverse_cruise;
|
||
|
||
// uiState()->scene.reverse_cruise = !currentReverseCruise;
|
||
// params.putBoolNonBlocking("ReverseCruise", !currentReverseCruise);
|
||
|
||
// paramsMemory.putBool("FrogPilotTogglesUpdated", true);
|
||
// std::this_thread::sleep_for(std::chrono::seconds(1));
|
||
// paramsMemory.putBool("FrogPilotTogglesUpdated", false);
|
||
// }).detach();
|
||
|
||
// } else if (isSpeedClicked) {
|
||
// bool currentHideSpeed = scene.hide_speed;
|
||
|
||
// uiState()->scene.hide_speed = !currentHideSpeed;
|
||
// params.putBoolNonBlocking("HideSpeed", !currentHideSpeed);
|
||
// } else if (isSpeedLimitClicked) {
|
||
// bool currentShowSLCOffset = scene.show_slc_offset;
|
||
|
||
// scene.show_slc_offset = !currentShowSLCOffset;
|
||
// params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset);
|
||
// }
|
||
|
||
// widgetClicked = true;
|
||
// } else if (isLeftSideClicked || isRightSideClicked) {
|
||
// bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
|
||
// paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed);
|
||
// paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true);
|
||
|
||
// widgetClicked = true;
|
||
// // If the click wasn't for anything specific, change the value of "ExperimentalMode"
|
||
// } else if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
|
||
// if (clickTimer.isActive()) {
|
||
// clickTimer.stop();
|
||
|
||
// if (scene.conditional_experimental) {
|
||
// int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 6) ? 0 : scene.conditional_status >= 7 ? 5 : 6;
|
||
// paramsMemory.putIntNonBlocking("CEStatus", override_value);
|
||
// } else {
|
||
// bool experimentalMode = params.getBool("ExperimentalMode");
|
||
// params.putBoolNonBlocking("ExperimentalMode", !experimentalMode);
|
||
// }
|
||
|
||
// } else {
|
||
// clickTimer.start(500);
|
||
// }
|
||
// widgetClicked = true;
|
||
// }
|
||
|
||
// propagation event to parent(HomeWindow)
|
||
if (!widgetClicked) {
|
||
QWidget::mousePressEvent(e);
|
||
}
|
||
}
|
||
|
||
void OnroadWindow::offroadTransition(bool offroad) {
|
||
alerts->updateAlert({});
|
||
}
|
||
|
||
void OnroadWindow::paintEvent(QPaintEvent *event) {
|
||
QPainter p(this);
|
||
// CLEARPILOT: hide engagement border in nightrider mode
|
||
int dm = paramsMemory.getInt("ScreenDisplayMode");
|
||
bool nightrider = (dm == 1 || dm == 4);
|
||
if (nightrider) {
|
||
p.fillRect(rect(), Qt::black);
|
||
} else {
|
||
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
|
||
}
|
||
|
||
QString logicsDisplayString = QString();
|
||
if (scene.show_jerk) {
|
||
logicsDisplayString += QString("Acceleration Jerk: %1 (%2%3) | Speed Jerk: %4 (%5%6) | ")
|
||
.arg(scene.acceleration_jerk, 0, 'f', 3)
|
||
.arg(scene.acceleration_jerk_difference > 0 ? "-" : "", 0)
|
||
.arg(abs(scene.acceleration_jerk_difference), 0, 'f', 3)
|
||
.arg(scene.ego_jerk, 0, 'f', 3)
|
||
.arg(scene.ego_jerk_difference > 0 ? "-" : "", 0)
|
||
.arg(abs(scene.ego_jerk_difference), 0, 'f', 3);
|
||
}
|
||
if (scene.show_tuning) {
|
||
if (!scene.live_valid) {
|
||
logicsDisplayString += "Friction: Calculating... | Lateral Acceleration: Calculating...";
|
||
} else {
|
||
logicsDisplayString += QString("Friction: %1 | Lateral Acceleration: %2")
|
||
.arg(scene.friction, 0, 'f', 3)
|
||
.arg(scene.lat_accel, 0, 'f', 3);
|
||
}
|
||
}
|
||
if (logicsDisplayString.endsWith(" | ")) {
|
||
logicsDisplayString.chop(3);
|
||
}
|
||
|
||
if (!logicsDisplayString.isEmpty()) {
|
||
p.setFont(InterFont(28, QFont::DemiBold));
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
p.setPen(Qt::white);
|
||
|
||
QRect currentRect = rect();
|
||
int logicsWidth = p.fontMetrics().horizontalAdvance(logicsDisplayString);
|
||
int logicsX = (currentRect.width() - logicsWidth) / 2;
|
||
int logicsY = currentRect.top() + 27;
|
||
|
||
p.drawText(logicsX, logicsY, logicsDisplayString);
|
||
update();
|
||
}
|
||
}
|
||
|
||
// void OnroadWindow::update_screen_on_off() {
|
||
// int screenDisaplayMode = paramsMemory.getInt("ScreenDisaplayMode");
|
||
// if (screenDisaplayMode == 1) {
|
||
// // Conditionally off
|
||
// }
|
||
// }
|
||
|
||
|
||
// ***** onroad widgets *****
|
||
|
||
// OnroadAlerts
|
||
void OnroadAlerts::updateAlert(const Alert &a) {
|
||
if (!alert.equal(a)) {
|
||
alert = a;
|
||
update();
|
||
}
|
||
}
|
||
|
||
void OnroadAlerts::paintEvent(QPaintEvent *event) {
|
||
alert_is_visible = false;
|
||
|
||
if (alert.size == cereal::ControlsState::AlertSize::NONE) {
|
||
return;
|
||
}
|
||
|
||
if (scene.hide_alerts && alert.status == cereal::ControlsState::AlertStatus::NORMAL) {
|
||
return;
|
||
}
|
||
|
||
alert_is_visible = true;
|
||
|
||
static std::map<cereal::ControlsState::AlertSize, const int> alert_heights = {
|
||
{cereal::ControlsState::AlertSize::SMALL, 271},
|
||
{cereal::ControlsState::AlertSize::MID, 420},
|
||
{cereal::ControlsState::AlertSize::FULL, height()},
|
||
};
|
||
int h = alert_heights[alert.size];
|
||
|
||
int margin = 40;
|
||
int radius = 30;
|
||
int offset = scene.show_aol_status_bar || scene.show_cem_status_bar || scene.road_name_ui ? 25 : 0;
|
||
if (alert.size == cereal::ControlsState::AlertSize::FULL) {
|
||
margin = 0;
|
||
radius = 0;
|
||
offset = 0;
|
||
}
|
||
QRect r = QRect(0 + margin, height() - h + margin - offset, width() - margin*2, h - margin*2);
|
||
|
||
QPainter p(this);
|
||
|
||
// draw background + gradient
|
||
p.setPen(Qt::NoPen);
|
||
p.setCompositionMode(QPainter::CompositionMode_SourceOver);
|
||
p.setBrush(QBrush(alert_colors[alert.status]));
|
||
p.drawRoundedRect(r, radius, radius);
|
||
|
||
QLinearGradient g(0, r.y(), 0, r.bottom());
|
||
g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05));
|
||
g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35));
|
||
|
||
p.setCompositionMode(QPainter::CompositionMode_DestinationOver);
|
||
p.setBrush(QBrush(g));
|
||
p.drawRoundedRect(r, radius, radius);
|
||
p.setCompositionMode(QPainter::CompositionMode_SourceOver);
|
||
|
||
// text
|
||
const QPoint c = r.center();
|
||
p.setPen(QColor(0xff, 0xff, 0xff));
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
if (alert.size == cereal::ControlsState::AlertSize::SMALL) {
|
||
p.setFont(InterFont(74, QFont::DemiBold));
|
||
p.drawText(r, Qt::AlignCenter, alert.text1);
|
||
} else if (alert.size == cereal::ControlsState::AlertSize::MID) {
|
||
p.setFont(InterFont(88, QFont::Bold));
|
||
p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1);
|
||
p.setFont(InterFont(66));
|
||
p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2);
|
||
} else if (alert.size == cereal::ControlsState::AlertSize::FULL) {
|
||
bool l = alert.text1.length() > 15;
|
||
p.setFont(InterFont(l ? 132 : 177, QFont::Bold));
|
||
p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1);
|
||
p.setFont(InterFont(88));
|
||
p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// Window that shows camera view and variety of info drawn on top
|
||
AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent), scene(uiState()->scene) {
|
||
pm = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"uiDebug"});
|
||
|
||
main_layout = new QVBoxLayout(this);
|
||
main_layout->setMargin(UI_BORDER_SIZE);
|
||
main_layout->setSpacing(0);
|
||
|
||
QHBoxLayout *buttons_layout = new QHBoxLayout();
|
||
buttons_layout->setSpacing(0);
|
||
|
||
// Neokii screen recorder — DISABLED: using dashcamd instead
|
||
|
||
QVBoxLayout *top_right_layout = new QVBoxLayout();
|
||
top_right_layout->setSpacing(0);
|
||
top_right_layout->addLayout(buttons_layout);
|
||
|
||
main_layout->addLayout(top_right_layout, 0);
|
||
main_layout->setAlignment(top_right_layout, Qt::AlignTop | Qt::AlignRight);
|
||
|
||
dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
|
||
|
||
// Initialize FrogPilot widgets
|
||
initializeFrogPilotWidgets();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateState(const UIState &s) {
|
||
const int SET_SPEED_NA = 255;
|
||
const SubMaster &sm = *(s.sm);
|
||
|
||
const bool cs_alive = sm.alive("controlsState");
|
||
const auto cs = sm["controlsState"].getControlsState();
|
||
const auto car_state = sm["carState"].getCarState();
|
||
(void)car_state; // CLEARPILOT: suppress unused warning, will use later
|
||
|
||
// Handle older routes where vCruiseCluster is not set
|
||
float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster();
|
||
setSpeed = cs_alive ? v_cruise : SET_SPEED_NA;
|
||
is_cruise_set = setSpeed > 0 && (int)setSpeed != SET_SPEED_NA;
|
||
if (is_cruise_set && !s.scene.is_metric) {
|
||
setSpeed *= KM_TO_MILE;
|
||
}
|
||
|
||
// CLEARPILOT: read speed and speed limit from params (written by speed_logic.py ~2Hz)
|
||
clpParamFrame++;
|
||
if (clpParamFrame % 10 == 0) { // ~2Hz at 20Hz UI update rate
|
||
clpHasSpeed = paramsMemory.get("ClearpilotHasSpeed") == "1";
|
||
clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay"));
|
||
clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay"));
|
||
clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit"));
|
||
clpCruiseWarning = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarning"));
|
||
clpCruiseWarningSpeed = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarningSpeed"));
|
||
}
|
||
|
||
// auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
|
||
// speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||
// speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||
// if (speedLimitController && !slcOverridden) {
|
||
// speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
|
||
// }
|
||
|
||
// has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
|
||
// has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) && !(speedLimitController && !useViennaSLCSign) || (speedLimitController && useViennaSLCSign);
|
||
// is_metric = s.scene.is_metric;
|
||
// speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
|
||
// hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals != 0 && (turnSignalLeft || turnSignalRight));
|
||
// status = s.status;
|
||
|
||
// update engageability/experimental mode button
|
||
// CLEARPILOT
|
||
|
||
// update DM icon
|
||
auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
|
||
dmActive = dm_state.getIsActiveMode();
|
||
rightHandDM = dm_state.getIsRHD();
|
||
// DM icon transition
|
||
dm_fade_state = std::clamp(dm_fade_state+0.2*(0.5-dmActive), 0.0, 1.0);
|
||
|
||
updateFrogPilotWidgets();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateLaneEdgeColor(QColor &bgColor) {
|
||
if (edgeColor != bgColor) {
|
||
edgeColor = bgColor;
|
||
}
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
||
// CLEARPILOT: display power control based on ScreenDisplayMode
|
||
p.save();
|
||
|
||
if (displayMode == 3 && !alert_is_visible) {
|
||
Hardware::set_display_power(false);
|
||
p.restore();
|
||
return;
|
||
} else {
|
||
Hardware::set_display_power(true);
|
||
}
|
||
|
||
// CLEARPILOT: blinking blue circle when telemetry is recording
|
||
if (paramsMemory.getBool("TelemetryEnabled")) {
|
||
// Blink: visible for 500ms, hidden for 500ms
|
||
int phase = (QDateTime::currentMSecsSinceEpoch() / 500) % 2;
|
||
if (phase == 0) {
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(QColor(30, 100, 220));
|
||
p.drawEllipse(width() - 150, 50, 100, 100);
|
||
}
|
||
}
|
||
|
||
// Header gradient
|
||
QLinearGradient bg(0, UI_HEADER_HEIGHT - (UI_HEADER_HEIGHT / 2.5), 0, UI_HEADER_HEIGHT);
|
||
bg.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.45));
|
||
bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
|
||
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
|
||
|
||
// Todo: Display stock image if no camera
|
||
|
||
// QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
|
||
// QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
|
||
// QString speedStr = QString::number(std::nearbyint(speed));
|
||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
|
||
|
||
p.restore();
|
||
|
||
// if (!scene.hide_max_speed) {
|
||
// drawSpeedWidget(p, 60, 45, QString("MAX"), setSpeedStr, QColor(0xff, 0xff, 0xff));
|
||
// }
|
||
|
||
// // Todo: needs to be changed to calculate off of actual speed limit for release
|
||
// QString speedLimitStr = QString::number(paramsMemory.getInt("CarSpeedLimitLiteral"));
|
||
// drawSpeedWidget(p, 60, 45 + (225), QString("Limit"), speedLimitStr, QColor(0xff, 0xff, 0xff));
|
||
|
||
// // Todo: needs to be changed to calculate off of actual speed limit for release
|
||
// QString SpeedLimitLatDesired = QString::number(paramsMemory.getInt("SpeedLimitLatDesired"));
|
||
// drawSpeedWidget(p, 60, 45 + (225 * 2), QString("Exp"), SpeedLimitLatDesired, QColor(0xff, 0xff, 0xff));
|
||
|
||
// // Todo: needs to be changed to calculate off of actual speed limit for release
|
||
// QString adjustedCruise = QString::number(paramsMemory.getInt("SpeedLimitVTSC"));
|
||
// drawSpeedWidget(p, 60, 45 + (225 * 3), QString("VTSC"), adjustedCruise, QColor(0xff, 0xff, 0xff));
|
||
|
||
// Todo: lead speed
|
||
// Todo: Experimental speed
|
||
|
||
// CLEARPILOT: show speed from speed_logic params, hide when no speed or speed=0
|
||
if (clpHasSpeed && !clpSpeedDisplay.isEmpty() && !scene.hide_speed) {
|
||
p.setFont(InterFont(140, QFont::Bold));
|
||
drawText(p, rect().center().x(), 210, clpSpeedDisplay);
|
||
p.setFont(InterFont(50));
|
||
drawText(p, rect().center().x(), 290, clpSpeedUnit, 200);
|
||
}
|
||
|
||
// CLEARPILOT: speed limit sign in lower-left, cruise warning above it
|
||
drawSpeedLimitSign(p);
|
||
drawCruiseWarningSign(p);
|
||
|
||
// Draw FrogPilot widgets
|
||
paintFrogPilotWidgets(p);
|
||
}
|
||
|
||
|
||
// void drawSpeedWidgetRegional(QPainter &p, int x, int y, QString title, QString text, QColor colorSpeed) {
|
||
|
||
// const QRect sign_rect = set_speed_rect.adjusted(sign_margin, default_size.height(), -sign_margin, -sign_margin);
|
||
// // US/Canada (MUTCD style) sign
|
||
// if (has_us_speed_limit) {
|
||
// p.setPen(Qt::NoPen);
|
||
// p.setBrush(whiteColor());
|
||
// p.drawRoundedRect(sign_rect, 24, 24);
|
||
// p.setPen(QPen(blackColor(), 6));
|
||
// p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
|
||
|
||
// p.save();
|
||
// p.setOpacity(slcOverridden ? 0.25 : 1.0);
|
||
// if (speedLimitController && showSLCOffset && !slcOverridden) {
|
||
// p.setFont(InterFont(28, QFont::DemiBold));
|
||
// p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
|
||
// p.setFont(InterFont(70, QFont::Bold));
|
||
// p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||
// p.setFont(InterFont(50, QFont::DemiBold));
|
||
// p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
|
||
// } else {
|
||
// p.setFont(InterFont(28, QFont::DemiBold));
|
||
// p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
|
||
// p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
|
||
// p.setFont(InterFont(70, QFont::Bold));
|
||
// p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||
// }
|
||
// p.restore();
|
||
// }
|
||
|
||
// // EU (Vienna style) sign
|
||
// if (has_eu_speed_limit) {
|
||
// p.setPen(Qt::NoPen);
|
||
// p.setBrush(whiteColor());
|
||
// p.drawEllipse(sign_rect);
|
||
// p.setPen(QPen(Qt::red, 20));
|
||
// p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
|
||
|
||
// p.save();
|
||
// p.setOpacity(slcOverridden ? 0.25 : 1.0);
|
||
// p.setPen(blackColor());
|
||
// if (showSLCOffset) {
|
||
// p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||
// p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
|
||
// p.setFont(InterFont(40, QFont::DemiBold));
|
||
// p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
|
||
// } else {
|
||
// p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||
// p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||
// }
|
||
// p.restore();
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
void AnnotatedCameraWidget::drawSpeedWidget(QPainter &p, int x, int y, const QString &title, const QString &speedLimitStr, QColor colorSpeed, int width) {
|
||
// Draw outer box + border to contain set speed and speed limit
|
||
const int sign_margin = 12;
|
||
const int us_sign_height = 186;
|
||
const int eu_sign_size = width;
|
||
|
||
const QSize default_size = {width, 204};
|
||
QSize set_speed_size = default_size;
|
||
if (is_metric || has_eu_speed_limit) set_speed_size.rwidth() = 200;
|
||
if (has_us_speed_limit && speedLimitStr.size() >= 3) set_speed_size.rwidth() = 223;
|
||
|
||
if (has_us_speed_limit) set_speed_size.rheight() += us_sign_height + sign_margin;
|
||
else if (has_eu_speed_limit) set_speed_size.rheight() += eu_sign_size + sign_margin;
|
||
|
||
int top_radius = 32;
|
||
int bottom_radius = has_eu_speed_limit ? 100 : 32;
|
||
|
||
QRect set_speed_rect(QPoint(x + (default_size.width() - set_speed_size.width()) / 2, y), set_speed_size);
|
||
p.setPen(QPen(colorSpeed));
|
||
p.setBrush(blackColor(166));
|
||
drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
|
||
|
||
// Draw MAX
|
||
QColor max_color = QColor(0xaa, 0xaa, 0xaa, 0xff);
|
||
p.setFont(InterFont(40, QFont::DemiBold));
|
||
p.setPen(max_color);
|
||
p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, title);
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.setPen(colorSpeed);
|
||
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||
}
|
||
|
||
|
||
void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
|
||
// Hide when no speed limit or speed limit is 0
|
||
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
|
||
|
||
p.save();
|
||
|
||
const int signW = 189;
|
||
const int signH = 239;
|
||
const int margin = 20;
|
||
const int borderW = 6;
|
||
const int innerBorderW = 4;
|
||
const int innerMargin = 10;
|
||
const int cornerR = 15;
|
||
|
||
// Position: 20px from lower-left corner
|
||
QRect signRect(margin, height() - signH - margin, signW, signH);
|
||
|
||
if (nightriderMode) {
|
||
// Nightrider: black background, light gray-blue border and text
|
||
QColor borderColor(160, 180, 210);
|
||
QColor textColor(160, 180, 210);
|
||
|
||
// Outer border
|
||
p.setPen(QPen(borderColor, borderW));
|
||
p.setBrush(QColor(0, 0, 0, 220));
|
||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||
|
||
// Inner border
|
||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||
p.setPen(QPen(borderColor, innerBorderW));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||
|
||
// "SPEED" text
|
||
p.setPen(textColor);
|
||
p.setFont(InterFont(30, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||
|
||
// "LIMIT" text
|
||
p.setFont(InterFont(30, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
|
||
|
||
// Speed limit number
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
|
||
} else {
|
||
// Normal: white background, black border and text
|
||
QColor borderColor(0, 0, 0);
|
||
QColor textColor(0, 0, 0);
|
||
|
||
// Outer border
|
||
p.setPen(QPen(borderColor, borderW));
|
||
p.setBrush(QColor(255, 255, 255, 240));
|
||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||
|
||
// Inner border
|
||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||
p.setPen(QPen(borderColor, innerBorderW));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||
|
||
// "SPEED" text
|
||
p.setPen(textColor);
|
||
p.setFont(InterFont(30, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||
|
||
// "LIMIT" text
|
||
p.setFont(InterFont(30, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
|
||
|
||
// Speed limit number
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
|
||
}
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) {
|
||
// Only show when there's an active warning and the speed limit sign is visible
|
||
if (clpCruiseWarning.isEmpty() || clpCruiseWarningSpeed.isEmpty()) return;
|
||
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
|
||
|
||
bool isOver = (clpCruiseWarning == "over");
|
||
if (!isOver && clpCruiseWarning != "under") return;
|
||
|
||
p.save();
|
||
|
||
// Same dimensions as speed limit sign
|
||
const int signW = 189;
|
||
const int signH = 239;
|
||
const int margin = 20;
|
||
const int borderW = 6;
|
||
const int innerBorderW = 4;
|
||
const int innerMargin = 10;
|
||
const int cornerR = 15;
|
||
const int gap = 20;
|
||
|
||
// Position: directly above the speed limit sign
|
||
int speedLimitY = height() - signH - margin;
|
||
QRect signRect(margin, speedLimitY - signH - gap, signW, signH);
|
||
|
||
if (nightriderMode) {
|
||
// Nightrider: black background with colored border/text
|
||
QColor accentColor = isOver ? QColor(220, 50, 50) : QColor(50, 180, 80);
|
||
|
||
p.setPen(QPen(accentColor, borderW));
|
||
p.setBrush(QColor(0, 0, 0, 220));
|
||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||
|
||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||
p.setPen(QPen(accentColor, innerBorderW));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||
|
||
// "CRUISE" text
|
||
p.setPen(accentColor);
|
||
p.setFont(InterFont(26, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
|
||
|
||
// "SET" text
|
||
p.setFont(InterFont(26, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
|
||
|
||
// Cruise speed number
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
|
||
} else {
|
||
// Normal: colored background with white border/text
|
||
QColor bgColor = isOver ? QColor(200, 30, 30, 240) : QColor(40, 160, 60, 240);
|
||
QColor fgColor(255, 255, 255);
|
||
|
||
p.setPen(QPen(fgColor, borderW));
|
||
p.setBrush(bgColor);
|
||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||
|
||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||
p.setPen(QPen(fgColor, innerBorderW));
|
||
p.setBrush(Qt::NoBrush);
|
||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||
|
||
// "CRUISE" text
|
||
p.setPen(fgColor);
|
||
p.setFont(InterFont(26, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
|
||
|
||
// "SET" text
|
||
p.setFont(InterFont(26, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
|
||
|
||
// Cruise speed number
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
|
||
}
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
|
||
QRect real_rect = p.fontMetrics().boundingRect(text);
|
||
real_rect.moveCenter({x, y - real_rect.height() / 2});
|
||
|
||
p.setPen(QColor(0xff, 0xff, 0xff, alpha));
|
||
p.drawText(real_rect.x(), real_rect.bottom(), text);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::initializeGL() {
|
||
CameraWidget::initializeGL();
|
||
qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
|
||
qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR));
|
||
qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER));
|
||
qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));
|
||
|
||
prev_draw_t = millis_since_boot();
|
||
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateFrameMat() {
|
||
CameraWidget::updateFrameMat();
|
||
UIState *s = uiState();
|
||
int w = width(), h = height();
|
||
|
||
s->fb_w = w;
|
||
s->fb_h = h;
|
||
|
||
// Apply transformation such that video pixel coordinates match video
|
||
// 1) Put (0, 0) in the middle of the video
|
||
// 2) Apply same scaling as video
|
||
// 3) Put (0, 0) in top left corner of video
|
||
s->car_space_transform.reset();
|
||
s->car_space_transform.translate(w / 2 - x_offset, h / 2 - y_offset)
|
||
.scale(zoom, zoom)
|
||
.translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
|
||
painter.save();
|
||
|
||
// CLEARPILOT: color channel code rewriten to allow custom colors
|
||
|
||
SubMaster &sm = *(s->sm);
|
||
|
||
// CLEARPILOT: nightrider mode — outline only, no fill
|
||
bool outlineOnly = nightriderMode;
|
||
|
||
// CLEARPILOT: in nightrider mode, hide all lines when not engaged
|
||
if (outlineOnly && edgeColor == bg_colors[STATUS_DISENGAGED]) {
|
||
painter.restore();
|
||
return;
|
||
}
|
||
|
||
// CLEARPILOT: nightrider lines are 1px wider (3 instead of 2)
|
||
int outlineWidth = outlineOnly ? 3 : 2;
|
||
|
||
// lanelines
|
||
for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) {
|
||
QColor lineColor = QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(scene.lane_line_probs[i], 0.0, 0.7));
|
||
if (outlineOnly) {
|
||
painter.setPen(QPen(lineColor, outlineWidth));
|
||
painter.setBrush(Qt::NoBrush);
|
||
} else {
|
||
painter.setPen(Qt::NoPen);
|
||
painter.setBrush(lineColor);
|
||
}
|
||
painter.drawPolygon(scene.lane_line_vertices[i]);
|
||
}
|
||
if (outlineOnly) painter.setPen(Qt::NoPen);
|
||
|
||
// road edges
|
||
for (int i = 0; i < std::size(scene.road_edge_vertices); ++i) {
|
||
QColor edgeCol = QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
|
||
if (outlineOnly) {
|
||
painter.setPen(QPen(edgeCol, outlineWidth));
|
||
painter.setBrush(Qt::NoBrush);
|
||
} else {
|
||
painter.setPen(Qt::NoPen);
|
||
painter.setBrush(edgeCol);
|
||
}
|
||
painter.drawPolygon(scene.road_edge_vertices[i]);
|
||
}
|
||
if (outlineOnly) painter.setPen(Qt::NoPen);
|
||
|
||
// paint center lane path
|
||
// QColor bg_colors[CHANGE_LANE_PATH_COLOR];
|
||
|
||
bool is_no_lat_lane_change = paramsMemory.getBool("no_lat_lane_change");
|
||
|
||
QColor center_lane_color;
|
||
|
||
if (is_no_lat_lane_change) {
|
||
center_lane_color = bg_colors[CHANGE_LANE_PATH_COLOR];
|
||
} else {
|
||
center_lane_color = bg_colors[CENTER_LANE_COLOR];
|
||
}
|
||
|
||
QLinearGradient path_gradient(0, height(), 0, 0);
|
||
|
||
if (edgeColor != bg_colors[STATUS_DISENGAGED] || is_no_lat_lane_change) {
|
||
if (scene.acceleration_path) {
|
||
// The first half of track_vertices are the points for the right side of the path
|
||
// and the indices match the positions of accel from uiPlan
|
||
const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel();
|
||
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration_const.size());
|
||
|
||
// Copy of the acceleration vector
|
||
std::vector<float> acceleration;
|
||
for (int i = 0; i < acceleration_const.size(); i++) {
|
||
acceleration.push_back(acceleration_const[i]);
|
||
}
|
||
|
||
try {
|
||
for (int i = 0; i < max_len; ++i) {
|
||
// Rewrote to generate color based off of bg_colors[CENTER_LANE_COLOR] constant
|
||
if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue;
|
||
|
||
float lin_grad_point = (height() - scene.track_vertices[i].y()) / height();
|
||
|
||
double _h, _s, _l; // Use double for compatibility with QColor::getHslF()
|
||
center_lane_color.getHslF(&_h, &_s, &_l);
|
||
|
||
// Calculate saturation and lightness based on acceleration
|
||
float adjusted_saturation = std::min(std::abs(acceleration[i] * 1.5f), 1.f);
|
||
float adjusted_lightness = util::map_val(adjusted_saturation, 0.f, 1.f, static_cast<float>(_l), 0.95f); // Using base lightness as a starting point
|
||
|
||
// Calculate dynamic alpha based on lin_grad_point
|
||
float dynamic_alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, CENTER_LANE_ALPHA, 0.f);
|
||
|
||
QColor final_color = QColor::fromHslF(static_cast<float>(_h / 360.f), adjusted_saturation, adjusted_lightness, dynamic_alpha);
|
||
path_gradient.setColorAt(lin_grad_point, final_color);
|
||
|
||
i += (i + 2) < max_len ? 1 : 0; // Skipping a point to optimize rendering
|
||
}
|
||
} catch (const std::exception& e) {
|
||
// Default shading if for some reason the above code fails
|
||
path_gradient = QLinearGradient(0, height(), 0, 0);
|
||
path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5)));
|
||
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
|
||
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
|
||
}
|
||
} else {
|
||
path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5)));
|
||
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
|
||
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
|
||
}
|
||
|
||
if (outlineOnly) {
|
||
painter.setPen(QPen(QColor(center_lane_color.red(), center_lane_color.green(),
|
||
center_lane_color.blue(), 180), outlineWidth));
|
||
painter.setBrush(Qt::NoBrush);
|
||
} else {
|
||
painter.setPen(Qt::NoPen);
|
||
painter.setBrush(path_gradient);
|
||
}
|
||
painter.drawPolygon(scene.track_vertices);
|
||
if (outlineOnly) painter.setPen(Qt::NoPen);
|
||
}
|
||
|
||
// Paint path edges ,Use current background color
|
||
if (edgeColor != bg_colors[STATUS_DISENGAGED] && !outlineOnly) {
|
||
QLinearGradient edge_gradient;
|
||
edge_gradient.setColorAt(0.0, QColor(edgeColor.red(), edgeColor.green(), edgeColor.blue(), static_cast<int>(255)));
|
||
edge_gradient.setColorAt(0.5, QColor(edgeColor.red(), edgeColor.green(), edgeColor.blue(), static_cast<int>(255 * 0.8) ));
|
||
edge_gradient.setColorAt(1.0, QColor(edgeColor.red(), edgeColor.green(), edgeColor.blue(), static_cast<int>(255 * 0.7)));
|
||
|
||
QPainterPath path;
|
||
path.addPolygon(scene.track_vertices);
|
||
path.addPolygon(scene.track_edge_vertices);
|
||
|
||
painter.setBrush(edge_gradient);
|
||
painter.drawPath(path);
|
||
}
|
||
|
||
// Paint blindspot path
|
||
if (scene.blind_spot_path) {
|
||
QColor bsColor = QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6);
|
||
if (outlineOnly) {
|
||
painter.setPen(QPen(bsColor, outlineWidth));
|
||
painter.setBrush(Qt::NoBrush);
|
||
} else {
|
||
QLinearGradient bs(0, height(), 0, 0);
|
||
bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6));
|
||
bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4));
|
||
bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2));
|
||
painter.setBrush(bs);
|
||
}
|
||
if (blindSpotLeft) {
|
||
painter.drawPolygon(scene.track_adjacent_vertices[4]);
|
||
}
|
||
if (blindSpotRight) {
|
||
painter.drawPolygon(scene.track_adjacent_vertices[5]);
|
||
}
|
||
if (outlineOnly) painter.setPen(Qt::NoPen);
|
||
}
|
||
|
||
// Paint adjacent lane paths
|
||
if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) {
|
||
QString unit_d = is_metric ? tr(" meters") : tr(" feet");
|
||
|
||
float minLaneWidth = laneDetectionWidth * 0.5;
|
||
float maxLaneWidth = laneDetectionWidth * 1.5;
|
||
|
||
auto paintLane = [=](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) {
|
||
bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot;
|
||
float hue = redPath ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth);
|
||
|
||
if (outlineOnly) {
|
||
painter.setPen(QPen(QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6), 2));
|
||
painter.setBrush(Qt::NoBrush);
|
||
} else {
|
||
QLinearGradient al(0, height(), 0, 0);
|
||
al.setColorAt(0.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6));
|
||
al.setColorAt(0.5, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.4));
|
||
al.setColorAt(1.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.2));
|
||
painter.setBrush(al);
|
||
}
|
||
painter.drawPolygon(lane);
|
||
|
||
painter.setFont(InterFont(30, QFont::DemiBold));
|
||
painter.setPen(Qt::white);
|
||
|
||
QRectF boundingRect = lane.boundingRect();
|
||
if (scene.adjacent_path_metrics) {
|
||
QString text = blindspot ? tr("Vehicle in blind spot") :
|
||
QString("%1%2").arg(laneWidth * distanceConversion, 0, 'f', 2).arg(unit_d);
|
||
painter.drawText(boundingRect, Qt::AlignCenter, text);
|
||
}
|
||
painter.setPen(Qt::NoPen);
|
||
};
|
||
|
||
paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft);
|
||
paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight);
|
||
}
|
||
|
||
painter.restore();
|
||
}
|
||
|
||
|
||
void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego) {
|
||
painter.save();
|
||
|
||
const float speedBuff = customColors != 0 ? 25. : 10.; // Make the center of the chevron appear sooner if a theme is active
|
||
const float leadBuff = customColors != 0 ? 100. : 40.; // Make the center of the chevron appear sooner if a theme is active
|
||
const float d_rel = lead_data.getX()[0];
|
||
const float v_rel = lead_data.getV()[0] - v_ego;
|
||
|
||
float fillAlpha = 0;
|
||
if (d_rel < leadBuff) {
|
||
fillAlpha = 255 * (1.0 - (d_rel / leadBuff));
|
||
if (v_rel < 0) {
|
||
fillAlpha += 255 * (-1 * (v_rel / speedBuff));
|
||
}
|
||
fillAlpha = (int)(fmin(fillAlpha, 255));
|
||
}
|
||
|
||
float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
|
||
float x = std::clamp((float)vd.x(), 0.f, width() - sz / 2);
|
||
float y = std::fmin(height() - sz * .6, (float)vd.y());
|
||
|
||
float g_xo = sz / 5;
|
||
float g_yo = sz / 10;
|
||
|
||
QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
|
||
painter.setBrush(QColor(218, 202, 37, 255));
|
||
painter.drawPolygon(glow, std::size(glow));
|
||
|
||
// chevron
|
||
QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
|
||
painter.setBrush(redColor(fillAlpha));
|
||
painter.drawPolygon(chevron, std::size(chevron));
|
||
|
||
if (leadInfo) {
|
||
float lead_speed = std::max(v_rel + v_ego, 0.0f);
|
||
|
||
painter.setPen(Qt::white);
|
||
painter.setFont(InterFont(35, QFont::Bold));
|
||
|
||
QString text = QString("%1 %2 | %3 %4")
|
||
.arg(qRound(d_rel * distanceConversion))
|
||
.arg(leadDistanceUnit)
|
||
.arg(qRound(lead_speed * speedConversion))
|
||
.arg(leadSpeedUnit);
|
||
|
||
QFontMetrics metrics(painter.font());
|
||
int middle_x = (chevron[2].x() + chevron[0].x()) / 2;
|
||
int textWidth = metrics.horizontalAdvance(text);
|
||
painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text);
|
||
}
|
||
|
||
painter.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::paintGL() {
|
||
}
|
||
|
||
void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) {
|
||
UIState *s = uiState();
|
||
SubMaster &sm = *(s->sm);
|
||
QPainter painter(this);
|
||
const double start_draw_t = millis_since_boot();
|
||
const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2();
|
||
const float v_ego = sm["carState"].getCarState().getVEgo();
|
||
|
||
// CLEARPILOT: read display mode early — needed for camera suppression
|
||
displayMode = paramsMemory.getInt("ScreenDisplayMode");
|
||
nightriderMode = (displayMode == 1 || displayMode == 4);
|
||
|
||
// draw camera frame
|
||
{
|
||
std::lock_guard lk(frame_lock);
|
||
|
||
if (frames.empty()) {
|
||
if (skip_frame_count > 0) {
|
||
skip_frame_count--;
|
||
qDebug() << "skipping frame, not ready";
|
||
return;
|
||
}
|
||
} else {
|
||
// skip drawing up to this many frames if we're
|
||
// missing camera frames. this smooths out the
|
||
// transitions from the narrow and wide cameras
|
||
skip_frame_count = 5;
|
||
}
|
||
|
||
// Wide or narrow cam dependent on speed
|
||
bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD);
|
||
if (has_wide_cam && cameraView == 0) {
|
||
if ((v_ego < 10) || available_streams.size() == 1) {
|
||
wide_cam_requested = true;
|
||
} else if (v_ego > 15) {
|
||
wide_cam_requested = false;
|
||
}
|
||
wide_cam_requested = wide_cam_requested && experimentalMode;
|
||
// for replay of old routes, never go to widecam
|
||
wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
|
||
}
|
||
CameraWidget::setStreamType(cameraView == 1 ? VISION_STREAM_DRIVER :
|
||
cameraView == 3 || wide_cam_requested ? VISION_STREAM_WIDE_ROAD :
|
||
VISION_STREAM_ROAD);
|
||
|
||
s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
|
||
if (s->scene.calibration_valid) {
|
||
auto calib = s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;
|
||
CameraWidget::updateCalibration(calib);
|
||
} else {
|
||
CameraWidget::updateCalibration(DEFAULT_CALIBRATION);
|
||
}
|
||
// CLEARPILOT: force CameraWidget bg to black in nightrider to prevent color bleed
|
||
if (nightriderMode) {
|
||
CameraWidget::setBackgroundColor(Qt::black);
|
||
}
|
||
painter.beginNativePainting();
|
||
if (nightriderMode) {
|
||
// CLEARPILOT: black background, no camera feed
|
||
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
|
||
glClear(GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
|
||
} else {
|
||
CameraWidget::setFrameId(model.getFrameId());
|
||
CameraWidget::paintGL();
|
||
}
|
||
painter.endNativePainting();
|
||
}
|
||
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
painter.setPen(Qt::NoPen);
|
||
|
||
if (s->scene.world_objects_visible || true) { // fixme, test
|
||
update_model(s, model, sm["uiPlan"].getUiPlan());
|
||
drawLaneLines(painter, s);
|
||
|
||
// Clearpilot - draw leads even in stock long (test)
|
||
// if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) {
|
||
if (sm.rcv_frame("modelV2") > s->scene.started_frame) {
|
||
update_leads(s, model);
|
||
float prev_drel = -1;
|
||
for (int i = 0; i < model.getLeadsV3().size() && i < 2; i++) {
|
||
const auto &lead = model.getLeadsV3()[i];
|
||
auto lead_drel = lead.getX()[0];
|
||
if (lead.getProb() > 0.5 && (prev_drel < 0 || std::abs(lead_drel - prev_drel) > 3.0)) {
|
||
// Disabled for the moment.
|
||
// drawLead(painter, lead, s->scene.lead_vertices[i], v_ego);
|
||
}
|
||
prev_drel = lead_drel;
|
||
}
|
||
}
|
||
}
|
||
|
||
// DMoji
|
||
// if (!hideBottomIcons && (sm.rcv_frame("driverStateV2") > s->scene.started_frame)) {
|
||
// update_dmonitoring(s, sm["driverStateV2"].getDriverStateV2(), dm_fade_state, rightHandDM);
|
||
// }
|
||
|
||
drawHud(painter);
|
||
|
||
double cur_draw_t = millis_since_boot();
|
||
double dt = cur_draw_t - prev_draw_t;
|
||
fps = fps_filter.update(1. / dt * 1000);
|
||
if (fps < 15) {
|
||
LOGW("slow frame rate: %.2f fps", fps);
|
||
}
|
||
prev_draw_t = cur_draw_t;
|
||
|
||
// publish debug msg
|
||
MessageBuilder msg;
|
||
auto m = msg.initEvent().initUiDebug();
|
||
m.setDrawTimeMillis(cur_draw_t - start_draw_t);
|
||
pm->send("uiDebug", msg);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
|
||
CameraWidget::showEvent(event);
|
||
|
||
ui_update_params(uiState());
|
||
prev_draw_t = millis_since_boot();
|
||
}
|
||
|
||
// FrogPilot widgets
|
||
void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
|
||
bottom_layout = new QHBoxLayout();
|
||
|
||
|
||
// QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum);
|
||
// bottom_layout->addItem(spacer);
|
||
|
||
main_layout->addLayout(bottom_layout);
|
||
|
||
themeConfiguration = {
|
||
{1, {"frog_theme", QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))},
|
||
{0.5, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.5))},
|
||
{1.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.1))}}}},
|
||
};
|
||
|
||
animationTimer = new QTimer(this);
|
||
connect(animationTimer, &QTimer::timeout, this, [this] {
|
||
animationFrameIndex = (animationFrameIndex + 1) % totalFrames;
|
||
});
|
||
|
||
// CLEARPILOT: screen recorder disabled — replaced by dedicated dashcamd process
|
||
// QTimer *record_timer = new QTimer(this);
|
||
// connect(record_timer, &QTimer::timeout, this, [this]() {
|
||
// if (recorder_btn) {
|
||
// recorder_btn->update_screen();
|
||
// }
|
||
// });
|
||
// record_timer->start(1000 / UI_FREQ);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateFrogPilotWidgets() {
|
||
alertSize = scene.alert_size;
|
||
|
||
alwaysOnLateralActive = scene.always_on_lateral_active;
|
||
showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
|
||
|
||
blindSpotLeft = scene.blind_spot_left;
|
||
blindSpotRight = scene.blind_spot_right;
|
||
|
||
cameraView = scene.camera_view;
|
||
|
||
conditionalStatus = scene.conditional_status;
|
||
showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
|
||
|
||
bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc;
|
||
cruiseAdjustment = disableSmoothing || !is_cruise_set ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0);
|
||
vtscControllingCurve = scene.vtsc_controlling_curve;
|
||
|
||
customColors = scene.custom_colors;
|
||
|
||
// experimentalMode = scene.experimental_mode;
|
||
|
||
laneDetectionWidth = scene.lane_detection_width;
|
||
laneWidthLeft = scene.lane_width_left;
|
||
laneWidthRight = scene.lane_width_right;
|
||
|
||
leadInfo = scene.lead_info;
|
||
obstacleDistance = scene.obstacle_distance;
|
||
obstacleDistanceStock = scene.obstacle_distance_stock;
|
||
|
||
roadNameUI = scene.road_name_ui;
|
||
|
||
speedLimitController = scene.speed_limit_controller;
|
||
showSLCOffset = speedLimitController && scene.show_slc_offset;
|
||
slcOverridden = speedLimitController && scene.speed_limit_overridden;
|
||
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||
useViennaSLCSign = scene.use_vienna_slc_sign;
|
||
|
||
trafficModeActive = scene.traffic_mode_active;
|
||
|
||
turnSignalLeft = scene.turn_signal_left;
|
||
turnSignalRight = scene.turn_signal_right;
|
||
|
||
if (customSignals != scene.custom_signals) {
|
||
customSignals = scene.custom_signals;
|
||
|
||
QString themePath;
|
||
|
||
themePath = QString("../frogpilot/assets/custom_themes/%1/images").arg(
|
||
themeConfiguration.find(customSignals) != themeConfiguration.end() ?
|
||
std::get<0>(themeConfiguration[customSignals]) : "");
|
||
|
||
const QStringList imagePaths = {
|
||
themePath + "/turn_signal_1.png",
|
||
themePath + "/turn_signal_2.png",
|
||
themePath + "/turn_signal_3.png",
|
||
themePath + "/turn_signal_4.png"
|
||
};
|
||
|
||
signalImgVector.clear();
|
||
signalImgVector.reserve(2 * imagePaths.size() + 2);
|
||
|
||
for (const QString &imagePath : imagePaths) {
|
||
QPixmap pixmap(imagePath);
|
||
signalImgVector.push_back(pixmap);
|
||
signalImgVector.push_back(pixmap.transformed(QTransform().scale(-1, 1)));
|
||
}
|
||
|
||
const QPixmap blindSpotPixmap(themePath + "/turn_signal_1_red.png");
|
||
signalImgVector.push_back(blindSpotPixmap);
|
||
signalImgVector.push_back(blindSpotPixmap.transformed(QTransform().scale(-1, 1)));
|
||
}
|
||
}
|
||
|
||
void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
|
||
// CLEARPILOT: only show status bar when telemetry is enabled
|
||
if (paramsMemory.getBool("TelemetryEnabled")) {
|
||
drawStatusBar(p);
|
||
}
|
||
|
||
if (leadInfo) {
|
||
drawLeadInfo(p);
|
||
}
|
||
|
||
if (scene.speed_limit_changed) {
|
||
drawSLCConfirmation(p);
|
||
}
|
||
|
||
// CLEARPILOT: screen recorder disabled, using dashcamd instead
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
|
||
static QElapsedTimer timer;
|
||
static bool isFiveSecondsPassed = false;
|
||
static double maxAcceleration = 0.0;
|
||
constexpr int maxAccelDuration = 5000;
|
||
|
||
QString accelerationUnit = tr(" m/s²");
|
||
leadDistanceUnit = tr("meters");
|
||
leadSpeedUnit = tr("m/s");
|
||
|
||
float accelerationConversion = 1.0f;
|
||
distanceConversion = 1.0f;
|
||
speedConversion = 1.0f;
|
||
|
||
if (!scene.use_si) {
|
||
if (is_metric) {
|
||
leadSpeedUnit = tr("kph");
|
||
speedConversion = MS_TO_KPH;
|
||
} else {
|
||
accelerationUnit = tr(" ft/s²");
|
||
leadDistanceUnit = tr("feet");
|
||
leadSpeedUnit = tr("mph");
|
||
|
||
accelerationConversion = METER_TO_FOOT;
|
||
distanceConversion = METER_TO_FOOT;
|
||
speedConversion = MS_TO_MPH;
|
||
}
|
||
}
|
||
|
||
double acceleration = std::round(scene.acceleration * 100) / 100;
|
||
int randomEvent = scene.current_random_event;
|
||
|
||
if (acceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) {
|
||
maxAcceleration = acceleration;
|
||
isFiveSecondsPassed = false;
|
||
timer.start();
|
||
} else if (randomEvent == 2 && maxAcceleration < 3.0) {
|
||
maxAcceleration = 3.0;
|
||
isFiveSecondsPassed = false;
|
||
timer.start();
|
||
} else if (randomEvent == 3 && maxAcceleration < 3.5) {
|
||
maxAcceleration = 3.5;
|
||
isFiveSecondsPassed = false;
|
||
timer.start();
|
||
} else if (randomEvent == 4 && maxAcceleration < 4.0) {
|
||
maxAcceleration = 4.0;
|
||
isFiveSecondsPassed = false;
|
||
timer.start();
|
||
} else {
|
||
isFiveSecondsPassed = timer.hasExpired(maxAccelDuration);
|
||
}
|
||
|
||
auto createText = [&](const QString &title, const double data) {
|
||
return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit;
|
||
};
|
||
|
||
QString accelText = QString(tr("Accel: %1%2"))
|
||
.arg(acceleration * accelerationConversion, 0, 'f', 2)
|
||
.arg(accelerationUnit);
|
||
|
||
QString maxAccSuffix;
|
||
|
||
maxAccSuffix = tr(" - Max: %1%2")
|
||
.arg(maxAcceleration * accelerationConversion, 0, 'f', 2)
|
||
.arg(accelerationUnit);
|
||
|
||
|
||
QString obstacleText = createText(tr(" | Obstacle Factor: "), obstacleDistance);
|
||
QString stopText = createText(tr(" - Stop Factor: "), scene.stopped_equivalence);
|
||
QString followText = " = " + createText(tr("Follow Distance: "), scene.desired_follow);
|
||
|
||
auto createDiffText = [&](const double data, const double stockData) {
|
||
double difference = std::round((data - stockData) * distanceConversion);
|
||
return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString();
|
||
};
|
||
|
||
p.save();
|
||
|
||
QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100);
|
||
p.setBrush(QColor(0, 0, 0, 150));
|
||
p.drawRoundedRect(insightsRect, 30, 30);
|
||
p.setFont(InterFont(28, QFont::Bold));
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
|
||
QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27);
|
||
int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent();
|
||
|
||
int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText)
|
||
+ p.fontMetrics().horizontalAdvance(maxAccSuffix)
|
||
+ p.fontMetrics().horizontalAdvance(obstacleText)
|
||
+ p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock))
|
||
+ p.fontMetrics().horizontalAdvance(stopText)
|
||
+ p.fontMetrics().horizontalAdvance(followText);
|
||
|
||
int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2;
|
||
|
||
auto drawText = [&](const QString &text, const QColor &color) {
|
||
p.setPen(color);
|
||
p.drawText(textStartPos, textBaseLine, text);
|
||
textStartPos += p.fontMetrics().horizontalAdvance(text);
|
||
};
|
||
|
||
drawText(accelText, Qt::white);
|
||
if (!maxAccSuffix.isEmpty()) {
|
||
drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : redColor());
|
||
}
|
||
drawText(obstacleText, Qt::white);
|
||
drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red);
|
||
drawText(stopText, Qt::white);
|
||
drawText(followText, Qt::white);
|
||
|
||
p.restore();
|
||
}
|
||
|
||
|
||
void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
|
||
p.save();
|
||
|
||
QSize size = this->size();
|
||
|
||
QRect leftRect(0, 0, size.width() / 2, size.height());
|
||
QRect rightRect = leftRect.translated(size.width() / 2, 0);
|
||
|
||
p.setOpacity(0.5);
|
||
p.fillRect(leftRect, rightHandDM ? redColor() : greenColor());
|
||
p.fillRect(rightRect, rightHandDM ? greenColor() : redColor());
|
||
p.setOpacity(1.0);
|
||
|
||
p.setFont(InterFont(75, QFont::Bold));
|
||
p.setPen(Qt::white);
|
||
|
||
QString unitText = is_metric ? tr("kph") : tr("mph");
|
||
QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
|
||
QString confirmText = tr("Confirm speed limit\n") + speedText;
|
||
QString ignoreText = tr("Ignore speed limit\n") + speedText;
|
||
|
||
QRect textLeftRect = QRect(leftRect.left(), leftRect.top() + leftRect.height() / 2 - 225, leftRect.width(), leftRect.height() / 2);
|
||
QRect textRightRect = QRect(rightRect.left(), rightRect.top() + rightRect.height() / 2 - 225, rightRect.width(), rightRect.height() / 2);
|
||
|
||
p.drawText(textLeftRect, Qt::AlignCenter, rightHandDM ? ignoreText : confirmText);
|
||
p.drawText(textRightRect, Qt::AlignCenter, rightHandDM ? confirmText : ignoreText);
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
|
||
p.save();
|
||
|
||
static bool displayStatusText = false;
|
||
|
||
constexpr qreal fadeDuration = 1500.0;
|
||
constexpr qreal textDuration = 5000.0;
|
||
|
||
static QElapsedTimer timer;
|
||
static QString lastShownStatus;
|
||
|
||
QString newStatus;
|
||
|
||
QRect currentRect = rect();
|
||
QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100);
|
||
|
||
p.setBrush(QColor(0, 0, 0, 150));
|
||
p.setOpacity(1.0);
|
||
p.drawRoundedRect(statusBarRect, 30, 30);
|
||
|
||
std::map<int, QString> conditionalStatusMap = {
|
||
{0, tr("Conditional Experimental Mode ready")},
|
||
{1, tr("Conditional Experimental overridden")},
|
||
{2, tr("Experimental Mode manually activated")},
|
||
{3, tr("Conditional Experimental overridden")},
|
||
{4, tr("Experimental Mode manually activated")},
|
||
{5, tr("Conditional Experimental overridden")},
|
||
{6, tr("Experimental Mode manually activated")},
|
||
{7, tr("Experimental Mode activated for") + (tr(" upcoming intersection"))},
|
||
{8, tr("Experimental Mode activated for") + (tr(" upcoming turn"))},
|
||
{9, tr("Experimental Mode activated due to") + (tr(" no speed limit set"))},
|
||
{10, tr("Experimental Mode activated due to") + (tr(" speed being less than ") + QString::number(scene.conditional_speed_lead) + (is_metric ? tr(" kph") : tr(" mph")))},
|
||
{11, tr("Experimental Mode activated due to") + (tr(" speed being less than ") + QString::number(scene.conditional_speed) + (is_metric ? tr(" kph") : tr(" mph")))},
|
||
{12, tr("Experimental Mode activated for slower lead")},
|
||
{13, tr("Experimental Mode activated for turn") + (tr(" / lane change"))},
|
||
{14, tr("Experimental Mode activated for curve")},
|
||
{15, tr("Experimental Mode activated for stop") + (tr(" sign / stop light"))},
|
||
};
|
||
|
||
QString roadName = roadNameUI ? QString::fromStdString(paramsMemory.get("RoadName")) : QString();
|
||
|
||
// CLEARPILOT: telemetry status bar — show live stats when telemetry enabled
|
||
if (paramsMemory.getBool("TelemetryEnabled")) {
|
||
SubMaster &sm = *(uiState()->sm);
|
||
auto deviceState = sm["deviceState"].getDeviceState();
|
||
int maxTempC = deviceState.getMaxTempC();
|
||
int fanPct = deviceState.getFanSpeedPercentDesired();
|
||
bool standstill = sm["carState"].getCarState().getStandstill();
|
||
|
||
static double last_model_status_t = 0;
|
||
static float model_status_fps = 0;
|
||
if (sm.updated("modelV2")) {
|
||
double now = millis_since_boot();
|
||
if (last_model_status_t > 0) {
|
||
double dt = now - last_model_status_t;
|
||
if (dt > 0) model_status_fps = model_status_fps * 0.8 + (1000.0 / dt) * 0.2;
|
||
}
|
||
last_model_status_t = now;
|
||
}
|
||
|
||
newStatus = QString("%1\u00B0C FAN %2% MDL %3")
|
||
.arg(maxTempC).arg(fanPct).arg(model_status_fps, 0, 'f', 0);
|
||
if (standstill) newStatus += " STANDSTILL";
|
||
// CLEARPILOT: suppress "Always On Lateral active" status bar message
|
||
} else if (false && alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {
|
||
newStatus = tr("Always On Lateral active") + (tr(". Press the \"Cruise Control\" button to disable"));
|
||
} else if (showConditionalExperimentalStatusBar) {
|
||
newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
|
||
}
|
||
|
||
QString distanceSuffix = tr(". Long press the \"distance\" button to revert");
|
||
QString lkasSuffix = tr(". Double press the \"LKAS\" button to revert");
|
||
QString screenSuffix = tr(". Double tap the screen to revert");
|
||
|
||
if (!alwaysOnLateralActive && status != STATUS_DISENGAGED && !newStatus.isEmpty()) {
|
||
if (conditionalStatus == 1 || conditionalStatus == 2) {
|
||
newStatus += distanceSuffix;
|
||
} else if (conditionalStatus == 3 || conditionalStatus == 4) {
|
||
newStatus += lkasSuffix;
|
||
} else if (conditionalStatus == 5 || conditionalStatus == 6) {
|
||
newStatus += screenSuffix;
|
||
}
|
||
}
|
||
|
||
if (newStatus != lastShownStatus || roadName.isEmpty()) {
|
||
displayStatusText = true;
|
||
lastShownStatus = newStatus;
|
||
timer.restart();
|
||
} else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) {
|
||
displayStatusText = false;
|
||
}
|
||
|
||
p.setFont(InterFont(40, QFont::Bold));
|
||
p.setPen(Qt::white);
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
|
||
static qreal roadNameOpacity;
|
||
static qreal statusTextOpacity;
|
||
int elapsed = timer.elapsed();
|
||
if (displayStatusText) {
|
||
statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0);
|
||
roadNameOpacity = 1.0 - statusTextOpacity;
|
||
} else {
|
||
roadNameOpacity = qBound(0.0, elapsed / fadeDuration, 1.0);
|
||
statusTextOpacity = 0.0;
|
||
}
|
||
|
||
p.setOpacity(statusTextOpacity);
|
||
QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
|
||
textRect.moveBottom(statusBarRect.bottom() - 50);
|
||
p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
|
||
|
||
if (!roadName.isEmpty()) {
|
||
p.setOpacity(roadNameOpacity);
|
||
textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, roadName);
|
||
textRect.moveBottom(statusBarRect.bottom() - 50);
|
||
p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, roadName);
|
||
}
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawTurnSignals(QPainter &p) {
|
||
constexpr int signalHeight = 480;
|
||
constexpr int signalWidth = 360;
|
||
|
||
p.setRenderHint(QPainter::Antialiasing);
|
||
|
||
int baseYPosition = (height() - signalHeight) / 2 + (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar || roadNameUI ? 225 : 300) - alertSize;
|
||
int leftSignalXPosition = 75 + width() - signalWidth - 300 * (blindSpotLeft ? 0 : animationFrameIndex);
|
||
int rightSignalXPosition = -75 + 300 * (blindSpotRight ? 0 : animationFrameIndex);
|
||
|
||
if (animationFrameIndex < signalImgVector.size()) {
|
||
auto drawSignal = [&](bool signalActivated, int xPosition, bool flip, bool blindspot) {
|
||
if (signalActivated) {
|
||
int uniqueImages = signalImgVector.size() / 4;
|
||
int index = (blindspot ? 2 * uniqueImages : 2 * animationFrameIndex % totalFrames) + (flip ? 1 : 0);
|
||
QPixmap &signal = signalImgVector[index];
|
||
p.drawPixmap(xPosition, baseYPosition, signalWidth, signalHeight, signal);
|
||
}
|
||
};
|
||
|
||
drawSignal(turnSignalLeft, leftSignalXPosition, false, blindSpotLeft);
|
||
drawSignal(turnSignalRight, rightSignalXPosition, true, blindSpotRight);
|
||
}
|
||
}
|