diff --git a/selfdrive/frogpilot/assets/other_images/compass_inner.png b/selfdrive/frogpilot/assets/other_images/compass_inner.png new file mode 100644 index 0000000..aa2d451 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/compass_inner.png differ diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 695ee44..332ea42 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -401,8 +401,8 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { // hide map settings button for alerts and flip for right hand DM if (map_settings_btn->isEnabled()) { - map_settings_btn->setVisible(!hideBottomIcons); - main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom); + map_settings_btn->setVisible(!hideBottomIcons && compass); + main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignTop); } // Update FrogPilot widgets @@ -832,6 +832,9 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); bottom_layout->addItem(spacer); + compass_img = new Compass(this); + bottom_layout->addWidget(compass_img); + map_settings_btn_bottom = new MapSettingsButton(this); bottom_layout->addWidget(map_settings_btn_bottom); @@ -862,6 +865,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { alwaysOnLateralActive = scene.always_on_lateral_active; showAlwaysOnLateralStatusBar = scene.show_aol_status_bar; + compass = scene.compass; + conditionalStatus = scene.conditional_status; showConditionalExperimentalStatusBar = scene.show_cem_status_bar; @@ -893,6 +898,13 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { drawSLCConfirmation(p); } + bool enableCompass = compass && !hideBottomIcons; + compass_img->setVisible(enableCompass); + if (enableCompass) { + compass_img->updateState(); + bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight)); + } + bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons; distance_btn->setVisible(enableDistanceButton); if (enableDistanceButton) { @@ -902,11 +914,117 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { - map_settings_btn_bottom->setVisible(!hideBottomIcons); + map_settings_btn_bottom->setVisible(!hideBottomIcons && !compass); bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight); } } +Compass::Compass(QWidget *parent) : QWidget(parent), scene(uiState()->scene) { + setFixedSize(btn_size * 1.5, btn_size * 1.5); + + compassSize = btn_size; + circleOffset = compassSize / 2; + degreeLabelOffset = circleOffset + 25; + innerCompass = compassSize / 2; + + x = (btn_size * 1.5) / 2 + 20; + y = (btn_size * 1.5) / 2; + + compassInnerImg = loadPixmap("../frogpilot/assets/other_images/compass_inner.png", QSize(compassSize / 1.75, compassSize / 1.75)); + + staticElements = QPixmap(size()); + staticElements.fill(Qt::transparent); + QPainter p(&staticElements); + + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + QPen whitePen(Qt::white, 2); + p.setPen(whitePen); + + p.setOpacity(1.0); + p.setBrush(QColor(0, 0, 0, 100)); + p.drawEllipse(x - circleOffset, y - circleOffset, circleOffset * 2, circleOffset * 2); + + p.setBrush(Qt::NoBrush); + p.drawEllipse(x - (innerCompass + 5), y - (innerCompass + 5), (innerCompass + 5) * 2, (innerCompass + 5) * 2); + p.drawEllipse(x - degreeLabelOffset, y - degreeLabelOffset, degreeLabelOffset * 2, degreeLabelOffset * 2); + + QPainterPath outerCircle, innerCircle; + outerCircle.addEllipse(x - degreeLabelOffset, y - degreeLabelOffset, degreeLabelOffset * 2, degreeLabelOffset * 2); + innerCircle.addEllipse(x - circleOffset, y - circleOffset, compassSize, compassSize); + p.fillPath(outerCircle.subtracted(innerCircle), Qt::black); +} + +void Compass::updateState() { + if (bearingDeg != scene.bearing_deg) { + update(); + bearingDeg = scene.bearing_deg; + } +} + +void Compass::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + bearingDeg = fmod(bearingDeg, 360); + if (bearingDeg < 0) { + bearingDeg += 360; + } + + p.drawPixmap(0, 0, staticElements); + + p.translate(x, y); + p.rotate(bearingDeg); + p.drawPixmap(-compassInnerImg.width() / 2, -compassInnerImg.height() / 2, compassInnerImg); + p.rotate(-bearingDeg); + p.translate(-x, -y); + + QFont font = InterFont(10, QFont::Normal); + for (int i = 0; i < 360; i += 15) { + bool isBold = abs(i - bearingDeg) <= 7; + font.setWeight(isBold ? QFont::Bold : QFont::Normal); + p.setFont(font); + p.setPen(QPen(Qt::white, i % 90 == 0 ? 2 : 1)); + + p.save(); + p.translate(x, y); + p.rotate(i); + p.drawLine(0, -(compassSize / 2 - (i % 90 == 0 ? 12 : 8)), 0, -(compassSize / 2)); + p.translate(0, -(compassSize / 2 + 12)); + p.rotate(-i); + p.drawText(QRect(-20, -10, 40, 20), Qt::AlignCenter, QString::number(i)); + p.restore(); + } + + p.setFont(InterFont(20, QFont::Bold)); + std::map, int, QColor>> directionInfo = { + {"N", {{292.5, 67.5}, Qt::AlignTop | Qt::AlignHCenter, Qt::white}}, + {"E", {{22.5, 157.5}, Qt::AlignRight | Qt::AlignVCenter, Qt::white}}, + {"S", {{112.5, 247.5}, Qt::AlignBottom | Qt::AlignHCenter, Qt::white}}, + {"W", {{202.5, 337.5}, Qt::AlignLeft | Qt::AlignVCenter, Qt::white}} + }; + int directionOffset = 20; + + for (auto &item : directionInfo) { + QString direction = item.first; + auto &[range, alignmentFlag, color] = item.second; + auto &[minRange, maxRange] = range; + + QRect textRect(x - innerCompass + directionOffset, y - innerCompass + directionOffset, innerCompass * 2 - 2 * directionOffset, innerCompass * 2 - 2 * directionOffset); + + bool isInRange = false; + if (minRange > maxRange) { + isInRange = bearingDeg >= minRange || bearingDeg <= maxRange; + } else { + isInRange = bearingDeg >= minRange && bearingDeg <= maxRange; + } + + p.setOpacity(isInRange ? 1.0 : 0.2); + p.setPen(QPen(color)); + p.drawText(textRect, alignmentFlag, direction); + } +} + DistanceButton::DistanceButton(QWidget *parent) : QPushButton(parent), scene(uiState()->scene) { setFixedSize(btn_size * 1.5, btn_size * 1.5); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index d54538d..425dd4a 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -34,6 +34,30 @@ private: UIScene &scene; }; +class Compass : public QWidget { +public: + explicit Compass(QWidget *parent = nullptr); + + void updateState(); + +protected: + void paintEvent(QPaintEvent *event) override; + +private: + UIScene &scene; + + int bearingDeg; + int circleOffset; + int compassSize; + int degreeLabelOffset; + int innerCompass; + int x; + int y; + + QPixmap compassInnerImg; + QPixmap staticElements; +}; + class DistanceButton : public QPushButton { public: explicit DistanceButton(QWidget *parent = nullptr); @@ -143,11 +167,13 @@ private: Params paramsMemory{"/dev/shm/params"}; UIScene &scene; + Compass *compass_img; DistanceButton *distance_btn; QHBoxLayout *bottom_layout; bool alwaysOnLateralActive; + bool compass; bool experimentalMode; bool mapOpen; bool onroadDistanceButton; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 06d7d33..1a6b40d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -248,6 +248,10 @@ static void update_state(UIState *s) { } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); + auto orientation = liveLocationKalman.getCalibratedOrientationNED(); + if (orientation.getValid()) { + scene.bearing_deg = RAD2DEG(orientation.getValue()[2]); + } } if (sm.updated("liveTorqueParameters")) { auto torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters(); @@ -284,6 +288,9 @@ void ui_update_frogpilot_params(UIState *s) { scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0; scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar"); + bool custom_onroad_ui = params.getBool("CustomUI"); + scene.compass = custom_onroad_ui && params.getBool("Compass"); + scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 81776ca..bd97413 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -187,6 +187,7 @@ typedef struct UIScene { // FrogPilot variables bool always_on_lateral_active; + bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; bool disable_smoothing_vtsc; @@ -222,6 +223,7 @@ typedef struct UIScene { float unconfirmed_speed_limit; int alert_size; + int bearing_deg; int conditional_speed; int conditional_speed_lead; int conditional_status;