Onroad compass

Added toggle for an onroad compass.

common/params.cc

@@ -100,6 +100,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarParamsPersistent", PERSISTENT},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"Compass", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CurrentBootlog", PERSISTENT},

selfdrive/ui/qt/offroad/frogpilot_settings.cc

@@ -63,6 +63,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(QWidget *parent) : FrogPilotPanel(p
 
   static const std::vector<std::tuple<QString, QString, QString, QString>> toggles = {
     {"FrogTheme", "FrogPilot Theme", "Enable the beloved FrogPilot Theme! Disable toggle to revert back to the stock openpilot theme.", "../assets/images/frog_button_home.png"},
+    {"Compass", "Compass", "Add a compass to the onroad UI that indicates your current driving direction.", "../assets/offroad/icon_compass.png"},
     {"CustomRoadUI", "Custom Road UI", "Customize the road UI to your liking.", "../assets/offroad/icon_road.png"},
     {"NumericalTemp", "Numerical Temperature Gauge", "Replace openpilot's 'GOOD', 'OK', and 'HIGH' temperature statuses with numerical values.\n\nTap the gauge to switch between Celsius and Fahrenheit.", "../assets/offroad/icon_temp.png"},
     {"RotatingWheel", "Rotating Steering Wheel", "The steering wheel in top right corner of the onroad UI rotates alongside your physical steering wheel.", "../assets/offroad/icon_rotate.png"},

selfdrive/ui/qt/onroad.cc

@@ -334,7 +334,10 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
   main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
 
   map_settings_btn = new MapSettingsButton(this);
-  main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight);
+  const bool flip_side = rightHandDM || compass;
+  const bool move_up = false;
+  const bool move_up_top = compass && (!muteDM);
+  main_layout->addWidget(map_settings_btn, 0, (flip_side ? Qt::AlignLeft : Qt::AlignRight) | (move_up ? Qt::AlignCenter : move_up_top ? Qt::AlignTop : Qt::AlignBottom));
 
   dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
 
@@ -348,6 +351,7 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
   }
 
   // FrogPilot images
+  compass_inner_img = loadPixmap("../assets/images/compass_inner.png", {img_size, img_size});
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
 
@@ -420,12 +424,17 @@ 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);
+    const bool flip_side = rightHandDM || compass;
+    const bool move_up = false;
+    const bool move_up_top = compass && (!muteDM);
+    main_layout->setAlignment(map_settings_btn, (flip_side ? Qt::AlignLeft : Qt::AlignRight) | (move_up ? Qt::AlignCenter : move_up_top ? Qt::AlignTop : Qt::AlignBottom));
   }
 
   // FrogPilot properties
+  setProperty("bearingDeg", s.scene.bearing_deg);
   setProperty("blindSpotLeft", s.scene.blind_spot_left);
   setProperty("blindSpotRight", s.scene.blind_spot_right);
+  setProperty("compass", s.scene.compass);
   setProperty("experimentalMode", s.scene.experimental_mode);
   setProperty("frogColors", s.scene.frog_colors);
   setProperty("muteDM", s.scene.mute_dm);
@@ -537,6 +546,11 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
 
   p.restore();
 
+  // Compass
+  if (compass && !hideBottomIcons) {
+    drawCompass(p);
+  }
+
   // Rotating steering wheel
   if (rotatingWheel) {
     const auto &scene = uiState()->scene;
@@ -897,6 +911,94 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
 
 // FrogPilot widgets
 
+void AnnotatedCameraWidget::drawCompass(QPainter &p) {
+  p.save();
+
+  // Variable declarations
+  constexpr int circle_size = 250;
+  constexpr int circle_offset = circle_size / 2;
+  constexpr int degreeLabelOffset = circle_offset + 25;
+  constexpr int inner_compass = btn_size / 2;
+  int x = !rightHandDM ? rect().right() - btn_size / 2 - (UI_BORDER_SIZE * 2) - 10 : btn_size / 2 + (UI_BORDER_SIZE * 2) + 10;
+  const int y = rect().bottom() - 20 - 140;
+
+  // Enable Antialiasing
+  p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing);
+
+  // Configure the circles
+  p.setPen(QPen(Qt::white, 2));
+  const auto drawCircle = [&](const int offset, const QBrush brush = Qt::NoBrush) {
+    p.setOpacity(1.0);
+    p.setBrush(brush);
+    p.drawEllipse(x - offset, y - offset, offset * 2, offset * 2);
+  };
+
+  // Draw the circle background and white inner circle
+  drawCircle(circle_offset, blackColor(100));
+
+  // Rotate and draw the compass_inner_img image
+  p.save();
+  p.translate(x, y);
+  p.rotate(bearingDeg);
+  p.drawPixmap(-compass_inner_img.width() / 2, -compass_inner_img.height() / 2, compass_inner_img);
+  p.restore();
+
+  // Draw the cardinal directions
+  const auto drawDirection = [&](const QString &text, const int from, const int to, const int align) {
+    // Move the "E" and "W" directions a bit closer to the middle so they're more uniform
+    const int offset = (text == "E") ? -5 : ((text == "W") ? 5 : 0);
+    // Set the opacity based on whether the direction label is currently being pointed at
+    p.setOpacity((bearingDeg >= from && bearingDeg < to) ? 1.0 : 0.2);
+    p.drawText(QRect(x - inner_compass + offset, y - inner_compass, btn_size, btn_size), align, text);
+  };
+  p.setFont(InterFont(25, QFont::Bold));
+  drawDirection("N", 0, 68, Qt::AlignTop | Qt::AlignHCenter);
+  drawDirection("E", 23, 158, Qt::AlignRight | Qt::AlignVCenter);
+  drawDirection("S", 113, 248, Qt::AlignBottom | Qt::AlignHCenter);
+  drawDirection("W", 203, 338, Qt::AlignLeft | Qt::AlignVCenter);
+  drawDirection("N", 293, 360, Qt::AlignTop | Qt::AlignHCenter);
+
+  // Draw the white circle outlining the cardinal directions
+  drawCircle(inner_compass + 5);
+
+  // Draw the white circle outlining the bearing degrees
+  drawCircle(degreeLabelOffset);
+
+  // Draw the black background for the bearing degrees
+  QPainterPath outerCircle, innerCircle;
+  outerCircle.addEllipse(x - degreeLabelOffset, y - degreeLabelOffset, degreeLabelOffset * 2, degreeLabelOffset * 2);
+  innerCircle.addEllipse(x - circle_offset, y - circle_offset, circle_size, circle_size);
+  p.setOpacity(1.0);
+  p.fillPath(outerCircle.subtracted(innerCircle), Qt::black);
+
+  // Draw the degree lines and bearing degrees
+  const auto drawCompassElements = [&](const int angle) {
+    const bool isCardinalDirection = angle % 90 == 0;
+    const int lineLength = isCardinalDirection ? 15 : 10;
+    const bool isBold = abs(angle - static_cast<int>(bearingDeg)) <= 7;
+
+    // Set the current bearing degree value to bold
+    p.setFont(QFont("Inter", 8, isBold ? QFont::Bold : QFont::Normal));
+    p.setPen(QPen(Qt::white, isCardinalDirection ? 3 : 1));
+
+    // Place the elements in their respective spots around their circles
+    p.save();
+    p.translate(x, y);
+    p.rotate(angle);
+    p.drawLine(0, -(circle_size / 2 - lineLength), 0, -(circle_size / 2));
+    p.translate(0, -(circle_size / 2 + 12));
+    p.rotate(-angle);
+    p.drawText(QRect(-20, -10, 40, 20), Qt::AlignCenter, QString::number(angle));
+    p.restore();
+  };
+
+  for (int i = 0; i < 360; i += 15) {
+    drawCompassElements(i);
+  }
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.save();
 

selfdrive/ui/qt/onroad.h

@@ -93,10 +93,12 @@ class AnnotatedCameraWidget : public CameraWidget {
   // FrogPilot properties
   Q_PROPERTY(bool blindSpotLeft MEMBER blindSpotLeft);
   Q_PROPERTY(bool blindSpotRight MEMBER blindSpotRight);
+  Q_PROPERTY(bool compass MEMBER compass);
   Q_PROPERTY(bool experimentalMode MEMBER experimentalMode);
   Q_PROPERTY(bool frogColors MEMBER frogColors);
   Q_PROPERTY(bool muteDM MEMBER muteDM);
   Q_PROPERTY(bool rotatingWheel MEMBER rotatingWheel);
+  Q_PROPERTY(int bearingDeg MEMBER bearingDeg);
   Q_PROPERTY(int steeringAngleDeg MEMBER steeringAngleDeg);
   Q_PROPERTY(int steeringWheel MEMBER steeringWheel);
 
@@ -110,6 +112,7 @@ class AnnotatedCameraWidget : public CameraWidget {
   void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
 
   // FrogPilot widgets
+  void drawCompass(QPainter &p);
   void drawStatusBar(QPainter &p);
 
   QVBoxLayout *main_layout;
@@ -137,12 +140,15 @@ class AnnotatedCameraWidget : public CameraWidget {
   // FrogPilot variables
   bool blindSpotLeft;
   bool blindSpotRight;
+  bool compass;
   bool experimentalMode;
   bool frogColors;
   bool muteDM;
   bool rotatingWheel;
+  int bearingDeg;
   int steeringAngleDeg;
   int steeringWheel;
+  QPixmap compass_inner_img;
   QPixmap engage_img;
   QPixmap experimental_img;
   std::map<int, QPixmap> wheel_images;

selfdrive/ui/ui.cc

@@ -234,6 +234,9 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("gpsLocationExternal")) {
     const auto gpsLocationExternal = sm["gpsLocationExternal"].getGpsLocationExternal();
+    if (scene.compass) {
+      scene.bearing_deg = gpsLocationExternal.getBearingDeg();
+    }
   }
   if (sm.updated("lateralPlan")) {
     const auto lateralPlan = sm["lateralPlan"].getLateralPlan();
@@ -267,6 +270,7 @@ void ui_update_params(UIState *s) {
     scene.default_params_set = params.getBool("DefaultParamsSet");
   }
   if (!toggles_checked && scene.default_params_set) {
+    scene.compass = params.getBool("Compass");
     scene.custom_road_ui = params.getBool("CustomRoadUI");
     scene.blind_spot_path = scene.custom_road_ui && params.getBool("BlindSpotPath");
     scene.frog_theme = params.getBool("FrogTheme");

selfdrive/ui/ui.h

@@ -147,6 +147,7 @@ typedef struct UIScene {
   bool blind_spot_left;
   bool blind_spot_path;
   bool blind_spot_right;
+  bool compass;
   bool custom_road_ui;
   bool default_params_set;
   bool enabled;
@@ -165,6 +166,7 @@ typedef struct UIScene {
   float path_edge_width;
   float path_width;
   float road_edge_width;
+  int bearing_deg;
   int screen_brightness;
   int steering_angle_deg;
   int steering_wheel;