From f672144789c523a16319687aaeeb3cb976cf9f9d Mon Sep 17 00:00:00 2001 From: pixhawk Date: Sat, 24 Apr 2010 23:39:57 +0200 Subject: [PATCH] Improved runtime performance, currently hunting down a bug where the whole application gets repainted although only parts of it are refreshed --- src/ui/HDDisplay.cc | 26 +++++++++-- src/ui/HDDisplay.h | 3 ++ src/ui/HUD.cc | 48 ++++++++++++-------- src/ui/HUD.h | 3 +- src/ui/MainWindow.cc | 6 ++- src/ui/MapWidget.cc | 8 ++-- src/ui/linechart/LinechartPlot.cc | 87 ++++++++++++++++++++----------------- src/ui/linechart/LinechartPlot.h | 2 + src/ui/linechart/LinechartWidget.cc | 8 ++++ src/ui/linechart/LinechartWidget.h | 1 + 10 files changed, 124 insertions(+), 68 deletions(-) diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 0e17ebd..f47b29a 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -35,6 +35,7 @@ This file is part of the PIXHAWK project #include "UASManager.h" #include "HDDisplay.h" #include "ui_HDDisplay.h" +#include "MG.h" #include @@ -68,14 +69,17 @@ HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) : normalStrokeWidth(1.0f), fineStrokeWidth(0.5f), acceptList(plotList), + lastPaintTime(0), m_ui(new Ui::HDDisplay) { //m_ui->setupUi(this); + this->setMinimumHeight(125); + this->setMinimumWidth(100); // Refresh timer - refreshTimer->setInterval(60); - connect(refreshTimer, SIGNAL(timeout()), this, SLOT(repaint())); + refreshTimer->setInterval(100); + connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update())); //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintGL())); fontDatabase = QFontDatabase(); @@ -99,11 +103,27 @@ HDDisplay::~HDDisplay() void HDDisplay::paintEvent(QPaintEvent * event) { - paintGL(); + //paintGL(); + static quint64 interval = 0; + //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; + interval = MG::TIME::getGroundTimeNow(); + paintDisplay(); } void HDDisplay::paintGL() { +} + +void HDDisplay::paintDisplay() +{ + int refreshInterval = 100; + quint64 currTime = MG::TIME::getGroundTimeNow(); + if (currTime - lastPaintTime < refreshInterval) + { + // FIXME Need to find the source of the spurious paint events + //return; + } + lastPaintTime = currTime; // Draw instruments // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW // Update scaling factor diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 58e75ce..319d38c 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -61,6 +61,7 @@ public slots: protected slots: void paintGL(); + void paintDisplay(); protected: void changeEvent(QEvent *e); @@ -122,6 +123,8 @@ protected: QStringList* acceptList; ///< Variable names to plot + quint64 lastPaintTime; ///< Last time this widget was refreshed + private: Ui::HDDisplay *m_ui; }; diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 22aff85..9469a93 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -126,8 +126,9 @@ HUD::HUD(int width, int height, QWidget* parent) glImage = QGLWidget::convertToGLFormat(fill); // Refresh timer - refreshTimer->setInterval(40); // 25 Hz - connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update())); + refreshTimer->setInterval(50); // 20 Hz + //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update())); + connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD())); // Resize to correct size and fill with image resize(fill.size()); @@ -216,7 +217,7 @@ void HUD::setActiveUAS(UASInterface* uas) disconnect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); disconnect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); - disconnect(uas, SIGNAL(modeChanged(UASInterface*,QString,QString)), this, SLOT(updateMode(UASInterface*,QString))); + disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); disconnect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); disconnect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeThrustSetPoint(UASInterface*,double,double,double,double,quint64))); disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); @@ -229,8 +230,10 @@ void HUD::setActiveUAS(UASInterface* uas) qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); - //connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); - //connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); + connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); + connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); //connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double))); //connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); //connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); @@ -264,6 +267,8 @@ void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int s updateValue(uas, "time remaining", seconds, MG::TIME::getGroundTimeNow()); updateValue(uas, "charge level", percent, MG::TIME::getGroundTimeNow()); + fuelStatus.sprintf("BAT [%02.0f \%% | %05.2fV] (%02dm:%02ds)", percent, voltage, seconds/60, seconds%60); + if (percent < 20.0f) { fuelColor = warningColor; @@ -327,10 +332,11 @@ void HUD::updateState(UASInterface* uas,QString state) * @param uas the system the state message originates from * @param mode short mode text, displayed in HUD */ -void HUD::updateMode(UASInterface* uas,QString mode) +void HUD::updateMode(int id,QString mode, QString description) { // Only one UAS is connected at a time - Q_UNUSED(uas); + Q_UNUSED(id); + Q_UNUSED(description); this->mode = mode; } @@ -365,7 +371,6 @@ float HUD::refToScreenY(float y) */ void HUD::paintCenterBackground(float roll, float pitch, float yaw) { - // Center indicator is 100 mm wide float referenceWidth = 70.0; float referenceHeight = 70.0; @@ -422,7 +427,6 @@ void HUD::paintCenterBackground(float roll, float pitch, float yaw) glVertex2f(-300,0); glEnd(); - } /** @@ -522,16 +526,26 @@ void HUD::paintRollPitchStrips() void HUD::paintEvent(QPaintEvent *event) { // Event is not needed + // the event is ignored as this widget + // is refreshed automatically Q_UNUSED(event); +} + +void HUD::paintHUD() +{ +// static quint64 interval = 0; +// qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; +// interval = MG::TIME::getGroundTimeNow(); // Read out most important values to limit hash table lookups - static float roll = 0.0; - static float pitch = 0.0; - static float yaw = 0.0; + static float roll = 0.0f; + static float pitch = 0.0f; + static float yaw = 0.0f; - roll = roll * 0.3 + 0.7 * values.value("roll", 0.0f); - pitch = pitch * 0.3 + 0.7 * values.value("pitch", 0.0f); - yaw = yaw * 0.3 + 0.7 * values.value("yaw", 0.0f); + // Low-pass roll, pitch and yaw + roll = roll * 0.2f + 0.8f * values.value("roll", 0.0f); + pitch = pitch * 0.2f + 0.8f * values.value("pitch", 0.0f); + yaw = yaw * 0.2f + 0.8f * values.value("yaw", 0.0f); // Translate for yaw const float maxYawTrans = 60.0f; @@ -555,8 +569,6 @@ void HUD::paintEvent(QPaintEvent *event) yawInt *= 0.6f; //qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw << "asin(yawInt)" << asinYaw; - - // Update scaling factor // adjust scaling to fit both horizontally and vertically scalingFactor = this->width()/vwidth; @@ -1304,6 +1316,8 @@ void HUD::resizeGL(int w, int h) glOrtho(0, w, 0, h, -1, 1); glMatrixMode(GL_MODELVIEW); glPolygonMode(GL_FRONT, GL_FILL); + //FIXME + paintHUD(); } void HUD::selectWaypoint(UASInterface* uas, int id) diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 14437d9..e599c55 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -80,7 +80,7 @@ public slots: void updateGlobalPosition(UASInterface*,double,double,double,quint64); void updateSpeed(UASInterface*,double,double,double,quint64); void updateState(UASInterface*,QString); - void updateMode(UASInterface*,QString); + void updateMode(int id,QString mode, QString description); void updateLoad(UASInterface*, double); void selectWaypoint(UASInterface* uas, int id); @@ -98,6 +98,7 @@ protected slots: void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter); /** @brief Setup the OpenGL view for drawing a sub-component of the HUD */ void setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight); + void paintHUD(); void paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter); void paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index fb66935..5525d67 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -77,6 +77,7 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) // Initialize views, NOT show them yet, only initialize model and controller centerStack = new QStackedWidget(this); linechart = new LinechartWidget(this); + linechart->setActive(false); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), linechart, SLOT(setActivePlot(UASInterface*))); centerStack->addWidget(linechart); control = new UASControlWidget(this); @@ -346,6 +347,7 @@ void MainWindow::clearView() { // Halt HUD hud->stop(); + linechart->setActive(false); headDown1->stop(); headDown2->stop(); @@ -399,7 +401,7 @@ void MainWindow::loadOperatorView() GAudioOutput::instance()->say("Switched to Operator View"); - // LINE CHART + // MAP centerStack->setCurrentWidget(map); // UAS CONTROL @@ -442,6 +444,7 @@ void MainWindow::loadSettingsView() GAudioOutput::instance()->say("Switched to Settings View"); // LINE CHART + linechart->setActive(true); centerStack->setCurrentWidget(linechart); // COMM XML @@ -463,6 +466,7 @@ void MainWindow::loadEngineerView() GAudioOutput::instance()->say("Switched to Engineer View"); // LINE CHART + linechart->setActive(true); centerStack->setCurrentWidget(linechart); // UAS CONTROL diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 51cc7ee..ecd78cc 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -153,12 +153,10 @@ void MapWidget::keyPressEvent(QKeyEvent *event) } } -void MapWidget::resizeEvent(QResizeEvent * event ) +void MapWidget::resizeEvent(QResizeEvent* event ) { - if (event->isAccepted()) - { - mc->resize(this->size()); - } + Q_UNUSED(event); + mc->resize(this->size()); } diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index c616e3e..9d1eecb 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -189,10 +189,10 @@ void LinechartPlot::setRefreshRate(int ms) updateTimer->setInterval(ms); } -//void LinechartPlot::setActive(bool active) -//{ - -//} +void LinechartPlot::setActive(bool active) +{ + m_active = active; +} /** * @brief Set the zero (center line) value @@ -497,7 +497,6 @@ void LinechartPlot::setPlotInterval(int interval) TimeSeriesData* d = data.value(j.key()); d->setInterval(interval); } - paintRealtime(); } /** @@ -552,56 +551,56 @@ void LinechartPlot::setAverageWindow(int windowSize) **/ void LinechartPlot::paintRealtime() { - // Update plot window value to new max time if the last time was also the max time - windowLock.lock(); - if (automaticScrollActive) { - if (MG::TIME::getGroundTimeNow() > maxTime && abs(MG::TIME::getGroundTimeNow() - maxTime) < 5000000) - { - plotPosition = MG::TIME::getGroundTimeNow(); - } - else - { - plotPosition = maxTime;// + lastMaxTimeAdded.msec(); - } - setAxisScale(QwtPlot::xBottom, plotPosition - plotInterval, plotPosition, timeScaleStep); - /* Notify about change. Even if the window position was not changed + if (m_active) + { + // Update plot window value to new max time if the last time was also the max time + windowLock.lock(); + if (automaticScrollActive) { + if (MG::TIME::getGroundTimeNow() > maxTime && abs(MG::TIME::getGroundTimeNow() - maxTime) < 5000000) + { + plotPosition = MG::TIME::getGroundTimeNow(); + } + else + { + plotPosition = maxTime;// + lastMaxTimeAdded.msec(); + } + setAxisScale(QwtPlot::xBottom, plotPosition - plotInterval, plotPosition, timeScaleStep); + /* Notify about change. Even if the window position was not changed * itself, the relative position of the window to the interval must * have changed, as the interval likely increased in length */ - emit windowPositionChanged(getWindowPosition()); - } + emit windowPositionChanged(getWindowPosition()); + } - windowLock.unlock(); + windowLock.unlock(); - // Defined both on windows 32- and 64 bit + // Defined both on windows 32- and 64 bit #ifndef _WIN32 - // const bool cacheMode = - // canvas()->testPaintAttribute(QwtPlotCanvas::PaintCached); - const bool oldDirectPaint = - canvas()->testAttribute(Qt::WA_PaintOutsidePaintEvent); + // const bool cacheMode = + // canvas()->testPaintAttribute(QwtPlotCanvas::PaintCached); + const bool oldDirectPaint = + canvas()->testAttribute(Qt::WA_PaintOutsidePaintEvent); - const QPaintEngine *pe = canvas()->paintEngine(); - bool directPaint = pe->hasFeature(QPaintEngine::PaintOutsidePaintEvent); - if ( pe->type() == QPaintEngine::X11 ) - { - // Even if not recommended by TrollTech, Qt::WA_PaintOutsidePaintEvent - // works on X11. This has an tremendous effect on the performance.. - directPaint = true; - } - canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, directPaint); + const QPaintEngine *pe = canvas()->paintEngine(); + bool directPaint = pe->hasFeature(QPaintEngine::PaintOutsidePaintEvent); + if ( pe->type() == QPaintEngine::X11 ) + { + // Even if not recommended by TrollTech, Qt::WA_PaintOutsidePaintEvent + // works on X11. This has an tremendous effect on the performance.. + directPaint = true; + } + canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, directPaint); #endif - if (m_active) - { + replot(); - } #ifndef _WIN32 - canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, oldDirectPaint); + canvas()->setAttribute(Qt::WA_PaintOutsidePaintEvent, oldDirectPaint); #endif - /* + /* QMap::iterator i; for(i = curves.begin(); i != curves.end(); ++i) { const bool cacheMode = canvas()->testPaintAttribute(QwtPlotCanvas::PaintCached); @@ -610,7 +609,13 @@ void LinechartPlot::paintRealtime() canvas()->setPaintAttribute(QwtPlotCanvas::PaintCached, cacheMode); }*/ - +// static quint64 timestamp = 0; +// +// +// qDebug() << "PLOT INTERVAL:" << MG::TIME::getGroundTimeNow() - timestamp; +// +// timestamp = MG::TIME::getGroundTimeNow(); + } } /** diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index 50a8aa3..ce491dd 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -225,6 +225,8 @@ public slots: void appendData(QString dataname, quint64 ms, double value); void hideCurve(QString id); void showCurve(QString id); + /** @brief Enable auto-refreshing of plot */ + void setActive(bool active); // Functions referring to the currently active plot void setVisible(QString id, bool visible); diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 1c8b3d5..5775fbd 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -384,6 +384,14 @@ LinechartPlot* LinechartWidget::getPlot(int plotid) { return plots.value(plotid); } +void LinechartWidget::setActive(bool active) +{ + if (activePlot) + { + activePlot->setActive(active); + } +} + void LinechartWidget::setActivePlot(UASInterface* uas) { setActivePlot(uas->getUASID()); diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index 7a0e1c7..4307ddf 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -81,6 +81,7 @@ public slots: void setPlotInterval(quint64 interval); void setActivePlot(int uasid); void setActivePlot(UASInterface* uas); + void setActive(bool active); /** @brief Set the number of values to average over */ void setAverageWindow(int windowSize);