diff --git a/common/params.cc b/common/params.cc index c94d9ee..41fd5a3 100644 --- a/common/params.cc +++ b/common/params.cc @@ -240,6 +240,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 0000000..aa2d451 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/compass_inner.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_compass.png b/selfdrive/frogpilot/assets/toggle_icons/icon_compass.png new file mode 100644 index 0000000..dd241e0 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_compass.png differ diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 5b9a3a2..27193fd 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -19,6 +19,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"WarningImmediateVolume", "Warning Immediate Volume", "Related alerts:\n\nDISENGAGE IMMEDIATELY, Driver Distracted\nDISENGAGE IMMEDIATELY, Driver Unresponsive", ""}, {"CameraView", "Camera View", "Choose your preferred camera view for the onroad UI. This is a visual change only and doesn't impact openpilot.", "../frogpilot/assets/toggle_icons/icon_camera.png"}, + {"Compass", "Compass", "Add a compass to your onroad UI.", "../frogpilot/assets/toggle_icons/icon_compass.png"}, {"CustomAlerts", "Custom Alerts", "Enable custom alerts for various logic or situational changes.", "../frogpilot/assets/toggle_icons/icon_green_light.png"}, {"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", ""}, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 65f57fe..df9eb76 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -459,8 +459,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) | (compass ? Qt::AlignTop : Qt::AlignBottom)); } } @@ -1039,6 +1039,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); @@ -1110,6 +1113,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { cameraView = scene.camera_view; + compass = scene.compass; + conditionalExperimental = scene.conditional_experimental; conditionalSpeed = scene.conditional_speed; conditionalSpeedLead = scene.conditional_speed_lead; @@ -1156,9 +1161,16 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } } + bool enableCompass = compass && !hideBottomIcons; + compass_img->setVisible(enableCompass); + if (enableCompass) { + compass_img->updateState(scene.bearing_deg); + 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); } @@ -1198,6 +1210,133 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } } +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)); + + staticElements = QPixmap(size()); + staticElements.fill(Qt::transparent); + QPainter p(&staticElements); + + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + // Configure the circles + QPen whitePen(Qt::white, 2); + p.setPen(whitePen); + + // Draw the circle background and white inner circle + p.setOpacity(1.0); + p.setBrush(QColor(0, 0, 0, 100)); + p.drawEllipse(x - circleOffset, y - circleOffset, circleOffset * 2, circleOffset * 2); + + // Draw the white circles + 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); + + // Draw the black background for the bearing degrees + 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); + + // Draw the static degree lines + for (int i = 0; i < 360; i += 15) { + bool isCardinalDirection = i % 90 == 0; + int lineLength = isCardinalDirection ? 15 : 10; + p.setPen(QPen(Qt::white, isCardinalDirection ? 3 : 1)); + p.save(); + p.translate(x, y); + p.rotate(i); + p.drawLine(0, -(compassSize / 2 - lineLength), 0, -(compassSize / 2)); + p.restore(); + } +} + +void Compass::updateState(int bearing_deg) { + if (bearingDeg != bearing_deg) { + update(); + bearingDeg = bearing_deg; + } +} + +void Compass::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + // Normalize bearingDeg to be in the range [0, 360) + bearingDeg = fmod(bearingDeg, 360); + if (bearingDeg < 0) { + bearingDeg += 360; + } + + // Draw static elements + p.drawPixmap(0, 0, staticElements); + + // Rotate and draw the compassInnerImg image + p.translate(x, y); + p.rotate(bearingDeg); + p.drawPixmap(-compassInnerImg.width() / 2, -compassInnerImg.height() / 2, compassInnerImg); + p.rotate(-bearingDeg); + p.translate(-x, -y); + + // Draw the bearing degree numbers + 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(); + } + + // Draw cardinal directions + 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) { // Declare the variables static QElapsedTimer timer; @@ -1357,7 +1496,7 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { QString lkasSuffix = ". Double press the \"LKAS\" button to revert"; QString screenSuffix = ". Double tap the screen to revert"; - if (!alwaysOnLateral && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) { + if (!alwaysOnLateralActive && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) { if (conditionalStatus == 1 || conditionalStatus == 2) { newStatus += screenSuffix; } else if (conditionalStatus == 3 || conditionalStatus == 4) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index c35eb04..1d7555d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -36,6 +36,27 @@ private: UIScene &scene; }; +class Compass : public QWidget { +public: + explicit Compass(QWidget *parent = nullptr); + + void updateState(int bearing_deg); + +protected: + void paintEvent(QPaintEvent *event) override; + +private: + 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 +142,15 @@ private: UIScene &scene; + Compass *compass_img; + QHBoxLayout *bottom_layout; bool alwaysOnLateral; bool alwaysOnLateralActive; bool blindSpotLeft; bool blindSpotRight; + bool compass; bool conditionalExperimental; bool experimentalMode; bool fullMapOpen; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index ad5d25b..50017d8 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -259,6 +259,12 @@ static void update_state(UIState *s) { } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); + if (scene.compass) { + auto orientation = liveLocationKalman.getCalibratedOrientationNED(); + if (orientation.getValid()) { + scene.bearing_deg = RAD2DEG(orientation.getValue()[2]); + } + } } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); @@ -288,6 +294,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.always_on_lateral = params.getBool("AlwaysOnLateral"); scene.camera_view = params.getInt("CameraView"); + scene.compass = params.getBool("Compass"); scene.conditional_experimental = params.getBool("ConditionalExperimental"); scene.conditional_speed = params.getInt("CESpeed"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index c2d8833..1bc16ec 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -177,6 +177,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; @@ -210,6 +211,7 @@ typedef struct UIScene { float path_width; float road_edge_width; + int bearing_deg; int camera_view; int conditional_speed; int conditional_speed_lead;