diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index 9f355c0365769056e174e6557f02e21f69319449..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -348,11 +348,12 @@ void MavlinkDriver<PktLength, OutQueueSize>::runSender()
             // Get the first packet in the queue, without removing it
             pkt = outQueue.get();
 
-            uint64_t age = (uint64_t)miosix::getTick() - pkt.timestamp();
             // If the packet is ready or too old, send it
+            uint64_t age = TimestampTimer::getInstance().getTimestamp() -
+                           pkt.getTimestamp();
             if (pkt.isReady() || age >= outBufferMaxAge)
             {
-                outQueue.pop();  // remove from queue
+                outQueue.pop();  //  Remove the packet from queue
 
                 LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})",
                           pkt.size(), age);
diff --git a/src/shared/utils/collections/CircularBuffer.h b/src/shared/utils/collections/CircularBuffer.h
index fa6a2e58286996793ee9c7b949b26da61e4cb1db..6182bdcc5b7967af9b90570dada7d62c635438ce 100644
--- a/src/shared/utils/collections/CircularBuffer.h
+++ b/src/shared/utils/collections/CircularBuffer.h
@@ -32,7 +32,7 @@ namespace Boardcore
 {
 
 /**
- * Implementation of an non-synchronized circular buffer
+ * Implementation of an non-synchronized circular buffer.
  */
 template <typename T, unsigned int Size>
 class CircularBuffer
@@ -41,21 +41,25 @@ class CircularBuffer
 
 public:
     CircularBuffer() {}
+
     virtual ~CircularBuffer() {}
 
     /**
-     * Puts a copy of the element in the buffer
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @param elem Element to be added to the queue.
+     * @return The element added.
      */
     virtual T& put(const T& elem)
     {
         buffer[writePtr] = elem;
         T& added         = buffer[writePtr];
 
+        // Advance the read pointer if the two pointers match
         if (!empty && writePtr == readPtr)
-        {
             readPtr = (readPtr + 1) % Size;
-        }
+
+        // Advance the write pointer
         writePtr = (writePtr + 1) % Size;
 
         empty = false;
@@ -64,20 +68,19 @@ public:
     }
 
     /**
-     * Gets an element from the buffer, without removing it
-     * Index starts from the oldest element in the buffer: get(0) returns the
-     * same element as get()
+     * @brief Gets an element from the buffer, without removing it.
      *
-     * @warning Remember to catch the exception!
-     * @throw range_error if index >= count()
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
-     * @param i Index of the elemnt to get, starting from the oldest
-     *
-     * @return the element
+     * @warning Remember to catch the exception!
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    virtual T& get(unsigned int i)
+    virtual T& get(unsigned int i = 0)
     {
-        if (i < CircularBuffer<T, Size>::count())
+        if (i < count())
         {
             int ptr = (readPtr + i) % Size;
             return buffer[ptr];
@@ -89,33 +92,18 @@ public:
     /**
      * @brief Returns the last element added in the buffer.
      *
-     * @throw range_error if buffer is empty
      * @warning Remember to catch the exception!
-     * @return the element
+     * @throw range_error if buffer is empty.
+     * @return The element.
      */
     virtual T& last() { return get(count() - 1); }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @throw range_error if buffer is empty
-     * @warning Remember to catch the exception!
-     * @return the element
-     */
-    virtual T& get()
-    {
-        if (!empty)
-        {
-            return buffer[readPtr];
-        }
-        else
-            throw range_error("CircularBuffer is empty!");
-    }
-
-    /**
-     * Pops the first element in the buffer.
-     * @throw range_error if buffer is empty
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     virtual const T& pop()
     {
@@ -133,8 +121,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     virtual size_t count() const
     {
@@ -157,16 +146,19 @@ public:
         return CircularBuffer<T, Size>::count() == Size;
     }
     /**
-     * Returns the maximum number of elements that can be stored in the buffer
-     * @return buffer size
+     * @brief Returns the maximum number of elements that can be stored in the
+     * buffer.
+     *
+     * @return Buffer size.
      */
     size_t getSize() const { return Size; }
 
 protected:
     T buffer[Size];
 
-    size_t writePtr = 0, readPtr = 0;
-    bool empty = true;
+    size_t writePtr = 0;
+    size_t readPtr  = 0;
+    bool empty      = true;
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/utils/collections/IRQCircularBuffer.h b/src/shared/utils/collections/IRQCircularBuffer.h
index a6a02e63664400c7b5efc1fc9af1287613862598..e27d15e2a37652df6e822d29fd8d3ff0cd3af104 100644
--- a/src/shared/utils/collections/IRQCircularBuffer.h
+++ b/src/shared/utils/collections/IRQCircularBuffer.h
@@ -34,7 +34,8 @@ namespace Boardcore
 {
 
 /**
- * Implementation of a synchronized circular buffer
+ * Implementation of a synchronized circular buffer that can be used inside
+ * interrupt service routines.
  */
 template <typename T, unsigned int Size>
 class IRQCircularBuffer : public CircularBuffer<T, Size>
@@ -44,8 +45,7 @@ class IRQCircularBuffer : public CircularBuffer<T, Size>
 
 public:
     /**
-     * Puts a copy of the element in the buffer
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
      */
     T& put(const T& elem) override
     {
@@ -55,37 +55,28 @@ public:
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& get() override
-    {
-        FastInterruptDisableLock d;
-        return Super::get();
-    }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& get(unsigned int i) override
+    T& get(unsigned int i = 0) override
     {
         FastInterruptDisableLock d;
         return Super::get(i);
     }
 
     /**
-     * Pops the first element in the buffer.
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& pop() override
     {
@@ -94,8 +85,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t count() const override
     {
@@ -116,9 +108,9 @@ public:
     }
 
     /**
-     * Puts a copy of the element in the buffer
-     * Only to be called inside an ISR or with interrupts disabled
-     * @param elem element
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     T& IRQput(const T& elem)
     {
@@ -127,8 +119,10 @@ public:
     }
 
     /**
-     * Puts a copy of the element in the buffer
-     * Only to be called inside an ISR or with interrupts disabled
+     * @brief Puts a copy of the element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
      * @param elem element
      * @param hppw Set to true if the woken thread is higher priority than the
      * current one, unchanged otherwise
@@ -137,68 +131,59 @@ public:
     {
         if (waiting && (waiting->IRQgetPriority() >
                         Thread::IRQgetCurrentThread()->IRQgetPriority()))
-        {
             hppw = true;
-        }
 
         IRQwakeWaitingThread();
         return Super::put(elem);
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * Only to be called inside an ISR or with interrupts disabled
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& IRQget() { return Super::get(); }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
-     * @warning Only to be called inside an ISR or with interrupts disabled
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
+     *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& IRQget(unsigned int i) { return Super::get(i); }
+    T& IRQget(unsigned int i = 0) { return Super::get(i); }
 
     /**
-     * Pops the first element in the buffer.
-     * @warning Only to be called inside an ISR or with interrupts disabled
+     * @brief Pops the first element in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& IRQpop() { return Super::pop(); }
 
     /**
-     * Counts the elements in the buffer
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @warning Only to be called inside an ISR or with interrupts disabled.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t IRQcount() const { return Super::count(); }
 
     /**
-     * @brief Returns true if the buffer is empty
-     *
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return empty or not
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     bool IRQisEmpty() const { return Super::isEmpty(); }
 
     /**
-     * @brief Returns true if the buffer is full
-     *
-     * @warning Only to be called inside an ISR or with interrupts disabled
-     * @return buffer full or not
+     * @warning Only to be called inside an ISR or with interrupts disabled.
      */
     bool IRQisFull() const { return Super::isFull(); }
 
     /**
-     * @brief Waits until the buffer contains at least one element
+     * @brief Waits until the buffer contains at least one element.
      */
     void waitUntilNotEmpty()
     {
@@ -228,4 +213,5 @@ private:
 
     Thread* waiting = nullptr;
 };
-}  // namespace Boardcore
\ No newline at end of file
+
+}  // namespace Boardcore
diff --git a/src/shared/utils/collections/SyncCircularBuffer.h b/src/shared/utils/collections/SyncCircularBuffer.h
index 9e05227df79823f100b0ff915530c451fd0862b8..cab438fbb629c00aa98122ed9b3512deca329e57 100644
--- a/src/shared/utils/collections/SyncCircularBuffer.h
+++ b/src/shared/utils/collections/SyncCircularBuffer.h
@@ -55,37 +55,28 @@ public:
     }
 
     /**
-     * Gets the first element from the buffer, without removing it
-     * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
-     */
-    T& get() override
-    {
-        Lock<FastMutex> l(mutex);
-        return Super::get();
-    }
-
-    /**
-     * Gets an element from the buffer, without removing it
-     * Index starts at the element returned by get() or pop(): get(0) is
-     * the same as get()
+     * @brief Gets an element from the buffer, without removing it.
+     *
+     * Index starts from the oldest element in the buffer.
+     * get() returns the first element.
      *
      * @warning Remember to catch the exception!
-     * @return the element
-     * @throws range_error if buffer is empty
+     * @throw range_error if index >= count().
+     * @param i Index of the element to get, starting from the oldest.
+     * @return The element.
      */
-    T& get(unsigned int i) override
+    T& get(unsigned int i = 0) override
     {
         Lock<FastMutex> l(mutex);
         return Super::get(i);
     }
 
     /**
-     * Pops the first element in the buffer.
+     * @brief Pops the first element in the buffer.
+     *
      * @warning Remember to catch the exception!
-     * @return the element that has been popped
-     * @throws range_error if buffer is empty
+     * @throw range_error if buffer is empty.
+     * @return The element that has been popped.
      */
     const T& pop() override
     {
@@ -94,8 +85,9 @@ public:
     }
 
     /**
-     * Counts the elements in the buffer
-     * @return number of elements in the buffer
+     * @brief Counts the elements in the buffer.
+     *
+     * @return Number of elements in the buffer.
      */
     size_t count() const override
     {
@@ -116,7 +108,7 @@ public:
     }
 
     /**
-     * @brief Waits until the buffer contains at least one element
+     * @brief Waits until the buffer contains at least one element.
      */
     void waitUntilNotEmpty()
     {
diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h
index a2bd35e33855484f33c6b96dae9655492ca77dd5..3d646d04da08bd600d663c951871e0960ee9b85d 100644
--- a/src/shared/utils/collections/SyncPacketQueue.h
+++ b/src/shared/utils/collections/SyncPacketQueue.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Alvise de'Faveri Tron
+/* Copyright (c) 2019-2022 Skyward Experimental Rocketry
+ * Author: Alvise de'Faveri Tron, Davide Mor, Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
 #include <utils/Debug.h>
 
@@ -46,10 +47,9 @@ namespace Boardcore
  * @brief The Packet class is used for packing together messages with variable
  * lengths into a fixed size packet. Useful for telemetry.
  *
- * The buffer can only be appended, read or flushed. The caller can also mark
- * the packet as ready to be sent.
+ * Data can only be appended to the payload. The packet can be marked ready.
  *
- * @tparam len Maximum length for the packet.
+ * @tparam len Packet's payload length.
  */
 template <unsigned int len>
 class Packet
@@ -59,7 +59,7 @@ public:
     /**
      * @brief Reserves a fixed length for the packet.
      */
-    Packet() : msgCounter(0), ts(0), ready(false) { content.reserve(len); };
+    Packet() { content.reserve(len); };
 
     /**
      * @brief Clears the buffer.
@@ -69,21 +69,23 @@ public:
     /**
      * @brief Append a given message to the packet.
      *
-     * If it's the first message, also set the timestamp.
+     * If the message can't fit inside the remaining space only the first bytes
+     * are copied.
      *
      * @param msg The message to be appended.
-     * @param msgLen Length of msg.
+     * @param msgLen Length of the message.
      * @return How many bytes were actually appended.
      */
     size_t append(const uint8_t* msg, size_t msgLen);
 
     /**
-     * @brief Mark the packet as ready to be sent.
+     * @brief Mark the packet as ready.
      */
     inline void markAsReady() { ready = true; }
 
     /**
-     * @brief Copies the content of the buffer at a given address.
+     * @brief Copies the content of the payload at the given address.
+     *
      * @param buf Where to copy the content.
      * @return How many bytes where copied, i.e. the size of the packet.
      */
@@ -112,10 +114,10 @@ public:
     /**
      * @return The timestamp of the first successful call to append().
      */
-    inline uint64_t timestamp() const { return ts; }
+    inline uint64_t getTimestamp() const { return timestamp; }
 
     /**
-     * @return The occupied portion of the buffer (bytes).
+     * @return The occupied portion of the buffer [bytes].
      */
     inline size_t size() const { return content.size(); }
 
@@ -133,7 +135,7 @@ public:
     /**
      * @brief Print information about this object.
      *
-     * @param os For example, std::cout
+     * @param os For example, std::cout.
      */
     void print(std::ostream& os) const;
 
@@ -141,9 +143,9 @@ public:
     std::vector<uint8_t> content;
 
 private:
-    unsigned int msgCounter;
-    uint64_t ts;
-    bool ready;
+    unsigned int msgCounter = 0;
+    uint64_t timestamp      = 0;
+    bool ready              = false;
 };
 
 template <unsigned int len>
@@ -156,9 +158,7 @@ size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
     {
         // Set the packet's timestamp when the first message is inserted
         if (content.size() == 0)
-        {
-            ts = miosix::getTick();
-        }
+            timestamp = TimestampTimer::getInstance().getTimestamp();
 
         // Append the message to the packet
         content.insert(content.end(), msg, msg + msgLen);
@@ -173,7 +173,7 @@ void Packet<len>::clear()
 {
     content.clear();
     msgCounter = 0;
-    ts         = 0;
+    timestamp  = 0;
     ready      = false;
 }
 
@@ -187,188 +187,167 @@ size_t Packet<len>::dump(uint8_t* buf)
 template <unsigned int len>
 void Packet<len>::print(std::ostream& os) const
 {
-    os << "timestamp=" << ts << ", ready=" << ready
+    os << "timestamp=" << timestamp << ", ready=" << ready
        << ", size=" << content.size() << ", msgCounter=" << msgCounter
        << ", content= ";
 
     for (auto const& i : content)
-    {
         os << i;
-    }
     os << '\n';
 }
 
-/******************************************************************************
- * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. The difference
- * is that you pop() Packets but you append() bytes. The bytes will be appended
- * to the first available packet. This class is suitable for synchronization
- * between two threads.
+/**
+ * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets.
  *
- * @tparam pktLen  Maximum length of each packet. (bytes)
- * @tparam pktNum  Total number of packets.
- ******************************************************************************/
+ * The difference is that you pop() Packets but you append() bytes. The bytes
+ * will be appended to the first available packet and the next ones.
+ * This class is suitable for synchronization between two threads.
+ *
+ * @tparam pktLen Maximum length of each packet [bytes].
+ * @tparam pktNum Total number of packets.
+ */
 template <unsigned int pktLen, unsigned int pktNum>
 class SyncPacketQueue
 {
-    using Pkt = Packet<pktLen>;
-
 public:
     /**
-     * @brief Try to append a given message to the last packet. If there isn't
-     * enough space, the packet is marked as ready and the message is appended
-     * to the next packet. If there are no more available packets, the oldest
-     * one is overwritten.
+     * @brief Try to append a given message to the packets queue.
+     *
+     * The message is appended to the last packet and if the space isn't enough,
+     * it is divided into successive packets. If there are no more available
+     * packets, the oldest one is overwritten.
+     *
+     * The message isn't added to the queue if there is no space considering all
+     * the queue packets.
      *
-     * @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
+     * @param msg The message to be appended.
+     * @param msgLen Length of the message [bytes].
+     * @return True if the message was appended.
      */
-    int put(uint8_t* msg, size_t msgLen)
+    bool put(uint8_t* msg, size_t msgLen)
     {
-        int dropped = 0;
-
+        // Check if the message is empty
         if (msgLen == 0)
-        {
-            return -1;
-        }
+            return false;
+
+        // Check if the queue can hold the packet
+        if (msgLen > pktLen * pktNum)
+            return false;
 
         {
+            // Lock the mutex on the buffer
             Lock<FastMutex> l(mutex);
+
             // Add an element if there isn't any
             if (buffer.count() == 0)
-            {
-                buffer.put(Pkt{});
-            }
+                buffer.put({});
 
+            // Write all the packet
             while (msgLen > 0)
             {
+                // If the last packet is ready append a new one
                 if (buffer.last().isReady())
-                {
-                    if (buffer.isFull())
-                    {
-                        // We have dropped a packet
-                        ++dropped;
-                    }
+                    buffer.put({});
 
-                    // 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?
+                // Append what data is possible to the last packet
+                size_t appendedLength = buffer.last().append(msg, msgLen);
 
-                    // last = buffer.last();
-                }
-
-                size_t sentLen = buffer.last().append(msg, msgLen);
-
-                msgLen -= sentLen;
-                msg += sentLen;
-
-                // Mark as ready if the packet is full
+                // If the packet is full mark it as ready
                 if (buffer.last().isFull())
-                {
                     buffer.last().markAsReady();
-                }
-            }
 
-            cvNotempty.broadcast();
-            return dropped;
+                // Go forward in the data
+                msgLen -= appendedLength;
+                msg += appendedLength;
+            }
         }
+
+        // Wake all waiting threads
+        condVerNotEmpty.broadcast();
+
+        return true;
     }
 
     /**
-     * @return a copy of the oldest packet, without removing it from the queue.
+     * @return The oldest packet, without removing it from the queue.
      */
-    const Pkt& get()
+    const Packet<pktLen>& get()
     {
         Lock<FastMutex> l(mutex);
         return buffer.get();
     }
 
     /**
-     * @return the oldest packet, removing it from the queue.
+     * @return The oldest packet, removing it from the queue.
      */
-    const Pkt& pop()
+    const Packet<pktLen>& pop()
     {
         Lock<FastMutex> l(mutex);
         return buffer.pop();
     }
 
     /**
-     * @return true if all the packets have been marked as ready.
+     * @return True if all the packets have been marked as ready.
      */
     bool isFull()
     {
         Lock<FastMutex> l(mutex);
 
         if (buffer.count() > 0)
-        {
             return buffer.isFull() && buffer.last().isReady();
-        }
         else
-        {
             return false;
-        }
     }
 
     /**
-     * @return true if all the packets are completely empty.
+     * @return True if all the packets are completely empty.
      */
     bool isEmpty()
     {
         Lock<FastMutex> l(mutex);
-
         return buffer.isEmpty();
     }
 
     /**
      * @brief Blocks the calling thread until the queue is not empty.
+     *
      * Returns immediately if already not empty.
      */
     void waitUntilNotEmpty()
     {
         Lock<FastMutex> l(mutex);
         if (buffer.isEmpty())
-        {
-            cvNotempty.wait(mutex);
-        }
+            condVerNotEmpty.wait(mutex);
     }
 
     /**
-     * @return the number of packets that are ready to be sent.
+     * @return The number of packets that are ready to be sent.
      */
     size_t countReady()
     {
         Lock<FastMutex> l(mutex);
 
         if (!buffer.isEmpty())
-        {
             return buffer.last().isReady() ? buffer.count()
                                            : buffer.count() - 1;
-        }
         else
-        {
             return 0;
-        }
     }
 
     /**
-     * @return the number of packets in use, that are either fully or partially
+     * @return The number of packets in use, that are either fully or partially
      * filled.
      */
     size_t countNotEmpty()
     {
         Lock<FastMutex> l(mutex);
-
         return buffer.count();
     }
 
 private:
     FastMutex mutex;
-    ConditionVariable cvNotempty;
-
-    CircularBuffer<Pkt, pktNum> buffer;
+    ConditionVariable condVerNotEmpty;
+    CircularBuffer<Packet<pktLen>, pktNum> buffer;
 };
 
 }  // namespace Boardcore
diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp
index aa2beb55466235a36a0f3336034b1c3bf8bf5262..e0cf2865ac8da3f9dbc691dbcd583c2aaa79ec03 100644
--- a/src/tests/catch/test-packetqueue.cpp
+++ b/src/tests/catch/test-packetqueue.cpp
@@ -82,10 +82,12 @@ TEST_CASE("Packet tests")
 
     SECTION("Adding stuff to packet")
     {
+        // Add 5 bytes
+        REQUIRE(p.append(messageBase, 5));
+        uint64_t ts = p.getTimestamp();
 
-        REQUIRE(p.tryAppend(messageBase, 5));
-        uint64_t ts = p.timestamp();
-        REQUIRE(miosix::getTick() - ts < 5);
+        REQUIRE(Boardcore::TimestampTimer::getInstance().getTimestamp() - ts <
+                5);
         REQUIRE(p.dump(buf) == 5);
         COMPARE(buf, BUF_LEN, "01234");
 
@@ -93,29 +95,21 @@ TEST_CASE("Packet tests")
         REQUIRE(p.size() == 5);
         REQUIRE(p.getMsgCount() == 1);
 
-        REQUIRE(p.tryAppend(messageBase + 5, 3));
+        // Add 3 bytes
+        REQUIRE(p.append(messageBase + 5, 3));
         REQUIRE(p.dump(buf) == 8);
         COMPARE(buf, BUF_LEN, "01234567");
         REQUIRE(p.isEmpty() == false);
         REQUIRE(p.size() == 8);
 
-        REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3));
-        REQUIRE(p.dump(buf) == 8);
-        COMPARE(buf, BUF_LEN, "01234567");
-        REQUIRE(p.isEmpty() == false);
-        REQUIRE(p.size() == 8);
-        REQUIRE(p.getMsgCount() == 2);
-
-        REQUIRE(p.tryAppend(messageBase + 8, 2));
-        REQUIRE(p.dump(buf) == 10);
+        // Trying to add 3 more bytes, only 2 should be written
+        REQUIRE(p.append(messageBase + 8, 3) == 2);
+        REQUIRE(p.dump(buf) == PKT_LEN);
         COMPARE(buf, BUF_LEN, "0123456789");
         REQUIRE(p.isEmpty() == false);
-        REQUIRE(p.isFull());
-        REQUIRE(p.size() == 10);
+        REQUIRE(p.size() == PKT_LEN);
         REQUIRE(p.getMsgCount() == 3);
 
-        REQUIRE(p.timestamp() == ts);
-
         p.clear();
         REQUIRE(p.isEmpty());
         REQUIRE(p.isFull() == false);
@@ -130,7 +124,7 @@ TEST_CASE("Packet tests")
     SECTION("Edge cases")
     {
         INFO("Adding empty msg");
-        REQUIRE_FALSE(p.tryAppend(messageBase, 0));
+        REQUIRE_FALSE(p.append(messageBase, 0));
         REQUIRE(p.isEmpty());
         REQUIRE(p.isFull() == false);
         REQUIRE(p.isReady() == false);
@@ -141,29 +135,16 @@ TEST_CASE("Packet tests")
         REQUIRE(p.dump(buf) == 0);
 
         INFO("Adding too big msg");
-        REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1));
-
-        REQUIRE(p.isEmpty());
-        REQUIRE(p.isFull() == false);
-        REQUIRE(p.isReady() == false);
-        REQUIRE(p.size() == 0);
-        REQUIRE(p.maxSize() == PKT_LEN);
+        REQUIRE(p.append(messageBase, PKT_LEN + 1) == PKT_LEN);
 
-        REQUIRE(p.getMsgCount() == 0);
-        REQUIRE(p.dump(buf) == 0);
-
-        INFO("Adding something to full packet");
-        REQUIRE(p.tryAppend(messageBase, PKT_LEN));
-        REQUIRE_FALSE(p.tryAppend(messageBase, 1));
-
-        REQUIRE(p.isEmpty() == false);
+        REQUIRE_FALSE(p.isEmpty());
         REQUIRE(p.isFull());
-        REQUIRE(p.isReady() == false);
+        REQUIRE_FALSE(p.isReady());
         REQUIRE(p.size() == PKT_LEN);
         REQUIRE(p.maxSize() == PKT_LEN);
 
         REQUIRE(p.getMsgCount() == 1);
-        REQUIRE(p.dump(buf) == 10);
+        REQUIRE(p.dump(buf) == PKT_LEN);
         COMPARE(buf, 10, "0123456789");
     }
 }
@@ -182,100 +163,142 @@ TEST_CASE("PacketQueue tests")
     SECTION("Normal operation")
     {
         INFO("Adding two elements to first packet");
-        REQUIRE(pq.put(messageBase, 4) == 0);
-        REQUIRE(pq.put(messageBase, 4) == 0);
+        REQUIRE(pq.put(messageBase, 4));
+        REQUIRE(pq.put(messageBase, 4));
 
+        // No packet should be ready
         REQUIRE(pq.countReady() == 0);
         REQUIRE(pq.countNotEmpty() == 1);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
         INFO("Adding third element and filling first packet");
-        REQUIRE(pq.put(messageBase, 2) == 0);
+        REQUIRE(pq.put(messageBase, 2));
 
+        // Now one single packet should be filled and ready
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 1);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
+        // Check the packet content
         Packet<PKT_LEN> p = pq.get();
         REQUIRE(p.getMsgCount() == 3);
         REQUIRE(p.isFull());
         REQUIRE(p.isReady());
         COMPARE(p, "0123012301");
 
-        INFO("Adding element to second packet");
-        REQUIRE(pq.put(messageBase + 10, 4) == 0);
+        INFO("Adding more data to create a second packet");
+        REQUIRE(pq.put(messageBase + 10, 4));
 
+        // The second packet should not be ready
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 2);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
         REQUIRE_FALSE(pq.buffer.get(1).isReady());
-
         COMPARE(pq.buffer.get(1), "abcd");
 
         p = pq.get();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
 
-        INFO(
-            "Adding element not fitting the second packet, added to the third");
-        REQUIRE(pq.put(messageBase + 10, 7) == 0);
+        INFO("Adding more data to create a third packet");
+        REQUIRE(pq.put(messageBase + 10, 7));
+
         p = pq.get();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
 
-        REQUIRE(pq.countReady() == 2);
-        REQUIRE(pq.countNotEmpty() == 3);
-        REQUIRE_FALSE(pq.isEmpty());
-        REQUIRE_FALSE(pq.isFull());
-
+        // Check all the packages
         REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "0123012301");
         REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "abcdabcdef");
         REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "g");
 
-        COMPARE(pq.buffer.get(0), "0123012301");
-        COMPARE(pq.buffer.get(1), "abcd");
-        COMPARE(pq.buffer.get(2), "abcdefg");
+        // Check the queue stats
+        REQUIRE(pq.countReady() == 2);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE_FALSE(pq.isEmpty());
+        REQUIRE_FALSE(pq.isFull());
 
         INFO("Popping first element");
         p = pq.pop();  // Should still return first packet
         REQUIRE(p.getMsgCount() == 3);
         COMPARE(p, "0123012301");
 
-        // Should now return what was the second element
-        COMPARE(pq.get(), "abcd");
-
+        // The packets should now be shifted
         REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "abcdabcdef");
         REQUIRE_FALSE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == 1);
+        COMPARE(pq.buffer.get(1), "g");
 
         REQUIRE(pq.countReady() == 1);
         REQUIRE(pq.countNotEmpty() == 2);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE_FALSE(pq.isFull());
 
-        INFO("Adding a msg back to the first packet and filling it");
-        REQUIRE(pq.put(messageBase, 10) == 0);
-        REQUIRE(pq.countReady() == 3);
+        INFO("Adding more data to fill the last packet");
+        REQUIRE(pq.put(messageBase, 10));
+        REQUIRE(pq.countReady() == 2);
         REQUIRE(pq.countNotEmpty() == 3);
         REQUIRE_FALSE(pq.isEmpty());
-        REQUIRE(pq.isFull());
+        REQUIRE_FALSE(pq.isFull());
 
-        COMPARE(pq.buffer.get(0), "abcd");
-        COMPARE(pq.buffer.get(1), "abcdefg");
-        COMPARE(pq.buffer.get(2), "0123456789");
+        // We should now have three packets
+        REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "abcdabcdef");
+        REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "g012345678");
+        REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "9");
+
+        // If we now add another 10 bytes the last packet, the last byte which
+        // does not fit should be put in a new packet at the start of the queue
+        REQUIRE(pq.put(messageBase, 10));
+        REQUIRE(pq.countReady() == 2);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE_FALSE(pq.isEmpty());
+        REQUIRE_FALSE(pq.isFull());
+        REQUIRE(pq.buffer.get(0).isReady());
+        REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(0), "g012345678");
+        REQUIRE(pq.buffer.get(1).isReady());
+        REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(1), "9012345678");
+        REQUIRE_FALSE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == 1);
+        COMPARE(pq.buffer.get(2), "9");
+
+        // And now by adding the last 9 bytes the queue should be marked ready
+        REQUIRE(pq.put(messageBase + 10, 9));
+        REQUIRE(pq.countReady() == 3);
+        REQUIRE(pq.countNotEmpty() == 3);
+        REQUIRE(pq.buffer.get(2).isReady());
+        REQUIRE(pq.buffer.get(2).size() == PKT_LEN);
+        COMPARE(pq.buffer.get(2), "9abcdefghi");
 
         INFO("Popping everything");
 
         p = pq.pop();
-        COMPARE(p, "abcd");
         REQUIRE(p.isReady());
+        REQUIRE(p.size() == PKT_LEN);
+        COMPARE(p, "g012345678");
 
         p = pq.pop();
-        COMPARE(p, "abcdefg");
+        COMPARE(p, "9012345678");
         REQUIRE(p.isReady());
 
         p = pq.pop();
-        COMPARE(p, "0123456789");
+        COMPARE(p, "9abcdefghi");
         REQUIRE(p.isReady());
 
         REQUIRE_FALSE(pq.isFull());
@@ -289,19 +312,19 @@ TEST_CASE("PacketQueue tests")
     SECTION("Edge cases")
     {
         INFO("Adding too big msg");
-        REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1);
+        REQUIRE_FALSE(pq.put(messageBase, PKT_LEN * QUEUE_LEN + 1));
         REQUIRE_FALSE(pq.isFull());
         REQUIRE(pq.isEmpty());
         REQUIRE(pq.countNotEmpty() == 0);
         REQUIRE(pq.countReady() == 0);
 
         INFO("Adding empty message");
-        REQUIRE(pq.put(messageBase, 0) == -1);
+        REQUIRE_FALSE(pq.put(messageBase, 0));
 
         INFO("Adding something to full queue");
-        REQUIRE(pq.put(messageBase, PKT_LEN) == 0);
-        REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0);
-        REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0);
+        REQUIRE(pq.put(messageBase, PKT_LEN));
+        REQUIRE(pq.put(messageBase + 5, PKT_LEN));
+        REQUIRE(pq.put(messageBase + 10, PKT_LEN));
 
         REQUIRE(pq.buffer.count() == 3);
         for (int i = 0; i < 3; i++)