From 5ca48bea573b69b12bcfff25f60ee00d027b952e Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sat, 11 May 2024 13:54:48 -0700
Subject: [PATCH] Visuals - Developer UI - FPS counter

Display the 'Frames Per Second' (FPS) of your onroad UI for monitoring system performance.
---
 selfdrive/ui/qt/onroad.cc              | 47 +++++++++++++++++++++++++-
 selfdrive/ui/qt/onroad.h               |  1 +
 selfdrive/ui/ui.cc                     |  1 +
 selfdrive/ui/ui.h                      |  1 +
 system/camerad/cameras/camera_qcom2.cc | 13 +++++++
 5 files changed, 62 insertions(+), 1 deletion(-)

diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index e317aff..a1814ce 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -306,6 +306,51 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
       blindspot_frames_left = blindspot_frames_right = 0;
     }
   }
+
+  if (scene.fps_counter) {
+    qint64 currentMillis = QDateTime::currentMSecsSinceEpoch();
+    auto fpsQueue = std::queue<std::pair<qint64, double>>();
+
+    static double avgFPS = 0.0;
+    static double maxFPS = 0.0;
+    static double minFPS = 99.9;
+
+    minFPS = qMin(minFPS, fps);
+    maxFPS = qMax(maxFPS, fps);
+
+    fpsQueue.push({currentMillis, fps});
+
+    while (!fpsQueue.empty() && currentMillis - fpsQueue.front().first > 60000) {
+      fpsQueue.pop();
+    }
+
+    if (!fpsQueue.empty()) {
+      double totalFPS = 0;
+      for (auto tempQueue = fpsQueue; !tempQueue.empty(); tempQueue.pop()) {
+        totalFPS += tempQueue.front().second;
+      }
+      avgFPS = totalFPS / fpsQueue.size();
+    }
+
+    QString fpsDisplayString = QString("FPS: %1 (%2) | Min: %3 | Max: %4 | Avg: %5")
+      .arg(qRound(fps))
+      .arg(paramsMemory.getInt("CameraFPS"))
+      .arg(qRound(minFPS))
+      .arg(qRound(maxFPS))
+      .arg(qRound(avgFPS));
+
+    p.setFont(InterFont(28, QFont::DemiBold));
+    p.setRenderHint(QPainter::TextAntialiasing);
+    p.setPen(Qt::white);
+
+    int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString);
+    int xPos = (rect.width() - textWidth) / 2;
+    int yPos = rect.bottom() - 5;
+
+    p.drawText(xPos, yPos, fpsDisplayString);
+
+    update();
+  }
 }
 
 // ***** onroad widgets *****
@@ -1087,7 +1132,7 @@ void AnnotatedCameraWidget::paintGL() {
 
   double cur_draw_t = millis_since_boot();
   double dt = cur_draw_t - prev_draw_t;
-  double fps = fps_filter.update(1. / dt * 1000);
+  fps = fps_filter.update(1. / dt * 1000);
   if (fps < 15) {
     LOGW("slow frame rate: %.2f fps", fps);
   }
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index e36097d..1207019 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -16,6 +16,7 @@
 const int btn_size = 192;
 const int img_size = (btn_size / 4) * 3;
 
+static double fps;
 
 // ***** onroad widgets *****
 class OnroadAlerts : public QWidget {
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 362e5f6..e24ae1f 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -329,6 +329,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.show_blind_spot = border_metrics && params.getBool("BlindSpotMetrics");
   scene.show_signal = border_metrics && params.getBool("SignalMetrics");
   scene.show_steering = border_metrics && params.getBool("ShowSteering");
+  scene.fps_counter = developer_ui && params.getBool("FPSCounter");
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");
   scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing");
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 2e0436f..c67c071 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -202,6 +202,7 @@ typedef struct UIScene {
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
+  bool fps_counter;
   bool holiday_themes;
   bool map_open;
   bool online;
diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc
index f7c28fd..f87ae14 100644
--- a/system/camerad/cameras/camera_qcom2.cc
+++ b/system/camerad/cameras/camera_qcom2.cc
@@ -935,6 +935,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
 void cameras_run(MultiCameraState *s) {
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
+  const std::chrono::seconds fpsUpdateInterval(1);
+  std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
+  int frameCount = 0;
 
   LOG("-- Starting threads");
   std::vector<std::thread> threads;
@@ -978,6 +981,16 @@ void cameras_run(MultiCameraState *s) {
         // for debugging
         //do_exit = do_exit || event_data->u.frame_msg.frame_id > (30*20);
 
+        frameCount++;
+
+        std::chrono::steady_clock::time_point currentTime = std::chrono::steady_clock::now();
+        if (currentTime - startTime >= fpsUpdateInterval) {
+          double fps = static_cast<double>(frameCount) / std::chrono::duration<double>(currentTime - startTime).count();
+          paramsMemory.putIntNonBlocking("CameraFPS", fps / 3);
+          frameCount = 0;
+          startTime = currentTime;
+        }
+
         if (event_data->session_hdl == s->road_cam.session_handle) {
           s->road_cam.handle_camera_event(event_data);
         } else if (event_data->session_hdl == s->wide_road_cam.session_handle) {