diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e5162364a0b16ffce590db067bfe8abc5401265..2ff5af146481747dfdf70125736764aba2ac10d2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -195,6 +195,46 @@ sbs_target(test-stepper stm32f429zi_stm32f4discovery)
 add_executable(test-fsm src/tests/events/fsm/test-fsm.cpp)
 sbs_target(test-fsm stm32f429zi_stm32f4discovery)
 
+#-----------------------------------------------------------------------------#
+#                               Tests - Radio                                 #
+#-----------------------------------------------------------------------------#
+
+add_executable(test-sx1278-bench
+    src/tests/drivers/sx1278/test-sx1278-bench.cpp
+    src/tests/drivers/sx1278/test-sx1278-core.cpp
+)
+sbs_target(test-sx1278-bench stm32f407vg_stm32f4discovery)
+
+add_executable(test-sx1278-bidir
+    src/tests/drivers/sx1278/test-sx1278-bidir.cpp
+    src/tests/drivers/sx1278/test-sx1278-core.cpp
+)
+sbs_target(test-sx1278-bidir stm32f407vg_stm32f4discovery)
+
+add_executable(test-sx1278-tx
+    src/tests/drivers/sx1278/test-sx1278-tx.cpp
+    src/tests/drivers/sx1278/test-sx1278-core.cpp
+)
+sbs_target(test-sx1278-tx stm32f407vg_stm32f4discovery)
+
+add_executable(test-sx1278-rx
+    src/tests/drivers/sx1278/test-sx1278-rx.cpp
+    src/tests/drivers/sx1278/test-sx1278-core.cpp
+)
+sbs_target(test-sx1278-rx stm32f407vg_stm32f4discovery)
+
+add_executable(test-sx1278-mavlink
+    src/tests/drivers/sx1278/test-sx1278-mavlink.cpp
+    src/tests/drivers/sx1278/test-sx1278-core.cpp
+)
+sbs_target(test-sx1278-mavlink stm32f407vg_stm32f4discovery)
+
+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)
+
 #-----------------------------------------------------------------------------#
 #                               Tests - Sensors                               #
 #-----------------------------------------------------------------------------#
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 9d99de5a811713715c005a7a24a917ecdb1ff5ae..6a95bd1307e2117c08aedbfba84058e8ab203c81 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -43,6 +43,7 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp
         ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp
         ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
+        ${SBS_BASE}/src/shared/drivers/SX1278/SX1278.cpp
 
         # Events
         ${SBS_BASE}/src/shared/events/EventBroker.cpp
diff --git a/src/shared/drivers/SX1278/SX1278.cpp b/src/shared/drivers/SX1278/SX1278.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52a91f3ba06520aa4b18974468319a22256b6774
--- /dev/null
+++ b/src/shared/drivers/SX1278/SX1278.cpp
@@ -0,0 +1,476 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "SX1278.h"
+
+#include <kernel/scheduler/scheduler.h>
+
+#include <cmath>
+
+namespace Boardcore
+{
+
+using namespace SX1278Defs;
+
+// Default values for registers
+constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK |
+                                        RegOpMode::MODULATION_TYPE_FSK |
+                                        RegOpMode::LOW_FREQUENCY_MODE_ON;
+
+constexpr uint8_t REG_SYNC_CONFIG_DEFAULT =
+    RegSyncConfig::AUTO_RESTART_RX_MODE_OFF |
+    RegSyncConfig::PREAMBLE_POLARITY_55 | RegSyncConfig::SYNC_ON;
+
+SX1278BusManager::SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs)
+    : slave(bus, cs, spiConfig()), mode(SX1278BusManager::Mode::MODE_SLEEP)
+{
+}
+
+void SX1278BusManager::lock(SX1278BusManager::Mode mode)
+{
+    mutex.lock();
+
+    enterMode(mode);
+    irq_wait_thread = nullptr;
+}
+
+void SX1278BusManager::unlock()
+{
+    if (rx_wait_thread != nullptr)
+    {
+        // Restore rx state
+        irq_wait_thread = rx_wait_thread;
+        enterMode(SX1278BusManager::Mode::MODE_RX);
+    }
+
+    mutex.unlock();
+}
+
+SPISlave &SX1278BusManager::getBus() { return slave; }
+
+void SX1278BusManager::handleDioIRQ()
+{
+    if (irq_wait_thread)
+    {
+        irq_wait_thread->IRQwakeup();
+        if (irq_wait_thread->IRQgetPriority() >
+            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
+        {
+            miosix::Scheduler::IRQfindNextThread();
+        }
+
+        // Check if we woke the rx thread
+        if (irq_wait_thread == rx_wait_thread)
+            rx_wait_thread = nullptr;
+
+        irq_wait_thread = nullptr;
+    }
+}
+
+void SX1278BusManager::waitForIrq(uint16_t mask)
+{
+    // Tight loop on IRQ register
+    while (!(getIrqFlags() & mask))
+        miosix::delayUs(10);
+}
+
+void SX1278BusManager::waitForRxIrq()
+{
+    // NOTE: waitForRxIrq is only available for RX
+
+    // Check before entering irq mode
+    if (getIrqFlags() & RegIrqFlags::PAYLOAD_READY)
+        return;
+
+    miosix::FastInterruptDisableLock dLock;
+    irq_wait_thread = rx_wait_thread = miosix::Thread::getCurrentThread();
+    // Release lock to allow for writers
+    mutex.unlock();
+
+    // Avoid spurious wakeups
+    while (rx_wait_thread != 0)
+    {
+        rx_wait_thread->IRQwait();
+        {
+            miosix::FastInterruptEnableLock eLock(dLock);
+            miosix::Thread::yield();
+        }
+    }
+
+    // Regain ownership of the lock
+    mutex.lock();
+}
+
+void SX1278BusManager::enterMode(Mode mode)
+{
+    // Check if necessary
+    if (mode == this->mode)
+        return;
+
+    setMode(mode);
+
+    // BUG: Removing the next line makes it, better?!?
+    // waitForIrq(RegIrqFlags::MODE_READY);
+
+    this->mode = mode;
+}
+
+uint16_t SX1278BusManager::getIrqFlags()
+{
+    SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED);
+
+    uint8_t flags[2] = {0, 0};
+    spi.readRegisters(REG_IRQ_FLAGS_1, flags, 2);
+
+    return (flags[0] | flags[1] << 8);
+}
+
+void SX1278BusManager::setMode(Mode mode)
+{
+    SPITransaction spi(getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_OP_MODE, REG_OP_MODE_DEFAULT | mode);
+}
+
+SX1278::SX1278(SPIBusInterface &bus, miosix::GpioPin cs) : bus_mgr(bus, cs) {}
+
+SX1278::Error SX1278::init(Config config)
+{
+    // Lock the bus
+    bus_mgr.lock(Mode::MODE_STDBY);
+    bus_mgr.waitForIrq(RegIrqFlags::MODE_READY);
+
+    if (getVersion() != 0x12)
+    {
+        return Error::BAD_VERSION;
+    }
+
+    setBitrate(config.bitrate);
+    setFreqDev(config.freq_dev);
+    setFreqRF(config.freq_rf);
+
+    setRxBw(config.rx_bw);
+    setAfcBw(config.afc_bw);
+
+    setOcp(config.ocp);
+
+    uint8_t sync_word[2] = {0x12, 0xad};
+    setSyncWord(sync_word, 2);
+    setPreableLen(2);
+    setPa(config.power, true);
+
+    // Setup generic parameters
+    {
+        SPITransaction spi(bus_mgr.getBus(),
+                           SPITransaction::WriteBit::INVERTED);
+
+        spi.writeRegister(REG_PA_RAMP,
+                          RegPaRamp::MODULATION_SHAPING_NONE | 0x09);
+        spi.writeRegister(REG_RX_CONFIG,
+                          RegRxConfig::AFC_AUTO_ON | RegRxConfig::AGC_AUTO_ON |
+                              RegRxConfig::RX_TRIGGER_PREAMBLE_DETECT |
+                              RegRxConfig::RX_TRIGGER_RSSI_INTERRUPT);
+        spi.writeRegister(REG_RSSI_THRESH, 0xff);
+        spi.writeRegister(
+            REG_PREAMBLE_DETECT,
+            RegPreambleDetector::PREAMBLE_DETECTOR_ON |
+                RegPreambleDetector::PREAMBLE_DETECTOR_SIZE_2_BYTES | 0x0a);
+        spi.writeRegister(REG_RX_TIMEOUT_1, 0x00);
+        spi.writeRegister(REG_RX_TIMEOUT_2, 0x00);
+        spi.writeRegister(REG_RX_TIMEOUT_3, 0x00);
+        spi.writeRegister(REG_PACKET_CONFIG_1,
+                          RegPacketConfig1::PACKET_FORMAT_VARIABLE_LENGTH |
+                              RegPacketConfig1::DC_FREE_NONE |
+                              RegPacketConfig1::CRC_ON |
+                              RegPacketConfig1::ADDRESS_FILTERING_NONE |
+                              RegPacketConfig1::CRC_WHITENING_TYPE_CCITT_CRC);
+        spi.writeRegister(REG_PACKET_CONFIG_2,
+                          RegPacketConfig2::DATA_MODE_PACKET);
+        spi.writeRegister(
+            REG_FIFO_THRESH,
+            RegFifoThresh::TX_START_CONDITION_FIFO_NOT_EMPTY | 0x0f);
+        spi.writeRegister(REG_NODE_ADRS, 0x00);
+        spi.writeRegister(REG_BROADCAST_ADRS, 0x00);
+
+        // Enable PayloadReady, PacketSent on DIO0
+        spi.writeRegister(REG_DIO_MAPPING_1, 0x00);
+    }
+
+    bus_mgr.unlock();
+    return Error::NONE;
+}
+
+ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
+{
+    bus_mgr.lock(Mode::MODE_RX);
+
+    uint8_t len = 0;
+    do
+    {
+        // Special wait for payload ready
+        bus_mgr.waitForRxIrq();
+
+        {
+            SPITransaction spi(bus_mgr.getBus(),
+                               SPITransaction::WriteBit::INVERTED);
+            len = spi.readRegister(REG_FIFO);
+            if (len > max_len)
+                return -1;
+
+            spi.readRegisters(REG_FIFO, pkt, len);
+        }
+
+        // For some reason this sometimes happen?
+    } while (len == 0);
+
+    bus_mgr.unlock();
+    return len;
+}
+
+bool SX1278::send(uint8_t *pkt, size_t len)
+{
+    // Packets longer than 255 are not supported
+    if (len > 255)
+        return false;
+
+    bus_mgr.lock(SX1278BusManager::Mode::MODE_TX);
+
+    // Wait for TX ready
+    bus_mgr.waitForIrq(RegIrqFlags::TX_READY);
+
+    {
+        SPITransaction spi(bus_mgr.getBus(),
+                           SPITransaction::WriteBit::INVERTED);
+
+        spi.writeRegister(REG_FIFO, static_cast<uint8_t>(len));
+        spi.writeRegisters(REG_FIFO, pkt, len);
+    }
+
+    // Wait for packet sent
+    bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT);
+    bus_mgr.unlock();
+    return true;
+}
+
+void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); }
+
+uint8_t SX1278::getVersion()
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+
+    return spi.readRegister(REG_VERSION);
+}
+
+void SX1278::setBitrate(int bitrate)
+{
+    uint16_t val = SX1278Defs::FXOSC / bitrate;
+
+    // Update values
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_BITRATE_MSB, val >> 8);
+    spi.writeRegister(REG_BITRATE_LSB, val);
+}
+
+void SX1278::setFreqDev(int freq_dev)
+{
+    uint16_t val = freq_dev / FSTEP;
+
+    // Update values
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_FDEV_MSB, (val >> 8) & 0x3f);
+    spi.writeRegister(REG_FDEV_LSB, val);
+}
+
+void SX1278::setFreqRF(int freq_rf)
+{
+    uint32_t val = freq_rf / FSTEP;
+
+    // Update values
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_FRF_MSB, val >> 16);
+    spi.writeRegister(REG_FRF_MID, val >> 8);
+    spi.writeRegister(REG_FRF_LSB, val);
+}
+
+void SX1278::setOcp(int ocp)
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    if (ocp == 0)
+    {
+        spi.writeRegister(REG_OCP, 0);
+    }
+    else if (ocp <= 120)
+    {
+        uint8_t raw = (ocp - 45) / 5;
+        spi.writeRegister(REG_OCP, RegOcp::OCP_ON | raw);
+    }
+    else
+    {
+        uint8_t raw = (ocp + 30) / 10;
+        spi.writeRegister(REG_OCP, RegOcp::OCP_ON | raw);
+    }
+}
+
+void SX1278::setSyncWord(uint8_t value[], int size)
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_SYNC_CONFIG, REG_SYNC_CONFIG_DEFAULT | size);
+
+    for (int i = 0; i < size; i++)
+    {
+        spi.writeRegister(REG_SYNC_VALUE_1 + i, value[i]);
+    }
+}
+
+void SX1278::setRxBw(RxBw rx_bw)
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_RX_BW, static_cast<uint8_t>(rx_bw));
+}
+
+void SX1278::setAfcBw(RxBw afc_bw)
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_AFC_BW, static_cast<uint8_t>(afc_bw));
+}
+
+void SX1278::setPreableLen(int len)
+{
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+    spi.writeRegister(REG_PREAMBLE_MSB, len >> 8);
+    spi.writeRegister(REG_PREAMBLE_LSB, len);
+}
+
+void SX1278::setPa(int power, bool pa_boost)
+{
+    // [2, 17] or 20 if PA_BOOST
+    // [0, 15] if !PA_BOOST
+
+    const uint8_t MAX_POWER = 0b111;
+
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+
+    if (!pa_boost)
+    {
+        // Don't use power amplifier boost
+        power = power - MAX_POWER + 15;
+        spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power);
+        spi.writeRegister(REG_PA_DAC,
+                          RegPaDac::PA_DAC_DEFAULT_VALUE | 0x10 << 3);
+    }
+    else if (power != 20)
+    {
+        // Run power amplifier boost but not at full power
+        power = power - 2;
+        spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power |
+                                             RegPaConfig::PA_SELECT_BOOST);
+        spi.writeRegister(REG_PA_DAC, RegPaDac::PA_DAC_PA_BOOST | 0x10 << 3);
+    }
+    else
+    {
+        // Run power amplifier boost at full power
+        power = 15;
+        spi.writeRegister(REG_PA_CONFIG, MAX_POWER << 4 | power |
+                                             RegPaConfig::PA_SELECT_BOOST);
+        spi.writeRegister(REG_PA_DAC, RegPaDac::PA_DAC_PA_BOOST | 0x10 << 3);
+    }
+}
+
+void SX1278::debugDumpRegisters()
+{
+    struct RegDef
+    {
+        const char *name;
+        int addr;
+    };
+
+    const RegDef defs[] = {
+        RegDef{"REG_OP_MODE", REG_OP_MODE},
+        RegDef{"REG_BITRATE_MSB", REG_BITRATE_MSB},
+        RegDef{"REG_BITRATE_LSB", REG_BITRATE_LSB},
+        RegDef{"REG_FDEV_MSB", REG_FDEV_MSB},
+        RegDef{"REG_FDEV_LSB", REG_FDEV_LSB},
+        RegDef{"REG_FRF_MSB", REG_FRF_MSB}, RegDef{"REG_FRF_MID", REG_FRF_MID},
+        RegDef{"REG_FRF_LSB", REG_FRF_LSB},
+        RegDef{"REG_PA_CONFIG", REG_PA_CONFIG},
+        RegDef{"REG_PA_RAMP", REG_PA_RAMP}, RegDef{"REG_OCP", REG_OCP},
+        RegDef{"REG_LNA", REG_LNA}, RegDef{"REG_RX_CONFIG", REG_RX_CONFIG},
+        RegDef{"REG_RSSI_CONFIG", REG_RSSI_CONFIG},
+        RegDef{"REG_RSSI_COLLISION", REG_RSSI_COLLISION},
+        RegDef{"REG_RSSI_THRESH", REG_RSSI_THRESH},
+        // RegDef { "REG_RSSI_VALUE", REG_RSSI_VALUE },
+        RegDef{"REG_RX_BW", REG_RX_BW}, RegDef{"REG_AFC_BW", REG_AFC_BW},
+        RegDef{"REG_OOK_PEAK", REG_OOK_PEAK},
+        RegDef{"REG_OOK_FIX", REG_OOK_FIX}, RegDef{"REG_OOK_AVG", REG_OOK_AVG},
+        RegDef{"REG_AFC_FEI", REG_AFC_FEI}, RegDef{"REG_AFC_MSB", REG_AFC_MSB},
+        RegDef{"REG_AFC_LSB", REG_AFC_LSB}, RegDef{"REG_FEI_MSB", REG_FEI_MSB},
+        RegDef{"REG_FEI_LSB", REG_FEI_LSB},
+        RegDef{"REG_PREAMBLE_DETECT", REG_PREAMBLE_DETECT},
+        RegDef{"REG_RX_TIMEOUT_1", REG_RX_TIMEOUT_1},
+        RegDef{"REG_RX_TIMEOUT_2", REG_RX_TIMEOUT_2},
+        RegDef{"REG_RX_TIMEOUT_3", REG_RX_TIMEOUT_3},
+        RegDef{"REG_RX_DELAY", REG_RX_DELAY}, RegDef{"REG_OSC", REG_OSC},
+        RegDef{"REG_PREAMBLE_MSB", REG_PREAMBLE_MSB},
+        RegDef{"REG_PREAMBLE_LSB", REG_PREAMBLE_LSB},
+        RegDef{"REG_SYNC_CONFIG", REG_SYNC_CONFIG},
+        RegDef{"REG_SYNC_VALUE_1", REG_SYNC_VALUE_1},
+        RegDef{"REG_SYNC_VALUE_2", REG_SYNC_VALUE_2},
+        RegDef{"REG_SYNC_VALUE_3", REG_SYNC_VALUE_3},
+        RegDef{"REG_SYNC_VALUE_4", REG_SYNC_VALUE_4},
+        RegDef{"REG_SYNC_VALUE_5", REG_SYNC_VALUE_5},
+        RegDef{"REG_SYNC_VALUE_6", REG_SYNC_VALUE_6},
+        RegDef{"REG_SYNC_VALUE_7", REG_SYNC_VALUE_7},
+        RegDef{"REG_SYNC_VALUE_8", REG_SYNC_VALUE_8},
+        RegDef{"REG_PACKET_CONFIG_1", REG_PACKET_CONFIG_1},
+        RegDef{"REG_PACKET_CONFIG_2", REG_PACKET_CONFIG_2},
+        RegDef{"REG_PACKET_PAYLOAD_LENGTH", REG_PACKET_PAYLOAD_LENGTH},
+        RegDef{"REG_NODE_ADRS", REG_NODE_ADRS},
+        RegDef{"REG_BROADCAST_ADRS", REG_BROADCAST_ADRS},
+        RegDef{"REG_FIFO_THRESH", REG_FIFO_THRESH},
+        RegDef{"REG_SEQ_CONFIG_1", REG_SEQ_CONFIG_1},
+        RegDef{"REG_SEQ_CONFIG_2", REG_SEQ_CONFIG_2},
+        RegDef{"REG_TIMER_RESOL", REG_TIMER_RESOL},
+        RegDef{"REG_TIMER_1_COEF", REG_TIMER_1_COEF},
+        RegDef{"REG_TIMER_2_COEF", REG_TIMER_2_COEF},
+        RegDef{"REG_IMAGE_CAL", REG_IMAGE_CAL},
+        // RegDef { "REG_TEMP", REG_TEMP },
+        RegDef{"REG_LOW_BAT", REG_LOW_BAT},
+        // RegDef { "REG_IRQ_FLAGS_1", REG_IRQ_FLAGS_1 },
+        // RegDef { "REG_IRQ_FLAGS_2", REG_IRQ_FLAGS_2 },
+        RegDef{"REG_DIO_MAPPING_1", REG_DIO_MAPPING_1},
+        RegDef{"REG_DIO_MAPPING_2", REG_DIO_MAPPING_2},
+        RegDef{"REG_VERSION", REG_VERSION}, RegDef{NULL, 0}};
+
+    SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED);
+
+    int i = 0;
+    while (defs[i].name)
+    {
+        auto name = defs[i].name;
+        auto addr = defs[i].addr;
+
+        LOG_DEBUG(logger, "%s: 0x%x\n", name, spi.readRegister(addr));
+
+        i++;
+    }
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/drivers/SX1278/SX1278.h b/src/shared/drivers/SX1278/SX1278.h
new file mode 100644
index 0000000000000000000000000000000000000000..6fb4c355d36f81331e7299e6a291e58c737e4156
--- /dev/null
+++ b/src/shared/drivers/SX1278/SX1278.h
@@ -0,0 +1,247 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/spi/SPIDriver.h>
+#include <kernel/kernel.h>
+#include <radio/Transceiver.h>
+
+#include <cstdint>
+
+#include "SX1278Defs.h"
+
+/*
+# Description
+## Concurrent access
+The device is stateful, in order to send/receive data you need to set in a
+particular state. This is handled by SX1278BusManager::setMode. This causes a
+lot of issues with multithreaded code, and receiving/transmitting concurrently.
+
+So the driver wraps the bus in a BusManager for this reason, in order to perform
+anything you need to lock the bus. The driver then locks the internal mutex and
+sets the device in the correct mode for that operation.
+
+But this causes problems when receiving, as receive can block indefinetly, and
+would prevent any thread from actually sending stuff. The solution is to release
+the lock only when waiting for a packet. This allows other threads to lock it
+and change mode, and unlocking the bus resets its state back to RX so it can
+continue to listen for incoming packets.
+
+The BusManager also multiplexes who is currently waiting for an interrupt. This
+is needed because due to the limited interrupts pins, the device signals
+multiples of them on one pin. And for some random reasons, the designers decided
+to put "packet received" and "packed sent" on same pin (DIO0)...
+
+Again, the solution is to track who is actually using the bus, so when sending a
+packet that triggers the "packet sent" interrupt, the manager knows to dispatch
+it to the currect thread, and not wake up the RX thread.
+*/
+
+namespace Boardcore
+{
+
+/**
+ * @brief SX1278 bus access manager.
+ */
+class SX1278BusManager
+{
+public:
+    using Mode = SX1278Defs::RegOpMode::Mode;
+
+    SX1278BusManager(SPIBusInterface &bus, miosix::GpioPin cs);
+
+    /**
+     * @brief Lock bus for exclusive access.
+     *
+     * @param mode Device mode requested
+     */
+    void lock(Mode mode);
+
+    /**
+     * @brief Release buf for exclusive access
+     */
+    void unlock();
+
+    /**
+     * @brief Get underlying bus.
+     */
+    SPISlave &getBus();
+
+    /**
+     * @brief Handle DIO0 irq.
+     */
+    void handleDioIRQ();
+
+    /**
+     * @brief Wait for generic irq (DOES NOT RELEASE LOCK!).
+     */
+    void waitForIrq(uint16_t mask);
+
+    /**
+     * @brief Wait for RX irq (releases lock safely).
+     */
+    void waitForRxIrq();
+
+private:
+    void enterMode(Mode mode);
+
+    uint16_t getIrqFlags();
+    void setMode(Mode mode);
+
+    miosix::Thread *irq_wait_thread = nullptr;
+    miosix::Thread *rx_wait_thread  = nullptr;
+    miosix::FastMutex mutex;
+
+    SPISlave slave;
+    Mode mode;
+};
+
+/**
+ * @brief Various SX1278 register/enums definitions.
+ */
+class SX1278 : public Transceiver
+{
+public:
+    /**
+     * @brief Channel filter bandwidth.
+     */
+    enum class RxBw
+    {
+        HZ_2600   = (0b10 << 3) | 7,
+        HZ_3100   = (0b01 << 3) | 7,
+        HZ_3900   = (0b00 << 3) | 7,
+        HZ_5200   = (0b10 << 3) | 6,
+        HZ_6300   = (0b01 << 3) | 6,
+        HZ_7800   = (0b00 << 3) | 6,
+        HZ_10400  = (0b10 << 3) | 5,
+        HZ_12500  = (0b01 << 3) | 5,
+        HZ_15600  = (0b00 << 3) | 5,
+        HZ_20800  = (0b10 << 3) | 4,
+        HZ_25000  = (0b01 << 3) | 4,
+        HZ_31300  = (0b00 << 3) | 4,
+        HZ_41700  = (0b10 << 3) | 3,
+        HZ_50000  = (0b01 << 3) | 3,
+        HZ_62500  = (0b00 << 3) | 3,
+        HZ_83300  = (0b10 << 3) | 2,
+        HZ_100000 = (0b01 << 3) | 2,
+        HZ_125000 = (0b00 << 3) | 2,
+        HZ_166700 = (0b10 << 3) | 1,
+        HZ_200000 = (0b01 << 3) | 1,
+        HZ_250000 = (0b00 << 3) | 1,
+    };
+
+    /**
+     * @brief Requested SX1278 configuration.
+     */
+    struct Config
+    {
+        int freq_rf  = 434000000;        //< RF Frequency in Hz.
+        int freq_dev = 50000;            //< Frequency deviation in Hz.
+        int bitrate  = 48000;            //< Bitrate in b/s.
+        RxBw rx_bw   = RxBw::HZ_125000;  //< Rx filter bandwidth.
+        RxBw afc_bw  = RxBw::HZ_125000;  //< Afc filter bandwidth.
+        int ocp =
+            120;  //< Over current protection limit in mA (0 for no limit).
+        int power = 10;  //< Output power in dB (Between +2 and +17, or +20 for
+                         // full power).
+    };
+
+    /**
+     * @brief Error enum.
+     */
+    enum class Error
+    {
+        NONE,         //< No error encountered.
+        BAD_VALUE,    //< A requested value was outside the valid range.
+        BAD_VERSION,  //< Chip didn't report the correct version.
+    };
+
+    /**
+     * @brief Construct a new SX1278
+     *
+     * @param bus SPI bus used.
+     * @param cs Chip select pin.
+     */
+    SX1278(SPIBusInterface &bus, miosix::GpioPin cs);
+
+    /**
+     * @brief Setup the device.
+     */
+    Error init(Config config);
+
+    /**
+     * @brief Wait until a new packet is received.
+     *
+     * @param pkt       Buffer to store the received packet into.
+     * @param pkt_len   Maximum length of the received data.
+     * @return          Size of the data received or -1 if failure
+     */
+    ssize_t receive(uint8_t *pkt, size_t max_len);
+
+    /**
+     * @brief Send a packet.
+     * The function must block until the packet is sent (successfully or not)
+     *
+     * @param pkt       Pointer to the packet (needs to be at least pkt_len
+     * bytes).
+     * @param pkt_len   Lenght of the packet to be sent.
+     * @return          True if the message was sent correctly.
+     */
+    bool send(uint8_t *pkt, size_t len);
+
+    /**
+     * @brief Return device version.
+     */
+    uint8_t getVersion();
+
+    /**
+     * @brief Handle an incoming interrupt.
+     */
+    void handleDioIRQ();
+
+    /**
+     * @brief Dump all registers via TRACE.
+     */
+    void debugDumpRegisters();
+
+public:
+    using Mode = SX1278BusManager::Mode;
+
+    void setBitrate(int bitrate);
+    void setFreqDev(int freq_dev);
+    void setFreqRF(int freq_rf);
+    void setOcp(int ocp);
+    void setSyncWord(uint8_t value[], int size);
+    void setRxBw(RxBw rx_bw);
+    void setAfcBw(RxBw afc_bw);
+    void setPreableLen(int len);
+    void setPa(int power, bool pa_boost);
+
+    void waitForIrq(uint16_t mask);
+
+    SX1278BusManager bus_mgr;
+    PrintLogger logger = Logging::getLogger("sx1278");
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/drivers/SX1278/SX1278Defs.h b/src/shared/drivers/SX1278/SX1278Defs.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd50d09a8c3294659f1c1186f2ad1fd0512937c6
--- /dev/null
+++ b/src/shared/drivers/SX1278/SX1278Defs.h
@@ -0,0 +1,356 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/spi/SPIDriver.h>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Various SX1278 register/enums definitions.
+ */
+namespace SX1278Defs
+{
+
+/**
+ * @brief Main oscillator frequency (Hz)
+ */
+constexpr int FXOSC = 32000000;
+
+/**
+ * @brief Frequency step (Hz) used in some calculations.
+ */
+constexpr int FSTEP = 61;
+
+constexpr int TS_OSC = 250;
+constexpr int TS_FS  = 60;
+
+/**
+ * @brief Get required spi config
+ */
+inline SPIBusConfig spiConfig()
+{
+    SPIBusConfig config = {};
+
+    // FIXME(davide.mor): This depends on the device
+    config.clockDivider = SPI::ClockDivider::DIV_64;
+    config.mode         = SPI::Mode::MODE_1;
+    config.bitOrder     = SPI::BitOrder::MSB_FIRST;
+    // config.cs_setup_time_us = 30;
+    // config.cs_hold_time_us  = 100;
+
+    return config;
+}
+
+namespace RegOpMode
+{
+
+enum LongRangeMode
+{
+    LONG_RANGE_MODE_FSK  = 0 << 7,
+    LONG_RANGE_MODE_LORA = 1 << 7
+};
+
+enum ModulationType
+{
+    MODULATION_TYPE_FSK = 0 << 6,
+    MODULATION_TYPE_OOK = 1 << 6,
+};
+
+constexpr uint8_t LOW_FREQUENCY_MODE_ON = 1 << 3;
+
+enum Mode
+{
+    MODE_SLEEP = 0b000,
+    MODE_STDBY = 0b001,
+    MODE_FSTX  = 0b010,
+    MODE_TX    = 0b011,
+    MODE_FSRX  = 0b100,
+    MODE_RX    = 0b101,
+};
+}  // namespace RegOpMode
+
+namespace RegPaConfig
+{
+constexpr uint8_t PA_SELECT_BOOST = 1 << 7;
+}
+
+namespace RegPaRamp
+{
+enum ModulationShaping
+{
+    MODULATION_SHAPING_NONE            = 0b00 << 5,
+    MODULATION_SHAPING_GAUSSIAN_BT_1_0 = 0b01 << 5,
+    MODULATION_SHAPING_GAUSSIAN_BT_0_5 = 0b10 << 5,
+    MODULATION_SHAPING_GAUSSIAN_BT_0_3 = 0b11 << 5,
+};
+}
+
+namespace RegOcp
+{
+constexpr uint8_t OCP_ON = 1 << 5;
+}
+
+namespace RegRxConfig
+{
+constexpr uint8_t RESTART_RX_ON_COLLISION      = 1 << 7;
+constexpr uint8_t RESTART_RX_WITHOUT_PILL_LOCK = 1 << 6;
+constexpr uint8_t RESTART_RX_WITH_PILL_LOCK    = 1 << 5;
+constexpr uint8_t AFC_AUTO_ON                  = 1 << 4;
+constexpr uint8_t AGC_AUTO_ON                  = 1 << 3;
+
+constexpr uint8_t RX_TRIGGER_RSSI_INTERRUPT  = 0b001;
+constexpr uint8_t RX_TRIGGER_PREAMBLE_DETECT = 0b110;
+}  // namespace RegRxConfig
+
+namespace RegPreambleDetector
+{
+constexpr uint8_t PREAMBLE_DETECTOR_ON = 1 << 7;
+
+enum PreambleDetectorSize
+{
+    PREAMBLE_DETECTOR_SIZE_1_BYTE  = 0b00 << 5,
+    PREAMBLE_DETECTOR_SIZE_2_BYTES = 0b01 << 5,
+    PREAMBLE_DETECTOR_SIZE_3_BYTES = 0b10 << 5,
+};
+}  // namespace RegPreambleDetector
+
+namespace RegSyncConfig
+{
+enum AutoRestartRxMode
+{
+    AUTO_RESTART_RX_MODE_OFF                  = 0b00 << 6,
+    AUTO_RESTART_RX_MODE_ON_WITHOUT_PILL_LOCK = 0b01 << 6,
+    AUTO_RESTART_RX_MODE_ON_WITH_PILL_LOCK    = 0b10 << 6,
+};
+
+enum PreamblePolarity
+{
+    PREAMBLE_POLARITY_AA = 0 << 5,
+    PREAMBLE_POLARITY_55 = 1 << 5,
+};
+
+constexpr uint8_t SYNC_ON = 1 << 4;
+}  // namespace RegSyncConfig
+
+namespace RegPacketConfig1
+{
+enum PacketFormat
+{
+    PACKET_FORMAT_FIXED_LENGTH    = 0 << 7,
+    PACKET_FORMAT_VARIABLE_LENGTH = 1 << 7,
+};
+
+enum DcFree
+{
+    DC_FREE_NONE       = 0b00 << 5,
+    DC_FREE_MANCHESTER = 0b01 << 5,
+    DC_FREE_WHITENING  = 0b10 << 5,
+};
+
+constexpr uint8_t CRC_ON             = 1 << 4;
+constexpr uint8_t CRC_AUTO_CLEAR_OFF = 1 << 3;
+
+enum AddressFiltering
+{
+    ADDRESS_FILTERING_NONE                    = 0b00 << 1,
+    ADDRESS_FILTERING_MATCH_NODE              = 0b01 << 1,
+    ADDRESS_FILTERING_MATCH_NODE_OR_BROADCAST = 0b10 << 1,
+};
+
+enum CrcWhiteningType
+{
+    CRC_WHITENING_TYPE_CCITT_CRC = 0,
+    CRC_WHITENING_TYPE_IBM_CRC   = 1,
+};
+}  // namespace RegPacketConfig1
+
+namespace RegPacketConfig2
+{
+enum DataMode
+{
+    DATA_MODE_CONTINUOS = 0 << 6,
+    DATA_MODE_PACKET    = 1 << 6
+};
+
+constexpr uint8_t IO_HOME_ON = 1 << 5;
+constexpr uint8_t BEACON_ON  = 1 << 4;
+}  // namespace RegPacketConfig2
+
+namespace RegFifoThresh
+{
+enum TxStartCondition
+{
+    TX_START_CONDITION_FIFO_LEVEL     = 0 << 7,
+    TX_START_CONDITION_FIFO_NOT_EMPTY = 1 << 7,
+};
+}
+
+namespace RegIrqFlags
+{
+constexpr uint16_t MODE_READY         = 1 << 7;
+constexpr uint16_t RX_READY           = 1 << 6;
+constexpr uint16_t TX_READY           = 1 << 5;
+constexpr uint16_t PILL_LOCK          = 1 << 4;
+constexpr uint16_t RSSI               = 1 << 3;
+constexpr uint16_t TIMEOUT            = 1 << 2;
+constexpr uint16_t PREAMBLE_DETECT    = 1 << 1;
+constexpr uint16_t SYNC_ADDRESS_MATCH = 1 << 0;
+constexpr uint16_t FIFO_FULL          = 1 << 15;
+constexpr uint16_t FIFO_EMPTY         = 1 << 14;
+constexpr uint16_t FIFO_LEVEL         = 1 << 13;
+constexpr uint16_t FIFO_OVERRUN       = 1 << 12;
+constexpr uint16_t PACKET_SENT        = 1 << 11;
+constexpr uint16_t PAYLOAD_READY      = 1 << 10;
+constexpr uint16_t CRC_OK             = 1 << 9;
+constexpr uint16_t LOW_BAT            = 1 << 8;
+
+}  // namespace RegIrqFlags
+
+namespace RegPaDac
+{
+enum PaDac
+{
+    PA_DAC_DEFAULT_VALUE = 0x04,
+    PA_DAC_PA_BOOST      = 0x07
+};
+}
+
+enum Registers
+{
+    REG_FIFO = 0x00,
+
+    // Registers for common settings
+    REG_OP_MODE     = 0x01,
+    REG_BITRATE_MSB = 0x02,
+    REG_BITRATE_LSB = 0x03,
+    REG_FDEV_MSB    = 0x04,
+    REG_FDEV_LSB    = 0x05,
+    REG_FRF_MSB     = 0x06,
+    REG_FRF_MID     = 0x07,
+    REG_FRF_LSB     = 0x08,
+
+    // Registers for the transmitter
+    REG_PA_CONFIG = 0x09,
+    REG_PA_RAMP   = 0x0a,
+    REG_OCP       = 0x0b,
+
+    // Registers for the receiver
+    REG_LNA            = 0x0c,
+    REG_RX_CONFIG      = 0x0d,
+    REG_RSSI_CONFIG    = 0x0e,
+    REG_RSSI_COLLISION = 0x0f,
+    REG_RSSI_THRESH    = 0x10,
+    REG_RSSI_VALUE     = 0x11,
+    REG_RX_BW          = 0x12,
+    REG_AFC_BW         = 0x13,
+    REG_OOK_PEAK       = 0x14,
+    REG_OOK_FIX        = 0x15,
+    REG_OOK_AVG        = 0x16,
+    // Reserved 17 to 19
+    REG_AFC_FEI         = 0x1a,
+    REG_AFC_MSB         = 0x1b,
+    REG_AFC_LSB         = 0x1c,
+    REG_FEI_MSB         = 0x1d,
+    REG_FEI_LSB         = 0x1e,
+    REG_PREAMBLE_DETECT = 0x1f,
+    REG_RX_TIMEOUT_1    = 0x20,
+    REG_RX_TIMEOUT_2    = 0x21,
+    REG_RX_TIMEOUT_3    = 0x22,
+    REG_RX_DELAY        = 0x23,
+
+    // RC Oscillator registers
+    REG_OSC = 0x24,
+
+    // Packet handling registers
+    REG_PREAMBLE_MSB          = 0x25,
+    REG_PREAMBLE_LSB          = 0x26,
+    REG_SYNC_CONFIG           = 0x27,
+    REG_SYNC_VALUE_1          = 0x28,
+    REG_SYNC_VALUE_2          = 0x29,
+    REG_SYNC_VALUE_3          = 0x2a,
+    REG_SYNC_VALUE_4          = 0x2b,
+    REG_SYNC_VALUE_5          = 0x2c,
+    REG_SYNC_VALUE_6          = 0x2d,
+    REG_SYNC_VALUE_7          = 0x2e,
+    REG_SYNC_VALUE_8          = 0x2f,
+    REG_PACKET_CONFIG_1       = 0x30,
+    REG_PACKET_CONFIG_2       = 0x31,
+    REG_PACKET_PAYLOAD_LENGTH = 0x32,
+    REG_NODE_ADRS             = 0x33,
+    REG_BROADCAST_ADRS        = 0x34,
+    REG_FIFO_THRESH           = 0x35,
+
+    // Sequencer registers
+    REG_SEQ_CONFIG_1 = 0x36,
+    REG_SEQ_CONFIG_2 = 0x37,
+    REG_TIMER_RESOL  = 0x38,
+    REG_TIMER_1_COEF = 0x39,
+    REG_TIMER_2_COEF = 0x3a,
+
+    // Service registers
+    REG_IMAGE_CAL = 0x3b,
+    REG_TEMP      = 0x3c,
+    REG_LOW_BAT   = 0x3d,
+
+    // Status registers
+    REG_IRQ_FLAGS_1 = 0x3e,
+    REG_IRQ_FLAGS_2 = 0x3f,
+
+    // IO Control registers
+    REG_DIO_MAPPING_1 = 0x40,
+    REG_DIO_MAPPING_2 = 0x41,
+
+    // Version register
+    REG_VERSION = 0x42,
+
+    // Additional registers
+    REG_PILL_HOP     = 0x44,
+    REG_TCXO         = 0x4b,
+    REG_PA_DAC       = 0x4d,
+    REG_FORMER_TEMP  = 0x5b,
+    REG_BITRATE_FRAC = 0x5d,
+    REG_AGC_REF      = 0x61,
+    REG_AGC_THRESH_1 = 0x62,
+    REG_AGC_THRESH_2 = 0x63,
+    REG_AGC_THRESH_3 = 0x64,
+
+    // Low frequency additional registers
+    REG_AGC_REF_LF      = 0x61,
+    REG_AGC_THRESH_1_LF = 0x62,
+    REG_AGC_THRESH_2_LF = 0x63,
+    REG_AGC_THRESH_3_LF = 0x61,
+    REG_AGC_PILL_LF     = 0x70,
+
+    // High frequency additional registers
+    REG_AGC_REF_HF      = 0x61,
+    REG_AGC_THRESH_1_HF = 0x62,
+    REG_AGC_THRESH_2_HF = 0x63,
+    REG_AGC_THRESH_3_HF = 0x61,
+    REG_AGC_PILL_HF     = 0x70,
+};
+
+}  // namespace SX1278Defs
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h b/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h
deleted file mode 100644
index ff3e1c83d3499f443de147b79996735208713988..0000000000000000000000000000000000000000
--- a/src/shared/sensors/UbloxGPS/UBXUnpackedFrame.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Damiano Amatruda
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <algorithm>
-
-namespace Boardcore
-{
-
-static constexpr uint16_t UBX_MAX_PAYLOAD_LENGTH = 92;
-static constexpr uint16_t UBX_MAX_FRAME_LENGTH   = UBX_MAX_PAYLOAD_LENGTH + 8;
-static constexpr uint8_t UBX_VALID_PREAMBLE[]    = {0xb5, 0x62};
-
-struct UBXUnpackedFrame
-{
-    uint8_t preamble[2];
-    uint8_t cls;
-    uint8_t id;
-    uint8_t payload[UBX_MAX_PAYLOAD_LENGTH];
-    uint16_t payloadLength;
-    uint8_t checksum[2];
-
-    UBXUnpackedFrame() = default;
-
-    UBXUnpackedFrame(uint8_t cls, uint8_t id, const uint8_t* payload,
-                     uint16_t payloadLength)
-        : cls(cls), id(id), payloadLength(payloadLength)
-    {
-        memcpy(preamble, UBX_VALID_PREAMBLE, 2);
-
-        memcpy(this->payload, payload,
-               std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
-
-        calcChecksum(checksum);
-    }
-
-    inline uint16_t getFrameLength() const { return payloadLength + 8; }
-
-    inline bool isValid() const
-    {
-        if (memcmp(preamble, UBX_VALID_PREAMBLE, 2) != 0)
-        {
-            return false;
-        }
-
-        if (payloadLength > UBX_MAX_PAYLOAD_LENGTH)
-        {
-            return false;
-        }
-
-        uint8_t validChecksum[2];
-        calcChecksum(validChecksum);
-        return memcmp(checksum, validChecksum, 2) == 0;
-    }
-
-    inline void writePacked(uint8_t* frame) const
-    {
-        memcpy(frame, preamble, 2);
-        frame[2] = cls;
-        frame[3] = id;
-        memcpy(&frame[4], &payloadLength, 2);
-        memcpy(&frame[6], payload, payloadLength);
-        memcpy(&frame[6 + payloadLength], checksum, 2);
-    }
-
-    inline void readPacked(const uint8_t* frame)
-    {
-        memcpy(preamble, frame, 2);
-        cls = frame[2];
-        id  = frame[3];
-        memcpy(&payloadLength, &frame[4], 2);
-        memcpy(payload, &frame[6],
-               std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
-        memcpy(checksum, &frame[6 + payloadLength], 2);
-    }
-
-    inline void calcChecksum(uint8_t* checksum) const
-    {
-        uint8_t data[UBX_MAX_FRAME_LENGTH];
-        data[0] = cls;
-        data[1] = id;
-        memcpy(&data[2], &payloadLength, 2);
-        memcpy(&data[4], payload,
-               std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH));
-
-        uint16_t dataLength =
-            std::min(payloadLength, UBX_MAX_PAYLOAD_LENGTH) + 4;
-
-        checksum[0] = 0;
-        checksum[1] = 0;
-
-        for (uint8_t i = 0; i < dataLength; ++i)
-        {
-            checksum[0] += data[i];
-            checksum[1] += checksum[0];
-        }
-    }
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
index 3bc0879ad7138d4a49b3c5860357a14f431eb3ed..dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
@@ -1,5 +1,5 @@
 /* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * Authors: Davide Bonomini, 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
@@ -28,27 +28,39 @@
 #include <fcntl.h>
 #include <filesystem/file_access.h>
 
+using namespace miosix;
+
 namespace Boardcore
 {
 
-using namespace miosix;
+UbloxGPS::UbloxGPS(int baudrate_, uint8_t sampleRate_, int serialPortNum_,
+                   const char* serialPortName_, int defaultBaudrate_)
+    : baudrate(baudrate_), sampleRate(sampleRate_),
+      serialPortNumber(serialPortNum_), serialPortName(serialPortName_),
+      defaultBaudrate(defaultBaudrate_)
+{
+    // Prepare the gps file path with the specified name
+    strcpy(gpsFilePath, "/dev/");
+    strcat(gpsFilePath, serialPortName);
+}
 
 bool UbloxGPS::init()
 {
     // Change the baud rate from the default value
-    if (!setupCommunication())
+    if (!serialCommuinicationSetup())
     {
         return false;
     }
 
+    Thread::sleep(10);
+
     // Reset configuration to default
+    // TODO: maybe move this on serial communication setup
     if (!resetConfiguration())
     {
         return false;
     }
 
-    Thread::sleep(100);
-
     // Disable NMEA messages
     if (!disableNMEAMessages())
     {
@@ -73,8 +85,8 @@ bool UbloxGPS::init()
 
     Thread::sleep(100);
 
-    // Set sample rate
-    if (!setSampleRate())
+    // Set rate
+    if (!setRate())
     {
         return false;
     }
@@ -86,57 +98,200 @@ bool UbloxGPS::selfTest() { return true; }
 
 UbloxGPSData UbloxGPS::sampleImpl()
 {
-    Lock<FastMutex> l(sampleMutex);
-    return lastSample;
+    Lock<FastMutex> l(mutex);
+    return threadSample;
 }
 
 void UbloxGPS::run()
 {
-    UBXUnpackedFrame frame;
+    /**
+     * UBX message structure:
+     * - 2B: Preamble
+     * - 1B: Message class
+     * - 1B: Message id
+     * - 2B: Payload length
+     * - lB: Payload
+     * - 2B: Checksum
+     */
+    uint8_t message[6 + UBX_MAX_PAYLOAD_LENGTH + 2];
+    uint16_t payloadLength;
 
     while (!shouldStop())
     {
         StackLogger::getInstance().updateStack(THID_GPS);
 
         // Try to read the message
-        if (!readUBXFrame(frame))
+        if (!readUBXMessage(message, payloadLength))
         {
             LOG_DEBUG(logger, "Unable to read a UBX message");
             continue;
         }
 
         // Parse the message
-        if (!parseUBXFrame(frame))
+        if (!parseUBXMessage(message))
         {
-            LOG_DEBUG(
-                logger,
-                "UBX message not recognized (class: {:#02x}, id: {:#02x})",
-                frame.cls, frame.id);
+            LOG_DEBUG(logger,
+                      "UBX message not recognized (class:0x{02x}, id: 0x{02x})",
+                      message[2], message[3]);
         }
     }
 }
 
-bool UbloxGPS::resetConfiguration()
+void UbloxGPS::ubxChecksum(uint8_t* msg, int len)
+{
+    uint8_t ck_a = 0, ck_b = 0;
+
+    // The length must be valid, at least 8 bytes (preamble, msg, length,
+    // checksum)
+    if (len <= 8)
+    {
+        return;
+    }
+
+    // Checksum calculation from byte 2 to end of payload
+    for (int i = 2; i < len - 2; i++)
+    {
+        ck_a = ck_a + msg[i];
+        ck_b = ck_b + ck_a;
+    }
+
+    msg[len - 2] = ck_a;
+    msg[len - 1] = ck_b;
+}
+
+bool UbloxGPS::writeUBXMessage(uint8_t* message, int length)
+{
+    // Compute the checksum
+    ubxChecksum(message, length);
+
+    // Write configuration
+    if (write(gpsFile, message, length) < 0)
+    {
+        LOG_ERR(logger,
+                "Failed to write ubx message (class:0x{02x}, id: 0x{02x})",
+                message[2], message[3]);
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::serialCommuinicationSetup()
 {
-    static constexpr uint16_t payloadLength = 4;
+    intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
+
+    // Change the baudrate only if it is different than the default
+    if (baudrate != defaultBaudrate)
+    {
+        // Close the gps file if already opened
+        devFs->remove(serialPortName);
+
+        // Open the serial port device with the default boudrate
+        if (!devFs->addDevice(serialPortName,
+                              intrusive_ref_ptr<Device>(new STM32Serial(
+                                  serialPortNumber, defaultBaudrate))))
+        {
+            LOG_ERR(logger,
+                    "[gps] Faild to open serial port {0} with baudrate {1} as "
+                    "file {2}",
+                    serialPortNumber, defaultBaudrate, serialPortName);
+            return false;
+        }
+
+        // Open the gps file
+        if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+        {
+            LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+            return false;
+        }
+
+        // Change boudrate
+        if (!setBaudrate())
+        {
+            return false;
+        };
+
+        // Close the gps file
+        if (close(gpsFile) < 0)
+        {
+            LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath);
+            return false;
+        }
+
+        // Close the serial port
+        if (!devFs->remove(serialPortName))
+        {
+            LOG_ERR(logger, "Failed to close serial port {} as file {}",
+                    serialPortNumber, serialPortName);
+            return false;
+        }
+    }
+
+    // Reopen the serial port with the configured boudrate
+    if (!devFs->addDevice(serialPortName,
+                          intrusive_ref_ptr<Device>(
+                              new STM32Serial(serialPortNumber, baudrate))))
+    {
+        LOG_ERR(logger,
+                "Faild to open serial port {} with baudrate {} as file {}\n",
+                serialPortNumber, defaultBaudrate, serialPortName);
+        return false;
+    }
+
+    // Reopen the gps file
+    if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0)
+    {
+        LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath);
+        return false;
+    }
+
+    return true;
+}
 
-    uint8_t payload[payloadLength] = {
+bool UbloxGPS::resetConfiguration()
+{
+    uint8_t ubx_cfg_prt[RESET_CONFIG_MSG_LEN] = {
+        0Xb5, 0x62,  // Preamble
+        0x06, 0x04,  // Message UBX-CFG-RST
+        0x04, 0x00,  // Length
         0x00, 0x00,  // navBbrMask (Hot start)
         0x00,        // Hardware reset immediately
-        0x00         // Reserved
+        0x00,        // Reserved
+        0xff, 0xff   // Checksum
     };
 
-    UBXUnpackedFrame frame{0x06, 0x04,  // Message UBX-CFG-RST
-                           payload, payloadLength};
+    return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN);
+}
 
-    return writeUBXFrame(frame);
+bool UbloxGPS::setBaudrate()
+{
+    uint8_t ubx_cfg_prt[SET_BAUDRATE_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x0c, 0x00,              // Length
+        0x00,                    // Version
+        0xff,                    // All layers
+        0x00, 0x00,              // Reserved
+        0x01, 0x00, 0x52, 0x40,  // Configuration item key ID
+        0xff, 0xff, 0xff, 0xff,  // Value
+        0xff, 0xff               // Checksum
+    };
+
+    // Prepare boud rate
+    ubx_cfg_prt[14] = baudrate;
+    ubx_cfg_prt[15] = baudrate >> 8;
+    ubx_cfg_prt[16] = baudrate >> 16;
+    ubx_cfg_prt[17] = baudrate >> 24;
+
+    return writeUBXMessage(ubx_cfg_prt, SET_BAUDRATE_MSG_LEN);
 }
 
 bool UbloxGPS::disableNMEAMessages()
 {
-    static constexpr uint16_t payloadLength = 34;
-
-    uint8_t payload[payloadLength] = {
+    uint8_t ubx_cfg_valset[DISABLE_NMEA_MESSAGES_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x22, 0x00,              // Length
         0x00,                    // Version
         0xff,                    // All layers
         0x00, 0x00,              // Reserved
@@ -151,146 +306,208 @@ bool UbloxGPS::disableNMEAMessages()
         0xac, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_RMC_UART1 key ID
         0x00,                    // CFG-MSGOUT-NMEA_ID_RMC_UART1 value
         0xb1, 0x00, 0x91, 0x20,  // CFG-MSGOUT-NMEA_ID_VTG_UART1 key ID
-        0x00                     // CFG-MSGOUT-NMEA_ID_VTG_UART1 value
+        0x00,                    // CFG-MSGOUT-NMEA_ID_VTG_UART1 value
+        0xff, 0xff               // Checksum
     };
 
-    UBXUnpackedFrame frame{0x06, 0x8a,  // Message UBX-CFG-VALSET
-                           payload, payloadLength};
-
-    return writeUBXFrame(frame);
+    return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN);
 }
 
 bool UbloxGPS::setGNSSConfiguration()
 {
-    static constexpr uint16_t payloadLength = 9;
-
-    uint8_t payload[payloadLength] = {
+    uint8_t ubx_cfg_valset[SET_GNSS_CONF_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x09, 0x00,              // Length
         0x00,                    // Version
         0x07,                    // All layers
         0x00, 0x00,              // Reserved
         0x21, 0x00, 0x11, 0x20,  // CFG-NAVSPG-DYNMODEL key ID
-        0x08                     // CFG-NAVSPG-DYNMODEL value
+        0x08,                    // CFG-NAVSPG-DYNMODEL value
+        0xff, 0xff               // Checksum
     };
 
-    UBXUnpackedFrame frame{0x06, 0x8a,  // Message UBX-CFG-VALSET
-                           payload, payloadLength};
-
-    return writeUBXFrame(frame);
+    return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN);
 }
 
 bool UbloxGPS::enableUBXMessages()
 {
-    static constexpr uint16_t payloadLength = 9;
-
-    uint8_t payload[payloadLength] = {
+    uint8_t ubx_cfg_valset[ENABLE_UBX_MESSAGES_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x09, 0x00,              // Length
         0x00,                    // Version
         0xff,                    // All layers
         0x00, 0x00,              // Reserved
         0x07, 0x00, 0x91, 0x20,  // CFG-MSGOUT-UBX_NAV_PVT_UART1 key ID
-        0x01                     // CFG-MSGOUT-UBX_NAV_PVT_UART1 value
+        0x01,                    // CFG-MSGOUT-UBX_NAV_PVT_UART1 value
+        0xff, 0xff               // Checksum
     };
 
-    UBXUnpackedFrame frame{0x06, 0x8a,  // Message UBX-CFG-VALSET
-                           payload, payloadLength};
-
-    return writeUBXFrame(frame);
+    return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN);
 }
 
-bool UbloxGPS::setSampleRate()
+bool UbloxGPS::setRate()
 {
-    static constexpr uint16_t payloadLength = 10;
+    uint16_t rate = 1000 / sampleRate;
+    LOG_DEBUG(logger, "Rate: {}", rate);
 
-    uint8_t payload[payloadLength] = {
+    uint8_t ubx_cfg_valset[SET_RATE_MSG_LEN] = {
+        0Xb5, 0x62,              // Preamble
+        0x06, 0x8a,              // Message UBX-CFG-VALSET
+        0x0a, 0x00,              // Length
         0x00,                    // Version
         0x07,                    // All layers
         0x00, 0x00,              // Reserved
         0x01, 0x00, 0x21, 0x30,  // CFG-RATE-MEAS key ID
-        0xff, 0xff               // CFG-RATE-MEAS value (placeholder)
+        0xff, 0xff,              // CFG-RATE-MEAS value
+        0xff, 0xff               // Checksum
     };
-    memcpy(&payload[8], &samplerate, 2);
 
-    UBXUnpackedFrame frame{0x06, 0x8a,  // Message UBX-CFG-VALSET
-                           payload, payloadLength};
+    // Prepare rate
+    ubx_cfg_valset[14] = rate;
+    ubx_cfg_valset[15] = rate >> 8;
 
-    return writeUBXFrame(frame);
+    return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN);
 }
 
-bool UbloxGPS::parseUBXFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength)
 {
-    switch (frame.cls)  // Message class
+    // Read preamble
+    do
+    {
+        // Read util the first byte of the preamble
+        do
+        {
+            if (read(gpsFile, &message[0], 1) <= 0)  // No more data available
+            {
+                return false;
+            }
+        } while (message[0] != PREAMBLE[0]);
+
+        // Read the next byte
+        if (read(gpsFile, &message[1], 1) <= 0)  // No more data available
+        {
+            return false;
+        }
+    } while (message[1] != PREAMBLE[1]);  // Continue
+
+    // Read message class and ID
+    if (read(gpsFile, &message[2], 1) <= 0)
+    {
+        return false;
+    }
+    if (read(gpsFile, &message[3], 1) <= 0)
+    {
+        return false;
+    }
+
+    // Read length
+    if (read(gpsFile, &message[4], 2) <= 0)
+    {
+        return false;
+    }
+    payloadLength = message[4] | (message[5] << 8);
+    if (payloadLength > UBX_MAX_PAYLOAD_LENGTH)
+    {
+        return false;
+    }
+
+    // Read paylaod and checksum
+    for (auto i = 0; i < payloadLength + 2; i++)
+    {
+        if (read(gpsFile, &message[6 + i], 1) <= 0)
+        {
+            return false;
+        }
+    }
+
+    // Verify the checksum
+    uint8_t msgChecksum1 = message[6 + payloadLength];
+    uint8_t msgChecksum2 = message[6 + payloadLength + 1];
+    ubxChecksum(message, 6 + payloadLength + 2);
+    if (msgChecksum1 != message[6 + payloadLength] ||
+        msgChecksum2 != message[6 + payloadLength + 1])
+    {
+        LOG_ERR(logger, "Message checksum verification failed");
+        return false;
+    }
+
+    return true;
+}
+
+bool UbloxGPS::parseUBXMessage(uint8_t* message)
+{
+    switch (message[2])  // Message class
     {
         case 0x01:  // UBX-NAV
-            return parseUBXNAVFrame(frame);
+            return parseUBXNAVMessage(message);
         case 0x05:  // UBX-ACK
-            return parseUBXACKFrame(frame);
+            return parseUBXACKMessage(message);
     }
     return false;
 }
 
-bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::parseUBXNAVMessage(uint8_t* message)
 {
-    switch (frame.id)  // Message ID
+    switch (message[3])  // Message id
     {
         case 0x07:  // UBX-NAV-PVT
-            // Lock the lastSample variable
-            Lock<FastMutex> l(sampleMutex);
+            // Lock the threadSample variable
+            Lock<FastMutex> l(mutex);
 
             // Latitude
-            int32_t rawLatitude = frame.payload[28] | frame.payload[29] << 8 |
-                                  frame.payload[30] << 16 |
-                                  frame.payload[31] << 24;
-            lastSample.latitude = (float)rawLatitude / 1e7;
+            int32_t rawLatitude = message[6 + 28] | message[6 + 29] << 8 |
+                                  message[6 + 30] << 16 | message[6 + 31] << 24;
+            threadSample.latitude = (float)rawLatitude / 1e7;
 
             // Longitude
-            int32_t rawLongitude = frame.payload[24] | frame.payload[25] << 8 |
-                                   frame.payload[26] << 16 |
-                                   frame.payload[27] << 24;
-            lastSample.longitude = (float)rawLongitude / 1e7;
+            int32_t rawLongitude = message[6 + 24] | message[6 + 25] << 8 |
+                                   message[6 + 26] << 16 |
+                                   message[6 + 27] << 24;
+            threadSample.longitude = (float)rawLongitude / 1e7;
 
             // Height
-            int32_t rawHeight = frame.payload[32] | frame.payload[33] << 8 |
-                                frame.payload[34] << 16 |
-                                frame.payload[35] << 24;
-            lastSample.height = (float)rawHeight / 1e3;
+            int32_t rawHeight = message[6 + 32] | message[6 + 33] << 8 |
+                                message[6 + 34] << 16 | message[6 + 35] << 24;
+            threadSample.height = (float)rawHeight / 1e3;
 
             // Velocity north
-            int32_t rawVelocityNorth =
-                frame.payload[48] | frame.payload[49] << 8 |
-                frame.payload[50] << 16 | frame.payload[51] << 24;
-            lastSample.velocityNorth = (float)rawVelocityNorth / 1e3;
+            int32_t rawVelocityNorth = message[6 + 48] | message[6 + 49] << 8 |
+                                       message[6 + 50] << 16 |
+                                       message[6 + 51] << 24;
+            threadSample.velocityNorth = (float)rawVelocityNorth / 1e3;
 
             // Velocity east
-            int32_t rawVelocityEast =
-                frame.payload[52] | frame.payload[53] << 8 |
-                frame.payload[54] << 16 | frame.payload[55] << 24;
-            lastSample.velocityEast = (float)rawVelocityEast / 1e3;
+            int32_t rawVelocityEast = message[6 + 52] | message[6 + 53] << 8 |
+                                      message[6 + 54] << 16 |
+                                      message[6 + 55] << 24;
+            threadSample.velocityEast = (float)rawVelocityEast / 1e3;
 
             // Velocity down
-            int32_t rawVelocityDown =
-                frame.payload[56] | frame.payload[57] << 8 |
-                frame.payload[58] << 16 | frame.payload[59] << 24;
-            lastSample.velocityDown = (float)rawVelocityDown / 1e3;
+            int32_t rawVelocityDown = message[6 + 56] | message[6 + 57] << 8 |
+                                      message[6 + 58] << 16 |
+                                      message[6 + 59] << 24;
+            threadSample.velocityDown = (float)rawVelocityDown / 1e3;
 
             // Speed
-            int32_t rawSpeed = frame.payload[60] | frame.payload[61] << 8 |
-                               frame.payload[62] << 16 |
-                               frame.payload[63] << 24;
-            lastSample.speed = (float)rawSpeed / 1e3;
+            int32_t rawSpeed = message[6 + 60] | message[6 + 61] << 8 |
+                               message[6 + 62] << 16 | message[6 + 63] << 24;
+            threadSample.speed = (float)rawSpeed / 1e3;
 
             // Track (heading of motion)
-            int32_t rawTrack = frame.payload[64] | frame.payload[65] << 8 |
-                               frame.payload[66] << 16 |
-                               frame.payload[67] << 24;
-            lastSample.track = (float)rawTrack / 1e5;
+            int32_t rawTrack = message[6 + 64] | message[6 + 65] << 8 |
+                               message[6 + 66] << 16 | message[6 + 67] << 24;
+            threadSample.track = (float)rawTrack / 1e5;
 
             // Number of satellite
-            lastSample.satellites = (uint8_t)frame.payload[23];
+            threadSample.satellites = (uint8_t)message[6 + 23];
 
             // Fix (every type of fix accepted)
-            lastSample.fix = frame.payload[20] != 0;
+            threadSample.fix = message[6 + 20] != 0;
 
             // Timestamp
-            lastSample.gpsTimestamp =
+            threadSample.gpsTimestamp =
                 TimestampTimer::getInstance().getTimestamp();
 
             return true;
@@ -299,209 +516,22 @@ bool UbloxGPS::parseUBXNAVFrame(const UBXUnpackedFrame& frame)
     return false;
 }
 
-bool UbloxGPS::parseUBXACKFrame(const UBXUnpackedFrame& frame)
+bool UbloxGPS::parseUBXACKMessage(uint8_t* message)
 {
-    switch (frame.id)  // Message ID
+    switch (message[3])  // Message id
     {
         case 0x00:  // UBX-ACK-NAC
             LOG_DEBUG(logger,
-                      "Received NAC for message (class: {:#02x}, id: {:#02x})",
-                      frame.cls, frame.id);
+                      "Received NAC for message (class:0x{02x}, id: 0x{02x})",
+                      message[6], message[7]);
             return true;
         case 0x01:  // UBX-ACK-ACK
             LOG_DEBUG(logger,
-                      "Received ACK for message (class: {:#02x}, id: {:#02x})",
-                      frame.cls, frame.id);
+                      "Received ACK for message (class:0x{02x}, id: 0x{02x})",
+                      message[6], message[7]);
             return true;
     }
     return false;
 }
 
-bool UbloxGPS::writeUBXFrame(const UBXUnpackedFrame& frame)
-{
-    if (!frame.isValid())
-    {
-        LOG_ERR(logger, "UBX frame to write is invalid");
-        return false;
-    }
-
-    uint8_t packedFrame[UBX_MAX_FRAME_LENGTH];
-    frame.writePacked(packedFrame);
-
-    writeRaw(packedFrame, frame.getFrameLength());
-
-    return true;
-}
-
-bool UbloxGPS::readUBXFrame(UBXUnpackedFrame& frame)
-{
-    bool synchronized = false;
-    while (!synchronized)
-    {
-        synchronized = true;
-        for (uint16_t i = 0; synchronized && i < 2; ++i)
-        {
-            if (!readRaw(&frame.preamble[i], 1))
-                return false;
-
-            if (frame.preamble[i] != UBX_VALID_PREAMBLE[i])
-                synchronized = false;
-        }
-    }
-
-    if (!readRaw(&frame.cls, 1) || !readRaw(&frame.id, 1) ||
-        !readRaw((uint8_t*)&frame.payloadLength, 2) ||
-        !readRaw(frame.payload, frame.payloadLength) ||
-        !readRaw(frame.checksum, 2))
-        return false;
-
-    if (!frame.isValid())
-    {
-        LOG_ERR(logger, "UBX frame to read is invalid");
-        return false;
-    }
-
-    return true;
-}
-
-UbloxGPSSPI::UbloxGPSSPI(SPIBusInterface& spiBus, GpioPin spiCs,
-                         SPIBusConfig spiConfig, uint8_t samplerate)
-    : UbloxGPS(samplerate), spiSlave(spiBus, spiCs, spiConfig)
-{
-}
-
-SPIBusConfig UbloxGPSSPI::getDefaultSPIConfig()
-{
-    SPIBusConfig spiConfig{};
-    spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
-    spiConfig.mode         = SPI::Mode::MODE_1;
-    return spiConfig;
-}
-
-bool UbloxGPSSPI::writeRaw(uint8_t* data, size_t size)
-{
-    SPITransaction spi{spiSlave};
-    spi.write(data, size);
-    return true;
-}
-
-bool UbloxGPSSPI::readRaw(uint8_t* data, size_t size)
-{
-    SPITransaction spi{spiSlave};
-    spi.read(data, size);
-    return true;
-}
-
-UbloxGPSSerial::UbloxGPSSerial(int serialBaudrate, uint8_t samplerate,
-                               int serialDefaultBaudrate, int serialPortNumber,
-                               const char* serialPortName)
-    : UbloxGPS(samplerate), serialPortNumber(serialPortNumber),
-      serialPortName(serialPortName), serialBaudrate(serialBaudrate),
-      serialDefaultBaudrate(serialDefaultBaudrate)
-{
-    strcpy(serialFilePath, "/dev/");
-    strcat(serialFilePath, serialPortName);
-}
-
-bool UbloxGPSSerial::setupCommunication()
-{
-    intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs();
-
-    // Close the serial file if already opened
-    devFs->remove(serialPortName);
-
-    // Change the baudrate only if it is different than the default
-    if (serialBaudrate != serialDefaultBaudrate)
-    {
-        // Open the serial port device with the default baudrate
-        if (!devFs->addDevice(serialPortName,
-                              intrusive_ref_ptr<Device>(new STM32Serial(
-                                  serialPortNumber, serialDefaultBaudrate))))
-        {
-            LOG_ERR(logger,
-                    "[gps] Faild to open serial port {} with baudrate {} as "
-                    "file {}",
-                    serialPortNumber, serialDefaultBaudrate, serialPortName);
-            return false;
-        }
-
-        // Open the serial file
-        if ((serialFile = open(serialFilePath, O_RDWR)) < 0)
-        {
-            LOG_ERR(logger, "Failed to open serial file {}", serialFilePath);
-            return false;
-        }
-
-        // Change baudrate
-        if (!setBaudrate())
-        {
-            return false;
-        }
-
-        // Close the serial file
-        if (close(serialFile) < 0)
-        {
-            LOG_ERR(logger, "Failed to close serial file {}", serialFilePath);
-            return false;
-        }
-
-        // Close the serial port
-        if (!devFs->remove(serialPortName))
-        {
-            LOG_ERR(logger, "Failed to close serial port {} as file {}",
-                    serialPortNumber, serialPortName);
-            return false;
-        }
-    }
-
-    // Reopen the serial port with the configured baudrate
-    if (!devFs->addDevice(serialPortName,
-                          intrusive_ref_ptr<Device>(new STM32Serial(
-                              serialPortNumber, serialBaudrate))))
-    {
-        LOG_ERR(logger,
-                "Faild to open serial port {} with baudrate {} as file {}\n",
-                serialPortNumber, serialDefaultBaudrate, serialPortName);
-        return false;
-    }
-
-    // Reopen the serial file
-    if ((serialFile = open(serialFilePath, O_RDWR)) < 0)
-    {
-        LOG_ERR(logger, "Failed to open serial file {}", serialFilePath);
-        return false;
-    }
-
-    return true;
-}
-
-bool UbloxGPSSerial::setBaudrate()
-{
-    static constexpr uint16_t payloadLength = 12;
-
-    uint8_t payload[payloadLength] = {
-        0x00,                    // Version
-        0xff,                    // All layers
-        0x00, 0x00,              // Reserved
-        0x01, 0x00, 0x52, 0x40,  // Configuration item key ID
-        0xff, 0xff, 0xff, 0xff   // Value (placeholder)
-    };
-    memcpy(&payload[8], &serialBaudrate, 4);
-
-    UBXUnpackedFrame frame{0x06, 0x8a,  // Message UBX-CFG-VALSET
-                           payload, payloadLength};
-
-    return writeUBXFrame(frame);
-}
-
-bool UbloxGPSSerial::writeRaw(uint8_t* data, size_t size)
-{
-    return write(serialFile, data, size) >= 0;
-}
-
-bool UbloxGPSSerial::readRaw(uint8_t* data, size_t size)
-{
-    return read(serialFile, data, size) >= 0;
-}
-
 }  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h
index 6d980d6420cf64441b46df319d8a904bdd930002..3f7a1616ef1c0c732527b91ed209964d9af3c893 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.h
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h
@@ -1,5 +1,5 @@
 /* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * Authors: Davide Bonomini, 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
@@ -24,57 +24,87 @@
 
 #include <ActiveObject.h>
 #include <diagnostic/PrintLogger.h>
-#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
 #include <sensors/Sensor.h>
 
-#include "UBXUnpackedFrame.h"
 #include "UbloxGPSData.h"
 
 namespace Boardcore
 {
 
 /**
- * @brief Sensor for Ublox GPS.
+ * @brief Driver for Ublox GPSs
  *
- * This sensor handles communication and setup with Ublox GPSs. It uses the
+ * This driver handles communication and setup with Ublox GPSs. It uses the
  * binary UBX protocol to retrieve and parse navigation data quicker than using
  * the string based NMEA.
  *
- * At initialization it configures the device with the specified sample rate,
- * resets the configuration and sets up UBX messages and GNSS parameters.
+ * At initialization it configures the device with the specified baudrate, reset
+ * the configuration and sets up UBX messages and GNSS parameters.
  *
- * Communication with the device is performed through SPI.
+ * Communication with the device is performed through a file, the driver opens
+ * the serial port under the filepath /dev/<serialPortName>.
+ * There is no need for the file to be setted up beforhand.
  *
  * This driver was written for a NEO-M9N gps with the latest version of UBX.
  */
 class UbloxGPS : public Sensor<UbloxGPSData>, public ActiveObject
 {
 public:
-    explicit UbloxGPS(uint8_t samplerate) : samplerate(samplerate) {}
+    /**
+     * @brief Construct a new UbloxGPS object
+     *
+     * @param boudrate_ Baudrate to communicate with the device (max: 921600,
+     * min: 4800 for NEO-M9N)
+     * @param sampleRate_ GPS sample rate (max: 25 for NEO-M9N)
+     * @param serialPortName_ Name of the file for the gps device
+     * @param defaultBaudrate_ Startup baudrate (38400 for NEO-M9N)
+     */
+    UbloxGPS(int boudrate_ = 921600, uint8_t sampleRate_ = 10,
+             int serialPortNumber_ = 2, const char *serialPortName_ = "gps",
+             int defaultBaudrate_ = 38400);
 
     /**
-     * @brief Disables the NMEA messages, configures GNSS options and enables
-     * UBX-PVT message
+     * @brief Sets up the serial port baudrate, disables the NMEA messages,
+     * configures GNSS options and enables UBX-PVT message
      *
      * @return True if the operation succeeded
      */
-    virtual bool init() override;
+    bool init() override;
 
     /**
-     * @brief Reads a single message form the GPS, waits 2 sample cycles.
+     * @brief Read a single message form the GPS, waits 2 sample cycle
      *
      * @return True if a message was sampled
      */
     bool selfTest() override;
 
-protected:
+private:
     UbloxGPSData sampleImpl() override;
 
     void run() override;
 
+    void ubxChecksum(uint8_t *msg, int len);
+
+    /**
+     * @brief Compute the checksum and write the message to the device
+     */
+    bool writeUBXMessage(uint8_t *message, int length);
+
+    /**
+     * @brief Sets up the serial port with the correct baudrate
+     *
+     * Opens the serial port with the defaul baudrate and changes it to
+     * the value specified in the constructor, then it reopens the serial port.
+     * If the device is already using the correct baudrate this won't have
+     * effect. However if the gps is using a different baudrate the diver won't
+     * be able to communicate.
+     */
+    bool serialCommuinicationSetup();
+
     bool resetConfiguration();
 
-    virtual bool setupCommunication() { return true; }
+    bool setBaudrate();
 
     bool disableNMEAMessages();
 
@@ -82,101 +112,40 @@ protected:
 
     bool enableUBXMessages();
 
-    bool setSampleRate();
-
-    bool parseUBXFrame(const UBXUnpackedFrame& frame);
-
-    bool parseUBXNAVFrame(const UBXUnpackedFrame& frame);
-
-    bool parseUBXACKFrame(const UBXUnpackedFrame& frame);
-
-    bool writeUBXFrame(const UBXUnpackedFrame& frame);
-
-    bool readUBXFrame(UBXUnpackedFrame& frame);
-
-    virtual bool writeRaw(uint8_t* data, size_t size) = 0;
-
-    virtual bool readRaw(uint8_t* data, size_t size) = 0;
-
-    const uint8_t samplerate;  // [Hz]
-
-    mutable miosix::FastMutex sampleMutex;
-    UbloxGPSData lastSample{};
-
-    PrintLogger logger = Logging::getLogger("ubloxgps");
-};
+    bool setRate();
 
-class UbloxGPSSPI : public UbloxGPS
-{
-public:
-    /**
-     * @brief Constructor.
-     *
-     * @param bus The Spi bus.
-     * @param cs The CS pin to lower when we need to sample.
-     * @param config The SPI configuration.
-     * @param samplerate Sample rate to communicate with the device
-     */
-    UbloxGPSSPI(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
-                SPIBusConfig spiConfig = getDefaultSPIConfig(),
-                uint8_t samplerate     = 250);
+    bool readUBXMessage(uint8_t *message, uint16_t &payloadLength);
 
-    /**
-     * Constructs the default config for SPI Bus.
-     *
-     * @returns The default SPIBusConfig.
-     */
-    static SPIBusConfig getDefaultSPIConfig();
+    bool parseUBXMessage(uint8_t *message);
 
-private:
-    virtual bool writeRaw(uint8_t* data, size_t size) override;
+    bool parseUBXNAVMessage(uint8_t *message);
 
-    virtual bool readRaw(uint8_t* data, size_t size) override;
+    bool parseUBXACKMessage(uint8_t *message);
 
-    SPISlave spiSlave;
-};
+    const int baudrate;        // [baud]
+    const uint8_t sampleRate;  // [Hz]
+    const int serialPortNumber;
+    const char *serialPortName;
+    const int defaultBaudrate;  // [baud]
 
-class UbloxGPSSerial : public UbloxGPS
-{
-public:
-    /**
-     * @brief Serial constructor.
-     *
-     * @param serialPortNumber Number of the serial port
-     * @param serialPortName Name of the file for the gps device
-     * @param serialBaudrate Baudrate to communicate with the device (max:
-     * 921600, min: 4800 for NEO-M9N)
-     * @param samplerate GPS sample rate (max: 25 for NEO-M9N)
-     * @param serialDefaultBaudrate Startup baudrate (38400 for NEO-M9N)
-     */
-    UbloxGPSSerial(int serialBaudrate = 921600, uint8_t samplerate = 10,
-                   int serialDefaultBaudrate = 38400, int serialPortNumber = 2,
-                   const char* serialPortName = "gps");
+    char gpsFilePath[16];  ///< Allows for a filename of up to 10 characters
+    int gpsFile;
 
-private:
-    /**
-     * @brief Sets up the serial port with the correct baudrate
-     *
-     * Opens the serial port with the default baudrate and changes it to
-     * the value specified in the constructor, then it reopens the serial port.
-     * If the device is already using the correct baudrate this won't have
-     * effect. However if the gps is using a different baudrate the diver won't
-     * be able to communicate.
-     */
-    virtual bool setupCommunication() override;
+    mutable miosix::FastMutex mutex;
+    UbloxGPSData threadSample{};
 
-    bool setBaudrate();
+    static constexpr int SET_BAUDRATE_MSG_LEN          = 20;
+    static constexpr int RESET_CONFIG_MSG_LEN          = 12;
+    static constexpr int DISABLE_NMEA_MESSAGES_MSG_LEN = 42;
+    static constexpr int SET_GNSS_CONF_LEN             = 17;
+    static constexpr int ENABLE_UBX_MESSAGES_MSG_LEN   = 17;
+    static constexpr int SET_RATE_MSG_LEN              = 18;
 
-    virtual bool writeRaw(uint8_t* data, size_t size) override;
+    static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62};
 
-    virtual bool readRaw(uint8_t* data, size_t size) override;
+    static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92;
 
-    const int serialPortNumber;
-    const char* serialPortName;
-    const int serialBaudrate;         // [baud]
-    const int serialDefaultBaudrate;  // [baud]
-    char serialFilePath[16];  // Allows for a filename of up to 10 characters
-    int serialFile = -1;
+    PrintLogger logger = Logging::getLogger("ubloxgps");
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
index f61d4fc00ab8f682c1d4afd18b3cb4d6c1ac1e0a..7f5570000b78b915a956b96e754d784e08f7611f 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPSData.h
+++ b/src/shared/sensors/UbloxGPS/UbloxGPSData.h
@@ -31,8 +31,8 @@ struct UbloxGPSData : public GPSData
 {
     static std::string header()
     {
-        return "gpsTimestamp,latitude,longitude,height,velocityNorth,"
-               "velocityEast,velocityDown,speed,track,satellites,fix\n";
+        return "gps_timestamp,latitude,longitude,height,velocity_north,"
+               "velocity_east,velocity_down,speed,track,num_satellites,fix\n";
     }
 
     void print(std::ostream &os) const
diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ddb67f00a8800f77f5597fa3c9538fb8496a6e5
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp
@@ -0,0 +1,315 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/SX1278/SX1278.h>
+#include <drivers/interrupt/external_interrupts.h>
+
+#include <thread>
+
+#include "test-sx1278-core.h"
+
+using namespace Boardcore;
+using namespace miosix;
+
+/**
+ * 0 -> RX
+ * 1 -> TX
+ *
+ * Connection diagram:
+ * sx1278[0]:nss  -> stm32:pa1
+ * sx1278[0]:dio0 -> stm32:pc15
+ * sx1278[0]:mosi -> stm32:pc12 (SPI3_MOSI)
+ * sx1278[0]:miso -> stm32:pc11 (SPI3_MISO)
+ * sx1278[0]:sck  -> stm32:pc10 (SPI3_SCK)
+
+ * sx1278[1]:nss  -> stm32:pa2
+ * sx1278[1]:dio0 -> stm32:pc0
+ * sx1278[1]:mosi -> stm32:pb15 (SPI2_MOSI)
+ * sx1278[1]:miso -> stm32:pb14 (SPI2_MISO)
+ * sx1278[1]:sck  -> stm32:pb13 (SPI2_SCK)
+ */
+
+SPIBus bus0(SPI3);
+SPIBus bus1(SPI2);
+
+GpioPin sck0(GPIOC_BASE, 10);
+GpioPin miso0(GPIOC_BASE, 11);
+GpioPin mosi0(GPIOC_BASE, 12);
+GpioPin cs0(GPIOA_BASE, 1);
+GpioPin dio0(GPIOC_BASE, 15);
+
+GpioPin sck1(GPIOB_BASE, 13);
+GpioPin miso1(GPIOB_BASE, 14);
+GpioPin mosi1(GPIOB_BASE, 15);
+GpioPin cs1(GPIOA_BASE, 2);
+GpioPin dio1(GPIOC_BASE, 0);
+
+struct Stats;
+
+const char *stringFromErr(SX1278::Error err);
+const char *stringFromRxBw(SX1278::RxBw rx_bw);
+
+void printStats(Stats stats);
+
+/// Status informations.
+struct Stats
+{
+    int last_recv_packet  = 0;  //< Last received packet ID.
+    int corrupted_packets = 0;  //< Packets that got mangled during tx.
+    int send_count        = 0;  //< Actual number of packets sent.
+    int recv_count        = 0;  //< Actual number of packets received.
+    int recv_errors       = 0;  //< Number of failed recvs.
+
+    float packet_loss() const
+    {
+        if (last_recv_packet != 0)
+        {
+            return 1.0f - ((float)recv_count / (float)last_recv_packet);
+        }
+        else
+        {
+            return 0.0f;
+        }
+    }
+
+} stats;
+
+/// Interval between transmissions.
+const int TX_INTERVAL = 1;
+
+SX1278 *sx1278[2] = {nullptr, nullptr};
+
+struct Msg
+{
+    int idx;
+    int dummy_1;
+    int dummy_2;
+    int dummy_3;
+
+    const static int DUMMY_1 = 0xdeadbeef;
+    const static int DUMMY_2 = 0xbeefdead;
+    const static int DUMMY_3 = 0x1234abcd;
+};
+
+void recvLoop(int idx)
+{
+    while (1)
+    {
+        Msg msg;
+        msg.idx     = 0;
+        msg.dummy_1 = 0;
+        msg.dummy_2 = 0;
+        msg.dummy_3 = 0;
+
+        int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg));
+        if (len != sizeof(msg))
+        {
+            stats.recv_errors++;
+        }
+        else if (msg.dummy_1 != Msg::DUMMY_1 || msg.dummy_2 != Msg::DUMMY_2 ||
+                 msg.dummy_3 != Msg::DUMMY_3)
+        {
+            stats.recv_errors++;
+            stats.corrupted_packets++;
+        }
+        else
+        {
+            stats.last_recv_packet = msg.idx;
+            stats.recv_count++;
+        }
+    }
+}
+
+void sendLoop(int idx)
+{
+    while (1)
+    {
+        miosix::Thread::sleep(TX_INTERVAL);
+
+        int next_idx = stats.send_count + 1;
+
+        Msg msg;
+        msg.idx     = next_idx;
+        msg.dummy_1 = Msg::DUMMY_1;
+        msg.dummy_2 = Msg::DUMMY_2;
+        msg.dummy_3 = Msg::DUMMY_3;
+
+        sx1278[idx]->send((uint8_t *)&msg, sizeof(msg));
+        stats.send_count = next_idx;
+    }
+}
+
+/// Get current time
+long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; }
+
+void __attribute__((used)) EXTI15_IRQHandlerImpl()
+{
+    if (sx1278[0])
+        sx1278[0]->handleDioIRQ();
+}
+
+void __attribute__((used)) EXTI0_IRQHandlerImpl()
+{
+    if (sx1278[1])
+        sx1278[1]->handleDioIRQ();
+}
+
+/// Initialize stm32f407g board (sx1278[0] only)
+void initBoard0()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI3
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC_SYNC();
+
+        // Setup SPI pins
+        sck0.mode(miosix::Mode::ALTERNATE);
+        sck0.alternateFunction(6);
+        miso0.mode(miosix::Mode::ALTERNATE);
+        miso0.alternateFunction(6);
+        mosi0.mode(miosix::Mode::ALTERNATE);
+        mosi0.alternateFunction(6);
+
+        cs0.mode(miosix::Mode::OUTPUT);
+        dio0.mode(miosix::Mode::INPUT);
+    }
+
+    cs0.high();
+    enableExternalInterrupt(dio0.getPort(), dio0.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+/// Initialize stm32f407g board (sx1278[1] only)
+void initBoard1()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI2
+        RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
+        RCC_SYNC();
+
+        sck1.mode(miosix::Mode::ALTERNATE);
+        sck1.alternateFunction(5);
+        miso1.mode(miosix::Mode::ALTERNATE);
+        miso1.alternateFunction(5);
+        mosi1.mode(miosix::Mode::ALTERNATE);
+        mosi1.alternateFunction(5);
+
+        cs1.mode(miosix::Mode::OUTPUT);
+        dio1.mode(miosix::Mode::INPUT);
+    }
+
+    cs1.high();
+    enableExternalInterrupt(dio1.getPort(), dio1.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+int main()
+{
+#ifndef DISABLE_RX
+    initBoard0();
+#endif
+#ifndef DISABLE_TX
+    initBoard1();
+#endif
+
+    // Run default configuration
+    SX1278::Config config;
+    SX1278::Error err;
+
+    // Configure them
+#ifndef DISABLE_RX
+    sx1278[0] = new SX1278(bus0, cs0);
+
+    printf("\n[sx1278] Configuring sx1278[0]...\n");
+    printConfig(config);
+    if ((err = sx1278[0]->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278[0]->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+#endif
+
+#ifndef DISABLE_TX
+    sx1278[1] = new SX1278(bus1, cs1);
+
+    printf("\n[sx1278] Configuring sx1278[1]...\n");
+    printConfig(config);
+    if ((err = sx1278[1]->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278[1]->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+#endif
+
+    // Run background threads
+#ifndef DISABLE_RX
+    std::thread recv([]() { recvLoop(0); });
+#endif
+#ifndef DISABLE_TX
+    std::thread send([]() { sendLoop(1); });
+#endif
+
+    // Finish!
+    long long start = now();
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+    miosix::Thread::sleep(4000);
+
+    while (1)
+    {
+        long long elapsed = now() - start;
+
+        // Calculate bitrates
+        float tx_bitrate = (float)(stats.send_count * sizeof(Msg) * 8) /
+                           ((float)elapsed / 1000.0f);
+
+        float rx_bitrate = (float)(stats.recv_count * sizeof(Msg) * 8) /
+                           ((float)elapsed / 1000.0f);
+
+        printf("\n[sx1278] Stats:\n");
+        printStats(stats);
+        printf("tx_bitrate: %.2f kb/s\n", tx_bitrate / 1000.0f);
+        printf("rx_bitrate: %.2f kb/s\n", rx_bitrate / 1000.0f);
+
+        miosix::Thread::sleep(2000);
+    }
+
+    return 0;
+}
+
+void printStats(Stats stats)
+{
+    // Prints are REALLY slow, so take a COPY of stats, so we can print an
+    // instant in time.
+
+    printf("stats.last_recv_packet = %d\n", stats.last_recv_packet);
+    printf("stats.corrupted_packets = %d\n", stats.corrupted_packets);
+    printf("stats.send_count = %d\n", stats.send_count);
+    printf("stats.recv_count = %d\n", stats.recv_count);
+    printf("stats.recv_errors = %d\n", stats.recv_errors);
+    printf("stats.packet_loss = %.2f %%\n", stats.packet_loss() * 100.0f);
+}
diff --git a/src/tests/drivers/sx1278/test-sx1278-bidir.cpp b/src/tests/drivers/sx1278/test-sx1278-bidir.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ecf7b42664660dd6331bf8c707f130135216e3b
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-bidir.cpp
@@ -0,0 +1,201 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/SX1278/SX1278.h>
+#include <drivers/interrupt/external_interrupts.h>
+
+#include <cstring>
+#include <thread>
+
+#include "test-sx1278-core.h"
+
+using namespace Boardcore;
+using namespace miosix;
+
+/**
+ * Connection diagram:
+ * sx1278[0]:nss  -> stm32:pa1
+ * sx1278[0]:dio0 -> stm32:pc15
+ * sx1278[0]:mosi -> stm32:pc12 (SPI3_MOSI)
+ * sx1278[0]:miso -> stm32:pc11 (SPI3_MISO)
+ * sx1278[0]:sck  -> stm32:pc10 (SPI3_SCK)
+
+ * sx1278[1]:nss  -> stm32:pa2
+ * sx1278[1]:dio0 -> stm32:pc0
+ * sx1278[1]:mosi -> stm32:pb15 (SPI2_MOSI)
+ * sx1278[1]:miso -> stm32:pb14 (SPI2_MISO)
+ * sx1278[1]:sck  -> stm32:pb13 (SPI2_SCK)
+ */
+
+SPIBus bus0(SPI3);
+SPIBus bus1(SPI2);
+
+GpioPin sck0(GPIOC_BASE, 10);
+GpioPin miso0(GPIOC_BASE, 11);
+GpioPin mosi0(GPIOC_BASE, 12);
+GpioPin cs0(GPIOA_BASE, 1);
+GpioPin dio0(GPIOC_BASE, 15);
+
+GpioPin sck1(GPIOB_BASE, 13);
+GpioPin miso1(GPIOB_BASE, 14);
+GpioPin mosi1(GPIOB_BASE, 15);
+GpioPin cs1(GPIOA_BASE, 2);
+GpioPin dio1(GPIOC_BASE, 0);
+
+SX1278 *sx1278[2] = {nullptr, nullptr};
+
+void __attribute__((used)) EXTI15_IRQHandlerImpl()
+{
+    if (sx1278[0])
+        sx1278[0]->handleDioIRQ();
+}
+
+void __attribute__((used)) EXTI0_IRQHandlerImpl()
+{
+    if (sx1278[1])
+        sx1278[1]->handleDioIRQ();
+}
+
+/// Initialize stm32f407g board (sx1278[0] only)
+void initBoard0()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI3
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC_SYNC();
+
+        // Setup SPI pins
+        sck0.mode(miosix::Mode::ALTERNATE);
+        sck0.alternateFunction(6);
+        miso0.mode(miosix::Mode::ALTERNATE);
+        miso0.alternateFunction(6);
+        mosi0.mode(miosix::Mode::ALTERNATE);
+        mosi0.alternateFunction(6);
+
+        cs0.mode(miosix::Mode::OUTPUT);
+        dio0.mode(miosix::Mode::INPUT);
+    }
+
+    cs0.high();
+    enableExternalInterrupt(dio0.getPort(), dio0.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+/// Initialize stm32f407g board (sx1278[1] only)
+void initBoard1()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI2
+        RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
+        RCC_SYNC();
+
+        sck1.mode(miosix::Mode::ALTERNATE);
+        sck1.alternateFunction(5);
+        miso1.mode(miosix::Mode::ALTERNATE);
+        miso1.alternateFunction(5);
+        mosi1.mode(miosix::Mode::ALTERNATE);
+        mosi1.alternateFunction(5);
+
+        cs1.mode(miosix::Mode::OUTPUT);
+        dio1.mode(miosix::Mode::INPUT);
+    }
+
+    cs1.high();
+    enableExternalInterrupt(dio1.getPort(), dio1.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+void recvLoop(int idx)
+{
+    char buf[256];
+    while (1)
+    {
+        if (sx1278[idx]->receive((uint8_t *)buf, sizeof(buf)) != -1)
+        {
+            // Make sure there is a terminator somewhere
+            buf[255] = 0;
+            printf("[sx1278 @ %p] Received '%s'\n", sx1278[idx], buf);
+        }
+    }
+}
+
+void sendLoop(int idx, int interval, char *data)
+{
+    while (1)
+    {
+        miosix::Thread::sleep(interval);
+
+        sx1278[idx]->send((uint8_t *)data, strlen(data) + 1);
+        printf("[sx1278 @ %p] Sent '%s'\n", sx1278[idx], data);
+    }
+}
+
+int main()
+{
+    initBoard0();
+    initBoard1();
+
+    // Run default configuration
+    SX1278::Config config;
+    SX1278::Error err;
+
+    // Configure them
+    sx1278[0] = new SX1278(bus0, cs0);
+
+    printf("\n[sx1278] Configuring sx1278[0]...\n");
+    printConfig(config);
+    if ((err = sx1278[0]->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278[0]->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+
+    sx1278[1] = new SX1278(bus1, cs1);
+
+    printf("\n[sx1278] Configuring sx1278[1]...\n");
+    printConfig(config);
+    if ((err = sx1278[1]->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278[1]->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+
+    // Spawn all threads
+    std::thread send0(
+        [=]() { sendLoop(0, 1000, const_cast<char *>("Hi from sx1278[0]!")); });
+    std::thread send1(
+        [=]() { sendLoop(1, 3333, const_cast<char *>("Hi from sx1278[1]!")); });
+
+    std::thread recv0([]() { recvLoop(0); });
+    std::thread recv1([]() { recvLoop(1); });
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+    while (1)
+        miosix::Thread::wait();
+
+    return 0;
+}
diff --git a/src/tests/drivers/sx1278/test-sx1278-core.cpp b/src/tests/drivers/sx1278/test-sx1278-core.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86d4bc25445433ae4bfdbad5818197f60ca029e1
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-core.cpp
@@ -0,0 +1,123 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "test-sx1278-core.h"
+
+using namespace Boardcore;
+
+const char *stringFromErr(SX1278::Error err)
+{
+    switch (err)
+    {
+        case SX1278::Error::BAD_VALUE:
+            return "Error::BAD_VALUE";
+
+        case SX1278::Error::BAD_VERSION:
+            return "Error::BAD_VERSION";
+
+        default:
+            return "<unknown>";
+    }
+}
+
+const char *stringFromRxBw(SX1278::RxBw rx_bw)
+{
+    switch (rx_bw)
+    {
+        case SX1278::RxBw::HZ_2600:
+            return "RxBw::HZ_2600";
+
+        case SX1278::RxBw::HZ_3100:
+            return "RxBw::HZ_3100";
+
+        case SX1278::RxBw::HZ_3900:
+            return "RxBw::HZ_3900";
+
+        case SX1278::RxBw::HZ_5200:
+            return "RxBw::HZ_5200";
+
+        case SX1278::RxBw::HZ_6300:
+            return "RxBw::HZ_6300";
+
+        case SX1278::RxBw::HZ_7800:
+            return "RxBw::HZ_7800";
+
+        case SX1278::RxBw::HZ_10400:
+            return "RxBw::HZ_10400";
+
+        case SX1278::RxBw::HZ_12500:
+            return "RxBw::HZ_12500";
+
+        case SX1278::RxBw::HZ_15600:
+            return "RxBw::HZ_15600";
+
+        case SX1278::RxBw::HZ_20800:
+            return "RxBw::HZ_20800";
+
+        case SX1278::RxBw::HZ_25000:
+            return "RxBw::HZ_25000";
+
+        case SX1278::RxBw::HZ_31300:
+            return "RxBw::HZ_31300";
+
+        case SX1278::RxBw::HZ_41700:
+            return "RxBw::HZ_41700";
+
+        case SX1278::RxBw::HZ_50000:
+            return "RxBw::HZ_50000";
+
+        case SX1278::RxBw::HZ_62500:
+            return "RxBw::HZ_62500";
+
+        case SX1278::RxBw::HZ_83300:
+            return "RxBw::HZ_83300";
+
+        case SX1278::RxBw::HZ_100000:
+            return "RxBw::HZ_100000";
+
+        case SX1278::RxBw::HZ_125000:
+            return "RxBw::HZ_125000";
+
+        case SX1278::RxBw::HZ_166700:
+            return "RxBw::HZ_166700";
+
+        case SX1278::RxBw::HZ_200000:
+            return "RxBw::HZ_200000";
+
+        case SX1278::RxBw::HZ_250000:
+            return "RxBw::HZ_250000";
+
+        default:
+            return "<unknown>";
+    }
+}
+
+void printConfig(SX1278::Config config)
+{
+    printf("config.freq_rf = %d\n", config.freq_rf);
+    printf("config.freq_dev = %d\n", config.freq_dev);
+    printf("config.bitrate = %d\n", config.bitrate);
+    printf("config.rx_bw = %s\n", stringFromRxBw(config.rx_bw));
+    printf("config.afc_bw = %s\n", stringFromRxBw(config.afc_bw));
+    printf("config.ocp = %d\n", config.ocp);
+    printf("config.power = %d\n", config.power);
+}
diff --git a/src/tests/drivers/sx1278/test-sx1278-core.h b/src/tests/drivers/sx1278/test-sx1278-core.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a5d224a4f887d899389c0dddbfed8788e6c33d5
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-core.h
@@ -0,0 +1,30 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <drivers/SX1278/SX1278.h>
+
+const char *stringFromErr(Boardcore::SX1278::Error err);
+const char *stringFromRxBw(Boardcore::SX1278::RxBw rx_bw);
+
+void printConfig(Boardcore::SX1278::Config config);
diff --git a/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp b/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7b37f60199d64283bee1a24a6b0267ac28de3bb
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-mavlink.cpp
@@ -0,0 +1,162 @@
+/* Copyright (c) 2018 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron, Nuno Barcellos, Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/SX1278/SX1278.h>
+#include <drivers/interrupt/external_interrupts.h>
+
+#include "test-sx1278-core.h"
+
+// Ignore warnings, as we don't want to change third party generated files to
+// fix them
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-align"
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#include <mavlink_lib/lynx/mavlink.h>
+#pragma GCC diagnostic pop
+
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+
+using namespace Boardcore;
+using namespace miosix;
+
+static const unsigned int queue_len          = 10;
+static const unsigned int packet_size        = 256;
+static const unsigned int silence_after_send = 250;
+static const unsigned int max_pkt_age        = 1000;
+static const unsigned int ping_period        = 1000;
+
+// Mavlink out buffer with 10 packets, 256 bytes each.
+using Mav = MavlinkDriver<packet_size, queue_len>;
+
+Mav* channel;
+
+SPIBus bus(SPI3);
+
+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);
+
+SX1278* sx1278 = nullptr;
+
+void __attribute__((used)) EXTI15_IRQHandlerImpl()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ();
+}
+
+/// Initialize stm32f407g board.
+void initBoard()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI3
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC_SYNC();
+
+        // Setup SPI pins
+        sck.mode(miosix::Mode::ALTERNATE);
+        sck.alternateFunction(6);
+        miso.mode(miosix::Mode::ALTERNATE);
+        miso.alternateFunction(6);
+        mosi.mode(miosix::Mode::ALTERNATE);
+        mosi.alternateFunction(6);
+
+        cs.mode(miosix::Mode::OUTPUT);
+        dio.mode(miosix::Mode::INPUT);
+    }
+
+    cs.high();
+    enableExternalInterrupt(dio.getPort(), dio.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+/**
+ * @brief Receive function: print the received message id and send an ACK.
+ */
+static void onReceive(Mav* channel, const mavlink_message_t& msg)
+{
+    printf("[TmtcTest] Received message, id: %d! Sending ack\n", msg.msgid);
+
+    // Prepare ack messages
+    mavlink_message_t ackMsg;
+    mavlink_msg_ack_tm_pack(1, 1, &ackMsg, msg.msgid, msg.seq);
+
+    // Send the ack back to the sender
+    bool ackSent = channel->enqueueMsg(ackMsg);
+
+    if (!ackSent)
+        printf("[Receiver] Could not enqueue ack\n");
+}
+
+/**
+ * @brief This test enqueues a ping message every second and replies to every
+ * received message with an ACK.
+ */
+int main()
+{
+    initBoard();
+
+    SX1278::Config config;
+    SX1278::Error err;
+
+    sx1278 = new SX1278(bus, cs);
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    printConfig(config);
+    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+
+    channel = new Mav(sx1278, &onReceive, silence_after_send, max_pkt_age);
+
+    channel->start();
+
+    // Send function: enqueue a ping every second
+    while (1)
+    {
+        printf("[TmtcTest] Enqueueing ping\n");
+
+        // Create a Mavlink message
+        mavlink_message_t pingMsg;
+        mavlink_msg_ping_tc_pack(1, 1, &pingMsg, miosix::getTick());
+
+        // Send the message
+        bool ackSent = channel->enqueueMsg(pingMsg);
+
+        if (!ackSent)
+            printf("[TmtcTest] Could not enqueue ping\n");
+
+        // LED blink
+        ledOn();
+        miosix::delayMs(200);
+        ledOff();
+
+        miosix::Thread::sleep(ping_period);
+    }
+
+    return 0;
+}
diff --git a/src/tests/drivers/sx1278/test-sx1278-rx.cpp b/src/tests/drivers/sx1278/test-sx1278-rx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a32b475743e974bc6d64689c58e61692539020eb
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-rx.cpp
@@ -0,0 +1,24 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#define DISABLE_TX
+#include "test-sx1278-bench.cpp"
diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..062eb72080c3854a538504825574b32562a88986
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp
@@ -0,0 +1,134 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/SX1278/SX1278.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <filesystem/console/console_device.h>
+
+#include <thread>
+
+#include "test-sx1278-core.h"
+
+using namespace Boardcore;
+using namespace miosix;
+
+SPIBus bus(SPI3);
+
+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);
+
+SX1278* sx1278 = nullptr;
+
+void __attribute__((used)) EXTI15_IRQHandlerImpl()
+{
+    if (sx1278)
+        sx1278->handleDioIRQ();
+}
+
+/// Initialize stm32f407g board.
+void initBoard()
+{
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Enable SPI3
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        RCC_SYNC();
+
+        // Setup SPI pins
+        sck.mode(miosix::Mode::ALTERNATE);
+        sck.alternateFunction(6);
+        miso.mode(miosix::Mode::ALTERNATE);
+        miso.alternateFunction(6);
+        mosi.mode(miosix::Mode::ALTERNATE);
+        mosi.alternateFunction(6);
+
+        cs.mode(miosix::Mode::OUTPUT);
+        dio.mode(miosix::Mode::INPUT);
+    }
+
+    cs.high();
+    enableExternalInterrupt(dio.getPort(), dio.getNumber(),
+                            InterruptTrigger::RISING_EDGE);
+}
+
+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?
+        }
+    }
+}
+
+void sendLoop()
+{
+    uint8_t msg[256];
+    auto console = miosix::DefaultConsole::instance().get();
+
+    while (1)
+    {
+        int len = console->readBlock(msg, sizeof(msg), 0);
+        if (len > 0)
+        {
+            sx1278->send(msg, len);
+        }
+    }
+}
+
+int main()
+{
+    initBoard();
+
+    SX1278::Config config;
+    SX1278::Error err;
+
+    sx1278 = new SX1278(bus, cs);
+
+    printf("\n[sx1278] Configuring sx1278...\n");
+    printConfig(config);
+    if ((err = sx1278->init(config)) != SX1278::Error::NONE)
+    {
+        printf("[sx1278] sx1278->init error: %s\n", stringFromErr(err));
+        return -1;
+    }
+
+    std::thread recv([]() { recvLoop(); });
+    std::thread send([]() { sendLoop(); });
+
+    printf("\n[sx1278] Initialization complete!\n");
+
+    while (1)
+        miosix::Thread::wait();
+
+    return 0;
+}
diff --git a/src/tests/drivers/sx1278/test-sx1278-tx.cpp b/src/tests/drivers/sx1278/test-sx1278-tx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b332d3accf06a9d21f0c54d30e7ef66612f20db3
--- /dev/null
+++ b/src/tests/drivers/sx1278/test-sx1278-tx.cpp
@@ -0,0 +1,24 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#define DISABLE_RX
+#include "test-sx1278-bench.cpp"
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp
index 1bf036893f5cd3463252d1b6d4a06a3da582b75f..0c611daf4e7ffc2ccabcc56da70d989496e1d45f 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -1,5 +1,5 @@
 /* Copyright (c) 2021 Skyward Experimental Rocketry
- * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio, Damiano Amatruda
+ * Authors: Davide Bonomini, 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
@@ -21,87 +21,69 @@
  */
 
 #include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
 #include <sensors/UbloxGPS/UbloxGPS.h>
 #include <utils/Debug.h>
 
-using namespace miosix;
+#include <cstdio>
+
 using namespace Boardcore;
+using namespace miosix;
+
+#define RATE 4
 
 int main()
 {
-    static constexpr uint8_t SAMPLE_RATE = 4;
-
-    PrintLogger logger = Logging::getLogger("test-ubloxgps");
-
-#if defined(USE_SPI)
-    SPIBus bus(SPI1);
-    GpioPin spiSck(GPIOA_BASE, 5);
-    GpioPin spiMiso(GPIOA_BASE, 6);
-    GpioPin spiMosi(GPIOA_BASE, 7);
-    GpioPin cs(GPIOA_BASE, 3);
-
-    spiSck.mode(Mode::ALTERNATE);
-    spiSck.alternateFunction(5);
-    spiMiso.mode(Mode::ALTERNATE);
-    spiMiso.alternateFunction(5);
-    spiMosi.mode(miosix::Mode::ALTERNATE);
-    spiMosi.alternateFunction(5);
-    cs.mode(Mode::OUTPUT);
-    cs.high();
-
-    UbloxGPSSPI sensor{bus, cs, UbloxGPSSPI::getDefaultSPIConfig(),
-                       SAMPLE_RATE};
-#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
-    // Keep GPS baud SAMPLE_RATE at default for easier testing
-    UbloxGPSSerial sensor{2, "gps", 921600, 38400, SAMPLE_RATE};
-#else
-    GpioPin tx(GPIOB_BASE, 6);
-    GpioPin rx(GPIOB_BASE, 7);
+    (void)TimestampTimer::getInstance();
 
-    tx.mode(miosix::Mode::ALTERNATE);
-    rx.mode(miosix::Mode::ALTERNATE);
+    printf("Welcome to the ublox test\n");
 
-    tx.alternateFunction(7);
-    rx.alternateFunction(7);
+    // Keep GPS baud rate at default for easier testing
+    UbloxGPS gps(921600, RATE, 2, "gps", 38400);
+    UbloxGPSData dataGPS;
+    printf("Gps allocated\n");
 
-    UbloxGPSSerial sensor{91600, SAMPLE_RATE, 38400, 1, "gps"};
-#endif
-
-    LOG_INFO(logger, "Initializing sensor...\n");
-
-    if (!sensor.init())
+    // Init the gps
+    if (gps.init())
     {
-        LOG_ERR(logger, "Initialization failed!\n");
-        return -1;
+        printf("Successful gps initialization\n");
+    }
+    else
+    {
+        printf("Failed gps initialization\n");
     }
 
-    LOG_INFO(logger, "Performing self-test...\n");
-
-    if (!sensor.selfTest())
+    // Perform the selftest
+    if (gps.selfTest())
+    {
+        printf("Successful gps selftest\n");
+    }
+    else
     {
-        LOG_ERR(logger, "Self-test failed! (code: %d)\n",
-                sensor.getLastError());
-        return -1;
+        printf("Failed gps selftest\n");
     }
 
-    // Start the sensor thread
-    LOG_INFO(logger, "Starting sensor...\n");
-    sensor.start();
+    // Start the gps thread
+    gps.start();
+    printf("Gps started\n");
 
     while (true)
     {
-        long long lastTick = miosix::getTick();
+        // Give time to the thread
+        Thread::sleep(1000 / RATE);
 
-        sensor.sample();
-        GPSData sample __attribute__((unused)) = sensor.getLastSample();
+        // Sample
+        gps.sample();
+        dataGPS = gps.getLastSample();
 
+        // Print out the latest sample
         TRACE(
-            "timestamp: %4.3f, fix: %01d, lat: %f, lon: %f, height: %4.1f, "
-            "nsat: %2d, speed: %3.2f, velN: %3.2f, velE: %3.2f, track %3.1f\n",
-            (float)sample.gpsTimestamp / 1000000, sample.fix, sample.latitude,
-            sample.longitude, sample.height, sample.satellites, sample.speed,
-            sample.velocityNorth, sample.velocityEast, sample.track);
-
-        Thread::sleepUntil(lastTick + 1000 / SAMPLE_RATE);  // Sample period
+            "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
+            "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
+            "track %3.1f\n",
+            (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix,
+            dataGPS.latitude, dataGPS.longitude, dataGPS.height,
+            dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth,
+            dataGPS.velocityEast, dataGPS.track);
     }
 }