diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.cpp b/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
index 4c41ed344521df3101056b0118f7b48d90cb588c..51df6d8586a3bb5044ee420aa0a208b414503be8 100644
--- a/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
+++ b/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
@@ -26,17 +26,18 @@
 
 #include "LPS28DFWDefs.h"
 
+using namespace Boardcore::LPS28DFWDefs;
+
 namespace Boardcore
 {
 LPS28DFW::LPS28DFW(I2C& i2c, SensorConfig sensorConfig)
-    : i2cConfig{sensorConfig.sa0 ? LPS28DFWDefs::lsp28dfwAddress1
-                                 : LPS28DFWDefs::lsp28dfwAddress0,
+    : i2cConfig{sensorConfig.sa0 ? lsp28dfwAddress1 : lsp28dfwAddress0,
                 I2CDriver::Addressing::BIT7, I2CDriver::Speed::MAX_SPEED},
       i2c(i2c), sensorConfig(sensorConfig)
 {
     pressureSensitivity = (sensorConfig.fsr == FullScaleRange::FS_1260
-                               ? LPS28DFWDefs::pressureSensitivity1260hPa
-                               : LPS28DFWDefs::pressureSensitivity4060hPa);
+                               ? pressureSensitivity1260hPa
+                               : pressureSensitivity4060hPa);
 }
 
 bool LPS28DFW::init()
@@ -48,14 +49,6 @@ bool LPS28DFW::init()
         return false;
     }
 
-    // Checking the whoami value to assure communication
-    if (!selfTest())
-    {
-        LOG_ERR(logger, "Self-test failed");
-        lastError = SELF_TEST_FAIL;
-        return false;
-    }
-
     // Setting the actual sensor configurations (Mode, ODR, AVG, FSR, DRDY)
     if (!setConfig(sensorConfig))
     {
@@ -69,52 +62,16 @@ bool LPS28DFW::init()
     return true;
 }
 
-bool LPS28DFW::setConfig(const SensorConfig& newSensorConfig)
-{
-    {
-        // Setting the mode and checking for consistency in the settings. If
-        // ONE_SHOT mode, other settings adapted to match this.
-        uint8_t fifo_ctrl{0};
-        switch (newSensorConfig.mode)
-        {
-            case Mode::ONE_SHOT_MODE:
-                sensorConfig = {newSensorConfig.sa0, newSensorConfig.fsr,
-                                newSensorConfig.avg, Mode::ONE_SHOT_MODE,
-                                ODR::ONE_SHOT,       false};
-                fifo_ctrl |= LPS28DFWDefs::FIFO_CTRL::BYPASS;
-                break;
-            case Mode::CONTINUOUS_MODE:
-                sensorConfig = newSensorConfig;
-                fifo_ctrl |= LPS28DFWDefs::FIFO_CTRL::CONTINUOUS;
-                break;
-            default:
-                LOG_ERR(logger, "Mode not supported");
-                break;
-        }
-
-        if (!i2c.writeRegister(i2cConfig, LPS28DFWDefs::FIFO_CTRL_addr,
-                               fifo_ctrl))
-        {
-            lastError = BUS_FAULT;
-            return false;
-        }
-    }
-
-    if (!(setFullScaleRange(sensorConfig.fsr) && setAverage(sensorConfig.avg) &&
-          setOutputDataRate(sensorConfig.odr) &&
-          setDRDYInterrupt(sensorConfig.drdy)))
-    {
-        LOG_ERR(logger, "Sensor not configured");
-        return false;
-    }
-
-    return true;
-}
-
 bool LPS28DFW::selfTest()
 {
     // Since the sensor doesn't provide any self-test feature we just try to
     // probe the sensor and read his whoami register.
+    if (!isInitialized)
+    {
+        LOG_ERR(logger, "Invoked selfTest() but sensor was uninitialized");
+        lastError = NOT_INIT;
+        return false;
+    }
 
     // Trying to probe the sensor to check if it is connected
     if (!i2c.probe(i2cConfig))
@@ -127,7 +84,7 @@ bool LPS28DFW::selfTest()
 
     // Reading the whoami value to assure communication
     uint8_t whoamiValue{0};
-    if (!i2c.readRegister(i2cConfig, LPS28DFWDefs::WHO_AM_I, whoamiValue))
+    if (!i2c.readRegister(i2cConfig, WHO_AM_I_addr, whoamiValue))
     {
         LOG_ERR(logger, "Can't communicate with the sensor");
         lastError = BUS_FAULT;
@@ -135,12 +92,12 @@ bool LPS28DFW::selfTest()
     }
 
     // Checking the whoami value to assure correct communication
-    if (whoamiValue != LPS28DFWDefs::WHO_AM_I_VALUE)
+    if (whoamiValue != WHO_AM_I_VALUE)
     {
         LOG_ERR(logger,
                 "WHO_AM_I value differs from expectation: read 0x{02x} "
                 "but expected 0x{02x}",
-                whoamiValue, LPS28DFWDefs::WHO_AM_I_VALUE);
+                whoamiValue, WHO_AM_I_VALUE);
         lastError = INVALID_WHOAMI;
         return false;
     }
@@ -148,14 +105,38 @@ bool LPS28DFW::selfTest()
     return true;
 }
 
+bool LPS28DFW::setConfig(const SensorConfig& newSensorConfig)
+{
+    // Setting the FIFO_CTRL register to the correct mode (according to the
+    // ODR set: BYPASS for the one shot mode and CONTINUOUS for the
+    // continuous mode).
+    if (!i2c.writeRegister(
+            i2cConfig, FIFO_CTRL_addr,
+            (newSensorConfig.odr == ODR::ONE_SHOT ? FIFO_CTRL::BYPASS
+                                                  : FIFO_CTRL::CONTINUOUS)))
+    {
+        lastError = BUS_FAULT;
+        return false;
+    }
+
+    if (!(setFullScaleRange(sensorConfig.fsr) && setAverage(sensorConfig.avg) &&
+          setOutputDataRate(sensorConfig.odr) &&
+          setDRDYInterrupt(sensorConfig.drdy)))
+    {
+        LOG_ERR(logger, "Sensor not configured");
+        return false;
+    }
+
+    return true;
+}
+
 bool LPS28DFW::setAverage(AVG avg)
 {
     // Since the CTRL_REG1 contains only the AVG and ODR settings, we use the
     // internal driver state to set the register with the wanted ODR and AVG
     // without previously reading it. This allows to avoid a useless
     // transaction.
-    if (!i2c.writeRegister(i2cConfig, LPS28DFWDefs::CTRL_REG1_addr,
-                           sensorConfig.odr | avg))
+    if (!i2c.writeRegister(i2cConfig, CTRL_REG1_addr, sensorConfig.odr | avg))
     {
         lastError = BUS_FAULT;
         return false;
@@ -171,8 +152,7 @@ bool LPS28DFW::setOutputDataRate(ODR odr)
     // internal driver state to set the register with the wanted ODR and AVG
     // without previously reading it. This allows to avoid a useless
     // transaction.
-    if (!i2c.writeRegister(i2cConfig, LPS28DFWDefs::CTRL_REG1_addr,
-                           odr | sensorConfig.avg))
+    if (!i2c.writeRegister(i2cConfig, CTRL_REG1_addr, odr | sensorConfig.avg))
     {
         lastError = BUS_FAULT;
         return false;
@@ -185,7 +165,7 @@ bool LPS28DFW::setOutputDataRate(ODR odr)
 bool LPS28DFW::setFullScaleRange(FullScaleRange fs)
 {
     uint8_t ctrl_reg2;
-    if (!i2c.readRegister(i2cConfig, LPS28DFWDefs::CTRL_REG2_addr, ctrl_reg2))
+    if (!i2c.readRegister(i2cConfig, CTRL_REG2_addr, ctrl_reg2))
     {
         lastError = BUS_FAULT;
         return false;
@@ -193,16 +173,16 @@ bool LPS28DFW::setFullScaleRange(FullScaleRange fs)
 
     if (fs == FullScaleRange::FS_1260)
     {
-        pressureSensitivity = LPS28DFWDefs::pressureSensitivity1260hPa;
-        ctrl_reg2           = (ctrl_reg2 & ~LPS28DFWDefs::CTRL_REG2::FS_MODE);
+        pressureSensitivity = pressureSensitivity1260hPa;
+        ctrl_reg2           = (ctrl_reg2 & ~CTRL_REG2::FS_MODE);
     }
     else
     {
-        pressureSensitivity = LPS28DFWDefs::pressureSensitivity4060hPa;
-        ctrl_reg2           = (ctrl_reg2 | LPS28DFWDefs::CTRL_REG2::FS_MODE);
+        pressureSensitivity = pressureSensitivity4060hPa;
+        ctrl_reg2           = (ctrl_reg2 | CTRL_REG2::FS_MODE);
     }
 
-    if (!i2c.writeRegister(i2cConfig, LPS28DFWDefs::CTRL_REG2_addr, ctrl_reg2))
+    if (!i2c.writeRegister(i2cConfig, CTRL_REG2_addr, ctrl_reg2))
     {
         lastError = BUS_FAULT;
         return false;
@@ -214,9 +194,8 @@ bool LPS28DFW::setFullScaleRange(FullScaleRange fs)
 
 bool LPS28DFW::setDRDYInterrupt(bool drdy)
 {
-    if (!i2c.writeRegister(
-            i2cConfig, LPS28DFWDefs::CTRL_REG4_addr,
-            (drdy ? (LPS28DFWDefs::INT_EN | LPS28DFWDefs::DRDY) : 0)))
+    if (!i2c.writeRegister(i2cConfig, CTRL_REG4_addr,
+                           (drdy ? (INT_EN | DRDY) : 0)))
     {
         lastError = BUS_FAULT;
         return false;
@@ -242,76 +221,85 @@ float LPS28DFW::convertTemperature(uint8_t tempL, uint8_t tempH)
 {
     // Temperature conversion
     int16_t temp_temp = (tempH << 8) | (tempL << 0);
-    return ((float)temp_temp) / LPS28DFWDefs::temperatureSensitivity;
+    return ((float)temp_temp) / temperatureSensitivity;
 }
 
 LPS28DFWData LPS28DFW::sampleImpl()
 {
-    uint8_t status = 0;
+    uint8_t statusValue{0};
     uint8_t val[5] = {0};
     LPS28DFWData data;
 
-    lastError              = NO_ERRORS;
-    data.pressureTimestamp = data.temperatureTimestamp =
-        TimestampTimer::getTimestamp();
+    lastError = NO_ERRORS;
 
-    if (sensorConfig.mode == Mode::ONE_SHOT_MODE)
+    if (sensorConfig.odr == ODR::ONE_SHOT)
     {
         uint8_t ctrl_reg2_val{0};
 
-        // Reading always 5 bytes because in one-shot mode the sensor samples
-        // both pressure and temperature
-        if (!(i2c.readRegister(i2cConfig, LPS28DFWDefs::CTRL_REG2_addr,
-                               ctrl_reg2_val) &&
-              i2c.writeRegister(
-                  i2cConfig, LPS28DFWDefs::CTRL_REG2_addr,
-                  ctrl_reg2_val | LPS28DFWDefs::CTRL_REG2::ONE_SHOT_START) &&
-              i2c.readFromRegister(i2cConfig, LPS28DFWDefs::PRESS_OUT_XL_addr,
-                                   val, 5)))
+        // Triggering sampling
+        if (!(i2c.readRegister(i2cConfig, CTRL_REG2_addr, ctrl_reg2_val) &&
+              i2c.writeRegister(i2cConfig, CTRL_REG2_addr,
+                                ctrl_reg2_val | CTRL_REG2::ONE_SHOT_START)))
         {
             lastError = BUS_FAULT;
             return lastSample;
         }
 
-        data.pressure    = convertPressure(val[0], val[1], val[2]);
-        data.temperature = convertTemperature(val[3], val[4]);
-
-        return data;
+        // Poll status register until the sample is ready
+        do
+        {
+            if (!i2c.readRegister(i2cConfig, STATUS_addr, statusValue))
+            {
+                lastError = BUS_FAULT;
+                return lastSample;
+            }
+        } while ((statusValue & (STATUS::P_DA | STATUS::T_DA)) !=
+                 (STATUS::P_DA | STATUS::T_DA));
     }
-
-    if (!i2c.readRegister(i2cConfig, LPS28DFWDefs::STATUS_addr, status))
+    else
     {
-        lastError = BUS_FAULT;
-        return lastSample;
+        // read status register value
+        if (!i2c.readRegister(i2cConfig, STATUS_addr, statusValue))
+        {
+            lastError = BUS_FAULT;
+            return lastSample;
+        }
     }
 
-    // If pressure new data present
-    if (!(status & LPS28DFWDefs::STATUS::P_DA))
-    {
-        lastError = NO_NEW_DATA;
-        return lastSample;
-    }
+    auto ts = TimestampTimer::getTimestamp();
 
     // reading 5 bytes if also Temperature new sample, otherwise only the 3
     // pressure bytes
-    if (!i2c.readFromRegister(i2cConfig, LPS28DFWDefs::PRESS_OUT_XL_addr, val,
-                              ((status & LPS28DFWDefs::STATUS::T_DA) ? 5 : 3)))
+    if (!i2c.readFromRegister(i2cConfig, PRESS_OUT_XL_addr, val,
+                              ((statusValue & STATUS::T_DA) ? 5 : 3)))
     {
         lastError = BUS_FAULT;
         return lastSample;
     }
 
-    data.pressure = convertPressure(val[0], val[1], val[2]);
+    // If pressure new data present
+    if (statusValue & STATUS::P_DA)
+    {
+        data.pressureTimestamp = ts;
+        data.pressure          = convertPressure(val[0], val[1], val[2]);
+    }
+    else
+    {
+        lastError              = NO_NEW_DATA;
+        data.pressureTimestamp = lastSample.pressureTimestamp;
+        data.pressure          = lastSample.pressure;
+    }
 
     // If temperature new data present
-    if (status & LPS28DFWDefs::STATUS::T_DA)
+    if (statusValue & STATUS::T_DA)
     {
-        data.temperature = convertTemperature(val[3], val[4]);
+        data.temperatureTimestamp = ts;
+        data.temperature          = convertTemperature(val[3], val[4]);
     }
     else
     {
-        data.temperature          = lastSample.temperature;
         data.temperatureTimestamp = lastSample.temperatureTimestamp;
+        data.temperature          = lastSample.temperature;
     }
 
     return data;
diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.h b/src/shared/sensors/LPS28DFW/LPS28DFW.h
index 1f52ef259dfe40cf1f93b97bd90d1e1c1fcfc4f2..33012760c5b6eeac46f921c3bee81ab5af94ccd0 100644
--- a/src/shared/sensors/LPS28DFW/LPS28DFW.h
+++ b/src/shared/sensors/LPS28DFW/LPS28DFW.h
@@ -48,9 +48,11 @@ public:
     };
 
     /**
-     * @brief Enumeration for the output data rate to set on the sensor.
+     * @brief Enumeration for Output Data Rate Configuration.
+     *
      * Available are One shot (only one sample calculated when signal sent), 1
-     * Hz to 200 Hz.
+     * Hz to 200 Hz. ODR configuration is valid for both pressure and
+     * temperature.
      */
     enum ODR : uint8_t
     {
@@ -66,9 +68,14 @@ public:
     };
 
     /**
-     * @brief Enumeration for the oversampling to set on the sensor. The value
-     * read from the sensor will actually be the average of multiple samples.
-     * Available are from 4 to 512 averaged samples.
+     * @brief Enumeration for the oversampling to set on the sensor.
+     *
+     * The value read from the sensor will actually be the average of multiple
+     * samples. Available are from 4 to 512 averaged samples.
+     *
+     * @warning For an AGV value of 512, 128, 64 the maximum ODR values are
+     * respectively of 25, 75 and 100 Hz. For any other AVG value all ODR
+     * configurations are possible.
      */
     enum AVG : uint8_t
     {
@@ -81,17 +88,6 @@ public:
         AVG_512 = 0b111
     };
 
-    /**
-     * @brief Working mode of the sensor. Available ones are one shot mode
-     * (samples acquired by the sensors only upon an explicit request) and
-     * continuous mode (samples calculated continuously).
-     */
-    enum Mode
-    {
-        ONE_SHOT_MODE,   // BYPASS, ODR = ONE_SHOT
-        CONTINUOUS_MODE  // BYPASS, ODR = odr
-    };
-
     /**
      * @brief Struct that sums up all the settings of the sensor.
      */
@@ -101,7 +97,6 @@ public:
                    ///< the sensor is connected to GND or VDD (3.3V, not 5V!).
         FullScaleRange fsr;  ///< Full scale range
         AVG avg;             ///< Average avg samples
-        Mode mode;           ///< Mode of operation
         ODR odr;             ///< Output data rate
         bool drdy;           ///< Enable Interrupt for Data Ready
     } SensorConfig;
@@ -118,6 +113,7 @@ public:
 
     /**
      * @brief Initializes the sensor with the current settings.
+     * @return true if initialization succeeded, false otherwise.
      */
     bool init() override;
 
@@ -130,7 +126,7 @@ public:
      * @brief The self test method returns true if we read the right whoami
      * value. We can't make a better self test due to the fact that the sensor
      * doesn't support this feature.
-     * @return true if the right whoami has been written.
+     * @return true if the right whoami has been read.
      */
     bool selfTest() override;
 
diff --git a/src/shared/sensors/LPS28DFW/LPS28DFWData.h b/src/shared/sensors/LPS28DFW/LPS28DFWData.h
index dd2732a05f07d2eb3b72b74f3959f0869511138f..9f3f38d22fe1328cf66a1121c80a294d68a28b52 100644
--- a/src/shared/sensors/LPS28DFW/LPS28DFWData.h
+++ b/src/shared/sensors/LPS28DFW/LPS28DFWData.h
@@ -33,6 +33,15 @@ namespace Boardcore
  */
 struct LPS28DFWData : public PressureData, TemperatureData
 {
+
+    LPS28DFWData() : PressureData{0, 0.0}, TemperatureData{0, 0.0} {}
+
+    LPS28DFWData(uint64_t pressT, float pressure, uint64_t tempT,
+                 float temperature)
+        : PressureData{pressT, pressure}, TemperatureData{tempT, temperature}
+    {
+    }
+
     static std::string header()
     {
         return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n ";
diff --git a/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h b/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h
index 0e194bbd289d976bcf2f23d77277027007cc8422..36287c7a88befcb22e10e1dd00f81c472ebaa4c4 100644
--- a/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h
+++ b/src/shared/sensors/LPS28DFW/LPS28DFWDefs.h
@@ -20,6 +20,8 @@
  * THE SOFTWARE.
  */
 
+#pragma once
+
 namespace Boardcore
 {
 
@@ -30,6 +32,7 @@ namespace LPS28DFWDefs
 {
 static const uint16_t lsp28dfwAddress0{0b1011100};
 static const uint16_t lsp28dfwAddress1{0b1011101};
+static const float WHO_AM_I_VALUE{0xB4};  ///< Device Who am I value
 static const float pressureSensitivity1260hPa{
     40.96};  ///< pressure sensitivity with FSR at 1260 hPa [LSB/Pa]
 static const float pressureSensitivity4060hPa{
@@ -37,16 +40,50 @@ static const float pressureSensitivity4060hPa{
 static const float temperatureSensitivity{
     100};  ///< temperature sensitivity [LSB/°C]
 
-static const uint8_t INTERRUPT_CFG{
-    0x0B};  ///< Interrupt mode for pressure acquisition configuration
-static const uint8_t THS_P_L{0x0C};   ///< User-defined threshold LSB register
-static const uint8_t THS_P_H{0x0D};   ///< User-defined threshold MSB register
-static const uint8_t IF_CTRL{0x0E};   ///< Interface control register
-static const uint8_t WHO_AM_I{0x0F};  ///< Device Who am I register
-static const uint8_t WHO_AM_I_VALUE{0xB4};  ///< Device Who am I value
-static const uint8_t CTRL_REG1_addr{0x10};  ///< Control Register 1 [ODR, AVG]
+enum Registers : uint8_t
+{
+    INTERRUPT_CFG_addr = 0x0B,  ///< Interrupt mode for pressure acquisition
+
+    THS_P_L_addr = 0x0C,  ///< User-defined threshold LSB register
+    THS_P_H_addr = 0x0D,  ///< User-defined threshold MSB register
+
+    IF_CTRL_addr = 0x0E,  ///< Interface control register
+
+    WHO_AM_I_addr = 0x0F,  ///< Device Who am I register
+
+    CTRL_REG1_addr = 0x10,  ///< Control Register 1 [ODR, AVG]
+    CTRL_REG2_addr = 0x11,  ///< Control Register 2
+    CTRL_REG3_addr = 0x12,  ///< Control Register 3
+    CTRL_REG4_addr = 0x13,  ///< Control Register 4
+
+    FIFO_CTRL_addr = 0x14,  ///< FIFO control register
+    FIFO_WTM_addr  = 0x15,  ///< FIFO threshold setting register
+
+    REF_P_L_addr = 0x16,  ///< Reference pressure LSB data
+    REF_P_H_addr = 0x17,  ///< Reference pressure MSB data
+
+    RPDS_L_addr = 0x1a,  ///< Pressure offset LSB data
+    RPDS_H_addr = 0x1b,  ///< Pressure offset HSB data
+
+    INT_SOURCE_addr = 0x24,  ///< Interrupt source register
+
+    FIFO_STATUS1_addr = 0x25,  ///< FIFO status register 1
+    FIFO_STATUS2_addr = 0x26,  ///< FIFO status register 2
+
+    STATUS_addr = 0x27,  ///< Status register
+
+    PRESS_OUT_XL_addr = 0x28,  ///< Pressure output value LSB data
+    PRESS_OUT_L_addr  = 0x29,  ///< Pressure output value middle data
+    PRESS_OUT_H_addr  = 0x2a,  ///< Pressure output value MSB data
+
+    TEMP_OUT_L_addr = 0x2b,  ///< Temperature output value LSB data
+    TEMP_OUT_H_addr = 0x2c,  ///< Temperature output value MSB data
+
+    FIFO_DATA_OUT_PRESS_XL_addr = 0x78,  ///< FIFO pressure output LSB data
+    FIFO_DATA_OUT_PRESS_L_addr  = 0x79,  ///< FIFO pressure output middle data
+    FIFO_DATA_OUT_PRESS_H_addr  = 0x7a,  ///< FIFO pressure output MSB data
+};
 
-static const uint8_t CTRL_REG2_addr{0x11};  ///< Control Register 2
 enum CTRL_REG2 : uint8_t
 {
     ONE_SHOT_START = (0b1 << 0),  ///< Enable one-shot mode
@@ -58,7 +95,6 @@ enum CTRL_REG2 : uint8_t
     BOOT           = (0b1 << 7)   ///< Reboot memory content
 };
 
-static const uint8_t CTRL_REG3_addr{0x12};  ///< Control Register 3
 enum CTRL_REG3 : uint8_t
 {
     IF_ADD_INC =
@@ -67,7 +103,6 @@ enum CTRL_REG3 : uint8_t
     INT_H_L = (0b1 << 3)   ///< Select interrupt active-high, active-low
 };
 
-static const uint8_t CTRL_REG4_addr{0x13};  ///< Control Register 4
 enum CTRL_REG4 : uint8_t
 {
     INT_F_OVR  = (0b1 << 0),  ///< FIFO overrun status on INT_DRDY pin
@@ -78,7 +113,6 @@ enum CTRL_REG4 : uint8_t
     DRDY_PLS   = (0b1 << 6)   ///< Data-ready pulsed on INT_DRDY pin
 };
 
-static const uint8_t FIFO_CTRL_addr{0x14};  ///< FIFO control register
 enum FIFO_CTRL : uint8_t
 {
     BYPASS               = 0b000,
@@ -90,12 +124,6 @@ enum FIFO_CTRL : uint8_t
     STOP_ON_WTM          = (0b1 << 3)  ///< Stop-on-FIFO watermark
 };
 
-static const uint8_t FIFO_WTM_addr{0x15};  ///< FIFO threshold setting register
-static const uint8_t REF_P_L_addr{0x16};   ///< Reference pressure LSB data
-static const uint8_t REF_P_H_addr{0x17};   ///< Reference pressure MSB data
-static const uint8_t RPDS_L_addr{0x1a};    ///< Pressure offset LSB data
-static const uint8_t RPDS_H_addr{0x1b};    ///< Pressure offset HSB data
-static const uint8_t INT_SOURCE_addr{0x24};  ///< Interrupt source register
 enum INT_SOURCE : uint8_t
 {
     PH      = (0b1 << 0),  ///< Differential pressure High
@@ -104,8 +132,6 @@ enum INT_SOURCE : uint8_t
     BOOT_ON = (0b1 << 7)   ///< Reboot phase is running
 };
 
-static const uint8_t FIFO_STATUS1_addr{0x25};  ///< FIFO status register 1
-static const uint8_t FIFO_STATUS2_addr{0x26};  ///< FIFO status register 2
 enum FIFO_STATUS2 : uint8_t
 {
     FIFO_FULL_IA = (0b1 << 5),  ///< FIFO full status
@@ -113,7 +139,6 @@ enum FIFO_STATUS2 : uint8_t
     FIFO_WTM_IA  = (0b1 << 7)   ///< FIFO threshold status
 };
 
-static const uint8_t STATUS_addr{0x27};  ///< Status register
 enum STATUS : uint8_t
 {
     P_DA = (0b1 << 0),  ///< Pressure data available
@@ -122,21 +147,5 @@ enum STATUS : uint8_t
     T_OR = (0b1 << 5)   ///< Temperature data overrun
 };
 
-static const uint8_t PRESS_OUT_XL_addr{
-    0x28};  ///< Pressure output value LSB data
-static const uint8_t PRESS_OUT_L_addr{
-    0x29};  ///< Pressure output value middle data
-static const uint8_t PRESS_OUT_H_addr{
-    0x2a};  ///< Pressure output value MSB data
-static const uint8_t TEMP_OUT_L_addr{
-    0x2b};  ///< Temperature output value LSB data
-static const uint8_t TEMP_OUT_H_addr{
-    0x2c};  ///< Temperature output value MSB data
-static const uint8_t FIFO_DATA_OUT_PRESS_XL_addr{
-    0x78};  ///< FIFO pressure output LSB data
-static const uint8_t FIFO_DATA_OUT_PRESS_L_addr{
-    0x79};  ///< FIFO pressure output middle data
-static const uint8_t FIFO_DATA_OUT_PRESS_H_addr{
-    0x7a};  ///< FIFO pressure output MSB data
 }  // namespace LPS28DFWDefs
 }  // namespace Boardcore
\ No newline at end of file
diff --git a/src/tests/sensors/test-lps28dfw.cpp b/src/tests/sensors/test-lps28dfw.cpp
index 594d9fe0ea7c7b62019bcc681ef2aec04b35d70c..c39bfd4431ea27075f5cf6c1817712e7d335ba61 100644
--- a/src/tests/sensors/test-lps28dfw.cpp
+++ b/src/tests/sensors/test-lps28dfw.cpp
@@ -62,12 +62,9 @@ void sampleOneShotMode(I2C &i2c)
     printf("Start One-Shot\n");
 
     // Setting up the Sensor
-    LPS28DFW::SensorConfig lps28dfwConfig{false,
-                                          LPS28DFW::FullScaleRange::FS_1260,
-                                          LPS28DFW::AVG::AVG_64,
-                                          LPS28DFW::Mode::ONE_SHOT_MODE,
-                                          LPS28DFW::ODR::ONE_SHOT,
-                                          false};
+    LPS28DFW::SensorConfig lps28dfwConfig{
+        false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64,
+        LPS28DFW::ODR::ONE_SHOT, false};
     LPS28DFW lps28dfw(i2c, lps28dfwConfig);
     if (!lps28dfw.init())
     {
@@ -98,12 +95,9 @@ void sampleContinuousMode(I2C &i2c)
     printf("Start Continuous\n");
 
     // Setting up the Sensor
-    LPS28DFW::SensorConfig lps28dfwConfig{false,
-                                          LPS28DFW::FullScaleRange::FS_1260,
-                                          LPS28DFW::AVG::AVG_64,
-                                          LPS28DFW::Mode::CONTINUOUS_MODE,
-                                          LPS28DFW::ODR::ODR_10,
-                                          false};
+    LPS28DFW::SensorConfig lps28dfwConfig{
+        false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64,
+        LPS28DFW::ODR::ODR_10, false};
     LPS28DFW lps28dfw(i2c, lps28dfwConfig);
     if (!lps28dfw.init())
     {
@@ -134,12 +128,9 @@ void sampleInterruptMode(I2C &i2c)
 {
     printf("Start Interrupt\n");
     // Setting up the Sensor
-    LPS28DFW::SensorConfig lps28dfwConfig{false,
-                                          LPS28DFW::FullScaleRange::FS_1260,
-                                          LPS28DFW::AVG::AVG_64,
-                                          LPS28DFW::Mode::CONTINUOUS_MODE,
-                                          LPS28DFW::ODR::ODR_10,
-                                          true};
+    LPS28DFW::SensorConfig lps28dfwConfig{
+        false, LPS28DFW::FullScaleRange::FS_1260, LPS28DFW::AVG::AVG_64,
+        LPS28DFW::ODR::ODR_10, true};
     LPS28DFW lps28dfw(i2c, lps28dfwConfig);
     if (!lps28dfw.init())
     {