diff --git a/common/params.cc b/common/params.cc index 9d9114658..296069571 100644 --- a/common/params.cc +++ b/common/params.cc @@ -245,6 +245,7 @@ std::unordered_map keys = { {"CEStatus", PERSISTENT}, {"CEStopLights", PERSISTENT}, {"CEStopLightsLead", PERSISTENT}, + {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CurrentHolidayTheme", PERSISTENT}, {"CustomAlerts", PERSISTENT}, 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..aa2d45196 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/compass_inner.png differ diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index b41d0c7c9..391d9d746 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -20,6 +20,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"LoudBlindspotAlert", tr("Loud Blindspot Alert"), tr("Enable a louder alert for when a vehicle is detected in the blindspot when attempting to change lanes."), ""}, {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"}, + {"Compass", tr("Compass"), tr("Add a compass to the onroad UI."), ""}, {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""}, {"FPSCounter", tr("FPS Counter"), tr("Display the 'Frames Per Second' (FPS) of your onroad UI for monitoring system performance."), ""}, {"LeadInfo", tr("Lead Info and Logics"), tr("Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index 171c09cb4..75d93c9c3 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -25,7 +25,7 @@ private: std::set alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"}; std::set customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert"}; - std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"}; + std::set customOnroadUIKeys = {"Compass", "CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds", "HolidayThemes"}; std::set modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"BigMap", "CameraView", "DriverCamera", "FullMap", "HideSpeed", "NumericalTemp"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 69da6859e..fb53589ec 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -489,8 +489,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); } updateFrogPilotWidgets(); @@ -1058,6 +1058,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); @@ -1128,6 +1131,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { cameraView = scene.camera_view; + compass = scene.compass; + conditionalStatus = scene.conditional_status; showConditionalExperimentalStatusBar = scene.show_cem_status_bar; @@ -1208,13 +1213,126 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { drawLeadInfo(p); } + bool enableCompass = compass && !hideBottomIcons; + compass_img->setVisible(enableCompass); + if (enableCompass) { + compass_img->updateState(); + bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight)); + } + 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); + } +} + void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { static QElapsedTimer timer; static bool isFiveSecondsPassed = false; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3b0022591..0e7d44f58 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -35,6 +35,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 ExperimentalButton : public QPushButton { Q_OBJECT @@ -121,12 +145,15 @@ private: Params paramsMemory{"/dev/shm/params"}; UIScene &scene; + Compass *compass_img; + QHBoxLayout *bottom_layout; bool alwaysOnLateralActive; bool bigMapOpen; bool blindSpotLeft; bool blindSpotRight; + bool compass; bool experimentalMode; bool leadInfo; bool mapOpen; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9cb3f2eaa..4c8a43f22 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -266,6 +266,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(); @@ -315,6 +319,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.adjacent_path = custom_paths && params.getBool("AdjacentPath"); scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics"); scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath"); + scene.compass = custom_onroad_ui && params.getBool("Compass"); scene.fps_counter = custom_onroad_ui && params.getBool("FPSCounter"); scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo"); scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 588c2d1ad..a39db7630 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -179,6 +179,7 @@ typedef struct UIScene { bool blind_spot_left; bool blind_spot_path; bool blind_spot_right; + bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; bool driver_camera; @@ -230,6 +231,7 @@ typedef struct UIScene { float road_edge_width; int alert_size; + int bearing_deg; int camera_view; int conditional_speed; int conditional_speed_lead;