Early work on the MAVLink Inspector Controller

qgroundcontrol.pro

@@ -396,10 +396,11 @@ SOURCES += \
 # Main QGC Headers and Source files
 
 HEADERS += \
-    src/AnalyzeView/ExifParser.h \
     src/AnalyzeView/LogDownloadController.h \
     src/AnalyzeView/PX4LogParser.h \
     src/AnalyzeView/ULogParser.h \
+    src/AnalyzeView/MAVLinkInspectorController.h \
+    src/AnalyzeView/MavlinkConsoleController.h \
     src/Audio/AudioOutput.h \
     src/Camera/QGCCameraControl.h \
     src/Camera/QGCCameraIO.h \
@@ -506,7 +507,6 @@ HEADERS += \
     src/uas/UASInterface.h \
     src/uas/UASMessageHandler.h \
     src/UTM.h \
-    src/AnalyzeView/MavlinkConsoleController.h \
 
 
 AndroidBuild {
@@ -542,6 +542,7 @@ HEADERS += \
 !MobileBuild {
 HEADERS += \
     src/AnalyzeView/GeoTagController.h \
+    src/AnalyzeView/ExifParser.h \
     src/GPS/Drivers/src/gps_helper.h \
     src/GPS/Drivers/src/rtcm.h \
     src/GPS/Drivers/src/ashtech.h \
@@ -575,10 +576,11 @@ AndroidBuild {
 }
 
 SOURCES += \
-    src/AnalyzeView/ExifParser.cc \
     src/AnalyzeView/LogDownloadController.cc \
     src/AnalyzeView/PX4LogParser.cc \
     src/AnalyzeView/ULogParser.cc \
+    src/AnalyzeView/MAVLinkInspectorController.cc \
+    src/AnalyzeView/MavlinkConsoleController.cc \
     src/Audio/AudioOutput.cc \
     src/Camera/QGCCameraControl.cc \
     src/Camera/QGCCameraIO.cc \
@@ -680,7 +682,6 @@ SOURCES += \
     src/uas/UAS.cc \
     src/uas/UASMessageHandler.cc \
     src/UTM.cpp \
-    src/AnalyzeView/MavlinkConsoleController.cc \
 
 DebugBuild {
 SOURCES += \
@@ -703,6 +704,7 @@ contains(DEFINES, QGC_ENABLE_BLUETOOTH) {
 !MobileBuild {
 SOURCES += \
     src/AnalyzeView/GeoTagController.cc \
+    src/AnalyzeView/ExifParser.cc \
     src/GPS/Drivers/src/gps_helper.cpp \
     src/GPS/Drivers/src/rtcm.cpp \
     src/GPS/Drivers/src/ashtech.cpp \

src/AnalyzeView/CMakeLists.txt

@@ -9,6 +9,7 @@ endif()
 add_library(AnalyzeView
 	ExifParser.cc
 	GeoTagController.cc
+	MAVLinkInspectorController.cc
 	LogDownloadController.cc
 	MavlinkConsoleController.cc
 	PX4LogParser.cc

src/AnalyzeView/GeoTagController.cc

@@ -8,11 +8,9 @@
  ****************************************************************************/
 
 #include "GeoTagController.h"
-//#include "QGCQFileDialog.h"
 #include "QGCLoggingCategory.h"
 #include <math.h>
 #include <QtEndian>
-#include <QMessageBox>
 #include <QDebug>
 #include <cfloat>
 #include <QDir>

src/AnalyzeView/MAVLinkInspectorController.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "MAVLinkInspectorController.h"
+#include "QGCApplication.h"
+#include "MultiVehicleManager.h"
+
+//-----------------------------------------------------------------------------
+MAVLinkInspectorController::MAVLinkInspectorController()
+{
+    MultiVehicleManager* multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager();
+    connect(multiVehicleManager, &MultiVehicleManager::vehicleAdded,   this, &MAVLinkInspectorController::_vehicleAdded);
+    connect(multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkInspectorController::_vehicleRemoved);
+    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
+    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &MAVLinkInspectorController::_receiveMessage);
+}
+
+//-----------------------------------------------------------------------------
+MAVLinkInspectorController::~MAVLinkInspectorController()
+{
+    _reset();
+}
+
+//-----------------------------------------------------------------------------
+void
+MAVLinkInspectorController::_vehicleAdded(Vehicle* vehicle)
+{
+    _vehicleIDs.append(vehicle->id());
+}
+
+//-----------------------------------------------------------------------------
+void
+MAVLinkInspectorController::_vehicleRemoved(Vehicle* vehicle)
+{
+    int idx = _vehicleIDs.indexOf(vehicle->id());
+    if(idx >= 0) {
+        _vehicleIDs.removeAt(idx);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+MAVLinkInspectorController::_receiveMessage(LinkInterface* link,mavlink_message_t message)
+{
+    Q_UNUSED(link);
+    quint64 receiveTime;
+    if (_selectedSystemID    != 0 && _selectedSystemID    != message.sysid)  return;
+    if (_selectedComponentID != 0 && _selectedComponentID != message.compid) return;
+    // Create dynamically an array to store the messages for each UAS
+    if (!_uasMessageStorage.contains(message.sysid)) {
+        mavlink_message_t* msg = new mavlink_message_t;
+        *msg = message;
+        _uasMessageStorage.insertMulti(message.sysid,msg);
+    }
+    bool msgFound = false;
+    QMap<int, mavlink_message_t*>::const_iterator iteMsg = _uasMessageStorage.find(message.sysid);
+    mavlink_message_t* uasMessage = iteMsg.value();
+    while((iteMsg != _uasMessageStorage.end()) && (iteMsg.key() == message.sysid)) {
+        if (iteMsg.value()->msgid == message.msgid) {
+            msgFound   = true;
+            uasMessage = iteMsg.value();
+            break;
+        }
+        ++iteMsg;
+    }
+    if (!msgFound) {
+        mavlink_message_t* msgIdMessage = new mavlink_message_t;
+        *msgIdMessage = message;
+        _uasMessageStorage.insertMulti(message.sysid,msgIdMessage);
+    } else {
+        *uasMessage = message;
+    }
+    // Looking if this message has already been received once
+    msgFound = false;
+    QMap<int, QMap<int, quint64>* >::const_iterator ite = _uasLastMessageUpdate.find(message.sysid);
+    QMap<int, quint64>* lastMsgUpdate = ite.value();
+    while((ite != _uasLastMessageUpdate.end()) && (ite.key() == message.sysid)) {
+        if(ite.value()->contains(message.msgid)) {
+            msgFound = true;
+            //-- Point to the found message
+            lastMsgUpdate = ite.value();
+            break;
+        }
+        ++ite;
+    }
+    receiveTime = QGC::groundTimeMilliseconds();
+    //-- If the message doesn't exist, create a map for the frequency, message count and time of reception
+    if(!msgFound) {
+        //-- Create a map for the message frequency
+        QMap<int, float>* messageHz = new QMap<int,float>;
+        messageHz->insert(message.msgid,0.0f);
+        _uasMessageHz.insertMulti(message.sysid,messageHz);
+        //-- Create a map for the message count
+        QMap<int, unsigned int>* messagesCount = new QMap<int, unsigned int>;
+        messagesCount->insert(message.msgid,0);
+        _uasMessageCount.insertMulti(message.sysid,messagesCount);
+        //-- Create a map for the time of reception of the message
+        QMap<int, quint64>* lastMessage = new QMap<int, quint64>;
+        lastMessage->insert(message.msgid,receiveTime);
+        _uasLastMessageUpdate.insertMulti(message.sysid,lastMessage);
+        //-- Point to the created message
+        lastMsgUpdate = lastMessage;
+    } else {
+        //-- The message has been found/created
+        if((lastMsgUpdate->contains(message.msgid)) && (_uasMessageCount.contains(message.sysid))) {
+            //-- Looking for and updating the message count
+            unsigned int count = 0;
+            QMap<int, QMap<int, unsigned int>* >::const_iterator iter = _uasMessageCount.find(message.sysid);
+            QMap<int, unsigned int> * uasMsgCount = iter.value();
+            while((iter != _uasMessageCount.end()) && (iter.key() == message.sysid)) {
+                if(iter.value()->contains(message.msgid)) {
+                    uasMsgCount = iter.value();
+                    count = uasMsgCount->value(message.msgid,0);
+                    uasMsgCount->insert(message.msgid,count+1);
+                    break;
+                }
+                ++iter;
+            }
+        }
+        lastMsgUpdate->insert(message.msgid,receiveTime);
+    }
+}
+
+//-----------------------------------------------------------------------------
+void
+MAVLinkInspectorController::_reset()
+{
+    QMap<int, mavlink_message_t* >::iterator ite;
+    for(ite = _uasMessageStorage.begin(); ite != _uasMessageStorage.end(); ++ite) {
+        delete ite.value();
+        ite.value() = nullptr;
+    }
+    _uasMessageStorage.clear();
+    QMap<int, QMap<int, float>* >::iterator iteHz;
+    for(iteHz = _uasMessageHz.begin(); iteHz != _uasMessageHz.end(); ++iteHz) {
+        iteHz.value()->clear();
+        delete iteHz.value();
+        iteHz.value() = nullptr;
+    }
+    _uasMessageHz.clear();
+    QMap<int, QMap<int, unsigned int>*>::iterator iteCount;
+    for(iteCount = _uasMessageCount.begin(); iteCount != _uasMessageCount.end(); ++iteCount) {
+        iteCount.value()->clear();
+        delete iteCount.value();
+        iteCount.value() = nullptr;
+    }
+    _uasMessageCount.clear();
+    QMap<int, QMap<int, quint64>* >::iterator iteLast;
+    for(iteLast = _uasLastMessageUpdate.begin(); iteLast != _uasLastMessageUpdate.end(); ++iteLast) {
+        iteLast.value()->clear();
+        delete iteLast.value();
+        iteLast.value() = nullptr;
+    }
+    _uasLastMessageUpdate.clear();
+}

src/AnalyzeView/MAVLinkInspectorController.h

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "MAVLinkProtocol.h"
+#include "Vehicle.h"
+
+#include <QObject>
+#include <QString>
+#include <QDebug>
+
+class MAVLinkInspectorController : public QObject
+{
+    Q_OBJECT
+public:
+    MAVLinkInspectorController();
+    ~MAVLinkInspectorController();
+
+private slots:
+    void _receiveMessage        (LinkInterface* link, mavlink_message_t message);
+    void _vehicleAdded          (Vehicle* vehicle);
+    void _vehicleRemoved        (Vehicle* vehicle);
+
+private:
+    void _reset                 ();
+
+private:
+    int         _selectedSystemID       = 0;                    ///< Currently selected system
+    int         _selectedComponentID    = 0;                    ///< Currently selected component
+    QList<int>  _vehicleIDs;
+    QMap<int, mavlink_message_t*>       _uasMessageStorage;     ///< Stores the messages for every UAS
+    QMap<int, QMap<int, quint64>*>      _uasLastMessageUpdate;  ///< Stores the time of the last message for each message of each UAS
+    QMap<int, QMap<int, float>*>        _uasMessageHz;          ///< Stores the frequency of each message of each UAS
+    QMap<int, QMap<int, unsigned int>*> _uasMessageCount;       ///< Stores the message count of each message of each UAS
+};

src/QGCApplication.cc

@@ -91,6 +91,7 @@
 #include "QGCFileDownload.h"
 #include "FirmwareImage.h"
 #include "MavlinkConsoleController.h"
+#include "MAVLinkInspectorController.h"
 #ifndef __mobile__
 #include "FirmwareUpgradeController.h"
 #endif
@@ -455,6 +456,7 @@ void QGCApplication::_initCommon()
     qmlRegisterType<GeoTagController>               (kQGCControllers,                       1, 0, "GeoTagController");
 #endif
     qmlRegisterType<MavlinkConsoleController>       (kQGCControllers,                       1, 0, "MavlinkConsoleController");
+    qmlRegisterType<MAVLinkInspectorController>     (kQGCControllers,                       1, 0, "MAVLinkInspectorController");
 
     // Register Qml Singletons
     qmlRegisterSingletonType<QGroundControlQmlGlobal>   ("QGroundControl",                          1, 0, "QGroundControl",         qgroundcontrolQmlGlobalSingletonFactory);