Skip to content

Q
qgroundcontrol
Commit c2009e85 authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Also show MAVLink traffic without a valid system connected

parent c78ad877
Hide whitespace changes
Inline Side-by-side
Showing with 48 additions and 53 deletions
src/comm/MAVLinkProtocol.cc
View file @ c2009e85
...@@ -393,70 +393,65 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -393,70 +393,65 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
} }
// Only count message if UAS exists for this message // Increase receive counter
if (uas != NULL) totalReceiveCounter[linkId]++;
{ currReceiveCounter[linkId]++;
// Increase receive counter // Determine what the next expected sequence number is, accounting for
totalReceiveCounter[linkId]++; // never having seen a message for this system/component pair.
currReceiveCounter[linkId]++; int lastSeq = lastIndex[message.sysid][message.compid];
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// Determine what the next expected sequence number is, accounting for // And if we didn't encounter that sequence number, record the error
// never having seen a message for this system/component pair. if (message.seq != expectedSeq)
int lastSeq = lastIndex[message.sysid][message.compid]; {
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// And if we didn't encounter that sequence number, record the error // Determine how many messages were skipped
if (message.seq != expectedSeq) int lostMessages = message.seq - expectedSeq;
{
// Determine how many messages were skipped // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
int lostMessages = message.seq - expectedSeq; if (lostMessages < 0)
{
lostMessages = 0;
}
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity // And log how many were lost for all time and just this timestep
if (lostMessages < 0) totalLossCounter[linkId] += lostMessages;
{ currLossCounter[linkId] += lostMessages;
lostMessages = 0; }
}
// And log how many were lost for all time and just this timestep // And update the last sequence number for this system/component pair
totalLossCounter[linkId] += lostMessages; lastIndex[message.sysid][message.compid] = expectedSeq;
currLossCounter[linkId] += lostMessages;
}
// And update the last sequence number for this system/component pair // Update on every 32th packet
lastIndex[message.sysid][message.compid] = expectedSeq; if ((totalReceiveCounter[linkId] & 0x1F) == 0)
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
receiveLoss *= 100.0f;
currLossCounter[linkId] = 0;
currReceiveCounter[linkId] = 0;
emit receiveLossChanged(message.sysid, receiveLoss);
}
// Update on every 32th packet // The packet is emitted as a whole, as it is only 255 - 261 bytes short
if ((totalReceiveCounter[linkId] & 0x1F) == 0) // kind of inefficient, but no issue for a groundstation pc.
{ // It buys as reentrancy for the whole code over all threads
// Calculate new loss ratio emit messageReceived(link, message);
// Receive loss
float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
receiveLoss *= 100.0f;
currLossCounter[linkId] = 0;
currReceiveCounter[linkId] = 0;
emit receiveLossChanged(message.sysid, receiveLoss);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short // Multiplex message if enabled
// kind of inefficient, but no issue for a groundstation pc. if (m_multiplexingEnabled)
// It buys as reentrancy for the whole code over all threads {
emit messageReceived(link, message); // Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// Multiplex message if enabled // Emit message on all links that are currently connected
if (m_multiplexingEnabled) foreach (LinkInterface* currLink, links)
{ {
// Get all links connected to this unit // Only forward this message to the other links,
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this); // not the link the message was received on
if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
// Emit message on all links that are currently connected
foreach (LinkInterface* currLink, links)
{
// Only forward this message to the other links,
// not the link the message was received on
if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
}
} }
} }
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment