diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
index 94a8db6bef5fe0b1137663c84afbf7858f83ca66..743a26acc4044015518ab7d559e2926d6f0279d6 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
@@ -24,6 +24,12 @@
 
 #include "LSM9DS1_Magneto.h"
 
+#include <math.h>
+
+#include <iostream>
+
+#include "math/Stats.h"
+
 using miosix::GpioPin;
 using std::vector;
 
@@ -42,11 +48,10 @@ LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
 
 bool LSM9DS1_M::init()
 {
-    if (sensor_initialized)
-    {
-        TRACE("[LSM9DS1 MAG] init() : already initialized\n");
-        return false;
-    }
+
+#ifdef DEBUG
+    assert(sensor_initialized == false);
+#endif
 
     SPITransaction spi(spislave);
 
@@ -61,11 +66,11 @@ bool LSM9DS1_M::init()
     }
 
     // X,Y axes in ultra-high performance mode, ODR defined by constructor
-    uint8_t CTRL_REG1_M_VAL = 0x60 | (int)odr << 2;
+    uint8_t CTRL_REG1_M_VAL = 0x60 | odr << 2;
     spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL);
 
     // FSR defined by constructor
-    uint8_t CTRL_REG2_M_VAL = (int)magFSR << 5;
+    uint8_t CTRL_REG2_M_VAL = magFSR << 5;
     spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL);
 
     // Z axis in ultra-high performance mode
@@ -81,64 +86,184 @@ bool LSM9DS1_M::init()
 
     if (spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL)
     {
+        TRACE("[LSM9DS1 MAG] init() : CTRL_REG1_M readback failed\n");
         return false;
     }
     if (spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL)
     {
+        TRACE("[LSM9DS1 MAG] init() : CTRL_REG2_M readback failed\n");
         return false;
     }
     if (spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL)
     {
+        TRACE("[LSM9DS1 MAG] init() : CTRL_REG3_M readback failed\n");
         return false;
     }
     if (spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL)
     {
+        TRACE("[LSM9DS1 MAG] init() : CTRL_REG4_M readback failed\n");
         return false;
     }
     if (spi.read(regMapM::INT_CFG_M) != INT_CFG_M_VAL)
     {
+        TRACE("[LSM9DS1 MAG] init() : INT_CFG_M readback failed\n");
         return false;
     }
 
-    // select Sensitivity
-    switch (magFSR)
-    {
-        case MagFSR::FS_4:
-            magSensitivity = 0.14f;
-            break;
-        case MagFSR::FS_8:
-            magSensitivity = 0.29f;
-            break;
-        case MagFSR::FS_12:
-            magSensitivity = 0.43f;
-            break;
-        case MagFSR::FS_16:
-            magSensitivity = 0.58f;
-            break;
-        default:
-            magSensitivity = 0.14f;
-            break;
-    }
+    // wait 20ms for stable output
+    miosix::Thread::sleep(20);
 
-    TRACE("[LSM9DS1 XLG] init() : done\n");
+    TRACE("[LSM9DS1 MAG] init() : done\n");
 
     sensor_initialized = true;
 
     return true;
 }
 
-bool LSM9DS1_M::selfTest() { return true; }
+bool LSM9DS1_M::selfTest()
+{
+    TRACE("[LSM9DS1 MAG] selfTest() : starting self-test\n");
+
+    Stats nostX, nostY, nostZ;
+    Stats stX, stY, stZ;
+    bool selfTestResult = false;
+
+    selfTest_mode = true;
+
+    {
+        SPITransaction spi(spislave);
+
+        // Reset all registers (if called after init())
+        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
+        uint8_t CTRL_REG1_M_VAL = (ODR::ODR_80 << 2);
+        spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL);
+
+        uint8_t CTRL_REG2_M_VAL = MagFSR::FS_12 << 5;
+        spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL);
+
+        // wait 20ms for stable output
+        miosix::Thread::sleep(20);
+
+        // out enable - continuous mode
+        spi.write(regMapM::CTRL_REG3_M, 0x00);
+
+        // wait data-ready bit on STATUS REG - read first sample and discard
+        while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0)
+        {
+        }
+
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6);
+    }
+
+    // wait data-ready bit on STATUS REG - read NOST sample at least 5 times
+    LSM9DS1_M::getSelfTestData(nostX, nostY, nostZ);
+
+    // enable self-test mode and wait 60ms
+    {
+        SPITransaction spi(spislave);
+
+        uint8_t CTRL_REG1_M_VAL = (ODR::ODR_80 << 2) | SELFTEST_ENABLE;
+        spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL);
+
+        miosix::Thread::sleep(60);
+
+        // wait data-ready bit on STATUS REG - read first sample and discard
+        while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0)
+        {
+        }
+
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6);
+    }
+
+    LSM9DS1_M::getSelfTestData(stX, stY, stZ);
+
+    float deltaX = fabsf(stX.getStats().mean - nostX.getStats().mean);
+    float deltaY = fabsf(stY.getStats().mean - nostY.getStats().mean);
+    float deltaZ = fabsf(stZ.getStats().mean - nostZ.getStats().mean);
+
+// verify if sensor is inside parameters
+
+// 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;
+#endif
+
+    // clang-format off
+
+    if(ST_XY_MIN < deltaX && deltaX < ST_XY_MAX &&
+       ST_XY_MIN < deltaY && deltaY < ST_XY_MAX &&
+       ST_Z_MIN  < deltaZ && deltaZ < ST_Z_MAX)
+    {
+        TRACE("[LSM9DS1 MAG] selfTest() : self-test passed\n");
+        selfTestResult = true;
+    }
+    else
+    {
+        TRACE("[LSM9DS1 MAG] selfTest() : self-test failed\n");    
+        selfTestResult = false;
+    }
+
+    // clang-format on 
+
+    // re-init if necessary 
+    if(sensor_initialized == true)
+    {
+        sensor_initialized = false;
+        LSM9DS1_M::init(); 
+    }
+
+    selfTest_mode = true;    
+    
+    return selfTestResult;
+}
+
+void LSM9DS1_M::getSelfTestData(Stats& outxStats, Stats& outyStats,
+                                Stats& outzStats)
+{
+    for (int i = 0; i < SELFTEST_MAX_SAMPLES; i++)
+    {
+        {
+            SPITransaction spi(spislave);
+            while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0)
+            {
+            }
+        }
+        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()
 {
+#ifdef DEBUG
+    assert(sensor_initialized == true || selfTest_mode == true);
+#endif
 
     uint8_t magData[6];
 
-    // read output magneto raw data X,Y,Z
+    // read output magneto raw data X,Y,Z and timestamp
     {
         SPITransaction spi(spislave);
-        // bit 1 of SPI transaction = 1 means "auto-increment address"
-        spi.read(regMapM::OUT_X_L_M | 0x40, magData, 6);
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6);
+        lastMagneto.timestamp = miosix::getTick();
     }
 
     // compose signed 16-bit raw data as 2 bytes from the sensor
@@ -148,30 +273,10 @@ bool LSM9DS1_M::onSimpleUpdate()
     int16_t z = magData[4] | magData[5] << 8;
 
     //convert raw data
-    mLastCompass = Vec3(x * magSensitivity / 1000, 
-                        y * magSensitivity / 1000,
-                        z * magSensitivity / 1000);
+    lastMagneto.magData = Vec3(x * magFSR_SMap.at(magFSR), 
+                               y * magFSR_SMap.at(magFSR),
+                               z * magFSR_SMap.at(magFSR));
     // clang-format on
 
     return true;
-}
-
-bool LSM9DS1_M::setOffset(vector<uint16_t>& offVect)
-{
-    if (offVect.size() != 3)
-        return false;
-
-    uint8_t toStore[6];
-    
-    //separate each byte (MSB first)
-    for (int i = 6; i > 0; i = i - 2)
-    {
-        toStore[i - 1] = offVect.back() & 0x00FF;  // LSB
-        toStore[i - 2] = offVect.back() >> 8;      // MSB
-        offVect.pop_back();
-    }
-
-    SPITransaction spi(spislave);
-    // bit 1 of SPI transaction = 1 means "auto-increment address".
-    spi.write(regMapM::OFFSET_X_REG_L_M | 0x40, toStore, 6);
 }
\ 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 b1396989f6f8d73f3521677537c4013097d83a75..f53638fa9593904da9805c7e7b66a52b5b9ddc79 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
@@ -29,6 +29,7 @@
 
 #include "LSM9DS1_Data.h"
 #include "drivers/spi/SPIDriver.h"
+#include "math/Stats.h"
 
 using miosix::GpioPin;
 using std::vector;
@@ -41,7 +42,7 @@ using std::vector;
 class LSM9DS1_M : public CompassSensor
 {
 public:
-    enum MagFSR
+    enum MagFSR : uint8_t
     {
         FS_4  = 0x00,  // +/- 4Gauss
         FS_8  = 0x01,  // +/- 8Gauss
@@ -49,7 +50,7 @@ public:
         FS_16 = 0x03   // +/- 16Gauss
     };
 
-    enum ODR
+    enum ODR : uint8_t
     {
         ODR_0_625 = 0X00,  // 0.625Hz
         ODR_1_25  = 0x01,  // 1.25Hz
@@ -61,6 +62,12 @@ public:
         ODR_80    = 0x07   // 80Hz
     };
 
+    // Sesitivity Map (axelFSR)
+    const std::map<MagFSR, float> magFSR_SMap{{FS_4, 0.00014f},
+                                              {FS_8, 0.00029f},
+                                              {FS_12, 0.00043f},
+                                              {FS_16, 0.00058f}};
+
     /**
      * @brief Creates an instance of an LSM9DS1 magnetometer sensor.
      *
@@ -70,8 +77,8 @@ public:
      * @param    odr Output Data Rate (See datasheet)
      */
 
-    LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_8,
-              ODR odr = ODR::ODR_0_625);
+    LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_16,
+              ODR odr = ODR::ODR_80);
 
     /**
      * @brief Creates an instance of an LSM9DS1 magnetometer sensor.
@@ -84,7 +91,7 @@ public:
      */
 
     LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-              MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625);
+              MagFSR magRange = MagFSR::FS_16, ODR odr = ODR::ODR_80);
 
     /**
      * @brief initializes the LSM9DS1 Sensor (Magnetometer).
@@ -109,23 +116,18 @@ public:
 
     bool onSimpleUpdate() override;
 
-    /**
-     * @brief set offset of the compass sensor in case of offset errors.
-     * @param offVect offset values (X,Y,Z)
-     * @return true if register has been set
-     */
-
-    bool setOffset(vector<uint16_t>& offVect);
-
 private:
+    void getSelfTestData(Stats& outx, Stats& outy, Stats& outz);
+
     bool sensor_initialized = false;
+    bool selfTest_mode = false;
 
     SPISlave spislave;
 
     MagFSR magFSR;
     ODR odr;
 
-    float magSensitivity;
+    lsm9ds1MSample lastMagneto; 
 
     /**
      * @brief Registers' addresses definition.
@@ -161,4 +163,18 @@ private:
     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;
+
+    static const uint8_t AUTO_INCREMENT_ADDR = 0x40;
+    static const uint8_t SOFT_RESET          = 0x08;
+
+    // For selfTEST - limit for FS = +/-12Gauss
+    // LIS3MDL limits - seems to be same magnetometer as LSM9DS1
+    static constexpr float ST_XY_MIN = 1.0f;
+    static constexpr float ST_XY_MAX = 3.0f;
+    static constexpr float ST_Z_MIN  = 0.1f;
+    static constexpr float ST_Z_MAX  = 1.0f;
+
+    static const uint8_t DRDY_MASK            = 0x04;
+    static const uint8_t SELFTEST_MAX_SAMPLES = 10;
+    static const uint8_t SELFTEST_ENABLE      = 0x01;
 };
\ No newline at end of file