From 0f136f32ebf04756cd57493d6af7306d3ffce39e Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 17 Nov 2011 22:52:11 +0100 Subject: [PATCH] Added update rate calculation for incoming messages --- src/ui/QGCMAVLinkInspector.cc | 16 +++++++++++----- src/ui/QGCMAVLinkInspector.h | 2 +- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 0d89f3608..10733f4f8 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -38,7 +38,7 @@ void QGCMAVLinkInspector::refreshView() if (!msg) continue; // Update the tree view QString messageName("%1 (%2 Hz, #%3)"); - messageName = messageName.arg(messageInfo[msg->msgid].name).arg(messagesHz.value(msg->msgid, 0)).arg(msg->msgid); + messageName = messageName.arg(messageInfo[msg->msgid].name).arg(messagesHz.value(msg->msgid, 0), 2, 'f', 0).arg(msg->msgid); if (!treeWidgetItems.contains(msg->msgid)) { @@ -80,12 +80,18 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m quint64 receiveTime = QGC::groundTimeMilliseconds(); if (lastMessageUpdate.contains(message.msgid)) { - msgHz = ((float)((float)receiveTime - (float)lastMessageUpdate.value(message.msgid)))/1000.0f; - qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid); - messagesHz.insert(message.msgid, 0.1f*msgHz+0.9f*messagesHz.value(message.msgid, 1)); + msgHz = 1000.0/(double)(receiveTime - lastMessageUpdate.value(message.msgid)); + if (isinf(msgHz) || isnan(msgHz) || msgHz < 0.0f) + { + msgHz = 1; + } + //qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid); + float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1); + qDebug() << "HZ" << newHz; + messagesHz.insert(message.msgid, newHz); } - qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000); + //qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000); lastMessageUpdate.insert(message.msgid, receiveTime); } diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index 2e5311a53..56e3f2d03 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -27,7 +27,7 @@ public slots: protected: QMap lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color - QMap messagesHz; ///< Used to store update rate in Hz + QMap messagesHz; ///< Used to store update rate in Hz mavlink_message_t receivedMessages[256]; ///< Available / known messages QMap treeWidgetItems; ///< Available tree widget items QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI -- 2.22.0