From bcee25a87cb43d445202e6c8ddd9851b668b4ec0 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Sat, 14 Nov 2020 20:51:54 +0100
Subject: [PATCH 01/24] [bmx160] Initial commit. Implemented CHIPID and basic
 file structure.

---
 libs/miosix-kernel                 |  2 +-
 sbs.conf                           | 10 ++-
 src/shared/sensors/BMX160/BMX160.h | 97 ++++++++++++++++++++++++++++++
 src/tests/drivers/test-bmx160.cpp  | 81 +++++++++++++++++++++++++
 4 files changed, 188 insertions(+), 2 deletions(-)
 create mode 100644 src/shared/sensors/BMX160/BMX160.h
 create mode 100644 src/tests/drivers/test-bmx160.cpp

diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index f1665f630..a34d38270 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit f1665f63038e515ffaeb3d29afa769d7ab6a906d
+Subproject commit a34d3827097db99973d5119c5a0c817c2d7b306c
diff --git a/sbs.conf b/sbs.conf
index 8fb172f31..076cabb0e 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -523,4 +523,12 @@ BoardId:    stm32f407vg_skyward_tortellino
 BinName:    test-lis3dsh
 Include:    %shared %spi
 Defines:
-Main:       drivers/test-lis3dsh
\ No newline at end of file
+Main:       drivers/test-lis3dsh
+
+[test-bmx160]
+Type:       test
+BoardId:    stm32f407vg_stm32f4discovery
+BinName:    test-bmx160
+Include:    %shared %spi
+Defines:    -DDEBUG
+Main:       drivers/test-bmx160
\ No newline at end of file
diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
new file mode 100644
index 000000000..404b9dff0
--- /dev/null
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -0,0 +1,97 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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 <drivers/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+#include <cassert>
+
+class BMX160 : public Sensor
+{
+public:
+    BMX160(SPIBusInterface& bus, GpioPin cs)
+        : spi_slave(bus, cs, SPIBusConfig{})
+    {
+
+        // FIXME: random division, check for proper one.
+        spi_slave.config.clock_div = SPIClockDivider::DIV64;
+        // spi_slave.config.clock_div = SPIClockDivider::DIV64;
+    }
+
+    bool init() override
+    {
+#ifdef DEBUG
+        assert(!is_init && "init() should be called once");
+#endif
+
+        SPITransaction spi(spi_slave);
+
+        // Assert CHIPID
+        {
+            // Correct value for CHIPID
+            const uint8_t CHIPID = 0xD8;
+
+            uint8_t chipid = spi.read(REG_CHIPID);
+            if (chipid != CHIPID)
+            {
+                TRACE("[BMX160] Got bad CHIPID, value: %X\n", chipid);
+                return false;
+            }
+        }
+
+        return is_init = true;
+    }
+
+    bool selfTest() override
+    {
+#ifdef DEBUG
+        assert(is_init && "init() was not called");
+#endif
+
+        // TODO: Implement me!
+
+        return true;
+    }
+
+    bool onSimpleUpdate() override
+    {
+        // No-op
+#ifdef DEBUG
+        assert(is_init && "init() was not called");
+#endif
+
+        return true;
+    }
+
+private:
+    bool is_init = false;
+    SPISlave spi_slave;
+
+    enum Reg
+    {
+        REG_CHIPID = 0x00,
+        REG_ERR    = 0x02
+    };
+};
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
new file mode 100644
index 000000000..4190e27e2
--- /dev/null
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -0,0 +1,81 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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.
+ */
+
+/*
+Wiring on STM32F407VG Discovery:
+| BMX Shuttle | Discovery       |
+| ----------- | --------------- |
+| MISO (4)    | SPI3_MISO (PB4) |
+| MOSI (5)    | SPI3_MOSI (PB5) |
+| SCK  (6)    | SPI3_SCK  (PB3) |
+| CS   (7)    | GPIO      (PB7) |
+| INT1 (20)   | TBD             |
+*/
+
+#include <Common.h>
+#include <sensors/BMX160/BMX160.h>
+
+SPIBus bus(SPI3);
+GpioPin cs(GPIOB_BASE, 7);
+
+GpioPin spi_sck(GPIOB_BASE, 3);
+GpioPin spi_miso(GPIOB_BASE, 4);
+GpioPin spi_mosi(GPIOB_BASE, 5);
+
+int main()
+{
+
+    {
+        miosix::FastInterruptDisableLock _lock;
+
+        // Enable SPI3 bus
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+
+        // Setup correct alternate functions for SPI3 bus
+        spi_sck.mode(miosix::Mode::ALTERNATE);
+        spi_sck.alternateFunction(6);
+        spi_miso.mode(miosix::Mode::ALTERNATE);
+        spi_miso.alternateFunction(6);
+        spi_mosi.mode(miosix::Mode::ALTERNATE);
+        spi_mosi.alternateFunction(6);
+
+        // Setup CS
+        cs.mode(miosix::Mode::OUTPUT);
+    }
+
+    cs.high();
+
+    BMX160 sensor(bus, cs);
+
+    TRACE("Initializing BMX160...\n");
+
+    if (!sensor.init())
+    {
+        TRACE("Init failed! (code: %d)\n", sensor.getLastError());
+        return -1;
+    }
+
+    TRACE("BMX160 initialized...\n");
+
+    return 0;
+}
\ No newline at end of file
-- 
GitLab


From f7e6aa1a856ed6cbf258ec4b8ad23ef6422ff5ff Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 17 Nov 2020 17:27:06 +0100
Subject: [PATCH 02/24] [BMX160] Implemented basic configuration support, and
 fifo disabled operating mode.

---
 src/shared/sensors/BMX160/BMX160.h       | 263 ++++++++++++++++++++++-
 src/shared/sensors/BMX160/BMX160Config.h | 119 ++++++++++
 src/tests/drivers/test-bmx160.cpp        |   6 +
 3 files changed, 381 insertions(+), 7 deletions(-)
 create mode 100644 src/shared/sensors/BMX160/BMX160Config.h

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 404b9dff0..b4335d657 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -28,16 +28,45 @@
 
 #include <cassert>
 
+#include "BMX160Config.h"
+
 class BMX160 : public Sensor
 {
 public:
-    BMX160(SPIBusInterface& bus, GpioPin cs)
-        : spi_slave(bus, cs, SPIBusConfig{})
+    struct Data
     {
+        int16_t mag_x;
+        int16_t mag_y;
+        int16_t mag_z;
+        uint16_t rhall;
+
+        int16_t gyr_x;
+        int16_t gyr_y;
+        int16_t gyr_z;
+
+        int16_t acc_x;
+        int16_t acc_y;
+        int16_t acc_z;
+
+        void print()
+        {
+            printf("\n\n");
+            printf("BMX160 data\n");
+            printf("----MAG----\n");
+            printf("x: %d, y: %d, z: %d, rhall: %d\n", mag_x, mag_y, mag_z,
+                   rhall);
+            printf("----GYR----\n");
+            printf("x: %d, y: %d, z: %d\n", gyr_x, gyr_y, gyr_z);
+            printf("----ACC----\n");
+            printf("x: %d, y: %d, z: %d\n", acc_x, acc_y, acc_z);
+        }
+    };
 
+    BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
+        : spi_slave(bus, cs, SPIBusConfig{}), config(config)
+    {
         // FIXME: random division, check for proper one.
         spi_slave.config.clock_div = SPIClockDivider::DIV64;
-        // spi_slave.config.clock_div = SPIClockDivider::DIV64;
     }
 
     bool init() override
@@ -57,10 +86,69 @@ public:
             if (chipid != CHIPID)
             {
                 TRACE("[BMX160] Got bad CHIPID, value: %X\n", chipid);
+                last_error = Sensor::ERR_NOT_ME;
                 return false;
             }
         }
 
+        // Enable Gyro and Accel and Magneto interface
+        {
+            sendCmd(spi, Cmd::ACC_SET_PMU_MODE, Pmu::NORMAL);
+            sendCmd(spi, Cmd::GYR_SET_PMU_MODE, Pmu::NORMAL);
+            sendCmd(spi, Cmd::MAG_IF_SET_PMU_MODE, Pmu::NORMAL);
+        }
+
+        // Wait for interfaces to come up...
+        miosix::Thread::sleep(5);
+
+        // Init magnetometer
+        {
+            /*
+            Little explanation of this:
+            The magnetometer is not controlled directly,
+            instead we have a secondary controller, BMM150,
+            with its own register accessed with REG_MAG_IF_[1-3]
+            */
+
+            // Enable manual configuration mode
+            spi.write(REG_MAG_IF_0, 0x80);
+
+            // Put MAG into sleep mode (from )
+            spi.write(REG_MAG_IF_3, 0x01);
+            spi.write(REG_MAG_IF_2, 0x4B);
+
+            // Load high accuracy preset for REPXY
+            spi.write(REG_MAG_IF_3, 0x17);
+            spi.write(REG_MAG_IF_2, 0x51);
+
+            // Load high accuracy preset for REPZ
+            spi.write(REG_MAG_IF_3, 0x52);
+            spi.write(REG_MAG_IF_2, 0x52);
+
+            // Magic sequence to init it
+            spi.write(REG_MAG_IF_3, 0x02);
+            spi.write(REG_MAG_IF_2, 0x4C);
+            spi.write(REG_MAG_IF_1, 0x42);
+
+            // Set mag output data rate
+            spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
+
+            // Disable manual configuration mode
+            spi.write(REG_MAG_IF_0, 0x00);
+        }
+
+        // Setup accel
+        {
+            spi.write(REG_ACC_CONF, static_cast<uint8_t>(config.acc_odr));
+            spi.write(REG_ACC_RANGE, static_cast<uint8_t>(config.acc_range));
+        }
+
+        // Setup gyro
+        {
+            spi.write(REG_GYR_CONF, static_cast<uint8_t>(config.gyr_odr));
+            spi.write(REG_GYR_RANGE, static_cast<uint8_t>(config.gyr_range));
+        }
+
         return is_init = true;
     }
 
@@ -69,7 +157,6 @@ public:
 #ifdef DEBUG
         assert(is_init && "init() was not called");
 #endif
-
         // TODO: Implement me!
 
         return true;
@@ -77,21 +164,183 @@ public:
 
     bool onSimpleUpdate() override
     {
-        // No-op
 #ifdef DEBUG
         assert(is_init && "init() was not called");
 #endif
+        SPITransaction spi(spi_slave);
 
-        return true;
+        switch (config.fifo_mode)
+        {
+            case BMX160Config::FifoMode::Disabled:
+            {
+                if (spi.read(REG_STATUS) & 0xE0)
+                {
+                    data = readConfig(spi);
+                }
+
+                return true;
+            }
+
+            case BMX160Config::FifoMode::Headerless:
+            {
+                // TODO: Implement me!
+
+                return false;
+            }
+
+            case BMX160Config::FifoMode::Header:
+            {
+                // TODO: Implement me!
+
+                return false;
+            }
+        }
     }
 
+    Data getData() { return data; }
+
 private:
     bool is_init = false;
     SPISlave spi_slave;
 
+    BMX160Config config;
+    Data data;
+
+    enum class Cmd
+    {
+        START_FOC           = 0x03,
+        ACC_SET_PMU_MODE    = 0x10,  // 0b0001_00nn
+        GYR_SET_PMU_MODE    = 0x14,  // 0b0001_01nn
+        MAG_IF_SET_PMU_MODE = 0x18,  // 0b0001_10nn
+        PROG_NVM            = 0xA0,
+        FIFO_FLUSH          = 0xB0,
+        INT_RESET           = 0xB1,
+        STEP_CNT_CLR        = 0xB2,
+        SOFTRESET           = 0xB6,
+    };
+
+    enum class Pmu
+    {
+        SUSPEND       = 0x0,
+        NORMAL        = 0x1,
+        LOW_POWER     = 0x2,
+        FAST_START_UP = 0x3
+    };
+
+    /// Execute CMD,
+    /// Pmu parameter MUST be set to Pmu::SUSPEND,
+    /// unless cmd is one of:
+    /// - ACC_SET_PMU_MODE
+    /// - GYR_SET_PMU_MODE
+    /// - MAG_IF_SET_PMU_MODE
+    void sendCmd(SPITransaction& spi, Cmd cmd, Pmu pmu = Pmu::SUSPEND)
+    {
+        spi.write(REG_CMD,
+                  static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
+    }
+
+    Data readConfig(SPITransaction& spi)
+    {
+        return Data{
+            spi.read(REG_DATA_MAG_X_0) | (spi.read(REG_DATA_MAG_X_1) << 8),
+            spi.read(REG_DATA_MAG_Y_0) | (spi.read(REG_DATA_MAG_Y_1) << 8),
+            spi.read(REG_DATA_MAG_Z_0) | (spi.read(REG_DATA_MAG_Z_1) << 8),
+            spi.read(REG_DATA_RHALL_0) | (spi.read(REG_DATA_RHALL_1) << 8),
+            spi.read(REG_DATA_GYR_X_0) | (spi.read(REG_DATA_GYR_X_1) << 8),
+            spi.read(REG_DATA_GYR_Y_0) | (spi.read(REG_DATA_GYR_Y_1) << 8),
+            spi.read(REG_DATA_GYR_Z_0) | (spi.read(REG_DATA_GYR_Z_1) << 8),
+            spi.read(REG_DATA_ACC_X_0) | (spi.read(REG_DATA_ACC_X_1) << 8),
+            spi.read(REG_DATA_ACC_Y_0) | (spi.read(REG_DATA_ACC_Y_1) << 8),
+            spi.read(REG_DATA_ACC_Z_0) | (spi.read(REG_DATA_ACC_Z_1) << 8),
+        };
+    }
+
     enum Reg
     {
         REG_CHIPID = 0x00,
-        REG_ERR    = 0x02
+        REG_ERR    = 0x02,
+
+        REG_DATA_MAG_X_0 = 0x04,
+        REG_DATA_MAG_X_1 = 0x05,
+        REG_DATA_MAG_Y_0 = 0x06,
+        REG_DATA_MAG_Y_1 = 0x07,
+        REG_DATA_MAG_Z_0 = 0x08,
+        REG_DATA_MAG_Z_1 = 0x09,
+        REG_DATA_RHALL_0 = 0x0A,
+        REG_DATA_RHALL_1 = 0x0B,
+        REG_DATA_GYR_X_0 = 0x0C,
+        REG_DATA_GYR_X_1 = 0x0D,
+        REG_DATA_GYR_Y_0 = 0x0E,
+        REG_DATA_GYR_Y_1 = 0x0F,
+        REG_DATA_GYR_Z_0 = 0x10,
+        REG_DATA_GYR_Z_1 = 0x11,
+        REG_DATA_ACC_X_0 = 0x12,
+        REG_DATA_ACC_X_1 = 0x13,
+        REG_DATA_ACC_Y_0 = 0x14,
+        REG_DATA_ACC_Y_1 = 0x15,
+        REG_DATA_ACC_Z_0 = 0x16,
+        REG_DATA_ACC_Z_1 = 0x17,
+
+        REG_SENSORTIME_0 = 0x18,
+        REG_SENSORTIME_1 = 0x19,
+        REG_SENSORTIME_2 = 0x1A,
+
+        REG_STATUS = 0x1B,
+
+        REG_TEMPERATURE_0 = 0x20,
+        REG_TEMPERATURE_1 = 0x21,
+
+        REG_FIFO_LENGTH_0 = 0x22,
+        REG_FIFO_LENGTH_1 = 0x23,
+
+        REG_FIFO_DATA = 0x24,
+
+        REG_ACC_CONF  = 0x40,
+        REG_ACC_RANGE = 0x41,
+        REG_GYR_CONF  = 0x42,
+        REG_GYR_RANGE = 0x43,
+        REG_MAG_CONF  = 0x44,
+
+        REG_FIFO_DOWNS    = 0x45,
+        REG_FIFO_CONFIG_0 = 0x46,
+        REG_FIFO_CONFIG_1 = 0x47,
+
+        REG_MAG_IF_0 = 0x4C,
+        REG_MAG_IF_1 = 0x4D,
+        REG_MAG_IF_2 = 0x4E,
+        REG_MAG_IF_3 = 0x4F,
+
+        REG_INT_EN_0 = 0x50,
+        REG_INT_EN_1 = 0x51,
+        REG_INT_EN_2 = 0x52,
+
+        REG_INT_OUT_CTRL = 0x53,
+        REG_INT_LATCH    = 0x54,
+
+        REG_INT_MAP_0 = 0x55,
+        REG_INT_MAP_1 = 0x56,
+        REG_INT_MAP_2 = 0x57,
+
+        /* INT_DATA_[0-1] not needed */
+        /* INT_LOWHIGH_[0-5] not needed */
+        /* INT_MOTION_[0-3] not needed */
+        /* INT_TAP_[0-1] not needed */
+        /* INT_ORIENT_[0-1] not needed */
+        /* INT_FLAT_[0-1] not needed */
+
+        REG_FOC_CONF = 0x69,
+        REG_CONF     = 0x6A,
+        REG_IF_CONF  = 0x6B,
+
+        /* PMU_TRIGGER not needed */
+
+        REG_SELF_TEST = 0x6D,
+
+        /* NV_CONF not needed */
+        /* OFFSET_[0-6] not needed */
+        /* STEP_CNT_[0-1] not needed */
+        /* STEP_CONF_[0-1] not needed */
+
+        REG_CMD = 0x7E
     };
 };
\ No newline at end of file
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
new file mode 100644
index 000000000..2dbb171f6
--- /dev/null
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -0,0 +1,119 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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
+
+struct BMX160Config
+{
+    BMX160Config() {}
+
+    /// Fifo operating mode
+    enum class FifoMode
+    {
+        /// The fifo is completely disabled
+        Disabled,
+        /// the fifo operations have less overhead,
+        /// but EVERY sensor MUST have the same odr!!
+        Headerless,
+        /// the fifo operations have more overhead,
+        /// but sensors con have different odr
+        Header
+    };
+
+    /// Accellerometer ODR expressed in Hz
+    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx)
+    enum class AccOdr
+    {
+        HZ_25_32 = 0x01,
+        HZ_25_16 = 0x02,
+        HZ_25_8  = 0x03,
+        HZ_25_4  = 0x04,
+        HZ_25_2  = 0x05,
+        HZ_25    = 0x06,
+        HZ_50    = 0x07,
+        HZ_100   = 0x08,
+        HZ_200   = 0x09,
+        HZ_400   = 0x0a,
+        HZ_800   = 0x0b,
+        HZ_1600  = 0x0c,
+    };
+
+    /// Range of the accellerometer expressed in +/- g
+    enum class AccRange
+    {
+        G_2,
+        G_4,
+        G_8,
+        G_16
+    };
+
+    /// Gyroscope ODR expressed in Hz
+    enum class GyrOdr
+    {
+        HZ_25   = 0x06,
+        HZ_50   = 0x07,
+        HZ_100  = 0x08,
+        HZ_200  = 0x09,
+        HZ_400  = 0x0a,
+        HZ_800  = 0x0b,
+        HZ_1600 = 0x0c,
+        HZ_3200 = 0x0d,
+    };
+
+    /// Gyroscope range expressed in °/sec
+    enum class GyrRange
+    {
+        DEG_2000,
+        DEG_1000,
+        DEG_500,
+        DEG_250,
+        DEG_125
+    };
+
+    /// Magnetometer ODR expressed in Hz
+    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx)
+    enum class MagOdr
+    {
+        HZ_25_32 = 0x01,
+        HZ_25_16 = 0x02,
+        HZ_25_8  = 0x03,
+        HZ_25_4  = 0x04,
+        HZ_25_2  = 0x05,
+        HZ_25    = 0x06,
+        HZ_50    = 0x07,
+        HZ_100   = 0x08,
+        HZ_200   = 0x09,
+        HZ_400   = 0x0a,
+        HZ_800   = 0x0b,
+    };
+
+    FifoMode fifo_mode = FifoMode::Disabled;
+
+    AccOdr acc_odr     = AccOdr::HZ_100;
+    AccRange acc_range = AccRange::G_2;
+
+    GyrOdr gyr_odr     = GyrOdr::HZ_100;
+    GyrRange gyr_range = GyrRange::DEG_2000;
+
+    MagOdr mag_odr = MagOdr::HZ_100;
+};
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 4190e27e2..c7154ceea 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -77,5 +77,11 @@ int main()
 
     TRACE("BMX160 initialized...\n");
 
+    while(1) {
+        sensor.onSimpleUpdate();
+        sensor.getData().print();
+        miosix::Thread::sleep(5000);
+    }
+
     return 0;
 }
\ No newline at end of file
-- 
GitLab


From 493816541b26303eb89980f79275fd7e758b8788 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 17 Nov 2020 22:49:08 +0100
Subject: [PATCH 03/24] [BMX160] Initial headerless fifo implementation.

---
 src/shared/sensors/BMX160/BMX160.h | 71 ++++++++++++++++++++++++++----
 src/tests/drivers/test-bmx160.cpp  |  9 +++-
 2 files changed, 69 insertions(+), 11 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index b4335d657..aba48bcfd 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -149,6 +149,28 @@ public:
             spi.write(REG_GYR_RANGE, static_cast<uint8_t>(config.gyr_range));
         }
 
+        // Setup fifo if needed
+        if (config.fifo_mode != BMX160Config::FifoMode::Disabled)
+        {
+
+            // TODO: Setup fifo downsampling (do we really need it?)
+
+            uint8_t config = 0;
+            config |= (1 << 7);  // fifo_gyr_en
+            config |= (1 << 6);  // fifo_acc_en
+            config |= (1 << 5);  // fifo_mag_en
+
+            /*if (config.fifo_mode == BMX160Config::FifoMode::Header)
+            {
+                config |= (1 << 4);  // fifo_header_en
+            }*/
+
+            spi.write(REG_FIFO_CONFIG_1, config);
+
+            // Is this really needed?
+            sendCmd(spi, Cmd::FIFO_FLUSH);
+        }
+
         return is_init = true;
     }
 
@@ -173,19 +195,22 @@ public:
         {
             case BMX160Config::FifoMode::Disabled:
             {
-                if (spi.read(REG_STATUS) & 0xE0)
-                {
-                    data = readConfig(spi);
-                }
+                // Just push one sample
+                data_buffer[0] = readData(spi);
+                data_len       = 1;
+                data_idx       = 0;
 
                 return true;
             }
 
             case BMX160Config::FifoMode::Headerless:
             {
-                // TODO: Implement me!
+                // Read whole FIFO
+                data_len =
+                    readFifo(spi, reinterpret_cast<uint8_t*>(data_buffer));
+                data_idx = 0;
 
-                return false;
+                return true;
             }
 
             case BMX160Config::FifoMode::Header:
@@ -195,16 +220,25 @@ public:
                 return false;
             }
         }
+
+        // This is unreachable
+        return false;
     }
 
-    Data getData() { return data; }
+    Data getData() { return data_buffer[data_idx++]; }
+    int getLen() { return data_len; }
 
 private:
     bool is_init = false;
     SPISlave spi_slave;
 
     BMX160Config config;
-    Data data;
+
+    // TODO: Maybe allocate dynamically?
+    // TODO: Is this really the max?
+    Data data_buffer[50];
+    int data_idx = 0;
+    int data_len = 0;
 
     enum class Cmd
     {
@@ -239,7 +273,7 @@ private:
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
-    Data readConfig(SPITransaction& spi)
+    Data readData(SPITransaction& spi)
     {
         return Data{
             spi.read(REG_DATA_MAG_X_0) | (spi.read(REG_DATA_MAG_X_1) << 8),
@@ -255,6 +289,25 @@ private:
         };
     }
 
+    int readFifo(SPITransaction& spi, uint8_t* buf)
+    {
+        int len =
+            spi.read(REG_FIFO_LENGTH_0) | (spi.read(REG_FIFO_LENGTH_1) << 8);
+
+        // Manually use SPI for transfer (implementation of "burst read")
+        spi_slave.bus.select(spi_slave.cs);
+        spi_slave.bus.write(REG_FIFO_DATA | 0x80);
+
+        for (int i = 0; i < len; i++)
+        {
+            buf[i] = spi_slave.bus.read();
+        }
+
+        spi_slave.bus.deselect(spi_slave.cs);
+
+        return len;
+    }
+
     enum Reg
     {
         REG_CHIPID = 0x00,
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index c7154ceea..904dc23e8 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -61,11 +61,14 @@ int main()
 
         // Setup CS
         cs.mode(miosix::Mode::OUTPUT);
-    }
+    };
 
     cs.high();
 
-    BMX160 sensor(bus, cs);
+    BMX160Config config;
+    config.fifo_mode = BMX160Config::FifoMode::Headerless;
+
+    BMX160 sensor(bus, cs, config);
 
     TRACE("Initializing BMX160...\n");
 
@@ -80,6 +83,8 @@ int main()
     while(1) {
         sensor.onSimpleUpdate();
         sensor.getData().print();
+        sensor.getData().print();
+        sensor.getData().print();
         miosix::Thread::sleep(5000);
     }
 
-- 
GitLab


From ff99892126677691dc0280e620840167ff803730 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 17 Nov 2020 23:00:10 +0100
Subject: [PATCH 04/24] [BMX160] Minor fixes.

---
 src/shared/sensors/BMX160/BMX160.h | 18 +++++++++---------
 src/tests/drivers/test-bmx160.cpp  |  7 ++++---
 2 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index aba48bcfd..31bb69cde 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -155,17 +155,17 @@ public:
 
             // TODO: Setup fifo downsampling (do we really need it?)
 
-            uint8_t config = 0;
-            config |= (1 << 7);  // fifo_gyr_en
-            config |= (1 << 6);  // fifo_acc_en
-            config |= (1 << 5);  // fifo_mag_en
+            uint8_t config_byte = 0;
+            config_byte |= (1 << 7);  // fifo_gyr_en
+            config_byte |= (1 << 6);  // fifo_acc_en
+            config_byte |= (1 << 5);  // fifo_mag_en
 
-            /*if (config.fifo_mode == BMX160Config::FifoMode::Header)
+            if (config.fifo_mode == BMX160Config::FifoMode::Header)
             {
-                config |= (1 << 4);  // fifo_header_en
-            }*/
+                config_byte |= (1 << 4);  // fifo_header_en
+            }
 
-            spi.write(REG_FIFO_CONFIG_1, config);
+            spi.write(REG_FIFO_CONFIG_1, config_byte);
 
             // Is this really needed?
             sendCmd(spi, Cmd::FIFO_FLUSH);
@@ -226,7 +226,7 @@ public:
     }
 
     Data getData() { return data_buffer[data_idx++]; }
-    int getLen() { return data_len; }
+    int getCount() { return data_len; }
 
 private:
     bool is_init = false;
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 904dc23e8..f638effc8 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -82,9 +82,10 @@ int main()
 
     while(1) {
         sensor.onSimpleUpdate();
-        sensor.getData().print();
-        sensor.getData().print();
-        sensor.getData().print();
+        if(sensor.getCount() > 0) {
+            sensor.getData().print();
+        }
+        
         miosix::Thread::sleep(5000);
     }
 
-- 
GitLab


From becfbd047305c97f30445d8661b4acf22e090502 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Sat, 21 Nov 2020 00:23:50 +0100
Subject: [PATCH 05/24] [BMX160] Completed implementation of single data,
 headerless and header modes

---
 src/shared/sensors/BMX160/BMX160.h     | 190 +++++++++++++++++++------
 src/shared/sensors/BMX160/BMX160Data.h |  74 ++++++++++
 src/tests/drivers/test-bmx160.cpp      |   7 +-
 3 files changed, 221 insertions(+), 50 deletions(-)
 create mode 100644 src/shared/sensors/BMX160/BMX160Data.h

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 31bb69cde..7f235c285 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -28,11 +28,14 @@
 
 #include <cassert>
 
-#include "BMX160Config.h"
+#include "BMX160Config.h" 
+#include "BMX160Data.h"
 
 class BMX160 : public Sensor
 {
 public:
+    /* WIP
+
     struct Data
     {
         int16_t mag_x;
@@ -61,6 +64,7 @@ public:
             printf("x: %d, y: %d, z: %d\n", acc_x, acc_y, acc_z);
         }
     };
+    */
 
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
         : spi_slave(bus, cs, SPIBusConfig{}), config(config)
@@ -174,12 +178,32 @@ public:
         return is_init = true;
     }
 
+    /// Warning init() must be re-invoked after a selfTest()!
     bool selfTest() override
     {
 #ifdef DEBUG
         assert(is_init && "init() was not called");
 #endif
-        // TODO: Implement me!
+
+        if(config.fifo_mode != BMX160Config::FifoMode::Disabled) {
+            TRACE("[BMX160] Self-test with fifo is not (yet?) supported!");
+            return false;
+        }
+
+        SPITransaction spi(spi_slave);
+
+        // Self-test acc
+        {
+            // The acc will complain otherwise...
+            spi.write(REG_ACC_CONF, 0x2C);
+            spi.write(REG_ACC_RANGE, 0x08);
+
+            // WIP..
+        }
+
+        // The device will enter in an unusable state after all the tests.
+        sendCmd(spi, Cmd::SOFTRESET);
+        is_init = false;
 
         return true;
     }
@@ -196,9 +220,8 @@ public:
             case BMX160Config::FifoMode::Disabled:
             {
                 // Just push one sample
-                data_buffer[0] = readData(spi);
-                data_len       = 1;
-                data_idx       = 0;
+                data_len = readData(spi, data_buffer);
+                data_idx = 0;
 
                 return true;
             }
@@ -206,8 +229,7 @@ public:
             case BMX160Config::FifoMode::Headerless:
             {
                 // Read whole FIFO
-                data_len =
-                    readFifo(spi, reinterpret_cast<uint8_t*>(data_buffer));
+                data_len = readFifo(spi, data_buffer, sizeof(data_buffer));
                 data_idx = 0;
 
                 return true;
@@ -215,18 +237,35 @@ public:
 
             case BMX160Config::FifoMode::Header:
             {
-                // TODO: Implement me!
+                data_len = readFifo(spi, data_buffer, sizeof(data_buffer));
+                data_idx = 0;
 
-                return false;
+                return true;
             }
         }
 
-        // This is unreachable
         return false;
     }
 
-    Data getData() { return data_buffer[data_idx++]; }
-    int getCount() { return data_len; }
+    bool hasData() {
+        return data_idx < data_len;
+    }
+
+    BMX160Data getData() {
+        if(config.fifo_mode == BMX160Config::FifoMode::Header) {
+            // In header mode data in the fifo is stored as packets
+            return readPacket();
+        } else {
+            BMX160Data data;
+            data = BMX160DATA_GYRO | BMX160DATA_ACCEL | BMX160DATA_MAGNET;
+
+            data.magnet = readStruct<BMX160Magnet>();
+            data.accel = readStruct<BMX160Accel>();
+            data.gyro = readStruct<BMX160Gyro>();
+
+            return data;
+        }
+    }
 
 private:
     bool is_init = false;
@@ -234,11 +273,17 @@ private:
 
     BMX160Config config;
 
-    // TODO: Maybe allocate dynamically?
+    /* WIP
+
     // TODO: Is this really the max?
     Data data_buffer[50];
     int data_idx = 0;
     int data_len = 0;
+    */
+
+    uint8_t data_buffer[1000];
+    int data_idx;
+    int data_len;
 
     enum class Cmd
     {
@@ -273,27 +318,28 @@ private:
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
-    Data readData(SPITransaction& spi)
+    /// Read the contents of the DATA register into buf
+    int readData(SPITransaction& spi, uint8_t* buf)
     {
-        return Data{
-            spi.read(REG_DATA_MAG_X_0) | (spi.read(REG_DATA_MAG_X_1) << 8),
-            spi.read(REG_DATA_MAG_Y_0) | (spi.read(REG_DATA_MAG_Y_1) << 8),
-            spi.read(REG_DATA_MAG_Z_0) | (spi.read(REG_DATA_MAG_Z_1) << 8),
-            spi.read(REG_DATA_RHALL_0) | (spi.read(REG_DATA_RHALL_1) << 8),
-            spi.read(REG_DATA_GYR_X_0) | (spi.read(REG_DATA_GYR_X_1) << 8),
-            spi.read(REG_DATA_GYR_Y_0) | (spi.read(REG_DATA_GYR_Y_1) << 8),
-            spi.read(REG_DATA_GYR_Z_0) | (spi.read(REG_DATA_GYR_Z_1) << 8),
-            spi.read(REG_DATA_ACC_X_0) | (spi.read(REG_DATA_ACC_X_1) << 8),
-            spi.read(REG_DATA_ACC_Y_0) | (spi.read(REG_DATA_ACC_Y_1) << 8),
-            spi.read(REG_DATA_ACC_Z_0) | (spi.read(REG_DATA_ACC_Z_1) << 8),
-        };
+        // Read all registers
+        for(int i = REG_DATA_START; i <= REG_DATA_END; i++) {
+            buf[i] = spi.read(i);
+        }
+
+        return REG_DATA_END - REG_DATA_START + 1;
     }
 
-    int readFifo(SPITransaction& spi, uint8_t* buf)
+    /// Read the contents of the fifo into buf
+    int readFifo(SPITransaction& spi, uint8_t* buf, size_t max)
     {
         int len =
             spi.read(REG_FIFO_LENGTH_0) | (spi.read(REG_FIFO_LENGTH_1) << 8);
 
+        TRACE("FIFO len: %d\n", len);
+
+#ifdef DEBUG
+        assert(len < max && "FIFO buffer overflow!");
+#endif
         // Manually use SPI for transfer (implementation of "burst read")
         spi_slave.bus.select(spi_slave.cs);
         spi_slave.bus.write(REG_FIFO_DATA | 0x80);
@@ -301,6 +347,7 @@ private:
         for (int i = 0; i < len; i++)
         {
             buf[i] = spi_slave.bus.read();
+            // TRACE("Byte: %d\n", buf[i]);
         }
 
         spi_slave.bus.deselect(spi_slave.cs);
@@ -308,31 +355,80 @@ private:
         return len;
     }
 
+    BMX160Data parsePacket() {
+        BMX160Data data;
+        data.mask = 0;
+
+        do {
+            // <7:6> fh_mode (0b10 -> regular 0b01 -> control)
+            // <5:2> fh_parm
+            // <1:0> fh_ext
+            uint8_t header = data_buffer[data_idx++];
+            TRACE("Header: %d\n", header);
+
+            // mask out fh_ext
+            header &= ~3;
+
+            if(header & 0x80) {
+                // This is a regular packet
+                
+                if(header & 0x10) {
+                    // This contains magnet data
+                    data.mask |= BMX160DATA_MAGNET;
+                    data.magnet = parseStruct<BMX160Magnet>();
+                }
+                
+                if(header & 0x08) {
+                    // This contains gyro data
+                    data.mask |= BMX160DATA_GYRO;
+                    data.gyro = parseStruct<BMX160Gyro>();
+                } 
+                
+                if(header & 0x04) {
+                    // This contains accel data
+                    data.mask |= BMX160DATA_ACCEL;
+                    data.accel = parseStruct<BMX160Accel>();
+                }
+
+                return data;
+
+            } else if(header & 0x40) {
+                // Mask out everything but fh_parm
+                header &= 0x1C;
+                
+                // This is a control packet
+                if(header == 0) {
+                    // Skip frame
+                    data_idx++;
+
+                } else if(header == 0x04) {
+                    // Sensortime frame
+                    data_idx += 3;
+
+                } else if(header == 0x08) {
+                    // FIFO_input_config_frame
+                    data_idx++;
+                }
+            }
+        } while(hasData());
+    }
+
+    template<typename T>
+    T parseStruct() {
+        T magnet;
+        memcpy(&magnet, &data_buffer[data_idx], sizeof(T));
+        data_idx += sizeof(T);
+    
+        return magnet;
+    }
+
     enum Reg
     {
         REG_CHIPID = 0x00,
         REG_ERR    = 0x02,
 
-        REG_DATA_MAG_X_0 = 0x04,
-        REG_DATA_MAG_X_1 = 0x05,
-        REG_DATA_MAG_Y_0 = 0x06,
-        REG_DATA_MAG_Y_1 = 0x07,
-        REG_DATA_MAG_Z_0 = 0x08,
-        REG_DATA_MAG_Z_1 = 0x09,
-        REG_DATA_RHALL_0 = 0x0A,
-        REG_DATA_RHALL_1 = 0x0B,
-        REG_DATA_GYR_X_0 = 0x0C,
-        REG_DATA_GYR_X_1 = 0x0D,
-        REG_DATA_GYR_Y_0 = 0x0E,
-        REG_DATA_GYR_Y_1 = 0x0F,
-        REG_DATA_GYR_Z_0 = 0x10,
-        REG_DATA_GYR_Z_1 = 0x11,
-        REG_DATA_ACC_X_0 = 0x12,
-        REG_DATA_ACC_X_1 = 0x13,
-        REG_DATA_ACC_Y_0 = 0x14,
-        REG_DATA_ACC_Y_1 = 0x15,
-        REG_DATA_ACC_Z_0 = 0x16,
-        REG_DATA_ACC_Z_1 = 0x17,
+        REG_DATA_START = 0x04,
+        REG_DATA_END   = 0x17,
 
         REG_SENSORTIME_0 = 0x18,
         REG_SENSORTIME_1 = 0x19,
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
new file mode 100644
index 000000000..a0e8125ae
--- /dev/null
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -0,0 +1,74 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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 <stdio.h>
+
+static const uint8_t BMX160DATA_MAGNET = 1 << 0;
+static const uint8_t BMX160DATA_ACCEL = 1 << 1;
+static const uint8_t BMX160DATA_GYRO = 1 << 2;
+
+struct BMX160Magnet {
+    int16_t x, y, z;
+    uint16_t rhall;
+};
+
+struct BMX160Accel {
+    int16_t x, y, z;
+};
+
+struct BMX160Gyro {
+    int16_t x, y, z;
+};
+
+struct BMX160Data {
+    uint8_t mask;
+
+    BMX160Magnet magnet;
+    BMX160Accel accel;
+    BMX160Gyro gyro;
+
+    void print() {
+        printf("---- Magnet:\n");
+        if(mask & BMX160DATA_MAGNET) {
+            printf("%d\t%d\t%d\t%d\n", magnet.x, magnet.y, magnet.z, magnet.rhall);
+        } else {
+            printf("x\tx\tx\tx\n");
+        }
+
+        printf("---- Accel:\n");
+        if(mask & BMX160DATA_ACCEL) {
+            printf("%d\t%d\t%d\n", accel.x, accel.y, accel.z);
+        } else {
+            printf("x\tx\tx\n");
+        }
+
+        printf("---- Gyro:\n");
+        if(mask & BMX160DATA_GYRO) {
+            printf("%d\t%d\t%d\n", gyro.x, gyro.y, gyro.z);
+        } else {
+            printf("x\tx\tx\n");
+        }
+    }
+};
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index f638effc8..84bb4d64c 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -66,7 +66,7 @@ int main()
     cs.high();
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::Headerless;
+    config.fifo_mode = BMX160Config::FifoMode::Header;
 
     BMX160 sensor(bus, cs, config);
 
@@ -80,12 +80,13 @@ int main()
 
     TRACE("BMX160 initialized...\n");
 
+    miosix::Thread::sleep(5000);
     while(1) {
         sensor.onSimpleUpdate();
-        if(sensor.getCount() > 0) {
+        while(sensor.hasData()) {
             sensor.getData().print();
         }
-        
+
         miosix::Thread::sleep(5000);
     }
 
-- 
GitLab


From b852d0a7504818df7f05cd9a63273a42cc6f3f3e Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Sat, 21 Nov 2020 00:25:55 +0100
Subject: [PATCH 06/24] [BMX160] Fixed compilation errors

---
 src/shared/sensors/BMX160/BMX160.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 7f235c285..234bda75d 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -254,14 +254,14 @@ public:
     BMX160Data getData() {
         if(config.fifo_mode == BMX160Config::FifoMode::Header) {
             // In header mode data in the fifo is stored as packets
-            return readPacket();
+            return parsePacket();
         } else {
             BMX160Data data;
-            data = BMX160DATA_GYRO | BMX160DATA_ACCEL | BMX160DATA_MAGNET;
+            data.mask = BMX160DATA_GYRO | BMX160DATA_ACCEL | BMX160DATA_MAGNET;
 
-            data.magnet = readStruct<BMX160Magnet>();
-            data.accel = readStruct<BMX160Accel>();
-            data.gyro = readStruct<BMX160Gyro>();
+            data.magnet = parseStruct<BMX160Magnet>();
+            data.accel = parseStruct<BMX160Accel>();
+            data.gyro = parseStruct<BMX160Gyro>();
 
             return data;
         }
-- 
GitLab


From 7bb34fc4dbe85a9b34ec43c4180093787c4399cd Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Sat, 21 Nov 2020 19:36:39 +0100
Subject: [PATCH 07/24] [BMX160] Changed fifo implementation to separate data
 into different data structures.

---
 src/shared/sensors/BMX160/BMX160.h     | 283 +++++++++++--------------
 src/shared/sensors/BMX160/BMX160Data.h |  70 +++---
 src/tests/drivers/test-bmx160.cpp      |  18 +-
 3 files changed, 177 insertions(+), 194 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 234bda75d..43686e7db 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -28,44 +28,12 @@
 
 #include <cassert>
 
-#include "BMX160Config.h" 
+#include "BMX160Config.h"
 #include "BMX160Data.h"
 
 class BMX160 : public Sensor
 {
 public:
-    /* WIP
-
-    struct Data
-    {
-        int16_t mag_x;
-        int16_t mag_y;
-        int16_t mag_z;
-        uint16_t rhall;
-
-        int16_t gyr_x;
-        int16_t gyr_y;
-        int16_t gyr_z;
-
-        int16_t acc_x;
-        int16_t acc_y;
-        int16_t acc_z;
-
-        void print()
-        {
-            printf("\n\n");
-            printf("BMX160 data\n");
-            printf("----MAG----\n");
-            printf("x: %d, y: %d, z: %d, rhall: %d\n", mag_x, mag_y, mag_z,
-                   rhall);
-            printf("----GYR----\n");
-            printf("x: %d, y: %d, z: %d\n", gyr_x, gyr_y, gyr_z);
-            printf("----ACC----\n");
-            printf("x: %d, y: %d, z: %d\n", acc_x, acc_y, acc_z);
-        }
-    };
-    */
-
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
         : spi_slave(bus, cs, SPIBusConfig{}), config(config)
     {
@@ -117,7 +85,7 @@ public:
             // Enable manual configuration mode
             spi.write(REG_MAG_IF_0, 0x80);
 
-            // Put MAG into sleep mode (from )
+            // Put MAG into sleep mode (from suspend mode)
             spi.write(REG_MAG_IF_3, 0x01);
             spi.write(REG_MAG_IF_2, 0x4B);
 
@@ -185,7 +153,8 @@ public:
         assert(is_init && "init() was not called");
 #endif
 
-        if(config.fifo_mode != BMX160Config::FifoMode::Disabled) {
+        if (config.fifo_mode != BMX160Config::FifoMode::Disabled)
+        {
             TRACE("[BMX160] Self-test with fifo is not (yet?) supported!");
             return false;
         }
@@ -215,57 +184,36 @@ public:
 #endif
         SPITransaction spi(spi_slave);
 
+        // Delete old samples
+        gyro_fifo.clear();
+        accel_fifo.clear();
+        magnet_fifo.clear();
+
         switch (config.fifo_mode)
         {
             case BMX160Config::FifoMode::Disabled:
-            {
                 // Just push one sample
-                data_len = readData(spi, data_buffer);
-                data_idx = 0;
-
+                readData(spi);
                 return true;
-            }
 
             case BMX160Config::FifoMode::Headerless:
-            {
-                // Read whole FIFO
-                data_len = readFifo(spi, data_buffer, sizeof(data_buffer));
-                data_idx = 0;
-
+                // Read whole FIFO (headerless)
+                readFifo(spi, true);
                 return true;
-            }
 
             case BMX160Config::FifoMode::Header:
-            {
-                data_len = readFifo(spi, data_buffer, sizeof(data_buffer));
-                data_idx = 0;
-
+                // Read whole FIFO (header)
+                readFifo(spi, false);
                 return true;
-            }
         }
 
         return false;
     }
 
-    bool hasData() {
-        return data_idx < data_len;
-    }
-
-    BMX160Data getData() {
-        if(config.fifo_mode == BMX160Config::FifoMode::Header) {
-            // In header mode data in the fifo is stored as packets
-            return parsePacket();
-        } else {
-            BMX160Data data;
-            data.mask = BMX160DATA_GYRO | BMX160DATA_ACCEL | BMX160DATA_MAGNET;
-
-            data.magnet = parseStruct<BMX160Magnet>();
-            data.accel = parseStruct<BMX160Accel>();
-            data.gyro = parseStruct<BMX160Gyro>();
-
-            return data;
-        }
-    }
+    // For the time being make this public
+    BMX160Fifo<BMX160Magnet, 50> magnet_fifo;
+    BMX160Fifo<BMX160Accel, 50> accel_fifo;
+    BMX160Fifo<BMX160Gyro, 50> gyro_fifo;
 
 private:
     bool is_init = false;
@@ -273,18 +221,6 @@ private:
 
     BMX160Config config;
 
-    /* WIP
-
-    // TODO: Is this really the max?
-    Data data_buffer[50];
-    int data_idx = 0;
-    int data_len = 0;
-    */
-
-    uint8_t data_buffer[1000];
-    int data_idx;
-    int data_len;
-
     enum class Cmd
     {
         START_FOC           = 0x03,
@@ -318,108 +254,125 @@ private:
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
-    /// Read the contents of the DATA register into buf
-    int readData(SPITransaction& spi, uint8_t* buf)
+    /// Read the contents of the DATA register
+    void readData(SPITransaction& spi)
     {
-        // Read all registers
-        for(int i = REG_DATA_START; i <= REG_DATA_END; i++) {
-            buf[i] = spi.read(i);
-        }
-
-        return REG_DATA_END - REG_DATA_START + 1;
+        magnet_fifo.push(BMX160Magnet{
+            spi.read(REG_DATA_MAG_X_0) | (spi.read(REG_DATA_MAG_X_1) << 8),
+            spi.read(REG_DATA_MAG_Y_0) | (spi.read(REG_DATA_MAG_Y_1) << 8),
+            spi.read(REG_DATA_MAG_Z_0) | (spi.read(REG_DATA_MAG_Z_1) << 8),
+            spi.read(REG_DATA_RHALL_0) | (spi.read(REG_DATA_RHALL_1) << 8)});
+
+        gyro_fifo.push(BMX160Gyro{
+            spi.read(REG_DATA_GYR_X_0) | (spi.read(REG_DATA_GYR_X_1) << 8),
+            spi.read(REG_DATA_GYR_Y_0) | (spi.read(REG_DATA_GYR_Y_1) << 8),
+            spi.read(REG_DATA_GYR_Z_0) | (spi.read(REG_DATA_GYR_Z_1) << 8),
+        });
+
+        accel_fifo.push(BMX160Accel{
+            spi.read(REG_DATA_ACC_X_0) | (spi.read(REG_DATA_ACC_X_1) << 8),
+            spi.read(REG_DATA_ACC_Y_0) | (spi.read(REG_DATA_ACC_Y_1) << 8),
+            spi.read(REG_DATA_ACC_Z_0) | (spi.read(REG_DATA_ACC_Z_1) << 8),
+        });
     }
 
     /// Read the contents of the fifo into buf
-    int readFifo(SPITransaction& spi, uint8_t* buf, size_t max)
+    void readFifo(SPITransaction& spi, bool headerless)
     {
-        int len =
-            spi.read(REG_FIFO_LENGTH_0) | (spi.read(REG_FIFO_LENGTH_1) << 8);
+        int len = spi.read(REG_FIFO_LENGTH_0) |
+                  ((spi.read(REG_FIFO_LENGTH_1) & 7) << 8);
 
-        TRACE("FIFO len: %d\n", len);
+        uint8_t buf[1000];
 
 #ifdef DEBUG
-        assert(len < max && "FIFO buffer overflow!");
+        assert(len <= sizeof(buf) && "Buffer overflow!");
 #endif
+
         // Manually use SPI for transfer (implementation of "burst read")
         spi_slave.bus.select(spi_slave.cs);
         spi_slave.bus.write(REG_FIFO_DATA | 0x80);
 
         for (int i = 0; i < len; i++)
-        {
             buf[i] = spi_slave.bus.read();
-            // TRACE("Byte: %d\n", buf[i]);
-        }
 
         spi_slave.bus.deselect(spi_slave.cs);
 
-        return len;
-    }
-
-    BMX160Data parsePacket() {
-        BMX160Data data;
-        data.mask = 0;
+        int idx = 0;
+        while (idx < len && buf[idx] != 128)
+        {
 
-        do {
-            // <7:6> fh_mode (0b10 -> regular 0b01 -> control)
-            // <5:2> fh_parm
-            // <1:0> fh_ext
-            uint8_t header = data_buffer[data_idx++];
-            TRACE("Header: %d\n", header);
+            if (headerless)
+            {
+                magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
+                gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
+                accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
+            }
+            else
+            {
+                // <7:6> fh_mode (0b10 -> regular 0b01 -> control)
+                // <5:2> fh_parm
+                // <1:0> fh_ext
+                uint8_t header = buf[idx++];
+                // Mask out fh_ext
+                header &= ~3;
 
-            // mask out fh_ext
-            header &= ~3;
+                if (header & 0x80)
+                {
+                    // This is a regular packet
 
-            if(header & 0x80) {
-                // This is a regular packet
-                
-                if(header & 0x10) {
                     // This contains magnet data
-                    data.mask |= BMX160DATA_MAGNET;
-                    data.magnet = parseStruct<BMX160Magnet>();
-                }
-                
-                if(header & 0x08) {
+                    if (header & 0x10)
+                        magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
+
                     // This contains gyro data
-                    data.mask |= BMX160DATA_GYRO;
-                    data.gyro = parseStruct<BMX160Gyro>();
-                } 
-                
-                if(header & 0x04) {
+                    if (header & 0x08)
+                        gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
+
                     // This contains accel data
-                    data.mask |= BMX160DATA_ACCEL;
-                    data.accel = parseStruct<BMX160Accel>();
+                    if (header & 0x04)
+                        accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
                 }
+                else if (header & 0x40)
+                {
+                    // Mask out everything but fh_parm
+                    header &= 0x1C;
+
+                    // This is a control packet
+                    if (header == 0)
+                    {
+                        // Skip frame
+                        idx += 1;
+                    }
+                    else if (header == 0x04)
+                    {
+                        // Sensortime frame
+                        idx += 3;
+                    }
+                    else if (header == 0x08)
+                    {
+                        // FIFO_input_config_frame
+                        idx += 1;
+                    }
+                }
+                else
+                {
+                    TRACE("[BMX160] Malformed packet! Aborting fifo transfer...\n");
+                    break;
 
-                return data;
-
-            } else if(header & 0x40) {
-                // Mask out everything but fh_parm
-                header &= 0x1C;
-                
-                // This is a control packet
-                if(header == 0) {
-                    // Skip frame
-                    data_idx++;
-
-                } else if(header == 0x04) {
-                    // Sensortime frame
-                    data_idx += 3;
-
-                } else if(header == 0x08) {
-                    // FIFO_input_config_frame
-                    data_idx++;
+                    // TODO: Maybe find a way of recovering from this?
                 }
             }
-        } while(hasData());
+        }
     }
 
-    template<typename T>
-    T parseStruct() {
-        T magnet;
-        memcpy(&magnet, &data_buffer[data_idx], sizeof(T));
-        data_idx += sizeof(T);
-    
-        return magnet;
+    template <typename T>
+    T parseStruct(uint8_t* buf, int& idx)
+    {
+        T data;
+        memcpy(&data, buf + idx, sizeof(T));
+        idx += sizeof(T);
+
+        return data;
     }
 
     enum Reg
@@ -427,8 +380,26 @@ private:
         REG_CHIPID = 0x00,
         REG_ERR    = 0x02,
 
-        REG_DATA_START = 0x04,
-        REG_DATA_END   = 0x17,
+        REG_DATA_MAG_X_0 = 0x04,
+        REG_DATA_MAG_X_1 = 0x05,
+        REG_DATA_MAG_Y_0 = 0x06,
+        REG_DATA_MAG_Y_1 = 0x07,
+        REG_DATA_MAG_Z_0 = 0x08,
+        REG_DATA_MAG_Z_1 = 0x09,
+        REG_DATA_RHALL_0 = 0x0A,
+        REG_DATA_RHALL_1 = 0x0B,
+        REG_DATA_GYR_X_0 = 0x0C,
+        REG_DATA_GYR_X_1 = 0x0D,
+        REG_DATA_GYR_Y_0 = 0x0E,
+        REG_DATA_GYR_Y_1 = 0x0F,
+        REG_DATA_GYR_Z_0 = 0x10,
+        REG_DATA_GYR_Z_1 = 0x11,
+        REG_DATA_ACC_X_0 = 0x12,
+        REG_DATA_ACC_X_1 = 0x13,
+        REG_DATA_ACC_Y_0 = 0x14,
+        REG_DATA_ACC_Y_1 = 0x15,
+        REG_DATA_ACC_Z_0 = 0x16,
+        REG_DATA_ACC_Z_1 = 0x17,
 
         REG_SENSORTIME_0 = 0x18,
         REG_SENSORTIME_1 = 0x19,
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index a0e8125ae..a797b0b18 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -25,50 +25,56 @@
 
 #include <stdio.h>
 
-static const uint8_t BMX160DATA_MAGNET = 1 << 0;
-static const uint8_t BMX160DATA_ACCEL = 1 << 1;
-static const uint8_t BMX160DATA_GYRO = 1 << 2;
-
-struct BMX160Magnet {
+struct BMX160Magnet
+{
     int16_t x, y, z;
     uint16_t rhall;
+
+    void print() { printf("MAGNET:\t%d\t%d\t%d\t%d\n", x, y, z, rhall); }
 };
 
-struct BMX160Accel {
+struct BMX160Accel
+{
     int16_t x, y, z;
+
+    void print() { printf("ACCEL:\t%d\t%d\t%d\n", x, y, z); }
 };
 
-struct BMX160Gyro {
+struct BMX160Gyro
+{
     int16_t x, y, z;
+
+    void print() { printf("GYRO:\t%d\t%d\t%d\n", x, y, z); }
 };
 
-struct BMX160Data {
-    uint8_t mask;
+/// Class representing a BMX160 Data fifo
+template <typename T, size_t N>
+class BMX160Fifo
+{
+public:
+    BMX160Fifo() : len(0), idx(0) {}
 
-    BMX160Magnet magnet;
-    BMX160Accel accel;
-    BMX160Gyro gyro;
+    void push(T sample)
+    {
+#ifdef DEBUG
+        assert(len < N && "FIFO buffer overflow");
+#endif
+        data[len++] = sample;
+    }
 
-    void print() {
-        printf("---- Magnet:\n");
-        if(mask & BMX160DATA_MAGNET) {
-            printf("%d\t%d\t%d\t%d\n", magnet.x, magnet.y, magnet.z, magnet.rhall);
-        } else {
-            printf("x\tx\tx\tx\n");
-        }
+    T pop()
+    {
+#ifdef DEBUG
+        assert(len < N && "FIFO buffer overflow");
+#endif
+        return data[idx++];
+    }
 
-        printf("---- Accel:\n");
-        if(mask & BMX160DATA_ACCEL) {
-            printf("%d\t%d\t%d\n", accel.x, accel.y, accel.z);
-        } else {
-            printf("x\tx\tx\n");
-        }
+    void clear() { len = idx = 0; }
+    size_t count() { return len; }
 
-        printf("---- Gyro:\n");
-        if(mask & BMX160DATA_GYRO) {
-            printf("%d\t%d\t%d\n", gyro.x, gyro.y, gyro.z);
-        } else {
-            printf("x\tx\tx\n");
-        }
-    }
+private:
+    T data[N];
+    size_t len;
+    size_t idx;
 };
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 84bb4d64c..118e07190 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -80,14 +80,20 @@ int main()
 
     TRACE("BMX160 initialized...\n");
 
-    miosix::Thread::sleep(5000);
-    while(1) {
+    while (1)
+    {
+        miosix::Thread::sleep(5000);
+
         sensor.onSimpleUpdate();
-        while(sensor.hasData()) {
-            sensor.getData().print();
-        }
 
-        miosix::Thread::sleep(5000);
+        // Show only the first 5 samples
+        for (int i = 0; i < 5; i++)
+        {
+            printf("----------------------------\n");
+            sensor.magnet_fifo.pop().print();
+            sensor.accel_fifo.pop().print();
+            sensor.gyro_fifo.pop().print();
+        }
     }
 
     return 0;
-- 
GitLab


From 1ece69bee57d961cfd30d73b3d2b1dd3e5d02fd8 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 23 Nov 2020 12:16:01 +0100
Subject: [PATCH 08/24] [BMX160] Minor fixes - Improved readData implementation
 - Swapped manual bus operation with SPITransaction::read

---
 src/shared/sensors/BMX160/BMX160.h | 58 +++++++-----------------------
 src/tests/drivers/test-bmx160.cpp  |  2 +-
 2 files changed, 13 insertions(+), 47 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 43686e7db..0ea2b8b1a 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -257,23 +257,13 @@ private:
     /// Read the contents of the DATA register
     void readData(SPITransaction& spi)
     {
-        magnet_fifo.push(BMX160Magnet{
-            spi.read(REG_DATA_MAG_X_0) | (spi.read(REG_DATA_MAG_X_1) << 8),
-            spi.read(REG_DATA_MAG_Y_0) | (spi.read(REG_DATA_MAG_Y_1) << 8),
-            spi.read(REG_DATA_MAG_Z_0) | (spi.read(REG_DATA_MAG_Z_1) << 8),
-            spi.read(REG_DATA_RHALL_0) | (spi.read(REG_DATA_RHALL_1) << 8)});
-
-        gyro_fifo.push(BMX160Gyro{
-            spi.read(REG_DATA_GYR_X_0) | (spi.read(REG_DATA_GYR_X_1) << 8),
-            spi.read(REG_DATA_GYR_Y_0) | (spi.read(REG_DATA_GYR_Y_1) << 8),
-            spi.read(REG_DATA_GYR_Z_0) | (spi.read(REG_DATA_GYR_Z_1) << 8),
-        });
-
-        accel_fifo.push(BMX160Accel{
-            spi.read(REG_DATA_ACC_X_0) | (spi.read(REG_DATA_ACC_X_1) << 8),
-            spi.read(REG_DATA_ACC_Y_0) | (spi.read(REG_DATA_ACC_Y_1) << 8),
-            spi.read(REG_DATA_ACC_Z_0) | (spi.read(REG_DATA_ACC_Z_1) << 8),
-        });
+        uint8_t buf[20];
+        spi.read(REG_DATA, buf, sizeof(buf));
+
+        int idx = 0;
+        magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
+        gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
+        accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
     }
 
     /// Read the contents of the fifo into buf
@@ -288,14 +278,7 @@ private:
         assert(len <= sizeof(buf) && "Buffer overflow!");
 #endif
 
-        // Manually use SPI for transfer (implementation of "burst read")
-        spi_slave.bus.select(spi_slave.cs);
-        spi_slave.bus.write(REG_FIFO_DATA | 0x80);
-
-        for (int i = 0; i < len; i++)
-            buf[i] = spi_slave.bus.read();
-
-        spi_slave.bus.deselect(spi_slave.cs);
+        spi.read(REG_FIFO_DATA, buf, sizeof(buf));
 
         int idx = 0;
         while (idx < len && buf[idx] != 128)
@@ -356,7 +339,9 @@ private:
                 }
                 else
                 {
-                    TRACE("[BMX160] Malformed packet! Aborting fifo transfer...\n");
+                    TRACE(
+                        "[BMX160] Malformed packet! Aborting fifo "
+                        "transfer...\n");
                     break;
 
                     // TODO: Maybe find a way of recovering from this?
@@ -380,26 +365,7 @@ private:
         REG_CHIPID = 0x00,
         REG_ERR    = 0x02,
 
-        REG_DATA_MAG_X_0 = 0x04,
-        REG_DATA_MAG_X_1 = 0x05,
-        REG_DATA_MAG_Y_0 = 0x06,
-        REG_DATA_MAG_Y_1 = 0x07,
-        REG_DATA_MAG_Z_0 = 0x08,
-        REG_DATA_MAG_Z_1 = 0x09,
-        REG_DATA_RHALL_0 = 0x0A,
-        REG_DATA_RHALL_1 = 0x0B,
-        REG_DATA_GYR_X_0 = 0x0C,
-        REG_DATA_GYR_X_1 = 0x0D,
-        REG_DATA_GYR_Y_0 = 0x0E,
-        REG_DATA_GYR_Y_1 = 0x0F,
-        REG_DATA_GYR_Z_0 = 0x10,
-        REG_DATA_GYR_Z_1 = 0x11,
-        REG_DATA_ACC_X_0 = 0x12,
-        REG_DATA_ACC_X_1 = 0x13,
-        REG_DATA_ACC_Y_0 = 0x14,
-        REG_DATA_ACC_Y_1 = 0x15,
-        REG_DATA_ACC_Z_0 = 0x16,
-        REG_DATA_ACC_Z_1 = 0x17,
+        REG_DATA = 0x04,
 
         REG_SENSORTIME_0 = 0x18,
         REG_SENSORTIME_1 = 0x19,
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 118e07190..593155b6f 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -87,7 +87,7 @@ int main()
         sensor.onSimpleUpdate();
 
         // Show only the first 5 samples
-        for (int i = 0; i < 5; i++)
+        for (int i = 0; i < 1; i++)
         {
             printf("----------------------------\n");
             sensor.magnet_fifo.pop().print();
-- 
GitLab


From 1ae9ad0718ff868109c6554c8b0a476687789fd2 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 24 Nov 2020 23:32:42 +0100
Subject: [PATCH 09/24] [BMX160] Implemented selfTest and fixed small init bug
 related to magnetometer.

---
 src/shared/sensors/BMX160/BMX160.h | 171 +++++++++++++++++++++++++++--
 src/tests/drivers/test-bmx160.cpp  |  24 +++-
 2 files changed, 181 insertions(+), 14 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 0ea2b8b1a..01d5df758 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -63,15 +63,31 @@ public:
             }
         }
 
+        // Reset the state of the device, just to be sure.
+        sendCmd(spi, Cmd::SOFTRESET);
+        miosix::Thread::sleep(10);
+
+        // Dummy read of REG_COMM_TEST to enable SPI
+        spi.read(REG_COMM_TEST);
+        miosix::Thread::sleep(10);
+
         // Enable Gyro and Accel and Magneto interface
         {
-            sendCmd(spi, Cmd::ACC_SET_PMU_MODE, Pmu::NORMAL);
-            sendCmd(spi, Cmd::GYR_SET_PMU_MODE, Pmu::NORMAL);
             sendCmd(spi, Cmd::MAG_IF_SET_PMU_MODE, Pmu::NORMAL);
+            miosix::Thread::sleep(80);
+
+            sendCmd(spi, Cmd::GYR_SET_PMU_MODE, Pmu::NORMAL);
+            miosix::Thread::sleep(80);
+
+            sendCmd(spi, Cmd::ACC_SET_PMU_MODE, Pmu::NORMAL);
+            miosix::Thread::sleep(80);
         }
 
-        // Wait for interfaces to come up...
-        miosix::Thread::sleep(5);
+#ifdef DEBUG
+        // Make sure that all sensors are working!
+        assert((spi.read(REG_PMU_STATUS) & 0x3F) == 0x15 &&
+               "Not all sensors are up and running!");
+#endif
 
         // Init magnetometer
         {
@@ -84,29 +100,35 @@ public:
 
             // Enable manual configuration mode
             spi.write(REG_MAG_IF_0, 0x80);
+            miosix::Thread::sleep(10);
 
             // Put MAG into sleep mode (from suspend mode)
             spi.write(REG_MAG_IF_3, 0x01);
             spi.write(REG_MAG_IF_2, 0x4B);
+            miosix::Thread::sleep(10);
 
             // Load high accuracy preset for REPXY
             spi.write(REG_MAG_IF_3, 0x17);
             spi.write(REG_MAG_IF_2, 0x51);
+            miosix::Thread::sleep(10);
 
             // Load high accuracy preset for REPZ
             spi.write(REG_MAG_IF_3, 0x52);
             spi.write(REG_MAG_IF_2, 0x52);
+            miosix::Thread::sleep(10);
 
             // Magic sequence to init it
             spi.write(REG_MAG_IF_3, 0x02);
             spi.write(REG_MAG_IF_2, 0x4C);
             spi.write(REG_MAG_IF_1, 0x42);
+            miosix::Thread::sleep(10);
 
             // Set mag output data rate
             spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
 
             // Disable manual configuration mode
             spi.write(REG_MAG_IF_0, 0x00);
+            miosix::Thread::sleep(10);
         }
 
         // Setup accel
@@ -159,20 +181,105 @@ public:
             return false;
         }
 
+        // The device will enter in an unusable state when testing.
+        is_init = false;
+
         SPITransaction spi(spi_slave);
 
         // Self-test acc
         {
+            const uint16_t SELF_TEST_LIMIT = 8192;
+
             // The acc will complain otherwise...
             spi.write(REG_ACC_CONF, 0x2C);
             spi.write(REG_ACC_RANGE, 0x08);
 
-            // WIP..
+            // Enable acc self-test + positive force + self-test deflection
+            spi.write(REG_SELF_TEST, 0x0D);
+            miosix::Thread::sleep(50);
+
+            int16_t pos_acc[3];
+            spi.read(REG_DATA_ACC, reinterpret_cast<uint8_t*>(pos_acc),
+                     sizeof(pos_acc));
+
+            // Enable acc self-test + negative force + self-test deflection
+            spi.write(REG_SELF_TEST, 0x09);
+            miosix::Thread::sleep(50);
+
+            int16_t neg_acc[3];
+            spi.read(REG_DATA_ACC, reinterpret_cast<uint8_t*>(neg_acc),
+                     sizeof(neg_acc));
+
+            if ((neg_acc[0] - pos_acc[0]) < SELF_TEST_LIMIT ||
+                (neg_acc[1] - pos_acc[1]) < SELF_TEST_LIMIT ||
+                (neg_acc[2] - pos_acc[2]) < SELF_TEST_LIMIT)
+            {
+                TRACE("[BMX160] Accelerometer self-test failed!\n");
+                TRACE("pos_acc: %d %d %d\n", pos_acc[0], pos_acc[1],
+                      pos_acc[2]);
+                TRACE("neg_acc: %d %d %d\n", neg_acc[0], neg_acc[1],
+                      neg_acc[2]);
+                return false;
+            }
+
+            // Reset self-test
+            spi.write(REG_SELF_TEST, 0);
         }
 
-        // The device will enter in an unusable state after all the tests.
-        sendCmd(spi, Cmd::SOFTRESET);
-        is_init = false;
+        // Self-test gyro
+        {
+            // Start gyro self-test
+            spi.write(REG_SELF_TEST, 0x10);
+
+            miosix::Thread::sleep(50);
+
+            // Read back the results
+            if (!(spi.read(REG_STATUS) & 2))
+            {
+                TRACE("[BMX160] Gyroscope self-test failed!\n");
+                return false;
+            }
+        }
+
+        // Self-test magneto
+        {
+            // Enable manual configuration mode
+            spi.write(REG_MAG_IF_0, 0x80);
+            miosix::Thread::sleep(10);
+
+            // Enable self-test and put magnetometer in sleep
+            spi.write(REG_MAG_IF_3, 0x07);
+            spi.write(REG_MAG_IF_2, 0x4C);
+            miosix::Thread::sleep(200);
+
+            // Check if it has finished
+            spi.write(REG_MAG_IF_1, 0x4C);
+            miosix::Thread::sleep(10);
+
+            if (spi.read(REG_DATA_MAG) & 1)
+            {
+                TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
+                return false;
+            }
+
+            // Now enable 8 byte burst reads
+            spi.write(REG_MAG_IF_0, 0x83);
+            miosix::Thread::sleep(10);
+
+            // Read back test results
+            spi.write(REG_MAG_IF_1, 0x42);
+            miosix::Thread::sleep(10);
+
+            int16_t mag[4];
+            spi.read(REG_DATA_MAG, reinterpret_cast<uint8_t*>(mag), sizeof(mag));
+
+            if (!(mag[0] & 1) || !(mag[1] & 1) || !(mag[2] & 1))
+            {
+                TRACE("[BMX160] Magnetometer self-test failed!\n");
+                TRACE("result: %d %d %d %d\n", mag[0], mag[1], mag[2], mag[3]);
+                return false;
+            }
+        }
 
         return true;
     }
@@ -254,6 +361,42 @@ private:
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
+    /// Debug function used to print the current error state
+    const char* debugErr(SPITransaction& spi)
+    {
+        uint8_t err = spi.read(REG_ERR);
+
+        if (err & 1)
+        {
+            return "Chip not operable";
+        }
+        else if (err & 64)
+        {
+            return "Dropped command to register 0x7E";
+        }
+        else
+        {
+            // Mask error code
+            err = (err >> 1) & 0x0F;
+            switch (err)
+            {
+                case 0:
+                    return "No error";
+                case 1:
+                case 2:
+                    return "Generic error";
+                case 3:
+                    return "LPM and interrupt uses pre-filtered data";
+                case 6:
+                    return "ODR do not match";
+                case 7:
+                    return "LPM uses pre-filtered data";
+                default:
+                    return "Reserved error";
+            }
+        }
+    }
+
     /// Read the contents of the DATA register
     void readData(SPITransaction& spi)
     {
@@ -362,11 +505,16 @@ private:
 
     enum Reg
     {
-        REG_CHIPID = 0x00,
-        REG_ERR    = 0x02,
+        REG_CHIPID     = 0x00,
+        REG_ERR        = 0x02,
+        REG_PMU_STATUS = 0x03,
 
         REG_DATA = 0x04,
 
+        REG_DATA_MAG = 0x04,
+        REG_DATA_GYR = 0x0C,
+        REG_DATA_ACC = 0x12,
+
         REG_SENSORTIME_0 = 0x18,
         REG_SENSORTIME_1 = 0x19,
         REG_SENSORTIME_2 = 0x1A,
@@ -427,6 +575,7 @@ private:
         /* STEP_CNT_[0-1] not needed */
         /* STEP_CONF_[0-1] not needed */
 
-        REG_CMD = 0x7E
+        REG_CMD       = 0x7E,
+        REG_COMM_TEST = 0x7F,
     };
 };
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 593155b6f..86be52425 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -66,7 +66,7 @@ int main()
     cs.high();
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::Header;
+    config.fifo_mode = BMX160Config::FifoMode::Disabled;
 
     BMX160 sensor(bus, cs, config);
 
@@ -78,12 +78,30 @@ int main()
         return -1;
     }
 
-    TRACE("BMX160 initialized...\n");
+    // Perform self-test only when fifo disabled.
+    if(config.fifo_mode == BMX160Config::FifoMode::Disabled) {
+        TRACE("Performing self-test...\n");
+
+        if (!sensor.selfTest()) {
+            TRACE("Self-test failed! (code: %d)\n", sensor.getLastError());
+            return -1;
+        }
+
+        TRACE("Self-test successful!\n");
+        TRACE("Re-initialization required...\n");
+
+        if(!sensor.init()) {
+            TRACE("Init failed! (code: %d)\n", sensor.getLastError());
+            return -1;
+        }
+
+    }
+    
+    TRACE("Initialization successful! Ready to process updates...\n");
 
     while (1)
     {
         miosix::Thread::sleep(5000);
-
         sensor.onSimpleUpdate();
 
         // Show only the first 5 samples
-- 
GitLab


From 9c31d1f78860f47a1de4f919ace966dfa4debc19 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 4 Dec 2020 00:22:41 +0100
Subject: [PATCH 10/24] [BMX160] Iplemented interrupts.

---
 src/shared/sensors/BMX160/BMX160.h       | 36 +++++++++++---
 src/shared/sensors/BMX160/BMX160Config.h | 27 ++++++++--
 src/shared/sensors/BMX160/BMX160Data.h   |  2 +-
 src/tests/drivers/test-bmx160.cpp        | 63 ++++++++++++++----------
 4 files changed, 91 insertions(+), 37 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 01d5df758..25cfd6e2e 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -144,7 +144,7 @@ public:
         }
 
         // Setup fifo if needed
-        if (config.fifo_mode != BMX160Config::FifoMode::Disabled)
+        if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
         {
 
             // TODO: Setup fifo downsampling (do we really need it?)
@@ -154,7 +154,7 @@ public:
             config_byte |= (1 << 6);  // fifo_acc_en
             config_byte |= (1 << 5);  // fifo_mag_en
 
-            if (config.fifo_mode == BMX160Config::FifoMode::Header)
+            if (config.fifo_mode == BMX160Config::FifoMode::HEADER)
             {
                 config_byte |= (1 << 4);  // fifo_header_en
             }
@@ -165,6 +165,28 @@ public:
             sendCmd(spi, Cmd::FIFO_FLUSH);
         }
 
+        // Enable both interrupt pins, otherwise they'll just float.
+        spi.write(REG_INT_OUT_CTRL, 0x88);
+
+        // Setup interrupts if needed
+        if (config.fifo_int != BMX160Config::FifoInt::DISABLED) 
+        {
+            // Set fifo watermark
+            spi.write(REG_FIFO_CONFIG_0, config.fifo_watermark);
+            
+            // Enable FIFO full interrupt and fifo watermark
+            spi.write(REG_INT_EN_1, 1 << 5 | 1 << 6);
+
+            // Enable interrupt pin map
+            if(config.fifo_int == BMX160Config::FifoInt::PIN_INT1) {
+                // Configure to use INT1
+                spi.write(REG_INT_MAP_1, 1 << 5 | 1 << 6);
+            } else {
+                // Configure to use INT2
+                spi.write(REG_INT_MAP_1, 1 << 1 | 1 << 2);
+            }
+        }
+
         return is_init = true;
     }
 
@@ -175,7 +197,7 @@ public:
         assert(is_init && "init() was not called");
 #endif
 
-        if (config.fifo_mode != BMX160Config::FifoMode::Disabled)
+        if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
         {
             TRACE("[BMX160] Self-test with fifo is not (yet?) supported!");
             return false;
@@ -298,17 +320,17 @@ public:
 
         switch (config.fifo_mode)
         {
-            case BMX160Config::FifoMode::Disabled:
+            case BMX160Config::FifoMode::DISABLED:
                 // Just push one sample
                 readData(spi);
                 return true;
 
-            case BMX160Config::FifoMode::Headerless:
+            case BMX160Config::FifoMode::HEADERLESS:
                 // Read whole FIFO (headerless)
                 readFifo(spi, true);
                 return true;
 
-            case BMX160Config::FifoMode::Header:
+            case BMX160Config::FifoMode::HEADER:
                 // Read whole FIFO (header)
                 readFifo(spi, false);
                 return true;
@@ -421,7 +443,7 @@ private:
         assert(len <= sizeof(buf) && "Buffer overflow!");
 #endif
 
-        spi.read(REG_FIFO_DATA, buf, sizeof(buf));
+        spi.read(REG_FIFO_DATA, buf, len);
 
         int idx = 0;
         while (idx < len && buf[idx] != 128)
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 2dbb171f6..82ebe69b9 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -31,13 +31,24 @@ struct BMX160Config
     enum class FifoMode
     {
         /// The fifo is completely disabled
-        Disabled,
+        DISABLED,
         /// the fifo operations have less overhead,
         /// but EVERY sensor MUST have the same odr!!
-        Headerless,
+        HEADERLESS,
         /// the fifo operations have more overhead,
         /// but sensors con have different odr
-        Header
+        HEADER
+    };
+
+    /// Fifo interrupt mode (uses fifo full as trigger)
+    enum class FifoInt
+    {
+        /// The interrupts are completely disabled
+        DISABLED,
+        /// Interrupts are enabled on pin 2
+        PIN_INT1,
+        /// Interrupts are enabled on pin 1
+        PIN_INT2
     };
 
     /// Accellerometer ODR expressed in Hz
@@ -107,7 +118,15 @@ struct BMX160Config
         HZ_800   = 0x0b,
     };
 
-    FifoMode fifo_mode = FifoMode::Disabled;
+    /// Fifo watermark to use, in multiples of 4.
+    ///
+    /// Only values between [0-250] make sense to use.
+    /// A really high watermark value (the default) will
+    /// disable it, falling back to FIFO full
+    uint8_t fifo_watermark = -1;
+
+    FifoMode fifo_mode = FifoMode::DISABLED;
+    FifoInt fifo_int   = FifoInt::DISABLED;
 
     AccOdr acc_odr     = AccOdr::HZ_100;
     AccRange acc_range = AccRange::G_2;
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index a797b0b18..56dd7d639 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -65,7 +65,7 @@ public:
     T pop()
     {
 #ifdef DEBUG
-        assert(len < N && "FIFO buffer overflow");
+        assert(idx < N && "FIFO buffer underflow");
 #endif
         return data[idx++];
     }
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 86be52425..2718a462e 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -29,10 +29,11 @@ Wiring on STM32F407VG Discovery:
 | MOSI (5)    | SPI3_MOSI (PB5) |
 | SCK  (6)    | SPI3_SCK  (PB3) |
 | CS   (7)    | GPIO      (PB7) |
-| INT1 (20)   | TBD             |
+| INT1 (20)   | GPIO      (PB8) |
 */
 
 #include <Common.h>
+#include <drivers/interrupt/external_interrupts.h>
 #include <sensors/BMX160/BMX160.h>
 
 SPIBus bus(SPI3);
@@ -42,9 +43,15 @@ GpioPin spi_sck(GPIOB_BASE, 3);
 GpioPin spi_miso(GPIOB_BASE, 4);
 GpioPin spi_mosi(GPIOB_BASE, 5);
 
+BMX160 *sensor = nullptr;
+bool invoked   = false;
+
+void __attribute__((used)) EXTI8_IRQHandlerImpl() { 
+    invoked = true;
+}
+
 int main()
 {
-
     {
         miosix::FastInterruptDisableLock _lock;
 
@@ -61,57 +68,63 @@ int main()
 
         // Setup CS
         cs.mode(miosix::Mode::OUTPUT);
-    };
+    };    
 
     cs.high();
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::Disabled;
-
-    BMX160 sensor(bus, cs, config);
+    config.fifo_mode = BMX160Config::FifoMode::HEADER;
+    config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
+    
+    sensor = new BMX160(bus, cs, config);
 
     TRACE("Initializing BMX160...\n");
 
-    if (!sensor.init())
+    if (!sensor->init())
     {
-        TRACE("Init failed! (code: %d)\n", sensor.getLastError());
+        TRACE("Init failed! (code: %d)\n", sensor->getLastError());
         return -1;
     }
 
     // Perform self-test only when fifo disabled.
-    if(config.fifo_mode == BMX160Config::FifoMode::Disabled) {
+    if (config.fifo_mode == BMX160Config::FifoMode::DISABLED)
+    {
         TRACE("Performing self-test...\n");
 
-        if (!sensor.selfTest()) {
-            TRACE("Self-test failed! (code: %d)\n", sensor.getLastError());
+        if (!sensor->selfTest())
+        {
+            TRACE("Self-test failed! (code: %d)\n", sensor->getLastError());
             return -1;
         }
 
         TRACE("Self-test successful!\n");
         TRACE("Re-initialization required...\n");
 
-        if(!sensor.init()) {
-            TRACE("Init failed! (code: %d)\n", sensor.getLastError());
+        if (!sensor->init())
+        {
+            TRACE("Init failed! (code: %d)\n", sensor->getLastError());
             return -1;
         }
-
     }
-    
+
     TRACE("Initialization successful! Ready to process updates...\n");
 
+    // Just now enable interrupts
+    enableExternalInterrupt(GPIOB_BASE, 8, InterruptTrigger::RISING_EDGE);
+
     while (1)
     {
-        miosix::Thread::sleep(5000);
-        sensor.onSimpleUpdate();
+        miosix::Thread::sleep(1000);
 
-        // Show only the first 5 samples
-        for (int i = 0; i < 1; i++)
-        {
-            printf("----------------------------\n");
-            sensor.magnet_fifo.pop().print();
-            sensor.accel_fifo.pop().print();
-            sensor.gyro_fifo.pop().print();
-        }
+        TRACE("Interrupt invoked: %d\n", invoked);
+        invoked = false;
+
+        sensor->onSimpleUpdate();
+
+        printf("----------------------------\n");
+        sensor->magnet_fifo.pop().print();
+        sensor->accel_fifo.pop().print();
+        sensor->gyro_fifo.pop().print();
     }
 
     return 0;
-- 
GitLab


From 9a3e27d66918ec7e9ef371f55929f4e2e1825f94 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 4 Dec 2020 00:41:58 +0100
Subject: [PATCH 11/24] [BMX160] Factored out magnetometer functions for better
 code.

---
 src/shared/sensors/BMX160/BMX160.h | 83 ++++++++++++++++++------------
 1 file changed, 50 insertions(+), 33 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 25cfd6e2e..f51e0af4d 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -99,36 +99,26 @@ public:
             */
 
             // Enable manual configuration mode
-            spi.write(REG_MAG_IF_0, 0x80);
-            miosix::Thread::sleep(10);
+            confMag(spi, 0x80);
 
             // Put MAG into sleep mode (from suspend mode)
-            spi.write(REG_MAG_IF_3, 0x01);
-            spi.write(REG_MAG_IF_2, 0x4B);
-            miosix::Thread::sleep(10);
+            writeMag(spi, MAG_REG_RESET_0, 0x01);
 
             // Load high accuracy preset for REPXY
-            spi.write(REG_MAG_IF_3, 0x17);
-            spi.write(REG_MAG_IF_2, 0x51);
-            miosix::Thread::sleep(10);
+            writeMag(spi, MAG_REG_REPXY, 0x17);
 
             // Load high accuracy preset for REPZ
-            spi.write(REG_MAG_IF_3, 0x52);
-            spi.write(REG_MAG_IF_2, 0x52);
-            miosix::Thread::sleep(10);
+            writeMag(spi, MAG_REG_REPZ, 0x52);
 
             // Magic sequence to init it
-            spi.write(REG_MAG_IF_3, 0x02);
-            spi.write(REG_MAG_IF_2, 0x4C);
-            spi.write(REG_MAG_IF_1, 0x42);
-            miosix::Thread::sleep(10);
+            writeMag(spi, MAG_REG_CONTROL_1, 0x02);
+            readMag(spi, MAG_REG_DATA);
 
             // Set mag output data rate
             spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
 
             // Disable manual configuration mode
-            spi.write(REG_MAG_IF_0, 0x00);
-            miosix::Thread::sleep(10);
+            confMag(spi, 0x00);
         }
 
         // Setup accel
@@ -169,19 +159,22 @@ public:
         spi.write(REG_INT_OUT_CTRL, 0x88);
 
         // Setup interrupts if needed
-        if (config.fifo_int != BMX160Config::FifoInt::DISABLED) 
+        if (config.fifo_int != BMX160Config::FifoInt::DISABLED)
         {
             // Set fifo watermark
             spi.write(REG_FIFO_CONFIG_0, config.fifo_watermark);
-            
+
             // Enable FIFO full interrupt and fifo watermark
             spi.write(REG_INT_EN_1, 1 << 5 | 1 << 6);
 
             // Enable interrupt pin map
-            if(config.fifo_int == BMX160Config::FifoInt::PIN_INT1) {
+            if (config.fifo_int == BMX160Config::FifoInt::PIN_INT1)
+            {
                 // Configure to use INT1
                 spi.write(REG_INT_MAP_1, 1 << 5 | 1 << 6);
-            } else {
+            }
+            else
+            {
                 // Configure to use INT2
                 spi.write(REG_INT_MAP_1, 1 << 1 | 1 << 2);
             }
@@ -266,18 +259,14 @@ public:
         // Self-test magneto
         {
             // Enable manual configuration mode
-            spi.write(REG_MAG_IF_0, 0x80);
-            miosix::Thread::sleep(10);
+            confMag(spi, 0x80);
 
             // Enable self-test and put magnetometer in sleep
-            spi.write(REG_MAG_IF_3, 0x07);
-            spi.write(REG_MAG_IF_2, 0x4C);
+            writeMag(spi, MAG_REG_CONTROL_1, 1 | 3 << 1);
             miosix::Thread::sleep(200);
 
             // Check if it has finished
-            spi.write(REG_MAG_IF_1, 0x4C);
-            miosix::Thread::sleep(10);
-
+            readMag(spi, MAG_REG_CONTROL_1);
             if (spi.read(REG_DATA_MAG) & 1)
             {
                 TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
@@ -285,15 +274,14 @@ public:
             }
 
             // Now enable 8 byte burst reads
-            spi.write(REG_MAG_IF_0, 0x83);
-            miosix::Thread::sleep(10);
+            confMag(spi, 0x83);
 
             // Read back test results
-            spi.write(REG_MAG_IF_1, 0x42);
-            miosix::Thread::sleep(10);
+            readMag(spi, MAG_REG_DATA);
 
             int16_t mag[4];
-            spi.read(REG_DATA_MAG, reinterpret_cast<uint8_t*>(mag), sizeof(mag));
+            spi.read(REG_DATA_MAG, reinterpret_cast<uint8_t*>(mag),
+                     sizeof(mag));
 
             if (!(mag[0] & 1) || !(mag[1] & 1) || !(mag[2] & 1))
             {
@@ -383,6 +371,25 @@ private:
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
+    /// Convenience function to configure magnetometer
+    void confMag(SPITransaction& spi, uint8_t value) {
+        spi.write(REG_MAG_IF_0, value);
+        miosix::Thread::sleep(10);
+    }
+
+    /// Convenience function to read from magnetometer
+    void readMag(SPITransaction& spi, uint8_t reg) {
+        spi.write(REG_MAG_IF_1, reg);
+        miosix::Thread::sleep(10);
+    }
+
+    /// Convenience function to write to magnetometer
+    void writeMag(SPITransaction& spi, uint8_t reg, uint8_t value) {
+        spi.write(REG_MAG_IF_3, value);
+        spi.write(REG_MAG_IF_2, reg);
+        miosix::Thread::sleep(10);
+    }
+
     /// Debug function used to print the current error state
     const char* debugErr(SPITransaction& spi)
     {
@@ -600,4 +607,14 @@ private:
         REG_CMD       = 0x7E,
         REG_COMM_TEST = 0x7F,
     };
+
+    enum MagReg
+    {
+        MAG_REG_DATA    = 0x42,
+        MAG_REG_RESET_0   = 0x4B,
+        MAG_REG_CONTROL_1 = 0x4C,
+
+        MAG_REG_REPXY = 0x51,
+        MAG_REG_REPZ  = 0x52
+    };
 };
\ No newline at end of file
-- 
GitLab


From d21d16d0de05fe127437b548932ed5b8bf95fec4 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Sun, 6 Dec 2020 23:36:29 +0100
Subject: [PATCH 12/24] [BMX160] Exposed temperature sensor

---
 src/shared/sensors/BMX160/BMX160.h | 15 +++++++++++++++
 src/tests/drivers/test-bmx160.cpp  |  1 +
 2 files changed, 16 insertions(+)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index f51e0af4d..16bac8132 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -301,6 +301,9 @@ public:
 #endif
         SPITransaction spi(spi_slave);
 
+        // Read temperature
+        readTemp(spi);
+
         // Delete old samples
         gyro_fifo.clear();
         accel_fifo.clear();
@@ -331,6 +334,7 @@ public:
     BMX160Fifo<BMX160Magnet, 50> magnet_fifo;
     BMX160Fifo<BMX160Accel, 50> accel_fifo;
     BMX160Fifo<BMX160Gyro, 50> gyro_fifo;
+    float temperature;
 
 private:
     bool is_init = false;
@@ -426,6 +430,17 @@ private:
         }
     }
 
+    /// Read the value of the temperature sensor
+    void readTemp(SPITransaction& spi) 
+    {
+        const float TEMP_RES = 64.0f / 32768.0f;
+
+        int16_t val = spi.read(REG_TEMPERATURE_0) | (spi.read(REG_TEMPERATURE_1) << 8);
+
+        // Correct for resolution and offset
+        temperature = (val * TEMP_RES) + 23;
+    }
+
     /// Read the contents of the DATA register
     void readData(SPITransaction& spi)
     {
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 2718a462e..8e4f6388c 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -125,6 +125,7 @@ int main()
         sensor->magnet_fifo.pop().print();
         sensor->accel_fifo.pop().print();
         sensor->gyro_fifo.pop().print();
+        printf("Temp: %.2f deg\n", sensor->temperature);
     }
 
     return 0;
-- 
GitLab


From 90e93b7ea2ae6471f022815cc921c51d638e9e2d Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 7 Dec 2020 00:21:34 +0100
Subject: [PATCH 13/24] [BMX160] Implemented range compensantion

---
 src/shared/sensors/BMX160/BMX160.h     | 115 +++++++++++++++++++++----
 src/shared/sensors/BMX160/BMX160Data.h |  25 ++++--
 2 files changed, 118 insertions(+), 22 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 16bac8132..36774ab77 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -33,6 +33,26 @@
 
 class BMX160 : public Sensor
 {
+private:
+    /// Raw struct, directly from device
+    struct BMX160MagnetRaw
+    {
+        int16_t x, y, z;
+        uint16_t rhall;
+    };
+
+    /// Raw struct, directly from device
+    struct BMX160GyroRaw
+    {
+        int16_t x, y, z;
+    };
+
+    /// Raw struct, directly from device
+    struct BMX160AccelRaw
+    {
+        int16_t x, y, z;
+    };
+
 public:
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
         : spi_slave(bus, cs, SPIBusConfig{}), config(config)
@@ -376,24 +396,82 @@ private:
     }
 
     /// Convenience function to configure magnetometer
-    void confMag(SPITransaction& spi, uint8_t value) {
+    void confMag(SPITransaction& spi, uint8_t value)
+    {
         spi.write(REG_MAG_IF_0, value);
         miosix::Thread::sleep(10);
     }
 
     /// Convenience function to read from magnetometer
-    void readMag(SPITransaction& spi, uint8_t reg) {
+    void readMag(SPITransaction& spi, uint8_t reg)
+    {
         spi.write(REG_MAG_IF_1, reg);
         miosix::Thread::sleep(10);
     }
 
     /// Convenience function to write to magnetometer
-    void writeMag(SPITransaction& spi, uint8_t reg, uint8_t value) {
+    void writeMag(SPITransaction& spi, uint8_t reg, uint8_t value)
+    {
         spi.write(REG_MAG_IF_3, value);
         spi.write(REG_MAG_IF_2, reg);
         miosix::Thread::sleep(10);
     }
 
+    /// Correct magnetometer data for scale
+    BMX160Magnet correctMagnet(BMX160MagnetRaw data)
+    {
+        return BMX160Magnet{Vec3(data.x, data.y, data.z), data.rhall};
+    }
+
+    /// Correct accelerometer data for scale
+    BMX160Accel correctAccel(BMX160AccelRaw data)
+    {
+        float sens;
+        switch (config.acc_range)
+        {
+            case BMX160Config::AccRange::G_2:
+                sens = 1.0f / 16384.0f;
+                break;
+            case BMX160Config::AccRange::G_4:
+                sens = 1.0f / 8192.0f;
+                break;
+            case BMX160Config::AccRange::G_8:
+                sens = 1.0f / 4096.0f;
+                break;
+            case BMX160Config::AccRange::G_16:
+                sens = 1.0f / 2048.0f;
+                break;
+        }
+
+        return BMX160Accel{Vec3(data.x, data.y, data.z) * sens};
+    }
+
+    /// Correct gyroscope data for scale
+    BMX160Gyro correctGyro(BMX160GyroRaw data)
+    {
+        float sens;
+        switch (config.gyr_range)
+        {
+            case BMX160Config::GyrRange::DEG_2000:
+                sens = 1.0f / 16.4f;
+                break;
+            case BMX160Config::GyrRange::DEG_1000:
+                sens = 1.0f / 32.8f;
+                break;
+            case BMX160Config::GyrRange::DEG_500:
+                sens = 1.0f / 65.6f;
+                break;
+            case BMX160Config::GyrRange::DEG_250:
+                sens = 1.0f / 131.2f;
+                break;
+            case BMX160Config::GyrRange::DEG_125:
+                sens = 1.0f / 262.4f;
+                break;
+        }
+
+        return BMX160Gyro{Vec3(data.x, data.y, data.z) * sens};
+    }
+
     /// Debug function used to print the current error state
     const char* debugErr(SPITransaction& spi)
     {
@@ -431,11 +509,12 @@ private:
     }
 
     /// Read the value of the temperature sensor
-    void readTemp(SPITransaction& spi) 
+    void readTemp(SPITransaction& spi)
     {
         const float TEMP_RES = 64.0f / 32768.0f;
 
-        int16_t val = spi.read(REG_TEMPERATURE_0) | (spi.read(REG_TEMPERATURE_1) << 8);
+        int16_t val =
+            spi.read(REG_TEMPERATURE_0) | (spi.read(REG_TEMPERATURE_1) << 8);
 
         // Correct for resolution and offset
         temperature = (val * TEMP_RES) + 23;
@@ -448,9 +527,9 @@ private:
         spi.read(REG_DATA, buf, sizeof(buf));
 
         int idx = 0;
-        magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
-        gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
-        accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
+        magnet_fifo.push(correctMagnet(parseStruct<BMX160MagnetRaw>(buf, idx)));
+        gyro_fifo.push(correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
+        accel_fifo.push(correctAccel(parseStruct<BMX160AccelRaw>(buf, idx)));
     }
 
     /// Read the contents of the fifo into buf
@@ -473,9 +552,12 @@ private:
 
             if (headerless)
             {
-                magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
-                gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
-                accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
+                magnet_fifo.push(
+                    correctMagnet(parseStruct<BMX160MagnetRaw>(buf, idx)));
+                gyro_fifo.push(
+                    correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
+                accel_fifo.push(
+                    correctAccel(parseStruct<BMX160AccelRaw>(buf, idx)));
             }
             else
             {
@@ -492,15 +574,18 @@ private:
 
                     // This contains magnet data
                     if (header & 0x10)
-                        magnet_fifo.push(parseStruct<BMX160Magnet>(buf, idx));
+                        magnet_fifo.push(correctMagnet(
+                            parseStruct<BMX160MagnetRaw>(buf, idx)));
 
                     // This contains gyro data
                     if (header & 0x08)
-                        gyro_fifo.push(parseStruct<BMX160Gyro>(buf, idx));
+                        gyro_fifo.push(
+                            correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
 
                     // This contains accel data
                     if (header & 0x04)
-                        accel_fifo.push(parseStruct<BMX160Accel>(buf, idx));
+                        accel_fifo.push(correctAccel(
+                            parseStruct<BMX160AccelRaw>(buf, idx)));
                 }
                 else if (header & 0x40)
                 {
@@ -625,7 +710,7 @@ private:
 
     enum MagReg
     {
-        MAG_REG_DATA    = 0x42,
+        MAG_REG_DATA      = 0x42,
         MAG_REG_RESET_0   = 0x4B,
         MAG_REG_CONTROL_1 = 0x4C,
 
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 56dd7d639..641471e37 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -23,28 +23,39 @@
 
 #pragma once
 
+#include <math/Vec3.h>
 #include <stdio.h>
 
 struct BMX160Magnet
 {
-    int16_t x, y, z;
-    uint16_t rhall;
+    Vec3 data;
+    float rhall;
 
-    void print() { printf("MAGNET:\t%d\t%d\t%d\t%d\n", x, y, z, rhall); }
+    void print()
+    {
+        printf("MAGNET:\t%.2f\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ(),
+               rhall);
+    }
 };
 
 struct BMX160Accel
 {
-    int16_t x, y, z;
+    Vec3 data;
 
-    void print() { printf("ACCEL:\t%d\t%d\t%d\n", x, y, z); }
+    void print()
+    {
+        printf("ACCEL:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ());
+    }
 };
 
 struct BMX160Gyro
 {
-    int16_t x, y, z;
+    Vec3 data;
 
-    void print() { printf("GYRO:\t%d\t%d\t%d\n", x, y, z); }
+    void print()
+    {
+        printf("GYRO:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ());
+    }
 };
 
 /// Class representing a BMX160 Data fifo
-- 
GitLab


From 75ac2144f99458880d1cc0732b2c93924aea263e Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 7 Dec 2020 00:56:03 +0100
Subject: [PATCH 14/24] [BMX160] Better documentation and fixes

---
 src/shared/sensors/BMX160/BMX160.h       | 33 ++++++++++------
 src/shared/sensors/BMX160/BMX160Config.h | 48 ++++++++++++------------
 src/shared/sensors/BMX160/BMX160Data.h   | 17 +++++++--
 src/tests/drivers/test-bmx160.cpp        |  8 ++--
 4 files changed, 63 insertions(+), 43 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 36774ab77..adf9eb807 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -31,23 +31,24 @@
 #include "BMX160Config.h"
 #include "BMX160Data.h"
 
+/// BMX160 Driver.
 class BMX160 : public Sensor
 {
 private:
-    /// Raw struct, directly from device
+    /// Raw struct, read directly from device.
     struct BMX160MagnetRaw
     {
         int16_t x, y, z;
         uint16_t rhall;
     };
 
-    /// Raw struct, directly from device
+    /// Raw struct, read directly from device.
     struct BMX160GyroRaw
     {
         int16_t x, y, z;
     };
 
-    /// Raw struct, directly from device
+    /// Raw struct, read directly from device.
     struct BMX160AccelRaw
     {
         int16_t x, y, z;
@@ -55,12 +56,18 @@ private:
 
 public:
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
-        : spi_slave(bus, cs, SPIBusConfig{}), config(config)
+        : BMX160(bus, cs, config, SPIBusConfig{})
     {
-        // FIXME: random division, check for proper one.
-        spi_slave.config.clock_div = SPIClockDivider::DIV64;
+        spi_slave.config.clock_div = SPIClockDivider::DIV4;
     }
 
+    BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config,
+           SPIBusConfig bus_config)
+        : spi_slave(bus, cs, bus_config), config(config)
+    {
+    }
+
+    /// Initialize the driver.
     bool init() override
     {
 #ifdef DEBUG
@@ -203,6 +210,8 @@ public:
         return is_init = true;
     }
 
+    /// Perform self-test on the device.
+    ///
     /// Warning init() must be re-invoked after a selfTest()!
     bool selfTest() override
     {
@@ -314,6 +323,7 @@ public:
         return true;
     }
 
+    /// Gather data from FIFO/data registers and temperature sensor.
     bool onSimpleUpdate() override
     {
 #ifdef DEBUG
@@ -350,7 +360,6 @@ public:
         return false;
     }
 
-    // For the time being make this public
     BMX160Fifo<BMX160Magnet, 50> magnet_fifo;
     BMX160Fifo<BMX160Accel, 50> accel_fifo;
     BMX160Fifo<BMX160Gyro, 50> gyro_fifo;
@@ -420,13 +429,15 @@ private:
     /// Correct magnetometer data for scale
     BMX160Magnet correctMagnet(BMX160MagnetRaw data)
     {
-        return BMX160Magnet{Vec3(data.x, data.y, data.z), data.rhall};
+        // FIXME: What is the sensibility on this?
+        return BMX160Magnet{Vec3(data.x, data.y, data.z),
+                            static_cast<float>(data.rhall)};
     }
 
     /// Correct accelerometer data for scale
     BMX160Accel correctAccel(BMX160AccelRaw data)
     {
-        float sens;
+        float sens = 0.0f;
         switch (config.acc_range)
         {
             case BMX160Config::AccRange::G_2:
@@ -449,7 +460,7 @@ private:
     /// Correct gyroscope data for scale
     BMX160Gyro correctGyro(BMX160GyroRaw data)
     {
-        float sens;
+        float sens = 0.0f;
         switch (config.gyr_range)
         {
             case BMX160Config::GyrRange::DEG_2000:
@@ -541,7 +552,7 @@ private:
         uint8_t buf[1000];
 
 #ifdef DEBUG
-        assert(len <= sizeof(buf) && "Buffer overflow!");
+        assert(len <= static_cast<int>(sizeof(buf)) && "Buffer overflow!");
 #endif
 
         spi.read(REG_FIFO_DATA, buf, len);
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 82ebe69b9..9b0555ae7 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -27,31 +27,32 @@ struct BMX160Config
 {
     BMX160Config() {}
 
-    /// Fifo operating mode
+    /// Fifo operating mode.
     enum class FifoMode
     {
-        /// The fifo is completely disabled
+        /// The fifo is completely disabled.
         DISABLED,
         /// the fifo operations have less overhead,
         /// but EVERY sensor MUST have the same odr!!
         HEADERLESS,
         /// the fifo operations have more overhead,
-        /// but sensors con have different odr
+        /// but sensors con have different odr.
         HEADER
     };
 
     /// Fifo interrupt mode (uses fifo full as trigger)
     enum class FifoInt
     {
-        /// The interrupts are completely disabled
+        /// The interrupts are completely disabled.
         DISABLED,
-        /// Interrupts are enabled on pin 2
+        /// Interrupts are enabled on pin 2.
         PIN_INT1,
-        /// Interrupts are enabled on pin 1
-        PIN_INT2
+        /// Interrupts are enabled on pin 1.
+        PIN_INT2,
     };
 
-    /// Accellerometer ODR expressed in Hz
+    /// Accellerometer ODR expressed in Hz.
+    ///
     /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx)
     enum class AccOdr
     {
@@ -69,16 +70,16 @@ struct BMX160Config
         HZ_1600  = 0x0c,
     };
 
-    /// Range of the accellerometer expressed in +/- g
+    /// Range of the accellerometer expressed in +/- g.
     enum class AccRange
     {
-        G_2,
-        G_4,
-        G_8,
-        G_16
+        G_2  = 0x3,
+        G_4  = 0x5,
+        G_8  = 0x8,
+        G_16 = 0xC
     };
 
-    /// Gyroscope ODR expressed in Hz
+    /// Gyroscope ODR expressed in Hz.
     enum class GyrOdr
     {
         HZ_25   = 0x06,
@@ -91,18 +92,19 @@ struct BMX160Config
         HZ_3200 = 0x0d,
     };
 
-    /// Gyroscope range expressed in °/sec
+    /// Gyroscope range expressed in °/sec.
     enum class GyrRange
     {
-        DEG_2000,
-        DEG_1000,
-        DEG_500,
-        DEG_250,
-        DEG_125
+        DEG_2000 = 0x0,
+        DEG_1000 = 0x1,
+        DEG_500  = 0x2,
+        DEG_250  = 0x3,
+        DEG_125  = 0x4
     };
 
-    /// Magnetometer ODR expressed in Hz
-    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx)
+    /// Magnetometer ODR expressed in Hz.
+    ///
+    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx).
     enum class MagOdr
     {
         HZ_25_32 = 0x01,
@@ -122,7 +124,7 @@ struct BMX160Config
     ///
     /// Only values between [0-250] make sense to use.
     /// A really high watermark value (the default) will
-    /// disable it, falling back to FIFO full
+    /// disable it, falling back to FIFO full.
     uint8_t fifo_watermark = -1;
 
     FifoMode fifo_mode = FifoMode::DISABLED;
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 641471e37..d5683f165 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -26,6 +26,7 @@
 #include <math/Vec3.h>
 #include <stdio.h>
 
+/// Output from the BMX160 magnetometer
 struct BMX160Magnet
 {
     Vec3 data;
@@ -33,28 +34,32 @@ struct BMX160Magnet
 
     void print()
     {
-        printf("MAGNET:\t%.2f\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ(),
-               rhall);
+        printf("MAGNET:\t%.2f\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
+               data.getZ(), rhall);
     }
 };
 
+/// Output from the BMX160 accelerometer
 struct BMX160Accel
 {
     Vec3 data;
 
     void print()
     {
-        printf("ACCEL:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ());
+        printf("ACCEL:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
+               data.getZ());
     }
 };
 
+/// Output from the BMX160 Gyroscope
 struct BMX160Gyro
 {
     Vec3 data;
 
     void print()
     {
-        printf("GYRO:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(), data.getZ());
+        printf("GYRO:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
+               data.getZ());
     }
 };
 
@@ -65,6 +70,7 @@ class BMX160Fifo
 public:
     BMX160Fifo() : len(0), idx(0) {}
 
+    /// Push a single sample in the fifo (should not be called outside driver)
     void push(T sample)
     {
 #ifdef DEBUG
@@ -73,6 +79,7 @@ public:
         data[len++] = sample;
     }
 
+    /// Pop a single sample from the fifo
     T pop()
     {
 #ifdef DEBUG
@@ -81,7 +88,9 @@ public:
         return data[idx++];
     }
 
+    /// Clear every sample in the fifo (should not be called outside driver)
     void clear() { len = idx = 0; }
+    /// Get the remaining count of samples in the fifo
     size_t count() { return len; }
 
 private:
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 8e4f6388c..85f26ce7c 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -46,9 +46,7 @@ GpioPin spi_mosi(GPIOB_BASE, 5);
 BMX160 *sensor = nullptr;
 bool invoked   = false;
 
-void __attribute__((used)) EXTI8_IRQHandlerImpl() { 
-    invoked = true;
-}
+void __attribute__((used)) EXTI8_IRQHandlerImpl() { invoked = true; }
 
 int main()
 {
@@ -68,14 +66,14 @@ int main()
 
         // Setup CS
         cs.mode(miosix::Mode::OUTPUT);
-    };    
+    };
 
     cs.high();
 
     BMX160Config config;
     config.fifo_mode = BMX160Config::FifoMode::HEADER;
     config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
-    
+
     sensor = new BMX160(bus, cs, config);
 
     TRACE("Initializing BMX160...\n");
-- 
GitLab


From 5dfc1061ccbb5ea8e6ab2b707875a0fc40c619a2 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 8 Dec 2020 01:04:21 +0100
Subject: [PATCH 15/24] [BMX160] Exposed REPXY and REPZ registers, fixed
 magnetometer representation

---
 src/shared/sensors/BMX160/BMX160.h       | 29 ++++++++++++------------
 src/shared/sensors/BMX160/BMX160Config.h | 22 ++++++++++++++++++
 src/shared/sensors/BMX160/BMX160Data.h   |  8 ++++---
 src/tests/drivers/test-bmx160.cpp        |  4 ++--
 4 files changed, 43 insertions(+), 20 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index adf9eb807..f8ededdb3 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -129,20 +129,18 @@ public:
             confMag(spi, 0x80);
 
             // Put MAG into sleep mode (from suspend mode)
-            writeMag(spi, MAG_REG_RESET_0, 0x01);
+            writeMag(spi, MAG_REG_RESET, 0x01);
 
-            // Load high accuracy preset for REPXY
-            writeMag(spi, MAG_REG_REPXY, 0x17);
-
-            // Load high accuracy preset for REPZ
-            writeMag(spi, MAG_REG_REPZ, 0x52);
+            writeMag(spi, MAG_REG_REPXY, config.mag_repxy);
+            writeMag(spi, MAG_REG_REPZ, config.mag_repz);
 
             // Magic sequence to init it
-            writeMag(spi, MAG_REG_CONTROL_1, 0x02);
+            writeMag(spi, MAG_REG_CONTROL, 0x02);
             readMag(spi, MAG_REG_DATA);
 
             // Set mag output data rate
             spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
+            miosix::Thread::sleep(10);
 
             // Disable manual configuration mode
             confMag(spi, 0x00);
@@ -291,11 +289,11 @@ public:
             confMag(spi, 0x80);
 
             // Enable self-test and put magnetometer in sleep
-            writeMag(spi, MAG_REG_CONTROL_1, 1 | 3 << 1);
+            writeMag(spi, MAG_REG_CONTROL, 1 | 3 << 1);
             miosix::Thread::sleep(200);
 
             // Check if it has finished
-            readMag(spi, MAG_REG_CONTROL_1);
+            readMag(spi, MAG_REG_CONTROL);
             if (spi.read(REG_DATA_MAG) & 1)
             {
                 TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
@@ -429,9 +427,10 @@ private:
     /// Correct magnetometer data for scale
     BMX160Magnet correctMagnet(BMX160MagnetRaw data)
     {
-        // FIXME: What is the sensibility on this?
-        return BMX160Magnet{Vec3(data.x, data.y, data.z),
-                            static_cast<float>(data.rhall)};
+        // TODO: Use rhall (Hall sensor resistance) for temperature
+        // compensation.
+
+        return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3};
     }
 
     /// Correct accelerometer data for scale
@@ -721,9 +720,9 @@ private:
 
     enum MagReg
     {
-        MAG_REG_DATA      = 0x42,
-        MAG_REG_RESET_0   = 0x4B,
-        MAG_REG_CONTROL_1 = 0x4C,
+        MAG_REG_DATA    = 0x42,
+        MAG_REG_RESET   = 0x4B,
+        MAG_REG_CONTROL = 0x4C,
 
         MAG_REG_REPXY = 0x51,
         MAG_REG_REPZ  = 0x52
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 9b0555ae7..da2833b44 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -127,6 +127,28 @@ struct BMX160Config
     /// disable it, falling back to FIFO full.
     uint8_t fifo_watermark = -1;
 
+    /// Repetitions for the XY axis
+    ///
+    /// This are the reccomended presets:
+    /// - 0x01, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
+    /// - 0x04, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
+    /// (Default)
+    /// - 0x07, RMS Noise (x/y/z) 0.5/0.5/0.5, Current: 0.8mA  (Enhanced
+    /// regular)
+    /// - 0x17, RMS Noise (x/y/z) 0.3/0.3/0.3, Current: 4.9mA  (High accuracy)
+    uint8_t mag_repxy = 0x04;
+
+    /// Repetitions for the Z axis
+    ///
+    /// This are the reccomended presets:
+    /// - 0x02, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
+    /// - 0x0E, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
+    /// (Default)
+    /// - 0x1A, RMS Noise (x/y/z) 0.5/0.5/0.5, Current: 0.8mA  (Enhanced
+    /// regular)
+    /// - 0x52, RMS Noise (x/y/z) 0.3/0.3/0.3, Current: 4.9mA  (High accuracy)
+    uint8_t mag_repz = 0x0E;
+
     FifoMode fifo_mode = FifoMode::DISABLED;
     FifoInt fifo_int   = FifoInt::DISABLED;
 
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index d5683f165..696359fd6 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -29,19 +29,20 @@
 /// Output from the BMX160 magnetometer
 struct BMX160Magnet
 {
+    /// Data expressed in μT
     Vec3 data;
-    float rhall;
 
     void print()
     {
-        printf("MAGNET:\t%.2f\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
-               data.getZ(), rhall);
+        printf("MAGNET:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
+               data.getZ());
     }
 };
 
 /// Output from the BMX160 accelerometer
 struct BMX160Accel
 {
+    /// Data expressed in g
     Vec3 data;
 
     void print()
@@ -54,6 +55,7 @@ struct BMX160Accel
 /// Output from the BMX160 Gyroscope
 struct BMX160Gyro
 {
+    /// Data expressed in °/s
     Vec3 data;
 
     void print()
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 85f26ce7c..c87298551 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -71,7 +71,7 @@ int main()
     cs.high();
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::HEADER;
+    config.fifo_mode = BMX160Config::FifoMode::HEADERLESS;
     config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
 
     sensor = new BMX160(bus, cs, config);
@@ -112,7 +112,7 @@ int main()
 
     while (1)
     {
-        miosix::Thread::sleep(1000);
+        miosix::Thread::sleep(5000);
 
         TRACE("Interrupt invoked: %d\n", invoked);
         invoked = false;
-- 
GitLab


From ba3ae834120ee212abca345a4e932d769f8c55ee Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Thu, 10 Dec 2020 00:25:03 +0100
Subject: [PATCH 16/24] [BMX160] Implemented bosch black box compensantion
 algorithm.

---
 src/shared/sensors/BMX160/BMX160.h       | 269 ++++++++++++++++++++---
 src/shared/sensors/BMX160/BMX160Config.h |   9 +
 src/tests/drivers/test-bmx160.cpp        |   2 +-
 3 files changed, 243 insertions(+), 37 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index f8ededdb3..f20a769dc 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -36,24 +36,40 @@ class BMX160 : public Sensor
 {
 private:
     /// Raw struct, read directly from device.
-    struct BMX160MagnetRaw
+    struct MagnetRaw
     {
         int16_t x, y, z;
         uint16_t rhall;
     };
 
     /// Raw struct, read directly from device.
-    struct BMX160GyroRaw
+    struct GyroRaw
     {
         int16_t x, y, z;
     };
 
     /// Raw struct, read directly from device.
-    struct BMX160AccelRaw
+    struct AccelRaw
     {
         int16_t x, y, z;
     };
 
+    /// Struct holding trim data used for magnetomer compensation
+    struct TrimData
+    {
+        int8_t dig_x1;
+        int8_t dig_y1;
+        int8_t dig_x2;
+        int8_t dig_y2;
+        uint16_t dig_z1;
+        int16_t dig_z2;
+        int16_t dig_z3;
+        int16_t dig_z4;
+        uint8_t dig_xy1;
+        int8_t dig_xy2;
+        uint16_t dig_xyz1;
+    };
+
 public:
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
         : BMX160(bus, cs, config, SPIBusConfig{})
@@ -134,9 +150,45 @@ public:
             writeMag(spi, MAG_REG_REPXY, config.mag_repxy);
             writeMag(spi, MAG_REG_REPZ, config.mag_repz);
 
+            if (config.enable_compensation)
+            {
+                uint8_t trim_x1y1[2]     = {0};
+                uint8_t trim_xyz_data[4] = {0};
+                uint8_t trim_xy1xy2[10]  = {0};
+
+                readMag(spi, MAG_REG_DIG_X1, trim_x1y1, sizeof(trim_x1y1));
+                readMag(spi, MAG_REG_DIG_Z4_0, trim_xyz_data, sizeof(trim_xyz_data));
+                readMag(spi, MAG_REG_DIG_Z2_0, trim_xy1xy2, sizeof(trim_xy1xy2));
+
+                TRACE("-------- DUMP OF TRIM REGS --------\n");
+                for (int i = 0; i < 2; i++)
+                    TRACE("trim_x1y1[%d]: %d\n", i, trim_x1y1[i]);
+
+                for (int i = 0; i < 4; i++)
+                    TRACE("trim_xyz_data[%d]: %d\n", i, trim_xyz_data[i]);
+
+                for (int i = 0; i < 10; i++)
+                    TRACE("trim_xy1xy2[%d]: %d\n", i, trim_xy1xy2[i]);
+                TRACE("--------    END OF DUMP    --------\n");
+
+                // Read trim registers
+                trim_data.dig_x1  = trim_x1y1[0];
+                trim_data.dig_y1  = trim_x1y1[1];
+                trim_data.dig_x2  = trim_xyz_data[2];
+                trim_data.dig_y2  = trim_xyz_data[3];
+                trim_data.dig_z1  = trim_xy1xy2[2] | (trim_xy1xy2[3] << 8);
+                trim_data.dig_z2  = trim_xy1xy2[0] | (trim_xy1xy2[1] << 8);
+                trim_data.dig_z3  = trim_xy1xy2[6] | (trim_xy1xy2[7] << 8);
+                trim_data.dig_z4  = trim_xyz_data[0] | (trim_xyz_data[1] << 8);
+                trim_data.dig_xy1 = trim_xy1xy2[9];
+                trim_data.dig_xy2 = trim_xy1xy2[8];
+                trim_data.dig_xyz1 =
+                    trim_xy1xy2[4] | ((trim_xy1xy2[5] & 0x7F) << 8);
+            }
+
             // Magic sequence to init it
             writeMag(spi, MAG_REG_CONTROL, 0x02);
-            readMag(spi, MAG_REG_DATA);
+            mapMag(spi, MAG_REG_DATA);
 
             // Set mag output data rate
             spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
@@ -293,22 +345,16 @@ public:
             miosix::Thread::sleep(200);
 
             // Check if it has finished
-            readMag(spi, MAG_REG_CONTROL);
-            if (spi.read(REG_DATA_MAG) & 1)
+            if (readMag(spi, MAG_REG_CONTROL) & 1)
             {
                 TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
                 return false;
             }
 
-            // Now enable 8 byte burst reads
-            confMag(spi, 0x83);
-
             // Read back test results
-            readMag(spi, MAG_REG_DATA);
-
             int16_t mag[4];
-            spi.read(REG_DATA_MAG, reinterpret_cast<uint8_t*>(mag),
-                     sizeof(mag));
+            readMag(spi, MAG_REG_DATA, reinterpret_cast<uint8_t*>(mag),
+                    sizeof(mag));
 
             if (!(mag[0] & 1) || !(mag[1] & 1) || !(mag[2] & 1))
             {
@@ -366,6 +412,7 @@ public:
 private:
     bool is_init = false;
     SPISlave spi_slave;
+    TrimData trim_data;
 
     BMX160Config config;
 
@@ -409,13 +456,58 @@ private:
         miosix::Thread::sleep(10);
     }
 
-    /// Convenience function to read from magnetometer
-    void readMag(SPITransaction& spi, uint8_t reg)
+    /// Convenience function to map magnetometer for read
+    void mapMag(SPITransaction& spi, uint8_t reg)
     {
         spi.write(REG_MAG_IF_1, reg);
         miosix::Thread::sleep(10);
     }
 
+    /// Convenience function to read a single byte from magnetometer
+    uint8_t readMag(SPITransaction& spi, uint8_t reg)
+    {
+        mapMag(spi, reg);
+        return spi.read(REG_DATA_MAG);
+    }
+
+    /// Convenience function to read from magnetometer
+    void readMag(SPITransaction& spi, uint8_t reg, uint8_t* data, size_t size)
+    {
+        while (size != 0)
+        {
+            int burst = 0;
+            if (size >= 8)
+            {
+                confMag(spi, 0x83);
+                burst = 8;
+            }
+            else if (size >= 6)
+            {
+                confMag(spi, 0x82);
+                burst = 6;
+            }
+            else if (size >= 2)
+            {
+                confMag(spi, 0x81);
+                burst = 2;
+            }
+            else
+            {
+                confMag(spi, 0x80);
+                burst = 1;
+            }
+
+            mapMag(spi, reg);
+            spi.read(REG_DATA_MAG, data, burst);
+
+            reg += burst;
+            data += burst;
+            size -= burst;
+        }
+
+        confMag(spi, 0x80);
+    }
+
     /// Convenience function to write to magnetometer
     void writeMag(SPITransaction& spi, uint8_t reg, uint8_t value)
     {
@@ -424,17 +516,28 @@ private:
         miosix::Thread::sleep(10);
     }
 
-    /// Correct magnetometer data for scale
-    BMX160Magnet correctMagnet(BMX160MagnetRaw data)
+    /// Correct magnetometer data
+    BMX160Magnet correctMagnet(MagnetRaw data)
     {
-        // TODO: Use rhall (Hall sensor resistance) for temperature
-        // compensation.
+        // Strip the lower 3 bits
+        data.x >>= 3;
+        data.y >>= 3;
+        data.z >>= 3;
 
-        return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3};
+        if (config.enable_compensation)
+        {
+            return BMX160Magnet{Vec3(boschCompensateX(data.x, data.rhall),
+                                     boschCompensateY(data.y, data.rhall),
+                                     boschCompensateZ(data.z, data.rhall))};
+        }
+        else
+        {
+            return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3};
+        }
     }
 
     /// Correct accelerometer data for scale
-    BMX160Accel correctAccel(BMX160AccelRaw data)
+    BMX160Accel correctAccel(AccelRaw data)
     {
         float sens = 0.0f;
         switch (config.acc_range)
@@ -457,7 +560,7 @@ private:
     }
 
     /// Correct gyroscope data for scale
-    BMX160Gyro correctGyro(BMX160GyroRaw data)
+    BMX160Gyro correctGyro(GyroRaw data)
     {
         float sens = 0.0f;
         switch (config.gyr_range)
@@ -537,9 +640,9 @@ private:
         spi.read(REG_DATA, buf, sizeof(buf));
 
         int idx = 0;
-        magnet_fifo.push(correctMagnet(parseStruct<BMX160MagnetRaw>(buf, idx)));
-        gyro_fifo.push(correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
-        accel_fifo.push(correctAccel(parseStruct<BMX160AccelRaw>(buf, idx)));
+        magnet_fifo.push(correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
+        gyro_fifo.push(correctGyro(parseStruct<GyroRaw>(buf, idx)));
+        accel_fifo.push(correctAccel(parseStruct<AccelRaw>(buf, idx)));
     }
 
     /// Read the contents of the fifo into buf
@@ -563,11 +666,9 @@ private:
             if (headerless)
             {
                 magnet_fifo.push(
-                    correctMagnet(parseStruct<BMX160MagnetRaw>(buf, idx)));
-                gyro_fifo.push(
-                    correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
-                accel_fifo.push(
-                    correctAccel(parseStruct<BMX160AccelRaw>(buf, idx)));
+                    correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
+                gyro_fifo.push(correctGyro(parseStruct<GyroRaw>(buf, idx)));
+                accel_fifo.push(correctAccel(parseStruct<AccelRaw>(buf, idx)));
             }
             else
             {
@@ -584,18 +685,18 @@ private:
 
                     // This contains magnet data
                     if (header & 0x10)
-                        magnet_fifo.push(correctMagnet(
-                            parseStruct<BMX160MagnetRaw>(buf, idx)));
+                        magnet_fifo.push(
+                            correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
 
                     // This contains gyro data
                     if (header & 0x08)
                         gyro_fifo.push(
-                            correctGyro(parseStruct<BMX160GyroRaw>(buf, idx)));
+                            correctGyro(parseStruct<GyroRaw>(buf, idx)));
 
                     // This contains accel data
                     if (header & 0x04)
-                        accel_fifo.push(correctAccel(
-                            parseStruct<BMX160AccelRaw>(buf, idx)));
+                        accel_fifo.push(
+                            correctAccel(parseStruct<AccelRaw>(buf, idx)));
                 }
                 else if (header & 0x40)
                 {
@@ -725,6 +826,102 @@ private:
         MAG_REG_CONTROL = 0x4C,
 
         MAG_REG_REPXY = 0x51,
-        MAG_REG_REPZ  = 0x52
+        MAG_REG_REPZ  = 0x52,
+
+        // Factory calibrated trim registers.
+        // This is all undocumented territory (the datasheet mentions this as
+        // "reserved")
+
+        MAG_REG_DIG_X1     = 0x5D,
+        MAG_REG_DIG_Y1     = 0x5E,
+        MAG_REG_DIG_Z4_0   = 0x62,
+        MAG_REG_DIG_Z4_1   = 0x63,
+        MAG_REG_DIG_X2     = 0x64,
+        MAG_REG_DIG_Y2     = 0x65,
+        MAG_REG_DIG_Z2_0   = 0x68,
+        MAG_REG_DIG_Z2_1   = 0x69,
+        MAG_REG_DIG_Z1_0   = 0x6A,
+        MAG_REG_DIG_Z1_1   = 0x6B,
+        MAG_REG_DIG_XYZ1_0 = 0x6C,
+        MAG_REG_DIG_XYZ1_1 = 0x6D,
+        MAG_REG_DIG_Z3_0   = 0x6E,
+        MAG_REG_DIG_Z3_1   = 0x6F,
+        MAG_REG_DIG_XY2    = 0x70,
+        MAG_REG_DIG_XY1    = 0x71,
     };
+
+    // Warning: the following code is extrapolated from the bosch driver
+    // source code, I have no idea of what it does.
+
+    /// Bosch black-box algorithm
+    float boschCompensateX(int16_t x, uint16_t rhall)
+    {
+        /* clang-format off */
+        float retval = 0;
+        float process_comp_x0;
+        float process_comp_x1;
+        float process_comp_x2;
+        float process_comp_x3;
+        float process_comp_x4;
+
+        /* Processing compensation equations */
+        process_comp_x0 = (((float)trim_data.dig_xyz1) * 16384.0f / rhall);
+        retval = (process_comp_x0 - 16384.0f);
+        process_comp_x1 = ((float)trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+        process_comp_x2 = process_comp_x1 + retval * ((float)trim_data.dig_xy1) / 16384.0f;
+        process_comp_x3 = ((float)trim_data.dig_x2) + 160.0f;
+        process_comp_x4 = x * ((process_comp_x2 + 256.0f) * process_comp_x3);
+        retval = ((process_comp_x4 / 8192.0f) + (((float)trim_data.dig_x1) * 8.0f)) / 16.0f;
+
+        return retval;
+        /* clang-format on */
+    }
+
+    /// Bosch black-box algorithm
+    float boschCompensateY(int16_t y, uint16_t rhall)
+    {
+        /* clang-format off */
+        float retval = 0;
+        float process_comp_y0;
+        float process_comp_y1;
+        float process_comp_y2;
+        float process_comp_y3;
+        float process_comp_y4;
+
+        /* Processing compensation equations */
+        process_comp_y0 = ((float)trim_data.dig_xyz1) * 16384.0f / rhall;
+        retval = process_comp_y0 - 16384.0f;
+        process_comp_y1 = ((float)trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+        process_comp_y2 = process_comp_y1 + retval * ((float)trim_data.dig_xy1) / 16384.0f;
+        process_comp_y3 = ((float)trim_data.dig_y2) + 160.0f;
+        process_comp_y4 = y * (((process_comp_y2) + 256.0f) * process_comp_y3);
+        retval = ((process_comp_y4 / 8192.0f) + (((float)trim_data.dig_y1) * 8.0f)) / 16.0f;
+
+        return retval;
+        /* clang-format on */
+    }
+
+    /// Bosch black-box algorithm
+    float boschCompensateZ(int16_t z, uint16_t rhall)
+    {
+        /* clang-format off */
+        float retval = 0;
+        float process_comp_z0;
+        float process_comp_z1;
+        float process_comp_z2;
+        float process_comp_z3;
+        float process_comp_z4;
+        float process_comp_z5;
+
+        process_comp_z0 = ((float)z) - ((float)trim_data.dig_z4);
+        process_comp_z1 = ((float)rhall) - ((float)trim_data.dig_xyz1);
+        process_comp_z2 = (((float)trim_data.dig_z3) * process_comp_z1);
+        process_comp_z3 = ((float)trim_data.dig_z1) * ((float)rhall) / 32768.0f;
+        process_comp_z4 = ((float)trim_data.dig_z2) + process_comp_z3;
+        process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
+        retval = (process_comp_z5 / ((process_comp_z4)*4.0f)) / 16.0f;
+
+        return retval;
+        /* clang-format on */
+    }
 };
\ No newline at end of file
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index da2833b44..4f3f053cf 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -149,6 +149,15 @@ struct BMX160Config
     /// - 0x52, RMS Noise (x/y/z) 0.3/0.3/0.3, Current: 4.9mA  (High accuracy)
     uint8_t mag_repz = 0x0E;
 
+    /// Enable magnetometer data compensation
+    ///
+    /// The magnetomer support compensation, but it's not documented, and the
+    /// current implementation is based on the source of the bosch driver.
+    ///
+    /// The implementation is slow, probably buggy, and only god knows what it
+    /// does, it's so bad that I added a switch to disable it, use it wisely!
+    bool enable_compensation = true;
+
     FifoMode fifo_mode = FifoMode::DISABLED;
     FifoInt fifo_int   = FifoInt::DISABLED;
 
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index c87298551..4f6a8a403 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -71,7 +71,7 @@ int main()
     cs.high();
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::HEADERLESS;
+    config.fifo_mode = BMX160Config::FifoMode::DISABLED;
     config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
 
     sensor = new BMX160(bus, cs, config);
-- 
GitLab


From 940c6eaeccfbfa3da11eee515ad09e483ab4cf16 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Thu, 10 Dec 2020 20:05:26 +0100
Subject: [PATCH 17/24] [BMX160] Implemented data timestamping

---
 src/shared/sensors/BMX160/BMX160.h     | 102 ++++++++++++++++++-------
 src/shared/sensors/BMX160/BMX160Data.h |  47 +++++++-----
 src/tests/drivers/test-bmx160.cpp      |  51 +++++++++----
 3 files changed, 136 insertions(+), 64 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index f20a769dc..b5e79338b 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -157,8 +157,10 @@ public:
                 uint8_t trim_xy1xy2[10]  = {0};
 
                 readMag(spi, MAG_REG_DIG_X1, trim_x1y1, sizeof(trim_x1y1));
-                readMag(spi, MAG_REG_DIG_Z4_0, trim_xyz_data, sizeof(trim_xyz_data));
-                readMag(spi, MAG_REG_DIG_Z2_0, trim_xy1xy2, sizeof(trim_xy1xy2));
+                readMag(spi, MAG_REG_DIG_Z4_0, trim_xyz_data,
+                        sizeof(trim_xyz_data));
+                readMag(spi, MAG_REG_DIG_Z2_0, trim_xy1xy2,
+                        sizeof(trim_xy1xy2));
 
                 TRACE("-------- DUMP OF TRIM REGS --------\n");
                 for (int i = 0; i < 2; i++)
@@ -200,7 +202,8 @@ public:
 
         // Setup accel
         {
-            spi.write(REG_ACC_CONF, static_cast<uint8_t>(config.acc_odr));
+            spi.write(REG_ACC_CONF,
+                      static_cast<uint8_t>(config.acc_odr) | (1 << 5));
             spi.write(REG_ACC_RANGE, static_cast<uint8_t>(config.acc_range));
         }
 
@@ -404,16 +407,20 @@ public:
         return false;
     }
 
-    BMX160Fifo<BMX160Magnet, 50> magnet_fifo;
-    BMX160Fifo<BMX160Accel, 50> accel_fifo;
-    BMX160Fifo<BMX160Gyro, 50> gyro_fifo;
+    /// Update the timestamp from an IRQ, timestamp should be in microseconds
+    void updateIRQTimestamp(uint32_t timestamp) { irq_timestamp = timestamp; }
+
+    BMX160Fifo<BMX160Magnet, 200> magnet_fifo;
+    BMX160Fifo<BMX160Accel, 200> accel_fifo;
+    BMX160Fifo<BMX160Gyro, 200> gyro_fifo;
     float temperature;
 
 private:
     bool is_init = false;
     SPISlave spi_slave;
-    TrimData trim_data;
 
+    uint32_t irq_timestamp = 0;
+    TrimData trim_data;
     BMX160Config config;
 
     enum class Cmd
@@ -517,7 +524,7 @@ private:
     }
 
     /// Correct magnetometer data
-    BMX160Magnet correctMagnet(MagnetRaw data)
+    BMX160Magnet buildMagnetData(MagnetRaw data)
     {
         // Strip the lower 3 bits
         data.x >>= 3;
@@ -526,18 +533,19 @@ private:
 
         if (config.enable_compensation)
         {
-            return BMX160Magnet{Vec3(boschCompensateX(data.x, data.rhall),
-                                     boschCompensateY(data.y, data.rhall),
-                                     boschCompensateZ(data.z, data.rhall))};
+            return BMX160Magnet{Vec3(boschMagCompensateX(data.x, data.rhall),
+                                     boschMagCompensateY(data.y, data.rhall),
+                                     boschMagCompensateZ(data.z, data.rhall)),
+                                0};
         }
         else
         {
-            return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3};
+            return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3, 0};
         }
     }
 
     /// Correct accelerometer data for scale
-    BMX160Accel correctAccel(AccelRaw data)
+    BMX160Accel buildAccelData(AccelRaw data)
     {
         float sens = 0.0f;
         switch (config.acc_range)
@@ -556,11 +564,11 @@ private:
                 break;
         }
 
-        return BMX160Accel{Vec3(data.x, data.y, data.z) * sens};
+        return BMX160Accel{Vec3(data.x, data.y, data.z) * sens, 0};
     }
 
     /// Correct gyroscope data for scale
-    BMX160Gyro correctGyro(GyroRaw data)
+    BMX160Gyro buildGyroData(GyroRaw data)
     {
         float sens = 0.0f;
         switch (config.gyr_range)
@@ -582,7 +590,7 @@ private:
                 break;
         }
 
-        return BMX160Gyro{Vec3(data.x, data.y, data.z) * sens};
+        return BMX160Gyro{Vec3(data.x, data.y, data.z) * sens, 0};
     }
 
     /// Debug function used to print the current error state
@@ -621,6 +629,16 @@ private:
         }
     }
 
+    /// Convert Output Data Rate to the time between reads
+    uint32_t odrToTimeOffset(uint32_t odr)
+    {
+        // Hz = 100 / 2^(8-odr)
+        // Sec = 2^(13-odr) / 3200
+        // Micro = (2^(13-odr)) * 10000 / 32;
+
+        return ((1 << (13 - odr)) * 10000) >> 5;
+    }
+
     /// Read the value of the temperature sensor
     void readTemp(SPITransaction& spi)
     {
@@ -640,9 +658,9 @@ private:
         spi.read(REG_DATA, buf, sizeof(buf));
 
         int idx = 0;
-        magnet_fifo.push(correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
-        gyro_fifo.push(correctGyro(parseStruct<GyroRaw>(buf, idx)));
-        accel_fifo.push(correctAccel(parseStruct<AccelRaw>(buf, idx)));
+        magnet_fifo.push(buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
+        gyro_fifo.push(buildGyroData(parseStruct<GyroRaw>(buf, idx)));
+        accel_fifo.push(buildAccelData(parseStruct<AccelRaw>(buf, idx)));
     }
 
     /// Read the contents of the fifo into buf
@@ -651,7 +669,7 @@ private:
         int len = spi.read(REG_FIFO_LENGTH_0) |
                   ((spi.read(REG_FIFO_LENGTH_1) & 7) << 8);
 
-        uint8_t buf[1000];
+        uint8_t buf[2000];
 
 #ifdef DEBUG
         assert(len <= static_cast<int>(sizeof(buf)) && "Buffer overflow!");
@@ -666,9 +684,10 @@ private:
             if (headerless)
             {
                 magnet_fifo.push(
-                    correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
-                gyro_fifo.push(correctGyro(parseStruct<GyroRaw>(buf, idx)));
-                accel_fifo.push(correctAccel(parseStruct<AccelRaw>(buf, idx)));
+                    buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
+                gyro_fifo.push(buildGyroData(parseStruct<GyroRaw>(buf, idx)));
+                accel_fifo.push(
+                    buildAccelData(parseStruct<AccelRaw>(buf, idx)));
             }
             else
             {
@@ -686,17 +705,17 @@ private:
                     // This contains magnet data
                     if (header & 0x10)
                         magnet_fifo.push(
-                            correctMagnet(parseStruct<MagnetRaw>(buf, idx)));
+                            buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
 
                     // This contains gyro data
                     if (header & 0x08)
                         gyro_fifo.push(
-                            correctGyro(parseStruct<GyroRaw>(buf, idx)));
+                            buildGyroData(parseStruct<GyroRaw>(buf, idx)));
 
                     // This contains accel data
                     if (header & 0x04)
                         accel_fifo.push(
-                            correctAccel(parseStruct<AccelRaw>(buf, idx)));
+                            buildAccelData(parseStruct<AccelRaw>(buf, idx)));
                 }
                 else if (header & 0x40)
                 {
@@ -731,6 +750,31 @@ private:
                 }
             }
         }
+
+        // Update timestamps
+        uint32_t mag_offset =
+            odrToTimeOffset(static_cast<uint32_t>(config.mag_odr));
+        for (int i = 0; i < magnet_fifo.count(); i++)
+        {
+            magnet_fifo.data[magnet_fifo.count() - i - 1].timestamp =
+                irq_timestamp - mag_offset * i;
+        }
+
+        uint32_t acc_offset =
+            odrToTimeOffset(static_cast<uint32_t>(config.acc_odr));
+        for (int i = 0; i < accel_fifo.count(); i++)
+        {
+            accel_fifo.data[accel_fifo.count() - i - 1].timestamp =
+                irq_timestamp - acc_offset * i;
+        }
+
+        uint32_t gyr_offset =
+            odrToTimeOffset(static_cast<uint32_t>(config.gyr_odr));
+        for (int i = 0; i < gyro_fifo.count(); i++)
+        {
+            gyro_fifo.data[gyro_fifo.count() - i - 1].timestamp =
+                irq_timestamp - gyr_offset * i;
+        }
     }
 
     template <typename T>
@@ -854,7 +898,7 @@ private:
     // source code, I have no idea of what it does.
 
     /// Bosch black-box algorithm
-    float boschCompensateX(int16_t x, uint16_t rhall)
+    float boschMagCompensateX(int16_t x, uint16_t rhall)
     {
         /* clang-format off */
         float retval = 0;
@@ -878,7 +922,7 @@ private:
     }
 
     /// Bosch black-box algorithm
-    float boschCompensateY(int16_t y, uint16_t rhall)
+    float boschMagCompensateY(int16_t y, uint16_t rhall)
     {
         /* clang-format off */
         float retval = 0;
@@ -902,7 +946,7 @@ private:
     }
 
     /// Bosch black-box algorithm
-    float boschCompensateZ(int16_t z, uint16_t rhall)
+    float boschMagCompensateZ(int16_t z, uint16_t rhall)
     {
         /* clang-format off */
         float retval = 0;
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 696359fd6..f009373b9 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -31,11 +31,13 @@ struct BMX160Magnet
 {
     /// Data expressed in μT
     Vec3 data;
+    /// Timestamp of the data in microseconds
+    uint32_t timestamp;
 
     void print()
     {
-        printf("MAGNET:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
-               data.getZ());
+        printf("Mag [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+               data.getX(), data.getY(), data.getZ());
     }
 };
 
@@ -44,11 +46,13 @@ struct BMX160Accel
 {
     /// Data expressed in g
     Vec3 data;
+    /// Timestamp of the data in microseconds
+    uint32_t timestamp;
 
     void print()
     {
-        printf("ACCEL:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
-               data.getZ());
+        printf("Acc [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+               data.getX(), data.getY(), data.getZ());
     }
 };
 
@@ -57,11 +61,13 @@ struct BMX160Gyro
 {
     /// Data expressed in °/s
     Vec3 data;
+    /// Timestamp of the data in microseconds
+    uint32_t timestamp;
 
     void print()
     {
-        printf("GYRO:\t%.2f\t%.2f\t%.2f\n", data.getX(), data.getY(),
-               data.getZ());
+        printf("Gyr [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+               data.getX(), data.getY(), data.getZ());
     }
 };
 
@@ -70,33 +76,36 @@ template <typename T, size_t N>
 class BMX160Fifo
 {
 public:
+    friend class BMX160;
+
     BMX160Fifo() : len(0), idx(0) {}
 
-    /// Push a single sample in the fifo (should not be called outside driver)
-    void push(T sample)
+    /// Pop a single sample from the fifo
+    T pop()
     {
 #ifdef DEBUG
-        assert(len < N && "FIFO buffer overflow");
+        assert(idx < N && "FIFO buffer underflow");
 #endif
-        data[len++] = sample;
+        return data[idx++];
     }
 
-    /// Pop a single sample from the fifo
-    T pop()
+    /// Get the remaining count of samples in the fifo
+    int count() { return len; }
+
+private:
+    /// Push a single sample in the fifo (should not be called outside driver)
+    void push(T sample)
     {
 #ifdef DEBUG
-        assert(idx < N && "FIFO buffer underflow");
+        assert(len < N && "FIFO buffer overflow");
 #endif
-        return data[idx++];
+        data[len++] = sample;
     }
 
     /// Clear every sample in the fifo (should not be called outside driver)
     void clear() { len = idx = 0; }
-    /// Get the remaining count of samples in the fifo
-    size_t count() { return len; }
 
-private:
     T data[N];
-    size_t len;
-    size_t idx;
+    int len;
+    int idx;
 };
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 4f6a8a403..714771fea 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -33,6 +33,7 @@ Wiring on STM32F407VG Discovery:
 */
 
 #include <Common.h>
+#include <drivers/HardwareTimer.h>
 #include <drivers/interrupt/external_interrupts.h>
 #include <sensors/BMX160/BMX160.h>
 
@@ -44,24 +45,35 @@ GpioPin spi_miso(GPIOB_BASE, 4);
 GpioPin spi_mosi(GPIOB_BASE, 5);
 
 BMX160 *sensor = nullptr;
-bool invoked   = false;
+uint32_t tick  = 0;
 
-void __attribute__((used)) EXTI8_IRQHandlerImpl() { invoked = true; }
+HardwareTimer<uint32_t> hrclock{
+    TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
+
+void __attribute__((used)) EXTI8_IRQHandlerImpl()
+{
+    tick = hrclock.toIntMicroSeconds(hrclock.tick());
+    if (sensor)
+    {
+        sensor->updateIRQTimestamp(tick);
+    }
+}
 
 int main()
 {
     {
         miosix::FastInterruptDisableLock _lock;
 
-        // Enable SPI3 bus
-        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN;
+        // Enable TIM5 and SPI3 bus
+        RCC->APB1ENR |= RCC_APB1ENR_SPI3EN | RCC_APB1ENR_TIM5EN;
 
         // Setup correct alternate functions for SPI3 bus
         spi_sck.mode(miosix::Mode::ALTERNATE);
-        spi_sck.alternateFunction(6);
         spi_miso.mode(miosix::Mode::ALTERNATE);
-        spi_miso.alternateFunction(6);
         spi_mosi.mode(miosix::Mode::ALTERNATE);
+
+        spi_sck.alternateFunction(6);
+        spi_miso.alternateFunction(6);
         spi_mosi.alternateFunction(6);
 
         // Setup CS
@@ -70,8 +82,13 @@ int main()
 
     cs.high();
 
+    hrclock.setPrescaler(382);
+    hrclock.start();
+
+    enableExternalInterrupt(GPIOB_BASE, 8, InterruptTrigger::FALLING_EDGE);
+
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::DISABLED;
+    config.fifo_mode = BMX160Config::FifoMode::HEADER;
     config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
 
     sensor = new BMX160(bus, cs, config);
@@ -107,23 +124,25 @@ int main()
 
     TRACE("Initialization successful! Ready to process updates...\n");
 
-    // Just now enable interrupts
-    enableExternalInterrupt(GPIOB_BASE, 8, InterruptTrigger::RISING_EDGE);
-
     while (1)
     {
         miosix::Thread::sleep(5000);
 
-        TRACE("Interrupt invoked: %d\n", invoked);
-        invoked = false;
-
         sensor->onSimpleUpdate();
 
         printf("----------------------------\n");
-        sensor->magnet_fifo.pop().print();
-        sensor->accel_fifo.pop().print();
-        sensor->gyro_fifo.pop().print();
+        printf("Last tick: %.3f s\n", tick / 1000000.0f);
         printf("Temp: %.2f deg\n", sensor->temperature);
+        printf("Fill mag: %d\n", sensor->magnet_fifo.count());
+        printf("Fill acc: %d\n", sensor->accel_fifo.count());
+        printf("Fill gyr: %d\n", sensor->gyro_fifo.count());
+
+        for (int i = 0; i < 5; i++)
+        {
+            sensor->magnet_fifo.pop().print();
+            sensor->accel_fifo.pop().print();
+            sensor->gyro_fifo.pop().print();
+        }
     }
 
     return 0;
-- 
GitLab


From 9f5277e263c5d26215e9635a8c76908faa85aee7 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 11 Dec 2020 21:46:53 +0100
Subject: [PATCH 18/24] [BMX160] Small refactor and fixed warnings

---
 src/shared/sensors/BMX160/BMX160.h     | 90 ++++++++++++++------------
 src/shared/sensors/BMX160/BMX160Data.h |  8 +--
 src/tests/drivers/test-bmx160.cpp      | 14 ++--
 3 files changed, 60 insertions(+), 52 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index b5e79338b..862c4ebf4 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -36,20 +36,20 @@ class BMX160 : public Sensor
 {
 private:
     /// Raw struct, read directly from device.
-    struct MagnetRaw
+    struct MagRaw
     {
         int16_t x, y, z;
         uint16_t rhall;
     };
 
     /// Raw struct, read directly from device.
-    struct GyroRaw
+    struct GyrRaw
     {
         int16_t x, y, z;
     };
 
     /// Raw struct, read directly from device.
-    struct AccelRaw
+    struct AccRaw
     {
         int16_t x, y, z;
     };
@@ -275,6 +275,7 @@ public:
         if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
         {
             TRACE("[BMX160] Self-test with fifo is not (yet?) supported!");
+            // FIXME: Set correct error value
             return false;
         }
 
@@ -316,6 +317,8 @@ public:
                       pos_acc[2]);
                 TRACE("neg_acc: %d %d %d\n", neg_acc[0], neg_acc[1],
                       neg_acc[2]);
+
+                last_error = Sensor::ERR_ACCEL_SELFTEST;
                 return false;
             }
 
@@ -334,6 +337,7 @@ public:
             if (!(spi.read(REG_STATUS) & 2))
             {
                 TRACE("[BMX160] Gyroscope self-test failed!\n");
+                last_error = Sensor::ERR_GYRO_SELFTEST;
                 return false;
             }
         }
@@ -351,6 +355,7 @@ public:
             if (readMag(spi, MAG_REG_CONTROL) & 1)
             {
                 TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
+                // FIXME: Set correct error value
                 return false;
             }
 
@@ -363,6 +368,7 @@ public:
             {
                 TRACE("[BMX160] Magnetometer self-test failed!\n");
                 TRACE("result: %d %d %d %d\n", mag[0], mag[1], mag[2], mag[3]);
+                // FIXME: Set correct error value
                 return false;
             }
         }
@@ -382,9 +388,9 @@ public:
         readTemp(spi);
 
         // Delete old samples
-        gyro_fifo.clear();
-        accel_fifo.clear();
-        magnet_fifo.clear();
+        gyr_fifo.clear();
+        acc_fifo.clear();
+        mag_fifo.clear();
 
         switch (config.fifo_mode)
         {
@@ -410,12 +416,15 @@ public:
     /// Update the timestamp from an IRQ, timestamp should be in microseconds
     void updateIRQTimestamp(uint32_t timestamp) { irq_timestamp = timestamp; }
 
-    BMX160Fifo<BMX160Magnet, 200> magnet_fifo;
-    BMX160Fifo<BMX160Accel, 200> accel_fifo;
-    BMX160Fifo<BMX160Gyro, 200> gyro_fifo;
-    float temperature;
+    float getTemperature() { return temperature; }
+
+    BMX160Fifo<BMX160Mag, 200> mag_fifo;
+    BMX160Fifo<BMX160Acc, 200> acc_fifo;
+    BMX160Fifo<BMX160Gyr, 200> gyr_fifo;
 
 private:
+    float temperature;
+
     bool is_init = false;
     SPISlave spi_slave;
 
@@ -524,7 +533,7 @@ private:
     }
 
     /// Correct magnetometer data
-    BMX160Magnet buildMagnetData(MagnetRaw data)
+    BMX160Mag buildMagData(MagRaw data)
     {
         // Strip the lower 3 bits
         data.x >>= 3;
@@ -533,19 +542,19 @@ private:
 
         if (config.enable_compensation)
         {
-            return BMX160Magnet{Vec3(boschMagCompensateX(data.x, data.rhall),
-                                     boschMagCompensateY(data.y, data.rhall),
-                                     boschMagCompensateZ(data.z, data.rhall)),
-                                0};
+            return BMX160Mag{Vec3(boschMagCompensateX(data.x, data.rhall),
+                                  boschMagCompensateY(data.y, data.rhall),
+                                  boschMagCompensateZ(data.z, data.rhall)),
+                             0};
         }
         else
         {
-            return BMX160Magnet{Vec3(data.x, data.y, data.z) * 0.3, 0};
+            return BMX160Mag{Vec3(data.x, data.y, data.z) * 0.3, 0};
         }
     }
 
     /// Correct accelerometer data for scale
-    BMX160Accel buildAccelData(AccelRaw data)
+    BMX160Acc buildAccData(AccRaw data)
     {
         float sens = 0.0f;
         switch (config.acc_range)
@@ -564,11 +573,11 @@ private:
                 break;
         }
 
-        return BMX160Accel{Vec3(data.x, data.y, data.z) * sens, 0};
+        return BMX160Acc{Vec3(data.x, data.y, data.z) * sens, 0};
     }
 
     /// Correct gyroscope data for scale
-    BMX160Gyro buildGyroData(GyroRaw data)
+    BMX160Gyr buildGyrData(GyrRaw data)
     {
         float sens = 0.0f;
         switch (config.gyr_range)
@@ -590,7 +599,7 @@ private:
                 break;
         }
 
-        return BMX160Gyro{Vec3(data.x, data.y, data.z) * sens, 0};
+        return BMX160Gyr{Vec3(data.x, data.y, data.z) * sens, 0};
     }
 
     /// Debug function used to print the current error state
@@ -658,9 +667,9 @@ private:
         spi.read(REG_DATA, buf, sizeof(buf));
 
         int idx = 0;
-        magnet_fifo.push(buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
-        gyro_fifo.push(buildGyroData(parseStruct<GyroRaw>(buf, idx)));
-        accel_fifo.push(buildAccelData(parseStruct<AccelRaw>(buf, idx)));
+        mag_fifo.push(buildMagData(parseStruct<MagRaw>(buf, idx)));
+        gyr_fifo.push(buildGyrData(parseStruct<GyrRaw>(buf, idx)));
+        acc_fifo.push(buildAccData(parseStruct<AccRaw>(buf, idx)));
     }
 
     /// Read the contents of the fifo into buf
@@ -669,7 +678,8 @@ private:
         int len = spi.read(REG_FIFO_LENGTH_0) |
                   ((spi.read(REG_FIFO_LENGTH_1) & 7) << 8);
 
-        uint8_t buf[2000];
+        // Sometimes the buffer gets over 1000
+        uint8_t buf[1100];
 
 #ifdef DEBUG
         assert(len <= static_cast<int>(sizeof(buf)) && "Buffer overflow!");
@@ -683,11 +693,9 @@ private:
 
             if (headerless)
             {
-                magnet_fifo.push(
-                    buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
-                gyro_fifo.push(buildGyroData(parseStruct<GyroRaw>(buf, idx)));
-                accel_fifo.push(
-                    buildAccelData(parseStruct<AccelRaw>(buf, idx)));
+                mag_fifo.push(buildMagData(parseStruct<MagRaw>(buf, idx)));
+                gyr_fifo.push(buildGyrData(parseStruct<GyrRaw>(buf, idx)));
+                acc_fifo.push(buildAccData(parseStruct<AccRaw>(buf, idx)));
             }
             else
             {
@@ -704,18 +712,18 @@ private:
 
                     // This contains magnet data
                     if (header & 0x10)
-                        magnet_fifo.push(
-                            buildMagnetData(parseStruct<MagnetRaw>(buf, idx)));
+                        mag_fifo.push(
+                            buildMagData(parseStruct<MagRaw>(buf, idx)));
 
                     // This contains gyro data
                     if (header & 0x08)
-                        gyro_fifo.push(
-                            buildGyroData(parseStruct<GyroRaw>(buf, idx)));
+                        gyr_fifo.push(
+                            buildGyrData(parseStruct<GyrRaw>(buf, idx)));
 
                     // This contains accel data
                     if (header & 0x04)
-                        accel_fifo.push(
-                            buildAccelData(parseStruct<AccelRaw>(buf, idx)));
+                        acc_fifo.push(
+                            buildAccData(parseStruct<AccRaw>(buf, idx)));
                 }
                 else if (header & 0x40)
                 {
@@ -754,25 +762,25 @@ private:
         // Update timestamps
         uint32_t mag_offset =
             odrToTimeOffset(static_cast<uint32_t>(config.mag_odr));
-        for (int i = 0; i < magnet_fifo.count(); i++)
+        for (int i = 0; i < mag_fifo.count(); i++)
         {
-            magnet_fifo.data[magnet_fifo.count() - i - 1].timestamp =
+            mag_fifo.data[mag_fifo.count() - i - 1].timestamp =
                 irq_timestamp - mag_offset * i;
         }
 
         uint32_t acc_offset =
             odrToTimeOffset(static_cast<uint32_t>(config.acc_odr));
-        for (int i = 0; i < accel_fifo.count(); i++)
+        for (int i = 0; i < acc_fifo.count(); i++)
         {
-            accel_fifo.data[accel_fifo.count() - i - 1].timestamp =
+            acc_fifo.data[acc_fifo.count() - i - 1].timestamp =
                 irq_timestamp - acc_offset * i;
         }
 
         uint32_t gyr_offset =
             odrToTimeOffset(static_cast<uint32_t>(config.gyr_odr));
-        for (int i = 0; i < gyro_fifo.count(); i++)
+        for (int i = 0; i < gyr_fifo.count(); i++)
         {
-            gyro_fifo.data[gyro_fifo.count() - i - 1].timestamp =
+            gyr_fifo.data[gyr_fifo.count() - i - 1].timestamp =
                 irq_timestamp - gyr_offset * i;
         }
     }
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index f009373b9..51e73ed15 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -27,7 +27,7 @@
 #include <stdio.h>
 
 /// Output from the BMX160 magnetometer
-struct BMX160Magnet
+struct BMX160Mag
 {
     /// Data expressed in μT
     Vec3 data;
@@ -42,7 +42,7 @@ struct BMX160Magnet
 };
 
 /// Output from the BMX160 accelerometer
-struct BMX160Accel
+struct BMX160Acc
 {
     /// Data expressed in g
     Vec3 data;
@@ -57,7 +57,7 @@ struct BMX160Accel
 };
 
 /// Output from the BMX160 Gyroscope
-struct BMX160Gyro
+struct BMX160Gyr
 {
     /// Data expressed in °/s
     Vec3 data;
@@ -72,7 +72,7 @@ struct BMX160Gyro
 };
 
 /// Class representing a BMX160 Data fifo
-template <typename T, size_t N>
+template <typename T, int N>
 class BMX160Fifo
 {
 public:
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 714771fea..9018d566b 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -132,16 +132,16 @@ int main()
 
         printf("----------------------------\n");
         printf("Last tick: %.3f s\n", tick / 1000000.0f);
-        printf("Temp: %.2f deg\n", sensor->temperature);
-        printf("Fill mag: %d\n", sensor->magnet_fifo.count());
-        printf("Fill acc: %d\n", sensor->accel_fifo.count());
-        printf("Fill gyr: %d\n", sensor->gyro_fifo.count());
+        printf("Temp: %.2f deg\n", sensor->getTemperature());
+        printf("Fill mag: %d\n", sensor->mag_fifo.count());
+        printf("Fill acc: %d\n", sensor->acc_fifo.count());
+        printf("Fill gyr: %d\n", sensor->gyr_fifo.count());
 
         for (int i = 0; i < 5; i++)
         {
-            sensor->magnet_fifo.pop().print();
-            sensor->accel_fifo.pop().print();
-            sensor->gyro_fifo.pop().print();
+            sensor->mag_fifo.pop().print();
+            sensor->acc_fifo.pop().print();
+            sensor->gyr_fifo.pop().print();
         }
     }
 
-- 
GitLab


From c021408fcbbebe1bbd62808acadbde4147ac04fd Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 14 Dec 2020 22:12:16 +0100
Subject: [PATCH 19/24] [BMX160] Better timestamp implementation

---
 src/shared/sensors/BMX160/BMX160.h | 68 +++++++++++++++---------------
 1 file changed, 34 insertions(+), 34 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 862c4ebf4..21c276158 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -533,7 +533,7 @@ private:
     }
 
     /// Correct magnetometer data
-    BMX160Mag buildMagData(MagRaw data)
+    BMX160Mag buildMagData(MagRaw data, uint32_t timestamp = 0)
     {
         // Strip the lower 3 bits
         data.x >>= 3;
@@ -545,7 +545,7 @@ private:
             return BMX160Mag{Vec3(boschMagCompensateX(data.x, data.rhall),
                                   boschMagCompensateY(data.y, data.rhall),
                                   boschMagCompensateZ(data.z, data.rhall)),
-                             0};
+                             timestamp};
         }
         else
         {
@@ -554,7 +554,7 @@ private:
     }
 
     /// Correct accelerometer data for scale
-    BMX160Acc buildAccData(AccRaw data)
+    BMX160Acc buildAccData(AccRaw data, uint32_t timestamp = 0)
     {
         float sens = 0.0f;
         switch (config.acc_range)
@@ -573,11 +573,11 @@ private:
                 break;
         }
 
-        return BMX160Acc{Vec3(data.x, data.y, data.z) * sens, 0};
+        return BMX160Acc{Vec3(data.x, data.y, data.z) * sens, timestamp};
     }
 
     /// Correct gyroscope data for scale
-    BMX160Gyr buildGyrData(GyrRaw data)
+    BMX160Gyr buildGyrData(GyrRaw data, uint32_t timestamp = 0)
     {
         float sens = 0.0f;
         switch (config.gyr_range)
@@ -599,7 +599,7 @@ private:
                 break;
         }
 
-        return BMX160Gyr{Vec3(data.x, data.y, data.z) * sens, 0};
+        return BMX160Gyr{Vec3(data.x, data.y, data.z) * sens, timestamp};
     }
 
     /// Debug function used to print the current error state
@@ -685,7 +685,15 @@ private:
         assert(len <= static_cast<int>(sizeof(buf)) && "Buffer overflow!");
 #endif
 
+        // Calculate time offset
+        uint32_t time_offset = std::min({
+            odrToTimeOffset(static_cast<uint32_t>(config.mag_odr)),
+            odrToTimeOffset(static_cast<uint32_t>(config.gyr_odr)),
+            odrToTimeOffset(static_cast<uint32_t>(config.acc_odr)),
+        });
+
         spi.read(REG_FIFO_DATA, buf, len);
+        uint32_t timestamp = 0;
 
         int idx = 0;
         while (idx < len && buf[idx] != 128)
@@ -693,9 +701,14 @@ private:
 
             if (headerless)
             {
-                mag_fifo.push(buildMagData(parseStruct<MagRaw>(buf, idx)));
-                gyr_fifo.push(buildGyrData(parseStruct<GyrRaw>(buf, idx)));
-                acc_fifo.push(buildAccData(parseStruct<AccRaw>(buf, idx)));
+                mag_fifo.push(
+                    buildMagData(parseStruct<MagRaw>(buf, idx), timestamp));
+                gyr_fifo.push(
+                    buildGyrData(parseStruct<GyrRaw>(buf, idx), timestamp));
+                acc_fifo.push(
+                    buildAccData(parseStruct<AccRaw>(buf, idx), timestamp));
+
+                timestamp += time_offset;
             }
             else
             {
@@ -712,18 +725,20 @@ private:
 
                     // This contains magnet data
                     if (header & 0x10)
-                        mag_fifo.push(
-                            buildMagData(parseStruct<MagRaw>(buf, idx)));
+                        mag_fifo.push(buildMagData(
+                            parseStruct<MagRaw>(buf, idx), timestamp));
 
                     // This contains gyro data
                     if (header & 0x08)
-                        gyr_fifo.push(
-                            buildGyrData(parseStruct<GyrRaw>(buf, idx)));
+                        gyr_fifo.push(buildGyrData(
+                            parseStruct<GyrRaw>(buf, idx), timestamp));
 
                     // This contains accel data
                     if (header & 0x04)
-                        acc_fifo.push(
-                            buildAccData(parseStruct<AccRaw>(buf, idx)));
+                        acc_fifo.push(buildAccData(
+                            parseStruct<AccRaw>(buf, idx), timestamp));
+
+                    timestamp += time_offset;
                 }
                 else if (header & 0x40)
                 {
@@ -759,30 +774,15 @@ private:
             }
         }
 
-        // Update timestamps
-        uint32_t mag_offset =
-            odrToTimeOffset(static_cast<uint32_t>(config.mag_odr));
+        // Adjust timestamps
         for (int i = 0; i < mag_fifo.count(); i++)
-        {
-            mag_fifo.data[mag_fifo.count() - i - 1].timestamp =
-                irq_timestamp - mag_offset * i;
-        }
+            mag_fifo.data[i].timestamp += irq_timestamp - timestamp;
 
-        uint32_t acc_offset =
-            odrToTimeOffset(static_cast<uint32_t>(config.acc_odr));
         for (int i = 0; i < acc_fifo.count(); i++)
-        {
-            acc_fifo.data[acc_fifo.count() - i - 1].timestamp =
-                irq_timestamp - acc_offset * i;
-        }
+            acc_fifo.data[i].timestamp += irq_timestamp - timestamp;
 
-        uint32_t gyr_offset =
-            odrToTimeOffset(static_cast<uint32_t>(config.gyr_odr));
         for (int i = 0; i < gyr_fifo.count(); i++)
-        {
-            gyr_fifo.data[gyr_fifo.count() - i - 1].timestamp =
-                irq_timestamp - gyr_offset * i;
-        }
+            gyr_fifo.data[i].timestamp += irq_timestamp - timestamp;
     }
 
     template <typename T>
-- 
GitLab


From bced34cc73fca0378a4cd544b302589b1e2ac049 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 21 Dec 2020 18:38:24 +0100
Subject: [PATCH 20/24] [BMX160] Major refactor after code-review.

Changes:
- Added proper returns values on failure of init() and readFifo()
- Automatic re-initialization after self-test
- Self-test no longer requires fifo disabled
- Moved most of the definitions out of BMX160.h
- Splitted large functions for better code readability
- Fixed documentation
---
 src/shared/sensors/BMX160/BMX160.h       | 1056 +++++++++++-----------
 src/shared/sensors/BMX160/BMX160Config.h |    4 +-
 src/shared/sensors/BMX160/BMX160Data.h   |   14 +-
 src/shared/sensors/BMX160/BMX160Defs.h   |  284 ++++++
 src/tests/drivers/test-bmx160.cpp        |   44 +-
 5 files changed, 849 insertions(+), 553 deletions(-)
 create mode 100644 src/shared/sensors/BMX160/BMX160Defs.h

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 21c276158..b63ff758c 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Copyright (c) 2020 Skyward Experimental Rocketry
  * Authors: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -30,362 +30,106 @@
 
 #include "BMX160Config.h"
 #include "BMX160Data.h"
+#include "BMX160Defs.h"
 
-/// BMX160 Driver.
+/// @brief BMX160 Driver.
 class BMX160 : public Sensor
 {
-private:
-    /// Raw struct, read directly from device.
-    struct MagRaw
-    {
-        int16_t x, y, z;
-        uint16_t rhall;
-    };
-
-    /// Raw struct, read directly from device.
-    struct GyrRaw
-    {
-        int16_t x, y, z;
-    };
-
-    /// Raw struct, read directly from device.
-    struct AccRaw
-    {
-        int16_t x, y, z;
-    };
-
-    /// Struct holding trim data used for magnetomer compensation
-    struct TrimData
-    {
-        int8_t dig_x1;
-        int8_t dig_y1;
-        int8_t dig_x2;
-        int8_t dig_y2;
-        uint16_t dig_z1;
-        int16_t dig_z2;
-        int16_t dig_z3;
-        int16_t dig_z4;
-        uint8_t dig_xy1;
-        int8_t dig_xy2;
-        uint16_t dig_xyz1;
-    };
-
 public:
+    /// @brief BMX160 Constructor.
+    /// @param bus SPI bus
+    /// @param cs SPI Chip Select pin
+    /// @param config BMX160 configuration
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config = {})
         : BMX160(bus, cs, config, SPIBusConfig{})
     {
         spi_slave.config.clock_div = SPIClockDivider::DIV4;
     }
 
+    /// @brief BMX160 Constructor.
+    /// @param bus SPI bus
+    /// @param cs SPI Chip Select pin
+    /// @param config BMX160 configuration
+    /// @param bus_config SPI bus configuration
     BMX160(SPIBusInterface& bus, GpioPin cs, BMX160Config config,
            SPIBusConfig bus_config)
         : spi_slave(bus, cs, bus_config), config(config)
     {
     }
 
-    /// Initialize the driver.
+    /// @brief Initialize the driver.
     bool init() override
     {
 #ifdef DEBUG
         assert(!is_init && "init() should be called once");
 #endif
 
-        SPITransaction spi(spi_slave);
-
-        // Assert CHIPID
+        if (!checkChipid())
         {
-            // Correct value for CHIPID
-            const uint8_t CHIPID = 0xD8;
-
-            uint8_t chipid = spi.read(REG_CHIPID);
-            if (chipid != CHIPID)
-            {
-                TRACE("[BMX160] Got bad CHIPID, value: %X\n", chipid);
-                last_error = Sensor::ERR_NOT_ME;
-                return false;
-            }
-        }
-
-        // Reset the state of the device, just to be sure.
-        sendCmd(spi, Cmd::SOFTRESET);
-        miosix::Thread::sleep(10);
-
-        // Dummy read of REG_COMM_TEST to enable SPI
-        spi.read(REG_COMM_TEST);
-        miosix::Thread::sleep(10);
-
-        // Enable Gyro and Accel and Magneto interface
-        {
-            sendCmd(spi, Cmd::MAG_IF_SET_PMU_MODE, Pmu::NORMAL);
-            miosix::Thread::sleep(80);
-
-            sendCmd(spi, Cmd::GYR_SET_PMU_MODE, Pmu::NORMAL);
-            miosix::Thread::sleep(80);
-
-            sendCmd(spi, Cmd::ACC_SET_PMU_MODE, Pmu::NORMAL);
-            miosix::Thread::sleep(80);
+            TRACE("[BMX160] Got bad CHIPID\n");
+            last_error = Sensor::ERR_NOT_ME;
+            return false;
         }
 
-#ifdef DEBUG
-        // Make sure that all sensors are working!
-        assert((spi.read(REG_PMU_STATUS) & 0x3F) == 0x15 &&
-               "Not all sensors are up and running!");
-#endif
+        softReset();
 
-        // Init magnetometer
+        if (!setPowerMode())
         {
-            /*
-            Little explanation of this:
-            The magnetometer is not controlled directly,
-            instead we have a secondary controller, BMM150,
-            with its own register accessed with REG_MAG_IF_[1-3]
-            */
-
-            // Enable manual configuration mode
-            confMag(spi, 0x80);
-
-            // Put MAG into sleep mode (from suspend mode)
-            writeMag(spi, MAG_REG_RESET, 0x01);
-
-            writeMag(spi, MAG_REG_REPXY, config.mag_repxy);
-            writeMag(spi, MAG_REG_REPZ, config.mag_repz);
-
-            if (config.enable_compensation)
-            {
-                uint8_t trim_x1y1[2]     = {0};
-                uint8_t trim_xyz_data[4] = {0};
-                uint8_t trim_xy1xy2[10]  = {0};
-
-                readMag(spi, MAG_REG_DIG_X1, trim_x1y1, sizeof(trim_x1y1));
-                readMag(spi, MAG_REG_DIG_Z4_0, trim_xyz_data,
-                        sizeof(trim_xyz_data));
-                readMag(spi, MAG_REG_DIG_Z2_0, trim_xy1xy2,
-                        sizeof(trim_xy1xy2));
-
-                TRACE("-------- DUMP OF TRIM REGS --------\n");
-                for (int i = 0; i < 2; i++)
-                    TRACE("trim_x1y1[%d]: %d\n", i, trim_x1y1[i]);
-
-                for (int i = 0; i < 4; i++)
-                    TRACE("trim_xyz_data[%d]: %d\n", i, trim_xyz_data[i]);
-
-                for (int i = 0; i < 10; i++)
-                    TRACE("trim_xy1xy2[%d]: %d\n", i, trim_xy1xy2[i]);
-                TRACE("--------    END OF DUMP    --------\n");
-
-                // Read trim registers
-                trim_data.dig_x1  = trim_x1y1[0];
-                trim_data.dig_y1  = trim_x1y1[1];
-                trim_data.dig_x2  = trim_xyz_data[2];
-                trim_data.dig_y2  = trim_xyz_data[3];
-                trim_data.dig_z1  = trim_xy1xy2[2] | (trim_xy1xy2[3] << 8);
-                trim_data.dig_z2  = trim_xy1xy2[0] | (trim_xy1xy2[1] << 8);
-                trim_data.dig_z3  = trim_xy1xy2[6] | (trim_xy1xy2[7] << 8);
-                trim_data.dig_z4  = trim_xyz_data[0] | (trim_xyz_data[1] << 8);
-                trim_data.dig_xy1 = trim_xy1xy2[9];
-                trim_data.dig_xy2 = trim_xy1xy2[8];
-                trim_data.dig_xyz1 =
-                    trim_xy1xy2[4] | ((trim_xy1xy2[5] & 0x7F) << 8);
-            }
-
-            // Magic sequence to init it
-            writeMag(spi, MAG_REG_CONTROL, 0x02);
-            mapMag(spi, MAG_REG_DATA);
 
-            // Set mag output data rate
-            spi.write(REG_MAG_CONF, static_cast<uint8_t>(config.mag_odr));
-            miosix::Thread::sleep(10);
-
-            // Disable manual configuration mode
-            confMag(spi, 0x00);
-        }
-
-        // Setup accel
-        {
-            spi.write(REG_ACC_CONF,
-                      static_cast<uint8_t>(config.acc_odr) | (1 << 5));
-            spi.write(REG_ACC_RANGE, static_cast<uint8_t>(config.acc_range));
-        }
-
-        // Setup gyro
-        {
-            spi.write(REG_GYR_CONF, static_cast<uint8_t>(config.gyr_odr));
-            spi.write(REG_GYR_RANGE, static_cast<uint8_t>(config.gyr_range));
+            TRACE("[BMX160] Not all interfaces are up and running!\n");
+            // FIXME: Set correct error value
+            return false;
         }
 
-        // Setup fifo if needed
-        if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
         {
+            SPITransaction spi(spi_slave);
 
-            // TODO: Setup fifo downsampling (do we really need it?)
-
-            uint8_t config_byte = 0;
-            config_byte |= (1 << 7);  // fifo_gyr_en
-            config_byte |= (1 << 6);  // fifo_acc_en
-            config_byte |= (1 << 5);  // fifo_mag_en
-
-            if (config.fifo_mode == BMX160Config::FifoMode::HEADER)
-            {
-                config_byte |= (1 << 4);  // fifo_header_en
-            }
-
-            spi.write(REG_FIFO_CONFIG_1, config_byte);
-
-            // Is this really needed?
-            sendCmd(spi, Cmd::FIFO_FLUSH);
+            // Enable both interrupt pins, otherwise they'll just float.
+            spi.write(BMX160Defs::REG_INT_OUT_CTRL,
+                      BMX160Defs::INT_OUT_CTRL_INT2_OUT |
+                          BMX160Defs::INT_OUT_CTRL_INT1_OUT);
         }
 
-        // Enable both interrupt pins, otherwise they'll just float.
-        spi.write(REG_INT_OUT_CTRL, 0x88);
-
-        // Setup interrupts if needed
-        if (config.fifo_int != BMX160Config::FifoInt::DISABLED)
-        {
-            // Set fifo watermark
-            spi.write(REG_FIFO_CONFIG_0, config.fifo_watermark);
-
-            // Enable FIFO full interrupt and fifo watermark
-            spi.write(REG_INT_EN_1, 1 << 5 | 1 << 6);
+        initAcc();
+        initGyr();
+        initMag();
 
-            // Enable interrupt pin map
-            if (config.fifo_int == BMX160Config::FifoInt::PIN_INT1)
-            {
-                // Configure to use INT1
-                spi.write(REG_INT_MAP_1, 1 << 5 | 1 << 6);
-            }
-            else
-            {
-                // Configure to use INT2
-                spi.write(REG_INT_MAP_1, 1 << 1 | 1 << 2);
-            }
-        }
+        initFifo();
+        initInt();
 
         return is_init = true;
     }
 
-    /// Perform self-test on the device.
-    ///
-    /// Warning init() must be re-invoked after a selfTest()!
+    /// @brief Perform self-test on the device.
     bool selfTest() override
     {
 #ifdef DEBUG
         assert(is_init && "init() was not called");
 #endif
 
-        if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
-        {
-            TRACE("[BMX160] Self-test with fifo is not (yet?) supported!");
-            // FIXME: Set correct error value
-            return false;
-        }
-
         // The device will enter in an unusable state when testing.
         is_init = false;
 
-        SPITransaction spi(spi_slave);
-
-        // Self-test acc
+        if (!testAcc() || !testGyr() || !testMag())
         {
-            const uint16_t SELF_TEST_LIMIT = 8192;
-
-            // The acc will complain otherwise...
-            spi.write(REG_ACC_CONF, 0x2C);
-            spi.write(REG_ACC_RANGE, 0x08);
-
-            // Enable acc self-test + positive force + self-test deflection
-            spi.write(REG_SELF_TEST, 0x0D);
-            miosix::Thread::sleep(50);
-
-            int16_t pos_acc[3];
-            spi.read(REG_DATA_ACC, reinterpret_cast<uint8_t*>(pos_acc),
-                     sizeof(pos_acc));
-
-            // Enable acc self-test + negative force + self-test deflection
-            spi.write(REG_SELF_TEST, 0x09);
-            miosix::Thread::sleep(50);
-
-            int16_t neg_acc[3];
-            spi.read(REG_DATA_ACC, reinterpret_cast<uint8_t*>(neg_acc),
-                     sizeof(neg_acc));
-
-            if ((neg_acc[0] - pos_acc[0]) < SELF_TEST_LIMIT ||
-                (neg_acc[1] - pos_acc[1]) < SELF_TEST_LIMIT ||
-                (neg_acc[2] - pos_acc[2]) < SELF_TEST_LIMIT)
-            {
-                TRACE("[BMX160] Accelerometer self-test failed!\n");
-                TRACE("pos_acc: %d %d %d\n", pos_acc[0], pos_acc[1],
-                      pos_acc[2]);
-                TRACE("neg_acc: %d %d %d\n", neg_acc[0], neg_acc[1],
-                      neg_acc[2]);
-
-                last_error = Sensor::ERR_ACCEL_SELFTEST;
-                return false;
-            }
-
-            // Reset self-test
-            spi.write(REG_SELF_TEST, 0);
+            // FIXME: Set correct error value
+            return false;
         }
-
-        // Self-test gyro
+        else
         {
-            // Start gyro self-test
-            spi.write(REG_SELF_TEST, 0x10);
-
-            miosix::Thread::sleep(50);
-
-            // Read back the results
-            if (!(spi.read(REG_STATUS) & 2))
-            {
-                TRACE("[BMX160] Gyroscope self-test failed!\n");
-                last_error = Sensor::ERR_GYRO_SELFTEST;
-                return false;
-            }
+            // Finally reinitialize the device into a known state
+            return init();
         }
-
-        // Self-test magneto
-        {
-            // Enable manual configuration mode
-            confMag(spi, 0x80);
-
-            // Enable self-test and put magnetometer in sleep
-            writeMag(spi, MAG_REG_CONTROL, 1 | 3 << 1);
-            miosix::Thread::sleep(200);
-
-            // Check if it has finished
-            if (readMag(spi, MAG_REG_CONTROL) & 1)
-            {
-                TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
-                // FIXME: Set correct error value
-                return false;
-            }
-
-            // Read back test results
-            int16_t mag[4];
-            readMag(spi, MAG_REG_DATA, reinterpret_cast<uint8_t*>(mag),
-                    sizeof(mag));
-
-            if (!(mag[0] & 1) || !(mag[1] & 1) || !(mag[2] & 1))
-            {
-                TRACE("[BMX160] Magnetometer self-test failed!\n");
-                TRACE("result: %d %d %d %d\n", mag[0], mag[1], mag[2], mag[3]);
-                // FIXME: Set correct error value
-                return false;
-            }
-        }
-
-        return true;
     }
 
-    /// Gather data from FIFO/data registers and temperature sensor.
+    /// @brief Gather data from FIFO/data registers and temperature sensor.
     bool onSimpleUpdate() override
     {
 #ifdef DEBUG
         assert(is_init && "init() was not called");
 #endif
-        SPITransaction spi(spi_slave);
-
         // Read temperature
-        readTemp(spi);
+        readTemp();
 
         // Delete old samples
         gyr_fifo.clear();
@@ -396,30 +140,33 @@ public:
         {
             case BMX160Config::FifoMode::DISABLED:
                 // Just push one sample
-                readData(spi);
+                readData();
                 return true;
 
             case BMX160Config::FifoMode::HEADERLESS:
                 // Read whole FIFO (headerless)
-                readFifo(spi, true);
-                return true;
+                return readFifo(true);
 
             case BMX160Config::FifoMode::HEADER:
                 // Read whole FIFO (header)
-                readFifo(spi, false);
-                return true;
+                return readFifo(false);
         }
 
         return false;
     }
 
-    /// Update the timestamp from an IRQ, timestamp should be in microseconds
+    /// @brief Update the timestamp from an IRQ.
+    /// @param timestamp Timestamp in microseconds
     void updateIRQTimestamp(uint32_t timestamp) { irq_timestamp = timestamp; }
 
+    /// @brief Get last read temperature
     float getTemperature() { return temperature; }
 
+    /// @brief Exported internal magnetometer fifo.
     BMX160Fifo<BMX160Mag, 200> mag_fifo;
+    /// @brief Exported internal accellerometer fifo.
     BMX160Fifo<BMX160Acc, 200> acc_fifo;
+    /// @brief Exported internal gyroscope fifo.
     BMX160Fifo<BMX160Gyr, 200> gyr_fifo;
 
 private:
@@ -429,9 +176,12 @@ private:
     SPISlave spi_slave;
 
     uint32_t irq_timestamp = 0;
-    TrimData trim_data;
+    BMX160Defs::TrimData trim_data;
     BMX160Config config;
 
+    float gyr_sensibility = 0.0f;
+    float acc_sensibility = 0.0f;
+
     enum class Cmd
     {
         START_FOC           = 0x03,
@@ -445,7 +195,7 @@ private:
         SOFTRESET           = 0xB6,
     };
 
-    enum class Pmu
+    enum class PowerMode
     {
         SUSPEND       = 0x0,
         NORMAL        = 0x1,
@@ -453,40 +203,51 @@ private:
         FAST_START_UP = 0x3
     };
 
-    /// Execute CMD,
-    /// Pmu parameter MUST be set to Pmu::SUSPEND,
-    /// unless cmd is one of:
-    /// - ACC_SET_PMU_MODE
-    /// - GYR_SET_PMU_MODE
-    /// - MAG_IF_SET_PMU_MODE
-    void sendCmd(SPITransaction& spi, Cmd cmd, Pmu pmu = Pmu::SUSPEND)
+    /// @brief Execute CMD.
+    ///
+    /// @param cmd Command to be executed.
+    /// @param pmu Powermode (MUST be set to PowerMode::SUSPEND when not
+    /// needed).
+    void sendCmd(SPITransaction& spi, Cmd cmd,
+                 PowerMode pmu = PowerMode::SUSPEND)
     {
-        spi.write(REG_CMD,
+        spi.write(BMX160Defs::REG_CMD,
                   static_cast<uint8_t>(cmd) | static_cast<uint8_t>(pmu));
     }
 
-    /// Convenience function to configure magnetometer
+    /// @brief Convenience function to configure magnetometer.
+    ///
+    /// @param value bmm150 configuration value.
     void confMag(SPITransaction& spi, uint8_t value)
     {
-        spi.write(REG_MAG_IF_0, value);
+        spi.write(BMX160Defs::REG_MAG_IF_0, value);
         miosix::Thread::sleep(10);
     }
 
-    /// Convenience function to map magnetometer for read
+    /// @brief Convenience function to map magnetometer for read.
+    ///
+    /// @param reg bmm150 register to be mapped in REG_DATA_MAG.
     void mapMag(SPITransaction& spi, uint8_t reg)
     {
-        spi.write(REG_MAG_IF_1, reg);
+        spi.write(BMX160Defs::REG_MAG_IF_1, reg);
         miosix::Thread::sleep(10);
     }
 
-    /// Convenience function to read a single byte from magnetometer
+    /// @brief Convenience function to read a single byte from magnetometer.
+    ///
+    /// @param reg Register to be read.
+    /// @return Value read from the register.
     uint8_t readMag(SPITransaction& spi, uint8_t reg)
     {
         mapMag(spi, reg);
-        return spi.read(REG_DATA_MAG);
+        return spi.read(BMX160Defs::REG_DATA_MAG);
     }
 
-    /// Convenience function to read from magnetometer
+    /// @brief Convenience function to read from magnetometer.
+    ///
+    /// @param reg Register to be read.
+    /// @param[out] data Buffer to fill with the data.
+    /// @param size Size of the buffer.
     void readMag(SPITransaction& spi, uint8_t reg, uint8_t* data, size_t size)
     {
         while (size != 0)
@@ -494,118 +255,418 @@ private:
             int burst = 0;
             if (size >= 8)
             {
-                confMag(spi, 0x83);
+                confMag(spi, BMX160Defs::MAG_IF_0_MANUAL |
+                                 BMX160Defs::MAG_IF_0_BURST_8);
                 burst = 8;
             }
             else if (size >= 6)
             {
-                confMag(spi, 0x82);
+                confMag(spi, BMX160Defs::MAG_IF_0_MANUAL |
+                                 BMX160Defs::MAG_IF_0_BURST_6);
                 burst = 6;
             }
             else if (size >= 2)
             {
-                confMag(spi, 0x81);
+                confMag(spi, BMX160Defs::MAG_IF_0_MANUAL |
+                                 BMX160Defs::MAG_IF_0_BURST_2);
                 burst = 2;
             }
             else
             {
-                confMag(spi, 0x80);
+                confMag(spi, BMX160Defs::MAG_IF_0_MANUAL |
+                                 BMX160Defs::MAG_IF_0_BURST_1);
                 burst = 1;
             }
 
             mapMag(spi, reg);
-            spi.read(REG_DATA_MAG, data, burst);
+            spi.read(BMX160Defs::REG_DATA_MAG, data, burst);
 
             reg += burst;
             data += burst;
             size -= burst;
         }
 
-        confMag(spi, 0x80);
+        confMag(spi, BMX160Defs::MAG_IF_0_MANUAL);
     }
 
-    /// Convenience function to write to magnetometer
+    /// @brief Convenience function to write to magnetometer.
+    ///
+    /// @param reg Register to be written to.
+    /// @param value Value to write to the register.
     void writeMag(SPITransaction& spi, uint8_t reg, uint8_t value)
     {
-        spi.write(REG_MAG_IF_3, value);
-        spi.write(REG_MAG_IF_2, reg);
+        spi.write(BMX160Defs::REG_MAG_IF_3, value);
+        spi.write(BMX160Defs::REG_MAG_IF_2, reg);
         miosix::Thread::sleep(10);
     }
 
-    /// Correct magnetometer data
-    BMX160Mag buildMagData(MagRaw data, uint32_t timestamp = 0)
+    /// @brief Check for chipid validity.
+    ///
+    /// @return Returns false on failure.
+    bool checkChipid()
     {
-        // Strip the lower 3 bits
-        data.x >>= 3;
-        data.y >>= 3;
-        data.z >>= 3;
+        SPITransaction spi(spi_slave);
 
-        if (config.enable_compensation)
-        {
-            return BMX160Mag{Vec3(boschMagCompensateX(data.x, data.rhall),
-                                  boschMagCompensateY(data.y, data.rhall),
-                                  boschMagCompensateZ(data.z, data.rhall)),
-                             timestamp};
-        }
-        else
-        {
-            return BMX160Mag{Vec3(data.x, data.y, data.z) * 0.3, 0};
-        }
+        return spi.read(BMX160Defs::REG_CHIPID) == BMX160Defs::CHIPID;
+    }
+
+    /// @brief Perform a soft-reset.
+    void softReset()
+    {
+        SPITransaction spi(spi_slave);
+
+        // Reset the state of the device, just to be sure.
+        sendCmd(spi, Cmd::SOFTRESET);
+        miosix::Thread::sleep(10);
+
+        // Dummy read of REG_COMM_TEST to enable SPI
+        spi.read(BMX160Defs::REG_COMM_TEST);
+        miosix::Thread::sleep(10);
+    }
+
+    /// @brief Wake interfaces up, should be called after soft-reset.
+    ///
+    /// @return Returns false on failure.
+    bool setPowerMode()
+    {
+        SPITransaction spi(spi_slave);
+
+        sendCmd(spi, Cmd::MAG_IF_SET_PMU_MODE, PowerMode::NORMAL);
+        miosix::Thread::sleep(80);
+
+        sendCmd(spi, Cmd::GYR_SET_PMU_MODE, PowerMode::NORMAL);
+        miosix::Thread::sleep(80);
+
+        sendCmd(spi, Cmd::ACC_SET_PMU_MODE, PowerMode::NORMAL);
+        miosix::Thread::sleep(80);
+
+        // Check if all sensors are up and running
+        return (spi.read(BMX160Defs::REG_PMU_STATUS) &
+                BMX160Defs::PMU_STATUS_ALL_MASK) ==
+               BMX160Defs::PMU_STATUS_ALL_NORMAL;
     }
 
-    /// Correct accelerometer data for scale
-    BMX160Acc buildAccData(AccRaw data, uint32_t timestamp = 0)
+    /// @brief Initialize accelerometer.
+    void initAcc()
     {
-        float sens = 0.0f;
+        // Calculate accellerometer sensibility
         switch (config.acc_range)
         {
             case BMX160Config::AccRange::G_2:
-                sens = 1.0f / 16384.0f;
+                acc_sensibility = 1.0f / 16384.0f;
                 break;
             case BMX160Config::AccRange::G_4:
-                sens = 1.0f / 8192.0f;
+                acc_sensibility = 1.0f / 8192.0f;
                 break;
             case BMX160Config::AccRange::G_8:
-                sens = 1.0f / 4096.0f;
+                acc_sensibility = 1.0f / 4096.0f;
                 break;
             case BMX160Config::AccRange::G_16:
-                sens = 1.0f / 2048.0f;
+                acc_sensibility = 1.0f / 2048.0f;
                 break;
         }
 
-        return BMX160Acc{Vec3(data.x, data.y, data.z) * sens, timestamp};
+        SPITransaction spi(spi_slave);
+
+        // Setup ODR and default downsampling
+        spi.write(BMX160Defs::REG_ACC_CONF,
+                  static_cast<uint8_t>(config.acc_odr) | (1 << 5));
+        spi.write(BMX160Defs::REG_ACC_RANGE,
+                  static_cast<uint8_t>(config.acc_range));
     }
 
-    /// Correct gyroscope data for scale
-    BMX160Gyr buildGyrData(GyrRaw data, uint32_t timestamp = 0)
+    /// @brief Initialize gyroscope.
+    void initGyr()
     {
-        float sens = 0.0f;
+        // Calculate gyro sensibility
         switch (config.gyr_range)
         {
             case BMX160Config::GyrRange::DEG_2000:
-                sens = 1.0f / 16.4f;
+                gyr_sensibility = 1.0f / 16.4f;
                 break;
             case BMX160Config::GyrRange::DEG_1000:
-                sens = 1.0f / 32.8f;
+                gyr_sensibility = 1.0f / 32.8f;
                 break;
             case BMX160Config::GyrRange::DEG_500:
-                sens = 1.0f / 65.6f;
+                gyr_sensibility = 1.0f / 65.6f;
                 break;
             case BMX160Config::GyrRange::DEG_250:
-                sens = 1.0f / 131.2f;
+                gyr_sensibility = 1.0f / 131.2f;
                 break;
             case BMX160Config::GyrRange::DEG_125:
-                sens = 1.0f / 262.4f;
+                gyr_sensibility = 1.0f / 262.4f;
                 break;
         }
 
-        return BMX160Gyr{Vec3(data.x, data.y, data.z) * sens, timestamp};
+        SPITransaction spi(spi_slave);
+        spi.write(BMX160Defs::REG_GYR_CONF,
+                  static_cast<uint8_t>(config.gyr_odr));
+        spi.write(BMX160Defs::REG_GYR_RANGE,
+                  static_cast<uint8_t>(config.gyr_range));
+    }
+
+    /// @brief Initialize magnetometer.
+    void initMag()
+    {
+        /*
+        Little explanation of this:
+        The magnetometer is not controlled directly,
+        instead we have a secondary controller, BMM150,
+        with its own register accessed with REG_MAG_IF_[1-3]
+        */
+
+        SPITransaction spi(spi_slave);
+
+        // Enable manual configuration mode
+        confMag(spi, BMX160Defs::MAG_IF_0_MANUAL);
+
+        // Put MAG into sleep mode (from suspend mode)
+        writeMag(spi, BMX160Defs::MAG_REG_RESET,
+                 BMX160Defs::MAG_RESET_POWER_CONTROL);
+
+        writeMag(spi, BMX160Defs::MAG_REG_REPXY, config.mag_repxy);
+        writeMag(spi, BMX160Defs::MAG_REG_REPZ, config.mag_repz);
+
+        if (config.enable_compensation)
+            boschReadTrim(spi);
+
+        // Magic sequence to init it
+        writeMag(spi, BMX160Defs::MAG_REG_CONTROL,
+                 BMX160Defs::MAG_CONTROL_FORCED);
+        mapMag(spi, BMX160Defs::MAG_REG_DATA);
+
+        // Set mag output data rate
+        spi.write(BMX160Defs::REG_MAG_CONF,
+                  static_cast<uint8_t>(config.mag_odr));
+        miosix::Thread::sleep(10);
+
+        // Disable manual configuration mode
+        confMag(spi, 0);
+    }
+
+    /// @brief Initialize FIFO.
+    void initFifo()
+    {
+        if (config.fifo_mode != BMX160Config::FifoMode::DISABLED)
+        {
+            SPITransaction spi(spi_slave);
+
+            uint8_t config_byte = BMX160Defs::FIFO_CONFIG_1_ACC_EN |
+                                  BMX160Defs::FIFO_CONFIG_1_GYR_EN |
+                                  BMX160Defs::FIFO_CONFIG_1_MAG_EN;
+
+            if (config.fifo_mode == BMX160Config::FifoMode::HEADER)
+                config_byte |= BMX160Defs::FIFO_CONFIG_1_HEADER_EN;
+
+            spi.write(BMX160Defs::REG_FIFO_CONFIG_1, config_byte);
+
+            sendCmd(spi, Cmd::FIFO_FLUSH);
+        }
+    }
+
+    /// @brief Initialize interrupts.
+    void initInt()
+    {
+        if (config.fifo_int != BMX160Config::FifoInt::DISABLED)
+        {
+            SPITransaction spi(spi_slave);
+
+            // Set fifo watermark
+            spi.write(BMX160Defs::REG_FIFO_CONFIG_0, config.fifo_watermark);
+
+            // Enable FIFO full interrupt and fifo watermark
+            spi.write(BMX160Defs::REG_INT_EN_1,
+                      BMX160Defs::INT_EN_1_FIFO_FULL |
+                          BMX160Defs::INT_EN_1_FIFO_WATERMARK);
+
+            // Enable interrupt pin map
+            if (config.fifo_int == BMX160Config::FifoInt::PIN_INT1)
+            {
+                // Configure to use INT1
+                spi.write(BMX160Defs::REG_INT_MAP_1,
+                          BMX160Defs::INT_MAP_1_INT_1_FIFO_FULL |
+                              BMX160Defs::INT_MAP_1_INT_1_FIFO_WATERMARK);
+            }
+            else
+            {
+                // Configure to use INT2
+                spi.write(BMX160Defs::REG_INT_MAP_1,
+                          BMX160Defs::INT_MAP_1_INT_2_FIFO_FULL |
+                              BMX160Defs::INT_MAP_1_INT_2_FIFO_WATERMARK);
+            }
+        }
+    }
+
+    /// @brief Self-test accelerometer.
+    ///
+    /// @return Returns false on failure.
+    bool testAcc()
+    {
+        const uint16_t SELF_TEST_LIMIT = 8192;
+        const uint8_t ACC_CONF_TEST    = 0x2C;
+        const uint8_t ACC_RANGE_TEST   = 0x08;
+
+        SPITransaction spi(spi_slave);
+
+        // The acc will complain otherwise...
+        spi.write(BMX160Defs::REG_ACC_CONF, ACC_CONF_TEST);
+        spi.write(BMX160Defs::REG_ACC_RANGE, ACC_RANGE_TEST);
+
+        // Enable acc self-test + positive force + self-test deflection
+        spi.write(BMX160Defs::REG_SELF_TEST,
+                  BMX160Defs::SELF_TEST_ACC_AMP |
+                      BMX160Defs::SELF_TEST_ACC_SIGN |
+                      BMX160Defs::SELF_TEST_ACC_ENABLE);
+        miosix::Thread::sleep(50);
+
+        int16_t pos_acc[3];
+        spi.read(BMX160Defs::REG_DATA_ACC, reinterpret_cast<uint8_t*>(pos_acc),
+                 sizeof(pos_acc));
+
+        // Enable acc self-test + negative force + self-test deflection
+        spi.write(
+            BMX160Defs::REG_SELF_TEST,
+            BMX160Defs::SELF_TEST_ACC_AMP | BMX160Defs::SELF_TEST_ACC_ENABLE);
+        miosix::Thread::sleep(50);
+
+        int16_t neg_acc[3];
+        spi.read(BMX160Defs::REG_DATA_ACC, reinterpret_cast<uint8_t*>(neg_acc),
+                 sizeof(neg_acc));
+
+        if ((neg_acc[0] - pos_acc[0]) < SELF_TEST_LIMIT ||
+            (neg_acc[1] - pos_acc[1]) < SELF_TEST_LIMIT ||
+            (neg_acc[2] - pos_acc[2]) < SELF_TEST_LIMIT)
+        {
+            TRACE("[BMX160] Accelerometer self-test failed!\n");
+            TRACE("pos_acc: %d %d %d\n", pos_acc[0], pos_acc[1], pos_acc[2]);
+            TRACE("neg_acc: %d %d %d\n", neg_acc[0], neg_acc[1], neg_acc[2]);
+
+            return false;
+        }
+
+        // Reset self-test
+        spi.write(BMX160Defs::REG_SELF_TEST, 0);
+        return true;
+    }
+
+    /// @brief Self-test gyroscope.
+    ///
+    /// @return Returns false on failure.
+    bool testGyr()
+    {
+        // Start gyro self-test
+        SPITransaction spi(spi_slave);
+
+        spi.write(BMX160Defs::REG_SELF_TEST, BMX160Defs::SELF_TEST_GYR);
+
+        miosix::Thread::sleep(50);
+
+        // Read back the results
+        if (!(spi.read(BMX160Defs::REG_STATUS) & 2))
+        {
+            TRACE("[BMX160] Gyroscope self-test failed!\n");
+            return false;
+        }
+        else
+        {
+            return true;
+        }
+    }
+
+    /// @brief Self-test magnetometer.
+    ///
+    /// @return Returns false on failure.
+    bool testMag()
+    {
+        SPITransaction spi(spi_slave);
+
+        // Enable manual configuration mode
+        confMag(spi, BMX160Defs::MAG_IF_0_MANUAL);
+
+        // Enable self-test and put magnetometer in sleep
+        writeMag(
+            spi, BMX160Defs::MAG_REG_CONTROL,
+            BMX160Defs::MAG_CONTROL_SELF_TEST | BMX160Defs::MAG_CONTROL_SLEEP);
+        miosix::Thread::sleep(200);
+
+        // Check if it has finished
+        if (readMag(spi, BMX160Defs::MAG_REG_CONTROL) &
+            BMX160Defs::MAG_CONTROL_SELF_TEST)
+        {
+            TRACE("[BMX160] Magnetometer didn't finish self-test!\n");
+            return false;
+        }
+
+        // Read back test results
+        int16_t mag[4];
+        readMag(spi, BMX160Defs::MAG_REG_DATA, reinterpret_cast<uint8_t*>(mag),
+                sizeof(mag));
+
+        // Test results are stored in the lower bit of the 3 axis
+        if (!(mag[0] & 1) || !(mag[1] & 1) || !(mag[2] & 1))
+        {
+            TRACE("[BMX160] Magnetometer self-test failed!\n");
+            TRACE("result: %d %d %d %d\n", mag[0], mag[1], mag[2], mag[3]);
+            return false;
+        }
+        else
+        {
+            return true;
+        }
     }
 
-    /// Debug function used to print the current error state
+    /// @brief Build magnetometer data.
+    ///
+    /// @param data Raw input data.
+    /// @param timestamp Timestamp associated with the data.
+    BMX160Mag buildMagData(BMX160Defs::MagRaw data, uint32_t timestamp = 0)
+    {
+        // Strip the lower 3 bits
+        data.x >>= 3;
+        data.y >>= 3;
+        data.z >>= 3;
+
+        if (config.enable_compensation)
+        {
+            return BMX160Mag{Vec3(boschMagCompensateX(data.x, data.rhall),
+                                  boschMagCompensateY(data.y, data.rhall),
+                                  boschMagCompensateZ(data.z, data.rhall)),
+                             timestamp};
+        }
+        else
+        {
+            return BMX160Mag{
+                Vec3(data.x, data.y, data.z) * BMX160Defs::MAG_SENSIBILITY,
+                timestamp};
+        }
+    }
+
+    /// @brief Build gyroscope data.
+    ///
+    /// @param data Raw input data.
+    /// @param timestamp Timestamp associated with the data.
+    BMX160Acc buildAccData(BMX160Defs::AccRaw data, uint32_t timestamp = 0)
+    {
+        return BMX160Acc{Vec3(data.x, data.y, data.z) * acc_sensibility,
+                         timestamp};
+    }
+
+    /// @brief Build accelerometer data.
+    ///
+    /// @param data Raw input data.
+    /// @param timestamp Timestamp associated with the data.
+    BMX160Gyr buildGyrData(BMX160Defs::GyrRaw data, uint32_t timestamp = 0)
+    {
+        return BMX160Gyr{Vec3(data.x, data.y, data.z) * gyr_sensibility,
+                         timestamp};
+    }
+
+    /// @brief Debug function used to print the current error state
+    ///
+    /// @return String representing the error.
     const char* debugErr(SPITransaction& spi)
     {
-        uint8_t err = spi.read(REG_ERR);
+        uint8_t err = spi.read(BMX160Defs::REG_ERR);
 
         if (err & 1)
         {
@@ -638,7 +699,10 @@ private:
         }
     }
 
-    /// Convert Output Data Rate to the time between reads
+    /// @brief Convert Output Data Rate to the time between samples.
+    ///
+    /// @param odr input output data rate of the sensor.
+    /// @return Time between samples.
     uint32_t odrToTimeOffset(uint32_t odr)
     {
         // Hz = 100 / 2^(8-odr)
@@ -648,35 +712,41 @@ private:
         return ((1 << (13 - odr)) * 10000) >> 5;
     }
 
-    /// Read the value of the temperature sensor
-    void readTemp(SPITransaction& spi)
+    /// @brief Read the value of the temperature sensor
+    void readTemp()
     {
-        const float TEMP_RES = 64.0f / 32768.0f;
+        SPITransaction spi(spi_slave);
 
-        int16_t val =
-            spi.read(REG_TEMPERATURE_0) | (spi.read(REG_TEMPERATURE_1) << 8);
+        int16_t val = spi.read(BMX160Defs::REG_TEMPERATURE_0) |
+                      (spi.read(BMX160Defs::REG_TEMPERATURE_1) << 8);
 
-        // Correct for resolution and offset
-        temperature = (val * TEMP_RES) + 23;
+        // Correct for sensibility and offset
+        temperature = (val * BMX160Defs::TEMP_SENSIBILITY) + 23.0f;
     }
 
-    /// Read the contents of the DATA register
-    void readData(SPITransaction& spi)
+    /// @brief Read the contents of the DATA register
+    void readData()
     {
+        SPITransaction spi(spi_slave);
+
         uint8_t buf[20];
-        spi.read(REG_DATA, buf, sizeof(buf));
+        spi.read(BMX160Defs::REG_DATA, buf, sizeof(buf));
 
         int idx = 0;
-        mag_fifo.push(buildMagData(parseStruct<MagRaw>(buf, idx)));
-        gyr_fifo.push(buildGyrData(parseStruct<GyrRaw>(buf, idx)));
-        acc_fifo.push(buildAccData(parseStruct<AccRaw>(buf, idx)));
+        mag_fifo.push(buildMagData(parseStruct<BMX160Defs::MagRaw>(buf, idx)));
+        gyr_fifo.push(buildGyrData(parseStruct<BMX160Defs::GyrRaw>(buf, idx)));
+        acc_fifo.push(buildAccData(parseStruct<BMX160Defs::AccRaw>(buf, idx)));
     }
 
-    /// Read the contents of the fifo into buf
-    void readFifo(SPITransaction& spi, bool headerless)
+    /// @brief Read the contents of the fifo into buf.
+    ///
+    /// @return Returns false on failure.
+    bool readFifo(bool headerless)
     {
-        int len = spi.read(REG_FIFO_LENGTH_0) |
-                  ((spi.read(REG_FIFO_LENGTH_1) & 7) << 8);
+        SPITransaction spi(spi_slave);
+
+        int len = spi.read(BMX160Defs::REG_FIFO_LENGTH_0) |
+                  ((spi.read(BMX160Defs::REG_FIFO_LENGTH_1) & 7) << 8);
 
         // Sometimes the buffer gets over 1000
         uint8_t buf[1100];
@@ -692,71 +762,75 @@ private:
             odrToTimeOffset(static_cast<uint32_t>(config.acc_odr)),
         });
 
-        spi.read(REG_FIFO_DATA, buf, len);
+        spi.read(BMX160Defs::REG_FIFO_DATA, buf, len);
         uint32_t timestamp = 0;
 
         int idx = 0;
-        while (idx < len && buf[idx] != 128)
+        while (idx < len && buf[idx] != BMX160Defs::FIFO_STOP_BYTE)
         {
 
             if (headerless)
             {
-                mag_fifo.push(
-                    buildMagData(parseStruct<MagRaw>(buf, idx), timestamp));
-                gyr_fifo.push(
-                    buildGyrData(parseStruct<GyrRaw>(buf, idx), timestamp));
-                acc_fifo.push(
-                    buildAccData(parseStruct<AccRaw>(buf, idx), timestamp));
+                mag_fifo.push(buildMagData(
+                    parseStruct<BMX160Defs::MagRaw>(buf, idx), timestamp));
+                gyr_fifo.push(buildGyrData(
+                    parseStruct<BMX160Defs::GyrRaw>(buf, idx), timestamp));
+                acc_fifo.push(buildAccData(
+                    parseStruct<BMX160Defs::AccRaw>(buf, idx), timestamp));
 
                 timestamp += time_offset;
             }
             else
             {
-                // <7:6> fh_mode (0b10 -> regular 0b01 -> control)
-                // <5:2> fh_parm
-                // <1:0> fh_ext
                 uint8_t header = buf[idx++];
-                // Mask out fh_ext
-                header &= ~3;
 
-                if (header & 0x80)
+                if ((header & BMX160Defs::FIFO_HEADER_MODE_MASK) ==
+                    BMX160Defs::FIFO_HEADER_MODE_REGULAR)
                 {
                     // This is a regular packet
 
+                    // Mask out everything but fh_parm
+                    header &= BMX160Defs::FIFO_HEADER_PARM_MASK;
+
                     // This contains magnet data
-                    if (header & 0x10)
+                    if (header & BMX160Defs::FIFO_HEADER_PARM_MAG_DATA)
                         mag_fifo.push(buildMagData(
-                            parseStruct<MagRaw>(buf, idx), timestamp));
+                            parseStruct<BMX160Defs::MagRaw>(buf, idx),
+                            timestamp));
 
                     // This contains gyro data
-                    if (header & 0x08)
+                    if (header & BMX160Defs::FIFO_HEADER_PARM_GYR_DATA)
                         gyr_fifo.push(buildGyrData(
-                            parseStruct<GyrRaw>(buf, idx), timestamp));
+                            parseStruct<BMX160Defs::GyrRaw>(buf, idx),
+                            timestamp));
 
                     // This contains accel data
-                    if (header & 0x04)
+                    if (header & BMX160Defs::FIFO_HEADER_PARM_ACC_DATA)
                         acc_fifo.push(buildAccData(
-                            parseStruct<AccRaw>(buf, idx), timestamp));
+                            parseStruct<BMX160Defs::AccRaw>(buf, idx),
+                            timestamp));
 
                     timestamp += time_offset;
                 }
-                else if (header & 0x40)
+                else if ((header & BMX160Defs::FIFO_HEADER_MODE_MASK) ==
+                         BMX160Defs::FIFO_HEADER_MODE_CONTROL)
                 {
+                    // This is a control packet
+
                     // Mask out everything but fh_parm
-                    header &= 0x1C;
+                    header &= BMX160Defs::FIFO_HEADER_PARM_MASK;
 
-                    // This is a control packet
-                    if (header == 0)
+                    if (header == BMX160Defs::FIFO_HEADER_PARM_SKIP)
                     {
                         // Skip frame
                         idx += 1;
                     }
-                    else if (header == 0x04)
+                    else if (header == BMX160Defs::FIFO_HEADER_PARM_SENSORTIME)
                     {
                         // Sensortime frame
                         idx += 3;
                     }
-                    else if (header == 0x08)
+                    else if (header == BMX160Defs::FIFO_HEADER_PARM_CONFIG)
                     {
                         // FIFO_input_config_frame
                         idx += 1;
@@ -768,8 +842,6 @@ private:
                         "[BMX160] Malformed packet! Aborting fifo "
                         "transfer...\n");
                     break;
-
-                    // TODO: Maybe find a way of recovering from this?
                 }
             }
         }
@@ -783,8 +855,16 @@ private:
 
         for (int i = 0; i < gyr_fifo.count(); i++)
             gyr_fifo.data[i].timestamp += irq_timestamp - timestamp;
+
+        // At the end index should be _exactly_ length, otherwise an error
+        // occurred
+        return idx == len;
     }
 
+    /// @brief Parses T out of a buffer.
+    ///
+    /// @param buf Input buffer.
+    /// @param[in,out] idx Input buffer read index.
     template <typename T>
     T parseStruct(uint8_t* buf, int& idx)
     {
@@ -795,117 +875,51 @@ private:
         return data;
     }
 
-    enum Reg
-    {
-        REG_CHIPID     = 0x00,
-        REG_ERR        = 0x02,
-        REG_PMU_STATUS = 0x03,
-
-        REG_DATA = 0x04,
-
-        REG_DATA_MAG = 0x04,
-        REG_DATA_GYR = 0x0C,
-        REG_DATA_ACC = 0x12,
-
-        REG_SENSORTIME_0 = 0x18,
-        REG_SENSORTIME_1 = 0x19,
-        REG_SENSORTIME_2 = 0x1A,
-
-        REG_STATUS = 0x1B,
-
-        REG_TEMPERATURE_0 = 0x20,
-        REG_TEMPERATURE_1 = 0x21,
-
-        REG_FIFO_LENGTH_0 = 0x22,
-        REG_FIFO_LENGTH_1 = 0x23,
-
-        REG_FIFO_DATA = 0x24,
-
-        REG_ACC_CONF  = 0x40,
-        REG_ACC_RANGE = 0x41,
-        REG_GYR_CONF  = 0x42,
-        REG_GYR_RANGE = 0x43,
-        REG_MAG_CONF  = 0x44,
-
-        REG_FIFO_DOWNS    = 0x45,
-        REG_FIFO_CONFIG_0 = 0x46,
-        REG_FIFO_CONFIG_1 = 0x47,
-
-        REG_MAG_IF_0 = 0x4C,
-        REG_MAG_IF_1 = 0x4D,
-        REG_MAG_IF_2 = 0x4E,
-        REG_MAG_IF_3 = 0x4F,
-
-        REG_INT_EN_0 = 0x50,
-        REG_INT_EN_1 = 0x51,
-        REG_INT_EN_2 = 0x52,
-
-        REG_INT_OUT_CTRL = 0x53,
-        REG_INT_LATCH    = 0x54,
-
-        REG_INT_MAP_0 = 0x55,
-        REG_INT_MAP_1 = 0x56,
-        REG_INT_MAP_2 = 0x57,
-
-        /* INT_DATA_[0-1] not needed */
-        /* INT_LOWHIGH_[0-5] not needed */
-        /* INT_MOTION_[0-3] not needed */
-        /* INT_TAP_[0-1] not needed */
-        /* INT_ORIENT_[0-1] not needed */
-        /* INT_FLAT_[0-1] not needed */
-
-        REG_FOC_CONF = 0x69,
-        REG_CONF     = 0x6A,
-        REG_IF_CONF  = 0x6B,
-
-        /* PMU_TRIGGER not needed */
-
-        REG_SELF_TEST = 0x6D,
-
-        /* NV_CONF not needed */
-        /* OFFSET_[0-6] not needed */
-        /* STEP_CNT_[0-1] not needed */
-        /* STEP_CONF_[0-1] not needed */
-
-        REG_CMD       = 0x7E,
-        REG_COMM_TEST = 0x7F,
-    };
-
-    enum MagReg
-    {
-        MAG_REG_DATA    = 0x42,
-        MAG_REG_RESET   = 0x4B,
-        MAG_REG_CONTROL = 0x4C,
-
-        MAG_REG_REPXY = 0x51,
-        MAG_REG_REPZ  = 0x52,
-
-        // Factory calibrated trim registers.
-        // This is all undocumented territory (the datasheet mentions this as
-        // "reserved")
-
-        MAG_REG_DIG_X1     = 0x5D,
-        MAG_REG_DIG_Y1     = 0x5E,
-        MAG_REG_DIG_Z4_0   = 0x62,
-        MAG_REG_DIG_Z4_1   = 0x63,
-        MAG_REG_DIG_X2     = 0x64,
-        MAG_REG_DIG_Y2     = 0x65,
-        MAG_REG_DIG_Z2_0   = 0x68,
-        MAG_REG_DIG_Z2_1   = 0x69,
-        MAG_REG_DIG_Z1_0   = 0x6A,
-        MAG_REG_DIG_Z1_1   = 0x6B,
-        MAG_REG_DIG_XYZ1_0 = 0x6C,
-        MAG_REG_DIG_XYZ1_1 = 0x6D,
-        MAG_REG_DIG_Z3_0   = 0x6E,
-        MAG_REG_DIG_Z3_1   = 0x6F,
-        MAG_REG_DIG_XY2    = 0x70,
-        MAG_REG_DIG_XY1    = 0x71,
-    };
-
+    // =================================================================
     // Warning: the following code is extrapolated from the bosch driver
     // source code, I have no idea of what it does.
+    // https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1614
+    // =================================================================
+
+    /// @brief Read and parse bosch trim registers
+    void boschReadTrim(SPITransaction& spi)
+    {
+        uint8_t trim_x1y1[2]     = {0};
+        uint8_t trim_xyz_data[4] = {0};
+        uint8_t trim_xy1xy2[10]  = {0};
+
+        readMag(spi, BMX160Defs::MAG_REG_DIG_X1, trim_x1y1, sizeof(trim_x1y1));
+        readMag(spi, BMX160Defs::MAG_REG_DIG_Z4_0, trim_xyz_data,
+                sizeof(trim_xyz_data));
+        readMag(spi, BMX160Defs::MAG_REG_DIG_Z2_0, trim_xy1xy2,
+                sizeof(trim_xy1xy2));
+
+        TRACE("-------- DUMP OF TRIM REGS --------\n");
+        for (int i = 0; i < 2; i++)
+            TRACE("trim_x1y1[%d]: %d\n", i, trim_x1y1[i]);
+
+        for (int i = 0; i < 4; i++)
+            TRACE("trim_xyz_data[%d]: %d\n", i, trim_xyz_data[i]);
+
+        for (int i = 0; i < 10; i++)
+            TRACE("trim_xy1xy2[%d]: %d\n", i, trim_xy1xy2[i]);
+        TRACE("--------    END OF DUMP    --------\n");
+
+        // Read trim registers
+        trim_data.dig_x1   = trim_x1y1[0];
+        trim_data.dig_y1   = trim_x1y1[1];
+        trim_data.dig_x2   = trim_xyz_data[2];
+        trim_data.dig_y2   = trim_xyz_data[3];
+        trim_data.dig_z1   = trim_xy1xy2[2] | (trim_xy1xy2[3] << 8);
+        trim_data.dig_z2   = trim_xy1xy2[0] | (trim_xy1xy2[1] << 8);
+        trim_data.dig_z3   = trim_xy1xy2[6] | (trim_xy1xy2[7] << 8);
+        trim_data.dig_z4   = trim_xyz_data[0] | (trim_xyz_data[1] << 8);
+        trim_data.dig_xy1  = trim_xy1xy2[9];
+        trim_data.dig_xy2  = trim_xy1xy2[8];
+        trim_data.dig_xyz1 = trim_xy1xy2[4] | ((trim_xy1xy2[5] & 0x7F) << 8);
+    }
 
-    /// Bosch black-box algorithm
+    /// @brief Bosch black-box algorithm
     float boschMagCompensateX(int16_t x, uint16_t rhall)
     {
         /* clang-format off */
@@ -929,7 +943,7 @@ private:
         /* clang-format on */
     }
 
-    /// Bosch black-box algorithm
+    /// @brief Bosch black-box algorithm
     float boschMagCompensateY(int16_t y, uint16_t rhall)
     {
         /* clang-format off */
@@ -953,7 +967,7 @@ private:
         /* clang-format on */
     }
 
-    /// Bosch black-box algorithm
+    /// @brief Bosch black-box algorithm
     float boschMagCompensateZ(int16_t z, uint16_t rhall)
     {
         /* clang-format off */
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 4f3f053cf..68a8b4b19 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Copyright (c) 2020 Skyward Experimental Rocketry
  * Authors: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -129,6 +129,7 @@ struct BMX160Config
 
     /// Repetitions for the XY axis
     ///
+    /// (FIXME: Investigate on default values not working)
     /// This are the reccomended presets:
     /// - 0x01, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
     /// - 0x04, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
@@ -140,6 +141,7 @@ struct BMX160Config
 
     /// Repetitions for the Z axis
     ///
+    /// (FIXME: Investigate on default values not working)
     /// This are the reccomended presets:
     /// - 0x02, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
     /// - 0x0E, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 51e73ed15..1c2eb8ecc 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Copyright (c) 2020 Skyward Experimental Rocketry
  * Authors: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -36,8 +36,8 @@ struct BMX160Mag
 
     void print()
     {
-        printf("Mag [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
-               data.getX(), data.getY(), data.getZ());
+        TRACE("Mag [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+              data.getX(), data.getY(), data.getZ());
     }
 };
 
@@ -51,8 +51,8 @@ struct BMX160Acc
 
     void print()
     {
-        printf("Acc [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
-               data.getX(), data.getY(), data.getZ());
+        TRACE("Acc [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+              data.getX(), data.getY(), data.getZ());
     }
 };
 
@@ -66,8 +66,8 @@ struct BMX160Gyr
 
     void print()
     {
-        printf("Gyr [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
-               data.getX(), data.getY(), data.getZ());
+        TRACE("Gyr [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+              data.getX(), data.getY(), data.getZ());
     }
 };
 
diff --git a/src/shared/sensors/BMX160/BMX160Defs.h b/src/shared/sensors/BMX160/BMX160Defs.h
new file mode 100644
index 000000000..490b1417c
--- /dev/null
+++ b/src/shared/sensors/BMX160/BMX160Defs.h
@@ -0,0 +1,284 @@
+/**
+ * Copyright (c) 2020 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * 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 <cstdint>
+
+/// Various BMX160 register/enums definitions.
+namespace BMX160Defs
+{
+
+/// Temperature sensor sensibility.
+const float TEMP_SENSIBILITY = 64.0f / 32768.0f;
+/// Magnetometer fixed sensibility.
+const float MAG_SENSIBILITY = 0.3f;
+
+/// BMX160 Chip Id.
+const uint8_t CHIPID = 0xD8;
+
+/// Default value for FIFO_CONFIG_1.
+const uint8_t FIFO_CONFIG_1_DEFAULT = 0x10;
+
+/// Values for SELF_TEST.
+enum SELF_TEST
+{
+    SELF_TEST_GYR         = 1 << 4,
+    SELF_TEST_ACC_AMP     = 1 << 3,
+    SELF_TEST_ACC_SIGN    = 1 << 2,
+    SELF_TEST_ACC_ENABLE  = 0x01,
+    SELF_TEST_ACC_DISABLE = 0x00
+};
+
+/// Values for FIFO_CONFIG_1.
+enum FIGO_CONFIG_1
+{
+    FIFO_CONFIG_1_ACC_EN    = 1 << 7,
+    FIFO_CONFIG_1_GYR_EN    = 1 << 6,
+    FIFO_CONFIG_1_MAG_EN    = 1 << 5,
+    FIFO_CONFIG_1_HEADER_EN = 1 << 4,
+};
+
+/// Values for INT_OUT_CTRL.
+enum INT_OUT_CTRL
+{
+    INT_OUT_CTRL_INT2_OUT = 1 << 7,
+    INT_OUT_CTRL_INT1_OUT = 1 << 3,
+};
+
+/// Values for INT_MAP_1.
+enum INT_MAP_1
+{
+    INT_MAP_1_INT_1_FIFO_WATERMARK = 1 << 6,
+    INT_MAP_1_INT_1_FIFO_FULL      = 1 << 5,
+    INT_MAP_1_INT_2_FIFO_WATERMARK = 1 << 2,
+    INT_MAP_1_INT_2_FIFO_FULL      = 1 << 1,
+};
+
+/// Values for INT_EN_1.
+enum INT_EN_1
+{
+    INT_EN_1_FIFO_WATERMARK = 1 << 6,
+    INT_EN_1_FIFO_FULL      = 1 << 5,
+};
+
+/// Values for MAG_IF_0.
+enum MAG_IF_0
+{
+    MAG_IF_0_MANUAL  = 1 << 7,
+    MAG_IF_0_BURST_1 = 0x00,
+    MAG_IF_0_BURST_2 = 0x01,
+    MAG_IF_0_BURST_6 = 0x02,
+    MAG_IF_0_BURST_8 = 0x03,
+};
+
+/// Mask for PMU_STATUS.
+const uint8_t PMU_STATUS_ALL_MASK = (3 << 0) | (3 << 2) | (3 << 4);
+/// Value for PMU_STATUS all on normal status.
+const uint8_t PMU_STATUS_ALL_NORMAL = (1 << 0) | (1 << 2) | (1 << 4);
+
+/// This value indicates that the data in the FIFO stops prematurely.
+const uint8_t FIFO_STOP_BYTE = 128;
+
+/// Mask for fifo header mode.
+const uint8_t FIFO_HEADER_MODE_MASK = 0x03 << 6;
+/// Mask for fifo header parm.
+const uint8_t FIFO_HEADER_PARM_MASK = 0x0f << 2;
+/// Mask for fifo header ext.
+const uint8_t FIFO_HEADER_EXT_MASK = 0x03 << 0;
+
+/// Values for fifo header byte.
+enum FIFO_HEADER
+{
+    FIFO_HEADER_MODE_REGULAR = 0x02 << 6,
+    FIFO_HEADER_MODE_CONTROL = 0x01 << 6,
+
+    FIFO_HEADER_PARM_ACC_DATA = 1 << 2,
+    FIFO_HEADER_PARM_GYR_DATA = 1 << 3,
+    FIFO_HEADER_PARM_MAG_DATA = 1 << 4,
+
+    FIFO_HEADER_PARM_SKIP       = 0 << 2,
+    FIFO_HEADER_PARM_SENSORTIME = 1 << 2,
+    FIFO_HEADER_PARM_CONFIG     = 2 << 2,
+};
+
+/// Values for mag RESET.
+enum MAG_RESET
+{
+    MAG_RESET_POWER_CONTROL = 0x01
+};
+
+/// Values for mag CONTROL.
+enum MAG_CONTROL
+{
+    MAG_CONTROL_SELF_TEST = 0x01,
+    MAG_CONTROL_NORMAL    = 0x00 << 1,
+    MAG_CONTROL_FORCED    = 0x01 << 1,
+    MAG_CONTROL_SLEEP     = 0x03 << 1,
+};
+
+/// Raw struct, read directly from device.
+struct MagRaw
+{
+    int16_t x, y, z;
+    uint16_t rhall;
+};
+
+/// Raw struct, read directly from device.
+struct GyrRaw
+{
+    int16_t x, y, z;
+};
+
+/// Raw struct, read directly from device.
+struct AccRaw
+{
+    int16_t x, y, z;
+};
+
+/// Struct holding trim data used for magnetomer compensation
+struct TrimData
+{
+    int8_t dig_x1;
+    int8_t dig_y1;
+    int8_t dig_x2;
+    int8_t dig_y2;
+    uint16_t dig_z1;
+    int16_t dig_z2;
+    int16_t dig_z3;
+    int16_t dig_z4;
+    uint8_t dig_xy1;
+    int8_t dig_xy2;
+    uint16_t dig_xyz1;
+};
+
+/// Internal register definitions
+enum Reg
+{
+    REG_CHIPID     = 0x00,
+    REG_ERR        = 0x02,
+    REG_PMU_STATUS = 0x03,
+
+    REG_DATA = 0x04,
+
+    REG_DATA_MAG = 0x04,
+    REG_DATA_GYR = 0x0C,
+    REG_DATA_ACC = 0x12,
+
+    REG_SENSORTIME_0 = 0x18,
+    REG_SENSORTIME_1 = 0x19,
+    REG_SENSORTIME_2 = 0x1A,
+
+    REG_STATUS = 0x1B,
+
+    REG_TEMPERATURE_0 = 0x20,
+    REG_TEMPERATURE_1 = 0x21,
+
+    REG_FIFO_LENGTH_0 = 0x22,
+    REG_FIFO_LENGTH_1 = 0x23,
+
+    REG_FIFO_DATA = 0x24,
+
+    REG_ACC_CONF  = 0x40,
+    REG_ACC_RANGE = 0x41,
+    REG_GYR_CONF  = 0x42,
+    REG_GYR_RANGE = 0x43,
+    REG_MAG_CONF  = 0x44,
+
+    REG_FIFO_DOWNS    = 0x45,
+    REG_FIFO_CONFIG_0 = 0x46,
+    REG_FIFO_CONFIG_1 = 0x47,
+
+    REG_MAG_IF_0 = 0x4C,
+    REG_MAG_IF_1 = 0x4D,
+    REG_MAG_IF_2 = 0x4E,
+    REG_MAG_IF_3 = 0x4F,
+
+    REG_INT_EN_0 = 0x50,
+    REG_INT_EN_1 = 0x51,
+    REG_INT_EN_2 = 0x52,
+
+    REG_INT_OUT_CTRL = 0x53,
+    REG_INT_LATCH    = 0x54,
+
+    REG_INT_MAP_0 = 0x55,
+    REG_INT_MAP_1 = 0x56,
+    REG_INT_MAP_2 = 0x57,
+
+    /* INT_DATA_[0-1] not needed */
+    /* INT_LOWHIGH_[0-5] not needed */
+    /* INT_MOTION_[0-3] not needed */
+    /* INT_TAP_[0-1] not needed */
+    /* INT_ORIENT_[0-1] not needed */
+    /* INT_FLAT_[0-1] not needed */
+
+    REG_FOC_CONF = 0x69,
+    REG_CONF     = 0x6A,
+    REG_IF_CONF  = 0x6B,
+
+    /* PMU_TRIGGER not needed */
+
+    REG_SELF_TEST = 0x6D,
+
+    /* NV_CONF not needed */
+    /* OFFSET_[0-6] not needed */
+    /* STEP_CNT_[0-1] not needed */
+    /* STEP_CONF_[0-1] not needed */
+
+    REG_CMD       = 0x7E,
+    REG_COMM_TEST = 0x7F,
+};
+
+/// Internal magnetometer definitions
+enum MagReg
+{
+    MAG_REG_DATA    = 0x42,
+    MAG_REG_RESET   = 0x4B,
+    MAG_REG_CONTROL = 0x4C,
+
+    MAG_REG_REPXY = 0x51,
+    MAG_REG_REPZ  = 0x52,
+
+    // Factory calibrated trim registers.
+    // This is all undocumented territory (the datasheet mentions this as
+    // "reserved")
+
+    MAG_REG_DIG_X1     = 0x5D,
+    MAG_REG_DIG_Y1     = 0x5E,
+    MAG_REG_DIG_Z4_0   = 0x62,
+    MAG_REG_DIG_Z4_1   = 0x63,
+    MAG_REG_DIG_X2     = 0x64,
+    MAG_REG_DIG_Y2     = 0x65,
+    MAG_REG_DIG_Z2_0   = 0x68,
+    MAG_REG_DIG_Z2_1   = 0x69,
+    MAG_REG_DIG_Z1_0   = 0x6A,
+    MAG_REG_DIG_Z1_1   = 0x6B,
+    MAG_REG_DIG_XYZ1_0 = 0x6C,
+    MAG_REG_DIG_XYZ1_1 = 0x6D,
+    MAG_REG_DIG_Z3_0   = 0x6E,
+    MAG_REG_DIG_Z3_1   = 0x6F,
+    MAG_REG_DIG_XY2    = 0x70,
+    MAG_REG_DIG_XY1    = 0x71,
+};
+
+}  // namespace BMX160Defs
\ No newline at end of file
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 9018d566b..fc0ce82d3 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -1,5 +1,5 @@
 /**
- * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Copyright (c) 2020 Skyward Experimental Rocketry
  * Authors: Davide Mor
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -101,34 +101,25 @@ int main()
         return -1;
     }
 
-    // Perform self-test only when fifo disabled.
-    if (config.fifo_mode == BMX160Config::FifoMode::DISABLED)
-    {
-        TRACE("Performing self-test...\n");
-
-        if (!sensor->selfTest())
-        {
-            TRACE("Self-test failed! (code: %d)\n", sensor->getLastError());
-            return -1;
-        }
-
-        TRACE("Self-test successful!\n");
-        TRACE("Re-initialization required...\n");
+    TRACE("Performing self-test...\n");
 
-        if (!sensor->init())
-        {
-            TRACE("Init failed! (code: %d)\n", sensor->getLastError());
-            return -1;
-        }
+    if (!sensor->selfTest())
+    {
+        TRACE("Self-test failed! (code: %d)\n", sensor->getLastError());
+        return -1;
     }
 
-    TRACE("Initialization successful! Ready to process updates...\n");
+    TRACE("Self-test successful!\n");
 
     while (1)
     {
         miosix::Thread::sleep(5000);
 
-        sensor->onSimpleUpdate();
+        if (!sensor->onSimpleUpdate())
+        {
+            TRACE("Failed to read data!\n");
+            continue;
+        }
 
         printf("----------------------------\n");
         printf("Last tick: %.3f s\n", tick / 1000000.0f);
@@ -137,12 +128,17 @@ int main()
         printf("Fill acc: %d\n", sensor->acc_fifo.count());
         printf("Fill gyr: %d\n", sensor->gyr_fifo.count());
 
-        for (int i = 0; i < 5; i++)
-        {
+        printf("----------------------------\n");
+        for (int i = 0; i < std::min(sensor->mag_fifo.count(), 5); i++)
             sensor->mag_fifo.pop().print();
+
+        printf("----------------------------\n");
+        for (int i = 0; i < std::min(sensor->acc_fifo.count(), 5); i++)
             sensor->acc_fifo.pop().print();
+
+        printf("----------------------------\n");
+        for (int i = 0; i < std::min(sensor->gyr_fifo.count(), 5); i++)
             sensor->gyr_fifo.pop().print();
-        }
     }
 
     return 0;
-- 
GitLab


From 0d85e86e7e360f0c5c4f0bf9de47612cc580348a Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 21 Dec 2020 22:29:07 +0100
Subject: [PATCH 21/24] [BMX160] Fixed docs.

---
 src/shared/sensors/BMX160/BMX160Config.h | 62 +++++++++++++-----------
 src/shared/sensors/BMX160/BMX160Data.h   | 29 +++++------
 src/shared/sensors/BMX160/BMX160Defs.h   | 52 ++++++++++----------
 3 files changed, 72 insertions(+), 71 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 68a8b4b19..01bca2df5 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -23,37 +23,33 @@
 
 #pragma once
 
+/// @brief BMX160 Configuration
 struct BMX160Config
 {
     BMX160Config() {}
 
-    /// Fifo operating mode.
+    /// @brief Fifo operating mode.
     enum class FifoMode
     {
-        /// The fifo is completely disabled.
-        DISABLED,
-        /// the fifo operations have less overhead,
-        /// but EVERY sensor MUST have the same odr!!
-        HEADERLESS,
-        /// the fifo operations have more overhead,
-        /// but sensors con have different odr.
-        HEADER
+
+        DISABLED,    ///< The fifo is completely disabled.
+        HEADERLESS,  ///< Sensors MUST have the same odr.
+        HEADER       ///< Sensors can have different odr.
     };
 
-    /// Fifo interrupt mode (uses fifo full as trigger)
+    /// @brief Fifo interrupt mode.
+    /// Uses fifo full/watermark as triggers.
     enum class FifoInt
     {
-        /// The interrupts are completely disabled.
-        DISABLED,
-        /// Interrupts are enabled on pin 2.
-        PIN_INT1,
-        /// Interrupts are enabled on pin 1.
-        PIN_INT2,
+
+        DISABLED,  ///< The interrupts are completely disabled.
+        PIN_INT1,  ///< Interrupts are enabled on pin 2.
+        PIN_INT2,  ///< Interrupts are enabled on pin 1.
     };
 
-    /// Accellerometer ODR expressed in Hz.
+    /// @brief Accellerometer ODR expressed in Hz.
     ///
-    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx)
+    /// HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx.
     enum class AccOdr
     {
         HZ_25_32 = 0x01,
@@ -70,7 +66,7 @@ struct BMX160Config
         HZ_1600  = 0x0c,
     };
 
-    /// Range of the accellerometer expressed in +/- g.
+    /// @brief Range of the accellerometer expressed in +/- g.
     enum class AccRange
     {
         G_2  = 0x3,
@@ -79,7 +75,7 @@ struct BMX160Config
         G_16 = 0xC
     };
 
-    /// Gyroscope ODR expressed in Hz.
+    /// @brief Gyroscope ODR expressed in Hz.
     enum class GyrOdr
     {
         HZ_25   = 0x06,
@@ -92,7 +88,7 @@ struct BMX160Config
         HZ_3200 = 0x0d,
     };
 
-    /// Gyroscope range expressed in °/sec.
+    /// @brief Gyroscope range expressed in °/sec.
     enum class GyrRange
     {
         DEG_2000 = 0x0,
@@ -102,9 +98,9 @@ struct BMX160Config
         DEG_125  = 0x4
     };
 
-    /// Magnetometer ODR expressed in Hz.
+    /// @brief Magnetometer ODR expressed in Hz.
     ///
-    /// (HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx).
+    /// HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx.
     enum class MagOdr
     {
         HZ_25_32 = 0x01,
@@ -120,16 +116,20 @@ struct BMX160Config
         HZ_800   = 0x0b,
     };
 
-    /// Fifo watermark to use, in multiples of 4.
+    /// @brief Fifo watermark to use, in multiples of 4.
     ///
     /// Only values between [0-250] make sense to use.
     /// A really high watermark value (the default) will
     /// disable it, falling back to FIFO full.
     uint8_t fifo_watermark = -1;
 
-    /// Repetitions for the XY axis
+    /// @brief Repetitions for the XY axis
+    ///
+    /// Repetitions represent how many internal samples are averaged in order to
+    /// get the final outputted sample, these presets are the ones reccomended
+    /// in the BMX160 datasheet, for more informations consult the BMM150
+    /// datasheet, chapter 4.2.4 Active mode. 
     ///
-    /// (FIXME: Investigate on default values not working)
     /// This are the reccomended presets:
     /// - 0x01, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
     /// - 0x04, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
@@ -139,9 +139,13 @@ struct BMX160Config
     /// - 0x17, RMS Noise (x/y/z) 0.3/0.3/0.3, Current: 4.9mA  (High accuracy)
     uint8_t mag_repxy = 0x04;
 
-    /// Repetitions for the Z axis
+    /// @brief Repetitions for the Z axis
+    ///
+    /// Repetitions represent how many internal samples are averaged in order to
+    /// get the final outputted sample, these presets are the ones reccomended
+    /// in the BMX160 datasheet, for more informations consult the BMM150
+    /// datasheet, chapter 4.2.4 Active mode. 
     ///
-    /// (FIXME: Investigate on default values not working)
     /// This are the reccomended presets:
     /// - 0x02, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
     /// - 0x0E, RMS Noise (x/y/z) 0.6/0.6/0.6, Current: 0.5mA  (Regular)
@@ -151,7 +155,7 @@ struct BMX160Config
     /// - 0x52, RMS Noise (x/y/z) 0.3/0.3/0.3, Current: 4.9mA  (High accuracy)
     uint8_t mag_repz = 0x0E;
 
-    /// Enable magnetometer data compensation
+    /// @brief Enable magnetometer data compensation
     ///
     /// The magnetomer support compensation, but it's not documented, and the
     /// current implementation is based on the source of the bosch driver.
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index 1c2eb8ecc..bed48f33d 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -26,14 +26,13 @@
 #include <math/Vec3.h>
 #include <stdio.h>
 
-/// Output from the BMX160 magnetometer
+/// @brief Output from the BMX160 magnetometer.
 struct BMX160Mag
 {
-    /// Data expressed in μT
-    Vec3 data;
-    /// Timestamp of the data in microseconds
-    uint32_t timestamp;
+    Vec3 data;           ///< Data expressed in μT.
+    uint32_t timestamp;  ///< Timestamp of the data in microseconds.
 
+    /// @brief Debug function for printing the data.
     void print()
     {
         TRACE("Mag [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
@@ -41,14 +40,13 @@ struct BMX160Mag
     }
 };
 
-/// Output from the BMX160 accelerometer
+/// @brief Output from the BMX160 accelerometer.
 struct BMX160Acc
 {
-    /// Data expressed in g
-    Vec3 data;
-    /// Timestamp of the data in microseconds
-    uint32_t timestamp;
+    Vec3 data;           ///< Data expressed in g.
+    uint32_t timestamp;  ///< Timestamp of the data in microseconds.
 
+    /// @brief Debug function for printing the data.
     void print()
     {
         TRACE("Acc [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
@@ -56,14 +54,13 @@ struct BMX160Acc
     }
 };
 
-/// Output from the BMX160 Gyroscope
+/// @brief Output from the BMX160 Gyroscope.
 struct BMX160Gyr
 {
-    /// Data expressed in °/s
-    Vec3 data;
-    /// Timestamp of the data in microseconds
-    uint32_t timestamp;
+    Vec3 data;           ///< Data expressed in °/s.
+    uint32_t timestamp;  ///< Timestamp of the data in microseconds.
 
+    /// @brief Debug function for printing the data.
     void print()
     {
         TRACE("Gyr [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
@@ -71,7 +68,7 @@ struct BMX160Gyr
     }
 };
 
-/// Class representing a BMX160 Data fifo
+/// @brief Class representing a BMX160 Data fifo
 template <typename T, int N>
 class BMX160Fifo
 {
diff --git a/src/shared/sensors/BMX160/BMX160Defs.h b/src/shared/sensors/BMX160/BMX160Defs.h
index 490b1417c..359db508a 100644
--- a/src/shared/sensors/BMX160/BMX160Defs.h
+++ b/src/shared/sensors/BMX160/BMX160Defs.h
@@ -25,22 +25,22 @@
 
 #include <cstdint>
 
-/// Various BMX160 register/enums definitions.
+/// @brief Various BMX160 register/enums definitions.
 namespace BMX160Defs
 {
 
-/// Temperature sensor sensibility.
+/// @brief Temperature sensor sensibility.
 const float TEMP_SENSIBILITY = 64.0f / 32768.0f;
-/// Magnetometer fixed sensibility.
+/// @brief Magnetometer fixed sensibility.
 const float MAG_SENSIBILITY = 0.3f;
 
-/// BMX160 Chip Id.
+/// @brief BMX160 Chip Id.
 const uint8_t CHIPID = 0xD8;
 
-/// Default value for FIFO_CONFIG_1.
+/// @brief Default value for FIFO_CONFIG_1.
 const uint8_t FIFO_CONFIG_1_DEFAULT = 0x10;
 
-/// Values for SELF_TEST.
+/// @brief Values for SELF_TEST.
 enum SELF_TEST
 {
     SELF_TEST_GYR         = 1 << 4,
@@ -50,7 +50,7 @@ enum SELF_TEST
     SELF_TEST_ACC_DISABLE = 0x00
 };
 
-/// Values for FIFO_CONFIG_1.
+/// @brief Values for FIFO_CONFIG_1.
 enum FIGO_CONFIG_1
 {
     FIFO_CONFIG_1_ACC_EN    = 1 << 7,
@@ -59,14 +59,14 @@ enum FIGO_CONFIG_1
     FIFO_CONFIG_1_HEADER_EN = 1 << 4,
 };
 
-/// Values for INT_OUT_CTRL.
+/// @brief Values for INT_OUT_CTRL.
 enum INT_OUT_CTRL
 {
     INT_OUT_CTRL_INT2_OUT = 1 << 7,
     INT_OUT_CTRL_INT1_OUT = 1 << 3,
 };
 
-/// Values for INT_MAP_1.
+/// @brief Values for INT_MAP_1.
 enum INT_MAP_1
 {
     INT_MAP_1_INT_1_FIFO_WATERMARK = 1 << 6,
@@ -75,14 +75,14 @@ enum INT_MAP_1
     INT_MAP_1_INT_2_FIFO_FULL      = 1 << 1,
 };
 
-/// Values for INT_EN_1.
+/// @brief Values for INT_EN_1.
 enum INT_EN_1
 {
     INT_EN_1_FIFO_WATERMARK = 1 << 6,
     INT_EN_1_FIFO_FULL      = 1 << 5,
 };
 
-/// Values for MAG_IF_0.
+/// @brief Values for MAG_IF_0.
 enum MAG_IF_0
 {
     MAG_IF_0_MANUAL  = 1 << 7,
@@ -92,22 +92,22 @@ enum MAG_IF_0
     MAG_IF_0_BURST_8 = 0x03,
 };
 
-/// Mask for PMU_STATUS.
+/// @brief Mask for PMU_STATUS.
 const uint8_t PMU_STATUS_ALL_MASK = (3 << 0) | (3 << 2) | (3 << 4);
-/// Value for PMU_STATUS all on normal status.
+/// @brief Value for PMU_STATUS all on normal status.
 const uint8_t PMU_STATUS_ALL_NORMAL = (1 << 0) | (1 << 2) | (1 << 4);
 
-/// This value indicates that the data in the FIFO stops prematurely.
+/// @brief This value indicates that the data in the FIFO stops prematurely.
 const uint8_t FIFO_STOP_BYTE = 128;
 
-/// Mask for fifo header mode.
+/// @brief Mask for fifo header mode.
 const uint8_t FIFO_HEADER_MODE_MASK = 0x03 << 6;
-/// Mask for fifo header parm.
+/// @brief Mask for fifo header parm.
 const uint8_t FIFO_HEADER_PARM_MASK = 0x0f << 2;
-/// Mask for fifo header ext.
+/// @brief Mask for fifo header ext.
 const uint8_t FIFO_HEADER_EXT_MASK = 0x03 << 0;
 
-/// Values for fifo header byte.
+/// @brief Values for fifo header byte.
 enum FIFO_HEADER
 {
     FIFO_HEADER_MODE_REGULAR = 0x02 << 6,
@@ -122,13 +122,13 @@ enum FIFO_HEADER
     FIFO_HEADER_PARM_CONFIG     = 2 << 2,
 };
 
-/// Values for mag RESET.
+/// @brief Values for mag RESET.
 enum MAG_RESET
 {
     MAG_RESET_POWER_CONTROL = 0x01
 };
 
-/// Values for mag CONTROL.
+/// @brief Values for mag CONTROL.
 enum MAG_CONTROL
 {
     MAG_CONTROL_SELF_TEST = 0x01,
@@ -137,26 +137,26 @@ enum MAG_CONTROL
     MAG_CONTROL_SLEEP     = 0x03 << 1,
 };
 
-/// Raw struct, read directly from device.
+/// @brief Raw struct, read directly from device.
 struct MagRaw
 {
     int16_t x, y, z;
     uint16_t rhall;
 };
 
-/// Raw struct, read directly from device.
+/// @brief Raw struct, read directly from device.
 struct GyrRaw
 {
     int16_t x, y, z;
 };
 
-/// Raw struct, read directly from device.
+/// @brief Raw struct, read directly from device.
 struct AccRaw
 {
     int16_t x, y, z;
 };
 
-/// Struct holding trim data used for magnetomer compensation
+/// @brief Struct holding trim data used for magnetomer compensation
 struct TrimData
 {
     int8_t dig_x1;
@@ -172,7 +172,7 @@ struct TrimData
     uint16_t dig_xyz1;
 };
 
-/// Internal register definitions
+/// @brief Internal register definitions
 enum Reg
 {
     REG_CHIPID     = 0x00,
@@ -249,7 +249,7 @@ enum Reg
     REG_COMM_TEST = 0x7F,
 };
 
-/// Internal magnetometer definitions
+/// @brief Internal magnetometer definitions
 enum MagReg
 {
     MAG_REG_DATA    = 0x42,
-- 
GitLab


From ef21c31854ab66abfb9b60bb545abff7a276c700 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 21 Dec 2020 22:45:04 +0100
Subject: [PATCH 22/24] [BMX160] Added temperature divider.

---
 src/shared/sensors/BMX160/BMX160.h       |  9 +++++++--
 src/shared/sensors/BMX160/BMX160Config.h | 13 +++++++++++--
 src/tests/drivers/test-bmx160.cpp        |  5 +++--
 3 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index b63ff758c..8cde355c0 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -129,7 +129,10 @@ public:
         assert(is_init && "init() was not called");
 #endif
         // Read temperature
-        readTemp();
+        if (config.temp_divider != 0 && temp_counter % config.temp_divider == 0)
+            readTemp();
+
+        temp_counter++;
 
         // Delete old samples
         gyr_fifo.clear();
@@ -170,7 +173,7 @@ public:
     BMX160Fifo<BMX160Gyr, 200> gyr_fifo;
 
 private:
-    float temperature;
+    float temperature = 0.0f;
 
     bool is_init = false;
     SPISlave spi_slave;
@@ -182,6 +185,8 @@ private:
     float gyr_sensibility = 0.0f;
     float acc_sensibility = 0.0f;
 
+    int temp_counter = 0;
+
     enum class Cmd
     {
         START_FOC           = 0x03,
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 01bca2df5..a28f8bb2e 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -128,7 +128,7 @@ struct BMX160Config
     /// Repetitions represent how many internal samples are averaged in order to
     /// get the final outputted sample, these presets are the ones reccomended
     /// in the BMX160 datasheet, for more informations consult the BMM150
-    /// datasheet, chapter 4.2.4 Active mode. 
+    /// datasheet, chapter 4.2.4 Active mode.
     ///
     /// This are the reccomended presets:
     /// - 0x01, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
@@ -144,7 +144,7 @@ struct BMX160Config
     /// Repetitions represent how many internal samples are averaged in order to
     /// get the final outputted sample, these presets are the ones reccomended
     /// in the BMX160 datasheet, for more informations consult the BMM150
-    /// datasheet, chapter 4.2.4 Active mode. 
+    /// datasheet, chapter 4.2.4 Active mode.
     ///
     /// This are the reccomended presets:
     /// - 0x02, RMS Noise (x/y/z) 1.0/1.0/1.4, Current: 0.17mA (Low power)
@@ -164,6 +164,15 @@ struct BMX160Config
     /// does, it's so bad that I added a switch to disable it, use it wisely!
     bool enable_compensation = true;
 
+    /// @brief Divide the temperature sampling rate.
+    ///
+    /// This is used to limit the sampling of the temperature, use 0 to disable
+    /// it completely.
+    ///
+    /// Every time you call onSimpleUpdate a value is incremented and only if
+    /// the value is a multiple of this parameter a read is performed.
+    int temp_divider = 0;
+
     FifoMode fifo_mode = FifoMode::DISABLED;
     FifoInt fifo_int   = FifoInt::DISABLED;
 
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index fc0ce82d3..70c43c1c6 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -88,8 +88,9 @@ int main()
     enableExternalInterrupt(GPIOB_BASE, 8, InterruptTrigger::FALLING_EDGE);
 
     BMX160Config config;
-    config.fifo_mode = BMX160Config::FifoMode::HEADER;
-    config.fifo_int  = BMX160Config::FifoInt::PIN_INT1;
+    config.fifo_mode    = BMX160Config::FifoMode::HEADER;
+    config.fifo_int     = BMX160Config::FifoInt::PIN_INT1;
+    config.temp_divider = 1;
 
     sensor = new BMX160(bus, cs, config);
 
-- 
GitLab


From 9f19f94945b926cc5ea96e1f86e890927526a274 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 22 Dec 2020 00:43:13 +0100
Subject: [PATCH 23/24] [BMX160] Implemented acc/gyr filtering, and FIFO
 downsampling.

---
 src/shared/sensors/BMX160/BMX160.h       | 37 +++++++++---
 src/shared/sensors/BMX160/BMX160Config.h | 74 ++++++++++++------------
 src/shared/sensors/BMX160/BMX160Data.h   |  6 +-
 src/shared/sensors/BMX160/BMX160Defs.h   |  6 ++
 src/tests/drivers/test-bmx160.cpp        |  6 +-
 5 files changed, 78 insertions(+), 51 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index 8cde355c0..b775389bb 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -373,9 +373,9 @@ private:
 
         SPITransaction spi(spi_slave);
 
-        // Setup ODR and default downsampling
         spi.write(BMX160Defs::REG_ACC_CONF,
-                  static_cast<uint8_t>(config.acc_odr) | (1 << 5));
+                  static_cast<uint8_t>(config.acc_odr) |
+                      static_cast<uint8_t>(config.acc_bwp));
         spi.write(BMX160Defs::REG_ACC_RANGE,
                   static_cast<uint8_t>(config.acc_range));
     }
@@ -405,7 +405,8 @@ private:
 
         SPITransaction spi(spi_slave);
         spi.write(BMX160Defs::REG_GYR_CONF,
-                  static_cast<uint8_t>(config.gyr_odr));
+                  static_cast<uint8_t>(config.gyr_odr) |
+                      static_cast<uint8_t>(config.gyr_bwp));
         spi.write(BMX160Defs::REG_GYR_RANGE,
                   static_cast<uint8_t>(config.gyr_range));
     }
@@ -465,6 +466,17 @@ private:
 
             spi.write(BMX160Defs::REG_FIFO_CONFIG_1, config_byte);
 
+            config_byte = (config.fifo_gyr_downs & 3) |
+                          ((config.fifo_acc_downs & 3) << 4);
+
+            if (config.fifo_acc_filtered)
+                config_byte |= BMX160Defs::FIFO_DOWNS_ACC_FILT_DATA;
+
+            if (config.fifo_gyr_filtered)
+                config_byte |= BMX160Defs::FIFO_DOWNS_GYR_FILT_DATA;
+
+            spi.write(BMX160Defs::REG_FIFO_DOWNS, config_byte);
+
             sendCmd(spi, Cmd::FIFO_FLUSH);
         }
     }
@@ -706,15 +718,24 @@ private:
 
     /// @brief Convert Output Data Rate to the time between samples.
     ///
+    /// Warning, anything resulting in a frequency over 1Hz will underflow the
+    /// calculations!
+    ///
     /// @param odr input output data rate of the sensor.
+    /// @param downs downsampling of the input.
     /// @return Time between samples.
-    uint32_t odrToTimeOffset(uint32_t odr)
+    uint32_t odrToTimeOffset(BMX160Config::Odr odr, uint8_t downs)
     {
+        downs &= 3;
+
+        // Adjust ODR for downsampling
+        uint8_t real_odr = static_cast<uint32_t>(odr) - downs;
+
         // Hz = 100 / 2^(8-odr)
         // Sec = 2^(13-odr) / 3200
         // Micro = (2^(13-odr)) * 10000 / 32;
 
-        return ((1 << (13 - odr)) * 10000) >> 5;
+        return ((1 << (13 - real_odr)) * 10000) >> 5;
     }
 
     /// @brief Read the value of the temperature sensor
@@ -762,9 +783,9 @@ private:
 
         // Calculate time offset
         uint32_t time_offset = std::min({
-            odrToTimeOffset(static_cast<uint32_t>(config.mag_odr)),
-            odrToTimeOffset(static_cast<uint32_t>(config.gyr_odr)),
-            odrToTimeOffset(static_cast<uint32_t>(config.acc_odr)),
+            odrToTimeOffset(config.mag_odr, 0),
+            odrToTimeOffset(config.gyr_odr, config.fifo_gyr_downs),
+            odrToTimeOffset(config.acc_odr, config.fifo_acc_downs),
         });
 
         spi.read(BMX160Defs::REG_FIFO_DATA, buf, len);
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index a28f8bb2e..64812c136 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -47,25 +47,6 @@ struct BMX160Config
         PIN_INT2,  ///< Interrupts are enabled on pin 1.
     };
 
-    /// @brief Accellerometer ODR expressed in Hz.
-    ///
-    /// HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx.
-    enum class AccOdr
-    {
-        HZ_25_32 = 0x01,
-        HZ_25_16 = 0x02,
-        HZ_25_8  = 0x03,
-        HZ_25_4  = 0x04,
-        HZ_25_2  = 0x05,
-        HZ_25    = 0x06,
-        HZ_50    = 0x07,
-        HZ_100   = 0x08,
-        HZ_200   = 0x09,
-        HZ_400   = 0x0a,
-        HZ_800   = 0x0b,
-        HZ_1600  = 0x0c,
-    };
-
     /// @brief Range of the accellerometer expressed in +/- g.
     enum class AccRange
     {
@@ -75,19 +56,6 @@ struct BMX160Config
         G_16 = 0xC
     };
 
-    /// @brief Gyroscope ODR expressed in Hz.
-    enum class GyrOdr
-    {
-        HZ_25   = 0x06,
-        HZ_50   = 0x07,
-        HZ_100  = 0x08,
-        HZ_200  = 0x09,
-        HZ_400  = 0x0a,
-        HZ_800  = 0x0b,
-        HZ_1600 = 0x0c,
-        HZ_3200 = 0x0d,
-    };
-
     /// @brief Gyroscope range expressed in °/sec.
     enum class GyrRange
     {
@@ -98,10 +66,16 @@ struct BMX160Config
         DEG_125  = 0x4
     };
 
-    /// @brief Magnetometer ODR expressed in Hz.
+    /// @brief ODR expressed in Hz.
+    ///
+    /// Limits are as follows (wrong values will error the device):
+    /// - Accelerometer: 25/2  - 1600 (25/32 - 1600 if undersampling is enabled,
+    /// which is not)
+    /// - Gyroscope:     25    - 3200
+    /// - Magnetometer:  25/32 - 800
     ///
-    /// HZ_25_xx indicate fractional frequencies, given by: 25 * 1/xx.
-    enum class MagOdr
+    /// HZ_25_xx indicate fractional frequencies, given by: 25/xx.
+    enum class Odr
     {
         HZ_25_32 = 0x01,
         HZ_25_16 = 0x02,
@@ -114,6 +88,18 @@ struct BMX160Config
         HZ_200   = 0x09,
         HZ_400   = 0x0a,
         HZ_800   = 0x0b,
+        HZ_1600  = 0x0c,
+        HZ_3200  = 0x0d,
+    };
+
+    /// @brief Bandwidth parameter.
+    ///
+    /// 
+    enum class Bwp
+    {
+        NORMAL = 0x02 << 4,  ///< Normal filter operation.
+        OSR2   = 0x01 << 4,  ///< Oversampling rate of 2.
+        OSR4   = 0x00 << 4,  ///< Oversampling rate of 4.
     };
 
     /// @brief Fifo watermark to use, in multiples of 4.
@@ -173,14 +159,26 @@ struct BMX160Config
     /// the value is a multiple of this parameter a read is performed.
     int temp_divider = 0;
 
+    /// @brief Should the fifo use accellerometer filtered data?
+    bool fifo_acc_filtered = true;
+    /// @brief Fifo accellerometer downsampling (between 0 and 15).
+    uint8_t fifo_acc_downs = 0;
+
+    /// @brief Should the fifo use gyroscope filtered data?
+    bool fifo_gyr_filtered = true;
+    /// @brief Fifo gyroscope downsampling (between 0 and 15).
+    uint8_t fifo_gyr_downs = 0;
+
     FifoMode fifo_mode = FifoMode::DISABLED;
     FifoInt fifo_int   = FifoInt::DISABLED;
 
-    AccOdr acc_odr     = AccOdr::HZ_100;
+    Odr acc_odr        = Odr::HZ_100;
+    Bwp acc_bwp        = Bwp::NORMAL;
     AccRange acc_range = AccRange::G_2;
 
-    GyrOdr gyr_odr     = GyrOdr::HZ_100;
+    Odr gyr_odr        = Odr::HZ_100;
+    Bwp gyr_bwp        = Bwp::NORMAL;
     GyrRange gyr_range = GyrRange::DEG_2000;
 
-    MagOdr mag_odr = MagOdr::HZ_100;
+    Odr mag_odr = Odr::HZ_100;
 };
\ No newline at end of file
diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h
index bed48f33d..a032293a3 100644
--- a/src/shared/sensors/BMX160/BMX160Data.h
+++ b/src/shared/sensors/BMX160/BMX160Data.h
@@ -35,7 +35,7 @@ struct BMX160Mag
     /// @brief Debug function for printing the data.
     void print()
     {
-        TRACE("Mag [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+        TRACE("Mag [%.4f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
               data.getX(), data.getY(), data.getZ());
     }
 };
@@ -49,7 +49,7 @@ struct BMX160Acc
     /// @brief Debug function for printing the data.
     void print()
     {
-        TRACE("Acc [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+        TRACE("Acc [%.4f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
               data.getX(), data.getY(), data.getZ());
     }
 };
@@ -63,7 +63,7 @@ struct BMX160Gyr
     /// @brief Debug function for printing the data.
     void print()
     {
-        TRACE("Gyr [%.3f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
+        TRACE("Gyr [%.4f s]:\t%.2f\t%.2f\t%.2f\n", timestamp / 1000000.0f,
               data.getX(), data.getY(), data.getZ());
     }
 };
diff --git a/src/shared/sensors/BMX160/BMX160Defs.h b/src/shared/sensors/BMX160/BMX160Defs.h
index 359db508a..a15aa8492 100644
--- a/src/shared/sensors/BMX160/BMX160Defs.h
+++ b/src/shared/sensors/BMX160/BMX160Defs.h
@@ -59,6 +59,12 @@ enum FIGO_CONFIG_1
     FIFO_CONFIG_1_HEADER_EN = 1 << 4,
 };
 
+/// @brief Values for FIFO_DOWNS.
+enum FIFO_DOWNS {
+    FIFO_DOWNS_ACC_FILT_DATA = 1 << 7,
+    FIFO_DOWNS_GYR_FILT_DATA = 1 << 3,
+};
+
 /// @brief Values for INT_OUT_CTRL.
 enum INT_OUT_CTRL
 {
diff --git a/src/tests/drivers/test-bmx160.cpp b/src/tests/drivers/test-bmx160.cpp
index 70c43c1c6..a24bd3d05 100644
--- a/src/tests/drivers/test-bmx160.cpp
+++ b/src/tests/drivers/test-bmx160.cpp
@@ -116,14 +116,16 @@ int main()
     {
         miosix::Thread::sleep(5000);
 
+        printf("----------------------------\n");
+        
+        uint32_t now = hrclock.toIntMicroSeconds(hrclock.tick());
         if (!sensor->onSimpleUpdate())
         {
             TRACE("Failed to read data!\n");
             continue;
         }
 
-        printf("----------------------------\n");
-        printf("Last tick: %.3f s\n", tick / 1000000.0f);
+        printf("Tick: %.4f s, Now: %.4f s\n", tick / 100000.0f, now / 100000.0f);
         printf("Temp: %.2f deg\n", sensor->getTemperature());
         printf("Fill mag: %d\n", sensor->mag_fifo.count());
         printf("Fill acc: %d\n", sensor->acc_fifo.count());
-- 
GitLab


From 1b01d4edca3dd9c11f93cf4c893e2b77ddde7e5f Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 22 Dec 2020 00:54:44 +0100
Subject: [PATCH 24/24] [BMX160] Fixed docs.

---
 src/shared/sensors/BMX160/BMX160.h       |  8 +++-----
 src/shared/sensors/BMX160/BMX160Config.h | 10 ++++++----
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index b775389bb..dc110c4fd 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -167,7 +167,7 @@ public:
 
     /// @brief Exported internal magnetometer fifo.
     BMX160Fifo<BMX160Mag, 200> mag_fifo;
-    /// @brief Exported internal accellerometer fifo.
+    /// @brief Exported internal accelerometer fifo.
     BMX160Fifo<BMX160Acc, 200> acc_fifo;
     /// @brief Exported internal gyroscope fifo.
     BMX160Fifo<BMX160Gyr, 200> gyr_fifo;
@@ -354,7 +354,7 @@ private:
     /// @brief Initialize accelerometer.
     void initAcc()
     {
-        // Calculate accellerometer sensibility
+        // Calculate accelerometer sensibility
         switch (config.acc_range)
         {
             case BMX160Config::AccRange::G_2:
@@ -726,10 +726,8 @@ private:
     /// @return Time between samples.
     uint32_t odrToTimeOffset(BMX160Config::Odr odr, uint8_t downs)
     {
-        downs &= 3;
-
         // Adjust ODR for downsampling
-        uint8_t real_odr = static_cast<uint32_t>(odr) - downs;
+        uint8_t real_odr = static_cast<uint32_t>(odr) - (downs & 3);
 
         // Hz = 100 / 2^(8-odr)
         // Sec = 2^(13-odr) / 3200
diff --git a/src/shared/sensors/BMX160/BMX160Config.h b/src/shared/sensors/BMX160/BMX160Config.h
index 64812c136..a841ed4f6 100644
--- a/src/shared/sensors/BMX160/BMX160Config.h
+++ b/src/shared/sensors/BMX160/BMX160Config.h
@@ -47,7 +47,7 @@ struct BMX160Config
         PIN_INT2,  ///< Interrupts are enabled on pin 1.
     };
 
-    /// @brief Range of the accellerometer expressed in +/- g.
+    /// @brief Range of the accelerometer expressed in +/- g.
     enum class AccRange
     {
         G_2  = 0x3,
@@ -94,7 +94,9 @@ struct BMX160Config
 
     /// @brief Bandwidth parameter.
     ///
-    /// 
+    /// For more detailed explanation, check the 2.4.1 Data Processing
+    /// Accelerometer and 2.4.2 Data Processing Gyroscope chapters of the BMX160
+    /// datasheet.
     enum class Bwp
     {
         NORMAL = 0x02 << 4,  ///< Normal filter operation.
@@ -159,9 +161,9 @@ struct BMX160Config
     /// the value is a multiple of this parameter a read is performed.
     int temp_divider = 0;
 
-    /// @brief Should the fifo use accellerometer filtered data?
+    /// @brief Should the fifo use accelerometer filtered data?
     bool fifo_acc_filtered = true;
-    /// @brief Fifo accellerometer downsampling (between 0 and 15).
+    /// @brief Fifo accelerometer downsampling (between 0 and 15).
     uint8_t fifo_acc_downs = 0;
 
     /// @brief Should the fifo use gyroscope filtered data?
-- 
GitLab