From eda8dde1296f6332931a20b1308bb80cff34f707 Mon Sep 17 00:00:00 2001 From: ndousse Date: Wed, 10 Sep 2014 11:26:43 +0200 Subject: [PATCH] Change from an array of message to a map of message linking the sysid and the message --- src/ui/QGCMAVLinkInspector.cc | 231 ++++++++++++++++++++-------------- src/ui/QGCMAVLinkInspector.h | 3 +- 2 files changed, 138 insertions(+), 96 deletions(-) diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index eff73cc60..3dc95123f 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/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::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 - float msgHz = 0.0f; - QMap* >::const_iterator iteHz = uasMessageHz.find(msg->sysid); - QMap* uasMsgHz = iteHz.value(); + // Get the previous frequency for low-pass filtering + float msgHz = 0.0f; + QMap* >::const_iterator iteHz = uasMessageHz.find(msg->sysid); + QMap* 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); - break; - } - ++iteHz; + uasMsgHz = iteHz.value(); + msgHz = iteHz.value()->value(msg->msgid); + break; } + ++iteHz; + } - // Get the number of message received - float msgCount = 0; - QMap * >::const_iterator iter = uasMessageCount.find(msg->sysid); - QMap* uasMsgCount = iter.value(); - while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid)) + // Get the number of message received + float msgCount = 0; + QMap * >::const_iterator iter = uasMessageCount.find(msg->sysid); + QMap* uasMsgCount = iter.value(); + 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(); - break; - } - ++iter; + 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); + // 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); + // 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); + addUAStoTree(msg->sysid); - // Look for the tree for the UAS sysid - QMap* msgTreeItems = uasMsgTreeItems.value(msg->sysid); - if (!msgTreeItems) - { - // The UAS tree has not been created yet, no update - return; - } + // Look for the tree for the UAS sysid + QMap* 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)) + // 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) { - 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 groupKeys = msgTreeItems->uniqueKeys(); - int insertIndex = groupKeys.indexOf(msg->msgid); - uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget); + QTreeWidgetItem* field = new QTreeWidgetItem(); + widget->addChild(field); } + msgTreeItems->insert(msg->msgid,widget); + QList 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) + // 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) { - 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)); - } + 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]; - - // 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::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* >::const_iterator ite = uasLastMessageUpdate.find(message.sysid); QMap* 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; } - - // The message has been found/created - if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid))) + else { - // 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)) + // The message has been found/created + if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid))) { - if(iter.value()->contains(message.msgid)) + // 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)) { - uasMsgCount = iter.value(); - count = uasMsgCount->value(message.msgid,0); - uasMsgCount->insert(message.msgid,count+1); - break; + if(iter.value()->contains(message.msgid)) + { + uasMsgCount = iter.value(); + 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::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: diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index 92528393f..2c86e0730 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -53,7 +53,8 @@ protected: QMap uasTreeWidgetItems; ///< Tree of available uas with their widget QMap* > uasMsgTreeItems; ///< Stores the widget of the received message for each UAS - QMap uasMavlinkStorage; ///< Stores the message array for every UAS + QMap uasMessageStorage; ///< Stores the messages for every UAS + QMap* > uasMessageHz; ///< Stores the frequency of each message of each UAS QMap* > uasMessageCount; ///< Stores the message count of each message of each UAS -- 2.22.0