The messages are separeted by sysid and msgid

src/ui/QGCMAVLinkInspector.cc

@@ -23,14 +23,11 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
     ui->systemComboBox->addItem(tr("All"), 0);
     ui->componentComboBox->addItem(tr("All"), 0);
 
-	// Store metadata for all MAVLink messages.
-	mavlink_message_info_t msg_infos[256] = MAVLINK_MESSAGE_INFO;
-	memcpy(messageInfo, msg_infos, sizeof(mavlink_message_info_t)*256);
+    // Store metadata for all MAVLink messages.
+    mavlink_message_info_t msg_infos[256] = MAVLINK_MESSAGE_INFO;
+    memcpy(messageInfo, msg_infos, sizeof(mavlink_message_info_t)*256);
 
-	// Initialize the received data for all messages to invalid (0xFF)
-    memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256);
-
-	// Set up the column headers for the message listing
+    // Set up the column headers for the message listing
     QStringList header;
     header << tr("Name");
     header << tr("Value");
@@ -53,7 +50,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
     connect(ui->clearButton, SIGNAL(clicked()), this, SLOT(clearView()));
 
     // Connect external connections
-//    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addSystem(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addSystem(UASInterface*)));
     connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
 
     // Attach the UI's refresh rate to a timer.
@@ -64,6 +61,9 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
 void QGCMAVLinkInspector::addSystem(UASInterface* uas)
 {
     ui->systemComboBox->addItem(uas->getUASName(), uas->getUASID());
+
+    // Add a tree for a new UAS
+    addUAStoTree(uas->getUASID());
 }
 
 void QGCMAVLinkInspector::selectDropDownMenuSystem(int dropdownid)
@@ -95,6 +95,18 @@ void QGCMAVLinkInspector::rebuildComponentList()
     components.clear();
 
     ui->componentComboBox->addItem(tr("All"), 0);
+
+    // Fill
+    UASInterface* uas = UASManager::instance()->getUASForId(selectedSystemID);
+    if (uas)
+    {
+        QMap<int, QString> components = uas->getComponents();
+        foreach (int id, components.keys())
+        {
+            QString name = components.value(id);
+            ui->componentComboBox->addItem(name, id);
+        }
+    }
 }
 
 void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& name)
@@ -112,52 +124,111 @@ void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& na
  */
 void QGCMAVLinkInspector::clearView()
 {
-    memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256);
+    uasTreeWidgetItems.clear();
+    uasMsgTreeItems.clear();
+    uasMavlinkStorage.clear();
+    uasMessageHz.clear();
+    uasMessageCount.clear();
+    uasLastMessageUpdate.clear();
+
     onboardMessageInterval.clear();
-	lastMessageUpdate.clear();
-	messagesHz.clear();
-    treeWidgetItems.clear();
+
     ui->treeWidget->clear();
     ui->rateTreeWidget->clear();
 }
 
 void QGCMAVLinkInspector::refreshView()
 {
-    for (int i = 0; i < 256; ++i)//mavlink_message_t msg, receivedMessages)
+    QMap<int, mavlink_message_t* >::const_iterator ite;
+    
+    for(ite=uasMavlinkStorage.constBegin(); ite!=uasMavlinkStorage.constEnd();++ite)
     {
-        mavlink_message_t* msg = receivedMessages+i;
-        // Ignore NULL values
-        if (msg->msgid == 0xFF) continue;
-        // Update the tree view
-        QString messageName("%1 (%2 Hz, #%3)");
-        float msgHz = (1.0f-updateHzLowpass)*messagesHz.value(msg->msgid, 0) + updateHzLowpass*((float)messageCount.value(msg->msgid, 0))/((float)updateInterval/1000.0f);
-        messagesHz.insert(msg->msgid, msgHz);
-        messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
-        messageCount.insert(msg->msgid, 0);
-        if (!treeWidgetItems.contains(msg->msgid))
+        for (int i = 0; i < 256; ++i)
         {
-            QStringList fields;
-            fields << messageName;
-            QTreeWidgetItem* widget = new QTreeWidgetItem(fields);
-            widget->setFirstColumnSpanned(true);
+            mavlink_message_t* msg = ite.value()+i;
+            // Ignore NULL values
+            if (msg->msgid == 0xFF) continue;
+
+            // Update the message frenquency
+
+            // Get the previous frequency for low-pass filtering
+            float msgHz = 0.0f;
+            QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
+            QMap<int, float>* uasMsgHz = iteHz.value();
 
-            for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
+            while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
             {
-                QTreeWidgetItem* field = new QTreeWidgetItem();
-                widget->addChild(field);
+                if(iteHz.value()->contains(msg->msgid))
+                {
+                    uasMsgHz = iteHz.value();
+                    msgHz = iteHz.value()->value(msg->msgid);
+                    break;
+                }
+                ++iteHz;
             }
 
-            treeWidgetItems.insert(msg->msgid, widget);
-            ui->treeWidget->addTopLevelItem(widget);
-        }
+            // Get the number of message received
+            float msgCount = 0;
+            QMap<int, QMap<int, unsigned int> * >::const_iterator iter = uasMessageCount.find(msg->sysid);
+            QMap<int, unsigned int>* uasMsgCount = iter.value();
+            while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
+            {
+                if(iter.value()->contains(msg->msgid))
+                {
+                    msgCount = (float) iter.value()->value(msg->msgid);
+                    uasMsgCount = iter.value();
+                    break;
+                }
+                ++iter;
+            }
 
-        // Set Hz
-        QTreeWidgetItem* message = treeWidgetItems.value(msg->msgid);
-        message->setFirstColumnSpanned(true);
-        message->setData(0, Qt::DisplayRole, QVariant(messageName));
-        for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
-        {
-            updateField(msg->msgid, i, message->child(i));
+            // Compute the new low-pass filtered frequency and update the message count
+            msgHz = (1.0f-updateHzLowpass)* msgHz + updateHzLowpass*msgCount/((float)updateInterval/1000.0f);
+            uasMsgHz->insert(msg->msgid,msgHz);
+            uasMsgCount->insert(msg->msgid,(unsigned int) 0);
+
+            // Update the tree view
+            QString messageName("%1 (%2 Hz, #%3)");
+            messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
+
+            addUAStoTree(msg->sysid);
+
+            // Look for the tree for the UAS sysid
+            QMap<int, QTreeWidgetItem*>* msgTreeItems = uasMsgTreeItems.value(msg->sysid);
+            if (!msgTreeItems)
+            {
+                // The UAS tree has not been created yet, no update
+                return;
+            }
+
+            // Add the message with msgid to the tree if not done yet
+            if(!msgTreeItems->contains(msg->msgid))
+            {
+                QStringList fields;
+                fields << messageName;
+                QTreeWidgetItem* widget = new QTreeWidgetItem();
+                for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
+                {
+                    QTreeWidgetItem* field = new QTreeWidgetItem();
+                    widget->addChild(field);
+                }
+                msgTreeItems->insert(msg->msgid,widget);
+                QList<int> groupKeys = msgTreeItems->uniqueKeys();
+                int insertIndex = groupKeys.indexOf(msg->msgid);
+                uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
+            }
+
+            // Update the message
+            QTreeWidgetItem* message = msgTreeItems->value(msg->msgid);
+            if(message)
+            {
+                message->setFirstColumnSpanned(true);
+                message->setData(0, Qt::DisplayRole, QVariant(messageName));
+                for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
+                {
+                    updateField(msg->sysid,msg->msgid, i, message->child(i));
+                }
+            }
         }
     }
 
@@ -201,39 +272,121 @@ void QGCMAVLinkInspector::refreshView()
     }
 }
 
+void QGCMAVLinkInspector::addUAStoTree(int sysId)
+{
+    if(!uasTreeWidgetItems.contains(sysId))
+    {
+        // Add the UAS to the main tree after it has been created
+        UASInterface* uas = UASManager::instance()->getUASForId(sysId);
+
+        if (uas)
+        {
+            QStringList idstring;
+            if (uas->getUASName() == "")
+            {
+                idstring << tr("UAS ") + QString::number(uas->getUASID());
+            }
+            else
+            {
+                idstring << uas->getUASName();
+            }
+            QTreeWidgetItem* uasWidget = new QTreeWidgetItem(idstring);
+            uasWidget->setFirstColumnSpanned(true);
+            uasTreeWidgetItems.insert(sysId,uasWidget);
+            ui->treeWidget->addTopLevelItem(uasWidget);
+            uasMsgTreeItems.insert(sysId,new QMap<int, QTreeWidgetItem*>());
+        }
+    }
+}
+
 void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
 {
     Q_UNUSED(link);
-    if (selectedSystemID != 0 && selectedSystemID != message.sysid) return;
-    if (selectedComponentID != 0 && selectedComponentID != message.compid) return;
-    // Only overwrite if system filter is set
-    memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
-
-    // Add not yet seen systems / components
-    if (!systems.contains(message.sysid)) {
-        systems.insert(message.sysid, message.sysid);
-        ui->systemComboBox->addItem(tr("MAV%1").arg(message.sysid), message.sysid);
+
+    quint64 receiveTime;
+
+    // Create dynamically an array to store the messages for each UAS
+    if(!uasMavlinkStorage.contains(message.sysid))
+    {
+        mavlink_message_t* msg = new mavlink_message_t[256];
+
+        // Initialize the received data for all messages to invalid (0xFF)
+        memset(msg,0xFF, sizeof(mavlink_message_t)*256);
+        uasMavlinkStorage.insert(message.sysid,msg);
     }
 
-    if (!components.contains(message.compid)) {
-        components.insert(message.compid, message.compid);
-        ui->componentComboBox->addItem(tr("COMP%1").arg(message.compid), message.compid);
+    // Fill the slot with the message received
+    memcpy(uasMavlinkStorage.value(message.sysid)+message.msgid,&message,sizeof(mavlink_message_t));
+
+    // Looking if this message has already been received once
+    bool 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;
     }
 
-    quint64 receiveTime = QGC::groundTimeMilliseconds();
-    if (lastMessageUpdate.contains(message.msgid))
+    receiveTime = QGC::groundTimeMilliseconds();
+
+    // If the message doesn't exist, create a map for the frequency, message count and time of reception
+    if(!msgFound)
     {
-        int count = messageCount.value(message.msgid, 0);
-        messageCount.insert(message.msgid, count+1);
+        // 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;
     }
 
-    lastMessageUpdate.insert(message.msgid, receiveTime);
+    // 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);
 
-    if (selectedSystemID == 0 || selectedComponentID == 0) {
+    if (selectedSystemID == 0 || selectedComponentID == 0)
+    {
         return;
     }
 
-    switch (message.msgid) {
+    switch (message.msgid)
+    {
         case MAVLINK_MSG_ID_DATA_STREAM:
             {
                 mavlink_data_stream_t stream;
@@ -283,11 +436,11 @@ QGCMAVLinkInspector::~QGCMAVLinkInspector()
     delete ui;
 }
 
-void QGCMAVLinkInspector::updateField(int msgid, int fieldid, QTreeWidgetItem* item)
+void QGCMAVLinkInspector::updateField(int sysid, int msgid, int fieldid, QTreeWidgetItem* item)
 {
     // Add field tree widget item
     item->setData(0, Qt::DisplayRole, QVariant(messageInfo[msgid].fields[fieldid].name));
-    uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
+    uint8_t* m = ((uint8_t*)(uasMavlinkStorage.value(sysid)+msgid))+8;
     switch (messageInfo[msgid].fields[fieldid].type)
     {
     case MAVLINK_TYPE_CHAR:

src/ui/QGCMAVLinkInspector.h

@@ -26,9 +26,11 @@ public slots:
     void receiveMessage(LinkInterface* link,mavlink_message_t message);
     /** @brief Clear all messages */
     void clearView();
-    /** Update view */
+    /** @brief Update view */
     void refreshView();
+    /** @brief Add system to the list */
     void addSystem(UASInterface* uas);
+    /** @brief Add component to the list */
     void addComponent(int uas, int component, const QString& name);
     /** @Brief Select a system through the drop down menu */
     void selectDropDownMenuSystem(int dropdownid);
@@ -43,25 +45,31 @@ protected:
     int selectedComponentID;       ///< Currently selected component
     QMap<int, int> systems;     ///< Already observed systems
     QMap<int, int> components; ///< Already observed components
-    QMap<int, quint64> lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color
-    QMap<int, float> messagesHz; ///< Used to store update rate in Hz
     QMap<int, float> onboardMessageInterval; ///< Stores the onboard selected data rate
-    QMap<int, unsigned int> messageCount; ///< Used to store the message count
-    mavlink_message_t receivedMessages[256]; ///< Available / known messages
-    QMap<int, QTreeWidgetItem*> treeWidgetItems;   ///< Available tree widget items
     QMap<int, QTreeWidgetItem*> rateTreeWidgetItems; ///< Available rate tree widget items
     QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI
     mavlink_message_info_t messageInfo[256]; // Store the metadata for all available MAVLink messages.
 
-    // Update one message field
-    void updateField(int msgid, int fieldid, QTreeWidgetItem* item);
+    QMap<int, QTreeWidgetItem* > uasTreeWidgetItems; ///< Tree of available uas with their widget
+    QMap<int, QMap<int, QTreeWidgetItem*>* > uasMsgTreeItems; ///< Stores the widget of the received message for each UAS
+
+    QMap<int, mavlink_message_t* > uasMavlinkStorage; ///< Stores the message array for every 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
+
+    QMap<int, QMap<int, quint64>* > uasLastMessageUpdate; ///< Stores the time of the last message for each message of each UAS
+
+    /* @brief Update one message field */
+    void updateField(int sysid, int msgid, int fieldid, QTreeWidgetItem* item);
     /** @brief Rebuild the list of components */
     void rebuildComponentList();
     /** @brief Change the stream interval */
     void changeStreamInterval(int msgid, int interval);
+    /* @brief Create a new tree for a new UAS */
+    void addUAStoTree(int sysId);
 
-    static const unsigned int updateInterval;
-    static const float updateHzLowpass;
+    static const unsigned int updateInterval; ///< The update interval of the refresh function
+    static const float updateHzLowpass; ///< The low-pass filter value for the frequency of each message
 
 private:
     Ui::QGCMAVLinkInspector *ui;