Onroad compass

Added toggle for an onroad compass.

common/params.cc

@@ -239,6 +239,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CEStatus", PERSISTENT},
     {"CEStopLights", PERSISTENT},
     {"CEStopLightsLead", PERSISTENT},
+    {"Compass", PERSISTENT},
     {"ConditionalExperimental", PERSISTENT},
     {"CustomColors", PERSISTENT},
     {"CustomIcons", PERSISTENT},

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.", ""},

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);
+    map_settings_btn->setVisible(!hideBottomIcons && compass);
-    main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom);
+    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<int>(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;
 

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;

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");

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;