diff --git a/common/params.cc b/common/params.cc index 7c12037..0ce6e54 100644 --- a/common/params.cc +++ b/common/params.cc @@ -239,6 +239,7 @@ std::unordered_map keys = { {"CEStatus", PERSISTENT}, {"CEStopLights", PERSISTENT}, {"CEStopLightsLead", PERSISTENT}, + {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", 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 a3f8e64..755ddac 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -10,6 +10,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"CustomSounds", "Sound Pack", "Switch out the standard openpilot sounds with a set of custom sounds.\n\nWant to submit your own sound pack? Post it in the 'feature-request' channel in the FrogPilot Discord!", ""}, {"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"}, {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, {"AdjacentPath", "Adjacent Paths", "Display paths to the left and right of your car, visualizing where the model detects lanes.", ""}, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 5dfa020..824a9d8 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -477,8 +477,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)); } } @@ -1050,6 +1050,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); @@ -1090,6 +1093,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { blindSpotLeft = scene.blind_spot_left; blindSpotRight = scene.blind_spot_right; cameraView = scene.camera_view; + compass = scene.compass; conditionalExperimental = scene.conditional_experimental; conditionalSpeed = scene.conditional_speed; conditionalSpeedLead = scene.conditional_speed_lead; @@ -1130,9 +1134,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } } + const bool enableCompass = compass && !hideBottomIcons; + compass_img->setVisible(enableCompass); + if (enableCompass) { + if (bearingDeg != scene.bearing_deg) { + bearingDeg = scene.bearing_deg; + compass_img->updateState(bearingDeg); + } + 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); } @@ -1163,6 +1177,114 @@ 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)); + + initializeStaticElements(); +} + +void Compass::updateState(int bearing_deg) { + bearingDeg = bearing_deg; + update(); +} + +void Compass::initializeStaticElements() { + 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::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + // 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 dynamic bearing degree numbers and lines + QFont font = InterFont(10, QFont::Normal); + for (int i = 0; i < 360; i += 15) { + bool isBold = abs(i - static_cast(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)); + QString directions[] = {"N", "E", "S", "W", "N"}; + int fromAngles[] = {337, 68, 158, 248, 337}; + int toAngles[] = {22, 112, 202, 292, 360}; + int alignmentFlags[] = {Qt::AlignTop | Qt::AlignHCenter, Qt::AlignRight | Qt::AlignVCenter, Qt::AlignBottom | Qt::AlignHCenter, Qt::AlignLeft | Qt::AlignVCenter, Qt::AlignTop | Qt::AlignHCenter}; + int directionOffset = 20; + + for (int i = 0; i < 5; ++i) { + int offset = (directions[i] == "E") ? -5 : (directions[i] == "W" ? 5 : 0); + p.setOpacity((bearingDeg >= fromAngles[i] && bearingDeg < toAngles[i]) ? 1.0 : 0.2); + QRect textRect(x - innerCompass + offset + directionOffset, y - innerCompass + directionOffset, innerCompass * 2 - 2 * directionOffset, innerCompass * 2 - 2 * directionOffset); + p.drawText(textRect, alignmentFlags[i], directions[i]); + } +} + void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { SubMaster &sm = *uiState()->sm; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index c3e1fc5..7844d93 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -39,6 +39,28 @@ private: UIScene &scene; }; +class Compass : public QWidget { +public: + explicit Compass(QWidget *parent = nullptr); + + void initializeStaticElements(); + 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 @@ -126,6 +148,8 @@ private: UIScene &scene; + Compass *compass_img; + QHBoxLayout *bottom_layout; bool accelerationPath; @@ -133,6 +157,7 @@ private: bool alwaysOnLateral; bool blindSpotLeft; bool blindSpotRight; + bool compass; bool conditionalExperimental; bool experimentalMode; bool leadInfo; @@ -146,6 +171,7 @@ private: float cruiseAdjustment; float laneWidthLeft; float laneWidthRight; + int bearingDeg; int cameraView; int conditionalSpeed; int conditionalSpeedLead; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2e04a43..febda44 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -256,6 +256,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(); @@ -281,6 +287,7 @@ void ui_update_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 973d484..59ef4e1 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -176,6 +176,7 @@ typedef struct UIScene { bool blind_spot_left; bool blind_spot_path; bool blind_spot_right; + bool compass; bool conditional_experimental; bool custom_onroad_ui; bool custom_theme; @@ -200,6 +201,7 @@ typedef struct UIScene { float path_edge_width; float path_width; float road_edge_width; + int bearing_deg; int camera_view; int conditional_speed; int conditional_speed_lead;