Visuals - Custom Onroad UI - Compass

Add a compass to the onroad UI.

selfdrive/ui/qt/onroad.cc

@@ -401,8 +401,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) | Qt::AlignTop);
   }
 
   // Update FrogPilot widgets
@@ -832,6 +832,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);
 
@@ -862,6 +865,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
   alwaysOnLateralActive = scene.always_on_lateral_active;
   showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
 
+  compass = scene.compass;
+
   conditionalStatus = scene.conditional_status;
   showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
 
@@ -893,6 +898,13 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
     drawSLCConfirmation(p);
   }
 
+  bool enableCompass = compass && !hideBottomIcons;
+  compass_img->setVisible(enableCompass);
+  if (enableCompass) {
+    compass_img->updateState();
+    bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight));
+  }
+
   bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons;
   distance_btn->setVisible(enableDistanceButton);
   if (enableDistanceButton) {
@@ -902,11 +914,117 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
 
   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<QString, std::tuple<QPair<float, float>, 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);
+  }
+}
+
 DistanceButton::DistanceButton(QWidget *parent) : QPushButton(parent), scene(uiState()->scene) {
   setFixedSize(btn_size * 1.5, btn_size * 1.5);
 

selfdrive/ui/qt/onroad.h

@@ -34,6 +34,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 DistanceButton : public QPushButton {
 public:
   explicit DistanceButton(QWidget *parent = nullptr);
@@ -143,11 +167,13 @@ private:
   Params paramsMemory{"/dev/shm/params"};
   UIScene &scene;
 
+  Compass *compass_img;
   DistanceButton *distance_btn;
 
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateralActive;
+  bool compass;
   bool experimentalMode;
   bool mapOpen;
   bool onroadDistanceButton;

selfdrive/ui/ui.cc

@@ -248,6 +248,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();
@@ -284,6 +288,9 @@ void ui_update_frogpilot_params(UIState *s) {
   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");
 

selfdrive/ui/ui.h

@@ -187,6 +187,7 @@ typedef struct UIScene {
 
   // FrogPilot variables
   bool always_on_lateral_active;
+  bool compass;
   bool conditional_experimental;
   bool disable_smoothing_mtsc;
   bool disable_smoothing_vtsc;
@@ -222,6 +223,7 @@ typedef struct UIScene {
   float unconfirmed_speed_limit;
 
   int alert_size;
+  int bearing_deg;
   int conditional_speed;
   int conditional_speed_lead;
   int conditional_status;