diff --git a/sbs.conf b/sbs.conf
index b879db4ca58e2ddc186c2f739679543f985d1659..20f00f2773861e1e03f445adb1ddb0f92397041a 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -498,3 +498,11 @@ BinName:    test-rls
 Include:    %shared
 Defines:
 Main:       test-rls
+
+[test-lsm9ds1-mag]
+Type:       test
+BoardId:    stm32f407vg_stm32f4discovery
+BinName:    test-lsm9ds1-mag
+Include:    %shared %spi
+Defines:    -DDEBUG
+Main:       drivers/test-lsm9ds1-mag
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index 09f7767cc352a06fef5fc355d409f2830e5fa343..f24c3ed46ab896b6b503eebf8e227cb2a0ad1dfe 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -55,8 +55,8 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
         enum ODR
         {
             PWR_DW      =   0X00,
-            ODR_10      =   0X01,
-            ODR_50      =   0X02,
+            ODR_15      =   0X01,
+            ODR_60      =   0X02,
             ODR_119     =   0X03,
             ODR_238     =   0X04,
             ODR_476     =   0X05,
@@ -82,7 +82,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
            GpioPin          cs,
            AxelFSR          axelRange           = AxelFSR::FS_2,
            GyroFSR          gyroRange           = GyroFSR::FS_245,
-           ODR              odr                 = ODR::ODR_10,
+           ODR              odr                 = ODR::ODR_15,
            bool             fifo_enabled        = false,
            unsigned int     fifo_watermark      = 24
            ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
@@ -97,7 +97,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
            SPIBusConfig     config,
            AxelFSR          axelRange           = AxelFSR::FS_2,
            GyroFSR          gyroRange           = GyroFSR::FS_245,
-           ODR              odr                 = ODR::ODR_10,
+           ODR              odr                 = ODR::ODR_15,
            bool             fifo_enabled        = false,
            unsigned int     fifo_watermark      = 24
            ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
@@ -127,7 +127,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
                     break;
             }
 
-            switch (gyroFSR)
+            switch(gyroFSR)
             {
                 case GyroFSR::FS_245:
                     gyroSensitivity = 8.75f;
@@ -143,12 +143,40 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
                     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; 
+            }
+
             SPITransaction spi(spislave);
 
             //Who Am I check:
             uint8_t whoami = spi.read(regMapXLG::WHO_AM_I);
             if(whoami != WHO_AM_I_XLG_VAL){
-                TRACE("LSM9DS1 WAMI: %02X\n", whoami);
+                TRACE("LSM9DS1 AXEL+GYRO WAMI: %02X\n", whoami);
                 last_error = ERR_NOT_ME;
                 return false;
             }
@@ -197,16 +225,20 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
             
             
             //@ startup, some samples have to be discarded (datasheet)
-            uint16_t toWait_ms = samplesToDiscard * 1000 / (int)odr;
-            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++)
+            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)
                 {
-                    spi.read(regMapXLG::OUT_X_L_XL, 6);
-                    spi.read(regMapXLG::OUT_X_L_G,  6);
+                    //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);
+                    }
                 }
             }
             return true;  
@@ -263,7 +295,11 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
                     //read FIFO status and dump all the samples inside the FIFO
                     uint8_t fifo_src = spi.read(FIFO_SRC);
                     fifo_samples = fifo_src & 0x3F;
-
+                    //sanity check
+                    if(fifo_samples > 32)
+                    {
+                        fifo_samples = 32;
+                    }
                     spi.read(OUT_X_L_G, buf, fifo_samples*12); //format: gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh for each sample
                 }
                 //convert & store
@@ -322,6 +358,7 @@ class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSen
         
         float axelSensitivity;
         float gyroSensitivity;
+        float odrHz; 
         float tempZero = 25.0f;
         float tempSensistivity = 16.0f;
         static const uint8_t samplesToDiscard = 8; //max possible val
diff --git a/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h b/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h
index f1eb18151251300f7a417835a23b91bc3bef280d..658899900bd8d65693e054661ee0495fd9849f31 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DSI_Magneto.h
@@ -73,7 +73,7 @@ class LSM9DS1_M : public CompassSensor
            ODR              odr                 = ODR::ODR_0_625
            ): spislave(bus, cs), magFSR(magRange), odr(odr){
             //SPI config
-            spislave.config.br = SPIBaudRate::DIV_64; //baud = fclk/64
+            spislave.config.clock_div=SPIClockDivider::DIV64; 
         }
 
         LSM9DS1_M(
@@ -92,49 +92,53 @@ class LSM9DS1_M : public CompassSensor
             switch(magFSR)
             {
                 case MagFSR::FS_4:
-                    magFSRval = 4.0f;
+                    magSensitivity = 0.14f;
                     break;
                 case MagFSR::FS_8:
-                    magFSRval = 8.0f;
+                    magSensitivity = 0.29f;
                     break;
                 case MagFSR::FS_12:
-                    magFSRval = 12.0f;
+                    magSensitivity = 0.43f;
                     break;
                 case MagFSR::FS_16:
-                    magFSRval = 16.0f;
+                    magSensitivity = 0.58f;
                     break;
                 default:
-                    magFSRval = 4.0f;
+                    magSensitivity = 0.14f;
                     break;
             }
 
             SPITransaction spi(spislave);
 
             //Who Am I check:
-            uint8_t whoami = spi.read(regMapM::WHO_AM_I_M);
+            uint8_t whoami = spi.read(WHO_AM_I_M);//regMapM::WHO_AM_I_M);
+
             if(whoami != WHO_AM_I_M_VAL){
-                TRACE("LSM9DS1 WAMI: %d\n", whoami);
+                TRACE("LSM9DS1 MAG WAMI: 0x%02X\n", whoami);
                 last_error = ERR_NOT_ME;
                 return false;
             }
-
+            
             //setup     
             uint8_t CTRL_REG1_M_VAL = 0x60 | (int)odr<<2;
             spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); //X,Y axes in ultra-high performance mode, NO auto-temp compensation and ODR defined by constructor
-
+            
             uint8_t CTRL_REG2_M_VAL = (int)magFSR<<5;
             spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL); //FSR defined by constructor
 
             spi.write(regMapM::CTRL_REG4_M, CTRL_REG4_M_VAL); //Z axis in ultra-high performance mode
             spi.write(regMapM::CTRL_REG3_M, CTRL_REG3_M_VAL); //I2C disabled, SPI mode: read/write
+            
+            spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL); //disable all interrupts
 
 
             //check that all registers have been written correctly 
 
-            if(spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL) return false;
-            if(spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL) return false;
-            if(spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL) return false;
-            if(spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL) return false;
+            if(spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL) {return false;}
+            if(spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL) {return false;}
+            if(spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL) {return false;}
+            if(spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL) {return false;}
+            if(spi.read(regMapM::INT_CFG_M)   != INT_CFG_M_VAL)   {return false;}
 
             return true;
         }
@@ -161,14 +165,15 @@ class LSM9DS1_M : public CompassSensor
             // TRACE("LSM9DS1 mageto: %02X,%02X,%02X\n", x, y, z);
 
             mLastCompass =
-                    Vec3(x * magFSRval / 0xFFFF,
-                         y * magFSRval / 0xFFFF,
-                         z * magFSRval / 0xFFFF);
+                    Vec3(x * magSensitivity / 1000,
+                         y * magSensitivity / 1000,
+                         z * magSensitivity / 1000);
+            return true; 
         }
 
         void setOffset(vector<uint16_t>& offVect) //X,Y,Z
         {
-            if(offVect.size < 3) return;
+            if(offVect.size() < 3) return;
             uint8_t toStore[6];
             for(int i=6; i > 0 ; i=i-2){
                 toStore[i-1] = offVect.back() & 0x00FF; //LSB
@@ -188,7 +193,7 @@ class LSM9DS1_M : public CompassSensor
         MagFSR magFSR;
         ODR odr;
 
-        float magFSRval;
+        float magSensitivity;
 
         enum regMapM
         {
@@ -218,8 +223,9 @@ class LSM9DS1_M : public CompassSensor
         };
 
         static const uint8_t WHO_AM_I_M_VAL    = 0x3D;
-        static const uint8_t CTRL_REG3_M_VAL   = 0x84;
+        static const uint8_t CTRL_REG3_M_VAL   = 0x80;
         static const uint8_t CTRL_REG4_M_VAL   = 0x0C;
+        static const uint8_t INT_CFG_M_VAL     = 0x00;
 
 
 };
\ No newline at end of file
diff --git a/src/tests/drivers/test-lsm9ds1-mag.cpp b/src/tests/drivers/test-lsm9ds1-mag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40cf4e3bd805fc235730600d1cda018d9ab71b67
--- /dev/null
+++ b/src/tests/drivers/test-lsm9ds1-mag.cpp
@@ -0,0 +1,148 @@
+/**
+ * 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 e97c6059c9d7b26cef72ea99469c677be6115898..7967308a440b1437312a7eb383c43cb14ab5b6e3 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -39,7 +39,7 @@ float tdata;
 //SPI
 SPIBus bus(SPI1);
 SPIBusConfig cfg;
-GpioPin cs(GPIOE_BASE, 7);
+GpioPin cs_XLG(GPIOE_BASE, 7);
 
 
 
@@ -55,7 +55,8 @@ int main(){
         GpioSck::mode(Mode::ALTERNATE);
         GpioMiso::mode(Mode::ALTERNATE);
         GpioMosi::mode(Mode::ALTERNATE);
-        cs.mode(Mode::OUTPUT);
+        
+        cs_XLG.mode(Mode::OUTPUT);
 
         GpioSck::alternateFunction(5);
         GpioMiso::alternateFunction(5);
@@ -65,16 +66,33 @@ int main(){
 
     }
 
-    cs.high();
+    cs_XLG.high();
+
+//dump regiters
+/*
+    for(int reg=0; reg<=0x38; reg++)
+    {
+        SPISlave spislave(bus, cs_XLG, cfg);
+        SPITransaction spi(spislave);
+        uint8_t data = spi.read(reg);
+        printf("0x%02X-->0x%02X\n", reg,data);
+    }
+}*/
+    
+    
+  
     LSM9DS1_XLG lsm9ds1(
                     bus,
-                    cs,
+                    cs_XLG,
+                    cfg,
                     LSM9DS1_XLG::AxelFSR::FS_8, 
                     LSM9DS1_XLG::GyroFSR::FS_245,
                     LSM9DS1_XLG::ODR::ODR_952
                     );
 
+
     while(!lsm9ds1.init()){}
+
     Thread::sleep(500);
     printf("ax,ay,az,gx,gy,gz,t\n");
     long long first_tick = getTick();  
@@ -86,7 +104,7 @@ int main(){
         gdata = *(lsm9ds1.gyroDataPtr());
         tdata = *(lsm9ds1.tempDataPtr());
         printf("%d;%.3f;%.3f;%.3f;%.3f;%.3f;%.3f;%.1f\n",
-                (last_tick - first_tick), 
+                (int)(last_tick - first_tick), 
                 adata.getX(), adata.getY(), adata.getZ(),
                 gdata.getX(), gdata.getY(), gdata.getZ(),
                 tdata);