diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 159291f0bb2f9f19c0cdbbe7e01c5b695b14fac5..b600d2af98677534acbe0414a25b1022a5d7f720 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -8,7 +8,7 @@
             "defines": [
                 "DEBUG",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_stm32f429zi_stm32f4discovery",
+                "_BOARD_STM32F429ZI_STM32F4DISCOVERY",
                 "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed560e57e4b1e490afe4781e79a23b95721d933f..2ff5af146481747dfdf70125736764aba2ac10d2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -272,6 +272,9 @@ sbs_target(test-calibration stm32f407vg_stm32f4discovery)
 add_executable(test-bme280 src/tests/sensors/test-bme280.cpp)
 sbs_target(test-bme280 stm32f429zi_stm32f4discovery)
 
+add_executable(test-bmp280 src/tests/sensors/test-bmp280.cpp)
+sbs_target(test-bmp280 stm32f429zi_stm32f4discovery)
+
 add_executable(test-bmx160 src/tests/sensors/test-bmx160.cpp)
 sbs_target(test-bmx160 stm32f429zi_skyward_death_stack_x)
 
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 606c04e0e473de590618db29eb6003ac8a9613d1..a37df344373e492d13a75e30d802312f17430004 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -43,7 +43,6 @@ 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/radio/SX1278/SX1278.cpp
 
         # Events
         ${SBS_BASE}/src/shared/events/EventBroker.cpp
@@ -60,6 +59,7 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp
         ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
         ${SBS_BASE}/src/shared/radio/Xbee/Xbee.cpp
+        ${SBS_BASE}/src/shared/radio/SX1278/SX1278.cpp
 
         # Scheduler
         ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
@@ -69,6 +69,7 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04.cpp
         ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp
         ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
+        ${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp
         ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
         ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
         ${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp
diff --git a/scripts/generators/genutils.py b/scripts/generators/genutils.py
index d1f10e41b5a5e1cf7265e98643f548218a0d926e..ea03a91f3a98e8ccb0c94e81b866e5cdd91dee15 100644
--- a/scripts/generators/genutils.py
+++ b/scripts/generators/genutils.py
@@ -68,6 +68,7 @@ def parse_scxml(file):
                     (topic, event) = camel_case_to_underscores(
                         transition.attrib['event']).split('.')
                     topics.append(topic)
+                    event = topic + "_" + event
                 else:
                     event = camel_case_to_underscores(
                         transition.attrib['event'])
diff --git a/src/shared/drivers/servo/Servo.cpp b/src/shared/drivers/servo/Servo.cpp
index 3c37bd1fba313b94f36230249d186341b02559d9..ce0f9330209bbd1137bcc5000443a8fcda21618c 100644
--- a/src/shared/drivers/servo/Servo.cpp
+++ b/src/shared/drivers/servo/Servo.cpp
@@ -51,6 +51,8 @@ void Servo::setPosition(float position)
 
 void Servo::setPosition90Deg(float degrees) { setPosition(degrees / 90); }
 
+void Servo::setPosition120Deg(float degrees) { setPosition(degrees / 120); }
+
 void Servo::setPosition180Deg(float degrees) { setPosition(degrees / 180); }
 
 void Servo::setPosition360Deg(float degrees) { setPosition(degrees / 360); }
diff --git a/src/shared/drivers/servo/Servo.h b/src/shared/drivers/servo/Servo.h
index 3d93e7364a606faa6b9546a32cccc61834dceeab..f070f196b1a7a2d2af03534ac90f18f591b6e661 100644
--- a/src/shared/drivers/servo/Servo.h
+++ b/src/shared/drivers/servo/Servo.h
@@ -57,9 +57,9 @@ public:
     /**
      * @brief Prepare the timer and sets the PWM output to the minimum.
      *
-     * More specifically, the PWM output is prepared to be equal to minPulse bu
+     * More specifically, the PWM output is prepared to be equal to minPulse but
      * it is not enabled! After creating the object the PWM signal is not
-     * active. This is to ensure
+     * active. This is to ensure the servo motor doesn't move unexpectedly.
      *
      * Note that the timer peripheral's clock is enabled automatically when the
      * PWM object is created.
@@ -93,6 +93,8 @@ public:
 
     void setPosition90Deg(float degrees);
 
+    void setPosition120Deg(float degrees);
+
     void setPosition180Deg(float degrees);
 
     void setPosition360Deg(float degrees);
diff --git a/src/shared/drivers/spi/SPI.h b/src/shared/drivers/spi/SPI.h
index 2cf55bf809c17090b214a55541b3458e5f514e07..de60c5ed54ae18cfb7156e3a4f31826e6f434ee7 100644
--- a/src/shared/drivers/spi/SPI.h
+++ b/src/shared/drivers/spi/SPI.h
@@ -340,7 +340,7 @@ inline uint16_t SPI::read16() { return transfer(static_cast<uint16_t>(0)); }
 inline void SPI::read(uint8_t *data, size_t nBytes)
 {
     // Reset the data
-    for (size_t i = 0; i < nBytes / 2; i++)
+    for (size_t i = 0; i < nBytes; i++)
         data[i] = 0;
 
     // Read the data
diff --git a/src/shared/drivers/stepper/Stepper.h b/src/shared/drivers/stepper/Stepper.h
index 80f61e075e5b44f408b03fd1e3d43f9f89bee9f0..fa22b4641be1ce4e4a98c9ce85eac281ea360bce 100644
--- a/src/shared/drivers/stepper/Stepper.h
+++ b/src/shared/drivers/stepper/Stepper.h
@@ -1,5 +1,5 @@
 /* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Alberto Nidasio
+ * Author: 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
diff --git a/src/shared/logger/Logger.cpp b/src/shared/logger/Logger.cpp
index 9ba695f576df0c20884a7607b56cce91c1b8c1b9..2709fbcc2f7d3a6e9bf860f180f7036200b4a433 100644
--- a/src/shared/logger/Logger.cpp
+++ b/src/shared/logger/Logger.cpp
@@ -128,7 +128,12 @@ void Logger::stop()
     fileNumber = -1;  // Reset the fileNumber to an invalid value
 }
 
-bool Logger::testSDCard() { return ofstream("/sd/test").good(); }
+bool Logger::testSDCard()
+{
+    bool result = ofstream("/sd/test").good();
+    std::remove("/sd/test");
+    return result;
+}
 
 int Logger::getCurrentLogNumber() { return fileNumber; }
 
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..9f355c0365769056e174e6557f02e21f69319449 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg(
     const mavlink_message_t& msg)
 {
     // Convert mavlink message to char array
-    uint8_t msgtempBuf[PktLength];
+    // Use fixed buffer size to avoid overflows
+    uint8_t msgtempBuf[256];
     int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg);
 
     // Append message to the queue
diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp
index 6a7f718f422a9a298e225da2f7773b5b76c14062..accd5f31a0b474b5c06d4b4d4a3add72fd79fb2c 100644
--- a/src/shared/radio/SX1278/SX1278.cpp
+++ b/src/shared/radio/SX1278/SX1278.cpp
@@ -249,14 +249,15 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len)
 
 bool SX1278::send(uint8_t *pkt, size_t len)
 {
-    // Hacked together larger packets support
-    while (len > 0)
-    {
-        // This shouldn't be needed, but for some reason the device "lies" about
-        // being ready, so lock up if we are going too fast
-        rateLimitTx();
+    // Packets longer than FIFO_LEN (-1 for the len byte) are not supported
+    if (len > SX1278Defs::FIFO_LEN - 1)
+        return false;
+
+    // This shouldn't be needed, but for some reason the device "lies" about
+    // being ready, so lock up if we are going too fast
+    rateLimitTx();
 
-        bus_mgr.lock(SX1278BusManager::Mode::MODE_TX);
+    bus_mgr.lock(SX1278BusManager::Mode::MODE_TX);
 
         // Wait for TX ready
         bus_mgr.waitForIrq(RegIrqFlags::TX_READY);
@@ -282,6 +283,11 @@ bool SX1278::send(uint8_t *pkt, size_t len)
         last_tx = now();
     }
 
+    // Wait for packet sent
+    bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT);
+    bus_mgr.unlock();
+
+    last_tx = now();
     return true;
 }
 
diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h
index f38059551c2ca51b04393263044abc0abc004715..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644
--- a/src/shared/radio/SX1278/SX1278Defs.h
+++ b/src/shared/radio/SX1278/SX1278Defs.h
@@ -34,9 +34,9 @@ namespace SX1278Defs
 {
 
 /**
- * @brief Length of the fifo.
+ * @brief Length of the internal FIFO
  */
-constexpr size_t FIFO_LEN = 64;
+constexpr int FIFO_LEN = 64;
 
 /**
  * @brief Main oscillator frequency (Hz)
diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp
index 10be66cee23d465072cd20e6d9ec97907a986199..ccc4e90af3aa97187f137a45f28b0e6635ecea39 100644
--- a/src/shared/sensors/BME280/BME280.cpp
+++ b/src/shared/sensors/BME280/BME280.cpp
@@ -54,16 +54,6 @@ BME280::BME280(SPISlave spiSlave_, BME280Config config_)
 
 bool BME280::init()
 {
-    // Check if already initialized
-    if (initialized)
-    {
-        LOG_ERR(logger, "Already initialized");
-
-        lastError = SensorErrors::ALREADY_INIT;
-
-        return false;
-    }
-
     // Check WHO AM I
     if (!checkWhoAmI())
     {
@@ -121,20 +111,19 @@ bool BME280::init()
         return false;
     }
 
-    initialized = true;
     return true;
 }
 
-void BME280::setHumidityOversampling(Oversampling oversampling)
+void BME280::setSensorMode(Mode mode)
 {
-    config.bits.oversamplingHumidity = oversampling;
+    config.bits.mode = mode;
 
     setConfiguration();
 }
 
-void BME280::setSensorMode(Mode mode)
+void BME280::setHumidityOversampling(Oversampling oversampling)
 {
-    config.bits.mode = mode;
+    config.bits.oversamplingHumidity = oversampling;
 
     setConfiguration();
 }
@@ -237,12 +226,6 @@ TemperatureData BME280::readTemperature()
     return lastSample;
 }
 
-HumidityData BME280::getHumidity() { return lastSample; }
-
-PressureData BME280::getPressure() { return lastSample; }
-
-TemperatureData BME280::getTemperature() { return lastSample; }
-
 unsigned int BME280::calculateMaxMeasurementTime(BME280Config config_)
 {
     return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) +
diff --git a/src/shared/sensors/BME280/BME280.h b/src/shared/sensors/BME280/BME280.h
index 3dd66cce83dc94db771992ce508560981cdfe1a7..7ab2c6cdd39e4d1973091a4c4c40d24e546369db 100644
--- a/src/shared/sensors/BME280/BME280.h
+++ b/src/shared/sensors/BME280/BME280.h
@@ -163,12 +163,6 @@ public:
      */
     bool init() override;
 
-    /**
-     * @brief Sets the oversampling for humidity readings, use SKIPPED to
-     * disable humidity sampling
-     */
-    void setHumidityOversampling(Oversampling oversampling);
-
     /**
      * @brief Sets the sensor mode
      *
@@ -182,6 +176,12 @@ public:
      */
     void setSensorMode(Mode mode);
 
+    /**
+     * @brief Sets the oversampling for humidity readings, use SKIPPED to
+     * disable humidity sampling
+     */
+    void setHumidityOversampling(Oversampling oversampling);
+
     /**
      * @brief Sets the oversampling for pressure readings, use SKIPPED to
      * disable pressure sampling
@@ -220,12 +220,6 @@ public:
      */
     TemperatureData readTemperature();
 
-    HumidityData getHumidity();
-
-    PressureData getPressure();
-
-    TemperatureData getTemperature();
-
     /**
      * @brief Maximum measurement time formula from datasheet page 51
      *
@@ -301,8 +295,6 @@ private:
     BME280Comp compParams;
     int32_t fineTemperature;  // Used in compensation algorithm
 
-    bool initialized = false;  // Whether the sensor has been initialized
-
     PrintLogger logger = Logging::getLogger("bme280");
 };
 
diff --git a/src/shared/sensors/BME280/BME280Data.h b/src/shared/sensors/BME280/BME280Data.h
index 4cfe0e90794942102f34641687ff22763e9d557d..473989e103fe62904a75f1c13a1ee425102f5e4f 100644
--- a/src/shared/sensors/BME280/BME280Data.h
+++ b/src/shared/sensors/BME280/BME280Data.h
@@ -46,9 +46,8 @@ struct BME280Data : public TemperatureData,
 
     static std::string header()
     {
-        return "temperatureTimestamp,temp,pressureTimestamp,press,humid_"
-               "timestamp,"
-               "humid\n";
+        return "temperatureTimestamp,temperature,pressureTimestamp,pressure,"
+               "humid_timestamp,humidity\n";
     }
 
     void print(std::ostream& os) const
diff --git a/src/shared/sensors/BMP280/BMP280.cpp b/src/shared/sensors/BMP280/BMP280.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11839de28598dcfc60d049bc5985e4cba3c8f283
--- /dev/null
+++ b/src/shared/sensors/BMP280/BMP280.cpp
@@ -0,0 +1,309 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: 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
+ * 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 "BMP280.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <math.h>
+
+using namespace std;
+
+namespace Boardcore
+{
+
+const BMP280::BMP280Config BMP280::BMP280_DEFAULT_CONFIG = {
+    0, 0, SLEEP_MODE, SKIPPED, SKIPPED, 0, FILTER_OFF, STB_TIME_0_5};
+
+const BMP280::BMP280Config BMP280::BMP280_CONFIG_ALL_ENABLED = {0,
+                                                                0,
+                                                                NORMAL_MODE,
+                                                                OVERSAMPLING_16,
+                                                                OVERSAMPLING_2,
+                                                                0,
+                                                                FILTER_COEFF_16,
+                                                                STB_TIME_0_5};
+
+const BMP280::BMP280Config BMP280::BMP280_CONFIG_TEMP_SINGLE = {
+    0, 0, FORCED_MODE, SKIPPED, OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5};
+
+BMP280::BMP280(SPISlave spiSlave_, BMP280Config config_)
+    : spiSlave(spiSlave_), config(config_)
+{
+}
+
+bool BMP280::init()
+{
+    // Check WHO AM I
+    if (!checkWhoAmI())
+    {
+        LOG_ERR(logger, "Invalid WHO AM I");
+
+        lastError = SensorErrors::INVALID_WHOAMI;
+
+        return false;
+    }
+
+    loadCompensationParameters();
+
+    // Read once the temperature to compute fineTemperature
+    setConfiguration(BMP280_CONFIG_TEMP_SINGLE);
+    miosix::Thread::sleep(
+        calculateMaxMeasurementTime(BMP280_CONFIG_TEMP_SINGLE));
+    readTemperature();
+
+    setConfiguration();
+
+    BMP280Config readBackConfig = readConfiguration();
+
+    // Check if the configration on the device matches ours
+    if (config.bytes.ctrlPressureAndTemperature !=
+            readBackConfig.bytes.ctrlPressureAndTemperature ||
+        config.bytes.config != readBackConfig.bytes.config)
+    {
+        LOG_ERR(logger, "Device configuration incorrect, setup failed");
+
+        lastError = SensorErrors::NOT_INIT;
+
+        return false;
+    }
+
+    return true;
+}
+
+void BMP280::setSensorMode(Mode mode)
+{
+    config.bits.mode = mode;
+
+    setConfiguration();
+}
+
+void BMP280::setPressureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingPressure = oversampling;
+
+    setConfiguration();
+}
+
+void BMP280::setTemperatureOversampling(Oversampling oversampling)
+{
+    config.bits.oversamplingTemperature = oversampling;
+
+    setConfiguration();
+}
+
+void BMP280::setFilterCoeff(FilterCoeff filterCoeff)
+{
+    config.bits.filter = filterCoeff;
+
+    setConfiguration();
+}
+
+void BMP280::setStandbyTime(StandbyTime standbyTime)
+{
+    config.bits.standbyTime = standbyTime;
+
+    setConfiguration();
+}
+
+PressureData BMP280::readPressure()
+{
+    uint8_t buffer[3];
+    int32_t adc_P = 0;
+
+    {
+        SPITransaction transaction(spiSlave);
+
+        transaction.readRegisters(REG_PRESS_MSB, buffer, 3);
+    }
+
+    adc_P |= ((uint32_t)buffer[0]) << 12;
+    adc_P |= ((uint32_t)buffer[1]) << 4;
+    adc_P |= (buffer[2] >> 4) & 0x0F;
+
+    // Compensate pressure
+    lastSample.pressureTimestamp = TimestampTimer::getInstance().getTimestamp();
+    lastSample.pressure =
+        (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+
+    return lastSample;
+}
+
+TemperatureData BMP280::readTemperature()
+{
+    uint8_t buffer[3];
+    int32_t adcTemperature = 0;
+
+    {
+        SPITransaction transaction(spiSlave);
+
+        transaction.readRegisters(REG_TEMP_MSB, buffer, 3);
+    }
+
+    adcTemperature |= ((uint32_t)buffer[0]) << 12;
+    adcTemperature |= ((uint32_t)buffer[1]) << 4;
+    adcTemperature |= (buffer[2] >> 4) & 0x0F;
+
+    // Compensate temperature
+    fineTemperature = computeFineTemperature(adcTemperature);
+    lastSample.temperatureTimestamp =
+        TimestampTimer::getInstance().getTimestamp();
+    lastSample.temperature = (float)compensateTemperature(fineTemperature) /
+                             100;  // Converto to DegC
+
+    return lastSample;
+}
+
+unsigned int BMP280::calculateMaxMeasurementTime(BMP280Config config_)
+{
+    // TODO: This folrmula is not present in the BMP280's datasheet, it should
+    // be checked
+    return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) +
+                (2.3 * config_.bits.oversamplingPressure + 0.575));
+}
+
+unsigned int BMP280::getMaxMeasurementTime()
+{
+    return calculateMaxMeasurementTime(config);
+}
+
+bool BMP280::selfTest() { return checkWhoAmI(); }
+
+BMP280Data BMP280::sampleImpl()
+{
+    uint8_t buffer[8];
+    int32_t adcTemperature = 0;
+    int32_t adc_P          = 0;
+    BMP280Data data;
+
+    // TODO: implement selective read!
+
+    // Burst read pressure, temperature and humidity
+    {
+        SPITransaction transaction(spiSlave);
+
+        transaction.readRegisters(REG_PRESS_MSB, buffer, 8);
+    }
+
+    adcTemperature |= ((uint32_t)buffer[3]) << 12;
+    adcTemperature |= ((uint32_t)buffer[4]) << 4;
+    adcTemperature |= (buffer[5] >> 4) & 0x0F;
+
+    adc_P |= ((uint32_t)buffer[0]) << 12;
+    adc_P |= ((uint32_t)buffer[1]) << 4;
+    adc_P |= (buffer[2] >> 4) & 0x0F;
+
+    // Compensate temperature
+    fineTemperature           = computeFineTemperature(adcTemperature);
+    data.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp();
+    data.temperature          = (float)compensateTemperature(fineTemperature) /
+                       100;  // Converto to DegC
+
+    // Compensate pressure
+    data.pressureTimestamp = TimestampTimer::getInstance().getTimestamp();
+    data.pressure = (float)compensatePressure(adc_P) / 256;  // Convert to Pa
+
+    return data;
+}
+
+bool BMP280::checkWhoAmI()
+{
+    SPITransaction transaction(spiSlave);
+
+    uint8_t whoAmIValue = transaction.readRegister(REG_ID);
+
+    return whoAmIValue == REG_ID_VAL;
+}
+
+void BMP280::setConfiguration() { setConfiguration(config); }
+
+void BMP280::setConfiguration(BMP280Config config_)
+{
+    SPITransaction transaction(spiSlave);
+
+    transaction.writeRegister(REG_CONFIG & 0x7F, config_.bytes.config);
+    transaction.writeRegister(REG_CTRL_MEAS & 0x7F,
+                              config_.bytes.ctrlPressureAndTemperature);
+}
+
+BMP280::BMP280Config BMP280::readConfiguration()
+{
+    BMP280Config tmp;
+    SPITransaction transaction(spiSlave);
+
+    transaction.readRegisters(REG_STATUS, (uint8_t *)&tmp, 3);
+
+    return tmp;
+}
+
+void BMP280::loadCompensationParameters()
+{
+    // Read compensation parameters
+    {
+        SPITransaction transaction(spiSlave);
+
+        transaction.readRegisters(REG_CALIB_0, (uint8_t *)&compParams, 25);
+    }
+}
+
+int32_t BMP280::computeFineTemperature(int32_t adcTemperature)
+{
+    int32_t var1, var2;
+    var1 = ((((adcTemperature >> 3) - ((int32_t)compParams.bits.dig_T1 << 1))) *
+            ((int32_t)compParams.bits.dig_T2)) >>
+           11;
+    var2 = (((((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1)) *
+              ((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1))) >>
+             12) *
+            ((int32_t)compParams.bits.dig_T3)) >>
+           14;
+    return var1 + var2;
+}
+
+int32_t BMP280::compensateTemperature(int32_t fineTemperature)
+{
+    return (fineTemperature * 5 + 128) >> 8;
+}
+
+uint32_t BMP280::compensatePressure(int32_t adc_P)
+{
+    int64_t var1, var2, p;
+    var1 = ((int64_t)fineTemperature) - 128000;
+    var2 = var1 * var1 * (int64_t)compParams.bits.dig_P6;
+    var2 = var2 + ((var1 * (int64_t)compParams.bits.dig_P5) << 17);
+    var2 = var2 + (((int64_t)compParams.bits.dig_P4) << 35);
+    var1 = ((var1 * var1 * (int64_t)compParams.bits.dig_P3) >> 8) +
+           ((var1 * ((int64_t)compParams.bits.dig_P2) << 12));
+    var1 =
+        ((((int64_t)1) << 47) + var1) * ((int64_t)compParams.bits.dig_P1) >> 33;
+    if (var1 == 0)
+    {
+        return 0;  // avoid exception caused by division by zero
+    }
+    p    = 1048576 - adc_P;
+    p    = (((p << 31) - var2) * 3125) / var1;
+    var1 = (((int64_t)compParams.bits.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
+    var2 = (((int64_t)compParams.bits.dig_P8) * p) >> 19;
+    p    = ((p + var1 + var2) >> 8) + (((int64_t)compParams.bits.dig_P7) << 4);
+    return (uint32_t)p;
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h
new file mode 100644
index 0000000000000000000000000000000000000000..385a40695fc386cc647cd051431198c150f9b835
--- /dev/null
+++ b/src/shared/sensors/BMP280/BMP280.h
@@ -0,0 +1,273 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: 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
+ * 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 <sensors/Sensor.h>
+
+#include "BMP280Data.h"
+
+namespace Boardcore
+{
+
+class BMP280 : public Sensor<BMP280Data>
+{
+public:
+    enum Oversampling
+    {
+        SKIPPED         = 0x0,  ///< Skipped (output set to 0x8000)
+        OVERSAMPLING_1  = 0x1,  ///< Oversampling x1
+        OVERSAMPLING_2  = 0x2,  ///< Oversampling x2
+        OVERSAMPLING_4  = 0x3,  ///< Oversampling x4
+        OVERSAMPLING_8  = 0x4,  ///< Oversampling x8
+        OVERSAMPLING_16 = 0x5,  ///< Oversampling x16
+    };
+
+    enum Mode
+    {
+        SLEEP_MODE  = 0x0,  ///< Sleep mode
+        FORCED_MODE = 0x1,  ///< Forced mode
+        NORMAL_MODE = 0x3   ///< Normal mode
+    };
+
+    enum StandbyTime
+    {
+        STB_TIME_0_5  = 0x0,  ///< 0.5 ms
+        STB_TIME_62_5 = 0x1,  ///< 62.5 ms
+        STB_TIME_125  = 0x2,  ///< 125 ms
+        STB_TIME_250  = 0x3,  ///< 250 ms
+        STB_TIME_500  = 0x4,  ///< 500 ms
+        STB_TIME_1000 = 0x5,  ///< 1000 ms
+        STB_TIME_20   = 0x6,  ///< 20 ms
+        STB_TIME_40   = 0x7   ///< 40 ms
+    };
+
+    enum FilterCoeff
+    {
+        FILTER_OFF      = 0x0,  ///< Filter off
+        FILTER_COEFF_2  = 0x1,  ///< Filter coefficient = 2
+        FILTER_COEFF_4  = 0x2,  ///< Filter coefficient = 4
+        FILTER_COEFF_8  = 0x3,  ///< Filter coefficient = 8
+        FILTER_COEFF_16 = 0x4   ///< Filter coefficient = 16
+    };
+
+    union BMP280Config
+    {
+        struct __attribute__((packed)) BMP280ConfigBits
+        {
+            // status
+            /**
+             * '1' when the NVM data are being copied to image registers, '0'
+             * when the copying is done
+             */
+            uint8_t imUpdate : 1;
+            uint8_t : 2;
+            /**
+             * '1' whenever a conversion is running, '0' when the result have
+             * been transferred to the data registers
+             */
+            uint8_t measuring : 1;
+            uint8_t : 4;
+
+            Mode mode : 2;  ///< Device modes
+            Oversampling
+                oversamplingPressure : 3;  ///< Oversampling of pressure
+            Oversampling
+                oversamplingTemperature : 3;  ///< Oversampling of temperature
+
+            // config
+            uint8_t spi3wEn : 1;  ///< Enables 3-wire SPI interface
+            uint8_t : 1;
+            FilterCoeff filter : 3;       ///< Time constant of the IIR filter
+            StandbyTime standbyTime : 3;  ///< Inactive duration in normal mode
+        } bits;
+
+        struct
+        {
+            uint8_t status;                      ///< Device status
+            uint8_t ctrlPressureAndTemperature;  ///< Pressure and temperature
+                                                 ///< options
+            uint8_t config;  ///< Rate, filter and interface options
+        } bytes;
+
+        uint8_t bytesArray[3];
+    };
+
+    union BMP280Comp
+    {
+        struct __attribute__((packed))
+        {
+            uint16_t dig_T1;
+            int16_t dig_T2;
+            int16_t dig_T3;
+            uint16_t dig_P1;
+            int16_t dig_P2;
+            int16_t dig_P3;
+            int16_t dig_P4;
+            int16_t dig_P5;
+            int16_t dig_P6;
+            int16_t dig_P7;
+            int16_t dig_P8;
+            int16_t dig_P9;
+        } bits;
+
+        uint8_t bytesArray[24];
+    };
+
+    static constexpr uint8_t REG_ID_VAL = 0x58;  ///< Who am I value
+
+    static const BMP280Config BMP280_DEFAULT_CONFIG;      ///< Default register
+                                                          ///< values
+    static const BMP280Config BMP280_CONFIG_ALL_ENABLED;  ///< Datasheet
+                                                          ///< values for
+                                                          ///< indoor
+                                                          ///< navigation
+    static const BMP280Config BMP280_CONFIG_TEMP_SINGLE;  ///< Temperature
+                                                          ///< enabled in
+                                                          ///< forced mode
+
+    explicit BMP280(SPISlave spiSlave_,
+                    BMP280Config config_ = BMP280_CONFIG_ALL_ENABLED);
+
+    /**
+     * @brief Initialize the device with the specified configuration
+     */
+    bool init() override;
+
+    /**
+     * @brief Sets the sensor mode
+     *
+     * Values:
+     * - SLEEP_MODE: No measurements are performed
+     * - FORCED_MODE: A single measurement is performed when this function
+     * writes the configuration, after the measurement the sensor return to
+     * sleep mode
+     * - NORMAL_MODE: Automated cycling between measurements, standby time can
+     * be set using setStandbyTime()
+     */
+    void setSensorMode(Mode mode);
+
+    /**
+     * @brief Sets the oversampling for pressure readings, use SKIPPED to
+     * disable pressure sampling
+     */
+    void setPressureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the oversampling for temperature readings, use SKIPPED to
+     * disable temperature sampling
+     */
+    void setTemperatureOversampling(Oversampling oversampling);
+
+    /**
+     * @brief Sets the coefficient for the IIR filter (applied to temperature
+     * and pressure)
+     */
+    void setFilterCoeff(FilterCoeff filterCoeff);
+
+    /**
+     * @brief Sets the standby time between readings in normal mode
+     */
+    void setStandbyTime(StandbyTime standbyTime);
+
+    /**
+     * @brief Reads only the pressure, does not set the configuration
+     */
+    PressureData readPressure();
+
+    /**
+     * @brief Reads only the temperature, does not set the configuration
+     */
+    TemperatureData readTemperature();
+
+    /**
+     * @brief Maximum measurement time formula from datasheet page 51
+     *
+     * @return Time in milliseconds
+     */
+    static unsigned int calculateMaxMeasurementTime(BMP280Config config_);
+
+    unsigned int getMaxMeasurementTime();
+
+    /**
+     * @brief Reads the WHO AM I register
+     *
+     * @return True if everything ok
+     */
+    bool selfTest() override;
+
+private:
+    BMP280Data sampleImpl() override;
+
+    void setConfiguration();
+
+    void setConfiguration(BMP280Config config_);
+
+    BMP280Config readConfiguration();
+
+    void loadCompensationParameters();
+
+    // Compensation algorithm rev.1.1 from Bosh datasheet
+
+    int32_t computeFineTemperature(int32_t adcTemperature);
+
+    int32_t compensateTemperature(int32_t fineTemperature);
+
+    uint32_t compensatePressure(int32_t adcPressure);
+    /**
+     * @brief Check the WHO AM I code from the device.
+     *
+     * @return true if the device is recognized
+     */
+    bool checkWhoAmI();
+
+    enum BMP280Registers : uint8_t
+    {
+        REG_CALIB_0 = 0x88,
+        // Calibration register 1-25
+
+        REG_ID    = 0xD0,
+        REG_RESET = 0xE0,
+
+        REG_STATUS    = 0xF3,
+        REG_CTRL_MEAS = 0xF4,
+        REG_CONFIG    = 0xF5,
+
+        REG_PRESS_MSB  = 0xF7,
+        REG_PRESS_LSB  = 0xF8,
+        REG_PRESS_XLSB = 0xF9,
+        REG_TEMP_MSB   = 0xFA,
+        REG_TEMP_LSB   = 0xFB,
+        REG_TEMP_XLSB  = 0xFC
+    };
+
+    const SPISlave spiSlave;
+    BMP280Config config;
+    BMP280Comp compParams;
+    int32_t fineTemperature;  // Used in compensation algorithm
+
+    PrintLogger logger = Logging::getLogger("bmp280");
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/BMP280/BMP280Data.h b/src/shared/sensors/BMP280/BMP280Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfd3cecc200fb8be00b137bc725a6a62a2776f45
--- /dev/null
+++ b/src/shared/sensors/BMP280/BMP280Data.h
@@ -0,0 +1,55 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: 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
+ * 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 <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct BMP280Data : public TemperatureData, public PressureData
+{
+    BMP280Data() : TemperatureData{0, 0.0}, PressureData{0, 0.0} {}
+
+    BMP280Data(uint64_t timestamp, float temperature, float pressure,
+               float humidity)
+        : TemperatureData{timestamp, temperature}, PressureData{timestamp,
+                                                                pressure}
+
+    {
+    }
+
+    static std::string header()
+    {
+        return "temperatureTimestamp,temperature,pressureTimestamp,pressure,\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << temperatureTimestamp << "," << temperature << ","
+           << pressureTimestamp << "," << pressure << ","
+           << "\n";
+    }
+};
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h
index 3f7a1616ef1c0c732527b91ed209964d9af3c893..9495bc6f5fe792774f0bb9a59c32f35c6e97e5b8 100644
--- a/src/shared/sensors/UbloxGPS/UbloxGPS.h
+++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h
@@ -129,7 +129,7 @@ private:
     const int defaultBaudrate;  // [baud]
 
     char gpsFilePath[16];  ///< Allows for a filename of up to 10 characters
-    int gpsFile;
+    int gpsFile = -1;
 
     mutable miosix::FastMutex mutex;
     UbloxGPSData threadSample{};
diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h
index ff5e7b360a3f612cf73feae05e0283267d96108f..a2bd35e33855484f33c6b96dae9655492ca77dd5 100644
--- a/src/shared/utils/collections/SyncPacketQueue.h
+++ b/src/shared/utils/collections/SyncPacketQueue.h
@@ -67,16 +67,15 @@ public:
     ~Packet() { content.clear(); };
 
     /**
-     * @brief Try to append a given message to the packet.
+     * @brief Append a given message to the packet.
      *
      * If it's the first message, also set the timestamp.
      *
      * @param msg The message to be appended.
      * @param msgLen Length of msg.
-     * @return true if the message was appended correctly.
-     * @return false if there isn't enough space for the message.
+     * @return How many bytes were actually appended.
      */
-    bool tryAppend(const uint8_t* msg, const size_t msgLen);
+    size_t append(const uint8_t* msg, size_t msgLen);
 
     /**
      * @brief Mark the packet as ready to be sent.
@@ -111,7 +110,7 @@ public:
     inline bool isReady() const { return ready; }
 
     /**
-     * @return The timestamp of the first successful call to tryAppend().
+     * @return The timestamp of the first successful call to append().
      */
     inline uint64_t timestamp() const { return ts; }
 
@@ -126,7 +125,7 @@ public:
     inline unsigned int maxSize() const { return len; }
 
     /**
-     * @return How many times the tryAppend() function has been called
+     * @return How many times the append() function has been called
      * successfully.
      */
     inline unsigned int getMsgCount() const { return msgCounter; }
@@ -148,13 +147,12 @@ private:
 };
 
 template <unsigned int len>
-bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen)
+size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
 {
-    if (msgLen == 0 || content.size() + msgLen > len)
-    {
-        return false;
-    }
-    else
+    size_t remaining = len - content.size();
+    msgLen           = std::min(remaining, msgLen);
+
+    if (msgLen != 0)
     {
         // Set the packet's timestamp when the first message is inserted
         if (content.size() == 0)
@@ -165,9 +163,9 @@ bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen)
         // Append the message to the packet
         content.insert(content.end(), msg, msg + msgLen);
         msgCounter++;
-
-        return true;
     }
+
+    return msgLen;
 }
 
 template <unsigned int len>
@@ -230,7 +228,7 @@ public:
     {
         int dropped = 0;
 
-        if (msgLen == 0 || msgLen > pktLen)
+        if (msgLen == 0)
         {
             return -1;
         }
@@ -243,43 +241,37 @@ public:
                 buffer.put(Pkt{});
             }
 
-            Pkt& last = buffer.last();
-
-            bool added = false;
-
-            // Add to the current packet only if it isn't already ready
-            if (!last.isReady())
-            {
-                added = last.tryAppend(msg, msgLen);
-            }
-
-            // If already ready or cannot fit the new data, add to a new packet
-            if (!added)
+            while (msgLen > 0)
             {
-                // Mark the packet as ready (in the case it wasn't already)
-                last.markAsReady();
-
-                if (buffer.isFull())
+                if (buffer.last().isReady())
                 {
-                    // We have dropped a packet
-                    ++dropped;
+                    if (buffer.isFull())
+                    {
+                        // We have dropped a packet
+                        ++dropped;
+                    }
+
+                    // If the last pkt is ready, append a new one
+                    buffer.put(Pkt{});
+                    // FIXME(davide.mor): Figure out quantum shenanigans
+                    // uncommenting the following line causes everything to
+                    // break, why?
+
+                    // last = buffer.last();
                 }
-                // Add a new packet and fill that instead
-                Pkt& newpkt = buffer.put(Pkt{});
 
-                if (!newpkt.tryAppend(msg, msgLen))
+                size_t sentLen = buffer.last().append(msg, msgLen);
+
+                msgLen -= sentLen;
+                msg += sentLen;
+
+                // Mark as ready if the packet is full
+                if (buffer.last().isFull())
                 {
-                    TRACE("Packet is too big!\n");
-                    return -1;
+                    buffer.last().markAsReady();
                 }
             }
 
-            // Mark as ready if the packet is full
-            if (buffer.last().isFull())
-            {
-                buffer.last().markAsReady();
-            }
-
             cvNotempty.broadcast();
             return dropped;
         }
diff --git a/src/tests/drivers/stepper/test-stepper.cpp b/src/tests/drivers/stepper/test-stepper.cpp
index 4072e5269484a11e9fe2606943a468b35aed8573..3b27f5f0f565a98e6c41882148f92e31f2ef2ae2 100644
--- a/src/tests/drivers/stepper/test-stepper.cpp
+++ b/src/tests/drivers/stepper/test-stepper.cpp
@@ -1,5 +1,5 @@
 /* Copyright (c) 2022 Skyward Experimental Rocketry
- * Authors: Alberto Nidasio
+ * Author: 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
diff --git a/src/tests/sensors/test-bme280.cpp b/src/tests/sensors/test-bme280.cpp
index ab960d91ffcd84e51929f4deb927f596b4fd6875..afa223a4a46bab257afa859e1f4fd0ea1fad2cf4 100644
--- a/src/tests/sensors/test-bme280.cpp
+++ b/src/tests/sensors/test-bme280.cpp
@@ -54,15 +54,15 @@ GpioPin csPin   = GpioPin(GPIOC_BASE, 1);
 void initBoard()
 {
     // Alternate function configuration for SPI pins
-    sckPin.mode(miosix::Mode::ALTERNATE);
+    sckPin.mode(Mode::ALTERNATE);
     sckPin.alternateFunction(5);  // SPI function
-    mosiPin.mode(miosix::Mode::ALTERNATE);
+    mosiPin.mode(Mode::ALTERNATE);
     mosiPin.alternateFunction(5);  // SPI function
-    misoPin.mode(miosix::Mode::ALTERNATE);
+    misoPin.mode(Mode::ALTERNATE);
     misoPin.alternateFunction(5);  // SPI function
 
     // Chip select pin as output starting high
-    csPin.mode(miosix::Mode::OUTPUT);
+    csPin.mode(Mode::OUTPUT);
     csPin.high();
 }
 
@@ -98,7 +98,7 @@ int main()
     {
         bme280.setSensorMode(BME280::FORCED_MODE);
 
-        miosix::Thread::sleep(bme280.getMaxMeasurementTime());
+        Thread::sleep(bme280.getMaxMeasurementTime());
 
         bme280.sample();
 
@@ -106,7 +106,7 @@ int main()
               bme280.getLastSample().temperature,
               bme280.getLastSample().pressure, bme280.getLastSample().humidity);
 
-        miosix::Thread::sleep(1000);
+        Thread::sleep(1000);
     }
 
     TRACE("Normal mode\n");
@@ -119,6 +119,6 @@ int main()
               bme280.getLastSample().temperature,
               bme280.getLastSample().pressure, bme280.getLastSample().humidity);
 
-        miosix::Thread::sleep(50);  // 25Hz
+        Thread::sleep(50);  // 25Hz
     }
 }
diff --git a/src/tests/sensors/test-bmp280.cpp b/src/tests/sensors/test-bmp280.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d83478dede299492b607e2434a70c35b559b0906
--- /dev/null
+++ b/src/tests/sensors/test-bmp280.cpp
@@ -0,0 +1,107 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: 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
+ * 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/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/BMP280/BMP280.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+GpioPin sckPin  = GpioPin(GPIOA_BASE, 5);
+GpioPin misoPin = GpioPin(GPIOB_BASE, 4);
+GpioPin mosiPin = GpioPin(GPIOA_BASE, 7);
+GpioPin csPin   = GpioPin(GPIOB_BASE, 2);
+
+void initBoard()
+{
+    // Alternate function configuration for SPI pins
+    sckPin.mode(Mode::ALTERNATE);
+    sckPin.alternateFunction(5);  // SPI function
+    mosiPin.mode(Mode::ALTERNATE);
+    mosiPin.alternateFunction(5);  // SPI function
+    misoPin.mode(Mode::ALTERNATE);
+    misoPin.alternateFunction(5);  // SPI function
+
+    // Chip select pin as output starting high
+    csPin.mode(Mode::OUTPUT);
+    csPin.high();
+}
+
+int main()
+{
+    // Enable SPI clock and set gpios
+    initBoard();
+
+    // SPI configuration setup
+    SPIBusConfig spiConfig;
+    // spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+    spiConfig.mode = SPI::Mode::MODE_0;
+    SPIBus spiBus(SPI1);
+    SPISlave spiSlave(spiBus, csPin, spiConfig);
+
+    // Device initialization
+    BMP280 bmp280(spiSlave);
+
+    bmp280.init();
+
+    // In practice the self test reads the who am i reagister, this is already
+    // done in init()
+    if (!bmp280.selfTest())
+    {
+        TRACE("Self test failed!\n");
+
+        return -1;
+    }
+
+    // Try forced mode
+    TRACE("Forced mode\n");
+    for (int i = 0; i < 10; i++)
+    {
+        bmp280.setSensorMode(BMP280::FORCED_MODE);
+
+        Thread::sleep(bmp280.getMaxMeasurementTime());
+
+        bmp280.sample();
+
+        TRACE("temp: %.2f DegC\tpress: %.2f hPa\n",
+              bmp280.getLastSample().temperature,
+              bmp280.getLastSample().pressure);
+
+        Thread::sleep(1000);
+    }
+
+    TRACE("Normal mode\n");
+    bmp280.setSensorMode(BMP280::NORMAL_MODE);
+    while (true)
+    {
+        bmp280.sample();
+
+        TRACE("temp: %.2f DegC\tpress: %.2f Pa\thumid: %.2f %%RH\n",
+              bmp280.getLastSample().temperature,
+              bmp280.getLastSample().pressure);
+
+        Thread::sleep(50);  // 25Hz
+    }
+}