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)
}
// Only count message if UAS exists for this message
if (uas != NULL)
{
// Increase receive counter
totalReceiveCounter[linkId]++;
currReceiveCounter[linkId]++;
// Increase receive counter
totalReceiveCounter[linkId]++;
currReceiveCounter[linkId]++;
// 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);
// 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);
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// Determine how many messages were skipped
int lostMessages = message.seq - expectedSeq;
// 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)
{
lostMessages = 0;
}
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
if (lostMessages < 0)
{
lostMessages = 0;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[linkId] += lostMessages;
currLossCounter[linkId] += lostMessages;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[linkId] += lostMessages;
currLossCounter[linkId] += lostMessages;
}
// And update the last sequence number for this system/component pair
lastIndex[message.sysid][message.compid] = expectedSeq;
// 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] & 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
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);
}
// 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
emit messageReceived(link, message);
// 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
emit messageReceived(link, message);
// Multiplex message if enabled
if (m_multiplexingEnabled)
{
// Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// Multiplex message if enabled
if (m_multiplexingEnabled)
// Emit message on all links that are currently connected
foreach (LinkInterface* currLink, links)
{
// Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// 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);
}
// 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