FPS counter

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

common/params.cc

@@ -222,6 +222,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ApiCache_DriveStats", PERSISTENT},
     {"AutomaticUpdates", PERSISTENT},
     {"BlindSpotPath", PERSISTENT},
+    {"CameraFPS", PERSISTENT},
     {"CameraView", PERSISTENT},
     {"CameraViewReset", PERSISTENT},
     {"CarMake", PERSISTENT},
@@ -268,6 +269,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ExperimentalModeViaTap", PERSISTENT},
     {"ForceAutoTune", PERSISTENT},
     {"ForceFingerprint", PERSISTENT},
+    {"FPSCounter", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"FrogsGoMoo", PERSISTENT},
     {"GoatScream", PERSISTENT},

selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc

@@ -18,6 +18,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
 
     {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"},
     {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""},
+    {"FPSCounter", tr("FPS Counter"), tr("Display the 'Frames Per Second' (FPS) of your onroad UI for monitoring system performance."), ""},
     {"LeadInfo", tr("Lead Info and Logics"), tr("Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance."), ""},
     {"CustomPaths", tr("Paths"), tr("Show your projected acceleration on the driving path, detected adjacent lanes, or when a vehicle is detected in your blindspot."), ""},
 

selfdrive/frogpilot/ui/qt/offroad/visual_settings.h

@@ -25,7 +25,7 @@ private:
 
   std::set<QString> alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"};
   std::set<QString> customAlertsKeys = {};
-  std::set<QString> customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "LeadInfo"};
+  std::set<QString> customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
   std::set<QString> qolKeys = {"CameraView", "DriverCamera"};

selfdrive/ui/qt/onroad.cc

@@ -175,6 +175,52 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
 
+  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);
+
+    QRect currentRect = rect();
+    int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString);
+    int xPos = (currentRect.width() - textWidth) / 2;
+    int yPos = currentRect.bottom() - 5;
+
+    p.drawText(xPos, yPos, fpsDisplayString);
+
+    update();
+  }
+
   QString logicsDisplayString = QString();
   if (scene.show_jerk) {
     logicsDisplayString += QString("Acceleration Jerk: %1 (%2%3) | Speed Jerk: %4 (%5%6) | ")
@@ -909,7 +955,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

@@ -14,6 +14,7 @@
 const int btn_size = 192;
 const int img_size = (btn_size / 4) * 3;
 
+static double fps;
 
 // ***** onroad widgets *****
 class OnroadAlerts : public QWidget {

selfdrive/ui/ui.cc

@@ -307,6 +307,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.adjacent_path = custom_paths && params.getBool("AdjacentPath");
   scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics");
   scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath");
+  scene.fps_counter = custom_onroad_ui && params.getBool("FPSCounter");
   scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo");
   scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk");
   scene.show_tuning = scene.has_auto_tune && developer_ui && params.getBool("ShowTuning");

selfdrive/ui/ui.h

@@ -184,6 +184,7 @@ typedef struct UIScene {
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
+  bool fps_counter;
   bool has_auto_tune;
   bool hide_lead_marker;
   bool lead_info;

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) {