From 7576bdd4b38be6a6edbe223a3db21b9c6394cb52 Mon Sep 17 00:00:00 2001
From: Bryant Mairs <bwmairs@gmail.com>
Date: Sun, 15 Jun 2014 14:08:56 -0700
Subject: [PATCH] Refactored the lost packets calculations for MAVLink. Current
 version was not as clear as it could be.

---
 src/comm/MAVLinkProtocol.cc | 40 ++++++++++++++-----------------------
 1 file changed, 15 insertions(+), 25 deletions(-)

diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 450f45c1c..3ff291b5a 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -476,41 +476,31 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 totalReceiveCounter[linkId]++;
                 currReceiveCounter[linkId]++;
 
-                // Update last message sequence ID
-                uint8_t expectedIndex;
-                if (lastIndex[message.sysid][message.compid] == -1)
-                {
-                    lastIndex[message.sysid][message.compid] = message.seq;
-                    expectedIndex = message.seq;
-                }
-                else
-                {
-                    // NOTE: Using uint8_t here auto-wraps the number around to 0.
-                    expectedIndex = lastIndex[message.sysid][message.compid] + 1;
-                }
+                // Determine what the next expected sequence number is, accounting for
+                // never having seen a message for this system/component pair.
+                int lastSeq = lastIndex[message.sysid][message.compid];
+                int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
 
-                // Make some noise if a message was skipped
-                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
-                if (message.seq != expectedIndex)
+                // And if we didn't encounter that sequence number, record the error
+                if (message.seq != expectedSeq)
                 {
-                    // Determine how many messages were skipped accounting for 0-wraparound
-                    int16_t lostMessages = message.seq - expectedIndex;
+
+                    // Determine how many messages were skipped
+                    int lostMessages = message.seq - expectedSeq;
+
+                    // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
                     if (lostMessages < 0)
                     {
-                        // Usually, this happens in the case of an out-of order packet
                         lostMessages = 0;
                     }
-                    else
-                    {
-                        // 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);
-                    }
+
+                    // And log how many were lost for all time and just this timestep
                     totalLossCounter[linkId] += lostMessages;
                     currLossCounter[linkId] += lostMessages;
                 }
 
-                // Update the last sequence ID
-                lastIndex[message.sysid][message.compid] = message.seq;
+                // And update the last sequence number for this system/component pair
+                lastIndex[message.sysid][message.compid] = expectedSeq;
 
                 // Update on every 32th packet
                 if (totalReceiveCounter[linkId] % 32 == 0)
-- 
2.22.0