diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1c07b46355e37135c849ff6faa1c81f512fb09d7..53273bb8481f7b3ac5c2385fd4505fd29a0f90fe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -274,7 +274,7 @@ add_executable(test-sx1278-serial
     src/tests/drivers/sx1278/test-sx1278-serial.cpp
     src/tests/drivers/sx1278/test-sx1278-core.cpp
 )
-sbs_target(test-sx1278-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Sensors                               #
diff --git a/src/shared/drivers/timer/TimestampTimer.h b/src/shared/drivers/timer/TimestampTimer.h
index 3f54fb1ad6959892353230fce24cc75a5e813f5c..8b2bc6f2f445bc0a21f3fd32bd754be102759e25 100644
--- a/src/shared/drivers/timer/TimestampTimer.h
+++ b/src/shared/drivers/timer/TimestampTimer.h
@@ -101,7 +101,7 @@ private:
 inline uint64_t TimestampTimer::getTimestamp()
 {
 #ifdef COMPILE_FOR_HOST
-    return 0;
+    return time(NULL);
 #else
     // With a timer frequency of 250KHz, the conversion from timer ticks to
     // microseconds only take a 2 byte shift (x4)
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 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
@@ -347,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/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp
index 52a91f3ba06520aa4b18974468319a22256b6774..bed1ae549d6daf57f3b77e8f6f0787c3efa9b5d9 100644
--- a/src/shared/radio/SX1278/SX1278.cpp
+++ b/src/shared/radio/SX1278/SX1278.cpp
@@ -31,6 +31,8 @@ namespace Boardcore
 
 using namespace SX1278Defs;
 
+long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; }
+
 // Default values for registers
 constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK |
                                         RegOpMode::MODULATION_TYPE_FSK |
@@ -247,10 +249,14 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
 
 bool SX1278::send(uint8_t *pkt, size_t len)
 {
-    // Packets longer than 255 are not supported
-    if (len > 255)
+    // Packets longer than FIFO_LEN (-1 for the len byte) are not supported
+    if (len > SX1278Defs::FIFO_LEN - 1)
         return false;
 
+    // This shouldn't be needed, but for some reason the device "lies" about
+    // being ready, so lock up if we are going too fast
+    rateLimitTx();
+
     bus_mgr.lock(SX1278BusManager::Mode::MODE_TX);
 
     // Wait for TX ready
@@ -267,11 +273,24 @@ bool SX1278::send(uint8_t *pkt, size_t len)
     // Wait for packet sent
     bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT);
     bus_mgr.unlock();
+
+    last_tx = now();
     return true;
 }
 
 void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); }
 
+void SX1278::rateLimitTx()
+{
+    const long long RATE_LIMIT = 2;
+
+    long long delta = now() - last_tx;
+    if (delta <= RATE_LIMIT)
+    {
+        miosix::Thread::sleep(RATE_LIMIT - delta);
+    }
+}
+
 uint8_t SX1278::getVersion()
 {
     SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
diff --git a/src/shared/radio/SX1278/SX1278.h b/src/shared/radio/SX1278/SX1278.h
index 6fb4c355d36f81331e7299e6a291e58c737e4156..3d89a9f9494a95ddcae4871f84a8c21132571066 100644
--- a/src/shared/radio/SX1278/SX1278.h
+++ b/src/shared/radio/SX1278/SX1278.h
@@ -228,6 +228,8 @@ public:
 public:
     using Mode = SX1278BusManager::Mode;
 
+    void rateLimitTx();
+
     void setBitrate(int bitrate);
     void setFreqDev(int freq_dev);
     void setFreqRF(int freq_rf);
@@ -240,6 +242,7 @@ public:
 
     void waitForIrq(uint16_t mask);
 
+    long long last_tx = 0;
     SX1278BusManager bus_mgr;
     PrintLogger logger = Logging::getLogger("sx1278");
 };
diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h
index cd50d09a8c3294659f1c1186f2ad1fd0512937c6..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644
--- a/src/shared/radio/SX1278/SX1278Defs.h
+++ b/src/shared/radio/SX1278/SX1278Defs.h
@@ -33,6 +33,11 @@ namespace Boardcore
 namespace SX1278Defs
 {
 
+/**
+ * @brief Length of the internal FIFO
+ */
+constexpr int FIFO_LEN = 64;
+
 /**
  * @brief Main oscillator frequency (Hz)
  */
diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp
index 53f2b2160f01eaee81a6151f4d4fd931fd4ef362..c8fd6e5400f5209f54a43a2897d990a0505d585a 100644
--- a/src/shared/sensors/BME280/BME280.cpp
+++ b/src/shared/sensors/BME280/BME280.cpp
@@ -66,21 +66,47 @@ bool BME280::init()
 
     loadCompensationParameters();
 
-    // Read once the temperature to compute fineTemperature
-    setConfiguration(BME280_CONFIG_TEMP_SINGLE);
-    miosix::Thread::sleep(
-        calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
-    readTemperature();
+    // Set the configuration 10 times to be sure
+    for (int i = 0; i < 10; i++)
+    {
+        // Read once the temperature to compute fineTemperature
+        setConfiguration(BME280_CONFIG_TEMP_SINGLE);
+        miosix::Thread::sleep(
+            calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE));
+        readTemperature();
 
-    setConfiguration();
+        setConfiguration();
+    }
+
+    // Set a sleep time to allow the sensor to change internally the data
+    miosix::Thread::sleep(100);
 
-    BME280Config readBackConfig = readConfiguration();
+    // I create the config state which represents the logic or of all the
+    // 5 readConfiguration controls (We perform 5 checks to avoid that the
+    // sensor is busy implicating in wrong responses)
+    bool readConfigResult = false;
+    BME280Config readBackConfig;
+
+    for (int i = 0; i < 10; i++)
+    {
+        readBackConfig = readConfiguration();
+        // Check if the configration on the device matches ours
+        if (config.bytes.ctrlHumidity == readBackConfig.bytes.ctrlHumidity &&
+            config.bytes.ctrlPressureAndTemperature ==
+                readBackConfig.bytes.ctrlPressureAndTemperature &&
+            config.bytes.config == readBackConfig.bytes.config)
+        {
+            readConfigResult = true;
+            break;
+        }
+
+        // After the check i sleep 100 milliseconds
+        miosix::Thread::sleep(20);
+    }
 
-    // Check if the configration on the device matches ours
-    if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity ||
-        config.bytes.ctrlPressureAndTemperature !=
-            readBackConfig.bytes.ctrlPressureAndTemperature ||
-        config.bytes.config != readBackConfig.bytes.config)
+    //   If after the 5 iterations the sensor didn't report the configuration
+    //   set I can report the init error
+    if (!readConfigResult)
     {
         LOG_ERR(logger, "Device configuration incorrect, setup failed");
 
diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp
index c877dc04a378f68c6479fb8e9f53520a1a515de6..53ac4adc512f65aea68ca0e313ee6b51b3984870 100644
--- a/src/shared/sensors/SensorManager.cpp
+++ b/src/shared/sensors/SensorManager.cpp
@@ -50,7 +50,12 @@ SensorManager::~SensorManager()
         delete scheduler;
 }
 
-bool SensorManager::start() { return initResult ? scheduler->start() : false; }
+bool SensorManager::start()
+{
+    if (customScheduler)
+        scheduler->start();
+    return initResult;
+}
 
 void SensorManager::stop() { scheduler->stop(); }
 
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 ff5e7b360a3f612cf73feae05e0283267d96108f..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.
@@ -67,24 +67,25 @@ 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.
+     * 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.
-     * @return true if the message was appended correctly.
-     * @return false if there isn't enough space for the message.
+     * @param msgLen Length of 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.
+     * @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.
      */
@@ -111,12 +112,12 @@ 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; }
+    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(); }
 
@@ -126,7 +127,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; }
@@ -134,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;
 
@@ -142,32 +143,29 @@ 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>
-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)
-        {
-            ts = miosix::getTick();
-        }
+            timestamp = TimestampTimer::getInstance().getTimestamp();
 
         // Append the message to the packet
         content.insert(content.end(), msg, msg + msgLen);
         msgCounter++;
-
-        return true;
     }
+
+    return msgLen;
 }
 
 template <unsigned int len>
@@ -175,7 +173,7 @@ void Packet<len>::clear()
 {
     content.clear();
     msgCounter = 0;
-    ts         = 0;
+    timestamp  = 0;
     ready      = false;
 }
 
@@ -189,194 +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.
      *
-     * @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
+     * 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 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 false;
 
-        if (msgLen == 0 || msgLen > pktLen)
-        {
-            return -1;
-        }
+        // 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({});
 
-            Pkt& last = buffer.last();
+            // Write all the packet
+            while (msgLen > 0)
+            {
+                // If the last packet is ready append a new one
+                if (buffer.last().isReady())
+                    buffer.put({});
 
-            bool added = false;
+                // Append what data is possible to the last packet
+                size_t appendedLength = buffer.last().append(msg, msgLen);
 
-            // Add to the current packet only if it isn't already ready
-            if (!last.isReady())
-            {
-                added = last.tryAppend(msg, msgLen);
-            }
+                // If the packet is full mark it as ready
+                if (buffer.last().isFull())
+                    buffer.last().markAsReady();
 
-            // If already ready or cannot fit the new data, add to a new packet
-            if (!added)
-            {
-                // Mark the packet as ready (in the case it wasn't already)
-                last.markAsReady();
-
-                if (buffer.isFull())
-                {
-                    // We have dropped a packet
-                    ++dropped;
-                }
-                // Add a new packet and fill that instead
-                Pkt& newpkt = buffer.put(Pkt{});
-
-                if (!newpkt.tryAppend(msg, msgLen))
-                {
-                    TRACE("Packet is too big!\n");
-                    return -1;
-                }
+                // Go forward in the data
+                msgLen -= appendedLength;
+                msg += appendedLength;
             }
+        }
 
-            // Mark as ready if the packet is full
-            if (buffer.last().isFull())
-            {
-                buffer.last().markAsReady();
-            }
+        // Wake all waiting threads
+        condVerNotEmpty.broadcast();
 
-            cvNotempty.broadcast();
-            return dropped;
-        }
+        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++)
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
index d484d39cab98c3703d89d27da0c176900cca7690..31db75f4f5c47aa0a847be004e403718df858116 100644
--- a/src/tests/drivers/sx1278/test-sx1278-bench.cpp
+++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
@@ -93,9 +93,6 @@ struct Stats
 
 } stats;
 
-/// Interval between transmissions.
-const int TX_INTERVAL = 1;
-
 SX1278 *sx1278[2] = {nullptr, nullptr};
 
 struct Msg
@@ -115,12 +112,10 @@ void recvLoop(int idx)
     while (1)
     {
         Msg msg;
-        msg.idx     = 0;
-        msg.dummy_1 = 0;
-        msg.dummy_2 = 0;
-        msg.dummy_3 = 0;
+        memset(&msg, 0, sizeof(msg));
 
         int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg));
+
         if (len != sizeof(msg))
         {
             stats.recv_errors++;
@@ -143,8 +138,6 @@ void sendLoop(int idx)
 {
     while (1)
     {
-        miosix::Thread::sleep(TX_INTERVAL);
-
         int next_idx = stats.send_count + 1;
 
         Msg msg;
@@ -269,6 +262,7 @@ int main()
     std::thread recv([]() { recvLoop(0); });
 #endif
 #ifndef DISABLE_TX
+    miosix::Thread::sleep(500);
     std::thread send([]() { sendLoop(1); });
 #endif
 
diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
index 0078a16b949f927ca05b82cd7dd6f67b2023b40f..22df3fe51535b7a62aefe3bdce39aae66208ec5f 100644
--- a/src/tests/drivers/sx1278/test-sx1278-serial.cpp
+++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
@@ -31,17 +31,17 @@
 using namespace Boardcore;
 using namespace miosix;
 
-SPIBus bus(SPI3);
+SPIBus bus(SPI4);
 
-GpioPin sck(GPIOC_BASE, 10);
-GpioPin miso(GPIOC_BASE, 11);
-GpioPin mosi(GPIOC_BASE, 12);
-GpioPin cs(GPIOA_BASE, 1);
-GpioPin dio(GPIOC_BASE, 15);
+GpioPin sck(GPIOE_BASE, 2);
+GpioPin miso(GPIOE_BASE, 5);
+GpioPin mosi(GPIOE_BASE, 6);
+GpioPin cs(GPIOC_BASE, 1);
+GpioPin dio(GPIOF_BASE, 10);
 
 SX1278* sx1278 = nullptr;
 
-void __attribute__((used)) EXTI15_IRQHandlerImpl()
+void __attribute__((used)) EXTI10_IRQHandlerImpl()
 {
     if (sx1278)
         sx1278->handleDioIRQ();
@@ -54,16 +54,16 @@ void initBoard()
         miosix::FastInterruptDisableLock dLock;
 
         // Enable SPI3
-        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // Enable SPI4 bus
         RCC_SYNC();
 
         // Setup SPI pins
         sck.mode(miosix::Mode::ALTERNATE);
-        sck.alternateFunction(6);
+        sck.alternateFunction(5);
         miso.mode(miosix::Mode::ALTERNATE);
-        miso.alternateFunction(6);
+        miso.alternateFunction(5);
         mosi.mode(miosix::Mode::ALTERNATE);
-        mosi.alternateFunction(6);
+        mosi.alternateFunction(5);
 
         cs.mode(miosix::Mode::OUTPUT);
         dio.mode(miosix::Mode::INPUT);
@@ -77,30 +77,35 @@ void initBoard()
 void recvLoop()
 {
     uint8_t msg[256];
-    auto console = miosix::DefaultConsole::instance().get();
-
     while (1)
     {
         int len = sx1278->receive(msg, sizeof(msg));
         if (len > 0)
         {
-            console->writeBlock(msg, len, 0);
-            // TODO: Flushing?
+            auto serial = miosix::DefaultConsole::instance().get();
+            serial->writeBlock(msg, len, 0);
         }
     }
 }
 
 void sendLoop()
 {
-    uint8_t msg[256];
-    auto console = miosix::DefaultConsole::instance().get();
+    // I create a GPIO with the onboard led to tell the user that
+    // a package is being sent
+    miosix::GpioPin led(GPIOG_BASE, 13);
+    led.mode(miosix::Mode::OUTPUT);
+    led.low();
 
+    uint8_t msg[63];
     while (1)
     {
-        int len = console->readBlock(msg, sizeof(msg), 0);
+        auto serial = miosix::DefaultConsole::instance().get();
+        int len     = serial->readBlock(msg, sizeof(msg), 0);
         if (len > 0)
         {
+            led.high();
             sx1278->send(msg, len);
+            led.low();
         }
     }
 }
@@ -122,13 +127,20 @@ int main()
         return -1;
     }
 
+    printf("\n[sx1278] Initialization complete!\n");
+
     std::thread recv([]() { recvLoop(); });
     std::thread send([]() { sendLoop(); });
 
-    printf("\n[sx1278] Initialization complete!\n");
+    for (;;)
+    {
+        miosix::Thread::sleep(100);
+    }
 
-    while (1)
-        miosix::Thread::wait();
+    // God please forgive me
+    // FIXME(davide.mor): ABSOLUTELY fix this
+    // miosix::Thread::sleep(20000);
+    // miosix::reboot();
 
     return 0;
 }