FrogPilot setup - Setup onroad UI

selfdrive/ui/qt/onroad/alerts.cc

@@ -11,6 +11,9 @@ void OnroadAlerts::updateState(const UIState &s) {
     alert = a;
     update();
   }
+
+  // FrogPilot variables
+  const UIScene &scene = s.scene;
 }
 
 void OnroadAlerts::clear() {
@@ -67,11 +70,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);
 

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -24,6 +24,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) {
@@ -233,7 +236,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();
@@ -280,6 +283,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   // 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);
@@ -381,7 +385,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;
     }
@@ -441,6 +445,9 @@ void AnnotatedCameraWidget::paintGL() {
   auto m = msg.initEvent().initUiDebug();
   m.setDrawTimeMillis(cur_draw_t - start_draw_t);
   pm->send("uiDebug", msg);
+
+  // Paint FrogPilot widgets
+  paintFrogPilotWidgets(painter, s->scene);
 }
 
 void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
@@ -449,3 +456,97 @@ 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::paintFrogPilotWidgets(QPainter &painter, const UIScene &scene) {
+  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;
+
+  if (true) {
+    drawStatusBar(painter);
+  }
+
+  experimentalMode = scene.experimental_mode;
+
+  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);
+    bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight);
+  }
+}
+
+void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
+  p.save();
+
+  static QElapsedTimer timer;
+  static QString lastShownStatus;
+
+  static bool displayStatusText = false;
+
+  constexpr qreal fadeDuration = 1500.0;
+  constexpr qreal textDuration = 5000.0;
+
+  static qreal statusTextOpacity = 0.0;
+
+  QString newStatus;
+
+  QRect statusBarRect(rect().left() - 1, rect().bottom() - 50, rect().width() + 2, 100);
+  p.setBrush(QColor(0, 0, 0, 150));
+  p.setOpacity(1.0);
+  p.drawRoundedRect(statusBarRect, 30, 30);
+
+  if (newStatus != lastShownStatus) {
+    lastShownStatus = newStatus;
+    displayStatusText = true;
+    timer.restart();
+  } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) {
+    displayStatusText = false;
+  }
+
+  if (displayStatusText) {
+    statusTextOpacity = qBound(0.0, 1.0 - (timer.elapsed() - textDuration) / fadeDuration, 1.0);
+  } else {
+    statusTextOpacity = 0.0;
+  }
+
+  p.setFont(InterFont(40, QFont::Bold));
+  p.setOpacity(statusTextOpacity);
+  p.setPen(Qt::white);
+  p.setRenderHint(QPainter::TextAntialiasing);
+
+  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/annotated_camera.h

@@ -14,6 +14,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);
@@ -40,6 +41,30 @@ private:
   int skip_frame_count = 0;
   bool wide_cam_requested = false;
 
+  // FrogPilot widgets
+  void initializeFrogPilotWidgets();
+  void paintFrogPilotWidgets(QPainter &painter, const UIScene &scene);
+
+  void drawStatusBar(QPainter &p);
+
+  // FrogPilot variables
+  Params paramsMemory{"/dev/shm/params"};
+
+  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;
   void initializeGL() override;

selfdrive/ui/qt/onroad/buttons.cc

@@ -40,6 +40,9 @@ void ExperimentalButton::updateState(const UIState &s) {
     experimental_mode = cs.getExperimentalMode();
     update();
   }
+
+  // FrogPilot variables
+  const UIScene &scene = s.scene;
 }
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
@@ -50,7 +53,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)

selfdrive/ui/qt/onroad/buttons.h

@@ -23,6 +23,9 @@ private:
   QPixmap experimental_img;
   bool experimental_mode;
   bool engageable;
+
+  // FrogPilot variables
+  Params paramsMemory{"/dev/shm/params"};
 };
 
 

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -64,15 +64,31 @@ void OnroadWindow::updateState(const UIState &s) {
   alerts->updateState(s);
   nvg->updateState(s);
 
+  bool shouldUpdate = false;
+
   QColor bgColor = bg_colors[s.status];
   if (bg != bgColor) {
     // repaint border
     bg = bgColor;
+    shouldUpdate = true;
+  }
+
+  // FrogPilot variables
+  const UIScene &scene = s.scene;
+
+  if (shouldUpdate) {
     update();
   }
 }
 
 void OnroadWindow::mousePressEvent(QMouseEvent* e) {
+  // FrogPilot variables
+  UIState *s = uiState();
+  UIScene &scene = s->scene;
+
+  // FrogPilot clickable widgets
+  QPoint pos = e->pos();
+
 #ifdef ENABLE_MAPS
   if (map != nullptr) {
     bool sidebarVisible = geometry().x() > 0;
@@ -90,6 +106,7 @@ void OnroadWindow::createMapWidget() {
   map = m;
   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);
@@ -125,9 +142,12 @@ void OnroadWindow::primeChanged(bool prime) {
 
 void OnroadWindow::paintEvent(QPaintEvent *event) {
   QPainter p(this);
-  p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
 
   // FrogPilot variables
   UIState *s = uiState();
   SubMaster &sm = *(s->sm);
+
+  QRect rect = this->rect();
+  QColor bgColor(bg.red(), bg.green(), bg.blue(), 255);
+  p.fillRect(rect, bgColor);
 }

selfdrive/ui/qt/onroad/onroad_home.h

@@ -24,6 +24,10 @@ private:
   QWidget *map = nullptr;
   QHBoxLayout* split;
 
+  // FrogPilot variables
+  Params params;
+  Params paramsMemory{"/dev/shm/params"};
+
 private slots:
   void offroadTransition(bool offroad);
   void primeChanged(bool prime);

selfdrive/ui/ui.cc

@@ -147,6 +147,8 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers
     vec3 kpt_this = matvecmul3(r_xyz, default_face_kpts_3d[kpi]);
     scene.face_kpts_draw[kpi] = (vec3){{kpt_this.v[0], 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) {
@@ -208,6 +210,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();
@@ -256,7 +261,7 @@ void UIState::updateStatus() {
     if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) {
       status = STATUS_OVERRIDE;
     } else {
-      status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
+      status = scene.enabled ? STATUS_ENGAGED : STATUS_DISENGAGED;
     }
   }
 

selfdrive/ui/ui.h

@@ -50,6 +50,10 @@ typedef enum UIStatus {
   STATUS_DISENGAGED,
   STATUS_OVERRIDE,
   STATUS_ENGAGED,
+
+  // FrogPilot statuses
+  STATUS_EXPERIMENTAL_MODE_ACTIVE,
+  STATUS_NAVIGATION_ACTIVE,
 } UIStatus;
 
 enum PrimeType {
@@ -67,6 +71,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),
 };
 
 
@@ -103,7 +111,12 @@ typedef struct UIScene {
   uint64_t started_frame;
 
   // FrogPilot variables
+  bool enabled;
+  bool experimental_mode;
   bool map_open;
+  bool right_hand_drive;
+
+  int alert_size;
 
 } UIScene;