diff --git a/sbs.conf b/sbs.conf
index 7a57bd1566ea29088fa093087a2f412e55410c9f..5dcb6c528cef36ab8b1d7768e65000215166a623 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -498,7 +498,7 @@ Main:       drivers/test-l3gd20
 Type:       test
 BoardId:    stm32f407vg_skyward_tortellino
 BinName:    test-lsm9ds1
-Include:    %shared %spi
+Include:    %shared %spi %lsm9ds1
 Defines:    -DDEBUG
 Main:       drivers/test-lsm9ds1
 
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
index 255b4134ebfa0f1f94d6d7ff5bf4a4b6953705f0..4980e5f80e02063f2baf4f14de730ef927d9d2b9 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
@@ -27,11 +27,9 @@
 using miosix::GpioPin;
 using std::array;
 
-LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
-                         AxelFSR axelRange = AxelFSR::FS_2,
-                         GyroFSR gyroRange = GyroFSR::FS_245,
-                         ODR odr = ODR::ODR_15, bool fifo_enabled = false,
-                         unsigned int fifo_watermark = 24)
+LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange,
+                         GyroFSR gyroRange, ODR odr, bool fifo_enabled,
+                         unsigned int fifo_watermark)
     : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
       spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr)
 {
@@ -40,10 +38,8 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
 }
 
 LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-                         AxelFSR axelRange = AxelFSR::FS_2,
-                         GyroFSR gyroRange = GyroFSR::FS_245,
-                         ODR odr = ODR::ODR_15, bool fifo_enabled = false,
-                         unsigned int fifo_watermark = 24)
+                         AxelFSR axelRange, GyroFSR gyroRange, ODR odr,
+                         bool fifo_enabled, unsigned int fifo_watermark)
     : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
       spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange),
       odr(odr)
@@ -52,9 +48,100 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
 
 bool LSM9DS1_XLG::init()
 {
-    // Set FSR
-    switch (axelFSR)  // DA RIGUARDARE I COEFFICIENTI: SU DS C'E'
-                      // SENSITIVITY IN mg/LSB
+    if (sensor_initialized)
+    {
+        TRACE("[LSM9DS1 XLG] init() : already initialized\n");
+        return false;
+    }
+
+    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 XLG] init() : unexpected WAMI -> %02X\n", whoami);
+        last_error = ERR_NOT_ME;
+        return false;
+    }
+
+    // address auto-increment while reading/writing
+    spi.write(regMapXLG::CTRL_REG8, CTRL_REG8_VAL);
+
+    // FIFO setup:
+    if (fifo_enabled)
+    {
+        // FIFO continous mode + fifo watermark threshold setup
+        spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL | fifo_watermark));
+
+        // interrupt on FIFO treshold enabled
+        spi.write(regMapXLG::INT1_CTRL, INT1_CTRL_VAL);
+
+        // DRDY_mask_bit OFF, I2C OFF, FIFO ON
+        spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL | 0x02));
+    }
+    else
+    {
+        // DRDY_mask_bit OFF, I2C OFF, FIFO OFF
+        spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL);
+    }
+
+    /** Axel Setup:
+     * ODR, FSR defined by constructor, auto anti-aliasing BW
+     * (max), LPF2/HPF bypassed and disabled, axel output enabled by default
+     * @ startup
+     */
+    uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3;
+    spi.write(regMapXLG::CTRL_REG6_XL, CTRL_REG6_XL_VAL);
+
+    /** Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and
+     * disabled, gyro output enabled by default @ startup
+     */
+    uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3;
+    spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL);
+
+    // sign and orientation Setup <--- BOARD DEPENDENT
+    // spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL);
+
+    // Check all the registers have been written correctly
+    if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL)
+    {
+        return false;
+    }
+    if (fifo_enabled)
+    {
+        if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark))
+        {
+            return false;
+        }
+        if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL)
+        {
+            return false;
+        }
+        if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02))
+        {
+            return false;
+        }
+    }
+    else
+    {
+        if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL)
+        {
+            return false;
+        }
+    }
+    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;
+    }
+
+    // select Sensitivity and ODR
+    switch (axelFSR)
+
     {
         case AxelFSR::FS_2:
             axelSensitivity = 0.598f;
@@ -117,93 +204,12 @@ bool LSM9DS1_XLG::init()
             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 AXEL+GYRO WAMI: %02X\n", whoami);
-        last_error = ERR_NOT_ME;
-        return false;
-    }
-
-    // common setup
-    spi.write(regMapXLG::CTRL_REG8,
-              CTRL_REG8_VAL);  // addr auto-increment while reading/writing
-
-    // FIFO setup: FIFO enabled in continous mode, decimation OFF,
-    // temperature on FIFO ON.
-    if (fifo_enabled)
-    {
-
-        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 OFF, I2C OFF, FIFO ON
-    }
-    else
-    {
-        spi.write(regMapXLG::CTRL_REG9,
-                  CTRL_REG9_VAL);  // DRDY_mask_bit OFF, I2C OFF, FIFO OFF
-    }
-
-    // Axel Setup: ODR, FSR defined by constructor, auto anti-aliasing BW
-    // (max), LPF2/HPF bypassed and disabled, axel output enabled by default
-    // @ startup
-    uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3;
-    spi.write(regMapXLG::CTRL_REG6_XL,
-              CTRL_REG6_XL_VAL);  // ODR, FSR, auto BW (max) function of ODR
-
-    // Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and
-    // disabled, gyro output enabled by default @ startup
-    uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3;
-    spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL);  // ODR,FSR
-    // spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL); //angular rate
-    // sign and orientation Setup <--- BOARD DEPENDENT
+    // discard first samples (see datasheet)
+    LSM9DS1_XLG::discardSamples(spi);
 
-    // Check all the registers have been written correctly
-    if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL)
-    {
-        return false;
-    }
-    if (fifo_enabled)
-    {
-        if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark))
-        {
-            return false;
-        }
-        if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL)
-        {
-            return false;
-        }
-        if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02))
-        {
-            return false;
-        }
-    }
-    else
-    {
-        if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL)
-        {
-            return false;
-        }
-    }
-    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;
-    }
+    TRACE("[LSM9DS1 XLG] init() : done\n");
 
-    LSM9DS1_XLG::discardSamples(spi);  // LUCA SCS MERDA (x2) <3
+    sensor_initialized = true;
 
     return true;
 }
@@ -212,70 +218,82 @@ bool LSM9DS1_XLG::selfTest() { return true; }
 
 bool LSM9DS1_XLG::onSimpleUpdate()
 {
+    // if FIFO disabled
     if (!fifo_enabled)
-    {  // if FIFO disabled
+    {
         uint8_t data[12], tempData[2];
-        // Read output axel+gyro data X,Y,Z
+
+        // Read output axel+gyro raw data X,Y,Z and temp raw data
         {
             SPITransaction spi(spislave);
             spi.read(regMapXLG::OUT_X_L_G, data, 12);
             spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
         }
 
-        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;
+        // compose signed 16-bit raw data as 2 bytes from the sensor
+        // clang-format off
+        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_xl = data[6] | data[7] << 8;
-        int16_t y_xl = data[8] | data[9] << 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;
 
-        mLastAccel =
-            Vec3(x_xl * axelSensitivity / 1000, y_xl * axelSensitivity / 1000,
-                 z_xl * axelSensitivity / 1000);
+        //convert raw data
+        mLastAccel = Vec3(x_xl * axelSensitivity / 1000, 
+                          y_xl * axelSensitivity / 1000,
+                          z_xl * axelSensitivity / 1000);
+
+        mLastGyro =  Vec3(x_gy * gyroSensitivity / 1000, 
+                          y_gy * gyroSensitivity / 1000,
+                          z_gy * gyroSensitivity / 1000);
 
-        mLastGyro =
-            Vec3(x_gy * gyroSensitivity / 1000, y_gy * gyroSensitivity / 1000,
-                 z_gy * gyroSensitivity / 1000);
+        mLastTemp = tempZero + (temp / tempSensistivity);
 
-        mLastTemp =
-            tempZero + (temp / tempSensistivity);  // 25°C + TEMP*S devo
-                                                   // castare a float "temp"?
+        // clang-format on
     }
+
+    /** if FIFO enabled:
+     * dump fifo_watermark samples (axel+gyro only) from the sensor at one
+     * time. Temperature data can be read using temperatureUpdate() function
+     * at a lower frequency
+     */
     else
-    {  // if FIFO enabled: do not store temperature, it can be read using
-        // "temperatureUpdate()" function at low sampling frequency
-        uint8_t buf[384];  // 2 bytes per data * 3 axes * 2 (axel+gyro) *
-                           // 32(FIFO DEPTH MAX) = 384 samples
+    {
+        // 2 bytes per data * 3 axes per type * 2 types(axel+gyro) *
+        // 32(FIFO DEPTH MAX) = 384 samples
+        uint8_t buf[384];
+
+        // Read output axel+gyro FIFO raw data X,Y,Z
         {
             SPITransaction spi(spislave);
-
-            spi.read(
-                OUT_X_L_G, buf,
-                fifo_watermark *
-                    12);  // format:
-                          // gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh
-                          // for each sample
+            spi.read(OUT_X_L_G, buf, fifo_watermark * 12);
         }
+
         // convert & store
         for (int i = 0; i < fifo_watermark; i++)
         {
-            int16_t x_gy = buf[i * 12] | buf[i * 12 + 1] << 8;
-            int16_t y_gy = buf[i * 12 + 2] | buf[i * 12 + 3] << 8;
-            int16_t z_gy = buf[i * 12 + 4] | buf[i * 12 + 5] << 8;
-
-            int16_t x_xl = buf[i * 12 + 6] | buf[i * 12 + 7] << 8;
-            int16_t y_xl = buf[i * 12 + 8] | buf[i * 12 + 9] << 8;
+            // compose signed 16-bit raw data as 2 bytes from the sensor
+            // clang-format off
+            int16_t x_gy = buf[i * 12]      | buf[i * 12 + 1]  << 8;
+            int16_t y_gy = buf[i * 12 + 2]  | buf[i * 12 + 3]  << 8;
+            int16_t z_gy = buf[i * 12 + 4]  | buf[i * 12 + 5]  << 8;
+
+            int16_t x_xl = buf[i * 12 + 6]  | buf[i * 12 + 7]  << 8;
+            int16_t y_xl = buf[i * 12 + 8]  | buf[i * 12 + 9]  << 8;
             int16_t z_xl = buf[i * 12 + 10] | buf[i * 12 + 11] << 8;
 
+            //convert raw data
             fifo[i].gyroData = Vec3(x_gy * gyroSensitivity / 1000,
                                     y_gy * gyroSensitivity / 1000,
                                     z_gy * gyroSensitivity / 1000);
             fifo[i].axelData = Vec3(x_xl * axelSensitivity / 1000,
                                     y_xl * axelSensitivity / 1000,
                                     z_xl * axelSensitivity / 1000);
+            // clang-format on
         }
     }
     return true;
@@ -283,16 +301,16 @@ bool LSM9DS1_XLG::onSimpleUpdate()
 
 bool LSM9DS1_XLG::temperatureUpdate()
 {
+    // Read output temp raw data
     uint8_t tempData[2];
     {
         SPITransaction spi(spislave);
         spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
     }
-
+    // compose signed 16-bit raw data as 2 bytes from the sensor
     int16_t temp = tempData[0] | tempData[1] << 8;
-    mLastTemp =
-        tempZero +
-        temp / tempSensistivity;  // 25°C + TEMP/S devo castare a float "temp"?
+    // convert raw data
+    mLastTemp = tempZero + temp / tempSensistivity;
     return true;
 }
 
@@ -312,21 +330,18 @@ const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getLsm9ds1FIFO() const
 
 void LSM9DS1_XLG::discardSamples(SPITransaction& spi)
 {
-    //@ startup, some samples have to be discarded (datasheet)
+    //@ startup, some samples must be discarded (datasheet)
     if (odr != ODR::PWR_DW)
     {
+        // wait samples to be overwritten or stored (FIFO on)
         uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
-        // TRACE("toWait_ms: %d", toWait_ms);
-        miosix::Thread::sleep(toWait_ms);  // if FIFO is disabled, just wait
+        miosix::Thread::sleep(toWait_ms);
+
+        // if FIFO is enabled, read first "samplesToDiscard" samples and
+        // discard them.
         if (fifo_enabled)
         {
-            // if FIFO is enabled, read first <samplesToDiscard> samples and
-            // discard them
-            for (int i = 0; i < samplesToDiscard; i++)
-            {
-                spi.read(regMapXLG::OUT_X_L_XL, 6);
-                spi.read(regMapXLG::OUT_X_L_G, 6);
-            }
+            spi.read(OUT_X_L_G, samplesToDiscard * 12);
         }
     }
 }
\ No newline at end of file
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index 62801a5bf797107062f671c6e4fcd8771ba23667..2be91afcc0b020e9d0fc4aec99af2aa1a5f13174 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -34,7 +34,10 @@ using miosix::GpioPin;
 using std::array;
 
 /**
- * @brief descrizione classe 
+ * @brief LSM9DS1 gyro+axel+temp sensor driver.
+ * provides access to data generated by the sensor using SPI protocol.
+ * it is possible to set the sensor to have simple data samples or
+ * exploiting integrated FIFO and perform on-interrupt reading.
  */
 
 class LSM9DS1_XLG : public GyroSensor,
@@ -44,32 +47,33 @@ class LSM9DS1_XLG : public GyroSensor,
 public:
     enum AxelFSR
     {
-        FS_2  = 0x00,
-        FS_16 = 0x01,
-        FS_4  = 0x02,
-        FS_8  = 0x03
+        FS_2  = 0x00,  // +/- 2g
+        FS_16 = 0x01,  // +/- 16g
+        FS_4  = 0x02,  // +/- 4g
+        FS_8  = 0x03   // +/- 8g
     };
 
     enum GyroFSR
     {
-        FS_245  = 0x00,
-        FS_500  = 0x01,
-        FS_2000 = 0x03  // 1 -> 3 jump is ok
+        FS_245  = 0x00,  // +/- 245dps
+        FS_500  = 0x01,  // +/- 500dps
+        FS_2000 = 0x03   // +/- 2000dps
     };
 
     enum ODR
     {
-        PWR_DW  = 0X00,
-        ODR_15  = 0X01,
-        ODR_60  = 0X02,
-        ODR_119 = 0X03,
-        ODR_238 = 0X04,
-        ODR_476 = 0X05,
-        ODR_952 = 0X06
+        PWR_DW  = 0X00,  // power down
+        ODR_15  = 0X01,  // 15Hz
+        ODR_60  = 0X02,  // 60Hz
+        ODR_119 = 0X03,  // 119Hz
+        ODR_238 = 0X04,  // 238Hz
+        ODR_476 = 0X05,  // 476Hz
+        ODR_952 = 0X06   // 952Hz
     };
 
     /**
-     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor.
+     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
+     * sensor.
      *
      * @param    bus SPI bus the sensor is connected to
      * @param    cs Chip Select GPIO
@@ -81,12 +85,14 @@ public:
      * interrupt generation, see datasheet).
      */
 
-    LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange,
-                GyroFSR gyroRange, ODR odr, bool fifo_enabled,
-                unsigned int fifo_watermark);
+    LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
+                AxelFSR axelRange = AxelFSR::FS_2,
+                GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
+                bool fifo_enabled = false, unsigned int fifo_watermark = 24);
 
     /**
-     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor.
+     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
+     * sensor.
      *
      * @param    bus SPI bus the sensor is connected to
      * @param    cs Chip Select GPIO
@@ -100,13 +106,15 @@ public:
      */
 
     LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-                AxelFSR axelRange, GyroFSR gyroRange, ODR odr,
-                bool fifo_enabled, unsigned int fifo_watermark);
+                AxelFSR axelRange = AxelFSR::FS_2,
+                GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
+                bool fifo_enabled = false, unsigned int fifo_watermark = 24);
 
     /**
      * @brief initializes the LSM9DS1 Sensor (Accelerometer + Gyroscope).
      * @return true if all setup registers of the sensor have been written
-     * correctly
+     * correctly. false if already initialized, wrong who_am_i or uncorrect
+     * write.
      */
 
     bool init() override;
@@ -121,6 +129,8 @@ public:
     /**
      * @brief Dump single reading of Axel+Gyro+Temp from the sensor through SPI.
      * @return true if sensor sends data
+     * @warning if FIFO is enabled, call only after interrupt flag from the
+     * sensor has been set
      */
 
     bool onSimpleUpdate() override;
@@ -134,12 +144,15 @@ public:
 
     /**
      * @brief Clear the FIFO register inside the LSM9DS1 sensor.
+     * In order to perform a "clear", FIFO is disabled and then
+     * re-enabled; after that some samples are discarded according to datasheet.
      */
 
     void clearFIFO();
 
     /**
-     * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO mode.
+     * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO
+     * mode.
      * @return array containing the whole FIFO
      */
 
@@ -153,6 +166,7 @@ private:
 
     void discardSamples(SPITransaction& spi);
 
+    bool sensor_initialized = false;
     bool fifo_enabled;
     uint8_t fifo_watermark;
     array<lsm9ds1XLGSample, 32> fifo;
@@ -170,6 +184,9 @@ private:
     float tempSensistivity                = 16.0f;
     static const uint8_t samplesToDiscard = 8;  // max possible val
 
+    /**
+     * @brief Registers' addresses definition.
+     */
     enum regMapXLG
     {
         ACT_THS          = 0x04,
@@ -190,7 +207,7 @@ private:
         INT_GEN_SRC_G    = 0x14,
         OUT_TEMP_L       = 0x15,
         OUT_TEMP_H       = 0x16,
-        STATUS_REG_G     = 0x17,  // per check stato
+        STATUS_REG_G     = 0x17,
         OUT_X_L_G        = 0x18,
         OUT_X_H_G        = 0x19,
         OUT_Y_L_G        = 0x1A,
@@ -203,9 +220,9 @@ private:
         CTRL_REG7_XL     = 0x21,
         CTRL_REG8        = 0x22,
         CTRL_REG9        = 0x23,
-        CTRL_REG10       = 0x24,  // per self-test ma n.u.
+        CTRL_REG10       = 0x24,
         INT_GEN_SRC_XL   = 0x26,
-        STATUS_REG_XL    = 0x27,  // per check stato
+        STATUS_REG_XL    = 0x27,
         OUT_X_L_XL       = 0x28,
         OUT_X_H_XL       = 0x29,
         OUT_Y_L_XL       = 0x2A,
@@ -213,7 +230,7 @@ private:
         OUT_Z_L_XL       = 0x2C,
         OUT_Z_H_XL       = 0x2D,
         FIFO_CTRL        = 0x2E,
-        FIFO_SRC         = 0x2F,  // FIFO status register
+        FIFO_SRC         = 0x2F,
         INT_GEN_CFG_G    = 0x30,
         INT_GEN_THS_XH_G = 0x31,
         INT_GEN_THS_XL_G = 0x32,
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
index 51e68aa98a9a689115599a3c6301caa78418642c..0d4f83aaa890065309013809136f18f5dd4fd0af 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h
@@ -25,11 +25,16 @@
 #pragma once
 
 #include <miosix.h>
+
+#include <fstream>
+
 #include "../Sensor.h"
 
+using std::ofstream;
+
 // data Structs
 
-//ACCELEROMETER + GYROSCOPE 
+// ACCELEROMETER + GYROSCOPE
 struct lsm9ds1XLGSample
 {
     uint64_t timestamp;
@@ -43,22 +48,19 @@ struct lsm9ds1XLGSample
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << 
-              axelData.getX() << "," << axelData.getY() << "," << axelData.getZ() << "," << 
-              gyroData.getX() << "," << gyroData.getY() << "," << gyroData.getZ() << "\n";
+        os << timestamp << "," << axelData.getX() << "," << axelData.getY()
+           << "," << axelData.getZ() << "," << gyroData.getX() << ","
+           << gyroData.getY() << "," << gyroData.getZ() << "\n";
     }
 };
 
-//TEMPERATURE
+// TEMPERATURE
 struct lsm9ds1TSample
 {
     uint64_t timestamp;
     float tempData;
 
-    static std::string header()
-    {
-        return "timestamp,temp\n";
-    }
+    static std::string header() { return "timestamp,temp\n"; }
 
     void print(std::ostream& os) const
     {
@@ -66,20 +68,17 @@ struct lsm9ds1TSample
     }
 };
 
-//MAGNETOMETER
+// MAGNETOMETER
 struct lsm9ds1MSample
 {
     uint64_t timestamp;
     Vec3 magData;
 
-    static std::string header()
-    {
-        return "timestamp,mag_x,mag_y,mag_z\n";
-    }
+    static std::string header() { return "timestamp,mag_x,mag_y,mag_z\n"; }
 
     void print(std::ostream& os) const
     {
-        os << timestamp << "," << 
-              magData.getX() << "," << magData.getY() << "," << magData.getZ() << "\n";
+        os << timestamp << "," << magData.getX() << "," << magData.getY() << ","
+           << magData.getZ() << "\n";
     }
 };
diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
index 7879bc7dfd10f0faa9234f3befbcdc752481632e..94a8db6bef5fe0b1137663c84afbf7858f83ca66 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
@@ -27,8 +27,7 @@
 using miosix::GpioPin;
 using std::vector;
 
-LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs,
-                     MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625)
+LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange, ODR odr)
     : spislave(bus, cs, {}), magFSR(magRange), odr(odr)
 {
     // SPI config
@@ -36,62 +35,47 @@ LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs,
 }
 
 LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-                     MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625)
+                     MagFSR magRange, ODR odr)
     : spislave(bus, cs, config), magFSR(magRange), odr(odr)
 {
 }
 
 bool LSM9DS1_M::init()
 {
-    // Set FSR
-    switch (magFSR)
+    if (sensor_initialized)
     {
-        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;
+        TRACE("[LSM9DS1 MAG] init() : already initialized\n");
+        return false;
     }
 
     SPITransaction spi(spislave);
 
     // Who Am I check:
-    uint8_t whoami = spi.read(WHO_AM_I_M);  // regMapM::WHO_AM_I_M);
+    uint8_t whoami = spi.read(regMapM::WHO_AM_I_M);
 
     if (whoami != WHO_AM_I_M_VAL)
     {
-        TRACE("LSM9DS1 MAG WAMI: 0x%02X\n", whoami);
+        TRACE("[LSM9DS1 MAG] init() : unexpected WAMI -> %02X\n", whoami);
         last_error = ERR_NOT_ME;
         return false;
     }
 
-    // setup
+    // X,Y axes in ultra-high performance mode, ODR defined by constructor
     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
+    spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL);
 
+    // FSR 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_REG2_M, CTRL_REG2_M_VAL);
 
-    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
+    // Z axis in ultra-high performance mode
+    spi.write(regMapM::CTRL_REG4_M, CTRL_REG4_M_VAL);
 
-    spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL);  // disable all interrupts
+    // I2C disabled, SPI mode: read/write
+    spi.write(regMapM::CTRL_REG3_M, CTRL_REG3_M_VAL);
+
+    // disable all interrupts
+    spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL);
 
     // check that all registers have been written correctly
 
@@ -116,6 +100,30 @@ bool LSM9DS1_M::init()
         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;
+    }
+
+    TRACE("[LSM9DS1 XLG] init() : done\n");
+
+    sensor_initialized = true;
+
     return true;
 }
 
@@ -125,31 +133,37 @@ bool LSM9DS1_M::onSimpleUpdate()
 {
 
     uint8_t magData[6];
-    // read output data X,Y,Z
+
+    // read output magneto raw data X,Y,Z
     {
         SPITransaction spi(spislave);
-        // bit 1 of SPI transaction = 1 means "auto-increment address" (see
-        // DS).
-        spi.read(regMapM::OUT_X_L_M | 0x40, magData,
-                 6);  // so bit 6 of the address = 1
+        // bit 1 of SPI transaction = 1 means "auto-increment address"
+        spi.read(regMapM::OUT_X_L_M | 0x40, magData, 6);
     }
 
+    // compose signed 16-bit raw data as 2 bytes from the sensor
+    // clang-format off
     int16_t x = magData[0] | magData[1] << 8;
     int16_t y = magData[2] | magData[3] << 8;
     int16_t z = magData[4] | magData[5] << 8;
 
-    // TRACE("LSM9DS1 mageto: %02X,%02X,%02X\n", x, y, z);
-
-    mLastCompass = Vec3(x * magSensitivity / 1000, y * magSensitivity / 1000,
+    //convert raw data
+    mLastCompass = Vec3(x * magSensitivity / 1000, 
+                        y * magSensitivity / 1000,
                         z * magSensitivity / 1000);
+    // clang-format on
+
     return true;
 }
 
-void LSM9DS1_M::setOffset(vector<uint16_t>& offVect)
+bool LSM9DS1_M::setOffset(vector<uint16_t>& offVect)
 {
-    if (offVect.size() < 3)
-        return;
+    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
@@ -159,6 +173,5 @@ void LSM9DS1_M::setOffset(vector<uint16_t>& offVect)
 
     SPITransaction spi(spislave);
     // bit 1 of SPI transaction = 1 means "auto-increment address".
-    spi.write(regMapM::OFFSET_X_REG_L_M | 0x40, toStore,
-              6);  // so bit 6 of the address = 1
+    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 6bfdad5520af21fe85fc4463bd67dbee00faf7e1..b1396989f6f8d73f3521677537c4013097d83a75 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h
@@ -34,7 +34,8 @@ using miosix::GpioPin;
 using std::vector;
 
 /**
- * @brief descrizione classe
+ * @brief LSM9DS1 magnetometer sensor driver.
+ * provides access to data generated by the sensor using SPI protocol.
  */
 
 class LSM9DS1_M : public CompassSensor
@@ -42,22 +43,22 @@ class LSM9DS1_M : public CompassSensor
 public:
     enum MagFSR
     {
-        FS_4  = 0x00,
-        FS_8  = 0x01,
-        FS_12 = 0x02,
-        FS_16 = 0x03
+        FS_4  = 0x00,  // +/- 4Gauss
+        FS_8  = 0x01,  // +/- 8Gauss
+        FS_12 = 0x02,  // +/- 12Gauss
+        FS_16 = 0x03   // +/- 16Gauss
     };
 
     enum ODR
     {
-        ODR_0_625 = 0X00,
-        ODR_1_25  = 0x01,
-        ODR_2_5   = 0x02,
-        ODR_5     = 0x03,
-        ODR_10    = 0x04,
-        ODR_20    = 0x05,
-        ODR_40    = 0x06,
-        ODR_80    = 0x07
+        ODR_0_625 = 0X00,  // 0.625Hz
+        ODR_1_25  = 0x01,  // 1.25Hz
+        ODR_2_5   = 0x02,  // 2.5Hz
+        ODR_5     = 0x03,  // 5Hz
+        ODR_10    = 0x04,  // 10Hz
+        ODR_20    = 0x05,  // 20Hz
+        ODR_40    = 0x06,  // 40Hz
+        ODR_80    = 0x07   // 80Hz
     };
 
     /**
@@ -69,7 +70,8 @@ public:
      * @param    odr Output Data Rate (See datasheet)
      */
 
-    LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange, ODR odr);
+    LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_8,
+              ODR odr = ODR::ODR_0_625);
 
     /**
      * @brief Creates an instance of an LSM9DS1 magnetometer sensor.
@@ -82,14 +84,15 @@ public:
      */
 
     LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
-              MagFSR magRange, ODR odr);
-    
+              MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625);
+
     /**
      * @brief initializes the LSM9DS1 Sensor (Magnetometer).
      * @return true if all setup registers of the sensor have been written
-     * correctly
+     * correctly.false if already initialized, wrong who_am_i or uncorrect
+     * write.
      */
-    
+
     bool init() override;
 
     /**
@@ -109,11 +112,14 @@ public:
     /**
      * @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
      */
 
-    void setOffset(vector<uint16_t>& offVect);
+    bool setOffset(vector<uint16_t>& offVect);
 
 private:
+    bool sensor_initialized = false;
+
     SPISlave spislave;
 
     MagFSR magFSR;
@@ -121,6 +127,9 @@ private:
 
     float magSensitivity;
 
+    /**
+     * @brief Registers' addresses definition.
+     */
     enum regMapM
     {
         OFFSET_X_REG_L_M = 0x05,
diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp
index 378b8b3218db7785a247a956525549b871fd5109..03079c1436fd9a9dcc16c9475041c141fd67b310 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -22,119 +22,107 @@
  * THE SOFTWARE.
  */
 
-
 #include "drivers/spi/SPIDriver.h"
 #include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h"
 #include "sensors/LSM9DS1/LSM9DS1_Magneto.h"
 
 using namespace miosix;
 
-typedef Gpio<GPIOA_BASE, 5> GpioSck; //questi sono i pin SPI per f407_discovery
+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;
 
 static const bool FIFO_ENABLED = false;
 
-//SPI
+// SPI
 SPIBus bus(SPI1);
-//SPIBusConfig cfg;
+// SPIBusConfig cfg;
 GpioPin cs_XLG(GPIOE_BASE, 7);
 GpioPin cs_M(GPIOE_BASE, 9);
 
-//LEDs
+// LEDs
 GpioPin LED1(GPIOD_BASE, 15);
 GpioPin LED2(GPIOD_BASE, 13);
 
-
-
-int main(){
+int main()
+{
 
     Vec3 adata, gdata, mdata;
-    float tdata; 
+    float tdata;
 
     {
         FastInterruptDisableLock dLock;
 
-        RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; //SPI1 ENABLE
-        
+        RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;  // SPI1 ENABLE
+
         GpioSck::mode(Mode::ALTERNATE);
         GpioMiso::mode(Mode::ALTERNATE);
         GpioMosi::mode(Mode::ALTERNATE);
-        
+
         cs_XLG.mode(Mode::OUTPUT);
-        cs_M.mode(Mode::OUTPUT); 
+        cs_M.mode(Mode::OUTPUT);
 
         GpioSck::alternateFunction(5);
         GpioMiso::alternateFunction(5);
         GpioMosi::alternateFunction(5);
 
-        GpioSck::speed(Speed::_25MHz); 
+        GpioSck::speed(Speed::_25MHz);
 
         LED1.mode(Mode::OUTPUT);
         LED2.mode(Mode::OUTPUT);
-
     }
 
     cs_XLG.high();
-    cs_M.high(); 
-
-//dump regiters
-/*
-    for(int reg=0; reg<=0x38; reg++)
+    cs_M.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 lsm9ds1X(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
+                         LSM9DS1_XLG::GyroFSR::FS_245,
+                         LSM9DS1_XLG::ODR::ODR_952);
+
+    LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8,
+                       LSM9DS1_M::ODR::ODR_20);
+
+    while (!lsm9ds1X.init())
     {
-        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 lsm9ds1X(
-                    bus,
-                    cs_XLG,
-                    //cfg,
-                    LSM9DS1_XLG::AxelFSR::FS_8, 
-                    LSM9DS1_XLG::GyroFSR::FS_245,
-                    LSM9DS1_XLG::ODR::ODR_952
-                    );
-
-    LSM9DS1_M lsm9ds1M(
-                    bus,
-                    cs_M,
-                    LSM9DS1_M::MagFSR::FS_8,
-                    LSM9DS1_M::ODR::ODR_20
-                    );
-
-    while(!lsm9ds1X.init()){}
     LED1.high();
 
-    while(!lsm9ds1M.init()){}
+    while (!lsm9ds1M.init())
+    {
+    }
     LED2.high();
 
     Thread::sleep(500);
-    
+
     printf("time,ax,ay,az,gx,gy,gz,mx,my,mz,t\n");
-    long long first_tick = getTick();  
-    for(;;)
-    {   
-        long long last_tick = getTick(); 
+    long long first_tick = getTick();
+    for (;;)
+    {
+        long long last_tick = getTick();
         lsm9ds1X.onSimpleUpdate();
         adata = *(lsm9ds1X.accelDataPtr());
         gdata = *(lsm9ds1X.gyroDataPtr());
         tdata = *(lsm9ds1X.tempDataPtr());
-        lsm9ds1M.onSimpleUpdate(); 
+        lsm9ds1M.onSimpleUpdate();
         mdata = *(lsm9ds1M.compassDataPtr());
         printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f\n",
-                (int)(last_tick - first_tick), 
-                adata.getX(), adata.getY(), adata.getZ(),
-                gdata.getX(), gdata.getY(), gdata.getZ(),
-                mdata.getX(), mdata.getY(), mdata.getZ(),
-                tdata);
-        //printf("%.3f,%.3f,%.3f\n", mdata.getX(), mdata.getY(), mdata.getZ());
+               (int)(last_tick - first_tick), adata.getX(), adata.getY(),
+               adata.getZ(), gdata.getX(), gdata.getY(), gdata.getZ(),
+               mdata.getX(), mdata.getY(), mdata.getZ(), tdata);
+        // printf("%.3f,%.3f,%.3f\n", mdata.getX(), mdata.getY(), mdata.getZ());
     }
-    
-    
+
     return 0;
 }
\ No newline at end of file