Merge pull request mavlink#715 from Susurrus/seqid2

Refactor MAVLink packet loss calculations
D4U · Jun 21, 2014 · 27a9dab · 27a9dab
2 parents ea50f06 + 8d3e177
commit 27a9dab
Showing 1 changed file with 16 additions and 26 deletions.
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
@@ -476,44 +476,34 @@ 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)
+ if ((totalReceiveCounter[linkId] & 0x1F) == 0)
  {
  // Calculate new loss ratio
  // Receive loss