diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..9f355c0365769056e174e6557f02e21f69319449 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg(
     const mavlink_message_t& msg)
 {
     // Convert mavlink message to char array
-    uint8_t msgtempBuf[PktLength];
+    // Use fixed buffer size to avoid overflows
+    uint8_t msgtempBuf[256];
     int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg);
 
     // Append message to the queue
diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h
index ff5e7b360a3f612cf73feae05e0283267d96108f..a2bd35e33855484f33c6b96dae9655492ca77dd5 100644
--- a/src/shared/utils/collections/SyncPacketQueue.h
+++ b/src/shared/utils/collections/SyncPacketQueue.h
@@ -67,16 +67,15 @@ public:
     ~Packet() { content.clear(); };
 
     /**
-     * @brief Try to append a given message to the packet.
+     * @brief Append a given message to the packet.
      *
      * If it's the first message, also set the timestamp.
      *
      * @param msg The message to be appended.
      * @param msgLen Length of msg.
-     * @return true if the message was appended correctly.
-     * @return false if there isn't enough space for the message.
+     * @return How many bytes were actually appended.
      */
-    bool tryAppend(const uint8_t* msg, const size_t msgLen);
+    size_t append(const uint8_t* msg, size_t msgLen);
 
     /**
      * @brief Mark the packet as ready to be sent.
@@ -111,7 +110,7 @@ public:
     inline bool isReady() const { return ready; }
 
     /**
-     * @return The timestamp of the first successful call to tryAppend().
+     * @return The timestamp of the first successful call to append().
      */
     inline uint64_t timestamp() const { return ts; }
 
@@ -126,7 +125,7 @@ public:
     inline unsigned int maxSize() const { return len; }
 
     /**
-     * @return How many times the tryAppend() function has been called
+     * @return How many times the append() function has been called
      * successfully.
      */
     inline unsigned int getMsgCount() const { return msgCounter; }
@@ -148,13 +147,12 @@ private:
 };
 
 template <unsigned int len>
-bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen)
+size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
 {
-    if (msgLen == 0 || content.size() + msgLen > len)
-    {
-        return false;
-    }
-    else
+    size_t remaining = len - content.size();
+    msgLen           = std::min(remaining, msgLen);
+
+    if (msgLen != 0)
     {
         // Set the packet's timestamp when the first message is inserted
         if (content.size() == 0)
@@ -165,9 +163,9 @@ bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen)
         // Append the message to the packet
         content.insert(content.end(), msg, msg + msgLen);
         msgCounter++;
-
-        return true;
     }
+
+    return msgLen;
 }
 
 template <unsigned int len>
@@ -230,7 +228,7 @@ public:
     {
         int dropped = 0;
 
-        if (msgLen == 0 || msgLen > pktLen)
+        if (msgLen == 0)
         {
             return -1;
         }
@@ -243,43 +241,37 @@ public:
                 buffer.put(Pkt{});
             }
 
-            Pkt& last = buffer.last();
-
-            bool added = false;
-
-            // Add to the current packet only if it isn't already ready
-            if (!last.isReady())
-            {
-                added = last.tryAppend(msg, msgLen);
-            }
-
-            // If already ready or cannot fit the new data, add to a new packet
-            if (!added)
+            while (msgLen > 0)
             {
-                // Mark the packet as ready (in the case it wasn't already)
-                last.markAsReady();
-
-                if (buffer.isFull())
+                if (buffer.last().isReady())
                 {
-                    // We have dropped a packet
-                    ++dropped;
+                    if (buffer.isFull())
+                    {
+                        // We have dropped a packet
+                        ++dropped;
+                    }
+
+                    // If the last pkt is ready, append a new one
+                    buffer.put(Pkt{});
+                    // FIXME(davide.mor): Figure out quantum shenanigans
+                    // uncommenting the following line causes everything to
+                    // break, why?
+
+                    // last = buffer.last();
                 }
-                // Add a new packet and fill that instead
-                Pkt& newpkt = buffer.put(Pkt{});
 
-                if (!newpkt.tryAppend(msg, msgLen))
+                size_t sentLen = buffer.last().append(msg, msgLen);
+
+                msgLen -= sentLen;
+                msg += sentLen;
+
+                // Mark as ready if the packet is full
+                if (buffer.last().isFull())
                 {
-                    TRACE("Packet is too big!\n");
-                    return -1;
+                    buffer.last().markAsReady();
                 }
             }
 
-            // Mark as ready if the packet is full
-            if (buffer.last().isFull())
-            {
-                buffer.last().markAsReady();
-            }
-
             cvNotempty.broadcast();
             return dropped;
         }