diff --git a/src/shared/drivers/i2c/I2C.h b/src/shared/drivers/i2c/I2C.h
index f9b7d01c5e5ef88f0130e070b1c16fa3a8f17b8a..507239b1053d25483d4e04bc1c7c90bfbf5c56db 100644
--- a/src/shared/drivers/i2c/I2C.h
+++ b/src/shared/drivers/i2c/I2C.h
@@ -20,7 +20,10 @@
  * THE SOFTWARE.
  */
 
+#pragma once
+
 #include "I2CDriver.h"
+
 namespace Boardcore
 {
 
diff --git a/src/shared/sensors/MS5803/MS5803.cpp b/src/shared/sensors/MS5803/MS5803.cpp
index 681d662ee32cc18616093a1d5c7f83e6481acbf1..a7e38cdfa0dc5fd20ad348b8ff6babb89631509f 100644
--- a/src/shared/sensors/MS5803/MS5803.cpp
+++ b/src/shared/sensors/MS5803/MS5803.cpp
@@ -161,10 +161,8 @@ MS5803Data MS5803::updateData()
         (((((int64_t)rawPressure) * sens) / 2097152.0) - offs) / 32786.0;
 
     // Pressure in Pascal
-    temp /= 100.0f;
-
     return MS5803Data(TimestampTimer::getTimestamp(), pressure,
-                      lastTemperatureTimestamp, temp);
+                      lastTemperatureTimestamp, temp / 100.0f);
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/sensors/MS5803/MS5803I2C.cpp b/src/shared/sensors/MS5803/MS5803I2C.cpp
index edb63bf951cba60e75321cc96a74f74c458971ed..e6e887b7017b38f55eaa5ae808a03cc06c396e11 100644
--- a/src/shared/sensors/MS5803/MS5803I2C.cpp
+++ b/src/shared/sensors/MS5803/MS5803I2C.cpp
@@ -190,10 +190,8 @@ MS5803Data MS5803I2C::updateData()
         (((((int64_t)rawPressure) * sens) / 2097152.0) - offs) / 32786.0;
 
     // Pressure in Pascal
-    temp /= 100.0f;
-
     return MS5803Data(TimestampTimer::getTimestamp(), pressure,
-                      lastTemperatureTimestamp, temp);
+                      lastTemperatureTimestamp, temp / 100.0f);
 }
 
 uint16_t MS5803I2C::readReg(uint8_t reg)
diff --git a/src/tests/sensors/test-bmp280-i2c.cpp b/src/tests/sensors/test-bmp280-i2c.cpp
index 58edd892e2c6d8235bccc7ef5c19988a4adcc466..a913521bee1921e2b0c44c45e14c10cce13db6a9 100644
--- a/src/tests/sensors/test-bmp280-i2c.cpp
+++ b/src/tests/sensors/test-bmp280-i2c.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
+/* Copyright (c) 2023 Skyward Experimental Rocketry
  * Author: Alberto Nidasio
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -33,36 +33,24 @@ int main()
 {
     I2C bus(I2C3, scl, sda);
     BMP280I2C bmp280(bus);
-    bmp280.init();
 
-    if (!bmp280.selfTest())
-        printf("Self test failed!\n");
-
-    printf("Forced mode\n");
-    for (int i = 0; i < 10; i++)
+    if (!bmp280.init())
     {
-        bmp280.setSensorMode(BMP280I2C::FORCED_MODE);
-
-        Thread::sleep(bmp280.getMaxMeasurementTime());
-
-        bmp280.sample();
-
-        printf("temp: %.2f DegC\tpress: %.2f hPa\n",
-               bmp280.getLastSample().temperature,
-               bmp280.getLastSample().pressure);
+        printf("Init failed\n");
+    }
 
-        Thread::sleep(1000);
+    if (!bmp280.selfTest())
+    {
+        printf("Self test failed\n");
     }
 
-    printf("Normal mode\n");
-    bmp280.setSensorMode(BMP280I2C::NORMAL_MODE);
     while (true)
     {
         bmp280.sample();
 
-        printf("temp: %.2f DegC\tpress: %.2f Pa\n",
-               bmp280.getLastSample().temperature,
-               bmp280.getLastSample().pressure);
+        auto data = bmp280.getLastSample();
+        printf("[%.2f]: %.2fPa %.2f°\n", data.pressureTimestamp / 1e6,
+               data.pressure, data.temperature);
 
         Thread::sleep(50);  // 25Hz
     }
diff --git a/src/tests/sensors/test-ms5803-i2c.cpp b/src/tests/sensors/test-ms5803-i2c.cpp
index d8d90cc7628a9b9880ba604f1d0b5bc6bf85d1a9..7a7b83ddb96de0b6ee1e67297408c14c74a2421f 100644
--- a/src/tests/sensors/test-ms5803-i2c.cpp
+++ b/src/tests/sensors/test-ms5803-i2c.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Author: Nuno Barcellos
+/* Copyright (c) 2023 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
@@ -27,47 +27,30 @@ using namespace miosix;
 
 GpioPin scl(GPIOA_BASE, 8);
 GpioPin sda(GPIOC_BASE, 9);
-/**
- * This test is intended to be run on the Death Stack X
- */
 
 int main()
 {
-    printf("Starting...\n");
-
     I2C bus(I2C3, scl, sda);
-    I2CDriver::I2CSlaveConfig slaveConfig{0x76, I2CDriver::Addressing::BIT7,
-                                          I2CDriver::Speed::STANDARD};
+    MS5803I2C ms5803(bus, 10);
 
-    while (true)
+    if (!ms5803.init())
     {
-        uint8_t reg = 0xD0;
-        uint8_t data;
-        if (bus.readRegister(slaveConfig, reg, data))
-            printf("data: 0x%02x\n", data);
-
-        delayMs(100);
+        printf("MS5803 Init failed\n");
     }
-    // printf("2...\n");
-    // MS5803I2C sensor(bus, 10);
-
-    // printf("3...\n");
-
-    // if (!sensor.init())
-    // {
-    //     printf("MS5803 Init failed\n");
-    // }
 
-    // printf("pressureTimestamp,press,temperatureTimestamp,temp\n");
+    if (!ms5803.selfTest())
+    {
+        printf("Self test failed\n");
+    }
 
-    // while (true)
-    // {
-    //     sensor.sample();
+    while (true)
+    {
+        ms5803.sample();
 
-    //     MS5803Data data = sensor.getLastSample();
-    //     printf("%llu,%f,%llu,%f\n", data.pressureTimestamp, data.pressure,
-    //            data.temperatureTimestamp, data.temperature);
+        auto data = ms5803.getLastSample();
+        printf("[%.2f]: %.2fPa %.2f°\n", data.pressureTimestamp / 1e6,
+               data.pressure, data.temperature);
 
-    //     Thread::sleep(20);
-    // }
+        Thread::sleep(50);  // 25Hz
+    }
 }