Visuals - Custom Onroad UI - Compass

Add a compass to the onroad UI.

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -79,8 +79,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 && !compass || !rightHandDM && compass ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom);
   }
 }
 
@@ -513,6 +513,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, 0, Qt::AlignBottom | Qt::AlignRight);
+
   map_settings_btn_bottom = new MapSettingsButton(this);
   bottom_layout->addWidget(map_settings_btn_bottom, 0, Qt::AlignBottom | Qt::AlignRight);
 
@@ -546,6 +549,14 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
     drawStatusBar(painter);
   }
 
+  compass = scene.compass;
+  bool enableCompass = compass && !hideBottomIcons;
+  compass_img->setVisible(enableCompass);
+  if (enableCompass) {
+    compass_img->updateState(scene);
+    bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight));
+  }
+
   conditionalSpeed = scene.conditional_speed;
   conditionalSpeedLead = scene.conditional_speed_lead;
   conditionalStatus = scene.conditional_status;
@@ -560,7 +571,7 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
   mapOpen = scene.map_open;
   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) | Qt::AlignBottom);
   }
 
@@ -588,6 +599,103 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
   trafficModeActive = scene.traffic_mode_active;
 }
 
+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::initializeStaticElements() {
+  staticElements = QPixmap(size());
+  staticElements.fill(Qt::transparent);
+  QPainter p(&staticElements);
+
+  p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing);
+  p.setPen(QPen(Qt::white, 2));
+  p.setBrush(QColor(0, 0, 0, 100));
+
+  const int xOffset = x - circleOffset;
+  const int yOffset = y - circleOffset;
+
+  p.drawEllipse(xOffset, yOffset, compassSize, compassSize);
+  p.setBrush(Qt::NoBrush);
+  const int innerOffset = innerCompass + 5;
+  p.drawEllipse(x - innerOffset, y - innerOffset, innerOffset * 2, innerOffset * 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(xOffset, yOffset, compassSize, compassSize);
+  p.fillPath(outerCircle.subtracted(innerCircle), Qt::black);
+}
+
+void Compass::updateState(const UIScene &scene) {
+  if (bearingDeg != scene.bearing_deg) {
+    bearingDeg = (scene.bearing_deg + 360) % 360;
+    update();
+  }
+}
+
+void Compass::paintEvent(QPaintEvent *event) {
+  QPainter p(this);
+  p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing);
+  p.drawPixmap(0, 0, staticElements);
+  p.translate(x, y);
+  p.rotate(bearingDeg);
+  p.drawPixmap(-compassInnerImg.width() / 2, -compassInnerImg.height() / 2, compassInnerImg);
+  p.resetTransform();
+
+  QFont font = InterFont(10, QFont::Normal);
+  const int halfCompassSize = compassSize / 2;
+  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);
+    int lineLength = i % 90 == 0 ? 12 : 8;
+    p.drawLine(0, -(halfCompassSize - lineLength), 0, -halfCompassSize);
+    p.translate(0, -(halfCompassSize + 12));
+    p.rotate(-i);
+    p.drawText(QRect(-20, -10, 40, 20), Qt::AlignCenter, QString::number(i));
+    p.restore();
+  }
+
+  p.setFont(InterFont(20, QFont::Bold));
+  const 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}}
+  };
+  const int directionOffset = 20;
+
+  for (const auto &[direction, params] : directionInfo) {
+    const auto &[range, alignmentFlag, color] = params;
+    const auto &[minRange, maxRange] = range;
+
+    bool isInRange = (minRange > maxRange) ? (bearingDeg >= minRange || bearingDeg <= maxRange) : (bearingDeg >= minRange && bearingDeg <= maxRange);
+
+    QRect textRect(x - innerCompass + directionOffset, y - innerCompass + directionOffset, innerCompass * 2 - 2 * directionOffset, innerCompass * 2 - 2 * directionOffset);
+
+    p.setOpacity(isInRange ? 1.0 : 0.2);
+    p.setPen(QPen(color));
+    p.drawText(textRect, alignmentFlag, direction);
+  }
+}
+
 void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
   p.save();
 

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -6,6 +6,29 @@
 #include "selfdrive/ui/qt/onroad/buttons.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
+class Compass : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit Compass(QWidget *parent = 0);
+  void updateState(const UIScene &scene);
+
+private:
+  void initializeStaticElements();
+  void paintEvent(QPaintEvent *event) override;
+
+  int bearingDeg;
+  int circleOffset;
+  int compassSize;
+  int degreeLabelOffset;
+  int innerCompass;
+  int x;
+  int y;
+
+  QPixmap compassInnerImg;
+  QPixmap staticElements;
+};
+
 class AnnotatedCameraWidget : public CameraWidget {
   Q_OBJECT
 
@@ -51,11 +74,13 @@ private:
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
 
+  Compass *compass_img;
   DistanceButton *distance_btn;
 
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateralActive;
+  bool compass;
   bool experimentalMode;
   bool mapOpen;
   bool onroadDistanceButton;

selfdrive/ui/ui.cc

@@ -247,6 +247,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 liveTorqueParameters = sm["liveTorqueParameters"].getLiveTorqueParameters();
@@ -294,6 +298,9 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   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

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