Change from an array of message to a map of message linking the sysid and the message

src/ui/QGCMAVLinkInspector.cc

@@ -126,7 +126,7 @@ void QGCMAVLinkInspector::clearView()
 {
     uasTreeWidgetItems.clear();
     uasMsgTreeItems.clear();
-    uasMavlinkStorage.clear();
+    uasMessageStorage.clear();
     uasMessageHz.clear();
     uasMessageCount.clear();
     uasLastMessageUpdate.clear();
@@ -141,93 +141,91 @@ void QGCMAVLinkInspector::refreshView()
 {
     QMap<int, mavlink_message_t* >::const_iterator ite;
     
-    for(ite=uasMavlinkStorage.constBegin(); ite!=uasMavlinkStorage.constEnd();++ite)
+    for(ite=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
     {
-        for (int i = 0; i < 256; ++i)
-        {
-            mavlink_message_t* msg = ite.value()+i;
-            // Ignore NULL values
-            if (msg->msgid == 0xFF) continue;
 
-            // Update the message frenquency
+        mavlink_message_t* msg = ite.value();
+        // Ignore NULL values
+        if (msg->msgid == 0xFF) continue;
+
+        // Update the message frenquency
 
-            // Get the previous frequency for low-pass filtering
+        // Get the previous frequency for low-pass filtering
-            float msgHz = 0.0f;
+        float msgHz = 0.0f;
-            QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
+        QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
-            QMap<int, float>* uasMsgHz = iteHz.value();
+        QMap<int, float>* uasMsgHz = iteHz.value();
 
-            while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
+        while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
+        {
+            if(iteHz.value()->contains(msg->msgid))
             {
-                if(iteHz.value()->contains(msg->msgid))
+                uasMsgHz = iteHz.value();
-                {
+                msgHz = iteHz.value()->value(msg->msgid);
-                    uasMsgHz = iteHz.value();
+                break;
-                    msgHz = iteHz.value()->value(msg->msgid);
-                    break;
-                }
-                ++iteHz;
             }
+            ++iteHz;
+        }
 
-            // Get the number of message received
+        // Get the number of message received
-            float msgCount = 0;
+        float msgCount = 0;
-            QMap<int, QMap<int, unsigned int> * >::const_iterator iter = uasMessageCount.find(msg->sysid);
+        QMap<int, QMap<int, unsigned int> * >::const_iterator iter = uasMessageCount.find(msg->sysid);
-            QMap<int, unsigned int>* uasMsgCount = iter.value();
+        QMap<int, unsigned int>* uasMsgCount = iter.value();
-            while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
+        while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
+        {
+            if(iter.value()->contains(msg->msgid))
             {
-                if(iter.value()->contains(msg->msgid))
+                msgCount = (float) iter.value()->value(msg->msgid);
-                {
+                uasMsgCount = iter.value();
-                    msgCount = (float) iter.value()->value(msg->msgid);
+                break;
-                    uasMsgCount = iter.value();
-                    break;
-                }
-                ++iter;
             }
+            ++iter;
+        }
 
-            // Compute the new low-pass filtered frequency and update the message count
+        // Compute the new low-pass filtered frequency and update the message count
-            msgHz = (1.0f-updateHzLowpass)* msgHz + updateHzLowpass*msgCount/((float)updateInterval/1000.0f);
+        msgHz = (1.0f-updateHzLowpass)* msgHz + updateHzLowpass*msgCount/((float)updateInterval/1000.0f);
-            uasMsgHz->insert(msg->msgid,msgHz);
+        uasMsgHz->insert(msg->msgid,msgHz);
-            uasMsgCount->insert(msg->msgid,(unsigned int) 0);
+        uasMsgCount->insert(msg->msgid,(unsigned int) 0);
 
-            // Update the tree view
+        // Update the tree view
-            QString messageName("%1 (%2 Hz, #%3)");
+        QString messageName("%1 (%2 Hz, #%3)");
-            messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
+        messageName = messageName.arg(messageInfo[msg->msgid].name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
 
-            addUAStoTree(msg->sysid);
+        addUAStoTree(msg->sysid);
 
-            // Look for the tree for the UAS sysid
+        // Look for the tree for the UAS sysid
-            QMap<int, QTreeWidgetItem*>* msgTreeItems = uasMsgTreeItems.value(msg->sysid);
+        QMap<int, QTreeWidgetItem*>* msgTreeItems = uasMsgTreeItems.value(msg->sysid);
-            if (!msgTreeItems)
+        if (!msgTreeItems)
-            {
+        {
-                // The UAS tree has not been created yet, no update
+            // The UAS tree has not been created yet, no update
-                return;
+            return;
-            }
+        }
 
-            // Add the message with msgid to the tree if not done yet
+        // Add the message with msgid to the tree if not done yet
-            if(!msgTreeItems->contains(msg->msgid))
+        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)
             {
-                QStringList fields;
+                QTreeWidgetItem* field = new QTreeWidgetItem();
-                fields << messageName;
+                widget->addChild(field);
-                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);
             }
+            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
+        // Update the message
-            QTreeWidgetItem* message = msgTreeItems->value(msg->msgid);
+        QTreeWidgetItem* message = msgTreeItems->value(msg->msgid);
-            if(message)
+        if(message)
+        {
+            message->setFirstColumnSpanned(true);
+            message->setData(0, Qt::DisplayRole, QVariant(messageName));
+            for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
             {
-                message->setFirstColumnSpanned(true);
+                updateField(msg->sysid,msg->msgid, i, message->child(i));
-                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));
-                }
             }
         }
     }
@@ -306,20 +304,39 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
     quint64 receiveTime;
 
     // Create dynamically an array to store the messages for each UAS
-    if(!uasMavlinkStorage.contains(message.sysid))
+    if (!uasMessageStorage.contains(message.sysid))
     {
-        mavlink_message_t* msg = new mavlink_message_t[256];
+        mavlink_message_t* msg = new mavlink_message_t;
-
+        *msg = message;
-        // Initialize the received data for all messages to invalid (0xFF)
+        uasMessageStorage.insertMulti(message.sysid,msg);
-        memset(msg,0xFF, sizeof(mavlink_message_t)*256);
-        uasMavlinkStorage.insert(message.sysid,msg);
     }
 
-    // Fill the slot with the message received
+    bool msgFound = false;
-    memcpy(uasMavlinkStorage.value(message.sysid)+message.msgid,&message,sizeof(mavlink_message_t));
+    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
-    bool msgFound = false;
+    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))
@@ -358,27 +375,29 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
         // 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
+        // The message has been found/created
-        unsigned int count = 0;
+        if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid)))
-        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))
+            // 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))
             {
-                uasMsgCount = iter.value();
+                if(iter.value()->contains(message.msgid))
-                count = uasMsgCount->value(message.msgid,0);
+                {
-                uasMsgCount->insert(message.msgid,count+1);
+                    uasMsgCount = iter.value();
-                break;
+                    count = uasMsgCount->value(message.msgid,0);
+                    uasMsgCount->insert(message.msgid,count+1);
+                    break;
+                }
+                ++iter;
             }
-            ++iter;
         }
+        lastMsgUpdate->insert(message.msgid,receiveTime);
     }
-    lastMsgUpdate->insert(message.msgid,receiveTime);
 
     if (selectedSystemID == 0 || selectedComponentID == 0)
     {
@@ -440,7 +459,29 @@ void QGCMAVLinkInspector::updateField(int sysid, int msgid, int fieldid, QTreeWi
 {
     // Add field tree widget item
     item->setData(0, Qt::DisplayRole, QVariant(messageInfo[msgid].fields[fieldid].name));
-    uint8_t* m = ((uint8_t*)(uasMavlinkStorage.value(sysid)+msgid))+8;
+    
+    bool msgFound = false;
+    QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(sysid);
+    mavlink_message_t* uasMessage = iteMsg.value();
+    while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == sysid))
+    {
+        if (iteMsg.value()->msgid == msgid)
+        {
+            msgFound = true;
+            uasMessage = iteMsg.value();
+            break;
+        }
+        ++iteMsg;
+    }
+
+    if (!msgFound)
+    {
+        return;
+    }
+    
+    uint8_t* m = ((uint8_t*)uasMessage)+8;
+
+
     switch (messageInfo[msgid].fields[fieldid].type)
     {
     case MAVLINK_TYPE_CHAR:

src/ui/QGCMAVLinkInspector.h

@@ -53,7 +53,8 @@ protected:
     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, mavlink_message_t* > uasMessageStorage; ///< Stores the messages 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