From a30eaf473bb4a46212cba259bda4ad6bfba13b3e Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 26 Mar 2024 23:15:51 -0700 Subject: [PATCH] FPS counter Added toggle to enable an fps counter for the onroad UI and the current displayed camera. --- common/params.cc | 2 + .../ui/qt/offroad/visual_settings.cc | 1 + .../frogpilot/ui/qt/offroad/visual_settings.h | 2 +- selfdrive/ui/qt/onroad.cc | 48 ++++++++++++++++++- selfdrive/ui/qt/onroad.h | 1 + selfdrive/ui/ui.cc | 1 + selfdrive/ui/ui.h | 1 + system/camerad/cameras/camera_qcom2.cc | 13 +++++ 8 files changed, 67 insertions(+), 2 deletions(-) diff --git a/common/params.cc b/common/params.cc index 341655c76..d456cee48 100644 --- a/common/params.cc +++ b/common/params.cc @@ -222,6 +222,7 @@ std::unordered_map keys = { {"ApiCache_DriveStats", PERSISTENT}, {"AutomaticUpdates", PERSISTENT}, {"BlindSpotPath", PERSISTENT}, + {"CameraFPS", PERSISTENT}, {"CameraView", PERSISTENT}, {"CameraViewReset", PERSISTENT}, {"CarMake", PERSISTENT}, @@ -268,6 +269,7 @@ std::unordered_map keys = { {"ExperimentalModeViaTap", PERSISTENT}, {"ForceAutoTune", PERSISTENT}, {"ForceFingerprint", PERSISTENT}, + {"FPSCounter", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, {"GoatScream", PERSISTENT}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index cd958e3a0..cbb9d8fdc 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/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."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index baa6ec138..8c7a9c93d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -25,7 +25,7 @@ private: std::set alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"}; std::set customAlertsKeys = {}; - std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "LeadInfo"}; + std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"CameraView", "DriverCamera"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 916740640..8f2fead31 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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>(); + + 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); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index fda1a134e..c7ec7d68d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/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 { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1e341ca8c..f89f85a00 100644 --- a/selfdrive/ui/ui.cc +++ b/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"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 59fc2fd67..eb3389e96 100644 --- a/selfdrive/ui/ui.h +++ b/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; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index f7c28fd74..f87ae14ec 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 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(frameCount) / std::chrono::duration(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) {