From dd747599de567049e0cb08341ce3c33c6a1780cf Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Tue, 4 Sep 2018 14:59:29 -0400 Subject: [PATCH] Just compute total sent on the fly rather then keeping a running count. --- src/comm/MAVLinkProtocol.cc | 11 +++-------- src/comm/MAVLinkProtocol.h | 1 - 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index b96092d565..ffebfe6aa3 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -65,7 +65,6 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox) , _multiVehicleManager(nullptr) { memset(totalReceiveCounter, 0, sizeof(totalReceiveCounter)); - memset(totalSentCounter, 0, sizeof(totalSentCounter)); memset(totalLossCounter, 0, sizeof(totalLossCounter)); memset(runningLossPercent, 0, sizeof(runningLossPercent)); memset(firstMessage, 1, sizeof(firstMessage)); @@ -149,7 +148,6 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) int channel = link->mavlinkChannel(); totalReceiveCounter[channel] = 0; totalLossCounter[channel] = 0; - totalSentCounter[channel] = 0; runningLossPercent[channel] = 0.0f; for(int i = 0; i < 256; i++) { firstMessage[channel][i] = 1; @@ -221,16 +219,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } // Log how many were lost totalLossCounter[mavlinkChannel] += static_cast(lostMessages); - // Compute actual count of sent messages - totalSentCounter[mavlinkChannel] += static_cast(lostMessages); - } else { - totalSentCounter[mavlinkChannel]++; } // And update the last sequence number for this system/component pair lastIndex[_message.sysid][_message.compid] = _message.seq;; // Calculate new loss ratio - float receiveLossPercent = static_cast(totalLossCounter[mavlinkChannel]) / static_cast(totalSentCounter[mavlinkChannel]); + uint64_t totalSent = totalReceiveCounter[mavlinkChannel] + totalLossCounter[mavlinkChannel]; + float receiveLossPercent = static_cast(static_cast(totalLossCounter[mavlinkChannel]) / static_cast(totalSent)); receiveLossPercent *= 100.0f; receiveLossPercent = (receiveLossPercent * 0.5f) + (runningLossPercent[mavlinkChannel] * 0.5f); runningLossPercent[mavlinkChannel] = receiveLossPercent; @@ -310,7 +305,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Update MAVLink status on every 32th packet if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0) { - emit mavlinkMessageStatus(_message.sysid, totalSentCounter[mavlinkChannel], totalReceiveCounter[mavlinkChannel], totalLossCounter[mavlinkChannel], receiveLossPercent); + emit mavlinkMessageStatus(_message.sysid, totalSent, totalReceiveCounter[mavlinkChannel], totalLossCounter[mavlinkChannel], receiveLossPercent); } // The packet is emitted as a whole, as it is only 255 - 261 bytes short diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 01be94c528..35947162b6 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -105,7 +105,6 @@ protected: uint8_t lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair uint8_t firstMessage[256][256]; ///< First message flag uint64_t totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The total number of successfully received messages - uint64_t totalSentCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The calculated total number of messages sent to us uint64_t totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total messages lost during transmission. float runningLossPercent[MAVLINK_COMM_NUM_BUFFERS]; ///< Loss rate -- GitLab