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 000000000..d6517758c Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/compass_inner.png differ diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c79928014..a1364da52 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -79,8 +79,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 && !compass || !rightHandDM && compass ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom); } } @@ -513,6 +513,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, 0, Qt::AlignBottom | Qt::AlignRight); + map_settings_btn_bottom = new MapSettingsButton(this); bottom_layout->addWidget(map_settings_btn_bottom, 0, Qt::AlignBottom | Qt::AlignRight); @@ -546,6 +549,14 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce drawStatusBar(painter); } + compass = scene.compass; + bool enableCompass = compass && !hideBottomIcons; + compass_img->setVisible(enableCompass); + if (enableCompass) { + compass_img->updateState(scene); + bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight)); + } + conditionalSpeed = scene.conditional_speed; conditionalSpeedLead = scene.conditional_speed_lead; conditionalStatus = scene.conditional_status; @@ -560,7 +571,7 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce mapOpen = scene.map_open; 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) | Qt::AlignBottom); } @@ -588,6 +599,103 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce trafficModeActive = scene.traffic_mode_active; } +Compass::Compass(QWidget *parent) : QWidget(parent) { + 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)); + initializeStaticElements(); +} + +void Compass::initializeStaticElements() { + staticElements = QPixmap(size()); + staticElements.fill(Qt::transparent); + QPainter p(&staticElements); + + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + p.setPen(QPen(Qt::white, 2)); + p.setBrush(QColor(0, 0, 0, 100)); + + const int xOffset = x - circleOffset; + const int yOffset = y - circleOffset; + + p.drawEllipse(xOffset, yOffset, compassSize, compassSize); + p.setBrush(Qt::NoBrush); + const int innerOffset = innerCompass + 5; + p.drawEllipse(x - innerOffset, y - innerOffset, innerOffset * 2, innerOffset * 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(xOffset, yOffset, compassSize, compassSize); + p.fillPath(outerCircle.subtracted(innerCircle), Qt::black); +} + +void Compass::updateState(const UIScene &scene) { + if (bearingDeg != scene.bearing_deg) { + bearingDeg = (scene.bearing_deg + 360) % 360; + update(); + } +} + +void Compass::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + p.drawPixmap(0, 0, staticElements); + p.translate(x, y); + p.rotate(bearingDeg); + p.drawPixmap(-compassInnerImg.width() / 2, -compassInnerImg.height() / 2, compassInnerImg); + p.resetTransform(); + + QFont font = InterFont(10, QFont::Normal); + const int halfCompassSize = compassSize / 2; + 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); + int lineLength = i % 90 == 0 ? 12 : 8; + p.drawLine(0, -(halfCompassSize - lineLength), 0, -halfCompassSize); + p.translate(0, -(halfCompassSize + 12)); + p.rotate(-i); + p.drawText(QRect(-20, -10, 40, 20), Qt::AlignCenter, QString::number(i)); + p.restore(); + } + + p.setFont(InterFont(20, QFont::Bold)); + const 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}} + }; + const int directionOffset = 20; + + for (const auto &[direction, params] : directionInfo) { + const auto &[range, alignmentFlag, color] = params; + const auto &[minRange, maxRange] = range; + + bool isInRange = (minRange > maxRange) ? (bearingDeg >= minRange || bearingDeg <= maxRange) : (bearingDeg >= minRange && bearingDeg <= maxRange); + + QRect textRect(x - innerCompass + directionOffset, y - innerCompass + directionOffset, innerCompass * 2 - 2 * directionOffset, innerCompass * 2 - 2 * directionOffset); + + p.setOpacity(isInRange ? 1.0 : 0.2); + p.setPen(QPen(color)); + p.drawText(textRect, alignmentFlag, direction); + } +} + void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index be39dcb98..5297769c4 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -6,6 +6,29 @@ #include "selfdrive/ui/qt/onroad/buttons.h" #include "selfdrive/ui/qt/widgets/cameraview.h" +class Compass : public QWidget { + Q_OBJECT + +public: + explicit Compass(QWidget *parent = 0); + void updateState(const UIScene &scene); + +private: + void initializeStaticElements(); + void paintEvent(QPaintEvent *event) override; + + int bearingDeg; + int circleOffset; + int compassSize; + int degreeLabelOffset; + int innerCompass; + int x; + int y; + + QPixmap compassInnerImg; + QPixmap staticElements; +}; + class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -51,11 +74,13 @@ private: // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; + 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 a099c797d..fb41cf948 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -247,6 +247,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 liveTorqueParameters = sm["liveTorqueParameters"].getLiveTorqueParameters(); @@ -294,6 +298,9 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { 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 b129e6265..2f6203426 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -123,6 +123,7 @@ typedef struct UIScene { // FrogPilot variables bool always_on_lateral_active; + bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; bool disable_smoothing_vtsc; @@ -160,6 +161,7 @@ typedef struct UIScene { float unconfirmed_speed_limit; int alert_size; + int bearing_deg; int conditional_speed; int conditional_speed_lead; int conditional_status;