diff --git a/sbs.conf b/sbs.conf
index 20f00f2773861e1e03f445adb1ddb0f92397041a..a1e257e21ca3e424eae9c05fdfc5fe9d890917e6 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -499,10 +499,10 @@ Include:    %shared
 Defines:
 Main:       test-rls
 
-[test-lsm9ds1-mag]
+[test-lsm9ds1-fifo]
 Type:       test
 BoardId:    stm32f407vg_stm32f4discovery
-BinName:    test-lsm9ds1-mag
+BinName:    test-lsm9ds1-fifo
 Include:    %shared %spi
 Defines:    -DDEBUG
-Main:       drivers/test-lsm9ds1-mag
+Main:       drivers/test-lsm9ds1-fifo
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index f24c3ed46ab896b6b503eebf8e227cb2a0ad1dfe..652f982a920b1677a49810ababaed2502efda7ea 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -189,11 +189,11 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
             {
                 spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL|fifo_watermark)); //FIFO continous mode + fifo watermark threshold setup
                 spi.write(regMapXLG::INT1_CTRL, INT1_CTRL_VAL); //interrupt on FIFO treshold
-                spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL|0x02)); //DRDY_mask_bit ON, I2C OFF, FIFO ON
+                spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL|0x02)); //DRDY_mask_bit OFF, I2C OFF, FIFO ON
             }
             else
             {
-                spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); //DRDY_mask_bit ON, I2C OFF, FIFO OFF
+                spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); //DRDY_mask_bit OFF, I2C OFF, FIFO OFF
             }
             
             
@@ -223,24 +223,8 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
             if(spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL)                  {return false;}
             if(spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL)                    {return false;}
             
-            
-            //@ startup, some samples have to be discarded (datasheet)
-            if(odr != ODR::PWR_DW)
-            {   
-                uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
-                //TRACE("toWait_ms: %d", toWait_ms); 
-                miosix::Thread::sleep(toWait_ms); //if FIFO is disabled, just wait 
-                if(fifo_enabled)
-                {
-                    //if FIFO is enabled, read first <samplesToDiscard> samples and discard them
-                    SPITransaction spi(spislave);
-                    for(int i=0; i < samplesToDiscard; i++)
-                    {
-                        spi.read(regMapXLG::OUT_X_L_XL, 6);
-                        spi.read(regMapXLG::OUT_X_L_G,  6);
-                    }
-                }
-            }
+            discardSamples();
+
             return true;  
         }
 
@@ -253,28 +237,23 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
         {
             
             if(!fifo_enabled){ //if FIFO disabled
-                uint8_t axelData[6], gyroData[6], tempData[2];
+               uint8_t data[12], tempData[2];
                 // Read output axel+gyro data X,Y,Z
                 {
                     SPITransaction spi(spislave);
-                    spi.read(regMapXLG::OUT_X_L_XL, axelData, 6);
-                    spi.read(regMapXLG::OUT_X_L_G,  gyroData, 6);
+                    spi.read(regMapXLG::OUT_X_L_G, data, 12);
                     spi.read(regMapXLG::OUT_TEMP_L, tempData, 2); 
                 }
 
-                int16_t x_xl = axelData[0] | axelData[1] << 8;
-                int16_t y_xl = axelData[2] | axelData[3] << 8;
-                int16_t z_xl = axelData[4] | axelData[5] << 8;
+                int16_t x_gy = data[0]  |  data[1] << 8;
+                int16_t y_gy = data[2]  |  data[3] << 8;
+                int16_t z_gy = data[4]  |  data[5] << 8;
 
-                int16_t x_gy = gyroData[0] | gyroData[1] << 8;
-                int16_t y_gy = gyroData[2] | gyroData[3] << 8;
-                int16_t z_gy = gyroData[4] | gyroData[5] << 8;
+                int16_t x_xl = data[6]  |  data[7] << 8;
+                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;
-
-                // TRACE("LSM9DS1 axel: %02X,%02X,%02X\n", x_xl, y_xl, z_xl);
-                // TRACE("LSM9DS1 gyro: %02X,%02X,%02X\n", x_gy, y_gy, z_gy);
-                // TRACE("LSM9DS1 temp: %02X\n, temp");
                 
                 mLastAccel = 
                     Vec3(x_xl  * axelSensitivity / 1000,
@@ -289,7 +268,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
                 mLastTemp =  tempZero + (temp / tempSensistivity); //25°C + TEMP*S devo castare a float "temp"?
             }
             else{ //if FIFO enabled: do not store temperature, it can be read using "temperatureUpdate()" function at low sampling frequency
-                uint8_t buf[384];
+                uint8_t buf[384]; //2 bytes per data * 3 axes * 2 (axel+gyro) * 32(FIFO DEPTH) = 384 samples 
                 {
                     SPITransaction spi(spislave);
                     //read FIFO status and dump all the samples inside the FIFO
@@ -314,13 +293,13 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
                     uint16_t z_xl = buf[i*12 + 10] | buf[i*12 + 11] << 8;
 
                     gyro_fifo[i] = 
-                                Vec3(x_gy  / gyroSensitivity,
-                                     y_gy  / gyroSensitivity,
-                                     z_gy  / gyroSensitivity);
+                                Vec3(x_gy * gyroSensitivity / 1000,
+                                     y_gy * gyroSensitivity / 1000,
+                                     z_gy * gyroSensitivity / 1000);
                     axel_fifo[i] = 
-                                Vec3(x_xl  / axelSensitivity,
-                                     y_xl  / axelSensitivity,
-                                     z_xl  / axelSensitivity);
+                                Vec3(x_xl * axelSensitivity / 1000,
+                                     y_xl * axelSensitivity / 1000,
+                                     z_xl * axelSensitivity / 1000);
                 }
             }
             return true; 
@@ -339,10 +318,20 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
             return true;
         }
 
+        void clearFIFO() //FIFO clearing routine (see datasheet)
+        {
+            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
+            discardSamples();
+
+        }
+
         const array<Vec3, 32>& getGyroFIFO() const { return gyro_fifo; }
         const array<Vec3, 32>& getAxelFIFO() const { return axel_fifo; }
         uint8_t getFIFOSamples() const { return fifo_samples;} //fifo_samples is the same for both axel & gyro FIFOs
-
+        
     private:
 
         bool fifo_enabled;
@@ -363,6 +352,29 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
         float tempSensistivity = 16.0f;
         static const uint8_t samplesToDiscard = 8; //max possible val
 
+        void discardSamples()
+        {
+                        
+            //@ startup, some samples have to be discarded (datasheet)
+            if(odr != ODR::PWR_DW)
+            {   
+                uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
+                //TRACE("toWait_ms: %d", toWait_ms); 
+                miosix::Thread::sleep(toWait_ms); //if FIFO is disabled, just wait 
+                if(fifo_enabled)
+                {
+                    //if FIFO is enabled, read first <samplesToDiscard> samples and discard them
+                    SPITransaction spi(spislave);
+                    for(int i=0; i < samplesToDiscard; i++)
+                    {
+                        spi.read(regMapXLG::OUT_X_L_XL, 6);
+                        spi.read(regMapXLG::OUT_X_L_G,  6);
+                    }
+                }
+            }
+
+        }
+
         enum regMapXLG
         {
             ACT_THS             =   0x04,
diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a8e358de60e1d3254c7578fe5bf2216097b676cb
--- /dev/null
+++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp
@@ -0,0 +1,199 @@
+/**
+ * test LSM9DS1 axel + gyro
+ * Copyright (c) 2020 Skyward Experimental Rocketry
+ * Authors: Andrea Milluzzo
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "drivers/HardwareTimer.h"
+#include "drivers/spi/SPIDriver.h"
+#include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h"
+#include <array>
+
+using namespace miosix;
+using namespace std;
+
+typedef Gpio<GPIOA_BASE, 5> GpioSck; //questi sono i pin SPI per f407_discovery
+typedef Gpio<GPIOA_BASE, 6> GpioMiso;
+typedef Gpio<GPIOA_BASE, 7> GpioMosi;
+
+typedef Gpio<GPIOB_BASE, 1> GpioINT1;
+
+static const bool FIFO_ENABLED = true;
+static const uint8_t FIFO_WATERMARK = 12;
+
+//SPI
+SPIBus bus(SPI1);
+GpioPin cs_XLG(GPIOE_BASE, 7);
+
+//LED just for init 
+GpioPin LED1(GPIOD_BASE, 15);
+
+//SPI read flag 
+bool flagSPIReadRequest = false;
+
+//High Resolution hardware timer using TIM5
+HardwareTimer<uint32_t> hrclock{
+    TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
+
+//Last interrupt tick & delta
+volatile uint32_t last_tick;
+volatile uint32_t delta; 
+
+//Interrupt handlers
+void __attribute__((naked)) EXTI1_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z20EXTI1_IRQHandlerImplv");
+    restoreContext(); 
+}
+
+void __attribute__((used)) EXTI1_IRQHandlerImpl()
+{   
+    //Computing delta beetween interrupts 
+    uint32_t tick = hrclock.tick();
+    delta = tick - last_tick;
+    last_tick = tick; 
+
+    //IRQ routine func here
+    flagSPIReadRequest = true;
+
+    //Clear pending interrupt register
+    EXTI->PR |= EXTI_PR_PR1;
+
+    //Built-in LED on
+    LED1.high(); 
+
+}
+
+void gpioConfig();
+void timer5Config();
+void EXTI1Config(); 
+
+int main(){
+
+    gpioConfig();
+    timer5Config();
+    EXTI1Config();
+
+    LSM9DS1_XLG lsm9ds1(
+                    bus,
+                    cs_XLG,
+                    LSM9DS1_XLG::AxelFSR::FS_8,
+                    LSM9DS1_XLG::GyroFSR::FS_245,
+                    LSM9DS1_XLG::ODR::ODR_15,
+                    FIFO_ENABLED,
+                    FIFO_WATERMARK);
+
+    while(!lsm9ds1.init());
+    
+    long long last_tick = getTick();
+
+    while(1)
+    {
+        /*if(flagSPIReadRequest)
+        {
+            flagSPIReadRequest = false; 
+            Thread::sleep(500);
+            LED1.low();
+            Thread::sleep(500);
+            lsm9ds1.clearFIFO();
+            printf("interrupt occured...\n");
+        }*/
+
+        if(getTick()- last_tick> 10000) //every 10 sec Clear FIFO
+        {   
+            printf("Resetting ... \n");
+            last_tick = getTick();
+            lsm9ds1.clearFIFO();
+        }
+    }
+
+    return 0; 
+}
+
+void gpioConfig()
+{
+    {
+        FastInterruptDisableLock dLock;
+
+        //Enable SPI1 Peripheral 
+        RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+
+        //Select GPIO mode (ALTERNATE 5 for SPI1 on PA5, PA6, PA7)    
+        GpioSck::mode(Mode::ALTERNATE);
+        GpioMiso::mode(Mode::ALTERNATE);
+        GpioMosi::mode(Mode::ALTERNATE);
+
+
+        GpioSck::alternateFunction(5);
+        GpioMiso::alternateFunction(5);
+        GpioMosi::alternateFunction(5);
+
+        //Select Speed for clk (needs precision)
+        GpioSck::speed(Speed::_25MHz);
+
+        //Select GPIO mode for Chip Select    
+        cs_XLG.mode(Mode::OUTPUT);
+
+        //Select GPIO mode for INTERRUPT (PULL-DOWN because there's no pull-down in HW)
+        GpioINT1::mode(Mode::INPUT_PULL_DOWN);
+
+        //Select LED built in GPIO mode
+        LED1.mode(Mode::OUTPUT);
+    }
+
+    cs_XLG.high();
+
+}
+
+void timer5Config()
+{
+    {
+        FastInterruptDisableLock dl;
+        //Enable high resolution TIM5
+        RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+    }
+}
+
+void EXTI1Config()
+{   
+    //Enable SYSCFG for setting interrupts 
+    {
+        FastInterruptDisableLock dl;
+        RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; 
+    }   
+
+    //Configure mask bit of 1st interrupt line
+    EXTI->IMR  |= EXTI_IMR_MR1;
+
+    //Configure trigger selection bit (rising edge)
+    EXTI->RTSR |= EXTI_RTSR_TR1;
+
+    //Clear pending interrupt register before enable
+    EXTI->PR |= EXTI_PR_PR1;
+
+    //Select PB1 as interrupt source in line 1 
+    SYSCFG->EXTICR[0] &= 0xFFFFFF1F;
+
+    //Enable the interrupt in the interrupt controller 
+    NVIC_EnableIRQ(EXTI1_IRQn);
+    NVIC_SetPriority(EXTI1_IRQn, 14);
+} 
\ No newline at end of file
diff --git a/src/tests/drivers/test-lsm9ds1-mag.cpp b/src/tests/drivers/test-lsm9ds1-mag.cpp
deleted file mode 100644
index 40cf4e3bd805fc235730600d1cda018d9ab71b67..0000000000000000000000000000000000000000
--- a/src/tests/drivers/test-lsm9ds1-mag.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/**
- * test LSM9DS1 magneto
- * Copyright (c) 2020 Skyward Experimental Rocketry
- * Authors: Andrea Milluzzo
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "drivers/spi/SPIDriver.h"
-#include "sensors/LSM9DS1/LSM9DSI_Magneto.h"
-
-#define MAX_ADDR 0x33
-
-using namespace miosix;
-
-typedef Gpio<GPIOA_BASE, 5> GpioSck; //pin SPI1 per f407_discovery
-typedef Gpio<GPIOA_BASE, 6> GpioMiso;
-typedef Gpio<GPIOA_BASE, 7> GpioMosi;
-
-
-//GpioPin PushButton(GPIOA_BASE, 0);
-GpioPin LED(GPIOD_BASE, 15);
-
-Vec3 mdata;
-
-//SPI
-SPIBus bus(SPI1);
-SPIBusConfig cfg;
-GpioPin cs_M(GPIOE_BASE, 8);
-
-int main()
-{
-    cfg.clock_div=SPIClockDivider::DIV64;
-    {
-        FastInterruptDisableLock dLock;
-
-        RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; //SPI1 ENABLE
-
-        GpioSck::mode(Mode::ALTERNATE);
-        GpioMiso::mode(Mode::ALTERNATE);
-        GpioMosi::mode(Mode::ALTERNATE);
-
-        cs_M.mode(Mode::OUTPUT);
-
-        GpioSck::alternateFunction(5);
-        GpioMiso::alternateFunction(5);
-        GpioMosi::alternateFunction(5);
-
-        GpioSck::speed(Speed::_25MHz);
-
-        //PushButton.mode(Mode::INPUT);
-        LED.mode(Mode::OUTPUT);
-    }
-
-    cs_M.high(); 
-
-    Thread::sleep(200);
-    
-    //dump&test registers
-    /*while(1)
-    {
-        if(PushButton.value())
-        {
-            LED.high();
-
-            printf("\n\nREGISTER SET BEFORE INIT:\n\n");
-            uint8_t regset1[0x34]; 
-            for(int addr=0; addr<=MAX_ADDR; addr++)
-            {
-                SPISlave spislave(bus, cs_M, cfg);
-                SPITransaction spi(spislave);
-                regset1[addr] = spi.read(addr);
-                printf("0x%02X-->0x%02X\n", addr,regset1[addr]);
-            }
-
-            LSM9DS1_M lsm9ds1(
-                        bus,
-                        cs_M,
-                        LSM9DS1_M::MagFSR::FS_12,
-                        LSM9DS1_M::ODR::ODR_40
-                        );
-
-            lsm9ds1.init(); 
-
-            printf("\n\nREGISTER SET AFTER INIT:\n\n");
-            uint8_t regset2[0x34]; 
-            for(int addr=0; addr<=MAX_ADDR; addr++)
-            {
-                SPISlave spislave(bus, cs_M, cfg);
-                SPITransaction spi(spislave);
-                regset2[addr] = spi.read(addr);
-                if(regset1[addr] == regset2[addr])
-                {
-                    printf("0x%02X-->0x%02X\n", addr,regset2[addr]);
-                }
-                else
-                {
-                    printf("0x%02X-->0x%02X  <--\n", addr,regset2[addr]);
-                }
-            }
-        }
-        else
-            LED.low(); 
-    }*/
-
-    LSM9DS1_M lsm9ds1(
-                    bus,
-                    cs_M,
-                    LSM9DS1_M::MagFSR::FS_12,
-                    LSM9DS1_M::ODR::ODR_40
-                    );
-
-    while(!lsm9ds1.init());
-    LED.high(); //init OK
-
-    Thread::sleep(500);
-    printf("x;y;z\n");
-    long long first_tick = getTick();  
-    for(;;)
-    {   
-        long long last_tick = getTick(); 
-        lsm9ds1.onSimpleUpdate();
-        mdata = *(lsm9ds1.compassDataPtr());
-        printf("%d;%.3f;%.3f;%.3f\n",
-                (int)(last_tick - first_tick), 
-                mdata.getX(), mdata.getY(), mdata.getZ()
-                );
-    }
-    
-
-
-}
diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp
index 3a01ac312fab82029f25827413bd12da20c1ce97..68b30977551376c46b0e0f78fa9a03d71075c042 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -34,8 +34,6 @@ typedef Gpio<GPIOA_BASE, 6> GpioMiso;
 typedef Gpio<GPIOA_BASE, 7> GpioMosi;
 
 static const bool FIFO_ENABLED = false;
-Vec3 adata, gdata, mdata;
-float tdata; 
 
 //SPI
 SPIBus bus(SPI1);
@@ -51,6 +49,10 @@ GpioPin LED2(GPIOD_BASE, 13);
 
 int main(){
 
+    Vec3 adata, gdata, mdata;
+    float tdata; 
+
+
     cfg.clock_div=SPIClockDivider::DIV64;
 
     {