Skip to content

Comms: Fix Mavlink Loss Percent #12911

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
HTRamsey merged 1 commit into from
May 24, 2025
Merged

Comms: Fix Mavlink Loss Percent #12911

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 18 additions & 23 deletions src/Comms/MAVLinkProtocol.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,6 @@ void MAVLinkProtocol::init()
return;
}

(void) memset(_firstMessage, 1, sizeof(_firstMessage));

(void) connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &MAVLinkProtocol::_vehicleCountChanged);

_initialized = true;
Expand All @@ -77,9 +75,6 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
_totalReceiveCounter[channel] = 0;
_totalLossCounter[channel] = 0;
_runningLossPercent[channel] = 0.f;
for (int i = 0; i < 256; i++) {
_firstMessage[channel][i] = 1;
}

link->setDecodedFirstMavlinkPacket(false);
}
Expand Down Expand Up @@ -159,32 +154,32 @@ void MAVLinkProtocol::_updateVersion(LinkInterface *link, uint8_t mavlinkChannel

void MAVLinkProtocol::_updateCounters(uint8_t mavlinkChannel, const mavlink_message_t &message)
{
uint8_t lastSeq = _lastIndex[message.sysid][message.compid];
uint8_t expectedSeq = lastSeq + 1;
_totalReceiveCounter[mavlinkChannel]++;
if (_firstMessage[message.sysid][message.compid] != 0) {
_firstMessage[message.sysid][message.compid] = 0;
lastSeq = message.seq;

uint8_t &lastSeq = _lastIndex[message.sysid][message.compid];

const QPair<uint8_t,uint8_t> key(message.sysid, message.compid);
uint8_t expectedSeq;
if (!_firstMessageSeen.contains(key)) {
_firstMessageSeen.insert(key);
expectedSeq = message.seq;
} else {
expectedSeq = lastSeq + 1;
}

if (message.seq != expectedSeq) {
uint64_t lostMessages = message.seq;
if (message.seq < expectedSeq) {
lostMessages += 255;
}
lostMessages -= expectedSeq;
_totalLossCounter[mavlinkChannel] += lostMessages;
uint64_t lostMessages;
if (message.seq >= expectedSeq) {
lostMessages = message.seq - expectedSeq;
} else {
lostMessages = static_cast<uint64_t>(message.seq) + 256ULL - expectedSeq;
}
_totalLossCounter[mavlinkChannel] += lostMessages;

_lastIndex[message.sysid][message.compid] = message.seq;
lastSeq = message.seq;

const uint64_t totalSent = _totalReceiveCounter[mavlinkChannel] + _totalLossCounter[mavlinkChannel];
float receiveLossPercent = static_cast<float>(static_cast<double>(_totalLossCounter[mavlinkChannel]) / static_cast<double>(totalSent));
receiveLossPercent *= 100.0f;
receiveLossPercent *= 0.5f;
receiveLossPercent += (_runningLossPercent[mavlinkChannel] * 0.5f);
_runningLossPercent[mavlinkChannel] = receiveLossPercent;
const float currentLossPercent = (static_cast<double>(_totalLossCounter[mavlinkChannel]) / totalSent) * 100.0f;
_runningLossPercent[mavlinkChannel] = (currentLossPercent + _runningLossPercent[mavlinkChannel]) * 0.5f;
}

void MAVLinkProtocol::_forward(const mavlink_message_t &message)
Expand Down
2 changes: 1 addition & 1 deletion src/Comms/MAVLinkProtocol.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ private slots:
bool _vehicleWasArmed = false; ///< true: Vehicle was armed during log sequence

uint8_t _lastIndex[256][256]{}; ///< Store the last received sequence ID for each system/component pair
uint8_t _firstMessage[256][256]{}; ///< First message flag
QSet<QPair<uint8_t,uint8_t>> _firstMessageSeen;
uint64_t _totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]{}; ///< The total number of successfully received messages
uint64_t _totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Total messages lost during transmission.
float _runningLossPercent[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Loss rate
Expand Down
Loading