Enabled generic message decoding

qgroundcontrol.pro

@@ -322,7 +322,8 @@ HEADERS += src/MG.h \
     src/libs/qextserialport/qextserialenumerator.h \
     src/QGCGeo.h \
     src/ui/QGCToolBar.h \
-    src/ui/QGCMAVLinkInspector.h
+    src/ui/QGCMAVLinkInspector.h \
+    src/ui/MAVLinkDecoder.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -447,7 +448,8 @@ SOURCES += src/main.cc \
     src/ui/map/QGCMapTool.cc \
     src/ui/map/QGCMapToolBar.cc \
     src/ui/QGCToolBar.cc \
-    src/ui/QGCMAVLinkInspector.cc
+    src/ui/QGCMAVLinkInspector.cc \
+    src/ui/MAVLinkDecoder.cc
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
 macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc

src/comm/MAVLinkSimulationLink.cc

@@ -431,6 +431,7 @@ void MAVLinkSimulationLink::mainloop()
         static int rcCounter = 0;
         if (rcCounter == 2) {
             mavlink_rc_channels_raw_t chan;
+            chan.time_boot_ms = 0;
             chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000);
             chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000);
             chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000);

src/comm/MAVLinkSimulationMAV.cc

@@ -73,6 +73,7 @@ void MAVLinkSimulationMAV::mainloop()
         planner.handleMessage(msg);
 
         mavlink_servo_output_raw_t servos;
+        servos.time_usec = 0;
         servos.servo1_raw = 1000;
         servos.servo2_raw = 1250;
         servos.servo3_raw = 1400;

src/ui/MAVLinkDecoder.h

+#ifndef MAVLINKDECODER_H
+#define MAVLINKDECODER_H
+
+#include <QObject>
+#include "MAVLinkProtocol.h"
+
+class MAVLinkDecoder : public QObject
+{
+    Q_OBJECT
+public:
+    MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent = 0);
+
+signals:
+    void textMessageReceived(int uasid, int componentid, int severity, const QString& text);
+    void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec);
+    void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec);
+    void valueChanged(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec);
+    void valueChanged(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec);
+    void valueChanged(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec);
+
+
+public slots:
+    /** @brief Receive one message from the protocol and decode it */
+    void receiveMessage(LinkInterface* link,mavlink_message_t message);
+protected:
+    /** @brief Emit the value of one message field */
+    void emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time);
+
+    mavlink_message_t receivedMessages[256]; ///< Available / known messages
+    mavlink_message_info_t messageInfo[256];
+
+};
+
+#endif // MAVLINKDECODER_H

src/ui/MainWindow.cc

@@ -153,6 +153,9 @@ MainWindow::MainWindow(QWidget *parent):
 
     connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*)));
 
+    // Add generic MAVLink decoder
+    mavlinkDecoder = new MAVLinkDecoder(mavlink, this);
+
     // Connect user interface devices
     joystickWidget = 0;
     joystick = new JoystickInput();
@@ -439,6 +442,7 @@ void MainWindow::buildPxWidgets()
     if (!linechartWidget) {
         // Center widgets
         linechartWidget   = new Linecharts(this);
+        linechartWidget->addSource(mavlinkDecoder);
         addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
     }
 

src/ui/MainWindow.h

@@ -77,6 +77,8 @@ This file is part of the QGROUNDCONTROL project
 #include "UASControlParameters.h"
 #include "QGCMAVLinkInspector.h"
 
+#include "MAVLinkDecoder.h"
+
 class QGCMapTool;
 
 /**
@@ -426,6 +428,7 @@ protected:
     QPointer<QGCToolBar> toolBar;
 
     QPointer<QDockWidget> mavlinkInspectorWidget;
+    QPointer<MAVLinkDecoder> mavlinkDecoder;
 
 
     // Popup widgets

src/ui/linechart/LinechartPlot.cc

-/*=====================================================================
+ /*=====================================================================
 ======================================================================*/
 
 /**

src/ui/linechart/LinechartWidget.cc

@@ -71,7 +71,8 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
     logindex(1),
     logging(false),
     logStartTime(0),
-    updateTimer(new QTimer())
+    updateTimer(new QTimer()),
+    selectedMAV(-1)
 {
     // Add elements defined in Qt Designer
     ui.setupUi(this);
@@ -292,20 +293,24 @@ void LinechartWidget::createLayout()
 void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 usec)
 {
     static const QString unit("-");
-    if (isVisible()) {
+    if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
+    {
         // Order matters here, first append to plot, then update curve list
         activePlot->appendData(curve+unit, usec, value);
         // Store data
         QLabel* label = curveLabels->value(curve+unit, NULL);
         // Make sure the curve will be created if it does not yet exist
-        if(!label) {
+        if(!label)
+        {
             addCurve(curve, unit);
         }
     }
 
     // Log data
-    if (logging) {
-        if (activePlot->isVisible(curve+unit)) {
+    if (logging)
+    {
+        if (activePlot->isVisible(curve+unit))
+        {
             if (logStartTime == 0) logStartTime = usec;
             qint64 time = usec - logStartTime;
             if (time < 0) time = 0;
@@ -319,21 +324,25 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64
 
 void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec)
 {
-    if (isVisible()) {
+    if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
+    {
         // Order matters here, first append to plot, then update curve list
         activePlot->appendData(curve+unit, usec, value);
         // Store data
         QLabel* label = curveLabels->value(curve+unit, NULL);
         // Make sure the curve will be created if it does not yet exist
-        if(!label) {
+        if(!label)
+        {
             //qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE";
             addCurve(curve, unit);
         }
     }
 
     // Log data
-    if (logging) {
-        if (activePlot->isVisible(curve+unit)) {
+    if (logging)
+    {
+        if (activePlot->isVisible(curve+unit))
+        {
             if (logStartTime == 0) logStartTime = usec;
             qint64 time = usec - logStartTime;
             if (time < 0) time = 0;
@@ -346,13 +355,25 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
 
 void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec)
 {
-    if (isVisible()) {
+    appendData(uasId, curve, unit, static_cast<qint64>(value), usec);
+}
+
+void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec)
+{
+    appendData(uasId, curve, unit, static_cast<quint64>(value), usec);
+}
+
+void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec)
+{
+    if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
+    {
         // Order matters here, first append to plot, then update curve list
         activePlot->appendData(curve+unit, usec, value);
         // Store data
         QLabel* label = curveLabels->value(curve+unit, NULL);
         // Make sure the curve will be created if it does not yet exist
-        if(!label) {
+        if(!label)
+        {
             intData.insert(curve+unit, 0);
             addCurve(curve, unit);
         }
@@ -362,8 +383,44 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString&
     }
 
     // Log data
-    if (logging) {
-        if (activePlot->isVisible(curve+unit)) {
+    if (logging)
+    {
+        if (activePlot->isVisible(curve+unit))
+        {
+            if (logStartTime == 0) logStartTime = usec;
+            qint64 time = usec - logStartTime;
+            if (time < 0) time = 0;
+
+            logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1());
+            logFile->flush();
+        }
+    }
+}
+
+void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec)
+{
+    if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible()))
+    {
+        // Order matters here, first append to plot, then update curve list
+        activePlot->appendData(curve+unit, usec, value);
+        // Store data
+        QLabel* label = curveLabels->value(curve+unit, NULL);
+        // Make sure the curve will be created if it does not yet exist
+        if(!label)
+        {
+            intData.insert(curve+unit, 0);
+            addCurve(curve, unit);
+        }
+
+        // Add int data
+        intData.insert(curve+unit, value);
+    }
+
+    // Log data
+    if (logging)
+    {
+        if (activePlot->isVisible(curve+unit))
+        {
             if (logStartTime == 0) logStartTime = usec;
             qint64 time = usec - logStartTime;
             if (time < 0) time = 0;

src/ui/linechart/LinechartWidget.h

@@ -79,6 +79,12 @@ public slots:
     void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec);
     /** @brief Append data as int with unit */
     void appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec);
+    /** @brief Append data as unsigned int with unit */
+    void appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec);
+    /** @brief Append data as int64 with unit */
+    void appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec);
+    /** @brief Append data as uint64 with unit */
+    void appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec);
     void takeButtonClick(bool checked);
     void setPlotWindowPosition(int scrollBarValue);
     void setPlotWindowPosition(quint64 position);
@@ -88,6 +94,11 @@ public slots:
     /** @brief Stop automatic updates once hidden */
     void hideEvent(QHideEvent* event);
     void setActive(bool active);
+    void setActiveSystem(int systemid)
+    {
+        selectedMAV = systemid;
+    }
+
     /** @brief Set the number of values to average over */
     void setAverageWindow(int windowSize);
     /** @brief Start logging to file */
@@ -149,6 +160,7 @@ protected:
     QTimer* updateTimer;
     LogCompressor* compressor;
     QCheckBox* selectAllCheckBox;
+    int selectedMAV; ///< The MAV for which plot items are accepted, -1 for all systems
     static const int updateInterval = 400; ///< Time between number updates, in milliseconds
 
     static const int MAX_CURVE_MENUITEM_NUMBER = 8;

src/ui/linechart/Linecharts.cc

@@ -61,25 +61,32 @@ void Linecharts::hideEvent(QHideEvent* event)
 void Linecharts::selectSystem(int systemid)
 {
     QWidget* prevWidget = currentWidget();
-    if (prevWidget) {
+    if (prevWidget)
+    {
         LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
-        if (chart) {
+        if (chart)
+        {
             chart->setActive(false);
+            chart->setActiveSystem(systemid);
         }
     }
     QWidget* widget = plots.value(systemid, NULL);
-    if (widget) {
+    if (widget)
+    {
         setCurrentWidget(widget);
         LinechartWidget* chart = dynamic_cast<LinechartWidget*>(widget);
-        if (chart) {
+        if (chart)
+        {
             chart->setActive(true);
+            chart->setActiveSystem(systemid);
         }
     }
 }
 
 void Linecharts::addSystem(UASInterface* uas)
 {
-    if (!plots.contains(uas->getUASID())) {
+    if (!plots.contains(uas->getUASID()))
+    {
         LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this);
         addWidget(widget);
         plots.insert(uas->getUASID(), widget);
@@ -92,10 +99,38 @@ void Linecharts::addSystem(UASInterface* uas)
 
         connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString)));
         // Set system active if this is the only system
-        if (active) {
-            if (plots.size() == 1) {
+        if (active)
+        {
+            if (plots.size() == 1)
+            {
+                // FIXME XXX HACK
+                // Connect generic sources
+                for (int i = 0; i < genericSources.count(); ++i)
+                {
+                    connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64)));
+                    connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64)));
+                    connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64)));
+                    connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
+                    connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
+                }
+                // Select system
                 selectSystem(uas->getUASID());
             }
         }
     }
 }
+
+void Linecharts::addSource(QObject* obj)
+{
+    genericSources.append(obj);
+    // FIXME XXX HACK
+    if (plots.size() > 0)
+    {
+        // Connect generic source
+        connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64)));
+        connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64)));
+        connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64)));
+        connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
+        connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
+    }
+}

src/ui/linechart/Linecharts.h

@@ -3,6 +3,7 @@
 
 #include <QStackedWidget>
 #include <QMap>
+#include <QVector>
 
 #include "LinechartWidget.h"
 #include "UASInterface.h"
@@ -22,9 +23,12 @@ public slots:
     void selectSystem(int systemid);
     /** @brief Add a new system to the list of plots */
     void addSystem(UASInterface* uas);
+    /** @brief Add a new generic message source (not a system) */
+    void addSource(QObject* obj);
 
 protected:
     QMap<int, LinechartWidget*> plots;
+    QVector<QObject*> genericSources;
     bool active;
     /** @brief Start updating widget */
     void showEvent(QShowEvent* event);