diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
index 6ef5aa2283fd023d068adf9692518975b4b5efb4..d9a9218471d9de68537741fafb964eb3585d9a4a 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
@@ -196,7 +196,7 @@ bool LSM9DS1_XLG::onSimpleUpdate()
         int16_t z_xl = data[10] | data[11] << 8;
 
         //convert raw data
-        /*fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), 
+        fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), 
                                 y_xl * axelFSR_SMap.at(axelFSR),
                                 z_xl * axelFSR_SMap.at(axelFSR));
 
@@ -204,7 +204,7 @@ bool LSM9DS1_XLG::onSimpleUpdate()
                                 y_gy * gyroFSR_SMap.at(gyroFSR),
                                 z_gy * gyroFSR_SMap.at(gyroFSR));
         
-        fifo[0].timestamp = IRQ_timestamp;*/
+        fifo[0].timestamp = IRQ_timestamp;
         // clang-format on
     }
 
@@ -218,11 +218,9 @@ bool LSM9DS1_XLG::onSimpleUpdate()
         // 32(FIFO DEPTH MAX) = 384 samples
         uint8_t buf[384];
         uint8_t overrun            = 0;
-        uint32_t delta_transaction = 0;
 
         // Read output axel+gyro FIFO raw data X,Y,Z
         {
-            delta_transaction = miosix::getTick();
             SPITransaction spi(spislave);
 
             uint8_t fifo_src_reg;
@@ -232,7 +230,6 @@ bool LSM9DS1_XLG::onSimpleUpdate()
             overrun      = (fifo_src_reg & FIFO_OVERRUN_MASK) >> 6;
 
             spi.read(OUT_X_L_G, buf, fifo_samples * 12);
-            delta_transaction = miosix::getTick() - delta_transaction;
         }
 
         // compute delta time for each sample
@@ -253,25 +250,25 @@ 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 * gyroFSR_SMap.at(gyroFSR),
-                                    y_gy * gyroFSR_SMap.at(gyroFSR),
+            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));*/
+                                    z_xl * axelFSR_SMap.at(axelFSR));
             // clang-format on
 
             fifo[i].timestamp =
                 IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt;
-            //fifo[i].fifo_num = fifo_num;
+            fifo[i].fifo_num = fifo_num;
         }
 
+        //store Stats
         fifodebug.fifo_num         = fifo_num;
         fifodebug.overrun          = (bool)overrun;
         fifodebug.unread           = fifo_samples;
-        fifodebug.transaction_time = delta_transaction;
 
-        last_fifo_level = fifo_samples;  // unused
+        last_fifo_level = fifo_samples;
     }
 
     // temperature update if temp_count = temp_div_freq
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index 9feda81c4ee489317e111f389bab20feb9058543..8629533c488decc1029c88f70b06229c2036082d 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -167,7 +167,8 @@ public:
     bool onSimpleUpdate() override;
 
     /**
-     * @brief set timestamp on last FIFO
+     * @brief set timestamp on last FIFO (if FIFO enabled) or set timestamp for
+     * sample
      * @warning remember to update FIFO data calling onSimpleUpdate
      */
 
@@ -195,7 +196,7 @@ public:
 
     const array<lsm9ds1XLGSample, 32>& getFIFO() const;
 
-      /**
+    /**
      * @brief get FIFO stats after calling onSimpleUpdate() - Just on FIFO
      * mode.
      * @return stats
@@ -208,7 +209,7 @@ public:
      * @return number of samples
      */
 
-    const uint8_t& getFIFOdepth() const; 
+    const uint8_t& getFIFOdepth() const;
 
 private:
     /**
@@ -244,7 +245,7 @@ private:
     uint32_t delta         = 0;
 
     array<lsm9ds1XLGSample, 32> fifo;
-    lsm9ds1debug fifodebug;  
+    lsm9ds1debug fifodebug;
 
     lsm9ds1TSample lastTemp;
 
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
index bc0e87659866abb3f57b317159b8d5a866d7477c..af5187a2ce3da8d5b1cccf75bf1a9828524a5a90 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
@@ -37,41 +37,39 @@ using std::ofstream;
 // ACCELEROMETER + GYROSCOPE
 struct lsm9ds1XLGSample
 {
-
-    //uint16_t fifo_num;
+    uint16_t fifo_num;
     uint64_t timestamp;
-    //Vec3 axelData;
-    //Vec3 gyroData;
+    Vec3 axelData;
+    Vec3 gyroData;
 
-    /*static std::string header()
+    static std::string header()
     {
         return "fifo_num,timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z\n";
-    }*/
+    }
 
-    /*void print(std::ostream& os) const
+    void print(std::ostream& os) const
     {
         os << fifo_num << "," << timestamp << "," << axelData.getX() << "," << axelData.getY()
            << "," << axelData.getZ() << "," << gyroData.getX() << ","
            << gyroData.getY() << "," << gyroData.getZ() << "\n";
-    }*/
+    }
 };
 
-// XLG debug
+// FIFO debug
 struct lsm9ds1debug
 {
     uint16_t fifo_num;
     uint16_t unread;
     bool overrun;
-    uint64_t transaction_time;
 
     static std::string header()
     {
-        return "fifo_num,unread,overrun,transaction_time\n";
+        return "fifo_num,unread,overrun\n";
     }
 
     void print(std::ostream& os) const
     {
-        os << fifo_num << "," << unread << "," << overrun << "," << transaction_time << "\n";
+        os << fifo_num << "," << unread << "," << overrun << "," << "\n";
     }
 };
 
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
index 828963d182a0d8d3e06269f38b9f42f5d7c7a44a..233eada4f4083871dcd1e3c2ab6012c176b4e1dd 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
@@ -277,7 +277,7 @@ bool LSM9DS1_M::onSimpleUpdate()
         spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6);
     }
 
-    lastMagneto.timestamp = miosix::getTick();
+    lastMagneto.timestamp = lastTimestamp; 
 
     // compose signed 16-bit raw data as 2 bytes from the sensor
     // clang-format off
@@ -294,4 +294,9 @@ bool LSM9DS1_M::onSimpleUpdate()
     return true;
 }
 
+void LSM9DS1_M::updateTimestamp(uint64_t timestamp)
+{
+    lastTimestamp = timestamp;
+}
+
 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 8dd422f6be2a789704526664901b93157a43f008..bad903562df5b7d5cf69fe407d5f4b4a6fc9c26d 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
@@ -116,16 +116,22 @@ public:
      */
 
     bool onSimpleUpdate() override;
-    
+
+    /**
+     * @brief set timestamp for sample
+     * @warning remember to update FIFO data calling onSimpleUpdate
+     */
+
+    void updateTimestamp(uint64_t timestamp);
+
     /**
      * @brief get last valid sample
      * @return sample
      */
-    
-    const lsm9ds1MSample& getSample() const; 
 
-private:
+    const lsm9ds1MSample& getSample() const;
 
+private:
     /**
      * @brief get data from sensor and compute stats.
      */
@@ -133,15 +139,15 @@ private:
     void getSelfTestData(Stats& outx, Stats& outy, Stats& outz);
 
     bool sensor_initialized = false;
-    bool selfTest_mode = false;
+    bool selfTest_mode      = false;
 
     SPISlave spislave;
 
     MagFSR magFSR;
     ODR odr;
 
-    lsm9ds1MSample lastMagneto; 
-
+    lsm9ds1MSample lastMagneto;
+    uint64_t lastTimestamp = 0; 
     /**
      * @brief Registers' addresses definition.
      */
diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp
index 3457b84e3807daaedf371440408a8d558ac65569..aada23eec9b90a9bb7206464203c80c1ffbc8d53 100644
--- a/src/tests/drivers/test-lsm9ds1-fifo.cpp
+++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp
@@ -58,11 +58,10 @@ GpioPin PUSHBUTTON(GPIOA_BASE, 0);
 volatile bool flagSPIReadRequest = false;
 
 // IMU obj variables
-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 = 84;
+static const uint8_t FIFO_WATERMARK        = 12;
+static const uint8_t TEMP_DIV_FREQ         = 20;
+static const uint16_t MAG_SAMPLING_PERIOD  = 10;  // 100Hz
+static const uint16_t FIFO_SAMPLING_PERIOD = 84;  // 20 samples/ODR, ODR = 284Hz
 
 // High Resolution hardware timer using TIM5
 HardwareTimer<uint32_t> hrclock(
@@ -133,8 +132,10 @@ int main()
     lsm9ds1_xlg->enable_fifo(FIFO_WATERMARK);
 
     // perform self-tests
-    lsm9ds1_xlg->selfTest();
-    lsm9ds1_m->selfTest();
+    while (!lsm9ds1_xlg->selfTest())
+        ;
+    while (!lsm9ds1_m->selfTest())
+        ;
 
     // initialize sensor
     while (!lsm9ds1_xlg->init())
@@ -142,38 +143,41 @@ int main()
     while (!lsm9ds1_m->init())
         ;
 
-    // sampling until you push the button
+    // sampling until you push the button, then restart the microcontroller
     while (!PUSHBUTTON.value())
     {
         // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO)
         // TEMPERATURE UPDATE
         // an interrupt is set: time to dump the FIFO
-        
-        //if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD)
-        //if (flagSPIReadRequest)
-        if ((miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) && flagSPIReadRequest == true)
+
+        if ((miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) &&
+            flagSPIReadRequest == true)
         {
             flagSPIReadRequest = false;
-            lastFifotick = miosix::getTick();
+            lastFifotick       = miosix::getTick();
             // dump the fifo
             lsm9ds1_xlg->onSimpleUpdate();
 
             // log each sample
-            for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth() ; i++)
+            for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth(); i++)
             {
                 lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getFIFO()[i];
                 LED2.high();
                 logger.log(XLGsample);
                 LED2.low();
             }
-                lsm9ds1debug debugstats = lsm9ds1_xlg->getFIFOStats();
-                logger.log(debugstats);
 
+#ifdef DEBUG
+            lsm9ds1debug debugstats = lsm9ds1_xlg->getFIFOStats();
+            logger.log(debugstats);
+#endif
+
+            // update Temp
             if (lastTempcount == TEMP_DIV_FREQ)
             {
                 lastTempcount          = 0;
                 lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample();
-                //logger.log(Tsample);
+                logger.log(Tsample);
             }
             lastTempcount++;
 
@@ -186,15 +190,15 @@ int main()
             lastMagtick = miosix::getTick();
 
             // get sample from the sensor
+            lsm9ds1_m->updateTimestamp(miosix::getTick());
             lsm9ds1_m->onSimpleUpdate();
 
             // log the sample
             lsm9ds1MSample MAGsample = lsm9ds1_m->getSample();
-            //logger.log(MAGsample);
+            logger.log(MAGsample);
         }
 
-        //Thread::sleep(FIFO_SAMPLING_PERIOD);
-
+        Thread::sleep(FIFO_SAMPLING_PERIOD);
     }
 
     // stop log
diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp
index ad7e59812370c890b0a5f0d4131e1d7e59563e06..da3a2f90fe40a2f652e80e619eb189590c3affc5 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -104,12 +104,13 @@ int main()
     {
 
         // get axel+gyro+temp data
+        lsm9ds1X.updateTimestamp(miosix::getTick());
         lsm9ds1X.onSimpleUpdate();
         agdata = lsm9ds1X.getXLGSample();
         tdata  = lsm9ds1X.getTSample();
-        lsm9ds1X.updateTimestamp(miosix::getTick());
 
         // get magneto data
+        lsm9ds1M.updateTimestamp(miosix::getTick());
         lsm9ds1M.onSimpleUpdate();
         mdata = lsm9ds1M.getSample();