FPS counter

Added toggle to enable an fps counter for the onroad UI and the current displayed camera.

common/params.cc

@@ -221,6 +221,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AlwaysOnLateralMain", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"BlindSpotPath", PERSISTENT},
+    {"CameraFPS", PERSISTENT},
     {"CameraView", PERSISTENT},
     {"CarMake", PERSISTENT},
     {"CarModel", PERSISTENT},
@@ -264,6 +265,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"FireTheBabysitter", PERSISTENT},
     {"ForceAutoTune", PERSISTENT},
     {"ForceFingerprint", PERSISTENT},
+    {"FPSCounter", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"FrogsGoMoo", PERSISTENT},
     {"GoatScream", PERSISTENT},

selfdrive/frogpilot/ui/visual_settings.cc

@@ -25,6 +25,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},
     {"AdjacentPath", "Adjacent Paths", "Display paths to the left and right of your car, visualizing where the model detects lanes.", ""},
     {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""},
+    {"FPSCounter", "FPS Counter", "Display the Frames Per Second (FPS) of your onroad UI for monitoring system performance.", ""},
     {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""},
 
     {"DriverCamera", "Driver Camera On Reverse", "Show the driver's camera feed when you shift to reverse.", "../assets/img_driver_face_static.png"},

selfdrive/frogpilot/ui/visual_settings.h

@@ -30,7 +30,7 @@ private:
 
   std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
   std::set<QString> customAlertsKeys = {};
-  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "LeadInfo"};
+  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
   std::set<QString> qolKeys = {"DriveStats"};

selfdrive/ui/qt/onroad.cc

@@ -178,6 +178,58 @@ void OnroadWindow::primeChanged(bool prime) {
 void OnroadWindow::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
+
+  // Draw FPS on screen
+  if (scene.fps_counter) {
+    updateFPSCounter();
+
+    // Format the FPS string
+    QString fpsDisplayString = QString("FPS: %1 (%2) | Min: %3 | Max: %4 | Avg: %5")
+      .arg(fps, 0, 'f', 2)
+      .arg(paramsMemory.getInt("CameraFPS"))
+      .arg(minFPS, 0, 'f', 2)
+      .arg(maxFPS, 0, 'f', 2)
+      .arg(avgFPS, 0, 'f', 2);
+
+    // Configure the text
+    p.setFont(InterFont(30, QFont::DemiBold));
+    p.setRenderHint(QPainter::TextAntialiasing);
+    p.setPen(Qt::white);
+
+    // Center the text
+    QRect currentRect = rect();
+    int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString);
+    int xPos = (currentRect.width() - textWidth) / 2;
+    int yPos = currentRect.bottom() - 5;
+
+    // Draw the text
+    p.drawText(xPos, yPos, fpsDisplayString);
+
+    update();
+  }
+}
+
+void OnroadWindow::updateFPSCounter() {
+  qint64 currentMillis = QDateTime::currentMSecsSinceEpoch();
+  std::queue<std::pair<qint64, double>> fpsQueue = std::queue<std::pair<qint64, double>>();
+
+  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;
+    std::queue<std::pair<qint64, double>> tempQueue = fpsQueue;
+    while (!tempQueue.empty()) {
+      totalFPS += tempQueue.front().second;
+      tempQueue.pop();
+    }
+    avgFPS = totalFPS / fpsQueue.size();
+  }
 }
 
 // ***** onroad widgets *****
@@ -894,7 +946,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);
   }

selfdrive/ui/qt/onroad.h

@@ -15,6 +15,7 @@ const int btn_size = 192;
 const int img_size = (btn_size / 4) * 3;
 
 // FrogPilot global variables
+static double fps;
 
 // ***** onroad widgets *****
 class OnroadAlerts : public QWidget {
@@ -197,10 +198,17 @@ private:
   QWidget *map = nullptr;
   QHBoxLayout* split;
 
+  // FrogPilot widgets
+  void updateFPSCounter();
+
   // FrogPilot variables
   UIScene &scene;
   Params paramsMemory{"/dev/shm/params"};
 
+  double avgFPS;
+  double maxFPS = 0.0;
+  double minFPS = 99.9;
+
   QPoint timeoutPoint = QPoint(420, 69);
   QTimer clickTimer;
 

selfdrive/ui/ui.cc

@@ -297,6 +297,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.adjacent_path = custom_onroad_ui && params.getBool("AdjacentPath");
   scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics");
   scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath");
+  scene.fps_counter = custom_onroad_ui && params.getBool("FPSCounter");
   scene.lead_info = custom_onroad_ui && params.getBool("LeadInfo");
   scene.use_si = scene.lead_info && params.getBool("UseSI");
 

selfdrive/ui/ui.h

@@ -183,6 +183,7 @@ typedef struct UIScene {
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
+  bool fps_counter;
   bool lead_info;
   bool map_open;
   bool model_ui;

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,20 @@ void cameras_run(MultiCameraState *s) {
         // for debugging
         //do_exit = do_exit || event_data->u.frame_msg.frame_id > (30*20);
 
+        // Increment frame count
+        frameCount++;
+
+        // Calculate and display FPS every second
+        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);
+
+          // Reset counter and timer
+          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) {