From 2f38ab840ef499927f7004c157598f0fd362debf Mon Sep 17 00:00:00 2001 From: ndousse Date: Tue, 9 Sep 2014 14:15:22 +0200 Subject: [PATCH] The messages are separeted by sysid and msgid --- src/ui/QGCMAVLinkInspector.cc | 275 ++++++++++++++++++++++++++-------- src/ui/QGCMAVLinkInspector.h | 28 ++-- 2 files changed, 232 insertions(+), 71 deletions(-) diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 82c6464af..eff73cc60 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/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 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::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* >::const_iterator iteHz = uasMessageHz.find(msg->sysid); + QMap* 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 * >::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)) + { + 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* 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 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()); + } + } +} + 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* >::const_iterator ite = uasLastMessageUpdate.find(message.sysid); + QMap* 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* messageHz = new QMap; + messageHz->insert(message.msgid,0.0f); + uasMessageHz.insertMulti(message.sysid,messageHz); + + // Create a map for the message count + QMap* messagesCount = new QMap; + messagesCount->insert(message.msgid,0); + uasMessageCount.insertMulti(message.sysid,messagesCount); + + // Create a map for the time of reception of the message + QMap* lastMessage = new QMap; + 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* >::const_iterator iter = uasMessageCount.find(message.sysid); + QMap * 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: diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index e4e7820e8..92528393f 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/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 systems; ///< Already observed systems QMap components; ///< Already observed components - QMap lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color - QMap messagesHz; ///< Used to store update rate in Hz QMap onboardMessageInterval; ///< Stores the onboard selected data rate - QMap messageCount; ///< Used to store the message count - mavlink_message_t receivedMessages[256]; ///< Available / known messages - QMap treeWidgetItems; ///< Available tree widget items QMap 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 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* > uasMessageHz; ///< Stores the frequency of each message of each UAS + QMap* > uasMessageCount; ///< Stores the message count of each message of each UAS + + QMap* > 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; -- 2.22.0