Added loss information;

src/comm/MAVLinkProtocol.cc

@@ -59,6 +59,15 @@ 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;
+    for (int i = 0; i < 256; i++)
+    {
+        for (int j = 0; j < 256; j++)
+        {
+            lastIndex[i][j] = -1;
+        }
+    }
 }
 
 MAVLinkProtocol::~MAVLinkProtocol()
@@ -120,6 +129,40 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
                 // Now add UAS to "official" list, which makes the whole application aware of it
                 UASManager::instance()->addUAS(uas);
             }
+            // Increase receive counter
+            totalReceiveCounter++;
+            qint64 lastLoss = totalLossCounter;
+            // Update last packet index
+            if (lastIndex[message.sysid][message.compid] == -1)
+            {
+                lastIndex[message.sysid][message.compid] = message.seq;
+            }
+            else
+            {
+                int safeguard = 0;
+                while(lastIndex[message.sysid][message.compid]+1 != message.seq && safeguard < 100)
+                {
+                    lastIndex[message.sysid][message.compid] += 1;
+                    totalLossCounter++;
+                    safeguard++;
+                }
+            }
+//            if (lastIndex.contains(message.sysid))
+//            {
+//                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
+//                if (lastCompIndex->contains(message.compid))
+//                while (lastCompIndex->value(message.compid, 0)+1 )
+//            }
+            //if ()
+            if (lastLoss != totalLossCounter)
+            {
+                // Calculate new loss ratio
+                // Receive loss
+                float receiveLoss = (double)totalLossCounter/(double)(totalReceiveCounter+totalLossCounter);
+                receiveLoss *= 100.0f;
+                emit receiveLossChanged(receiveLoss);
+            }
+
             // The packet is emitted as a whole, as it is only 255 - 261 bytes short
             // kind of inefficient, but no issue for a groundstation pc.
             // It buys as reentrancy for the whole code over all threads

src/comm/MAVLinkProtocol.h

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
 #include <QMutex>
 #include <QString>
 #include <QTimer>
+#include <QMap>
 #include <QByteArray>
 #include "ProtocolInterface.h"
 #include "LinkInterface.h"
@@ -86,6 +87,9 @@ protected:
     int heartbeatRate;       ///< Heartbeat rate, controls the timer interval
     bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
     QMutex receiveMutex;     ///< Mutex to protect receiveBytes function
+    qint64 lastIndex[256][256];
+    int totalReceiveCounter;
+    int totalLossCounter;
 
 signals:
     /** @brief Message received and directly copied via signal */

src/comm/ProtocolInterface.h

@@ -1,26 +1,26 @@
 /*=====================================================================
- 
+
 PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
- 
+
 (c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
- 
+
 This file is part of the PIXHAWK project
- 
+
     PIXHAWK is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation, either version 3 of the License, or
     (at your option) any later version.
- 
+
     PIXHAWK is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.
- 
+
     You should have received a copy of the GNU General Public License
     along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
- 
+
 ======================================================================*/
- 
+
 /**
  * @file
  *   @brief Interface class for protocols
@@ -47,14 +47,17 @@ This file is part of the PIXHAWK project
  **/
 class ProtocolInterface : public QThread
 {
-Q_OBJECT
+    Q_OBJECT
 public:
-	//virtual ~ProtocolInterface() {};
-        virtual QString getName() = 0;
-	
+    //virtual ~ProtocolInterface() {};
+    virtual QString getName() = 0;
+
 public slots:
-	virtual void receiveBytes(LinkInterface *link) = 0;
-	
+    virtual void receiveBytes(LinkInterface *link) = 0;
+
+signals:
+    void receiveLossChanged(float loss);
+
 };
 
 #endif // _PROTOCOLINTERFACE_H_

src/ui/MainWindow.cc

@@ -164,6 +164,9 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent)
 
     // Adjust the size
     adjustSize();
+
+    //
+    connect(mavlink, SIGNAL(receiveLossChanged(float)), info, SLOT(updateReceiveLoss(float)));
 }
 
 MainWindow::~MainWindow()

src/ui/uas/UASInfoWidget.cc

@@ -115,6 +115,12 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
     }
 }
 
+void UASInfoWidget::updateReceiveLoss(float receiveLoss)
+{
+    ui.receiveLossBar->setValue(receiveLoss);
+    ui.receiveLossLabel->setText(QString::number(receiveLoss,'f', 2));
+}
+
 void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop)
 {
     Q_UNUSED(sysId);

src/ui/uas/UASInfoWidget.h

@@ -56,6 +56,7 @@ public slots:
 
     void updateBattery(UASInterface* uas, double voltage, double percent, int seconds);
     void updateCPULoad(UASInterface* uas, double load);
+    void updateReceiveLoss(float receiveLoss);
     void updateDropRate(int sysId, float receiveDrop, float sendDrop);
 
     void setVoltage(UASInterface* uas, double voltage);