diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
index 74d4ec8ac032f4b1c5cb213d88a98a251fb3b854..6ff0b898f76246d64f2880398c9886970cc9f224 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
     }
 
@@ -217,23 +217,26 @@ bool LSM9DS1_XLG::onSimpleUpdate()
         // 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;
+        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;
             spi.read(FIFO_SRC, &fifo_src_reg, 1);
 
             fifo_samples = fifo_src_reg & FIFO_UNREAD_MASK;
-            overrun     = (fifo_src_reg & FIFO_OVERRUN_MASK) >> 6;
+            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
-        uint64_t dt = delta / last_fifo_level;
+        uint64_t dt = delta / fifo_watermark;
         fifo_num++;
 
         // convert & store
@@ -250,20 +253,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),
+            /*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].unread    = fifo_samples;
-            fifo[i].overrun   = (bool)overrun;
-            fifo[i].fifo_num  = (uint16_t)i;
+            fifo[i].timestamp =
+                IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt;
+            //fifo[i].fifo_num = fifo_num;
         }
-        last_fifo_level = fifo_samples;
+
+        fifodebug.fifo_num         = fifo_num;
+        fifodebug.overrun          = (bool)overrun;
+        fifodebug.unread           = fifo_samples;
+        fifodebug.transaction_time = delta_transaction;
+
+        last_fifo_level = fifo_samples;  // unused
     }
 
     // temperature update if temp_count = temp_div_freq
@@ -307,6 +315,8 @@ const lsm9ds1XLGSample& LSM9DS1_XLG::getXLGSample() const { return fifo[0]; }
 
 const lsm9ds1TSample& LSM9DS1_XLG::getTSample() const { return lastTemp; }
 
+const lsm9ds1debug& LSM9DS1_XLG::getFIFOStats() const { return fifodebug; }
+
 void LSM9DS1_XLG::clearFIFO(SPITransaction& spi)
 {
     spi.write(FIFO_CTRL, 0);                               // Bypass Mode
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index 43bed351236de6c8d83e7cd863dea6c1813c5227..9feda81c4ee489317e111f389bab20feb9058543 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -195,6 +195,14 @@ public:
 
     const array<lsm9ds1XLGSample, 32>& getFIFO() const;
 
+      /**
+     * @brief get FIFO stats after calling onSimpleUpdate() - Just on FIFO
+     * mode.
+     * @return stats
+     */
+
+    const lsm9ds1debug& getFIFOStats() const;
+
     /**
      * @brief get number of samples inside the last FIFO
      * @return number of samples
@@ -236,6 +244,8 @@ private:
     uint32_t delta         = 0;
 
     array<lsm9ds1XLGSample, 32> fifo;
+    lsm9ds1debug fifodebug;  
+
     lsm9ds1TSample lastTemp;
 
     SPISlave spislave;
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
index e07edc11fc3d05bbb05706cbd69351131722cd68..bc0e87659866abb3f57b317159b8d5a866d7477c 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
@@ -37,25 +37,41 @@ using std::ofstream;
 // ACCELEROMETER + GYROSCOPE
 struct lsm9ds1XLGSample
 {
+
+    //uint16_t fifo_num;
     uint64_t timestamp;
-    Vec3 axelData;
-    Vec3 gyroData;
-    uint16_t unread;
+    //Vec3 axelData;
+    //Vec3 gyroData;
+
+    /*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
+    {
+        os << fifo_num << "," << timestamp << "," << axelData.getX() << "," << axelData.getY()
+           << "," << axelData.getZ() << "," << gyroData.getX() << ","
+           << gyroData.getY() << "," << gyroData.getZ() << "\n";
+    }*/
+};
+
+// XLG debug
+struct lsm9ds1debug
+{
     uint16_t fifo_num;
+    uint16_t unread;
     bool overrun;
+    uint64_t transaction_time;
 
     static std::string header()
     {
-        return "timestamp,unread,overrun,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_"
-               "z\n";
+        return "fifo_num,unread,overrun,transaction_time\n";
     }
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << unread << "," << overrun << "," << fifo_num
-           << "," << axelData.getX() << "," << axelData.getY() << ","
-           << axelData.getZ() << "," << gyroData.getX() << ","
-           << gyroData.getY() << "," << gyroData.getZ() << "\n";
+        os << fifo_num << "," << unread << "," << overrun << "," << transaction_time << "\n";
     }
 };
 
diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp
index 8cc6f6e3b7c29a406a50f1256faaabdcec0e2b02..26e2f0979ddadb0c8e0beed807d01865b71f16ad 100644
--- a/src/tests/drivers/test-lsm9ds1-fifo.cpp
+++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp
@@ -55,7 +55,7 @@ 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 uint8_t FIFO_WATERMARK       = 12;
@@ -90,7 +90,7 @@ void __attribute__((used)) EXTI13_IRQHandlerImpl()
     lsm9ds1_xlg->updateTimestamp(hrclock.toMicroSeconds(hrclock.tick()));
 
     // Set read flag
-    //flagSPIReadRequest = true;
+    flagSPIReadRequest = true;
 
     // Built-in LED on
     LED1.high();
@@ -124,7 +124,7 @@ int main()
 
     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_XLG::ODR::ODR_952, TEMP_DIV_FREQ);
 
     lsm9ds1_m = new LSM9DS1_M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8,
                               LSM9DS1_M::ODR::ODR_80);
@@ -141,7 +141,6 @@ int main()
         ;
     while (!lsm9ds1_m->init())
         ;
-    LED2.high();  // init OK
 
     // sampling until you push the button
     while (!PUSHBUTTON.value())
@@ -149,11 +148,12 @@ int main()
         // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO)
         // TEMPERATURE UPDATE
         // an interrupt is set: time to dump the FIFO
-        //if (flagSPIReadRequest)
-        if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD)
+        
+        //if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD)
+        if (flagSPIReadRequest)
         {
-            //flagSPIReadRequest = false;
-            lastFifotick = miosix::getTick();
+            flagSPIReadRequest = false;
+            //lastFifotick = miosix::getTick();
             // dump the fifo
             lsm9ds1_xlg->onSimpleUpdate();
 
@@ -161,14 +161,18 @@ int main()
             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);
 
             if (lastTempcount == TEMP_DIV_FREQ)
             {
                 lastTempcount          = 0;
                 lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample();
-                logger.log(Tsample);
+                //logger.log(Tsample);
             }
             lastTempcount++;
 
@@ -185,10 +189,10 @@ int main()
 
             // log the sample
             lsm9ds1MSample MAGsample = lsm9ds1_m->getSample();
-            logger.log(MAGsample);
+            //logger.log(MAGsample);
         }
 
-        Thread::sleep(FIFO_SAMPLING_PERIOD);
+        //Thread::sleep(FIFO_SAMPLING_PERIOD);
 
     }