diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc
index 0d89f36082278fa1d628d49812296cf0deddc4d3..10733f4f837e1bb8cb797e1319ae85cae9b44df2 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 2e5311a5310bd8e4203b9f929bd9d816dd75fc09..56e3f2d03513daee7f26a09bc5c9f0f9e2a1ec98 100644
--- a/src/ui/QGCMAVLinkInspector.h
+++ b/src/ui/QGCMAVLinkInspector.h
@@ -27,7 +27,7 @@ public slots:
 
 protected:
     QMap<int, quint64> lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color
-    QMap<int, quint64> messagesHz; ///< Used to store update rate in Hz
+    QMap<int, float> messagesHz; ///< Used to store update rate in Hz
     mavlink_message_t receivedMessages[256]; ///< Available / known messages
     QMap<int, QTreeWidgetItem*> treeWidgetItems;   ///< Available tree widget items
     QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI