diff --git a/common/params.cc b/common/params.cc index 07068d8..cce2649 100644 --- a/common/params.cc +++ b/common/params.cc @@ -221,6 +221,7 @@ std::unordered_map keys = { {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"BlindSpotPath", PERSISTENT}, + {"CameraFPS", PERSISTENT}, {"CameraView", PERSISTENT}, {"CarMake", PERSISTENT}, {"CarModel", PERSISTENT}, @@ -264,6 +265,7 @@ std::unordered_map keys = { {"FireTheBabysitter", PERSISTENT}, {"ForceAutoTune", PERSISTENT}, {"ForceFingerprint", PERSISTENT}, + {"FPSCounter", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, {"GoatScream", PERSISTENT}, diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 447c933..5678e52 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/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"}, diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index a03cee0..6e8e8cb 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -30,7 +30,7 @@ private: std::set alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"}; std::set customAlertsKeys = {}; - std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "LeadInfo"}; + std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"DriveStats"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index dffbaff..f4c8da8 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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> fpsQueue = std::queue>(); + + 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> 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); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index dd29007..65ac4a5 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/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; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 0ff71b5..cf85805 100644 --- a/selfdrive/ui/ui.cc +++ b/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"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index e66f1c0..9a4a8e2 100644 --- a/selfdrive/ui/ui.h +++ b/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; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index f7c28fd..b27b070 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,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(frameCount) / std::chrono::duration(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) {