FrogPilot setup - Setup UI

selfdrive/ui/qt/offroad/settings.h

@@ -99,4 +99,5 @@ private:
 
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
+  UIScene &scene;
 };

selfdrive/ui/qt/offroad/software_settings.cc

@@ -20,7 +20,7 @@ void SoftwarePanel::checkForUpdates() {
   std::system("pkill -SIGUSR1 -f selfdrive.updated.updated");
 }
 
-SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
+SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent), scene(uiState()->scene) {
   onroadLbl = new QLabel(tr("Updates are only downloaded while the car is off."));
   onroadLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;");
   addItem(onroadLbl);

selfdrive/ui/qt/onroad.cc

@@ -28,7 +28,7 @@ static void drawIcon(QPainter &p, const QPoint &center, const QPixmap &img, cons
   p.setOpacity(1.0);
 }
 
-OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
+OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->scene) {
   QVBoxLayout *main_layout  = new QVBoxLayout(this);
   main_layout->setMargin(UI_BORDER_SIZE);
   QStackedLayout *stacked_layout = new QStackedLayout;
@@ -114,6 +114,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
 
       QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested);
       QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings);
+      QObject::connect(nvg->map_settings_btn_bottom, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings);
       nvg->map_settings_btn->setEnabled(true);
 
       m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE);
@@ -170,11 +171,13 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
   int margin = 40;
   int radius = 30;
+  int offset = true ? 25 : 0;
   if (alert.size == cereal::ControlsState::AlertSize::FULL) {
     margin = 0;
     radius = 0;
+    offset = 0;
   }
-  QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2);
+  QRect r = QRect(0 + margin, height() - h + margin - offset, width() - margin*2, h - margin*2);
 
   QPainter p(this);
 
@@ -215,7 +218,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 }
 
 // ExperimentalButton
-ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) {
+ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) {
   setFixedSize(btn_size, btn_size);
 
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
@@ -250,7 +253,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) {
 
 // MapSettingsButton
 MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) {
-  setFixedSize(btn_size, btn_size);
+  setFixedSize(btn_size, btn_size + 20);
   settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size});
 
   // hidden by default, made visible if map is created (has prime or mapbox token)
@@ -265,7 +268,7 @@ void MapSettingsButton::paintEvent(QPaintEvent *event) {
 
 
 // Window that shows camera view and variety of info drawn on top
-AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) {
+AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent), scene(uiState()->scene) {
   pm = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"uiDebug"});
 
   main_layout = new QVBoxLayout(this);
@@ -279,6 +282,9 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
   main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight);
 
   dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
+
+  // Initialize FrogPilot widgets
+  initializeFrogPilotWidgets();
 }
 
 void AnnotatedCameraWidget::updateState(const UIState &s) {
@@ -331,6 +337,9 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
     map_settings_btn->setVisible(!hideBottomIcons);
     main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom);
   }
+
+  // Update FrogPilot widgets
+  updateFrogPilotWidgets();
 }
 
 void AnnotatedCameraWidget::drawHud(QPainter &p) {
@@ -429,6 +438,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   drawText(p, rect().center().x(), 290, speedUnit, 200);
 
   p.restore();
+
+  // Draw FrogPilot widgets
+  paintFrogPilotWidgets(p);
 }
 
 void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
@@ -471,7 +483,6 @@ void AnnotatedCameraWidget::updateFrameMat() {
 void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   painter.save();
 
-  const UIScene &scene = s->scene;
   SubMaster &sm = *(s->sm);
 
   // lanelines
@@ -488,7 +499,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
 
   // paint path
   QLinearGradient bg(0, height(), 0, 0);
-  if (sm["controlsState"].getControlsState().getExperimentalMode()) {
+  if (experimentalMode) {
     // The first half of track_vertices are the points for the right side of the path
     // and the indices match the positions of accel from uiPlan
     const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel();
@@ -528,13 +539,12 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
 }
 
 void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) {
-  const UIScene &scene = s->scene;
-
   painter.save();
 
   // base icon
   int offset = UI_BORDER_SIZE + btn_size / 2;
   int x = rightHandDM ? width() - offset : offset;
+  offset += true ? 25 : 0;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
   drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
@@ -636,7 +646,7 @@ void AnnotatedCameraWidget::paintGL() {
       } else if (v_ego > 15) {
         wide_cam_requested = false;
       }
-      wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode();
+      wide_cam_requested = wide_cam_requested && experimentalMode;
       // for replay of old routes, never go to widecam
       wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
     }
@@ -704,3 +714,100 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
   ui_update_params(uiState());
   prev_draw_t = millis_since_boot();
 }
+
+// FrogPilot widgets
+void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
+  bottom_layout = new QHBoxLayout();
+
+  QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum);
+  bottom_layout->addItem(spacer);
+
+  map_settings_btn_bottom = new MapSettingsButton(this);
+  bottom_layout->addWidget(map_settings_btn_bottom);
+
+  main_layout->addLayout(bottom_layout);
+}
+
+void AnnotatedCameraWidget::updateFrogPilotWidgets() {
+  if (is_metric) {
+    accelerationUnit = tr(" m/s²");
+    leadDistanceUnit = tr(mapOpen ? "m" : "meters");
+    leadSpeedUnit = tr("kph");
+
+    accelerationConversion = 1.0f;
+    distanceConversion = 1.0f;
+    speedConversion = MS_TO_KPH;
+  } else {
+    accelerationUnit = tr(" ft/s²");
+    leadDistanceUnit = tr(mapOpen ? "ft" : "feet");
+    leadSpeedUnit = tr("mph");
+
+    accelerationConversion = METER_TO_FOOT;
+    distanceConversion = METER_TO_FOOT;
+    speedConversion = MS_TO_MPH;
+  }
+
+  alertSize = scene.alert_size;
+
+  experimentalMode = scene.experimental_mode;
+
+  mapOpen = scene.map_open;
+}
+
+void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
+  if (true) {
+    drawStatusBar(p);
+  }
+
+  map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled());
+  if (map_settings_btn_bottom->isEnabled()) {
+    map_settings_btn_bottom->setVisible(!hideBottomIcons);
+    bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight);
+  }
+}
+
+void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
+  p.save();
+
+  static bool displayStatusText = false;
+
+  constexpr qreal fadeDuration = 1500.0;
+  constexpr qreal textDuration = 5000.0;
+
+  static QElapsedTimer timer;
+  static QString lastShownStatus;
+
+  QString newStatus;
+
+  QRect currentRect = rect();
+  QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100);
+
+  p.setBrush(QColor(0, 0, 0, 150));
+  p.setOpacity(1.0);
+  p.drawRoundedRect(statusBarRect, 30, 30);
+
+  if (newStatus != lastShownStatus) {
+    displayStatusText = true;
+    lastShownStatus = newStatus;
+    timer.restart();
+  } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) {
+    displayStatusText = false;
+  }
+
+  p.setFont(InterFont(40, QFont::Bold));
+  p.setPen(Qt::white);
+  p.setRenderHint(QPainter::TextAntialiasing);
+
+  static qreal statusTextOpacity;
+  int elapsed = timer.elapsed();
+  if (displayStatusText) {
+    statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0);
+  }
+
+  p.setOpacity(statusTextOpacity);
+  QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
+  textRect.moveBottom(statusBarRect.bottom() - 50);
+  p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
+
+  p.restore();
+}

selfdrive/ui/qt/onroad.h

@@ -20,7 +20,7 @@ class OnroadAlerts : public QWidget {
   Q_OBJECT
 
 public:
-  OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {}
+  OnroadAlerts(QWidget *parent = 0) : QWidget(parent), scene(uiState()->scene) {}
   void updateAlert(const Alert &a);
 
 protected:
@@ -29,6 +29,9 @@ protected:
 private:
   QColor bg;
   Alert alert = {};
+
+  // FrogPilot variables
+  UIScene &scene;
 };
 
 class ExperimentalButton : public QPushButton {
@@ -50,6 +53,7 @@ private:
 
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
+  UIScene &scene;
 };
 
 
@@ -74,6 +78,7 @@ public:
   void updateState(const UIState &s);
 
   MapSettingsButton *map_settings_btn;
+  MapSettingsButton *map_settings_btn_bottom;
 
 private:
   void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
@@ -100,8 +105,31 @@ private:
   int skip_frame_count = 0;
   bool wide_cam_requested = false;
 
+  // FrogPilot widgets
+  void initializeFrogPilotWidgets();
+  void paintFrogPilotWidgets(QPainter &p);
+  void updateFrogPilotWidgets();
+
+  void drawStatusBar(QPainter &p);
+
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
+  UIScene &scene;
+
+  QHBoxLayout *bottom_layout;
+
+  bool experimentalMode;
+  bool mapOpen;
+
+  float accelerationConversion;
+  float distanceConversion;
+  float speedConversion;
+
+  int alertSize;
+
+  QString accelerationUnit;
+  QString leadDistanceUnit;
+  QString leadSpeedUnit;
 
 protected:
   void paintGL() override;
@@ -142,6 +170,7 @@ private:
   QHBoxLayout* split;
 
   // FrogPilot variables
+  UIScene &scene;
   Params params;
   Params paramsMemory{"/dev/shm/params"};
 

selfdrive/ui/qt/sidebar.cc

@@ -24,7 +24,7 @@ void Sidebar::drawMetric(QPainter &p, const QPair<QString, QString> &label, QCol
   p.drawText(rect.adjusted(22, 0, 0, 0), Qt::AlignCenter, label.first + "\n" + label.second);
 }
 
-Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false) {
+Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false), scene(uiState()->scene) {
   home_img = loadPixmap("../assets/images/button_home.png", home_btn.size());
   flag_img = loadPixmap("../assets/images/button_flag.png", home_btn.size());
   settings_img = loadPixmap("../assets/images/button_settings.png", settings_btn.size(), Qt::IgnoreAspectRatio);

selfdrive/ui/qt/sidebar.h

@@ -62,4 +62,5 @@ private:
 
   // FrogPilot variables
   Params params;
+  UIScene &scene;
 };

selfdrive/ui/ui.cc

@@ -148,6 +148,8 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers
     kpt_this = matvecmul3(r_xyz, kpt_this);
     scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}};
   }
+
+  scene.right_hand_drive = is_rhd;
 }
 
 static void update_sockets(UIState *s) {
@@ -210,6 +212,9 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("controlsState")) {
     auto controlsState = sm["controlsState"].getControlsState();
+    scene.alert_size = controlsState.getAlertSize() == cereal::ControlsState::AlertSize::MID ? 350 : controlsState.getAlertSize() == cereal::ControlsState::AlertSize::SMALL ? 200 : 0;
+    scene.enabled = controlsState.getEnabled();
+    scene.experimental_mode = controlsState.getExperimentalMode();
   }
   if (sm.updated("deviceState")) {
     auto deviceState = sm["deviceState"].getDeviceState();
@@ -217,6 +222,9 @@ static void update_state(UIState *s) {
   if (sm.updated("frogpilotCarControl")) {
     auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
   }
+  if (sm.updated("frogpilotCarState")) {
+    auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState();
+  }
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
   }
@@ -279,7 +287,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
     "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "carControl", "liveTorqueParameters",
-    "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan",
+    "frogpilotCarControl", "frogpilotCarState", "frogpilotDeviceState", "frogpilotPlan",
   });
 
   Params params;

selfdrive/ui/ui.h

@@ -107,6 +107,10 @@ typedef enum UIStatus {
   STATUS_DISENGAGED,
   STATUS_OVERRIDE,
   STATUS_ENGAGED,
+
+  // FrogPilot statuses
+  STATUS_EXPERIMENTAL_MODE_ACTIVE,
+  STATUS_NAVIGATION_ACTIVE,
 } UIStatus;
 
 enum PrimeType {
@@ -123,6 +127,10 @@ const QColor bg_colors [] = {
   [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
   [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1),
   [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
+
+  // FrogPilot colors
+  [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1),
+  [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1),
 };
 
 static std::map<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -164,6 +172,16 @@ typedef struct UIScene {
   bool started, ignition, is_metric, map_on_left, longitudinal_control;
   bool world_objects_visible = false;
   uint64_t started_frame;
+
+  // FrogPilot variables
+  bool enabled;
+  bool experimental_mode;
+  bool map_open;
+  bool online;
+  bool right_hand_drive;
+
+  int alert_size;
+
 } UIScene;
 
 class UIState : public QObject {