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