Onroad compass

Added toggle for a compass in the onroad UI.

common/params.cc

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

selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc

@@ -20,6 +20,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"LoudBlindspotAlert", tr("Loud Blindspot Alert"), tr("Enable a louder alert for when a vehicle is detected in the blindspot when attempting to change lanes."), ""},
 
     {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"},
+    {"Compass", tr("Compass"), tr("Add a compass to the onroad UI."), ""},
     {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""},
     {"FPSCounter", tr("FPS Counter"), tr("Display the 'Frames Per Second' (FPS) of your onroad UI for monitoring system performance."), ""},
     {"LeadInfo", tr("Lead Info and Logics"), tr("Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance."), ""},

selfdrive/frogpilot/ui/qt/offroad/visual_settings.h

@@ -25,7 +25,7 @@ private:
 
   std::set<QString> alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"};
   std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert"};
-  std::set<QString> customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"};
+  std::set<QString> customOnroadUIKeys = {"Compass", "CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds", "HolidayThemes"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
   std::set<QString> qolKeys = {"BigMap", "CameraView", "DriverCamera", "FullMap", "HideSpeed", "NumericalTemp"};

selfdrive/ui/qt/onroad.cc

@@ -489,8 +489,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) | Qt::AlignTop);
   }
 
   updateFrogPilotWidgets();
@@ -1058,6 +1058,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);
 
@@ -1128,6 +1131,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   cameraView = scene.camera_view;
 
+  compass = scene.compass;
+
   conditionalStatus = scene.conditional_status;
   showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
 
@@ -1208,13 +1213,126 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
     drawLeadInfo(p);
   }
 
+  bool enableCompass = compass && !hideBottomIcons;
+  compass_img->setVisible(enableCompass);
+  if (enableCompass) {
+    compass_img->updateState();
+    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);
   }
 }
 
+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);
+  }
+}
+
 void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
   static QElapsedTimer timer;
   static bool isFiveSecondsPassed = false;

selfdrive/ui/qt/onroad.h

@@ -35,6 +35,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 ExperimentalButton : public QPushButton {
   Q_OBJECT
 
@@ -121,12 +145,15 @@ private:
   Params paramsMemory{"/dev/shm/params"};
   UIScene &scene;
 
+  Compass *compass_img;
+
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateralActive;
   bool bigMapOpen;
   bool blindSpotLeft;
   bool blindSpotRight;
+  bool compass;
   bool experimentalMode;
   bool leadInfo;
   bool mapOpen;

selfdrive/ui/ui.cc

@@ -266,6 +266,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();
@@ -315,6 +319,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.adjacent_path = custom_paths && params.getBool("AdjacentPath");
   scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics");
   scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath");
+  scene.compass = custom_onroad_ui && params.getBool("Compass");
   scene.fps_counter = custom_onroad_ui && params.getBool("FPSCounter");
   scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo");
   scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk");

selfdrive/ui/ui.h

@@ -179,6 +179,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;
@@ -230,6 +231,7 @@ typedef struct UIScene {
   float road_edge_width;
 
   int alert_size;
+  int bearing_deg;
   int camera_view;
   int conditional_speed;
   int conditional_speed_lead;