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,11 +141,10 @@ 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;
+
+        mavlink_message_t* msg = ite.value();
         // Ignore NULL values
         if (msg->msgid == 0xFF) continue;
 
@@ -230,7 +229,6 @@ void QGCMAVLinkInspector::refreshView()
             }
         }
     }
-    }
 
     if (selectedSystemID == 0 || selectedComponentID == 0)
     {
@@ -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];
-
-        // Initialize the received data for all messages to invalid (0xFF)
-        memset(msg,0xFF, sizeof(mavlink_message_t)*256);
-        uasMavlinkStorage.insert(message.sysid,msg);
+        mavlink_message_t* msg = new mavlink_message_t;
+        *msg = message;
+        uasMessageStorage.insertMulti(message.sysid,msg);
     }
 
-    // Fill the slot with the message received
-    memcpy(uasMavlinkStorage.value(message.sysid)+message.msgid,&message,sizeof(mavlink_message_t));
+    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
-    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,7 +375,8 @@ 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)))
         {
@@ -379,6 +397,7 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
             }
         }
         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