diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
index 4980e5f80e02063f2baf4f14de730ef927d9d2b9..74d4ec8ac032f4b1c5cb213d88a98a251fb3b854 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
@@ -28,10 +28,9 @@ using miosix::GpioPin;
 using std::array;
 
 LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange,
-                         GyroFSR gyroRange, ODR odr, bool fifo_enabled,
-                         unsigned int fifo_watermark)
-    : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
-      spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr)
+                         GyroFSR gyroRange, ODR odr, uint8_t temp_div_freq)
+    : spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr),
+      temp_div_freq(temp_div_freq)
 {
     // SPI config
     spislave.config.clock_div = SPIClockDivider::DIV64;
@@ -39,20 +38,23 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange,
 
 LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
                          AxelFSR axelRange, GyroFSR gyroRange, ODR odr,
-                         bool fifo_enabled, unsigned int fifo_watermark)
-    : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
-      spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange),
-      odr(odr)
+                         uint8_t temp_div_freq)
+    : spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange),
+      odr(odr), temp_div_freq(temp_div_freq)
 {
 }
 
+void LSM9DS1_XLG::enable_fifo(uint8_t watermark)
+{
+    fifo_enabled   = true;
+    fifo_watermark = watermark;
+}
+
 bool LSM9DS1_XLG::init()
 {
-    if (sensor_initialized)
-    {
-        TRACE("[LSM9DS1 XLG] init() : already initialized\n");
-        return false;
-    }
+#ifdef DEBUG
+    assert(sensor_initialized == false);
+#endif
 
     SPITransaction spi(spislave);
 
@@ -71,6 +73,13 @@ bool LSM9DS1_XLG::init()
     // FIFO setup:
     if (fifo_enabled)
     {
+        // check fifo-watermark <= 32
+        if (fifo_watermark > 32)
+        {
+            TRACE("[LSM9DS1 XLG] init() : fifo watermark > 32, set to 32\n");
+            fifo_watermark = 32;
+        }
+
         // FIFO continous mode + fifo watermark threshold setup
         spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL | fifo_watermark));
 
@@ -91,13 +100,13 @@ bool LSM9DS1_XLG::init()
      * (max), LPF2/HPF bypassed and disabled, axel output enabled by default
      * @ startup
      */
-    uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3;
+    uint8_t CTRL_REG6_XL_VAL = odr << 5 | axelFSR << 3;
     spi.write(regMapXLG::CTRL_REG6_XL, CTRL_REG6_XL_VAL);
 
     /** Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and
      * disabled, gyro output enabled by default @ startup
      */
-    uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3;
+    uint8_t CTRL_REG1_G_VAL = odr << 5 | gyroFSR << 3;
     spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL);
 
     // sign and orientation Setup <--- BOARD DEPENDENT
@@ -106,20 +115,24 @@ bool LSM9DS1_XLG::init()
     // Check all the registers have been written correctly
     if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL)
     {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG8 readback failed\n");
         return false;
     }
     if (fifo_enabled)
     {
         if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark))
         {
+            TRACE("[LSM9DS1 XLG] init() : FIFO_CTRL readback failed\n");
             return false;
         }
         if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL)
         {
+            TRACE("[LSM9DS1 XLG] init() : INT1_CTRL readback failed\n");
             return false;
         }
         if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02))
         {
+            TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n");
             return false;
         }
     }
@@ -127,85 +140,23 @@ bool LSM9DS1_XLG::init()
     {
         if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL)
         {
+            TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n");
             return false;
         }
     }
     if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL)
     {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG6_XL readback failed\n");
         return false;
     }
     if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL)
     {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG1_G readback failed\n");
         return false;
     }
 
-    // select Sensitivity and ODR
-    switch (axelFSR)
-
-    {
-        case AxelFSR::FS_2:
-            axelSensitivity = 0.598f;
-            break;
-        case AxelFSR::FS_4:
-            axelSensitivity = 1.196f;
-            break;
-        case AxelFSR::FS_8:
-            axelSensitivity = 2.393f;
-            break;
-        case AxelFSR::FS_16:
-            axelSensitivity = 7.178f;
-            break;
-        default:
-            axelSensitivity = 0.598f;
-            break;
-    }
-
-    switch (gyroFSR)
-    {
-        case GyroFSR::FS_245:
-            gyroSensitivity = 8.75f;
-            break;
-        case GyroFSR::FS_500:
-            gyroSensitivity = 17.50f;
-            break;
-        case GyroFSR::FS_2000:
-            gyroSensitivity = 70.0f;
-            break;
-        default:
-            gyroSensitivity = 8.75f;
-            break;
-    }
-
-    switch (odr)
-    {
-        case ODR::PWR_DW:
-            odrHz = 0.0f;
-            break;
-        case ODR::ODR_15:
-            odrHz = 14.9f;
-            break;
-        case ODR::ODR_60:
-            odrHz = 59.5f;
-            break;
-        case ODR::ODR_119:
-            odrHz = 119.0f;
-            break;
-        case ODR::ODR_238:
-            odrHz = 238.0f;
-            break;
-        case ODR::ODR_476:
-            odrHz = 476.0f;
-            break;
-        case ODR::ODR_952:
-            odrHz = 952.0f;
-            break;
-        default:
-            odrHz = 14.9f;
-            break;
-    }
-
-    // discard first samples (see datasheet)
-    LSM9DS1_XLG::discardSamples(spi);
+    // clear FIFO and discard first samples
+    LSM9DS1_XLG::clearFIFO(spi);
 
     TRACE("[LSM9DS1 XLG] init() : done\n");
 
@@ -218,16 +169,20 @@ bool LSM9DS1_XLG::selfTest() { return true; }
 
 bool LSM9DS1_XLG::onSimpleUpdate()
 {
+// you have to call init() before
+#ifdef DEBUG
+    assert(sensor_initialized == true);
+#endif
+
     // if FIFO disabled
     if (!fifo_enabled)
     {
-        uint8_t data[12], tempData[2];
+        uint8_t data[12];
 
         // Read output axel+gyro raw data X,Y,Z and temp raw data
         {
             SPITransaction spi(spislave);
             spi.read(regMapXLG::OUT_X_L_G, data, 12);
-            spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
         }
 
         // compose signed 16-bit raw data as 2 bytes from the sensor
@@ -240,41 +195,49 @@ bool LSM9DS1_XLG::onSimpleUpdate()
         int16_t y_xl = data[8]  | data[9]  << 8;
         int16_t z_xl = data[10] | data[11] << 8;
 
-        int16_t temp = tempData[0] | tempData[1] << 8;
-
         //convert raw data
-        mLastAccel = Vec3(x_xl * axelSensitivity / 1000, 
-                          y_xl * axelSensitivity / 1000,
-                          z_xl * axelSensitivity / 1000);
-
-        mLastGyro =  Vec3(x_gy * gyroSensitivity / 1000, 
-                          y_gy * gyroSensitivity / 1000,
-                          z_gy * gyroSensitivity / 1000);
-
-        mLastTemp = tempZero + (temp / tempSensistivity);
-
+        fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), 
+                                y_xl * axelFSR_SMap.at(axelFSR),
+                                z_xl * axelFSR_SMap.at(axelFSR));
+
+        fifo[0].gyroData = Vec3(x_gy * gyroFSR_SMap.at(gyroFSR), 
+                                y_gy * gyroFSR_SMap.at(gyroFSR),
+                                z_gy * gyroFSR_SMap.at(gyroFSR));
+        
+        fifo[0].timestamp = IRQ_timestamp;
         // clang-format on
     }
 
     /** if FIFO enabled:
      * dump fifo_watermark samples (axel+gyro only) from the sensor at one
-     * time. Temperature data can be read using temperatureUpdate() function
-     * at a lower frequency
+     * time.
      */
     else
     {
         // 2 bytes per data * 3 axes per type * 2 types(axel+gyro) *
         // 32(FIFO DEPTH MAX) = 384 samples
         uint8_t buf[384];
+        uint8_t overrun     = 0;
 
         // Read output axel+gyro FIFO raw data X,Y,Z
         {
             SPITransaction spi(spislave);
-            spi.read(OUT_X_L_G, buf, fifo_watermark * 12);
+
+            uint8_t fifo_src_reg;
+            spi.read(FIFO_SRC, &fifo_src_reg, 1);
+
+            fifo_samples = fifo_src_reg & FIFO_UNREAD_MASK;
+            overrun     = (fifo_src_reg & FIFO_OVERRUN_MASK) >> 6;
+
+            spi.read(OUT_X_L_G, buf, fifo_samples * 12);
         }
 
+        // compute delta time for each sample
+        uint64_t dt = delta / last_fifo_level;
+        fifo_num++;
+
         // convert & store
-        for (int i = 0; i < fifo_watermark; i++)
+        for (uint8_t i = 0; i < fifo_samples; ++i)
         {
             // compose signed 16-bit raw data as 2 bytes from the sensor
             // clang-format off
@@ -287,18 +250,39 @@ bool LSM9DS1_XLG::onSimpleUpdate()
             int16_t z_xl = buf[i * 12 + 10] | buf[i * 12 + 11] << 8;
 
             //convert raw data
-            fifo[i].gyroData = Vec3(x_gy * gyroSensitivity / 1000,
-                                    y_gy * gyroSensitivity / 1000,
-                                    z_gy * gyroSensitivity / 1000);
-            fifo[i].axelData = Vec3(x_xl * axelSensitivity / 1000,
-                                    y_xl * axelSensitivity / 1000,
-                                    z_xl * axelSensitivity / 1000);
+            fifo[i].gyroData = Vec3(x_gy * gyroFSR_SMap.at(gyroFSR),
+                                    y_gy * gyroFSR_SMap.at(gyroFSR),
+                                    z_gy * gyroFSR_SMap.at(gyroFSR));
+            fifo[i].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR),
+                                    y_xl * axelFSR_SMap.at(axelFSR),
+                                    z_xl * axelFSR_SMap.at(axelFSR));
             // clang-format on
+
+            fifo[i].timestamp = IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt;
+            fifo[i].unread    = fifo_samples;
+            fifo[i].overrun   = (bool)overrun;
+            fifo[i].fifo_num  = (uint16_t)i;
         }
+        last_fifo_level = fifo_samples;
     }
+
+    // temperature update if temp_count = temp_div_freq
+    temp_count++;
+    if (temp_count == temp_div_freq)
+    {
+        temp_count = 0;
+        LSM9DS1_XLG::temperatureUpdate();
+    }
+
     return true;
 }
 
+void LSM9DS1_XLG::updateTimestamp(uint64_t timestamp)
+{
+    delta         = timestamp - IRQ_timestamp;
+    IRQ_timestamp = timestamp;
+}
+
 bool LSM9DS1_XLG::temperatureUpdate()
 {
     // Read output temp raw data
@@ -310,22 +294,28 @@ bool LSM9DS1_XLG::temperatureUpdate()
     // compose signed 16-bit raw data as 2 bytes from the sensor
     int16_t temp = tempData[0] | tempData[1] << 8;
     // convert raw data
-    mLastTemp = tempZero + temp / tempSensistivity;
+    lastTemp.tempData  = tempZero + temp / tempSensistivity;
+    lastTemp.timestamp = IRQ_timestamp;
     return true;
 }
 
-void LSM9DS1_XLG::clearFIFO()
+const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getFIFO() const { return fifo; }
+
+const uint8_t& LSM9DS1_XLG::getFIFOdepth() const { return fifo_samples; }
+
+const lsm9ds1XLGSample& LSM9DS1_XLG::getXLGSample() const { return fifo[0]; }
+
+const lsm9ds1TSample& LSM9DS1_XLG::getTSample() const { return lastTemp; }
+
+void LSM9DS1_XLG::clearFIFO(SPITransaction& spi)
 {
-    SPITransaction spi(spislave);
     spi.write(FIFO_CTRL, 0);                               // Bypass Mode
-    miosix::Thread::sleep(20);                             // Wait
     spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark);  // re-enable FIFO
-    LSM9DS1_XLG::discardSamples(spi);
-}
 
-const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getLsm9ds1FIFO() const
-{
-    return fifo;
+    // sleep 20ms for stable output
+    miosix::Thread::sleep(20);
+
+    LSM9DS1_XLG::discardSamples(spi);
 }
 
 void LSM9DS1_XLG::discardSamples(SPITransaction& spi)
@@ -334,14 +324,14 @@ void LSM9DS1_XLG::discardSamples(SPITransaction& spi)
     if (odr != ODR::PWR_DW)
     {
         // wait samples to be overwritten or stored (FIFO on)
-        uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
+        uint16_t toWait_ms = SAMPLES_TO_DISCARD * 1000 / odr_Map.at(odr);
         miosix::Thread::sleep(toWait_ms);
 
-        // if FIFO is enabled, read first "samplesToDiscard" samples and
+        // if FIFO is enabled, read first "SAMPLES_TO_DISCARD" samples and
         // discard them.
         if (fifo_enabled)
         {
-            spi.read(OUT_X_L_G, samplesToDiscard * 12);
+            spi.read(OUT_X_L_G, SAMPLES_TO_DISCARD * 12);
         }
     }
 }
\ No newline at end of file
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index 2be91afcc0b020e9d0fc4aec99af2aa1a5f13174..43bed351236de6c8d83e7cd863dea6c1813c5227 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -45,7 +45,7 @@ class LSM9DS1_XLG : public GyroSensor,
                     public TemperatureSensor
 {
 public:
-    enum AxelFSR
+    enum AxelFSR : uint8_t
     {
         FS_2  = 0x00,  // +/- 2g
         FS_16 = 0x01,  // +/- 16g
@@ -53,14 +53,14 @@ public:
         FS_8  = 0x03   // +/- 8g
     };
 
-    enum GyroFSR
+    enum GyroFSR : uint8_t
     {
         FS_245  = 0x00,  // +/- 245dps
         FS_500  = 0x01,  // +/- 500dps
         FS_2000 = 0x03   // +/- 2000dps
     };
 
-    enum ODR
+    enum ODR : uint8_t
     {
         PWR_DW  = 0X00,  // power down
         ODR_15  = 0X01,  // 15Hz
@@ -71,6 +71,30 @@ public:
         ODR_952 = 0X06   // 952Hz
     };
 
+    // clang-format off
+
+    //Sesitivity Map (axelFSR)
+    const std::map<AxelFSR, float> axelFSR_SMap{{FS_2,  0.000598f},
+                                                {FS_4,  0.001196f},
+                                                {FS_8,  0.002393f},
+                                                {FS_16, 0.007178f}};
+    
+    //Sesitivity Map (gyroFSR)
+    const std::map<GyroFSR, float> gyroFSR_SMap{{FS_245,  0.0001527f},
+                                                {FS_500,  0.0003054f},
+                                                {FS_2000, 0.0012217f}};
+    
+    //ODR Map
+    const std::map<ODR, float> odr_Map{{PWR_DW,  0.0f  },
+                                       {ODR_15,  14.9f },
+                                       {ODR_60,  59.5f },
+                                       {ODR_119, 119.0f},
+                                       {ODR_238, 238.0f},
+                                       {ODR_476, 476.0f},
+                                       {ODR_952, 952.0f}};
+
+    // clang-format on
+
     /**
      * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
      * sensor.
@@ -80,15 +104,13 @@ public:
      * @param    axelRange accelerometer Full Scale Range (See datasheet)
      * @param    gyroRange gyroscope Full Scale Range (See datasheet)
      * @param    odr Output Data Rate (See datasheet)
-     * @param    fifo_enabled Fifo enabled
-     * @param    fifo_watermark FIFO watermark level in range [1,32] (used for
-     * interrupt generation, see datasheet).
+     * @param    temp_div_freq Temperature update frequency division
      */
 
     LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
-                AxelFSR axelRange = AxelFSR::FS_2,
-                GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
-                bool fifo_enabled = false, unsigned int fifo_watermark = 24);
+                AxelFSR axelRange = AxelFSR::FS_16,
+                GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952,
+                uint8_t temp_div_freq = 10);
 
     /**
      * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
@@ -100,15 +122,22 @@ public:
      * @param    axelRange accelerometer Full Scale Range (See datasheet)
      * @param    gyroRange gyroscope Full Scale Range (See datasheet)
      * @param    odr Output Data Rate (See datasheet)
-     * @param    fifo_enabled Fifo enabled
-     * @param    fifo_watermark FIFO watermark level in range [1,32] (used for
-     * interrupt generation, see datasheet).
+     * @param    temp_div_freq Temperature update frequency division
      */
 
     LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-                AxelFSR axelRange = AxelFSR::FS_2,
-                GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
-                bool fifo_enabled = false, unsigned int fifo_watermark = 24);
+                AxelFSR axelRange = AxelFSR::FS_16,
+                GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952,
+                uint8_t temp_div_freq = 10);
+
+    /**
+     * @brief enables LSM9DS1 embedded FIFO.
+     * @param fifo_watermark FIFO watermark level in range [1,32] (used for
+     * interrupt generation, see datasheet).
+     * @warning call before init() member
+     */
+
+    void enable_fifo(uint8_t watermark);
 
     /**
      * @brief initializes the LSM9DS1 Sensor (Accelerometer + Gyroscope).
@@ -127,7 +156,9 @@ public:
     bool selfTest() override;
 
     /**
-     * @brief Dump single reading of Axel+Gyro+Temp from the sensor through SPI.
+     * @brief Dump single reading of Axel+Gyro+Temp from the sensor
+     * (if FIFO disabled) or dump fifo_watermark samples from the FIFO (if FIFO
+     * enabled) through SPI.
      * @return true if sensor sends data
      * @warning if FIFO is enabled, call only after interrupt flag from the
      * sensor has been set
@@ -136,19 +167,25 @@ public:
     bool onSimpleUpdate() override;
 
     /**
-     * @brief Dump single read of Temperature from the sensor through SPI.
-     * @return true if sensor sends data
+     * @brief set timestamp on last FIFO
+     * @warning remember to update FIFO data calling onSimpleUpdate
      */
 
-    bool temperatureUpdate();
+    void updateTimestamp(uint64_t timestamp);
 
     /**
-     * @brief Clear the FIFO register inside the LSM9DS1 sensor.
-     * In order to perform a "clear", FIFO is disabled and then
-     * re-enabled; after that some samples are discarded according to datasheet.
+     * @brief get last valid sample
+     * @return sample
+     */
+
+    const lsm9ds1XLGSample& getXLGSample() const;
+
+    /**
+     * @brief get last valid sample
+     * @return sample
      */
 
-    void clearFIFO();
+    const lsm9ds1TSample& getTSample() const;
 
     /**
      * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO
@@ -156,9 +193,30 @@ public:
      * @return array containing the whole FIFO
      */
 
-    const array<lsm9ds1XLGSample, 32>& getLsm9ds1FIFO() const;
+    const array<lsm9ds1XLGSample, 32>& getFIFO() const;
+
+    /**
+     * @brief get number of samples inside the last FIFO
+     * @return number of samples
+     */
+
+    const uint8_t& getFIFOdepth() const; 
 
 private:
+    /**
+     * @brief Dump single read of Temperature from the sensor through SPI.
+     * @return true if sensor sends data
+     */
+
+    bool temperatureUpdate();
+
+    /**
+     * @brief Clear the FIFO register inside the LSM9DS1 sensor.
+     * In order to perform a "clear", FIFO is disabled and then
+     * re-enabled; after that some samples are discarded according to datasheet.
+     */
+
+    void clearFIFO(SPITransaction& spi);
     /**
      * @brief discard some samples from the sensor. Must be used when switching.
      * from FIFO mode to CONTINUOUS mode (or viceversa) and during power on
@@ -167,9 +225,18 @@ private:
     void discardSamples(SPITransaction& spi);
 
     bool sensor_initialized = false;
-    bool fifo_enabled;
+
+    bool fifo_enabled = false;
     uint8_t fifo_watermark;
+    uint8_t fifo_samples;
+    uint8_t last_fifo_level = 20;
+
+    //
+    uint64_t IRQ_timestamp = 0;
+    uint32_t delta         = 0;
+
     array<lsm9ds1XLGSample, 32> fifo;
+    lsm9ds1TSample lastTemp;
 
     SPISlave spislave;
 
@@ -177,12 +244,10 @@ private:
     GyroFSR gyroFSR;
     ODR odr;
 
-    float axelSensitivity;
-    float gyroSensitivity;
-    float odrHz;
-    float tempZero                        = 25.0f;
-    float tempSensistivity                = 16.0f;
-    static const uint8_t samplesToDiscard = 8;  // max possible val
+    float tempZero         = 25.0f;
+    float tempSensistivity = 16.0f;
+    uint8_t temp_div_freq;
+    uint8_t temp_count = 0;
 
     /**
      * @brief Registers' addresses definition.
@@ -241,9 +306,14 @@ private:
         INT_GEN_DUR_G    = 0x37
     };
 
-    static const uint8_t INT1_CTRL_VAL    = 0x08;
-    static const uint8_t WHO_AM_I_XLG_VAL = 0x68;
-    static const uint8_t CTRL_REG8_VAL    = 0x04;
-    static const uint8_t CTRL_REG9_VAL    = 0x04;
-    static const uint8_t FIFO_CTRL_VAL    = 0xC0;
+    static const uint8_t INT1_CTRL_VAL      = 0x08;
+    static const uint8_t WHO_AM_I_XLG_VAL   = 0x68;
+    static const uint8_t CTRL_REG8_VAL      = 0x04;
+    static const uint8_t CTRL_REG9_VAL      = 0x04;
+    static const uint8_t FIFO_CTRL_VAL      = 0xC0;
+    static const uint8_t FIFO_UNREAD_MASK   = 0x3F;
+    static const uint8_t FIFO_OVERRUN_MASK  = 0x40;
+    static const uint8_t SAMPLES_TO_DISCARD = 8;
+
+    uint16_t fifo_num = 0;
 };
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
index 0d4f83aaa890065309013809136f18f5dd4fd0af..e07edc11fc3d05bbb05706cbd69351131722cd68 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
@@ -40,16 +40,21 @@ struct lsm9ds1XLGSample
     uint64_t timestamp;
     Vec3 axelData;
     Vec3 gyroData;
+    uint16_t unread;
+    uint16_t fifo_num;
+    bool overrun;
 
     static std::string header()
     {
-        return "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z\n";
+        return "timestamp,unread,overrun,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_"
+               "z\n";
     }
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << axelData.getX() << "," << axelData.getY()
-           << "," << axelData.getZ() << "," << gyroData.getX() << ","
+        os << timestamp << "," << unread << "," << overrun << "," << fifo_num
+           << "," << axelData.getX() << "," << axelData.getY() << ","
+           << axelData.getZ() << "," << gyroData.getX() << ","
            << gyroData.getY() << "," << gyroData.getZ() << "\n";
     }
 };
@@ -81,4 +86,4 @@ struct lsm9ds1MSample
         os << timestamp << "," << magData.getX() << "," << magData.getY() << ","
            << magData.getZ() << "\n";
     }
-};
+};
\ No newline at end of file
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
index 743a26acc4044015518ab7d559e2926d6f0279d6..828963d182a0d8d3e06269f38b9f42f5d7c7a44a 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
@@ -128,17 +128,31 @@ bool LSM9DS1_M::selfTest()
     Stats stX, stY, stZ;
     bool selfTestResult = false;
 
+    nostX.reset();
+    nostY.reset();
+    nostZ.reset();
+
     selfTest_mode = true;
 
+    // if already initialized
+    if (sensor_initialized == true)
+    {
+        TRACE("[LSM9DS1 MAG] selfTest() : sensor already initialized!\n");
+#ifdef DEBUG
+        assert(sensor_initialized == false);
+#endif
+        return false;
+    }
+
     {
         SPITransaction spi(spislave);
 
-        // Reset all registers (if called after init())
+        // Reset all registers
         spi.write(regMapM::CTRL_REG2_M, SOFT_RESET);
         miosix::Thread::sleep(20);
 
         // self-test ROUTINE of LIS3MDL - seems to be same magnetometer as
-        // LSM9DS1 init sensor for self-test: FSR = +/-12 Gauss, ODR = 80Hz
+        // LSM9DS1. init sensor for self-test: FSR = +/-12 Gauss, ODR = 80Hz
         uint8_t CTRL_REG1_M_VAL = (ODR::ODR_80 << 2);
         spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL);
 
@@ -156,7 +170,9 @@ bool LSM9DS1_M::selfTest()
         {
         }
 
-        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6);
+        uint8_t data[6];
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, data, 6);
+        UNUSED(data);
     }
 
     // wait data-ready bit on STATUS REG - read NOST sample at least 5 times
@@ -176,7 +192,9 @@ bool LSM9DS1_M::selfTest()
         {
         }
 
+        uint8_t data[6];
         spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6);
+        UNUSED(data);
     }
 
     LSM9DS1_M::getSelfTestData(stX, stY, stZ);
@@ -185,17 +203,18 @@ bool LSM9DS1_M::selfTest()
     float deltaY = fabsf(stY.getStats().mean - nostY.getStats().mean);
     float deltaZ = fabsf(stZ.getStats().mean - nostZ.getStats().mean);
 
-// verify if sensor is inside parameters
+    // verify if sensor is inside parameters
 
-// print stats
+    // print stats
 #ifdef DEBUG
-    std::cout << "[LSM9DS1 MAG] selfTest() :" << std::endl
-              << "X-AXIS stats : " << nostX.getStats() << std::endl
-              << "Y-AXIS stats : " << nostY.getStats() << std::endl
-              << "Z-AXIS stats : " << nostZ.getStats() << std::endl
-              << "deltaX : " << deltaX << std::endl
-              << "deltaY : " << deltaY << std::endl
-              << "deltaZ : " << deltaZ << std::endl;
+    std::cout << "[LSM9DS1 MAG] selfTest() : statistics"
+              << "\n"
+              << "X-AXIS stats : " << nostX.getStats() << "\n"
+              << "Y-AXIS stats : " << nostY.getStats() << "\n"
+              << "Z-AXIS stats : " << nostZ.getStats() << "\n"
+              << "deltaX : " << deltaX << "\n"
+              << "deltaY : " << deltaY << "\n"
+              << "deltaZ : " << deltaZ << "\n";
 #endif
 
     // clang-format off
@@ -214,15 +233,12 @@ bool LSM9DS1_M::selfTest()
     }
 
     // clang-format on 
-
-    // re-init if necessary 
-    if(sensor_initialized == true)
     {
-        sensor_initialized = false;
-        LSM9DS1_M::init(); 
+        SPITransaction spi(spislave);
+        spi.write(regMapM::CTRL_REG2_M, SOFT_RESET);
+        miosix::Thread::sleep(20);
     }
-
-    selfTest_mode = true;    
+    selfTest_mode = false;    
     
     return selfTestResult;
 }
@@ -240,15 +256,11 @@ void LSM9DS1_M::getSelfTestData(Stats& outxStats, Stats& outyStats,
         }
         LSM9DS1_M::onSimpleUpdate();
 
-        printf("%.3f,%.3f,%.3f\n", lastMagneto.magData.getX(), 
-                                   lastMagneto.magData.getY(), 
-                                   lastMagneto.magData.getZ());
         // compute statistics
         outxStats.add(lastMagneto.magData.getX());
         outyStats.add(lastMagneto.magData.getY());
         outzStats.add(lastMagneto.magData.getZ());
     }
-    printf("\n\n\n");
 }
 
 bool LSM9DS1_M::onSimpleUpdate()
@@ -263,9 +275,10 @@ bool LSM9DS1_M::onSimpleUpdate()
     {
         SPITransaction spi(spislave);
         spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6);
-        lastMagneto.timestamp = miosix::getTick();
     }
 
+    lastMagneto.timestamp = miosix::getTick();
+
     // compose signed 16-bit raw data as 2 bytes from the sensor
     // clang-format off
     int16_t x = magData[0] | magData[1] << 8;
@@ -279,4 +292,6 @@ bool LSM9DS1_M::onSimpleUpdate()
     // clang-format on
 
     return true;
-}
\ No newline at end of file
+}
+
+const lsm9ds1MSample& LSM9DS1_M::getSample() const { return lastMagneto; }
\ No newline at end of file
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
index f53638fa9593904da9805c7e7b66a52b5b9ddc79..8dd422f6be2a789704526664901b93157a43f008 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
@@ -105,6 +105,7 @@ public:
     /**
      * @brief Run a self-test of the Sensor.
      * @return true if sensor behave correclty
+     * @warning blocking.
      */
 
     bool selfTest() override;
@@ -115,8 +116,20 @@ public:
      */
 
     bool onSimpleUpdate() override;
+    
+    /**
+     * @brief get last valid sample
+     * @return sample
+     */
+    
+    const lsm9ds1MSample& getSample() const; 
 
 private:
+
+    /**
+     * @brief get data from sensor and compute stats.
+     */
+
     void getSelfTestData(Stats& outx, Stats& outy, Stats& outz);
 
     bool sensor_initialized = false;
diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp
index 25b84447edee848e7db21bcf73ee4f7d09b1f660..8cc6f6e3b7c29a406a50f1256faaabdcec0e2b02 100644
--- a/src/tests/drivers/test-lsm9ds1-fifo.cpp
+++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp
@@ -55,14 +55,14 @@ GpioPin LED3(GPIOD_BASE, 14);
 GpioPin PUSHBUTTON(GPIOA_BASE, 0);
 
 // SPI read flag
-volatile bool flagSPIReadRequest = false;
+//volatile bool flagSPIReadRequest = false;
 
 // IMU obj variables
-static const bool FIFO_ENABLED             = true;
-static const uint8_t FIFO_WATERMARK        = 20;
-static const uint16_t FIFO_SAMPLES         = 1000;
-static const uint16_t MAG_SAMPLING_PERIOD  = 10;   // 100Hz
-static const uint16_t TEMP_SAMPLING_PERIOD = 100;  // 10Hz
+static const uint8_t FIFO_WATERMARK       = 12;
+static const uint16_t FIFO_SAMPLES        = 1000;
+static const uint8_t TEMP_DIV_FREQ        = 20;
+static const uint16_t MAG_SAMPLING_PERIOD = 10;  // 100Hz
+static const uint16_t FIFO_SAMPLING_PERIOD = 25.5 * 1000 / 476.0f;
 
 // High Resolution hardware timer using TIM5
 HardwareTimer<uint32_t> hrclock(
@@ -86,13 +86,11 @@ void __attribute__((naked)) EXTI13_IRQHandler()
 
 void __attribute__((used)) EXTI13_IRQHandlerImpl()
 {
-    // Computing delta beetween interrupts
-    uint32_t tick = hrclock.tick();
-    delta         = tick - last_tick;
-    last_tick     = tick;
+    // Update timestamp
+    lsm9ds1_xlg->updateTimestamp(hrclock.toMicroSeconds(hrclock.tick()));
 
     // Set read flag
-    flagSPIReadRequest = true;
+    //flagSPIReadRequest = true;
 
     // Built-in LED on
     LED1.high();
@@ -109,10 +107,9 @@ void printStats(void*);
 int main()
 {
 
-    uint32_t dt;
-    uint64_t XLGtimestamp = 0;
-    uint64_t lastMagtick  = 0;
-    uint64_t lastTemptick = 0;
+    uint64_t lastMagtick   = 0;
+    uint64_t lastFifotick  = 0;
+    uint16_t lastTempcount = 0;
 
     // Spawn thread for loggings logger stats
     Thread::create(printStats, 4096);
@@ -125,88 +122,84 @@ int main()
     timer5Config();
     EXTI1Config();
 
-    lsm9ds1_xlg = new LSM9DS1_XLG(
-        bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, LSM9DS1_XLG::GyroFSR::FS_245,
-        LSM9DS1_XLG::ODR::ODR_238, FIFO_ENABLED, FIFO_WATERMARK);
+    lsm9ds1_xlg = new LSM9DS1_XLG(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
+                                  LSM9DS1_XLG::GyroFSR::FS_245,
+                                  LSM9DS1_XLG::ODR::ODR_476, TEMP_DIV_FREQ);
 
     lsm9ds1_m = new LSM9DS1_M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8,
-                              LSM9DS1_M::ODR::ODR_40);
+                              LSM9DS1_M::ODR::ODR_80);
 
+    // enable FIFO
+    lsm9ds1_xlg->enable_fifo(FIFO_WATERMARK);
+
+    // perform self-tests
+    lsm9ds1_xlg->selfTest();
+    lsm9ds1_m->selfTest();
+
+    // initialize sensor
     while (!lsm9ds1_xlg->init())
         ;
     while (!lsm9ds1_m->init())
         ;
     LED2.high();  // init OK
 
-    // just to be sure to intercept the first interrupt rising edge
-    lsm9ds1_xlg->clearFIFO();
-
     // sampling until you push the button
     while (!PUSHBUTTON.value())
     {
-        //ACCELEROMETER + GYROSCOPE UPDATE (FIFO)
-        //an interrupt is set: time to dump the FIFO
-        if (flagSPIReadRequest)
+        // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO)
+        // TEMPERATURE UPDATE
+        // an interrupt is set: time to dump the FIFO
+        //if (flagSPIReadRequest)
+        if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD)
         {
-            flagSPIReadRequest = false;
-
-            // delta of each sample = delta beetween interrupts / #samples 
-            dt = hrclock.toMicroSeconds(delta) / FIFO_WATERMARK;
-            
-            //dump the fifo 
+            //flagSPIReadRequest = false;
+            lastFifotick = miosix::getTick();
+            // dump the fifo
             lsm9ds1_xlg->onSimpleUpdate();
 
-            //update each timestamp and log the sample (da integrare su driver?)
-            for (int i = 0; i < FIFO_WATERMARK; i++)
+            // log each sample
+            for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth() ; i++)
             {
-                lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getLsm9ds1FIFO()[i];
-                XLGtimestamp += dt;
-                XLGsample.timestamp = XLGtimestamp;
+                lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getFIFO()[i];
                 logger.log(XLGsample);
             }
 
+            if (lastTempcount == TEMP_DIV_FREQ)
+            {
+                lastTempcount          = 0;
+                lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample();
+                logger.log(Tsample);
+            }
+            lastTempcount++;
+
             LED1.low();
         }
 
-        //MAGNETOMETER UPDATE (SIMPLE)
+        // MAGNETOMETER UPDATE (SIMPLE)
         if (miosix::getTick() - lastMagtick >= MAG_SAMPLING_PERIOD)
-        {   
+        {
             lastMagtick = miosix::getTick();
-            
-            //get sample from the sensor
+
+            // get sample from the sensor
             lsm9ds1_m->onSimpleUpdate();
-            
-            //update timestamp and log the sample
-            lsm9ds1MSample MAGsample;
-            MAGsample.magData   = *(lsm9ds1_m->compassDataPtr());
-            MAGsample.timestamp = lastMagtick;
+
+            // log the sample
+            lsm9ds1MSample MAGsample = lsm9ds1_m->getSample();
             logger.log(MAGsample);
         }
 
-        //TEMPERATURE UPDATE (SIMPLE)
-        if (miosix::getTick() - lastTemptick >= TEMP_SAMPLING_PERIOD)
-        {
-            lastTemptick = miosix::getTick();
-
-            //get sample from the sensor
-            lsm9ds1_xlg->temperatureUpdate();
-            
-            //update timestamp and log the sample
-            lsm9ds1TSample Tsample;
-            Tsample.tempData  = *(lsm9ds1_xlg->tempDataPtr());
-            Tsample.timestamp = lastTemptick;
-            logger.log(Tsample);
-        }
+        Thread::sleep(FIFO_SAMPLING_PERIOD);
+
     }
 
-    //stop log
+    // stop log
     logger.stop();
 
     LED1.low();
     LED2.low();
 
     Thread::sleep(10000);
-    reboot();
+    miosix::reboot();
 
     while (1)
         ;
diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp
index c93b66ea01449ed8db8fb14178539b45486fc6a3..ad7e59812370c890b0a5f0d4131e1d7e59563e06 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -33,7 +33,8 @@ typedef Gpio<GPIOA_BASE, 5> GpioSck;
 typedef Gpio<GPIOA_BASE, 6> GpioMiso;
 typedef Gpio<GPIOA_BASE, 7> GpioMosi;
 
-static const bool FIFO_ENABLED = false;
+static const bool FIFO_ENABLED     = false;
+static const uint8_t TEMP_DIV_FREQ = 20;
 
 // SPI
 SPIBus bus(SPI1);
@@ -46,9 +47,9 @@ GpioPin LED2(GPIOD_BASE, 13);
 
 int main()
 {
-
-    Vec3 adata, gdata, mdata;
-    float tdata;
+    lsm9ds1XLGSample agdata;
+    lsm9ds1MSample mdata;
+    lsm9ds1TSample tdata;
 
     {
         FastInterruptDisableLock dLock;
@@ -79,11 +80,13 @@ int main()
 
     LSM9DS1_XLG lsm9ds1X(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
                          LSM9DS1_XLG::GyroFSR::FS_245,
-                         LSM9DS1_XLG::ODR::ODR_952);
+                         LSM9DS1_XLG::ODR::ODR_952, TEMP_DIV_FREQ);
 
-    LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8,
-                       LSM9DS1_M::ODR::ODR_20);
+    LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_12,
+                       LSM9DS1_M::ODR::ODR_80);
 
+    lsm9ds1M.selfTest();
+    
     while (!lsm9ds1X.init())
     {
     }
@@ -94,33 +97,32 @@ int main()
     }
     LED2.high();
 
-    Thread::sleep(500);
+    Thread::sleep(5000);
 
     printf("time,ax,ay,az,gx,gy,gz,mx,my,mz,t\n");
-    long long first_tick = getTick();
     for (;;)
     {
-        //get timestamp
-        long long last_tick = getTick();
 
-        //get axel+gyro+temp data
+        // get axel+gyro+temp data
         lsm9ds1X.onSimpleUpdate();
-        adata = *(lsm9ds1X.accelDataPtr());
-        gdata = *(lsm9ds1X.gyroDataPtr());
-        tdata = *(lsm9ds1X.tempDataPtr());
+        agdata = lsm9ds1X.getXLGSample();
+        tdata  = lsm9ds1X.getTSample();
+        lsm9ds1X.updateTimestamp(miosix::getTick());
 
-        //get magneto data
+        // get magneto data
         lsm9ds1M.onSimpleUpdate();
-        mdata = *(lsm9ds1M.compassDataPtr());
+        mdata = lsm9ds1M.getSample();
 
         // clang-format off
                
-        printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f\n",
-               (int)(last_tick - first_tick), 
-               adata.getX(), adata.getY(), adata.getZ(), 
-               gdata.getX(), gdata.getY(), gdata.getZ(),
-               mdata.getX(), mdata.getY(), mdata.getZ(), 
-               tdata);
+        printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%.3f,%.3f,%d,%.1f\n",
+               (int)agdata.timestamp,
+               agdata.axelData.getX(), agdata.axelData.getY(), agdata.axelData.getZ(),
+               agdata.gyroData.getX(), agdata.gyroData.getY(), agdata.gyroData.getZ(),
+               (int)mdata.timestamp,
+               mdata.magData.getX(), mdata.magData.getY(), mdata.magData.getZ(),
+               (int)tdata.timestamp, 
+               tdata.tempData);
         
         // clang-format on