From 6f98185c3eea28a6cbef1869c94cdad5acf97b55 Mon Sep 17 00:00:00 2001 From: Bryant Date: Sat, 8 Jun 2013 22:19:11 -0700 Subject: [PATCH] The details on the success/errors for the MAVLink protocols are now shown for each link as tooltips that are updated every 5s. Values are currently wrong and unsure why as of yet. --- src/comm/LinkManager.cc | 4 ++++ src/comm/LinkManager.h | 2 ++ src/comm/MAVLinkProtocol.cc | 39 ++++++++++++++++++++++------------ src/comm/MAVLinkProtocol.h | 34 ++++++++++++++++++++++++------ src/comm/ProtocolInterface.h | 20 +++++++++++++++++- src/ui/uas/UASListWidget.cc | 41 ++++++++++++++++++++++++++++++++++++ src/ui/uas/UASListWidget.h | 4 ++++ src/ui/uas/UASView.cc | 1 + 8 files changed, 124 insertions(+), 21 deletions(-) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 0f0a87645..447f1d5d4 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -107,6 +107,10 @@ QList LinkManager::getLinksForProtocol(ProtocolInterface* protoc return protocolLinks.values(protocol); } +ProtocolInterface* LinkManager::getProtocolForLink(LinkInterface* link) +{ + return protocolLinks.key(link); +} bool LinkManager::connectAll() { diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index b960ebb7d..ae7590ce7 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -56,6 +56,8 @@ public: QList getLinksForProtocol(ProtocolInterface* protocol); + ProtocolInterface* getProtocolForLink(LinkInterface* link); + /** @brief Get the link for this id */ LinkInterface* getLinkForId(int id); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 0b8e31410..d3d3411f4 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -63,10 +63,14 @@ MAVLinkProtocol::MAVLinkProtocol() : // Start heartbeat timer, emitting a heartbeat at the configured rate connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat())); heartbeatTimer->start(1000/heartbeatRate); - totalReceiveCounter = 0; - totalLossCounter = 0; - currReceiveCounter = 0; - currLossCounter = 0; + for (int i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) + { + totalReceiveCounter[i] = 0; + totalLossCounter[i] = 0; + totalErrorCounter[i] = 0; + currReceiveCounter[i] = 0; + currLossCounter[i] = 0; + } for (int i = 0; i < 256; i++) { for (int j = 0; j < 256; j++) @@ -214,12 +218,16 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_message_t message; mavlink_status_t status; + // Cache the link ID for common use. + int linkId = link->getId(); + static int mavlink09Count = 0; static bool decodedFirstPacket = false; static bool warnedUser = false; + // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS for (int position = 0; position < b.size(); position++) { - unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b[position]), &message, &status); + unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status); if ((uint8_t)b[position] == 0x55) mavlink09Count++; if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser) @@ -230,6 +238,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) emit protocolStatusMessage("MAVLink Version or Baud Rate Mismatch", "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. Please upgrade the MAVLink version of your autopilot. If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."); } + // Count parser errors as well. + totalErrorCounter[linkId] += status.packet_rx_drop_count; + if (decodeState == 1) { decodedFirstPacket = true; @@ -391,8 +402,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { // Increase receive counter - totalReceiveCounter++; - currReceiveCounter++; + totalReceiveCounter[linkId]++; + currReceiveCounter[linkId]++; // Update last message sequence ID uint8_t expectedIndex; @@ -412,7 +423,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (message.seq != expectedIndex) { // Determine how many messages were skipped accounting for 0-wraparound - int16_t lostMessages = message.seq - expectedIndex; + int16_t lostMessages = message.seq - expectedIndex; if (lostMessages < 0) { // Usually, this happens in the case of an out-of order packet @@ -423,22 +434,22 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Console generates excessive load at high loss rates, needs better GUI visualization //qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid); } - totalLossCounter += lostMessages; - currLossCounter += lostMessages; + totalLossCounter[linkId] += lostMessages; + currLossCounter[linkId] += lostMessages; } // Update the last sequence ID lastIndex[message.sysid][message.compid] = message.seq; // Update on every 32th packet - if (totalReceiveCounter % 32 == 0) + if (totalReceiveCounter[linkId] % 32 == 0) { // Calculate new loss ratio // Receive loss - float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter); + float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]); receiveLoss *= 100.0f; - currLossCounter = 0; - currReceiveCounter = 0; + currLossCounter[linkId] = 0; + currReceiveCounter[linkId] = 0; emit receiveLossChanged(message.sysid, receiveLoss); } diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 829b8e6ac..605798749 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -124,6 +124,27 @@ public: int getActionRetransmissionTimeout() { return m_actionRetransmissionTimeout; } + /** + * Retrieve a total of all successfully parsed packets for the specified link. + * @returns -1 if this is not available for this protocol, # of packets otherwise. + */ + qint32 getReceivedPacketCount(LinkInterface *link) const { + return totalReceiveCounter[link->getId()]; + } + /** + * Retrieve a total of all parsing errors for the specified link. + * @returns -1 if this is not available for this protocol, # of errors otherwise. + */ + qint32 getParsingErrorCount(LinkInterface *link) const { + return totalErrorCounter[link->getId()]; + } + /** + * Retrieve a total of all dropped packets for the specified link. + * @returns -1 if this is not available for this protocol, # of packets otherwise. + */ + qint32 getDroppedPacketCount(LinkInterface *link) const { + return totalLossCounter[link->getId()]; + } public slots: /** @brief Receive bytes from a communication interface */ @@ -201,12 +222,13 @@ protected: bool m_paramGuardEnabled; ///< Parameter retransmission/rewrite enabled bool m_actionGuardEnabled; ///< Action request retransmission enabled int m_actionRetransmissionTimeout; ///< Timeout for parameter retransmission - QMutex receiveMutex; ///< Mutex to protect receiveBytes function - int lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair - int totalReceiveCounter; - int totalLossCounter; - int currReceiveCounter; - int currLossCounter; + QMutex receiveMutex; ///< Mutex to protect receiveBytes function + int lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair + int totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The total number of successfully received messages + int totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total messages lost during transmission. + int totalErrorCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total count of all parsing errors. Generally <= totalLossCounter. + int currReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Received messages during this sample time window. Used for calculating loss %. + int currLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Lost messages during this sample time window. Used for calculating loss %. bool versionMismatchIgnore; int systemId; #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index 758a0a79b..f6a9a4199 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -50,8 +50,26 @@ class ProtocolInterface : public QObject { Q_OBJECT public: - //virtual ~ProtocolInterface() {}; + virtual ~ProtocolInterface () {} virtual QString getName() = 0; + /** + * Retrieve a total of all successfully parsed packets for the specified link. + * @param link The link to return metadata about. + * @returns -1 if this is not available for this protocol, # of packets otherwise. + */ + virtual qint32 getReceivedPacketCount(LinkInterface *link) const = 0; + /** + * Retrieve a total of all parsing errors for the specified link. + * @param link The link to return metadata about. + * @returns -1 if this is not available for this protocol, # of errors otherwise. + */ + virtual qint32 getParsingErrorCount(LinkInterface *link) const = 0; + /** + * Retrieve a total of all dropped packets for the specified link. + * @param link The link to return metadata about. + * @returns -1 if this is not available for this protocol, # of packets otherwise. + */ + virtual qint32 getDroppedPacketCount(LinkInterface *link) const = 0; public slots: virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index aef3a085e..b9cd0684b 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -73,6 +73,10 @@ UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent), { addUAS(uas); } + + // Use a timer to update the link health display. + updateTimer = new QTimer(this); + connect(updateTimer,SIGNAL(timeout()),this,SLOT(updateStatus())); } UASListWidget::~UASListWidget() @@ -93,14 +97,48 @@ void UASListWidget::changeEvent(QEvent *e) } } +void UASListWidget::updateStatus() +{ + QMapIterator i(linkToBoxMapping); + while (i.hasNext()) { + i.next(); + LinkInterface* link = i.key(); + ProtocolInterface* p = LinkManager::instance()->getProtocolForLink(link); + + // Build the tooltip out of the protocol parsing data: received, dropped, and parsing errors. + QString displayString(""); + int c; + if ((c = p->getReceivedPacketCount(link)) != -1) + { + displayString += QString(tr("
Received: %2")).arg(QString::number(c)); + } + if ((c = p->getDroppedPacketCount(link)) != -1) + { + displayString += QString(tr("
Dropped: %2")).arg(QString::number(c)); + } + if ((c = p->getParsingErrorCount(link)) != -1) + { + displayString += QString(tr("
Errors: %2")).arg(QString::number(c)); + } + if (!displayString.isEmpty()) + { + displayString = QString("%1").arg(i.key()->getName()) + displayString; + } + qDebug() << p << ": " + displayString; + i.value()->setToolTip(displayString); + } +} void UASListWidget::addUAS(UASInterface* uas) { + // If the list was empty, remove the unconnected widget and start the update timer. if (uasViews.isEmpty()) { m_ui->verticalLayout->removeWidget(uWidget); uWidget->deleteLater(); uWidget = NULL; + + updateTimer->start(5000); } if (!uasViews.contains(uas)) @@ -125,6 +163,7 @@ void UASListWidget::addUAS(UASInterface* uas) newBox->setLayout(boxLayout); m_ui->verticalLayout->addWidget(newBox); linkToBoxMapping[li] = newBox; + updateStatus(); // Update the link status for this GroupBox. } // And add the new UAS to the UASList @@ -186,6 +225,7 @@ void UASListWidget::removeUAS(UASInterface* uas) box->deleteLater(); // And if no other QGroupBoxes are left, put the initial widget back. + // We also stop the update timer as there's nothing to update at this point. int otherBoxes = 0; foreach (const QGroupBox* otherBox, findChildren()) { @@ -198,6 +238,7 @@ void UASListWidget::removeUAS(UASInterface* uas) { uWidget = new QGCUnconnectedInfoWidget(this); m_ui->verticalLayout->addWidget(uWidget); + updateTimer->stop(); } } } diff --git a/src/ui/uas/UASListWidget.h b/src/ui/uas/UASListWidget.h index 4cd3a587b..b14e1e97d 100644 --- a/src/ui/uas/UASListWidget.h +++ b/src/ui/uas/UASListWidget.h @@ -62,10 +62,14 @@ protected: // Tie each view to their UAS object so they can be removed easily. QMap uasViews; QGCUnconnectedInfoWidget* uWidget; + QTimer* updateTimer; void changeEvent(QEvent *e); private: Ui::UASList* m_ui; + +private slots: + void updateStatus(); }; #endif // _UASLISTWIDGET_H_ diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 5d8e97f64..9b051369d 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -79,6 +79,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : m_ui(new Ui::UASView) { m_ui->setupUi(this); + setToolTip(""); // Make sure the QGroupBox's tooltip doesn't seep through. // FIXME XXX lowPowerModeEnabled = MainWindow::instance()->lowPowerModeEnabled(); -- 2.22.0