Merge pull request #864 from ndousse/sortMsgBySysId

In MAVLinkInspector, the messages are separeted by sysid and msgid

src/ui/QGCMAVLinkInspector.cc

@@ -27,9 +27,6 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
     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
     QStringList header;
     header << tr("Name");
@@ -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,12 +95,25 @@ 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)
 {
     Q_UNUSED(component);
     Q_UNUSED(name);
+    
     if (uas != selectedSystemID) return;
 
     rebuildComponentList();
@@ -112,52 +125,162 @@ void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& na
  */
 void QGCMAVLinkInspector::clearView()
 {
-    memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256);
+    QMap<int, mavlink_message_t* >::iterator ite;
+    for(ite=uasMessageStorage.begin(); ite!=uasMessageStorage.end();++ite)
+    {
+        delete ite.value();
+        ite.value() = NULL;
+    }
+    uasMessageStorage.clear();
+
+    QMap<int, QMap<int, QTreeWidgetItem*>* >::iterator iteMsg;
+    for (iteMsg=uasMsgTreeItems.begin(); iteMsg!=uasMsgTreeItems.end();++iteMsg)
+    {
+        QMap<int, QTreeWidgetItem*>* msgTreeItems = iteMsg.value();
+
+        QList<int> groupKeys = msgTreeItems->uniqueKeys();
+        QList<int>::iterator listKeys;
+        for (listKeys=groupKeys.begin();listKeys!=groupKeys.end();++listKeys)
+        {
+            delete msgTreeItems->take(*listKeys);
+        }
+    }
+    uasMsgTreeItems.clear();
+
+    QMap<int, QTreeWidgetItem* >::iterator iteTree;
+    for(iteTree=uasTreeWidgetItems.begin(); iteTree!=uasTreeWidgetItems.end();++iteTree)
+    {
+        delete iteTree.value();
+        iteTree.value() = NULL;
+    }
+    uasTreeWidgetItems.clear();
+    
+    QMap<int, QMap<int, float>* >::iterator iteHz;
+    for (iteHz=uasMessageHz.begin(); iteHz!=uasMessageHz.end();++iteHz)
+    {
+
+        iteHz.value()->clear();
+        delete iteHz.value();
+        iteHz.value() = NULL;
+    }
+    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() = NULL;
+    }
+    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() = NULL;
+    }
+    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=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
     {
-        mavlink_message_t* msg = receivedMessages+i;
+
+        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
+        float msgHz = 0.0f;
+        QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
+        QMap<int, float>* uasMsgHz = iteHz.value();
+
+        while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
+        {
+            if(iteHz.value()->contains(msg->msgid))
+            {
+                uasMsgHz = iteHz.value();
+                msgHz = iteHz.value()->value(msg->msgid);
+                break;
+            }
+            ++iteHz;
+        }
+
+        // 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;
+        }
+
+        // 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)");
-        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))
+
+        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(fields);
-            widget->setFirstColumnSpanned(true);
-
+            QTreeWidgetItem* widget = new QTreeWidgetItem();
             for (unsigned int i = 0; i < messageInfo[msg->msgid].num_fields; ++i)
             {
                 QTreeWidgetItem* field = new QTreeWidgetItem();
                 widget->addChild(field);
             }
-
-            treeWidgetItems.insert(msg->msgid, widget);
-            ui->treeWidget->addTopLevelItem(widget);
+            msgTreeItems->insert(msg->msgid,widget);
+            QList<int> groupKeys = msgTreeItems->uniqueKeys();
+            int insertIndex = groupKeys.indexOf(msg->msgid);
+            uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
         }
 
-        // Set Hz
-        QTreeWidgetItem* message = treeWidgetItems.value(msg->msgid);
+        // 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->msgid, i, message->child(i));
+                updateField(msg->sysid,msg->msgid, i, message->child(i));
+            }
         }
     }
 
@@ -201,39 +324,145 @@ 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);
+
+    quint64 receiveTime;
+    
     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);
+    // 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);
     }
 
-    if (!components.contains(message.compid)) {
-        components.insert(message.compid, message.compid);
-        ui->componentComboBox->addItem(tr("COMP%1").arg(message.compid), message.compid);
+    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;
     }
 
-    quint64 receiveTime = QGC::groundTimeMilliseconds();
-    if (lastMessageUpdate.contains(message.msgid))
+    // 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))
         {
-        int count = messageCount.value(message.msgid, 0);
-        messageCount.insert(message.msgid, count+1);
+            msgFound = true;
+
+            // Point to the found message
+            lastMsgUpdate = ite.value();
+            break;
+        }
+        ++ite;
     }
 
-    lastMessageUpdate.insert(message.msgid, receiveTime);
+    receiveTime = QGC::groundTimeMilliseconds();
 
-    if (selectedSystemID == 0 || selectedComponentID == 0) {
+    // 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);
+    }
+
+    if (selectedSystemID == 0 || selectedComponentID == 0)
+    {
         return;
     }
 
-    switch (message.msgid) {
+    switch (message.msgid)
+    {
         case MAVLINK_MSG_ID_DATA_STREAM:
             {
                 mavlink_data_stream_t stream;
@@ -245,7 +474,8 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
     }
 }
 
-void QGCMAVLinkInspector::changeStreamInterval(int msgid, int interval) {
+void QGCMAVLinkInspector::changeStreamInterval(int msgid, int interval)
+{
     //REQUEST_DATA_STREAM
     if (selectedSystemID == 0 || selectedComponentID == 0) {
         return;
@@ -280,14 +510,37 @@ void QGCMAVLinkInspector::rateTreeItemChanged(QTreeWidgetItem* paramItem, int co
 
 QGCMAVLinkInspector::~QGCMAVLinkInspector()
 {
+    clearView();
     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;
+    
+    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

@@ -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,32 @@ 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* > 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
+
+    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;