diff --git a/frogpilot/ui/qt/onroad/frogpilot_onroad.cc b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc index e9e0ea85..71cc40db 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_onroad.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc @@ -15,6 +15,30 @@ void FrogPilotOnroadWindow::updateState(const UIState &s, const FrogPilotUIState const cereal::CarState::Reader &carState = sm["carState"].getCarState(); const cereal::CarControl::Reader &carControl = fpsm["carControl"].getCarControl(); + showFPS = frogpilot_toggles.value("show_fps").toBool(); + + if (showFPS) { + static float avgFPS = 0.0f; + static float maxFPS = 0.0f; + static float minFPS = 99.9f; + + if (avgFPS == 0.0f) { + avgFPS = fps; + } + + static float alpha = 1.0f / (UI_FREQ * 60.0f); + avgFPS = alpha * fps + (1.0f - alpha) * avgFPS; + + minFPS = std::min(minFPS, fps); + maxFPS = std::max(maxFPS, fps); + + fpsDisplayString = QString("FPS: %1 | Min: %2 | Max: %3 | Avg: %4") + .arg(qRound(fps)) + .arg(qRound(minFPS)) + .arg(qRound(maxFPS)) + .arg(qRound(avgFPS)); + } + update(); } @@ -23,4 +47,22 @@ void FrogPilotOnroadWindow::paintEvent(QPaintEvent *event) { p.setClipRegion(marginRegion); p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + if (showFPS) { + paintFPS(p); + } +} + +void FrogPilotOnroadWindow::paintFPS(QPainter &p) { + p.save(); + + p.setFont(InterFont(28, QFont::DemiBold)); + p.setPen(Qt::white); + + int xPos = (rect.width() - p.fontMetrics().horizontalAdvance(fpsDisplayString)) / 2; + int yPos = rect.bottom() - 5; + + p.drawText(xPos, yPos, fpsDisplayString); + + p.restore(); } diff --git a/frogpilot/ui/qt/onroad/frogpilot_onroad.h b/frogpilot/ui/qt/onroad/frogpilot_onroad.h index 5964e965..e8f4d4a7 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_onroad.h +++ b/frogpilot/ui/qt/onroad/frogpilot_onroad.h @@ -10,6 +10,8 @@ public: void updateState(const UIState &s, const FrogPilotUIState &fs); + float fps; + FrogPilotUIScene frogpilot_scene; QColor bg; @@ -18,9 +20,14 @@ public: private: void paintEvent(QPaintEvent *event); + void paintFPS(QPainter &p); void resizeEvent(QResizeEvent *event); + bool showFPS; + QRect rect; QRegion marginRegion; + + QString fpsDisplayString; }; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 9551d557..0e00f9a4 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -163,7 +163,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/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 3179b7a8..85017a54 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -18,6 +18,8 @@ public: void updateState(const UIState &s, const FrogPilotUIState &fs); // FrogPilot variables + double fps; + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; FrogPilotUIScene frogpilot_scene; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 9d75cf49..0c968616 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -73,6 +73,7 @@ void OnroadWindow::updateState(const UIState &s, const FrogPilotUIState &fs) { frogpilot_nvg->alertHeight = alerts->alertHeight; frogpilot_onroad->bg = bg; + frogpilot_onroad->fps = nvg->fps; nvg->frogpilot_nvg = frogpilot_nvg;