From 5a5d5b5a4cb1093c9861ea437b78627fd09e33b2 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Tue, 17 May 2022 12:34:01 +0200
Subject: [PATCH] [Mavlink] Fixed updateQueueStats following SyncPacketQueue
 fixes

---
 src/shared/drivers/usart/USART.cpp             |  1 -
 src/shared/radio/MavlinkDriver/MavlinkDriver.h | 14 +++++++-------
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp
index f60758e2d..2ba7d4f00 100644
--- a/src/shared/drivers/usart/USART.cpp
+++ b/src/shared/drivers/usart/USART.cpp
@@ -330,7 +330,6 @@ USART::USART(USARTType *usart, Baudrate baudrate, unsigned int queueLen)
 
     // Enabling the peripheral on the right APB
     ClockUtils::enablePeripheralClock(usart);
-    RCC_SYNC();
 
     // Enabling the usart peripheral
     {
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index af6b337a8..4c50b275c 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -147,7 +147,7 @@ private:
         reinterpret_cast<MavlinkDriver*>(arg)->runSender();
     }
 
-    void updateQueueStats(int dropped);
+    void updateQueueStats(bool appended);
 
     void updateSenderStats(size_t msgCount, bool sent);
 
@@ -224,7 +224,7 @@ bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::start()
     }
 
     if (sndStarted && rcvStarted)
-        LOG_DEBUG(logger, "Start ok (sender and receiver)\n");
+        LOG_DEBUG(logger, "Sender and receiver started");
 
     return (sndStarted && rcvStarted);
 }
@@ -249,23 +249,23 @@ bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueMsg(
     int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg);
 
     // Append message to the queue
-    int dropped = outQueue.put(msgtempBuf, msgLen);
+    bool appended = outQueue.put(msgtempBuf, msgLen);
 
     // Update stats
-    updateQueueStats(dropped);
+    updateQueueStats(appended);
 
     // return ok even if a packet was discarded
-    return dropped != -1;
+    return appended;
 }
 
 template <unsigned int PktLength, unsigned int OutQueueSize,
           unsigned int MavMsgLength>
 void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::updateQueueStats(
-    int dropped)
+    bool appended)
 {
     miosix::Lock<miosix::FastMutex> l(mtxStatus);
 
-    if (dropped != 0)
+    if (!appended)
     {
         LOG_ERR(logger, "Buffer full, the oldest message has been discarded");
         status.nDroppedPackets++;
-- 
GitLab