From 6c59f5f4cb9e459fe05981c1aad017fb0c01b8b0 Mon Sep 17 00:00:00 2001 From: John Tapsell Date: Sun, 13 Oct 2013 18:20:38 +0100 Subject: [PATCH] Add an XY plot widget This allows you to add XY graph plots of any data. For example, x,y position data of a servo. --- qgroundcontrol.pro | 5 +- src/ui/MainWindow.cc | 3 + src/ui/MainWindow.h | 3 + src/ui/designer/QGCToolWidget.cc | 21 ++ src/ui/designer/QGCToolWidget.h | 2 + src/ui/designer/QGCXYPlot.cc | 320 +++++++++++++++++++++++++++ src/ui/designer/QGCXYPlot.h | 50 +++++ src/ui/designer/QGCXYPlot.ui | 360 +++++++++++++++++++++++++++++++ 8 files changed, 763 insertions(+), 1 deletion(-) create mode 100644 src/ui/designer/QGCXYPlot.cc create mode 100644 src/ui/designer/QGCXYPlot.h create mode 100644 src/ui/designer/QGCXYPlot.ui diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bf5ace6e1..e8940419a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -301,7 +301,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/configuration/ApmFirmwareConfig.ui \ src/ui/px4_configuration/QGCPX4AirframeConfig.ui \ src/ui/px4_configuration/QGCPX4MulticopterConfig.ui \ - src/ui/px4_configuration/QGCPX4SensorCalibration.ui + src/ui/px4_configuration/QGCPX4SensorCalibration.ui \ + src/ui/designer/QGCXYPlot.ui INCLUDEPATH += src \ src/ui \ @@ -506,6 +507,7 @@ HEADERS += src/MG.h \ src/ui/QGCBaseParamWidget.h \ src/ui/px4_configuration/QGCPX4MulticopterConfig.h \ src/ui/px4_configuration/QGCPX4SensorCalibration.h \ + src/ui/designer/QGCXYPlot.h \ src/ui/menuactionhelper.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler @@ -728,6 +730,7 @@ SOURCES += src/main.cc \ src/ui/QGCBaseParamWidget.cc \ src/ui/px4_configuration/QGCPX4MulticopterConfig.cc \ src/ui/px4_configuration/QGCPX4SensorCalibration.cc \ + src/ui/designer/QGCXYPlot.cc \ src/ui/menuactionhelper.cpp # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 3eb6b8d8b..a958dc1ed 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -480,6 +480,8 @@ void MainWindow::buildCommonWidgets() { // Add generic MAVLink decoder mavlinkDecoder = new MAVLinkDecoder(mavlink, this); + connect(mavlinkDecoder, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), + this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64))); // Log player logPlayer = new QGCMAVLinkLogPlayer(mavlink, customStatusBar); @@ -1641,6 +1643,7 @@ void MainWindow::UASCreated(UASInterface* uas) connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64))); // HIL showHILConfigurationWidget(uas); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 9d5cd528f..cc98d0f16 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -301,6 +301,9 @@ protected slots: signals: void styleChanged(MainWindow::QGC_MAINWINDOW_STYLE newTheme); void initStatusChanged(const QString& message, int alignment, const QColor &color); + /** Emitted when any value changes from any source */ + void valueChanged(const int uasId, const QString& name, const QString& unit, const QVariant& value, const quint64 msec); + #ifdef MOUSE_ENABLED_LINUX /** @brief Forward X11Event to catch 3DMouse inputs */ void x11EventOccured(XEvent *event); diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index 9f8a49e99..2eeae5b26 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -13,6 +13,7 @@ #include "QGCParamSlider.h" #include "QGCComboBox.h" #include "QGCTextLabel.h" +#include "QGCXYPlot.h" #include "QGCCommandButton.h" #include "UASManager.h" @@ -272,6 +273,11 @@ void QGCToolWidget::loadSettings(QVariantMap& settings) item = new QGCComboBox(this); //qDebug() << "CREATED COMBOBOX"; } + else if (type == "XYPLOT") + { + item = new QGCXYPlot(this); + //qDebug() << "CREATED XYPlot"; + } if (item) { // Configure and add to layout @@ -330,6 +336,11 @@ void QGCToolWidget::loadSettings(QSettings& settings) item->setObjectName(settings.value("QGC_TEXT_ID").toString()); item->setActiveUAS(mav); } + else if (type == "XYPLOT") + { + item = new QGCXYPlot(this); + item->setActiveUAS(mav); + } if (item) { @@ -428,6 +439,7 @@ void QGCToolWidget::contextMenuEvent (QContextMenuEvent* event) menu.addAction(addParamAction); menu.addAction(addCommandAction); menu.addAction(addLabelAction); + menu.addAction(addPlotAction); menu.addSeparator(); menu.addAction(setTitleAction); menu.addAction(exportAction); @@ -467,6 +479,10 @@ void QGCToolWidget::createActions() addLabelAction->setStatusTip(tr("Add a new label to the tool")); connect(addLabelAction, SIGNAL(triggered()), this, SLOT(addLabel())); + addPlotAction = new QAction(tr("New &XY Plot"), this); + addPlotAction->setStatusTip(tr("Add a XY Plot to the tool")); + connect(addPlotAction, SIGNAL(triggered()), this, SLOT(addPlot())); + setTitleAction = new QAction(tr("Set Widget Title"), this); setTitleAction->setStatusTip(tr("Set the title caption of this tool widget")); connect(setTitleAction, SIGNAL(triggered()), this, SLOT(setTitle())); @@ -515,6 +531,11 @@ void QGCToolWidget::addLabel() addToolWidgetAndEdit(new QGCTextLabel(this)); } +void QGCToolWidget::addPlot() +{ + addToolWidgetAndEdit(new QGCXYPlot(this)); +} + void QGCToolWidget::addToolWidgetAndEdit(QGCToolWidgetItem* widget) { addToolWidget(widget); diff --git a/src/ui/designer/QGCToolWidget.h b/src/ui/designer/QGCToolWidget.h index 8b2162003..7ab9ef679 100644 --- a/src/ui/designer/QGCToolWidget.h +++ b/src/ui/designer/QGCToolWidget.h @@ -74,6 +74,7 @@ protected: QVariantMap settingsMap; QAction* addParamAction; QAction* addCommandAction; + QAction* addPlotAction; QAction* addLabelAction; QAction* setTitleAction; QAction* deleteAction; @@ -100,6 +101,7 @@ public slots: protected slots: void addParam(); void addCommand(); + void addPlot(); void addLabel(); void setTitle(); void widgetRemoved(); diff --git a/src/ui/designer/QGCXYPlot.cc b/src/ui/designer/QGCXYPlot.cc new file mode 100644 index 000000000..cf433b00b --- /dev/null +++ b/src/ui/designer/QGCXYPlot.cc @@ -0,0 +1,320 @@ +#include + +#include "QGCXYPlot.h" +#include "ui_QGCXYPlot.h" + +#include "MAVLinkProtocol.h" +#include "UASManager.h" +#include "IncrementalPlot.h" +#include +#include +#include +#include + +class XYPlotCurve : public QwtPlotItem +{ +public: + XYPlotCurve() { + m_maxPoints = 15; + setItemAttribute(QwtPlotItem::AutoScale); + minMaxSet = false; + m_color = Qt::white; + } + + void setMaxDataPoints(int max) { m_maxPoints = max; } + int maxPoints() const { return m_maxPoints; } + + void appendData(const QPointF &data) { + if(!minMaxSet) { + xmin = xmax = data.x(); + ymin = ymax = data.y(); + minMaxSet = true; + } else if(m_autoScale) { + xmin = qMin(xmin, data.x()); + xmax = qMax(xmax, data.x()); + ymin = qMin(ymin, data.y()); + ymax = qMax(ymax, data.y()); + } + + + m_data.append(data); + while(m_data.size() > m_maxPoints) + m_data.removeFirst(); + itemChanged(); + } + void clear() { + minMaxSet = false; + m_data.clear(); + itemChanged(); + } + void setColor(const QColor &color) { + m_color = color; + } + void unsetMinMax() { + if(m_autoScale) + return; + m_autoScale = true; + clear(); + } + void setMinMax(double xmin, double xmax, double ymin, double ymax ) + { + this->xmin = xmin; + this->xmax = xmax; + this->ymin = ymin; + this->ymax = ymax; + m_autoScale = false; + minMaxSet = true; + itemChanged(); + } + + double xMin() const { return xmin; } + double xMax() const { return xmax; } + double yMin() const { return ymin; } + double yMax() const { return ymax; } + + virtual QwtDoubleRect boundingRect() const { + if(!minMaxSet) + return QwtDoubleRect(1,1,-2,-2); + return QwtDoubleRect(xmin,ymin,xmax-xmin,ymax-ymin); + } + +protected: + /* From QwtPlotItem. Draw the complete series */ + virtual void draw (QPainter *p, const QwtScaleMap &xMap, const QwtScaleMap &yMap, const QRect &canvasRect) const + { + Q_UNUSED(canvasRect); + QPointF lastPoint; + int i = 0; + if(m_data.isEmpty()) + return; + int dataSize = m_data.size(); + + foreach(const QPointF &point, m_data) { + QPointF paintCoord = QPointF(xMap.xTransform(point.x()), yMap.xTransform(point.y())); + m_color.setAlpha((i+m_maxPoints - dataSize)*255/m_maxPoints); + p->setPen(m_color); + if(i++) + p->drawLine(lastPoint, paintCoord); + if(i == dataSize) { + //Draw marker for first point + const int marker_radius = 2; + QRectF marker = QRectF(paintCoord.x()-marker_radius, paintCoord.y()-marker_radius, marker_radius*2+1,marker_radius*2+1); + p->fillRect(marker,QBrush(m_color)); + } + lastPoint = paintCoord; + } + } + +private: + QList< QPointF > m_data; + int m_maxPoints; + mutable QColor m_color; + + double xmin; + double xmax; + double ymin; + double ymax; + bool minMaxSet; + bool m_autoScale; +}; + +QGCXYPlot::QGCXYPlot(QWidget *parent) : + QGCToolWidgetItem("XY Plot", parent), + ui(new Ui::QGCXYPlot), + plot(0), + xycurve(0), + maxElementsToDraw(5), + x(0), + x_timestamp_us(0), + x_valid(false), + y(0), + y_timestamp_us(0), + y_valid(false), + max_timestamp_diff_us(10000) /* Default to 10ms tolerance between x and y values */ + + +{ + uas = 0; + ui->setupUi(this); + plot = new QwtPlot(); + + QHBoxLayout* layout = new QHBoxLayout(ui->xyPlotFrame); + layout->addWidget(plot); + + connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); + + connect(MainWindow::instance(), SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), + this, SLOT(appendData(int,QString,QString,QVariant,quint64))); + + connect(ui->editXParam, SIGNAL(editTextChanged(QString)), this, SLOT(clearPlot())); + connect(ui->editYParam, SIGNAL(editTextChanged(QString)), this, SLOT(clearPlot())); + + plot->plotLayout()->setAlignCanvasToScales(true); + + QwtLinearScaleEngine* yScaleEngine = new QwtLinearScaleEngine(); + plot->setAxisScaleEngine(QwtPlot::yLeft, yScaleEngine); + + plot->setAxisAutoScale(QwtPlot::xBottom); + plot->setAxisAutoScale(QwtPlot::yLeft); + plot->setAutoReplot(); + xycurve = new XYPlotCurve(); + xycurve->attach(plot); + styleChanged(MainWindow::instance()->getStyle()); + connect(MainWindow::instance(), SIGNAL(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE)), + this, SLOT(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE))); + connect(ui->minX, SIGNAL(valueChanged(double)),this, SLOT(updateMinMaxSettings())); + connect(ui->maxX, SIGNAL(valueChanged(double)),this, SLOT(updateMinMaxSettings())); + connect(ui->minY, SIGNAL(valueChanged(double)),this, SLOT(updateMinMaxSettings())); + connect(ui->maxY, SIGNAL(valueChanged(double)),this, SLOT(updateMinMaxSettings())); + connect(ui->automaticAxisRange, SIGNAL(toggled(bool)),this, SLOT(updateMinMaxSettings())); + connect(ui->maxDataSpinBox, SIGNAL(valueChanged(int)),this, SLOT(updateMinMaxSettings())); + setEditMode(false); +} + +QGCXYPlot::~QGCXYPlot() +{ + delete ui; +} + +void QGCXYPlot::clearPlot() +{ + xycurve->clear(); + plot->clear(); +} + +void QGCXYPlot::setEditMode(bool editMode) +{ + ui->lblXParam->setVisible(editMode); + ui->lblYParam->setVisible(editMode); + ui->editXParam->setVisible(editMode); + ui->editYParam->setVisible(editMode); + ui->editFinishButton->setVisible(editMode); + ui->editLine1->setVisible(editMode); + ui->editLine2->setVisible(editMode); + ui->lblMaxData->setVisible(editMode); + ui->lblMaxX->setVisible(editMode); + ui->lblMaxY->setVisible(editMode); + ui->lblMinX->setVisible(editMode); + ui->lblMinY->setVisible(editMode); + ui->maxX->setVisible(editMode); + ui->maxY->setVisible(editMode); + ui->minX->setVisible(editMode); + ui->minY->setVisible(editMode); + ui->maxDataSpinBox->setVisible(editMode); + ui->automaticAxisRange->setVisible(editMode); + + if(!editMode) { + plot->setAxisTitle(QwtPlot::xBottom, ui->editXParam->currentText()); + plot->setAxisTitle(QwtPlot::yLeft, ui->editYParam->currentText()); + } + + QGCToolWidgetItem::setEditMode(editMode); + updateMinMaxSettings(); //Do this after calling the parent +} + +void QGCXYPlot::writeSettings(QSettings& settings) +{ + settings.setValue("TYPE", "XYPLOT"); + settings.setValue("QGC_XYPLOT_X", ui->editXParam->currentText()); + settings.setValue("QGC_XYPLOT_Y", ui->editYParam->currentText()); + settings.setValue("QGC_XYPLOT_MINX", ui->minX->value()); + settings.setValue("QGC_XYPLOT_MAXX", ui->maxX->value()); + settings.setValue("QGC_XYPLOT_MINY", ui->minY->value()); + settings.setValue("QGC_XYPLOT_MAXY", ui->maxY->value()); + settings.setValue("QGC_XYPLOT_MAXDATA", ui->maxDataSpinBox->value()); + settings.setValue("QGC_XYPLOT_AUTO", ui->automaticAxisRange->isChecked()); + + settings.sync(); +} +void QGCXYPlot::readSettings(const QString& pre,const QVariantMap& settings) +{ + ui->editXParam->setEditText(settings.value(pre + "QGC_XYPLOT_X", "").toString()); + ui->editYParam->setEditText(settings.value(pre + "QGC_XYPLOT_Y", "").toString()); + ui->automaticAxisRange->setChecked(settings.value(pre + "QGC_XYPLOT_AUTO", true).toBool()); + ui->minX->setValue(settings.value(pre + "QGC_XYPLOT_MINX", 0).toDouble()); + ui->maxX->setValue(settings.value(pre + "QGC_XYPLOT_MAXX", 0).toDouble()); + ui->minY->setValue(settings.value(pre + "QGC_XYPLOT_MINY", 0).toDouble()); + ui->maxY->setValue(settings.value(pre + "QGC_XYPLOT_MAXY", 0).toDouble()); + ui->maxDataSpinBox->setValue(settings.value(pre + "QGC_XYPLOT_MAXDATA", 15).toInt()); + plot->setAxisTitle(QwtPlot::xBottom, ui->editXParam->currentText()); + plot->setAxisTitle(QwtPlot::yLeft, ui->editYParam->currentText()); + updateMinMaxSettings(); +} + +void QGCXYPlot::readSettings(const QSettings& settings) +{ + ui->editXParam->setEditText(settings.value("QGC_XYPLOT_X", "").toString()); + ui->editYParam->setEditText(settings.value("QGC_XYPLOT_Y", "").toString()); + ui->automaticAxisRange->setChecked(settings.value("QGC_XYPLOT_AUTO", true).toBool()); + ui->minX->setValue(settings.value("QGC_XYPLOT_MINX", 0).toDouble()); + ui->maxX->setValue(settings.value("QGC_XYPLOT_MAXX", 0).toDouble()); + ui->minY->setValue(settings.value("QGC_XYPLOT_MINY", 0).toDouble()); + ui->maxY->setValue(settings.value("QGC_XYPLOT_MAXY", 0).toDouble()); + ui->maxDataSpinBox->setValue(settings.value("QGC_XYPLOT_MAXDATA", 15).toInt()); + plot->setAxisTitle(QwtPlot::xBottom, ui->editXParam->currentText()); + plot->setAxisTitle(QwtPlot::yLeft, ui->editYParam->currentText()); + updateMinMaxSettings(); +} + +void QGCXYPlot::appendData(int uasId, const QString& curve, const QString& unit, const QVariant& variant, quint64 usec) +{ + Q_UNUSED(uasId); + Q_UNUSED(unit); + if(isEditMode()) { + //When in edit mode, add all the items to the combo box + if(ui->editXParam->findText(curve) == -1) { + QString oldX = ui->editXParam->currentText(); + QString oldY = ui->editYParam->currentText(); + ui->editXParam->addItem(curve); //Annoyingly this can wipe out the current text + ui->editYParam->addItem(curve); + ui->editXParam->setEditText(oldX); + ui->editYParam->setEditText(oldY); + } + } + + bool ok; + if(curve == ui->editXParam->currentText()) { + x = variant.toDouble(&ok); + if(!ok) + return; + x_timestamp_us = usec; + x_valid = true; + } else if(curve == ui->editYParam->currentText()) { + y = variant.toDouble(&ok); + if(!ok) + return; + y_timestamp_us = usec; + y_valid = true; + } else + return; + + if(x_valid && y_valid && (int)qAbs(y_timestamp_us - x_timestamp_us) <= max_timestamp_diff_us) { + xycurve->appendData( QPointF(x,y) ); + plot->update(); + x_valid = false; + y_valid = false; + } +} + +void QGCXYPlot::styleChanged(MainWindow::QGC_MAINWINDOW_STYLE style) +{ + if (style == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT) + xycurve->setColor(Qt::black); + else + xycurve->setColor(Qt::white); +} + +void QGCXYPlot::updateMinMaxSettings() +{ + bool automatic = ui->automaticAxisRange->isChecked(); + ui->minX->setEnabled(!automatic); + ui->maxX->setEnabled(!automatic); + ui->minY->setEnabled(!automatic); + ui->maxY->setEnabled(!automatic); + if(automatic) { + xycurve->unsetMinMax(); + } else { + xycurve->setMinMax(ui->minX->value(), ui->maxX->value(), ui->minY->value(), ui->maxY->value()); + } + xycurve->setMaxDataPoints(ui->maxDataSpinBox->value()); +} diff --git a/src/ui/designer/QGCXYPlot.h b/src/ui/designer/QGCXYPlot.h new file mode 100644 index 000000000..23f7f1ec7 --- /dev/null +++ b/src/ui/designer/QGCXYPlot.h @@ -0,0 +1,50 @@ +#ifndef QGCXYPLOT_H +#define QGCXYPLOT_H + +#include "QGCToolWidgetItem.h" +#include "MainWindow.h" + +namespace Ui +{ +class QGCXYPlot; +} + +class UASInterface; +class QwtPlot; +class XYPlotCurve; + +class QGCXYPlot : public QGCToolWidgetItem +{ + Q_OBJECT + +public: + explicit QGCXYPlot(QWidget *parent = 0); + ~QGCXYPlot(); + virtual void setEditMode(bool editMode); + +public slots: + void writeSettings(QSettings& settings); + void readSettings(const QSettings& settings); + void readSettings(const QString& pre,const QVariantMap& settings); + void appendData(int uasId, const QString& curve, const QString& unit, const QVariant& variant, quint64 usec); + void clearPlot(); + void styleChanged(MainWindow::QGC_MAINWINDOW_STYLE style); + void updateMinMaxSettings(); + +private: + Ui::QGCXYPlot *ui; + QwtPlot *plot; + XYPlotCurve* xycurve; + + int maxElementsToDraw; + + double x; /**< Last unused value for the x-coordinate */ + quint64 x_timestamp_us; /**< Timestamp that we last recieved a value for x */ + bool x_valid; /**< Whether we have recieved an x value but so far no corresponding y value */ + double y; /**< Last unused value for the x-coordinate */ + quint64 y_timestamp_us; /**< Timestamp that we last recieved a value for x */ + bool y_valid; /**< Whether we have recieved an x value but so far no corresponding y value */ + int max_timestamp_diff_us; /**< Only combine x and y to a data point if the timestamp for both doesn't differ by more than this */ +}; + +#endif // QGCXYPLOT_H diff --git a/src/ui/designer/QGCXYPlot.ui b/src/ui/designer/QGCXYPlot.ui new file mode 100644 index 000000000..3f9fb7fee --- /dev/null +++ b/src/ui/designer/QGCXYPlot.ui @@ -0,0 +1,360 @@ + + + QGCXYPlot + + + + 0 + 0 + 771 + 626 + + + + Form + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 50 + 0 + + + + X Parameter: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + editXParam + + + + + + + + 0 + 0 + + + + true + + + QComboBox::InsertAlphabetically + + + QComboBox::AdjustToContents + + + 10 + + + + + + + + 50 + 0 + + + + Y Parameter: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + editYParam + + + + + + + + 0 + 0 + + + + true + + + QComboBox::InsertAlphabetically + + + QComboBox::AdjustToContents + + + 10 + + + + + + + + + + + Number of data points to &show: + + + maxDataSpinBox + + + + + + + 2 + + + 999 + + + 15 + + + + + + + Automatic Axis Range + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + &Min X: + + + minX + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + &Max X: + + + maxX + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + Min &Y: + + + minY + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + Ma&x Y: + + + maxY + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + 0 + 0 + + + + 0 + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Done + + + + + + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + + + editXParam + editYParam + maxDataSpinBox + minX + maxX + minY + maxY + editFinishButton + + + + -- 2.22.0