From 942c554bbf6c8b63d7d92db47b1f152a52f1f1a2 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Fri, 12 Apr 2019 12:33:54 -0300 Subject: [PATCH] Early work on the MAVLink Inspector Controller --- qgroundcontrol.pro | 10 +- src/AnalyzeView/CMakeLists.txt | 1 + src/AnalyzeView/GeoTagController.cc | 2 - src/AnalyzeView/MAVLinkInspectorController.cc | 161 ++++++++++++++++++ src/AnalyzeView/MAVLinkInspectorController.h | 42 +++++ src/QGCApplication.cc | 2 + 6 files changed, 212 insertions(+), 6 deletions(-) create mode 100644 src/AnalyzeView/MAVLinkInspectorController.cc create mode 100644 src/AnalyzeView/MAVLinkInspectorController.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 885b7ef5f..f60decf7f 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/AnalyzeView/CMakeLists.txt b/src/AnalyzeView/CMakeLists.txt index 02640f620..a92627a29 100644 --- a/src/AnalyzeView/CMakeLists.txt +++ b/src/AnalyzeView/CMakeLists.txt @@ -9,6 +9,7 @@ endif() add_library(AnalyzeView ExifParser.cc GeoTagController.cc + MAVLinkInspectorController.cc LogDownloadController.cc MavlinkConsoleController.cc PX4LogParser.cc diff --git a/src/AnalyzeView/GeoTagController.cc b/src/AnalyzeView/GeoTagController.cc index dce5469f8..c2e36ef7e 100644 --- a/src/AnalyzeView/GeoTagController.cc +++ b/src/AnalyzeView/GeoTagController.cc @@ -8,11 +8,9 @@ ****************************************************************************/ #include "GeoTagController.h" -//#include "QGCQFileDialog.h" #include "QGCLoggingCategory.h" #include #include -#include #include #include #include diff --git a/src/AnalyzeView/MAVLinkInspectorController.cc b/src/AnalyzeView/MAVLinkInspectorController.cc new file mode 100644 index 000000000..e242824f4 --- /dev/null +++ b/src/AnalyzeView/MAVLinkInspectorController.cc @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * 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::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* >::const_iterator ite = _uasLastMessageUpdate.find(message.sysid); + QMap* 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* messageHz = new QMap; + messageHz->insert(message.msgid,0.0f); + _uasMessageHz.insertMulti(message.sysid,messageHz); + //-- Create a map for the message count + QMap* messagesCount = new QMap; + messagesCount->insert(message.msgid,0); + _uasMessageCount.insertMulti(message.sysid,messagesCount); + //-- Create a map for the time of reception of the message + QMap* lastMessage = new QMap; + 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* >::const_iterator iter = _uasMessageCount.find(message.sysid); + QMap * 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::iterator ite; + for(ite = _uasMessageStorage.begin(); ite != _uasMessageStorage.end(); ++ite) { + delete ite.value(); + ite.value() = nullptr; + } + _uasMessageStorage.clear(); + QMap* >::iterator iteHz; + for(iteHz = _uasMessageHz.begin(); iteHz != _uasMessageHz.end(); ++iteHz) { + iteHz.value()->clear(); + delete iteHz.value(); + iteHz.value() = nullptr; + } + _uasMessageHz.clear(); + QMap*>::iterator iteCount; + for(iteCount = _uasMessageCount.begin(); iteCount != _uasMessageCount.end(); ++iteCount) { + iteCount.value()->clear(); + delete iteCount.value(); + iteCount.value() = nullptr; + } + _uasMessageCount.clear(); + QMap* >::iterator iteLast; + for(iteLast = _uasLastMessageUpdate.begin(); iteLast != _uasLastMessageUpdate.end(); ++iteLast) { + iteLast.value()->clear(); + delete iteLast.value(); + iteLast.value() = nullptr; + } + _uasLastMessageUpdate.clear(); +} diff --git a/src/AnalyzeView/MAVLinkInspectorController.h b/src/AnalyzeView/MAVLinkInspectorController.h new file mode 100644 index 000000000..a9d76e975 --- /dev/null +++ b/src/AnalyzeView/MAVLinkInspectorController.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * 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 +#include +#include + +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 _vehicleIDs; + QMap _uasMessageStorage; ///< Stores the messages for every UAS + QMap*> _uasLastMessageUpdate; ///< Stores the time of the last message for each message of each UAS + QMap*> _uasMessageHz; ///< Stores the frequency of each message of each UAS + QMap*> _uasMessageCount; ///< Stores the message count of each message of each UAS +}; diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 116e7b390..cc9357a84 100644 --- a/src/QGCApplication.cc +++ b/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 (kQGCControllers, 1, 0, "GeoTagController"); #endif qmlRegisterType (kQGCControllers, 1, 0, "MavlinkConsoleController"); + qmlRegisterType (kQGCControllers, 1, 0, "MAVLinkInspectorController"); // Register Qml Singletons qmlRegisterSingletonType ("QGroundControl", 1, 0, "QGroundControl", qgroundcontrolQmlGlobalSingletonFactory); -- 2.22.0