mavlink_skyward_lib @ 00b5e98f

-Subproject commit 00a22f01347688d08464eafcbb86998a0690a8f8
+Subproject commit 00b5e98f28a1476587974dc623377af42883e6f0

sbs.conf

@@ -151,6 +151,11 @@ Files:      src/shared/sensors/SensorManager.cpp
 Type:       srcfiles
 Files:      src/shared/drivers/gamma868/Gamma868.cpp
 
+[lsm9ds1]
+Type:       srcfiles
+Files:      src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp
+            src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp
+
 [xbee]
 Type:       srcfiles
 Files:      src/shared/drivers/Xbee/Xbee.cpp
@@ -489,6 +494,14 @@ Include:    %shared %spi
 Defines:
 Main:       drivers/test-l3gd20
 
+[test-lsm9ds1]
+Type:       test
+BoardId:    stm32f407vg_skyward_tortellino
+BinName:    test-lsm9ds1
+Include:    %shared %spi %lsm9ds1
+Defines:    -DDEBUG
+Main:       drivers/test-lsm9ds1
+
 [test-l3gd20-fifo]
 Type:       test
 BoardId:    stm32f429zi_stm32f4discovery
@@ -505,6 +518,14 @@ Include:    %shared
 Defines:
 Main:       test-rls
 
+[test-lsm9ds1-fifo]
+Type:       test
+BoardId:    stm32f407vg_skyward_tortellino
+BinName:    test-lsm9ds1-fifo
+Include:    %shared %spi %logger %lsm9ds1
+Defines:    -DDEBUG
+Main:       drivers/test-lsm9ds1-fifo
+
 [test-lis3dsh]
 Type:       test
 BoardId:    stm32f407vg_skyward_tortellino

src/shared/diagnostic/SkywardStack.h

@@ -25,7 +25,7 @@
 
 #include <algorithm>
 
-static const unsigned int STACK_MIN_FOR_SKYWARD = 16*1024;
+static const unsigned int STACK_MIN_FOR_SKYWARD = 4*1024;
 
 inline unsigned int skywardStack(unsigned int stack)
 {

src/shared/logger/Logger.h

@@ -174,12 +174,11 @@ private:
         log(s);
     }
 
-    static const unsigned int filenameMaxRetry =
-        100;                                         ///< Limit on new filename
-    static const unsigned int maxRecordSize = 256;   ///< Limit on logged data
-    static const unsigned int numRecords    = 1024;  ///< Size of record queues
-    static const unsigned int bufferSize = 64 * 1024;  ///< Size of each buffer
-    static const unsigned int numBuffers = 8;          ///< Number of buffers
+    static const unsigned int filenameMaxRetry = 100; ///< Limit on new filename
+    static const unsigned int maxRecordSize = 64;   ///< Limit on logged data
+    static const unsigned int numRecords    = 256;  ///< Size of record queues
+    static const unsigned int bufferSize = 16 * 256;  ///< Size of each buffer
+    static const unsigned int numBuffers = 4;          ///< Number of buffers
 
     /**
      * A record is a single serialized logged class. Records are used to

src/shared/math/Stats.cpp

@@ -42,7 +42,7 @@ ostream& operator<<(ostream& os, const StatsResult& sr)
 
 Stats::Stats()
     : minValue(numeric_limits<float>::max()),
-      maxValue(numeric_limits<float>::min()), mean(0.f), m2(0.f), n(0)
+      maxValue(numeric_limits<float>::lowest()), mean(0.f), m2(0.f), n(0)
 {
 }
 
@@ -64,7 +64,7 @@ void Stats::add(float data)
 void Stats::reset()
 {
     minValue = numeric_limits<float>::max();
-    maxValue = numeric_limits<float>::min();
+    maxValue = numeric_limits<float>::lowest();
     mean     = 0.f;
     m2       = 0.f;
     n        = 0;

src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp

+/* LSM9DS1 accelerometer + giroscope Driver
+ *
+ * Copyright (c) 2016,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 "LSM9DS1_AxelGyro.h"
+
+using miosix::GpioPin;
+using std::array;
+
+LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange,
+                         GyroFSR gyroRange, ODR odr, uint8_t temp_div_freq)
+    : spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr),
+      temp_div_freq(temp_div_freq)
+{
+    // SPI config
+    spislave.config.clock_div = SPIClockDivider::DIV64;
+}
+
+LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
+                         AxelFSR axelRange, GyroFSR gyroRange, ODR odr,
+                         uint8_t temp_div_freq)
+    : spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange),
+      odr(odr), temp_div_freq(temp_div_freq)
+{
+}
+
+void LSM9DS1_XLG::enable_fifo(uint8_t watermark)
+{
+    fifo_enabled   = true;
+    fifo_watermark = watermark;
+}
+
+bool LSM9DS1_XLG::init()
+{
+#ifdef DEBUG
+    assert(sensor_initialized == false);
+#endif
+
+    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)
+    {
+        // check fifo-watermark <= 32
+        if (fifo_watermark > 32)
+        {
+            TRACE("[LSM9DS1 XLG] init() : fifo watermark > 32, set to 32\n");
+            fifo_watermark = 32;
+        }
+
+        // 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 = odr << 5 | 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 = odr << 5 | 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)
+    {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG8 readback failed\n");
+        return false;
+    }
+    if (fifo_enabled)
+    {
+        if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark))
+        {
+            TRACE("[LSM9DS1 XLG] init() : FIFO_CTRL readback failed\n");
+            return false;
+        }
+        if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL)
+        {
+            TRACE("[LSM9DS1 XLG] init() : INT1_CTRL readback failed\n");
+            return false;
+        }
+        if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02))
+        {
+            TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n");
+            return false;
+        }
+    }
+    else
+    {
+        if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL)
+        {
+            TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n");
+            return false;
+        }
+    }
+    if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL)
+    {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG6_XL readback failed\n");
+        return false;
+    }
+    if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL)
+    {
+        TRACE("[LSM9DS1 XLG] init() : CTRL_REG1_G readback failed\n");
+        return false;
+    }
+
+    // clear FIFO and discard first samples
+    LSM9DS1_XLG::clearFIFO(spi);
+
+    TRACE("[LSM9DS1 XLG] init() : done\n");
+
+    sensor_initialized = true;
+
+    return true;
+}
+
+bool LSM9DS1_XLG::selfTest() { return true; }
+
+bool LSM9DS1_XLG::onSimpleUpdate()
+{
+// you have to call init() before
+#ifdef DEBUG
+    assert(sensor_initialized == true);
+#endif
+
+    // if FIFO disabled
+    if (!fifo_enabled)
+    {
+        uint8_t data[12];
+
+        // 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);
+        }
+
+        // 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 z_xl = data[10] | data[11] << 8;
+
+        //convert raw data
+        fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), 
+                                y_xl * axelFSR_SMap.at(axelFSR),
+                                z_xl * axelFSR_SMap.at(axelFSR));
+
+        fifo[0].gyroData = Vec3(x_gy * gyroFSR_SMap.at(gyroFSR), 
+                                y_gy * gyroFSR_SMap.at(gyroFSR),
+                                z_gy * gyroFSR_SMap.at(gyroFSR));
+        
+        fifo[0].timestamp = IRQ_timestamp;
+        // clang-format on
+    }
+
+    /** if FIFO enabled:
+     * dump fifo_watermark samples (axel+gyro only) from the sensor at one
+     * time.
+     */
+    else
+    {
+        // 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;
+
+        // Read output axel+gyro FIFO raw data X,Y,Z
+        {
+            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;
+
+            spi.read(OUT_X_L_G, buf, fifo_samples * 12);
+        }
+
+        // compute delta time for each sample
+        uint64_t dt = delta / last_fifo_level;
+        fifo_num++;
+
+        // convert & store
+        for (uint8_t i = 0; i < fifo_samples; ++i)
+        {
+            // 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 * 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));
+            // clang-format on
+
+            fifo[i].timestamp =
+                IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt;
+            fifo[i].fifo_num = fifo_num;
+        }
+
+        //store Stats
+        fifodebug.fifo_num         = fifo_num;
+        fifodebug.overrun          = (bool)overrun;
+        fifodebug.unread           = fifo_samples;
+
+        last_fifo_level = fifo_samples;
+    }
+
+    // temperature update if temp_count = temp_div_freq
+    temp_count++;
+    if (temp_count == temp_div_freq)
+    {
+        temp_count = 0;
+        LSM9DS1_XLG::temperatureUpdate();
+    }
+
+    return true;
+}
+
+void LSM9DS1_XLG::updateTimestamp(uint64_t timestamp)
+{
+    delta         = timestamp - IRQ_timestamp;
+    IRQ_timestamp = timestamp;
+}
+
+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;
+    // convert raw data
+    lastTemp.tempData  = tempZero + temp / tempSensistivity;
+    lastTemp.timestamp = IRQ_timestamp;
+    return true;
+}
+
+const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getFIFO() const { return fifo; }
+
+const uint8_t& LSM9DS1_XLG::getFIFOdepth() const { return fifo_samples; }
+
+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
+    spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark);  // re-enable FIFO
+
+    // sleep 20ms for stable output
+    miosix::Thread::sleep(20);
+
+    LSM9DS1_XLG::discardSamples(spi);
+}
+
+void LSM9DS1_XLG::discardSamples(SPITransaction& spi)
+{
+    //@ 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 = SAMPLES_TO_DISCARD * 1000 / odr_Map.at(odr);
+        miosix::Thread::sleep(toWait_ms);
+
+        // if FIFO is enabled, read first "SAMPLES_TO_DISCARD" samples and
+        // discard them.
+        if (fifo_enabled)
+        {
+            spi.read(OUT_X_L_G, SAMPLES_TO_DISCARD * 12);
+        }
+    }
+}
\ No newline at end of file

src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h

+/* LSM9DS1 accelerometer + giroscope Driver
+ *
+ * Copyright (c) 2016,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.
+ */
+
+#pragma once
+#include <miosix.h>
+
+#include <array>
+
+#include "LSM9DS1_Data.h"
+#include "drivers/spi/SPIDriver.h"
+
+using miosix::GpioPin;
+using std::array;
+
+/**
+ * @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,
+                    public AccelSensor,
+                    public TemperatureSensor
+{
+public:
+    enum AxelFSR : uint8_t
+    {
+        FS_2  = 0x00,  // +/- 2g
+        FS_16 = 0x01,  // +/- 16g
+        FS_4  = 0x02,  // +/- 4g
+        FS_8  = 0x03   // +/- 8g
+    };
+
+    enum GyroFSR : uint8_t
+    {
+        FS_245  = 0x00,  // +/- 245dps
+        FS_500  = 0x01,  // +/- 500dps
+        FS_2000 = 0x03   // +/- 2000dps
+    };
+
+    enum ODR : uint8_t
+    {
+        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
+    };
+
+    // clang-format off
+
+    //Sesitivity Map (axelFSR)
+    const std::map<AxelFSR, float> axelFSR_SMap{{FS_2,  0.000598f},
+                                                {FS_4,  0.001196f},
+                                                {FS_8,  0.002393f},
+                                                {FS_16, 0.007178f}};
+    
+    //Sesitivity Map (gyroFSR)
+    const std::map<GyroFSR, float> gyroFSR_SMap{{FS_245,  0.0001527f},
+                                                {FS_500,  0.0003054f},
+                                                {FS_2000, 0.0012217f}};
+    
+    //ODR Map
+    const std::map<ODR, float> odr_Map{{PWR_DW,  0.0f  },
+                                       {ODR_15,  14.9f },
+                                       {ODR_60,  59.5f },
+                                       {ODR_119, 119.0f},
+                                       {ODR_238, 238.0f},
+                                       {ODR_476, 476.0f},
+                                       {ODR_952, 952.0f}};
+
+    // clang-format on
+
+    /**
+     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
+     * sensor.
+     *
+     * @param    bus SPI bus the sensor is connected to
+     * @param    cs Chip Select GPIO
+     * @param    axelRange accelerometer Full Scale Range (See datasheet)
+     * @param    gyroRange gyroscope Full Scale Range (See datasheet)
+     * @param    odr Output Data Rate (See datasheet)
+     * @param    temp_div_freq Temperature update frequency division
+     */
+
+    LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
+                AxelFSR axelRange = AxelFSR::FS_16,
+                GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952,
+                uint8_t temp_div_freq = 10);
+
+    /**
+     * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope
+     * sensor.
+     *
+     * @param    bus SPI bus the sensor is connected to
+     * @param    cs Chip Select GPIO
+     * @param    config (OPTIONAL) custom SPIBusConfig
+     * @param    axelRange accelerometer Full Scale Range (See datasheet)
+     * @param    gyroRange gyroscope Full Scale Range (See datasheet)
+     * @param    odr Output Data Rate (See datasheet)
+     * @param    temp_div_freq Temperature update frequency division
+     */
+
+    LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
+                AxelFSR axelRange = AxelFSR::FS_16,
+                GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952,
+                uint8_t temp_div_freq = 10);
+
+    /**
+     * @brief enables LSM9DS1 embedded FIFO.
+     * @param fifo_watermark FIFO watermark level in range [1,32] (used for
+     * interrupt generation, see datasheet).
+     * @warning call before init() member
+     */
+
+    void enable_fifo(uint8_t watermark);
+
+    /**
+     * @brief initializes the LSM9DS1 Sensor (Accelerometer + Gyroscope).
+     * @return true if all setup registers of the sensor have been written
+     * correctly. false if already initialized, wrong who_am_i or uncorrect
+     * write.
+     */
+
+    bool init() override;
+
+    /**
+     * @brief Run a self-test of the Sensor.
+     * @return true if sensor behave correclty
+     */
+
+    bool selfTest() override;
+
+    /**
+     * @brief Dump single reading of Axel+Gyro+Temp from the sensor
+     * (if FIFO disabled) or dump fifo_watermark samples from the FIFO (if FIFO
+     * enabled) 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;
+
+    /**
+     * @brief set timestamp on last FIFO (if FIFO enabled) or set timestamp for
+     * sample
+     * @warning remember to update FIFO data calling onSimpleUpdate
+     */
+
+    void updateTimestamp(uint64_t timestamp);
+
+    /**
+     * @brief get last valid sample
+     * @return sample
+     */
+
+    const lsm9ds1XLGSample& getXLGSample() const;
+
+    /**
+     * @brief get last valid sample
+     * @return sample
+     */
+
+    const lsm9ds1TSample& getTSample() const;
+
+    /**
+     * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO
+     * mode.
+     * @return array containing the whole FIFO
+     */
+
+    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
+     */
+
+    const uint8_t& getFIFOdepth() const;
+
+private:
+    /**
+     * @brief Dump single read of Temperature from the sensor through SPI.
+     * @return true if sensor sends data
+     */
+
+    bool temperatureUpdate();
+
+    /**
+     * @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(SPITransaction& spi);
+    /**
+     * @brief discard some samples from the sensor. Must be used when switching.
+     * from FIFO mode to CONTINUOUS mode (or viceversa) and during power on
+     */
+
+    void discardSamples(SPITransaction& spi);
+
+    bool sensor_initialized = false;
+
+    bool fifo_enabled = false;
+    uint8_t fifo_watermark;
+    uint8_t fifo_samples;
+    uint8_t last_fifo_level = 20;
+
+    //
+    uint64_t IRQ_timestamp = 0;
+    uint32_t delta         = 0;
+
+    array<lsm9ds1XLGSample, 32> fifo;
+    lsm9ds1debug fifodebug;
+
+    lsm9ds1TSample lastTemp;
+
+    SPISlave spislave;
+
+    AxelFSR axelFSR;
+    GyroFSR gyroFSR;
+    ODR odr;
+
+    float tempZero         = 25.0f;
+    float tempSensistivity = 16.0f;
+    uint8_t temp_div_freq;
+    uint8_t temp_count = 0;
+
+    /**
+     * @brief Registers' addresses definition.
+     */
+    enum regMapXLG
+    {
+        ACT_THS          = 0x04,
+        ACT_DUR          = 0x05,
+        INT_GEN_CFG_XL   = 0x06,
+        INT_GEN_THS_X_XL = 0x07,
+        INT_GEN_THS_Y_XL = 0x08,
+        INT_GEN_THS_Z_XL = 0x09,
+        INT_GEN_DUR_XL   = 0x0A,
+        REFERENCE_G      = 0x0B,
+        INT1_CTRL        = 0x0C,
+        INT2_CTRL        = 0x0D,
+        WHO_AM_I         = 0x0F,
+        CTRL_REG1_G      = 0x10,
+        CTRL_REG2_G      = 0x11,
+        CTRL_REG3_G      = 0x12,
+        ORIENT_CFG_G     = 0x13,
+        INT_GEN_SRC_G    = 0x14,
+        OUT_TEMP_L       = 0x15,
+        OUT_TEMP_H       = 0x16,
+        STATUS_REG_G     = 0x17,
+        OUT_X_L_G        = 0x18,
+        OUT_X_H_G        = 0x19,
+        OUT_Y_L_G        = 0x1A,
+        OUT_Y_H_G        = 0x1B,
+        OUT_Z_L_G        = 0x1C,
+        OUT_Z_H_G        = 0x1D,
+        CTRL_REG4        = 0x1E,
+        CTRL_REG5_XL     = 0x1F,
+        CTRL_REG6_XL     = 0x20,
+        CTRL_REG7_XL     = 0x21,
+        CTRL_REG8        = 0x22,
+        CTRL_REG9        = 0x23,
+        CTRL_REG10       = 0x24,
+        INT_GEN_SRC_XL   = 0x26,
+        STATUS_REG_XL    = 0x27,
+        OUT_X_L_XL       = 0x28,
+        OUT_X_H_XL       = 0x29,
+        OUT_Y_L_XL       = 0x2A,
+        OUT_Y_H_XL       = 0x2B,
+        OUT_Z_L_XL       = 0x2C,
+        OUT_Z_H_XL       = 0x2D,
+        FIFO_CTRL        = 0x2E,
+        FIFO_SRC         = 0x2F,
+        INT_GEN_CFG_G    = 0x30,
+        INT_GEN_THS_XH_G = 0x31,
+        INT_GEN_THS_XL_G = 0x32,
+        INT_GEN_THS_YH_G = 0x33,
+        INT_GEN_THS_YL_G = 0x34,
+        INT_GEN_THS_ZH_G = 0x35,
+        INT_GEN_THS_ZL_G = 0x36,
+        INT_GEN_DUR_G    = 0x37
+    };
+
+    static const uint8_t INT1_CTRL_VAL      = 0x08;
+    static const uint8_t WHO_AM_I_XLG_VAL   = 0x68;
+    static const uint8_t CTRL_REG8_VAL      = 0x04;
+    static const uint8_t CTRL_REG9_VAL      = 0x04;
+    static const uint8_t FIFO_CTRL_VAL      = 0xC0;
+    static const uint8_t FIFO_UNREAD_MASK   = 0x3F;
+    static const uint8_t FIFO_OVERRUN_MASK  = 0x40;
+    static const uint8_t SAMPLES_TO_DISCARD = 8;
+
+    uint16_t fifo_num = 0;
+};

src/shared/sensors/LSM9DS1/LSM9DS1_Data.h

+/* LSM9DS1 accelerometer + giroscope Driver
+ *
+ * Copyright (c) 2016,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.
+ */
+
+#pragma once
+
+#include <miosix.h>
+
+#include <fstream>
+
+#include "../Sensor.h"
+
+using std::ofstream;
+
+// data Structs
+
+// ACCELEROMETER + GYROSCOPE
+struct lsm9ds1XLGSample
+{
+    uint16_t fifo_num;
+    uint64_t timestamp;
+    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";
+    }
+};
+
+// FIFO debug
+struct lsm9ds1debug
+{
+    uint16_t fifo_num;
+    uint16_t unread;
+    bool overrun;
+
+    static std::string header()
+    {
+        return "fifo_num,unread,overrun\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << fifo_num << "," << unread << "," << overrun << "," << "\n";
+    }
+};
+
+// TEMPERATURE
+struct lsm9ds1TSample
+{
+    uint64_t timestamp;
+    float tempData;
+
+    static std::string header() { return "timestamp,temp\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << tempData << "\n";
+    }
+};
+
+// MAGNETOMETER
+struct lsm9ds1MSample
+{
+    uint64_t timestamp;
+    Vec3 magData;
+
+    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";
+    }
+};
\ No newline at end of file

src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp

+/* LSM9DS1 magnetometer Driver
+ *
+ * Copyright (c) 2016,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 "LSM9DS1_Magneto.h"
+
+#include <math.h>
+
+#include <iostream>
+
+#include "math/Stats.h"
+
+using miosix::GpioPin;
+using std::vector;
+
+LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange, ODR odr)
+    : spislave(bus, cs, {}), magFSR(magRange), odr(odr)
+{
+    // SPI config
+    spislave.config.clock_div = SPIClockDivider::DIV64;
+}
+
+LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
+                     MagFSR magRange, ODR odr)
+    : spislave(bus, cs, config), magFSR(magRange), odr(odr)
+{
+}
+
+bool LSM9DS1_M::init()
+{
+
+#ifdef DEBUG
+    assert(sensor_initialized == false);
+#endif
+
+    SPITransaction spi(spislave);
+
+    // Who Am I check:
+    uint8_t whoami = spi.read(regMapM::WHO_AM_I_M);
+
+    if (whoami != WHO_AM_I_M_VAL)
+    {
+        TRACE("[LSM9DS1 MAG] init() : unexpected WAMI -> %02X\n", whoami);
+        last_error = ERR_NOT_ME;
+        return false;
+    }
+
+    // X,Y axes in ultra-high performance mode, ODR defined by constructor
+    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 = magFSR << 5;
+    spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL);
+
+    // Z axis in ultra-high performance mode
+    spi.write(regMapM::CTRL_REG4_M, CTRL_REG4_M_VAL);
+
+    // 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
+
+    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;
+    }
+
+    // wait 20ms for stable output
+    miosix::Thread::sleep(20);
+
+    TRACE("[LSM9DS1 MAG] init() : done\n");
+
+    sensor_initialized = true;
+
+    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;
+
+    nostX.reset();
+    nostY.reset();
+    nostZ.reset();
+
+    selfTest_mode = true;
+
+    // if already initialized
+    if (sensor_initialized == true)
+    {
+        TRACE("[LSM9DS1 MAG] selfTest() : sensor already initialized!\n");
+#ifdef DEBUG
+        assert(sensor_initialized == false);
+#endif
+        return false;
+    }
+
+    {
+        SPITransaction spi(spislave);
+
+        // Reset all registers
+        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)
+        {
+        }
+
+        uint8_t data[6];
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, data, 6);
+        UNUSED(data);
+    }
+
+    // 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)
+        {
+        }
+
+        uint8_t data[6];
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6);
+        UNUSED(data);
+    }
+
+    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() : statistics"
+              << "\n"
+              << "X-AXIS stats : " << nostX.getStats() << "\n"
+              << "Y-AXIS stats : " << nostY.getStats() << "\n"
+              << "Z-AXIS stats : " << nostZ.getStats() << "\n"
+              << "deltaX : " << deltaX << "\n"
+              << "deltaY : " << deltaY << "\n"
+              << "deltaZ : " << deltaZ << "\n";
+#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 
+    {
+        SPITransaction spi(spislave);
+        spi.write(regMapM::CTRL_REG2_M, SOFT_RESET);
+        miosix::Thread::sleep(20);
+    }
+    selfTest_mode = false;    
+    
+    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();
+
+        // compute statistics
+        outxStats.add(lastMagneto.magData.getX());
+        outyStats.add(lastMagneto.magData.getY());
+        outzStats.add(lastMagneto.magData.getZ());
+    }
+}
+
+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 and timestamp
+    {
+        SPITransaction spi(spislave);
+        spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6);
+    }
+
+    lastMagneto.timestamp = lastTimestamp; 
+
+    // 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;
+
+    //convert raw data
+    lastMagneto.magData = Vec3(x * magFSR_SMap.at(magFSR), 
+                               y * magFSR_SMap.at(magFSR),
+                               z * magFSR_SMap.at(magFSR));
+    // clang-format on
+
+    return true;
+}
+
+void LSM9DS1_M::updateTimestamp(uint64_t timestamp)
+{
+    lastTimestamp = timestamp;
+}
+
+const lsm9ds1MSample& LSM9DS1_M::getSample() const { return lastMagneto; }
\ No newline at end of file

src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h

+/* LSM9DS1 magnetometer Driver
+ *
+ * Copyright (c) 2016,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.
+ */
+
+#pragma once
+#include <miosix.h>
+
+#include <vector>
+
+#include "LSM9DS1_Data.h"
+#include "drivers/spi/SPIDriver.h"
+#include "math/Stats.h"
+
+using miosix::GpioPin;
+using std::vector;
+
+/**
+ * @brief LSM9DS1 magnetometer sensor driver.
+ * provides access to data generated by the sensor using SPI protocol.
+ */
+
+class LSM9DS1_M : public CompassSensor
+{
+public:
+    enum MagFSR : uint8_t
+    {
+        FS_4  = 0x00,  // +/- 4Gauss
+        FS_8  = 0x01,  // +/- 8Gauss
+        FS_12 = 0x02,  // +/- 12Gauss
+        FS_16 = 0x03   // +/- 16Gauss
+    };
+
+    enum ODR : uint8_t
+    {
+        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
+    };
+
+    // 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.
+     *
+     * @param    bus SPI bus the sensor is connected to
+     * @param    cs Chip Select GPIO
+     * @param    magRange magnetometer Full Scale Range (See datasheet)
+     * @param    odr Output Data Rate (See datasheet)
+     */
+
+    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.
+     *
+     * @param    bus SPI bus the sensor is connected to
+     * @param    cs Chip Select GPIO
+     * @param    config custom SPIBusConfig
+     * @param    magRange magnetometer Full Scale Range (See datasheet)
+     * @param    odr Output Data Rate (See datasheet)
+     */
+
+    LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
+              MagFSR magRange = MagFSR::FS_16, ODR odr = ODR::ODR_80);
+
+    /**
+     * @brief initializes the LSM9DS1 Sensor (Magnetometer).
+     * @return true if all setup registers of the sensor have been written
+     * correctly.false if already initialized, wrong who_am_i or uncorrect
+     * write.
+     */
+
+    bool init() override;
+
+    /**
+     * @brief Run a self-test of the Sensor.
+     * @return true if sensor behave correclty
+     * @warning blocking.
+     */
+
+    bool selfTest() override;
+
+    /**
+     * @brief Dump single reading of Magneto from the sensor through SPI.
+     * @return true if sensor sends data
+     */
+
+    bool onSimpleUpdate() override;
+
+    /**
+     * @brief set timestamp for sample
+     * @warning remember to update FIFO data calling onSimpleUpdate
+     */
+
+    void updateTimestamp(uint64_t timestamp);
+
+    /**
+     * @brief get last valid sample
+     * @return sample
+     */
+
+    const lsm9ds1MSample& getSample() const;
+
+private:
+    /**
+     * @brief get data from sensor and compute stats.
+     */
+
+    void getSelfTestData(Stats& outx, Stats& outy, Stats& outz);
+
+    bool sensor_initialized = false;
+    bool selfTest_mode      = false;
+
+    SPISlave spislave;
+
+    MagFSR magFSR;
+    ODR odr;
+
+    lsm9ds1MSample lastMagneto;
+    uint64_t lastTimestamp = 0; 
+    /**
+     * @brief Registers' addresses definition.
+     */
+    enum regMapM
+    {
+        OFFSET_X_REG_L_M = 0x05,
+        OFFSET_X_REG_H_M = 0x06,
+        OFFSET_Y_REG_L_M = 0x07,
+        OFFSET_Y_REG_H_M = 0x08,
+        OFFSET_Z_REG_L_M = 0x09,
+        OFFSET_Z_REG_H_M = 0x0A,
+        WHO_AM_I_M       = 0x0F,
+        CTRL_REG1_M      = 0x20,
+        CTRL_REG2_M      = 0x21,
+        CTRL_REG3_M      = 0x22,
+        CTRL_REG4_M      = 0x23,
+        CTRL_REG5_M      = 0x24,
+        STATUS_REG_M     = 0x27,
+        OUT_X_L_M        = 0x28,
+        OUT_X_H_M        = 0x29,
+        OUT_Y_L_M        = 0x2A,
+        OUT_Y_H_M        = 0x2B,
+        OUT_Z_L_M        = 0x2C,
+        OUT_Z_H_M        = 0x2D,
+        INT_CFG_M        = 0x30,
+        INT_SRC_M        = 0x31,
+        INT_THS_L_M      = 0x32,
+        INT_THS_H_M      = 0x33
+    };
+
+    static const uint8_t WHO_AM_I_M_VAL  = 0x3D;
+    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

src/tests/drivers/test-lsm9ds1-fifo.cpp

+/**
+ * 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 <array>
+
+#include "drivers/HardwareTimer.h"
+#include "drivers/spi/SPIDriver.h"
+#include "logger/Logger.h"
+#include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h"
+#include "sensors/LSM9DS1/LSM9DS1_Magneto.h"
+
+using namespace miosix;
+using namespace std;
+
+// SPI1 f407
+typedef Gpio<GPIOA_BASE, 5> GpioSck;
+typedef Gpio<GPIOA_BASE, 6> GpioMiso;
+typedef Gpio<GPIOA_BASE, 7> GpioMosi;
+
+// INT1 A/G LSM9DS1
+typedef Gpio<GPIOC_BASE, 13> GpioINT1;
+
+// SPI bus & cs
+SPIBus bus(SPI1);
+GpioPin cs_XLG(GPIOE_BASE, 7);
+GpioPin cs_M(GPIOE_BASE, 9);
+
+// LED just for visual reference
+GpioPin LED1(GPIOD_BASE, 15);
+GpioPin LED2(GPIOD_BASE, 13);
+GpioPin LED3(GPIOD_BASE, 14);
+
+// USR pushbutton
+GpioPin PUSHBUTTON(GPIOA_BASE, 0);
+
+// SPI read flag
+volatile bool flagSPIReadRequest = false;
+
+// IMU obj variables
+static const uint8_t FIFO_WATERMARK        = 12;
+static const uint8_t TEMP_DIV_FREQ         = 20;
+static const uint16_t MAG_SAMPLING_PERIOD  = 10;  // 100Hz
+static const uint16_t FIFO_SAMPLING_PERIOD = 84;  // 20 samples/ODR, ODR = 284Hz
+
+// 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;
+
+// LSM9DS1 obj
+LSM9DS1_XLG* lsm9ds1_xlg = nullptr;
+LSM9DS1_M* lsm9ds1_m     = nullptr;
+
+// Interrupt handlers
+void __attribute__((naked)) EXTI13_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z20EXTI13_IRQHandlerImplv");
+    restoreContext();
+}
+
+void __attribute__((used)) EXTI13_IRQHandlerImpl()
+{
+    // Update timestamp
+    lsm9ds1_xlg->updateTimestamp(hrclock.toMicroSeconds(hrclock.tick()));
+
+    // Set read flag
+    flagSPIReadRequest = true;
+
+    // Built-in LED on
+    LED1.high();
+
+    // Clear pending interrupt register
+    EXTI->PR |= EXTI_PR_PR1;
+}
+
+void gpioConfig();
+void timer5Config();
+void EXTI1Config();
+void printStats(void*);
+
+int main()
+{
+
+    uint64_t lastMagtick   = 0;
+    uint64_t lastFifotick  = 0;
+    uint16_t lastTempcount = 0;
+
+    // Spawn thread for loggings logger stats
+    Thread::create(printStats, 4096);
+
+    // start logger
+    Logger& logger = Logger::instance();
+    logger.start();
+
+    gpioConfig();
+    timer5Config();
+    EXTI1Config();
+
+    lsm9ds1_xlg = new LSM9DS1_XLG(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
+                                  LSM9DS1_XLG::GyroFSR::FS_245,
+                                  LSM9DS1_XLG::ODR::ODR_238, TEMP_DIV_FREQ);
+
+    lsm9ds1_m = new LSM9DS1_M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8,
+                              LSM9DS1_M::ODR::ODR_80);
+
+    // enable FIFO
+    lsm9ds1_xlg->enable_fifo(FIFO_WATERMARK);
+
+    // perform self-tests
+    while (!lsm9ds1_xlg->selfTest())
+        ;
+    while (!lsm9ds1_m->selfTest())
+        ;
+
+    // initialize sensor
+    while (!lsm9ds1_xlg->init())
+        ;
+    while (!lsm9ds1_m->init())
+        ;
+
+    // sampling until you push the button, then restart the microcontroller
+    while (!PUSHBUTTON.value())
+    {
+        // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO)
+        // TEMPERATURE UPDATE
+        // an interrupt is set: time to dump the FIFO
+
+        if ((miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) &&
+            flagSPIReadRequest == true)
+        {
+            flagSPIReadRequest = false;
+            lastFifotick       = miosix::getTick();
+            // dump the fifo
+            lsm9ds1_xlg->onSimpleUpdate();
+
+            // log each sample
+            for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth(); i++)
+            {
+                lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getFIFO()[i];
+                LED2.high();
+                logger.log(XLGsample);
+                LED2.low();
+            }
+
+#ifdef DEBUG
+            lsm9ds1debug debugstats = lsm9ds1_xlg->getFIFOStats();
+            logger.log(debugstats);
+#endif
+
+            // update Temp
+            if (lastTempcount == TEMP_DIV_FREQ)
+            {
+                lastTempcount          = 0;
+                lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample();
+                logger.log(Tsample);
+            }
+            lastTempcount++;
+
+            LED1.low();
+        }
+
+        // MAGNETOMETER UPDATE (SIMPLE)
+        if (miosix::getTick() - lastMagtick >= MAG_SAMPLING_PERIOD)
+        {
+            lastMagtick = miosix::getTick();
+
+            // get sample from the sensor
+            lsm9ds1_m->updateTimestamp(miosix::getTick());
+            lsm9ds1_m->onSimpleUpdate();
+
+            // log the sample
+            lsm9ds1MSample MAGsample = lsm9ds1_m->getSample();
+            logger.log(MAGsample);
+        }
+
+        Thread::sleep(FIFO_SAMPLING_PERIOD);
+    }
+
+    // stop log
+    logger.stop();
+
+    LED1.low();
+    LED2.low();
+
+    Thread::sleep(10000);
+    miosix::reboot();
+
+    while (1)
+        ;
+
+    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);
+        cs_M.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);
+        LED2.mode(Mode::OUTPUT);
+        LED3.mode(Mode::OUTPUT);
+
+        // Select USR pushbutton
+        PUSHBUTTON.mode(Mode::INPUT);
+    }
+
+    cs_XLG.high();
+}
+
+void timer5Config()
+{
+    {
+        FastInterruptDisableLock dl;
+        // Enable high resolution TIM5
+        RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+    }
+    hrclock.setPrescaler(382);
+    hrclock.start();
+}
+
+void EXTI1Config()  // PC13
+{
+    // Enable SYSCFG for setting interrupts
+    {
+        FastInterruptDisableLock dl;
+        RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+    }
+
+    // Configure mask bit of 1st interrupt line
+    EXTI->IMR |= EXTI_IMR_MR13;
+
+    // Configure trigger selection bit (rising edge)
+    EXTI->RTSR |= EXTI_RTSR_TR13;
+
+    // Clear pending interrupt register before enable
+    EXTI->PR |= EXTI_PR_PR13;
+
+    // Select PC13 as interrupt source in line 13 (EXTICR4)
+    SYSCFG->EXTICR[3] &= 0xFFFFFF0F;
+    SYSCFG->EXTICR[3] |= 0xFFFFFF2F;
+
+    // Enable the interrupt in the interrupt controller
+    NVIC_EnableIRQ(IRQn_Type::EXTI15_10_IRQn);
+    NVIC_SetPriority(IRQn_Type::EXTI15_10_IRQn, 47);
+}
+
+void printStats(void*)
+{
+    Logger& log = Logger::instance();
+    while (!log.isStarted())
+        ;
+
+    while (log.isStarted())
+    {
+        LogStats stats = log.getLogStats();
+
+        stats.setTimestamp(miosix::getTick());
+        log.log(stats);
+
+        if (stats.statWriteError)
+        {
+            LED3.high();
+        }
+        else
+        {
+            LED3.low();
+        }
+
+        Thread::sleep(1000);
+    }
+}
\ No newline at end of file

src/tests/drivers/test-lsm9ds1.cpp

+/**
+ * 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/spi/SPIDriver.h"
+#include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h"
+#include "sensors/LSM9DS1/LSM9DS1_Magneto.h"
+
+using namespace miosix;
+
+// pin f407 discovery SPI1
+typedef Gpio<GPIOA_BASE, 5> GpioSck;
+typedef Gpio<GPIOA_BASE, 6> GpioMiso;
+typedef Gpio<GPIOA_BASE, 7> GpioMosi;
+
+static const bool FIFO_ENABLED     = false;
+static const uint8_t TEMP_DIV_FREQ = 20;
+
+// SPI
+SPIBus bus(SPI1);
+GpioPin cs_XLG(GPIOE_BASE, 7);
+GpioPin cs_M(GPIOE_BASE, 9);
+
+// LEDs
+GpioPin LED1(GPIOD_BASE, 15);
+GpioPin LED2(GPIOD_BASE, 13);
+
+int main()
+{
+    lsm9ds1XLGSample agdata;
+    lsm9ds1MSample mdata;
+    lsm9ds1TSample tdata;
+
+    {
+        FastInterruptDisableLock dLock;
+
+        // SPI1 ENABLE
+        RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+
+        GpioSck::mode(Mode::ALTERNATE);
+        GpioMiso::mode(Mode::ALTERNATE);
+        GpioMosi::mode(Mode::ALTERNATE);
+
+        cs_XLG.mode(Mode::OUTPUT);
+        cs_M.mode(Mode::OUTPUT);
+
+        GpioSck::alternateFunction(5);
+        GpioMiso::alternateFunction(5);
+        GpioMosi::alternateFunction(5);
+
+        GpioSck::speed(Speed::_25MHz);
+
+        LED1.mode(Mode::OUTPUT);
+        LED2.mode(Mode::OUTPUT);
+    }
+
+    // chip select high
+    cs_XLG.high();
+    cs_M.high();
+
+    LSM9DS1_XLG lsm9ds1X(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
+                         LSM9DS1_XLG::GyroFSR::FS_245,
+                         LSM9DS1_XLG::ODR::ODR_952, TEMP_DIV_FREQ);
+
+    LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_12,
+                       LSM9DS1_M::ODR::ODR_80);
+
+    lsm9ds1M.selfTest();
+    
+    while (!lsm9ds1X.init())
+    {
+    }
+    LED1.high();
+
+    while (!lsm9ds1M.init())
+    {
+    }
+    LED2.high();
+
+    Thread::sleep(5000);
+
+    printf("time,ax,ay,az,gx,gy,gz,mx,my,mz,t\n");
+    for (;;)
+    {
+
+        // get axel+gyro+temp data
+        lsm9ds1X.updateTimestamp(miosix::getTick());
+        lsm9ds1X.onSimpleUpdate();
+        agdata = lsm9ds1X.getXLGSample();
+        tdata  = lsm9ds1X.getTSample();
+
+        // get magneto data
+        lsm9ds1M.updateTimestamp(miosix::getTick());
+        lsm9ds1M.onSimpleUpdate();
+        mdata = lsm9ds1M.getSample();
+
+        // clang-format off
+               
+        printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%.3f,%.3f,%d,%.1f\n",
+               (int)agdata.timestamp,
+               agdata.axelData.getX(), agdata.axelData.getY(), agdata.axelData.getZ(),
+               agdata.gyroData.getX(), agdata.gyroData.getY(), agdata.gyroData.getZ(),
+               (int)mdata.timestamp,
+               mdata.magData.getX(), mdata.magData.getY(), mdata.magData.getZ(),
+               (int)tdata.timestamp, 
+               tdata.tempData);
+        
+        // clang-format on        
+
+    }
+
+    return 0;
+}
\ No newline at end of file