Refactored part of MAVLinkProtocol::receiveBytes() as I was trying to debug...

Refactored part of MAVLinkProtocol::receiveBytes() as I was trying to debug within it. Nothing major, though I did remove an unnecessary loop.

src/comm/MAVLinkProtocol.cc

@@ -344,39 +344,42 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             // Only count message if UAS exists for this message
             if (uas != NULL)
             {
+
                 // Increase receive counter
                 totalReceiveCounter++;
                 currReceiveCounter++;
-                // Update last packet index
-                if (lastIndex[message.sysid][message.compid] == -1)
+
+                // 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
                 {
-                    uint8_t expectedIndex = lastIndex[message.sysid][message.compid];
-                    // Now increase to the expected index
-                    expectedIndex++;
-
-                    //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
-                    while(expectedIndex != message.seq)
-                    {
-                        expectedIndex++;
-                        totalLossCounter++;
-                        currLossCounter++;
-                        //qDebug() << "COUNTING ONE DROP!";
-                    }
-
-                    // Set new lastindex
-                    lastIndex[message.sysid][message.compid] = message.seq;
-                }
-                //            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 ()
+					// NOTE: Using uint8_t here auto-wraps the number around to 0.
+                    expectedIndex = lastIndex[message.sysid][message.compid] + 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)
+				{
+					// Determine how many messages were skipped accounting for 0-wraparound
+					int16_t lostMessages = message.seq - expectedIndex;
+					if (lostMessages < 0)
+					{
+						lostMessages += 256;
+					}
+					qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq);
+					
+					totalLossCounter += lostMessages;
+					currLossCounter += lostMessages;
+				}
+
+                // Update the last sequence ID
+                lastIndex[message.sysid][message.compid] = message.seq;
 
                 // Update on every 32th packet
                 if (totalReceiveCounter % 32 == 0)
@@ -385,11 +388,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                     // Receive loss
                     float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                     receiveLoss *= 100.0f;
-                    // qDebug() << "LOSSCHANGED" << receiveLoss;
                     currLossCounter = 0;
                     currReceiveCounter = 0;
                     emit receiveLossChanged(message.sysid, receiveLoss);
-                    //qDebug() << "LOSSCHANGED" << message.sysid<<" "<<receiveLoss;
                 }
 
                 // The packet is emitted as a whole, as it is only 255 - 261 bytes short
@@ -414,7 +415,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
             }
         }
     }
-//    receiveMutex.unlock();
 }
 
 /**

src/comm/MAVLinkProtocol.h

@@ -199,7 +199,7 @@ protected:
     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];
+    int lastIndex[256][256];	///< Store the last received sequence ID for each system/componenet pair
     int totalReceiveCounter;
     int totalLossCounter;
     int currReceiveCounter;

src/uas/UAS.cc

@@ -755,7 +755,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
                 else
                 {
-                    qDebug() << "Got waypoint message, but was not for me";
+                    qDebug() << QString("Received mission count, but was for %1 not me (%2)").arg(wpc.target_system).arg(mavlink->getSystemId());
                 }
             }
             break;
@@ -764,14 +764,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_mission_item_t wp;
                 mavlink_msg_mission_item_decode(&message, &wp);
-                //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
                 if(wp.target_system == mavlink->getSystemId())
                 {
                     waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
                 }
                 else
                 {
-                    qDebug() << "Got waypoint message, but was not for me";
+                    qDebug() << QString("Received mission item, but was for %1 not me (%2)").arg(wp.target_system).arg(mavlink->getSystemId());
                 }
             }
             break;
@@ -797,7 +796,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
                 else
                 {
-                    qDebug() << "Got waypoint message, but was not for me";
+                    qDebug() << QString("Received mission request, but was for %1 not me (%2)").arg(wpr.target_system).arg(mavlink->getSystemId());
                 }
             }
             break;