From 64fc5e6ccbab12610ea145792d0401e016fc7e44 Mon Sep 17 00:00:00 2001 From: rasmuskleist Date: Mon, 17 Jul 2023 18:30:51 +0200 Subject: [PATCH 1/5] [math] Adding long vector typedefs --- src/modm/math/geometry/vector3.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modm/math/geometry/vector3.hpp b/src/modm/math/geometry/vector3.hpp index 6287ee671b..e140b8e4ca 100644 --- a/src/modm/math/geometry/vector3.hpp +++ b/src/modm/math/geometry/vector3.hpp @@ -211,6 +211,8 @@ namespace modm typedef Vector Vector3f; typedef Vector Vector3i; typedef Vector Vector3u; + typedef Vector Vector3li; + typedef Vector Vector3lu; } #include "vector3_impl.hpp" From 7bdcab4be6e7c5ddeff6ca71d663c9043543c039 Mon Sep 17 00:00:00 2001 From: rasmuskleist Date: Mon, 17 Jul 2023 18:56:11 +0200 Subject: [PATCH 2/5] [driver] Adding InvenSense 6-Axis IMU driver --- README.md | 19 +- src/modm/driver/inertial/ixm42xxx.hpp | 158 +++ src/modm/driver/inertial/ixm42xxx.lb | 47 + src/modm/driver/inertial/ixm42xxx_data.hpp | 105 ++ .../driver/inertial/ixm42xxx_definitions.hpp | 936 ++++++++++++++++++ src/modm/driver/inertial/ixm42xxx_impl.hpp | 191 ++++ .../driver/inertial/ixm42xxx_transport.hpp | 105 ++ .../inertial/ixm42xxx_transport_impl.hpp | 135 +++ 8 files changed, 1687 insertions(+), 9 deletions(-) create mode 100644 src/modm/driver/inertial/ixm42xxx.hpp create mode 100644 src/modm/driver/inertial/ixm42xxx.lb create mode 100644 src/modm/driver/inertial/ixm42xxx_data.hpp create mode 100644 src/modm/driver/inertial/ixm42xxx_definitions.hpp create mode 100644 src/modm/driver/inertial/ixm42xxx_impl.hpp create mode 100644 src/modm/driver/inertial/ixm42xxx_transport.hpp create mode 100644 src/modm/driver/inertial/ixm42xxx_transport_impl.hpp diff --git a/README.md b/README.md index 5c6fd1cb21..579f3d93d7 100644 --- a/README.md +++ b/README.md @@ -751,68 +751,69 @@ your specific needs. IS31FL3733 ITG3200 +IXM42XXX L3GD20 LAN8720A LAWICEL LIS302DL -LIS3DSH +LIS3DSH LIS3MDL LM75 LP503x LSM303A LSM6DS33 -LSM6DSO +LSM6DSO LTC2984 MAX31855 MAX31865 MAX6966 MAX7219 -MCP23x17 +MCP23x17 MCP2515 MCP3008 MCP7941x MCP990X MMC5603 -MS5611 +MS5611 MS5837 NOKIA5110 NRF24 TFT-DISPLAY PAT9125EL -PCA8574 +PCA8574 PCA9535 PCA9548A PCA9685 SH1106 SIEMENS-S65 -SIEMENS-S75 +SIEMENS-S75 SK6812 SK9822 SSD1306 ST7586S ST7789 -STTS22H +STTS22H STUSB4500 SX1276 TCS3414 TCS3472 TLC594x -TMP102 +TMP102 TMP12x TMP175 TOUCH2046 VL53L0 VL6180 -WS2812 +WS2812 diff --git a/src/modm/driver/inertial/ixm42xxx.hpp b/src/modm/driver/inertial/ixm42xxx.hpp new file mode 100644 index 0000000000..daed1c8155 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx.hpp @@ -0,0 +1,158 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_HPP +#define MODM_IXM42XXX_HPP + +#include "ixm42xxx_data.hpp" +#include "ixm42xxx_definitions.hpp" +#include "ixm42xxx_transport.hpp" + +namespace modm +{ + +/** + * @tparam Transport Either the I2C or SPI Transport Layer. + * @see Ixm42xxxTransportI2c + * @see Ixm42xxxTransportSpi + * + * @ingroup modm_driver_ixm42xxx + * @author Rasmus Kleist Hørlyck Sørensen + */ +template < class Transport > +class Ixm42xxx : public ixm42xxx, public Transport +{ +private: + using Data = modm::ixm42xxxdata::Data; + +public: + /** + * @brief Constructor + * + * @param data A ixm42xxx::data object + * @param address the I2C address of the device + */ + Ixm42xxx(Data &data, uint8_t address = 0b11010000); + + /** + * @brief Initialize the device to use default settings and the endianess of the microcontroller + * @warning Calling this functions resets the device and blocks for 1 ms + */ + modm::ResumableResult + initialize(); + + /** + * @brief Read the temperature data from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readTempData(); + + /** + * @brief Read the accel data from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readAccelData(); + + /** + * @brief Read the gyro data from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readGyroData(); + + /** + * @brief Read the sensor data from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readSensorData(); + +public: + /** + * @brief update a single register. + * + * @param reg The register to be updated + * @param setMask The bits to be setted in the register + * @param clearMask The bits to be cleared in the register. + * @return False in case of any error, e.g. if some register access is not + * permitted. + * + * @warning Only the registers GYRO_CONFIG0, ACCEL_CONFIG0 and PWR_MGMT0 + * can be modified during sensor operation + */ + modm::ResumableResult + updateRegister(Register reg, Register_t setMask, Register_t clearMask = Register_t(0xff)); + + /** + * @brief Write a single register. + * + * @param reg The register to be read + * @param value The value to write to the register + * @return False in case of any error, e.g. if some register access is not + * permitted. + * + * @warning Only the registers GYRO_CONFIG0, ACCEL_CONFIG0 and PWR_MGMT0 + * can be modified during sensor operation + */ + modm::ResumableResult + writeRegister(Register reg, uint8_t value); + + /** + * @brief Read a single register. + * + * @param reg The register to be read + * @param value The placeholder for the read value to be stored + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readRegister(Register reg, uint8_t *value); + + /** + * @brief Read consecutive registers. + * + * @param reg The register to start reading from + * @param data The placeholder for the read values to be stored + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readRegister(Register reg, uint8_t *buffer, std::size_t length); + +public: + /// Get the data object for this sensor. + inline Data& + getData() + { return data; } + +protected: + inline modm::ResumableResult + setRegisterBank(Register regi); + +private: + Data &data; + uint8_t readByte; + uint8_t prevBank; +}; + +} // namespace modm + +#include "ixm42xxx_impl.hpp" + +#endif // MODM_IXM42XXX_HPP \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx.lb b/src/modm/driver/inertial/ixm42xxx.lb new file mode 100644 index 0000000000..a919a2cb17 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx.lb @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen +# +# This file is part of the modm project. +# +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at http://mozilla.org/MPL/2.0/. +# ----------------------------------------------------------------------------- + + +def init(module): + module.name = ":driver:ixm42xxx" + module.description = """\ +# InvenSense 6-Axis IMU + +High Precision 6-Axis MEMS MotionTracking Device + +!!! note "The registers in user bank 1, 2, 3 and 4 are currently not mapped out" + +!!! note "The IXM-42xxx driver has been developed for IIM-42652" + Some functionality may not be implemented or may not be applicable for other InvenSense 6-Axis IMU + +!!! warning "The IXM-42xxx driver I2C transport layer is untested" + +""" + +def prepare(module, options): + module.depends( + ":architecture:register", + ":architecture:i2c.device", + ":architecture:spi.device", + ":math:geometry", + ":math:utils", + ":processing:resumable") + return True + +def build(env): + env.outbasepath = "modm/src/modm/driver/inertial" + env.copy("ixm42xxx.hpp") + env.copy("ixm42xxx_impl.hpp") + env.copy("ixm42xxx_data.hpp") + env.copy("ixm42xxx_definitions.hpp") + env.copy("ixm42xxx_transport.hpp") + env.copy("ixm42xxx_transport_impl.hpp") diff --git a/src/modm/driver/inertial/ixm42xxx_data.hpp b/src/modm/driver/inertial/ixm42xxx_data.hpp new file mode 100644 index 0000000000..08c83b491a --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_data.hpp @@ -0,0 +1,105 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_DATA_HPP +#define MODM_IXM42XXX_DATA_HPP + + +#include + +namespace modm +{ + +template < class Transport > +class Ixm42xxx; + +namespace ixm42xxxdata +{ + +/// @ingroup modm_driver_ixm42xxx +struct +Data +{ + template < class Transport > + friend class ::modm::Ixm42xxx; + + Data() : + accelScale(16.f), + gyroScale(2000.f) + { + } + + // DATA ACCESS + ///@{ + int16_t inline + getTemp() const + { return sensorData.temp; } + + void inline + getTemp(int16_t *temp) const + { *temp = getTemp(); } + + void inline + getTemp(float *temp) const + { float t = getTemp(); *temp = t / 132.48f + 25.f; } + + Vector3i inline + getAccel() const + { return Vector3i(sensorData.accel); } + + void inline + getAccel(Vector3i *accel) const + { Vector3li a = getAccel(); *accel = a; } + + void inline + getAccel(Vector3f *accel) const + { Vector3f a = getAccel(); a *= accelScale / INT16_MAX; *accel = a; } + + Vector3i inline + getGyro() const + { return Vector3i(sensorData.gyro); } + + void inline + getGyro(Vector3i *gyro) const + { *gyro = getGyro(); } + + void inline + getGyro(Vector3f *gyro) const + { Vector3f g = getGyro(); g *= gyroScale / INT16_MAX; *gyro = g; } + ///@} + + constexpr float + getAccelScale() const + { return accelScale; } + + constexpr float + getGyroScale() const + { return gyroScale; } + +private: + struct + SensorData { + int16_t temp; + int16_t accel[3]; + int16_t gyro[3]; + } sensorData; + + float accelScale; + float gyroScale; +}; + +} // ixm42xxxdata namespace + +} // namespace modm + +#endif // MODM_IXM42XXX_DATA_HPP \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx_definitions.hpp b/src/modm/driver/inertial/ixm42xxx_definitions.hpp new file mode 100644 index 0000000000..e5c95159b5 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_definitions.hpp @@ -0,0 +1,936 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_DEFINITIONS_HPP +#define MODM_IXM42XXX_DEFINITIONS_HPP + +#include +#include + +namespace modm +{ + +/// @ingroup modm_driver_ixm42xxx +struct ixm42xxx +{ + enum class + Register : uint16_t + { + REG_BANK_SEL = 0x76, + + // USER BANK 0 REGISTER MAP START + // *** ORed with 0x000 to indicate user bank 0 *** + DEVICE_CONFIG = 0x011, + DRIVE_CONFIG = 0x013, + INT_CONFIG = 0x014, + FIFO_CONFIG = 0x016, + + // Temperature data registers + TEMP_DATA1 = 0x01D, + TEMP_DATA0 = 0x01E, + + // Accel data registers + ACCEL_DATA_X1 = 0x01F, + ACCEL_DATA_X0 = 0x020, + ACCEL_DATA_Y1 = 0x021, + ACCEL_DATA_Y0 = 0x022, + ACCEL_DATA_Z1 = 0x023, + ACCEL_DATA_Z0 = 0x024, + + // Gyro data registers + GYRO_DATA_X1 = 0x025, + GYRO_DATA_X0 = 0x026, + GYRO_DATA_Y1 = 0x027, + GYRO_DATA_Y0 = 0x028, + GYRO_DATA_Z1 = 0x029, + GYRO_DATA_Z0 = 0x02A, + + TMST_FSYNCH = 0x02B, + TMST_FSYNCL = 0x02C, + INT_STATUS = 0x02D, + + // FIFO data registers + FIFO_COUNTH = 0x02E, + FIFO_COUNTL = 0x02F, + FIFO_DATA = 0x030, + + // Apex data register + APEX_DATA0 = 0x031, + APEX_DATA1 = 0x032, + APEX_DATA2 = 0x033, + APEX_DATA3 = 0x034, + APEX_DATA4 = 0x035, + APEX_DATA5 = 0x036, + + INT_STATUS2 = 0x037, + INT_STATUS3 = 0x038, + + SIGNAL_PATH_RESET = 0x04B, + + INTF_CONFIG0 = 0x04C, + INTF_CONFIG1 = 0x04D, + + PWR_MGMT0 = 0x04E, + GYRO_CONFIG0 = 0x04F, + ACCEL_CONFIG0 = 0x050, + GYRO_CONFIG1 = 0x051, + GYRO_ACCEL_CONFIG0 = 0x052, + ACCEL_CONFIG1 = 0x053, + + TMST_CONFIG = 0x054, + APEX_CONFIG0 = 0x056, + SMD_CONFIG = 0x057, + + // FIFO configuration registers + FIFO_CONFIG1 = 0x05F, + FIFO_CONFIG2 = 0x060, + FIFO_CONFIG3 = 0x061, + FSYNC_CONFIG = 0x062, + + // Interrupt configuration registers + INT_CONFIG0 = 0x063, + INT_CONFIG1 = 0x064, + + // Interrupt source registers + INT_SOURCE0 = 0x065, + INT_SOURCE1 = 0x066, + INT_SOURCE3 = 0x068, + INT_SOURCE4 = 0x069, + + FIFO_LOST_PKT0 = 0x06C, + FIFO_LOST_PKT1 = 0x06D, + + SELF_TEST_CONFIG = 0x070, + WHO_AM_I = 0x075, + // USER BANK 0 REGISTER MAP END + + // USER BANK 1 REGISTER MAP START + // *** ORed with 0x100 to indicate user bank 1 *** + SENSOR_CONFIG0 = 0x103, + GYRO_CONFIG_STATIC2 = 0x10B, + GYRO_CONFIG_STATIC3 = 0x10C, + GYRO_CONFIG_STATIC4 = 0x10D, + GYRO_CONFIG_STATIC5 = 0x10E, + GYRO_CONFIG_STATIC6 = 0x10F, + GYRO_CONFIG_STATIC7 = 0x110, + GYRO_CONFIG_STATIC8 = 0x111, + GYRO_CONFIG_STATIC9 = 0x112, + GYRO_CONFIG_STATIC10 = 0x113, + + XG_ST_DATA = 0x15F, + YG_ST_DATA = 0x160, + ZG_ST_DATA = 0x161, + + TMSTVAL0 = 0x162, + TMSTVAL1 = 0x163, + TMSTVAL2 = 0x164, + + INTF_CONFIG4 = 0x17A, + INTF_CONFIG5 = 0x17B, + INTF_CONFIG6 = 0x17C, + // USER BANK 1 REGISTER MAP END + + // USER BANK 2 REGISTER MAP START + // *** ORed with 0x200 to indicate user bank 2 *** + ACCEL_CONFIG_STATIC2 = 0x203, + ACCEL_CONFIG_STATIC3 = 0x204, + ACCEL_CONFIG_STATIC4 = 0x205, + + // Accelerometer self-test data + XA_ST_DATA = 0x23B, + YA_ST_DATA = 0x23C, + ZA_ST_DATA = 0x23D, + // USER BANK 2 REGISTER MAP END + + // USER BANK 3 REGISTER MAP START + // *** ORed with 0x300 to indicate user bank 3 *** + PU_PD_CONFIG1 = 0x306, + PU_PD_CONFIG2 = 0x30E, + // USER BANK 3 REGISTER MAP END + + // USER BANK 4 REGISTER MAP START + // *** ORed with 0x400 to indicate user bank 4 *** + // Apex configuration registers + FDR_CONFIG = 0x409, + APEX_CONFIG1 = 0x440, + APEX_CONFIG2 = 0x441, + APEX_CONFIG3 = 0x442, + APEX_CONFIG4 = 0x443, + APEX_CONFIG5 = 0x444, + APEX_CONFIG6 = 0x445, + APEX_CONFIG7 = 0x446, + APEX_CONFIG8 = 0x447, + APEX_CONFIG9 = 0x448, + APEX_CONFIG10 = 0x449, + + // Wake on motion configuration registers + ACCEL_WOM_X_THR = 0x44A, + ACCEL_WOM_Y_THR = 0x44B, + ACCEL_WOM_Z_THR = 0x44C, + + // Interupt sources registers + INT_SOURCE6 = 0x44D, + INT_SOURCE7 = 0x44E, + INT_SOURCE8 = 0x44F, + INT_SOURCE9 = 0x450, + INT_SOURCE10 = 0x451, + + // User programable offsets registers + OFFSET_USER0 = 0x477, + OFFSET_USER1 = 0x478, + OFFSET_USER2 = 0x479, + OFFSET_USER3 = 0x47A, + OFFSET_USER4 = 0x47B, + OFFSET_USER5 = 0x47C, + OFFSET_USER6 = 0x47D, + OFFSET_USER7 = 0x47E, + OFFSET_USER8 = 0x47F, + // USER BANK 4 REGISTER MAP END + }; + +public: + + /// REG_BANK_SEL reset value is 0x00 + enum class + RegBankSel : uint8_t + { + BANK_SEL2 = Bit2, ///< See BankSel_t + BANK_SEL1 = Bit1, ///< See BankSel_t + BANK_SEL0 = Bit0, ///< See BankSel_t + }; + MODM_FLAGS8(RegBankSel); + + enum class + BankSel : uint8_t + { + Bank0 = 0b000, + Bank1 = 0b001, + Bank2 = 0b010, + Bank3 = 0b011, + Bank4 = 0b100, + }; + typedef modm::Configuration BankSel_t; + + // ------------------------------------------------------------------------------- + + /// DEVICE_CONFIG reset value is 0x00 + enum class + DeviceConfig : uint8_t + { + SPI_MODE = Bit4, ///< SPI mode selection + SOFT_RESET_CONFIG = Bit0, ///< Software reset configuration + }; + MODM_FLAGS8(DeviceConfig); + + enum class + DataMode : uint8_t + { + Mode0 = 0, + Mode1 = int(DeviceConfig::SPI_MODE), + Mode2 = int(DeviceConfig::SPI_MODE), + Mode3 = 0, + }; + typedef modm::Configuration DataMode_t; + + enum class + SoftResetConfig : uint8_t + { + Normal = 0, + Enable = int(DeviceConfig::SOFT_RESET_CONFIG), + }; + typedef modm::Configuration SoftResetConfig_t; + + // ------------------------------------------------------------------------------- + + /// DRIVE_CONFIG reset value is 0x05 + enum class + DriveConfig : uint8_t + { + I2C_SLEW_RATE2 = Bit5, ///< See I2cSlewRate_t + I2C_SLEW_RATE1 = Bit4, ///< See I2cSlewRate_t + I2C_SLEW_RATE0 = Bit3, ///< See I2cSlewRate_t + SPI_SLEW_RATE2 = Bit2, ///< See SpiSlewRate_t + SPI_SLEW_RATE1 = Bit1, ///< See SpiSlewRate_t + SPI_SLEW_RATE0 = Bit0, ///< See SpiSlewRate_t + }; + MODM_FLAGS8(DriveConfig); + + enum class + SlewRate : uint8_t + { + ns20ns60 = 0, + ns12ns36 = int(DriveConfig::SPI_SLEW_RATE0), + ns6ns18 = int(DriveConfig::SPI_SLEW_RATE1), + ns4ns12 = int(DriveConfig::SPI_SLEW_RATE1) | int(DriveConfig::SPI_SLEW_RATE0), + ns2ns6 = int(DriveConfig::SPI_SLEW_RATE2), + ns2 = int(DriveConfig::SPI_SLEW_RATE2) | int(DriveConfig::SPI_SLEW_RATE0), + }; + typedef modm::Configuration I2cSlewRate_t; + typedef modm::Configuration SpiSlewRate_t; + + // ------------------------------------------------------------------------------- + + /// INT_CONFIG reset value is 0x00 + enum class + IntConfig : uint8_t + { + INT2_MODE = Bit5, ///< INT2 pulsed/latched interrupt mode + INT2_DRIVE_CIRCUIT = Bit4, ///< INT2 open drain/push pull drive circuit + INT2_POLARITY = Bit3, ///< INT2 active low/high interrupt polarity + INT1_MODE = Bit2, ///< INT1 pulsed/latched interrupt mode + INT1_DRIVE_CIRCUIT = Bit1, ///< INT1 open drain/push pull drive circuit + INT1_POLARITY = Bit0, ///< INT1 active low/high interrupt polarity + }; + MODM_FLAGS8(IntConfig); + + // ------------------------------------------------------------------------------- + + /// FIFO_CONFIG reset value is 0x00 + enum class + FifoConfig : uint8_t + { + FIFO_MODE1 = Bit7, ///< See FifoMode_t + FIFO_MODE0 = Bit6, ///< See FifoMode_t + }; + MODM_FLAGS8(FifoConfig); + + enum class + FifoMode : uint8_t + { + Bypass = 0, + StreamToFifo = int(FifoConfig::FIFO_MODE0), + StopOnFull = int(FifoConfig::FIFO_MODE1), + }; + typedef modm::Configuration FifoMode_t; + + // ------------------------------------------------------------------------------- + + /// INT_STATUS reset value is 0x10 + enum class + IntStatus : uint8_t + { + UI_FSYNC_INT = Bit6, ///< UI FSYNC Interrupt, clears on read + PLL_RDY_INT = Bit5, ///< PLL Ready Interrupt, clears on read + RESET_DONE_INT = Bit4, ///< Software Reset Complete Interrupt, clears on read + DATA_RDY_INT = Bit3, ///< Data Ready Interrupt, clears on read + FIFO_THS_INT = Bit2, ///< FIFO Buffer Threshold Interrupt, clears on read + FIFO_FULL_INT = Bit1, ///< FIFO Buffer Full Interrupt, clears on read + AGC_RDY_INT = Bit0, ///< AGC Ready Interrupt, clears on read + }; + MODM_FLAGS8(IntStatus); + + // ------------------------------------------------------------------------------- + + /// INT_STATUS2 reset value is 0x00 + enum class + IntStatus2 : uint8_t + { + SMD_INT = Bit3, ///< Significant Motion Detection Interrupt, clears on read + WOM_Z_INT = Bit2, ///< Wake on Motion Interrupt on Z-axis, clears on read + WOM_Y_INT = Bit1, ///< Wake on Motion Interrupt on Y-axis, clears on read + WOM_X_INT = Bit0, ///< Wake on Motion Interrupt on X-axis, clears on read + }; + MODM_FLAGS8(IntStatus2); + + // ------------------------------------------------------------------------------- + + /// INT_STATUS3 reset value is 0x00 + enum class + IntStatus3 : uint8_t + { + STEP_DET_INT = Bit5, ///< Step Detection Interrupt, clears on read + STEP_CNT_OVF_INT = Bit4, ///< Step Count Overflow Interrupt, clears on read + TILT_DET_INT = Bit3, ///< Tilt Detection Interrupt, clears on read + FF_DET_INT = Bit1, ///< Freefall Interrupt, clears on read + TAP_DET_INT = Bit0, ///< Tap Detection Interrupt, clears on read + }; + MODM_FLAGS8(IntStatus3); + + // ------------------------------------------------------------------------------- + + /// SIGNAL_PATH_RESET reset value is 0x00 + enum class + SignalPathReset : uint8_t + { + DMP_INIT_EN = Bit6, ///< DMP enable/disable + DMP_MEM_RESET_EN = Bit5, ///< DMP memory reset enable/disable + ABORT_AND_RESET = Bit3, ///< Signal path reset enable/disable + TMST_STROBE = Bit2, ///< Strobe enable/disable + FIFO_FLUSH = Bit1, ///< FIFO flush enable/disable + }; + MODM_FLAGS8(SignalPathReset); + + // ------------------------------------------------------------------------------- + + /// INTF_CONFIG0 reset value is 0x30 + enum class + IntfConfig0 : uint8_t + { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, ///< FIFO count is reported in records enable/disable + FIFO_COUNT_ENDIAN = Bit5, ///< FIFO count is reported in Little/Big Endian format + SENSOR_DATA_ENDIAN = Bit4, ///< Sensor data is reported in Little/Big Endian + UI_SIFS_CFG1 = Bit1, ///< See UiSifsCfg_t + UI_SIFS_CFG0 = Bit0, ///< See UiSifsCfg_t + }; + MODM_FLAGS8(IntfConfig0); + + enum class + UiSifsCfg : uint8_t + { + DisableSpi = int(IntfConfig0::UI_SIFS_CFG1), + DisableI2c = int(IntfConfig0::UI_SIFS_CFG1) | int(IntfConfig0::UI_SIFS_CFG0), + }; + typedef modm::Configuration UiSifsCfg_t; + + // ------------------------------------------------------------------------------- + + /// INTF_CONFIG1 reset value is 0x91 + enum class + IntfConfig1 : uint8_t + { + ACCEL_LP_CLK_SEL = Bit3, ///< Accelerometer LP mode uses Wake Up/RC oscillator clock + RTC_MODE = Bit2, ///< Require input RTC clock + CLK_SEL1 = Bit1, ///< See ClkSel_t + CLK_SEL0 = Bit0, ///< See ClkSel_t + }; + MODM_FLAGS8(IntfConfig1); + + enum class + ClkSel : uint8_t + { + Rc = 0, + Pll = int(IntfConfig1::CLK_SEL0), + Disable = int(IntfConfig1::CLK_SEL1) | int(IntfConfig1::CLK_SEL0), + }; + typedef modm::Configuration ClkSel_t; + + // ------------------------------------------------------------------------------- + + /// PWR_MGMT0 reset value is 0x00 + enum class + PwrMgmt0 : uint8_t + { + TEMP_DIS = Bit5, ///< Temperature sensor is enable/disable + IDLE = Bit4, ///< Idle mode is enable/disable + GYRO_MODE1 = Bit3, ///< See GyroMode_t + GYRO_MODE0 = Bit2, ///< See GyroMode_t + ACCEL_MODE1 = Bit1, ///< See AccelMode_t + ACCEL_MODE0 = Bit0, ///< See AccelMode_t + }; + MODM_FLAGS8(PwrMgmt0); + + enum class + GyroMode : uint8_t + { + Off = 0, + Standby = int(PwrMgmt0::GYRO_MODE0), + LowNoise = int(PwrMgmt0::GYRO_MODE1) | int(PwrMgmt0::GYRO_MODE0), + }; + typedef modm::Configuration GyroMode_t; + + enum class + AccelMode : uint8_t + { + Off = 0, + LowPower = int(PwrMgmt0::ACCEL_MODE1), + LowNoise = int(PwrMgmt0::ACCEL_MODE1) | int(PwrMgmt0::ACCEL_MODE0), + }; + typedef modm::Configuration AccelMode_t; + + // ------------------------------------------------------------------------------- + + /// GYRO_CONFIG0 reset value is 0x06 + enum class + GyroConfig0 : uint8_t + { + GYRO_FS_SEL2 = Bit7, ///< See GyroFs_t + GYRO_FS_SEL1 = Bit6, ///< See GyroFs_t + GYRO_FS_SEL0 = Bit5, ///< See GyroFs_t + GYRO_ODR3 = Bit3, ///< See GyroOdr_t + GYRO_ODR2 = Bit2, ///< See GyroOdr_t + GYRO_ODR1 = Bit1, ///< See GyroOdr_t + GYRO_ODR0 = Bit0, ///< See GyroOdr_t + }; + MODM_FLAGS8(GyroConfig0); + + enum class + GyroFs: uint8_t + { + dps2000 = 0b000, + dps1000 = 0b001, + dps500 = 0b010, + dps250 = 0b011, + dps125 = 0b100, + dps62_5 = 0b101, + dps31_25 = 0b110, + dps15_625 = 0b111, + }; + typedef modm::Configuration GyroFs_t; + + enum class + GyroOdr: uint8_t + { + kHz32 = 0b0001, + kHz16 = 0b0010, + kHz8 = 0b0011, + kHz4 = 0b0100, + kHz2 = 0b0101, + kHz1 = 0b0110, + Hz200 = 0b0111, + Hz100 = 0b1000, + Hz50 = 0b1001, + Hz25 = 0b1010, + Hz12_5 = 0b1011, + Hz500 = 0b1111, + }; + typedef modm::Configuration GyroOdr_t; + + // ------------------------------------------------------------------------------- + + /// ACCEL_CONFIG0 reset value is 0x06 + enum class + AccelConfig0 : uint8_t + { + ACCEL_FS_SEL2 = Bit7, ///< See AccelFs_t + ACCEL_FS_SEL1 = Bit6, ///< See AccelFs_t + ACCEL_FS_SEL0 = Bit5, ///< See AccelFs_t + ACCEL_ODR3 = Bit3, ///< See AccelOdr_t + ACCEL_ODR2 = Bit2, ///< See AccelOdr_t + ACCEL_ODR1 = Bit1, ///< See AccelOdr_t + ACCEL_ODR0 = Bit0, ///< See AccelOdr_t + }; + MODM_FLAGS8(AccelConfig0); + + enum class + AccelFs : uint8_t + { + g16 = 0b000, ///< ±16g + g8 = 0b001, ///< ±8g + g4 = 0b010, ///< ±4g + g2 = 0b011, ///< ±2g + }; + typedef modm::Configuration AccelFs_t; + + enum class + AccelOdr: uint8_t + { + kHz32 = 0b0001, ///< 32 kHz + kHz16 = 0b0010, ///< 16 kHz + kHz8 = 0b0011, ///< 8 kHz (LN mode) + kHz4 = 0b0100, ///< 4 kHz (LN mode) + kHz2 = 0b0101, ///< 2 kHz (LN mode) + kHz1 = 0b0110, ///< 1 kHz (LN mode) (default) + Hz200 = 0b0111, ///< 200 Hz (LN or LP mode) + Hz100 = 0b1000, ///< 100 Hz (LN or LP mode) + Hz50 = 0b1001, ///< 50 Hz (LN or LP mode) + Hz25 = 0b1010, ///< 25 Hz (LN or LP mode) + Hz12_5 = 0b1011, ///< 12.5 Hz (LN or LP mode) + Hz6_25 = 0b1100, ///< 6.25 Hz (LP mode) + Hz3_125 = 0b1101, ///< 3.125 Hz (LP mode) + Hz1_5625 = 0b1110, ///< 1.5625 Hz (LP mode) + Hz500 = 0b1111, ///< 500 Hz (LN or LP mode) + }; + typedef modm::Configuration AccelOdr_t; + + // ------------------------------------------------------------------------------- + + /// GYRO_CONFIG1 reset value is 0x16 + enum class + GyroConfig1 : uint8_t + { + TEMP_FILTER_BW2 = Bit7, ///< See TempFiltBw_t + TEMP_FILTER_BW1 = Bit6, ///< See TempFiltBw_t + TEMP_FILTER_BW0 = Bit5, ///< See TempFiltBw_t + GYRO_UI_FILTER_ORD1 = Bit3, ///< See GyroUiFiltOrd_t + GYRO_UI_FILTER_ORD0 = Bit2, ///< See GyroUiFiltOrd_t + GYRO_DEC2_M2_ORD1 = Bit1, ///< See GyroDec2M2Ord_t + GYRO_DEC2_M2_ORD0 = Bit0, ///< See GyroDec2M2Ord_t + }; + MODM_FLAGS8(GyroConfig1); + + enum class + TempFiltBw : uint8_t + { + Hz4000 = 0b000, + Hz170 = 0b001, + Hz82 = 0b010, + Hz40 = 0b011, + Hz20 = 0b100, + Hz10 = 0b101, + Hz5 = 0b110, + }; + typedef modm::Configuration TempFiltBw_t; + + enum class + GyroUiFiltOrd : uint8_t + { + First = 0, + Second = int(GyroConfig1::GYRO_UI_FILTER_ORD0), + Third = int(GyroConfig1::GYRO_UI_FILTER_ORD1), + }; + typedef modm::Configuration GyroUiFiltOrd_t; + + enum class + GyroDec2M2Ord : uint8_t + { + Third = int(GyroConfig1::GYRO_DEC2_M2_ORD1), + }; + typedef modm::Configuration GyroDec2M2Ord_t; + + // ------------------------------------------------------------------------------- + + /// GYRO_ACCEL_CONFIG0 reset value is 0x11 + enum class + GyroAccelCongfig0 : uint8_t + { + ACCEL_UI_FILTER_BW3 = Bit7, ///< See AccelUiFilterBw_t + ACCEL_UI_FILTER_BW2 = Bit6, ///< See AccelUiFilterBw_t + ACCEL_UI_FILTER_BW1 = Bit5, ///< See AccelUiFilterBw_t + ACCEL_UI_FILTER_BW0 = Bit4, ///< See AccelUiFilterBw_t + GRYO_UI_FILTER_BW3 = Bit3, ///< See GyroUiFilterBw_t + GRYO_UI_FILTER_BW2 = Bit2, ///< See GyroUiFilterBw_t + GRYO_UI_FILTER_BW1 = Bit1, ///< See GyroUiFilterBw_t + GRYO_UI_FILTER_BW0 = Bit0, ///< See GyroUiFilterBw_t + }; + MODM_FLAGS8(GyroAccelCongfig0); + + enum class + UiFiltBw : uint8_t + { + /// Low Noise Mode: + Div2 = 0, ///< BW = ODR / 2 + Div4 = 1, ///< BW = max(400 Hz, ODR) / 2 (default) + Div5 = 2, ///< BW = max(400 Hz, ODR) / 5 + Div8 = 3, ///< BW = max(400 Hz, ODR) / 8 + Div10 = 4, ///< BW = max(400 Hz, ODR) / 10 + Div16 = 5, ///< BW = max(400 Hz, ODR) / 16 + Div20 = 6, ///< BW = max(400 Hz, ODR) / 20 + Div40 = 7, ///< BW = max(400 Hz, ODR) / 40 + LowLatency1 = 14, ///< Dec2 runs at max(400 Hz, ODR) + LowLatency2 = 15, ///< Dec2 runs at max(200 Hz, 8 * ODR) + + /// Low Power Mode: + Avg1 = 1, ///< 1x AVG filter (default) + Avg16 = 6, ///< 16x AVG filter + }; + typedef modm::Configuration AccelUiFiltBw_t; + typedef modm::Configuration GyroUiFiltBw_t; + + // ------------------------------------------------------------------------------- + + /// ACCEL_CONFIG1 reset value is 0x0D + enum class + AccelConfig1 : uint8_t + { + ACCEL_UI_FILTER_ORD1 = Bit4, ///< See AccelUiFiltOrd_t + ACCEL_UI_FILTER_ORD0 = Bit3, ///< See AccelUiFiltOrd_t + ACCEL_DEC2_M2_ORD1 = Bit2, ///< See AccelDec2M2Ord_t + ACCEL_DEC2_M2_ORD0 = Bit1, ///< See AccelDec2M2Ord_t + }; + MODM_FLAGS8(AccelConfig1); + + enum class + AccelUiFiltOrd : uint8_t + { + First = 0, + Second = int(AccelConfig1::ACCEL_UI_FILTER_ORD0), + Third = int(AccelConfig1::ACCEL_UI_FILTER_ORD1), + }; + typedef modm::Configuration AccelUiFiltOrd_t; + + enum class + AccelDec2M2Ord : uint8_t + { + Third = int(AccelConfig1::ACCEL_DEC2_M2_ORD1), + }; + typedef modm::Configuration AccelDec2M2Ord_t; + + // ------------------------------------------------------------------------------- + + /// TMST_CONFIG reset value is 0x23 + enum class + TmstConfig : uint8_t + { + TMST_TO_REGS_EN = Bit4, ///< Time stamp to register enable/disable + TMST_RES = Bit3, ///< Time stamp resolution 1 us/16 us or one RTC clock period + TMST_DELTA_EN = Bit2, ///< Time stamp delta enable/disable + TMST_FSYNC_EN = Bit1, ///< Time stamp register FSYNC enable/disable + TMST_EN = Bit0, ///< Time stamp register enable/disable + }; + MODM_FLAGS8(TmstConfig); + + // ------------------------------------------------------------------------------- + + /// APEX_CONFIG0 reset value is 0x82 + enum class + ApexConfig0 : uint8_t + { + DMP_POWER_SAVE = Bit7, ///< DMP power save mode enable/disable + TAP_ENABLE = Bit6, ///< Tap detection enable/disable + PED_ENABLE = Bit5, ///< Pedometer enable/disable + TILT_ENABLE = Bit4, ///< Tile detection enable/disable + FF_ENABLE = Bit2, ///< Freefall detection enable/disable + DMP_ODR1 = Bit1, ///< See DmpOdr_t + DMP_ODR0 = Bit0, ///< See DmpOdr_t + }; + MODM_FLAGS8(ApexConfig0); + + enum class + DmpOdr : uint8_t + { + Hz25 = 0, + Hz500 = int(ApexConfig0::DMP_ODR0), + Hz50 = int(ApexConfig0::DMP_ODR1), + Hz100 = int(ApexConfig0::DMP_ODR1) | int(ApexConfig0::DMP_ODR0), + }; + typedef modm::Configuration DmpOdr_t; + + // ------------------------------------------------------------------------------- + + /// SMD_CONFIG reset value is 0x00 + enum class + SmdConfig : uint8_t + { + WOM_INT_MODE = Bit3, ///< Set WoM interrupt on the OR/AND of all enabled accelerometer thresholds + WOM_MODE = Bit2, ///< Compare samples to initial/previous sample + SMD_MODE1 = Bit1, ///< See SmdMode_t + SMD_MODE0 = Bit0, ///< See SmdMode_t + }; + MODM_FLAGS8(SmdConfig); + + enum class + SmdMode : uint8_t + { + Disabled = 0, + Wom = int(SmdConfig::SMD_MODE0), + Short = int(SmdConfig::SMD_MODE1), + Long = int(SmdConfig::SMD_MODE1) | int(SmdConfig::SMD_MODE0), + }; + typedef modm::Configuration SmdMode_t; + + // ------------------------------------------------------------------------------- + + /// FIFO_CONFIG1 reset value is 0x00 + enum class + FifoConfig1 : uint8_t + { + FIFO_RESUME_PARTIAL_RD = Bit6, ///< Partial FIFO read enable/disable + FIFO_WM_GT_TH = Bit5, ///< FIFO watermark interrupt enable/disable + FIFO_HIRES_EN = Bit4, ///< FIFO extended resolution enable/disable + FIFO_TMST_FSYNC_EN = Bit3, ///< FIFO FSYNC enable/disable + FIFO_TEMP_EN = Bit2, ///< FIFO temperature data enable/disable + FIFO_GYRO_EN = Bit1, ///< FIFO gyro data enable/disable + FIFO_ACCEL_EN = Bit0, ///< FIFO acdel data enable/disable + }; + MODM_FLAGS8(FifoConfig1); + + // ------------------------------------------------------------------------------- + + /// FSYNC_CONFIG reset value is 0x10 + enum class + FsyncConfig : uint8_t + { + FSYNC_UI_SEL2 = Bit6, ///< See FsyncUiSel_t + FSYNC_UI_SEL1 = Bit5, ///< See FsyncUiSel_t + FSYNC_UI_SEL0 = Bit4, ///< See FsyncUiSel_t + FSYNC_UI_FLAG_CLEAR_SEL = Bit1, ///< FSYNC flag is cleared when UI sensor register is updated/read lsb + FSYNC_POLARITY = Bit0, ///< Start from Rising/Falling edge of FSYNC pulse to measure FSYNC interval + }; + MODM_FLAGS8(FsyncConfig); + + enum class + FsyncUiSel : uint8_t + { + NoTag = 0, + TempOut = 0b001, + GyroXout = 0b010, + GyroYout = 0b011, + GyroZout = 0b100, + AccelXout = 0b101, + AccelYout = 0b110, + AccelZout = 0b111, + }; + typedef modm::Configuration FsyncUiSel_t; + + // ------------------------------------------------------------------------------- + + /// INT_CONFIG0 reset value is 0x00 + enum class + IntConfig0 : uint8_t + { + UI_DRDY_INT_CLEAR1 = Bit5, ///< See UiDrdyIntClear_t + UI_DRDY_INT_CLEAR0 = Bit4, ///< See UiDrdyIntClear_t + FIFO_THS_INT_CLEAR1 = Bit3, ///< See FifoThsIntClear_t + FIFO_THS_INT_CLEAR0 = Bit2, ///< See FifoThsIntClear_t + FIFO_FULL_INT_CLEAR1 = Bit1, ///< See FifoFullIntClear_t + FIFO_FULL_INT_CLEAR0 = Bit0, ///< See FifoFullIntClear_t + }; + MODM_FLAGS8(IntConfig0); + + enum class + UiDrdyIntClear : uint8_t + { + StatusBitRead = 0, + SensorRegisterRead = int(IntConfig0::UI_DRDY_INT_CLEAR1), + Both = int(IntConfig0::UI_DRDY_INT_CLEAR1) | int(IntConfig0::UI_DRDY_INT_CLEAR0), + }; + typedef modm::Configuration UiDrdyIntClear_t; + + enum class + FifoThsIntClear : uint8_t + { + StatusBitRead = 0, + FifoDataRead = int(IntConfig0::FIFO_THS_INT_CLEAR1), + Both = int(IntConfig0::FIFO_THS_INT_CLEAR1) | int(IntConfig0::FIFO_THS_INT_CLEAR0), + }; + typedef modm::Configuration FifoThsIntClear_t; + + enum class + FifoFullIntClear : uint8_t + { + StatusBitRead = 0, + FifoDataRead = int(IntConfig0::FIFO_FULL_INT_CLEAR1), + Both = int(IntConfig0::FIFO_FULL_INT_CLEAR1) | int(IntConfig0::FIFO_FULL_INT_CLEAR0), + }; + typedef modm::Configuration FifoFullIntClear_t; + + // ------------------------------------------------------------------------------- + + /// INT_CONFIG1 reset value is 0x10 + enum class + IntConfig1 : uint8_t + { + INT_TPULSE_DURATION = Bit6, ///< Interrupt pulse duration is 100 us/8 us + INT_TDASSERT_DISABLE = Bit5, ///< De-assert duration enable/disable + INT_ASYNC_RESET = Bit4, ///< Change to 0 for proper INT1 and INT2 pin operation + }; + MODM_FLAGS8(IntConfig1); + + // ------------------------------------------------------------------------------- + + /// INT_SOURCE0 reset value is 0x10 + enum class + IntSource0 : uint8_t + { + UI_FSYNC_INT1_EN = Bit6, ///< UI FSYNC interrupt routed to INT1 enable/disable + PLL_RDY_INT1_EN = Bit5, ///< PLL ready interrupt routed to INT1 enable/disable + REST_DONE_INT1_EN = Bit4, ///< Reset done interrupt routed to INT1 enable/disable + UI_DRDY_INT1_EN = Bit3, ///< UI data ready interrupt routed to INT1 enable/disable + FIFO_THS_INT1_EN = Bit2, ///< FIFO threshold interrupt routed to INT1 enable/disable + FIFO_FULL_INT1_EN = Bit1, ///< FIFO full interrupt routed to INT1 enable/disable + UI_AGC_RDY_INT1_EN = Bit0, ///< UI AGC ready interrupt routed to INT1 enable/disable + }; + MODM_FLAGS8(IntSource0); + + // ------------------------------------------------------------------------------- + + /// INT_SOURCE1 reset value is 0x00 + enum class + IntSource1 : uint8_t + { + I3C_PROTOCOL_ERROR_INT1_EN = Bit6, ///< I3c protocol error interrupt routed to INT1 enable/disable + SMD_INT1_EN = Bit3, ///< SMD interrupt routed to INT1 enable/disable + WOM_Z_INT1_EN = Bit2, ///< Z-axis WOM interrupt routed to INT1 enable/disable + WOM_Y_INT1_EN = Bit1, ///< Y-axis WOM interrupt routed to INT1 enable/disable + WOM_X_INT1_EN = Bit0, ///< X-axis WOM interrupt routed to INT1 enable/disable + }; + MODM_FLAGS8(IntSource1); + + // ------------------------------------------------------------------------------- + + /// INT_SOURCE3 reset value is 0x00 + enum class + IntSource3 : uint8_t + { + UI_FSYNC_INT2_EN = Bit6, ///< UI FSYNC interrupt routed to INT2 enable/disable + PLL_RDY_INT2_EN = Bit5, ///< PLL ready interrupt routed to INT2 enable/disable + RESET_DONE_INT2_EN = Bit4, ///< Reset done interrupt routed to INT2 enable/disable + UI_DRDY_INT2_EN = Bit3, ///< UI data ready interrupt routed to INT2 enable/disable + FIFO_THS_INT2_EN = Bit2, ///< FIFO threshold interrupt routed to INT2 enable/disable + FIFO_FULL_INT2_EN = Bit1, ///< FIFO full interrupt routed to INT2 enable/disable + UI_AGC_RDY_INT2_EN = Bit0, ///< UI AGC ready interrupt routed to INT2 enable/disable + }; + MODM_FLAGS8(IntSource3); + + // ------------------------------------------------------------------------------- + + /// INT_SOURCE4 reset value is 0x00 + enum class + IntSource4 : uint8_t + { + I3C_PROTOCOL_ERROR_INT2_EN = Bit6, ///< I3c protocol error interrupt routed to INT2 enable/disable + SMD_INT2_EN = Bit3, ///< SMD interrupt routed to INT2 enable/disable + WOM_Z_INT2_EN = Bit2, ///< Z-axis WOM interrupt routed to INT2 enable/disable + WOM_Y_INT2_EN = Bit1, ///< Y-axis WOM interrupt routed to INT2 enable/disable + WOM_X_INT2_EN = Bit0, ///< X-axis WOM interrupt routed to INT2 enable/disable + }; + MODM_FLAGS8(IntSource4); + + // ------------------------------------------------------------------------------- + + /// SELF_TEST_CONFIG reset value is 0x00 + enum class + SelfTestConfig : uint8_t + { + ACCEL_ST_POWER = Bit6, ///< Accel self-test enable/disable + EN_AZ_ST = Bit5, ///< Enable Z-accel self-test + EN_AY_ST = Bit4, ///< Enable Y-accel self-test + EN_AX_ST = Bit3, ///< Enable X-accel self-test + EN_GZ_ST = Bit2, ///< Enable Z-gyro self-test + EN_GY_ST = Bit1, ///< Enable Y-gyro self-test + EN_GX_ST = Bit0, ///< Enable X-gyro self-test + }; + MODM_FLAGS8(SelfTestConfig); + +public: + + using Register_t = FlagsGroup< + RegBankSel_t, + DeviceConfig_t, + DriveConfig_t, + IntConfig_t, + FifoConfig_t, + SignalPathReset_t, + IntfConfig0_t, + IntfConfig1_t, + PwrMgmt0_t, + GyroConfig0_t, + AccelConfig0_t, + GyroConfig1_t, + GyroAccelCongfig0_t, + AccelConfig1_t, + TmstConfig_t, + ApexConfig0_t, + SmdConfig_t, + FifoConfig1_t, + FsyncConfig_t, + IntConfig0_t, + IntConfig1_t, + IntSource0_t, + IntSource1_t, + IntSource3_t, + IntSource4_t, + SelfTestConfig_t + >; + +protected: + /// @cond + static constexpr uint8_t + i(Register reg) { return uint8_t(reg); } + /// @endcond +}; + +} // namespace modm + +#endif // MODM_IXM42XXX_DEFINITIONS_HPP \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx_impl.hpp b/src/modm/driver/inertial/ixm42xxx_impl.hpp new file mode 100644 index 0000000000..5b1cccd4a8 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_impl.hpp @@ -0,0 +1,191 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_HPP +# error "Don't include this file directly, use 'ixm42xxx.hpp' instead!" +#endif + +#include "ixm42xxx.hpp" + +namespace modm +{ + +template < class Transport > +Ixm42xxx< Transport >::Ixm42xxx(Data &data, uint8_t address) : Transport(address), data(data) +{ +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::initialize() +{ + RF_BEGIN(); + + /// Synchronize the register bank selection + RF_CALL(readRegister(Register::REG_BANK_SEL, &prevBank)); + + /// Reset the device and wait 1 ms for the reset to be effective + RF_CALL(writeRegister(Register::DEVICE_CONFIG, uint8_t(DeviceConfig::SOFT_RESET_CONFIG))); + modm::delay_ms(1); + + /// Configure the device to use the endianess of the microcontroller + if (isBigEndian()) + { + RF_CALL(writeRegister(Register::INTF_CONFIG0, 0x30)); + } + else + { + RF_CALL(writeRegister(Register::INTF_CONFIG0, 0x0)); + } + + /// Configure the device to use default full scale and odr for gyro and accel + RF_CALL(updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps1000) | GyroOdr_t(GyroOdr::kHz1))); + RF_CALL(updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); + + RF_END(); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readTempData() +{ + return readRegister(Register::TEMP_DATA1, data.sensorData.temp, 2); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readAccelData() +{ + return readRegister(Register::ACCEL_DATA_X1, data.sensorData.accel, 6); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readGyroData() +{ + return readRegister(Register::GYRO_DATA_X1, data.sensorData.gyro, 6); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readSensorData() +{ + return readRegister(Register::TEMP_DATA1, reinterpret_cast(&data.sensorData), 14); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +modm::Ixm42xxx< Transport >::updateRegister(Register reg, Register_t setMask, Register_t clearMask) +{ + RF_BEGIN(); + + if (RF_CALL(setRegisterBank(reg)) && RF_CALL(this->read(i(reg), readByte))) + { + readByte = (readByte & ~clearMask.value) | setMask.value; + if (reg == Register::GYRO_CONFIG0) + { + GyroConfig0_t gyroConfig0 = GyroConfig0_t(readByte); + data.gyroScale = 2000.0f / float(1 << uint8_t(GyroFs_t::get(gyroConfig0))); + } + else if (reg == Register::ACCEL_CONFIG0) + { + AccelConfig0_t accelConfig0 = AccelConfig0_t(readByte); + data.accelScale = 16.0f / float(1 << uint8_t(AccelFs_t::get(accelConfig0))); + } + RF_RETURN_CALL( this->write(i(reg), readByte) ); + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +modm::Ixm42xxx< Transport >::writeRegister(Register reg, uint8_t value) +{ + RF_BEGIN(); + + if (RF_CALL(setRegisterBank(reg))) + { + RF_RETURN_CALL( this->write(i(reg), value) ); + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +modm::Ixm42xxx< Transport >::readRegister(Register reg, uint8_t *value) +{ + RF_BEGIN(); + + if (RF_CALL(setRegisterBank(reg))) + { + RF_RETURN_CALL( this->read(i(reg), value, 1) ); + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +modm::Ixm42xxx< Transport >::readRegister(Register reg, uint8_t *buffer, std::size_t length) +{ + RF_BEGIN(); + + if (RF_CALL(setRegisterBank(reg))) + { + RF_RETURN_CALL( this->read(i(reg), buffer, length) ); + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +modm::Ixm42xxx< Transport >::setRegisterBank(Register regi) +{ + const uint8_t bank = (uint16_t(regi) >> 8) & 0xFF; + + RF_BEGIN(); + + if (bank != prevBank) + { + RF_CALL( this->write(i(Register::REG_BANK_SEL), bank) ); + RF_CALL( this->read(i(Register::REG_BANK_SEL), prevBank) ); + RF_RETURN(bank == prevBank); + } + + RF_END_RETURN(true); +} + +} // namespace modm \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx_transport.hpp b/src/modm/driver/inertial/ixm42xxx_transport.hpp new file mode 100644 index 0000000000..caed0ef485 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_transport.hpp @@ -0,0 +1,105 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_TRANSPORT_HPP +#define MODM_IXM42XXX_TRANSPORT_HPP + +#include +#include +#include + +namespace modm +{ + +/** + * IXM42xxx I2C Transport Layer. + * + * @see Ixm42xxx + * + * @tparam I2cMaster I2C interface + * + * @ingroup modm_driver_ixm42xxx_transport + * @author Rasmus Kleist Hørlyck Sørensen + */ +template < class I2cMaster > +class Ixm42xxxTransportI2c : public modm::I2cDevice< I2cMaster, 4 > +{ +public: + Ixm42xxxTransportI2c(uint8_t address); + +protected: + // RAW REGISTER ACCESS + /// write a 8bit value + modm::ResumableResult + write(uint8_t reg, uint8_t value); + + /// read a 8bit value + modm::ResumableResult + read(uint8_t reg, uint8_t &value); + + /// read multiple 8bit values from a start register + modm::ResumableResult + read(uint8_t reg, uint8_t *buffer, std::size_t length); + +private: + uint8_t buffer[2]; +}; + +/** + * IXM42xxx SPI Transport Layer. + * + * @see Ixm42xxx + * + * @tparam SpiMaster SpiMaster interface + * @tparam Cs Chip-select pin + * + * @ingroup modm_driver_ixm42xxx_transport + * @author Rasmus Kleist Hørlyck Sørensen + */ +template < class SpiMaster, class Cs > +class Ixm42xxxTransportSpi : public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable< 4 > +{ +public: + Ixm42xxxTransportSpi(uint8_t /*address*/); + + /// pings the sensor + modm::ResumableResult + ping(); + +protected: + // RAW REGISTER ACCESS + /// write a 8bit value + modm::ResumableResult + write(uint8_t reg, uint8_t value); + + /// read a 8bit value + modm::ResumableResult + read(uint8_t reg, uint8_t &value); + + /// read multiple 8bit values from a start register + modm::ResumableResult + read(uint8_t reg, uint8_t *buffer, std::size_t length); + +private: + uint8_t whoAmI; + + // write read bit on the address + static constexpr uint8_t Read = Bit7; + static constexpr uint8_t Write = 0x00; +}; + +} // namespace modm + +#include "ixm42xxx_transport_impl.hpp" + +#endif // MODM_IXM42XXX_TRANSPORT_HPP diff --git a/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp b/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp new file mode 100644 index 0000000000..bc3853de5e --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp @@ -0,0 +1,135 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#ifndef MODM_IXM42XXX_TRANSPORT_HPP +# error "Don't include this file directly, use 'ixm42xxx_transport.hpp' instead!" +#endif + +// ---------------------------------------------------------------------------- + +template < class I2cMaster > +modm::Ixm42xxxTransportI2c::Ixm42xxxTransportI2c(uint8_t address) + : I2cDevice(address) +{ +} + +// ---------------------------------------------------------------------------- + +template < class I2cMaster > +modm::ResumableResult +modm::Ixm42xxxTransportI2c::write(uint8_t reg, uint8_t value) +{ + RF_BEGIN(); + + buffer[0] = reg; + buffer[1] = value; + + this->transaction.configureWrite(buffer, 2); + + RF_END_RETURN_CALL( this->runTransaction() ); +} + +// ---------------------------------------------------------------------------- + +template < class I2cMaster > +modm::ResumableResult +modm::Ixm42xxxTransportI2c::read(uint8_t reg, uint8_t &value) +{ + return read(reg, &value, 1); +} + +// ---------------------------------------------------------------------------- + +template < class I2cMaster > +modm::ResumableResult +modm::Ixm42xxxTransportI2c::read(uint8_t reg, uint8_t *buffer, std::size_t length) +{ + RF_BEGIN(); + + this->buffer[0] = reg; + this->transaction.configureWriteRead(this->buffer, 1, buffer, length); + + RF_END_RETURN_CALL( this->runTransaction() ); +} + +// ---------------------------------------------------------------------------- + +template < class SpiMaster, class Cs > +modm::Ixm42xxxTransportSpi::Ixm42xxxTransportSpi(uint8_t /*address*/) + : whoAmI(0) +{ + Cs::setOutput(modm::Gpio::High); +} + +// ---------------------------------------------------------------------------- + +template < class SpiMaster, class Cs > +modm::ResumableResult +modm::Ixm42xxxTransportSpi::ping() +{ + RF_BEGIN(); + + whoAmI = 0; + RF_CALL(read(0x75, whoAmI)); + + RF_END_RETURN(whoAmI != 0); +} + +// ---------------------------------------------------------------------------- + +template < class SpiMaster, class Cs > +modm::ResumableResult +modm::Ixm42xxxTransportSpi::write(uint8_t reg, uint8_t value) +{ + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + Cs::reset(); + + RF_CALL(SpiMaster::transfer(reg | Write)); + RF_CALL(SpiMaster::transfer(value)); + + if (this->releaseMaster()) + Cs::set(); + + RF_END_RETURN(true); +} + +// ---------------------------------------------------------------------------- + +template < class SpiMaster, class Cs > +modm::ResumableResult +modm::Ixm42xxxTransportSpi::read(uint8_t reg, uint8_t &value) +{ + return read(reg, &value, 1); +} + +// ---------------------------------------------------------------------------- + +template < class SpiMaster, class Cs > +modm::ResumableResult +modm::Ixm42xxxTransportSpi::read(uint8_t reg, uint8_t *buffer, std::size_t length) +{ + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + Cs::reset(); + + RF_CALL(SpiMaster::transfer(reg | Read)); + RF_CALL(SpiMaster::transfer(nullptr, buffer, length)); + + if (this->releaseMaster()) + Cs::set(); + + RF_END_RETURN(true); +} From 79f66ff026511b45114a1d0800acd4670181e80a Mon Sep 17 00:00:00 2001 From: rasmuskleist Date: Mon, 17 Jul 2023 18:56:23 +0200 Subject: [PATCH 3/5] [example] Adding InvenSense 6-Axis IMU example --- examples/nucleo_g474re/ixm42xxx/main.cpp | 100 ++++++++++++++++++++ examples/nucleo_g474re/ixm42xxx/project.xml | 15 +++ 2 files changed, 115 insertions(+) create mode 100644 examples/nucleo_g474re/ixm42xxx/main.cpp create mode 100644 examples/nucleo_g474re/ixm42xxx/project.xml diff --git a/examples/nucleo_g474re/ixm42xxx/main.cpp b/examples/nucleo_g474re/ixm42xxx/main.cpp new file mode 100644 index 0000000000..823410c239 --- /dev/null +++ b/examples/nucleo_g474re/ixm42xxx/main.cpp @@ -0,0 +1,100 @@ +// coding: utf-8 +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include +#include +#include +#include +#include + +using SpiMaster = modm::platform::SpiMaster1; +using Mosi = GpioA7; +using Miso = GpioA6; +using Sck = GpioA5; +using Cs = GpioC5; + +class ImuThread : public modm::pt::Protothread, public modm::ixm42xxx +{ + using Transport = modm::Ixm42xxxTransportSpi< SpiMaster, Cs >; + +public: + ImuThread() : imu(data), timer(std::chrono::milliseconds(500)) {} + + bool + run() + { + PT_BEGIN(); + + /// Initialize the IMU and verify that it is connected + PT_CALL(imu.initialize()); + while (not PT_CALL(imu.ping())) + { + MODM_LOG_ERROR << "Cannot ping IXM42xxx" << modm::endl; + PT_WAIT_UNTIL(timer.execute()); + } + + /// Configure data sensors + PT_CALL(imu.updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps2000) | GyroOdr_t(GyroOdr::kHz1))); + PT_CALL(imu.updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); + PT_CALL(imu.updateRegister(Register::PWR_MGMT0, GyroMode_t(GyroMode::LowNoise) | AccelMode_t(AccelMode::LowNoise))); + + while (true) + { + PT_WAIT_UNTIL(timer.execute()); + PT_CALL(imu.readSensorData()); + + data.getTemp(&temp); + data.getAccel(&accel); + data.getGyro(&gyro); + + MODM_LOG_INFO.printf("Temp: %.3f\n", temp); + MODM_LOG_INFO.printf("Accel: (%.3f, %.3f, %.3f)\n", accel.x, accel.y, accel.z); + MODM_LOG_INFO.printf("Gyro: (%.3f, %.3f, %.3f)\n", gyro.x, gyro.y, gyro.z); + } + PT_END(); + } + +private: + float temp; + modm::Vector3f accel; + modm::Vector3f gyro; + + modm::ixm42xxxdata::Data data; + modm::Ixm42xxx< Transport > imu; + + modm::PeriodicTimer timer; + +} imuThread; + +int +main() +{ + Board::initialize(); + + Cs::setOutput(modm::Gpio::High); + SpiMaster::connect(); + SpiMaster::initialize(); + + MODM_LOG_INFO << "==========IXM-42xxx Test==========" << modm::endl; + MODM_LOG_DEBUG << "Debug logging here" << modm::endl; + MODM_LOG_INFO << "Info logging here" << modm::endl; + MODM_LOG_WARNING << "Warning logging here" << modm::endl; + MODM_LOG_ERROR << "Error logging here" << modm::endl; + MODM_LOG_INFO << "==================================" << modm::endl; + + while (true) + { + imuThread.run(); + } + + return 0; +} diff --git a/examples/nucleo_g474re/ixm42xxx/project.xml b/examples/nucleo_g474re/ixm42xxx/project.xml new file mode 100644 index 0000000000..5262c37935 --- /dev/null +++ b/examples/nucleo_g474re/ixm42xxx/project.xml @@ -0,0 +1,15 @@ + + modm:nucleo-g474re + + + + + modm:driver:ixm42xxx + modm:math:geometry + modm:platform:gpio + modm:platform:spi:1 + modm:processing:protothread + modm:processing:timer + modm:build:scons + + \ No newline at end of file From 11a782955af8420410460ea31e45827ba4de7bdc Mon Sep 17 00:00:00 2001 From: rasmuskleist Date: Mon, 17 Jul 2023 18:57:09 +0200 Subject: [PATCH 4/5] [driver] Adding InvenSense FIFO support --- src/modm/driver/inertial/ixm42xxx.hpp | 33 ++++ src/modm/driver/inertial/ixm42xxx.lb | 1 + src/modm/driver/inertial/ixm42xxx_data.hpp | 162 +++++++++++++++++- .../driver/inertial/ixm42xxx_data_impl.hpp | 136 +++++++++++++++ src/modm/driver/inertial/ixm42xxx_impl.hpp | 71 ++++++++ 5 files changed, 402 insertions(+), 1 deletion(-) create mode 100644 src/modm/driver/inertial/ixm42xxx_data_impl.hpp diff --git a/src/modm/driver/inertial/ixm42xxx.hpp b/src/modm/driver/inertial/ixm42xxx.hpp index daed1c8155..fdbd6c25ed 100644 --- a/src/modm/driver/inertial/ixm42xxx.hpp +++ b/src/modm/driver/inertial/ixm42xxx.hpp @@ -83,6 +83,39 @@ class Ixm42xxx : public ixm42xxx, public Transport modm::ResumableResult readSensorData(); + /** + * @brief Read the FSYNC timestamp from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readFsyncTimestamp(uint16_t *timestamp); + + /** + * @brief Read the FIFO count from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readFifoCount(uint16_t *count); + + /** + * @brief Read the FIFO data from the device + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + readFifoData(); + + /** + * @brief Set the FIFO watermark used to generate FIFO_WM_GT interrupt + * @warning The FIFO watermarkl should be set, before choosing this interrupt source. + * @return False in case of any error, e.g. if some register access is not + * permitted. + */ + modm::ResumableResult + writeFifoWatermark(uint16_t watermark); + public: /** * @brief update a single register. diff --git a/src/modm/driver/inertial/ixm42xxx.lb b/src/modm/driver/inertial/ixm42xxx.lb index a919a2cb17..4246abc773 100644 --- a/src/modm/driver/inertial/ixm42xxx.lb +++ b/src/modm/driver/inertial/ixm42xxx.lb @@ -42,6 +42,7 @@ def build(env): env.copy("ixm42xxx.hpp") env.copy("ixm42xxx_impl.hpp") env.copy("ixm42xxx_data.hpp") + env.copy("ixm42xxx_data_impl.hpp") env.copy("ixm42xxx_definitions.hpp") env.copy("ixm42xxx_transport.hpp") env.copy("ixm42xxx_transport_impl.hpp") diff --git a/src/modm/driver/inertial/ixm42xxx_data.hpp b/src/modm/driver/inertial/ixm42xxx_data.hpp index 08c83b491a..3bcd3003f6 100644 --- a/src/modm/driver/inertial/ixm42xxx_data.hpp +++ b/src/modm/driver/inertial/ixm42xxx_data.hpp @@ -14,6 +14,7 @@ #ifndef MODM_IXM42XXX_DATA_HPP #define MODM_IXM42XXX_DATA_HPP +#include #include @@ -33,7 +34,9 @@ Data template < class Transport > friend class ::modm::Ixm42xxx; - Data() : + Data(std::span fifoBuffer = std::span()) : + fifoCount(0), + fifoBuffer(fifoBuffer), accelScale(16.f), gyroScale(2000.f) { @@ -86,6 +89,18 @@ Data getGyroScale() const { return gyroScale; } + constexpr uint16_t + getFifoCount() const + { return fifoCount; } + + constexpr uint16_t + getFifoBufferSize() const + { return fifoBuffer.size(); } + + constexpr std::span + getFifoData() const + { return fifoBuffer.subspan(0, fifoCount); } + private: struct SensorData { @@ -94,12 +109,157 @@ Data int16_t gyro[3]; } sensorData; + uint16_t fifoCount; + std::span fifoBuffer; + float accelScale; float gyroScale; }; +/// @ingroup modm_driver_ixm42xxx +struct +FifoPacket +{ + FifoPacket() : header(0), accel(0), gyro(0), temp(0), timestamp(0), extension(0) {} + + // DATA ACCESS + ///@{ + int16_t + getTemp() const + { return temp; } + + void + getTemp(int16_t *temp) const + { *temp = getTemp(); } + + void + getTemp(float *temp) const + { float t = getTemp(); *temp = (header & HEADER_20) ? t / 132.48f + 25.f : t / 2.07f + 25.f; } + + Vector3li + getAccel() const; + + void + getAccel(Vector3li *accel) const + { Vector3li a = getAccel(); *accel = a; } + + void + getAccel(Vector3f *accel, float scale = 16.f) const + { Vector3f a = getAccel(); *accel = (header & HEADER_20) ? a / 8192.f : a * scale / INT16_MAX; } + + Vector3li + getGyro() const; + + void + getGyro(Vector3li *gyro) const + { Vector3li g = getGyro(); *gyro = g; } + + void + getGyro(Vector3f *gyro, float scale = 2000.f) const + { Vector3f g = getGyro(); *gyro = (header & HEADER_20) ? g / 131.f : g * scale / INT16_MAX; } + + uint16_t + getTimestamp() const + { return timestamp; } + ///@} + + constexpr bool + containsSensorData() const + { return (header & HEADER_MSG) == 0; } + + constexpr bool + containsAccelData() const + { return header & HEADER_ACCEL; } + + constexpr bool + containsGyroData() const + { return header & HEADER_GYRO; } + + constexpr bool + containsOdrTimestamp() const + { return header & HEADER_TIMESTAMP_ODR; } + + constexpr bool + containsFsyncTimestamp() const + { return header & HEADER_TIMESTAMP_FSYNC; } + + constexpr bool + isExtended() const + { return header & HEADER_20; } + +public: + + static uint16_t + parse(std::span fifoData, FifoPacket &fifoPacket, uint16_t fifoIndex = 0); + +private: + + int header; + int16_t accel[3]; + int16_t gyro[3]; + int16_t temp; + uint16_t timestamp; + int8_t extension[3]; + + enum + { + HEADER_MSG = Bit7, + HEADER_ACCEL = Bit6, + HEADER_GYRO = Bit5, + HEADER_20 = Bit4, + HEADER_TIMESTAMP_ODR = Bit3, + HEADER_TIMESTAMP_FSYNC = Bit2, + HEADER_ODR_ACCEL = Bit1, + HEADER_ODR_GYRO = Bit0, + }; + +public: + constexpr bool operator==(const FifoPacket& rhs) const; +}; + +/// @ingroup modm_driver_ixm42xxx +template +struct +FifoData : public Data +{ + FifoData() : Data(std::span{fifoBuffer}) {} + + struct iterator + { + public: + typedef std::input_iterator_tag iterator_category; + typedef FifoPacket value_type; + typedef FifoPacket& reference; + typedef FifoPacket* pointer; + + constexpr iterator(std::span data, uint16_t index) : fifoData(data), fifoIndex(index) + { + fifoIndex = FifoPacket::parse(fifoData, fifoPacket, fifoIndex); + } + + constexpr iterator operator++() { fifoIndex = FifoPacket::parse(fifoData, fifoPacket, fifoIndex); return *this; } + constexpr reference operator*() { return fifoPacket; } + constexpr pointer operator->() { return &fifoPacket; } + constexpr bool operator==(const iterator& rhs) { return fifoIndex == rhs.fifoIndex && fifoPacket == rhs.fifoPacket; } + + private: + std::span fifoData; + uint16_t fifoIndex; + + FifoPacket fifoPacket; + }; + + constexpr iterator begin() const { return iterator(getFifoData(), 0); } + constexpr iterator end() const { return iterator(getFifoData(), getFifoCount()); } + +private: + uint8_t fifoBuffer[FifoBufferSize]; +}; + } // ixm42xxxdata namespace } // namespace modm +#include "ixm42xxx_data_impl.hpp" + #endif // MODM_IXM42XXX_DATA_HPP \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx_data_impl.hpp b/src/modm/driver/inertial/ixm42xxx_data_impl.hpp new file mode 100644 index 0000000000..38c18b65c7 --- /dev/null +++ b/src/modm/driver/inertial/ixm42xxx_data_impl.hpp @@ -0,0 +1,136 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include + +#ifndef MODM_IXM42XXX_DATA_HPP +# error "Don't include this file directly, use 'ixm42xxx_data.hpp' instead!" +#endif + +namespace modm +{ + +namespace ixm42xxxdata +{ + +Vector3li +FifoPacket::getAccel() const +{ + Vector3li a; + if (header & HEADER_20) + { + a.x = accel[0] << 2 | ((extension[0] & 0xF0) >> 6); + a.y = accel[1] << 2 | ((extension[1] & 0xF0) >> 6); + a.z = accel[2] << 2 | ((extension[2] & 0xF0) >> 6); + } + else + { + a.x = accel[0]; + a.y = accel[1]; + a.z = accel[2]; + } + return a; +} + +Vector3li +FifoPacket::getGyro() const +{ + Vector3li g; + if (header & HEADER_20) + { + g.x = ((gyro[0] << 3) | (extension[0] & 0x0F)); + g.y = ((gyro[1] << 3) | (extension[1] & 0x0F)); + g.z = ((gyro[2] << 3) | (extension[2] & 0x0F)); + } + else + { + g.x = gyro[0]; + g.y = gyro[1]; + g.z = gyro[2]; + } + return g; +} + +uint16_t +FifoPacket::parse(std::span fifoData, FifoPacket &fifoPacket, uint16_t fifoIndex) +{ + if (fifoIndex < fifoData.size() && (fifoData[fifoIndex] & HEADER_MSG) == 0) + { + // Packet contains sensor data + fifoPacket.header = fifoData[fifoIndex++]; + if (fifoPacket.header & HEADER_ACCEL) + { + // Packet is sized so that accel data have location in the packet + std::memcpy(fifoPacket.accel, fifoData.data() + fifoIndex, 6); + fifoIndex += 6; + } + + if (fifoPacket.header & HEADER_GYRO) + { + // Packet is sized so that gyro data have location in the packet + std::memcpy(fifoPacket.gyro, fifoData.data() + fifoIndex, 6); + fifoIndex += 6; + } + + if ((fifoPacket.header & HEADER_ACCEL) || (fifoPacket.header & HEADER_GYRO)) + { + if (fifoPacket.header & HEADER_20) + { + std::memcpy(&fifoPacket.temp, fifoData.data() + fifoIndex, 2); + fifoIndex += 2; + } + else + { + std::memcpy(&fifoPacket.temp, fifoData.data() + fifoIndex, 1); + fifoIndex += 1; + } + } + + if ((fifoPacket.header & HEADER_TIMESTAMP_ODR) || (fifoPacket.header & HEADER_TIMESTAMP_FSYNC)) + { + // Packet contains ODR or FSYNC Timestamp + std::memcpy(&fifoPacket.timestamp, fifoData.data() + fifoIndex, 2); + fifoIndex += 2; + } + + if (fifoPacket.header & HEADER_20) + { + // Packet has a new and valid sample of extended 20-bit data for gyro and/or accel + std::memcpy(fifoPacket.extension, fifoData.data() + fifoIndex, 3); + fifoIndex += 3; + } + } + else + { + // FIFO is empty + fifoPacket = FifoPacket(); + fifoIndex = fifoData.size(); + } + + return fifoIndex; +} + +constexpr bool +FifoPacket::operator==(const FifoPacket& rhs) const +{ + return ((this->header == rhs.header) && + (this->temp == rhs.temp) && + (this->timestamp == rhs.timestamp) && + (std::equal(accel, accel + 3, rhs.accel)) && + (std::equal(gyro, gyro + 3, rhs.gyro)) && + (std::equal(extension, extension + 3, rhs.extension))); +} + +} // ixm42xxxdata namespace + +} // modm namespace \ No newline at end of file diff --git a/src/modm/driver/inertial/ixm42xxx_impl.hpp b/src/modm/driver/inertial/ixm42xxx_impl.hpp index 5b1cccd4a8..574841d362 100644 --- a/src/modm/driver/inertial/ixm42xxx_impl.hpp +++ b/src/modm/driver/inertial/ixm42xxx_impl.hpp @@ -95,6 +95,77 @@ Ixm42xxx< Transport >::readSensorData() // ----------------------------------------------------------------------------- +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readFsyncTimestamp(uint16_t *timestamp) +{ + /// TODO: Verify endianess + return readRegister(Register::TMST_FSYNCH, reinterpret_cast(timestamp), 2); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readFifoCount(uint16_t *count) +{ + RF_BEGIN(); + + /// TODO: Figure out why the endianess seem to be wrong (possibly like FIFO_CONFIG2-3) + /// Read FIFO_COUNTL to latch new data for both FIFO_COUNTH and FIFO_COUNTL + if (RF_CALL(readRegister(Register::FIFO_COUNTL, &readByte))) + { + *count = uint16_t(readByte) << 8; + if (RF_CALL(readRegister(Register::FIFO_COUNTH, &readByte))) + { + *count |= uint16_t(readByte); + RF_RETURN(true); + } + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::readFifoData() +{ + RF_BEGIN(); + + if (RF_CALL(readFifoCount(&data.fifoCount))) + { + data.fifoCount = std::min(data.fifoCount, data.fifoBuffer.size()); + if (data.fifoCount > 0) + { + RF_RETURN_CALL(readRegister(Register::FIFO_DATA, data.fifoBuffer.data(), data.fifoCount)); + } + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + +template < class Transport > +modm::ResumableResult +Ixm42xxx< Transport >::writeFifoWatermark(uint16_t watermark) +{ + RF_BEGIN(); + + /// TODO: Determine what endianess to use + if (RF_CALL(writeRegister(Register::FIFO_CONFIG2, watermark & 0xFF)) && + RF_CALL(writeRegister(Register::FIFO_CONFIG3, (watermark >> 8) & 0xFF))) + { + RF_RETURN(true); + } + + RF_END_RETURN(false); +} + +// ----------------------------------------------------------------------------- + template < class Transport > modm::ResumableResult modm::Ixm42xxx< Transport >::updateRegister(Register reg, Register_t setMask, Register_t clearMask) From 8012d824bcef380cddc401a62f0c1a5f0596297a Mon Sep 17 00:00:00 2001 From: rasmuskleist Date: Mon, 17 Jul 2023 18:57:27 +0200 Subject: [PATCH 5/5] [example] Adding InvenSense FIFO example --- examples/nucleo_g474re/ixm42xxx_fifo/main.cpp | 148 ++++++++++++++++++ .../nucleo_g474re/ixm42xxx_fifo/project.xml | 16 ++ 2 files changed, 164 insertions(+) create mode 100644 examples/nucleo_g474re/ixm42xxx_fifo/main.cpp create mode 100644 examples/nucleo_g474re/ixm42xxx_fifo/project.xml diff --git a/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp b/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp new file mode 100644 index 0000000000..2f9b46bb8d --- /dev/null +++ b/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp @@ -0,0 +1,148 @@ +// coding: utf-8 +/* + * Copyright (c) 2023, Rasmus Kleist Hørlyck Sørensen + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#include +#include +#include +#include + +namespace +{ + volatile bool interrupt = false; +} + +using SpiMaster = modm::platform::SpiMaster1; +using Mosi = GpioA7; +using Miso = GpioA6; +using Sck = GpioA5; +using Cs = GpioC5; + +using Int1 = GpioC3; + +class ImuThread : public modm::pt::Protothread, public modm::ixm42xxx +{ + using Transport = modm::Ixm42xxxTransportSpi< SpiMaster, Cs >; + +public: + ImuThread() : imu(fifoData), timer(std::chrono::milliseconds(500)) {} + + bool + run() + { + PT_BEGIN(); + + Int1::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) + { + interrupt = true; + }); + + /// Initialize the IMU and verify that it is connected + PT_CALL(imu.initialize()); + while (not PT_CALL(imu.ping())) + { + MODM_LOG_ERROR << "Cannot ping IXM-42xxx" << modm::endl; + PT_WAIT_UNTIL(timer.execute()); + } + + MODM_LOG_INFO << "IXM-42xxx Initialized" << modm::endl; + MODM_LOG_INFO << "Fifo Buffer Size: " << fifoData.getFifoBufferSize() << modm::endl; + + /// Configure FIFO + PT_CALL(imu.updateRegister(Register::FIFO_CONFIG, FifoMode_t(FifoMode::StopOnFull))); + PT_CALL(imu.updateRegister(Register::FIFO_CONFIG1, FifoConfig1::FIFO_HIRES_EN | FifoConfig1::FIFO_TEMP_EN | FifoConfig1::FIFO_GYRO_EN | FifoConfig1::FIFO_ACCEL_EN)); + PT_CALL(imu.writeFifoWatermark(1024)); + + /// Configure interrupt + PT_CALL(imu.updateRegister(Register::INT_CONFIG, IntConfig::INT1_MODE | IntConfig::INT1_DRIVE_CIRCUIT | IntConfig::INT1_POLARITY)); + PT_CALL(imu.updateRegister(Register::INT_CONFIG1, IntConfig1::INT_ASYNC_RESET)); + PT_CALL(imu.updateRegister(Register::INT_SOURCE0, IntSource0::FIFO_THS_INT1_EN | IntSource0::FIFO_FULL_INT1_EN, IntSource0::UI_DRDY_INT1_EN)); + + /// Configure data sensors + PT_CALL(imu.updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps2000) | GyroOdr_t(GyroOdr::kHz1))); + PT_CALL(imu.updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); + PT_CALL(imu.updateRegister(Register::PWR_MGMT0, GyroMode_t(GyroMode::LowNoise) | AccelMode_t(AccelMode::LowNoise))); + + while (true) + { + if (interrupt) + { + PT_CALL(imu.readRegister(Register::INT_STATUS, &intStatus.value)); + interrupt = false; + } + + if (intStatus.any(IntStatus::FIFO_FULL_INT | IntStatus::FIFO_THS_INT)) + { + if (PT_CALL(imu.readFifoData())) + { + // Count packets in FIFO buffer and print contents of last packet + uint16_t count = 0; + modm::ixm42xxxdata::FifoPacket fifoPacket; + for (const auto &packet : fifoData) + { + fifoPacket = packet; + count++; + } + + float temp; + modm::Vector3f accel; + modm::Vector3f gyro; + + fifoPacket.getTemp(&temp); + fifoPacket.getAccel(&accel, fifoData.getAccelScale()); + fifoPacket.getGyro(&gyro, fifoData.getGyroScale()); + + MODM_LOG_INFO.printf("Temp: %f\n", temp); + MODM_LOG_INFO.printf("Accel: (%f, %f, %f)\n", accel.x, accel.y, accel.z); + MODM_LOG_INFO.printf("Gyro: (%f, %f, %f)\n", gyro.x, gyro.y, gyro.z); + MODM_LOG_INFO.printf("FIFO packet count: %u\n", count); + } + intStatus.reset(IntStatus::FIFO_FULL_INT | IntStatus::FIFO_THS_INT); + } + } + PT_END(); + } + +private: + + /// Due to the non-deterministic nature of system operation, driver memory allocation should always be the largest size of 2080 bytes. + modm::ixm42xxxdata::FifoData<2080> fifoData; + modm::ixm42xxx::IntStatus_t intStatus; + modm::Ixm42xxx< Transport > imu; + + modm::PeriodicTimer timer; + +} imuThread; + +int +main() +{ + Board::initialize(); + + Cs::setOutput(modm::Gpio::High); + SpiMaster::connect(); + SpiMaster::initialize(); + + MODM_LOG_INFO << "==========IXM-42xxx Test==========" << modm::endl; + MODM_LOG_DEBUG << "Debug logging here" << modm::endl; + MODM_LOG_INFO << "Info logging here" << modm::endl; + MODM_LOG_WARNING << "Warning logging here" << modm::endl; + MODM_LOG_ERROR << "Error logging here" << modm::endl; + MODM_LOG_INFO << "==================================" << modm::endl; + + while (true) + { + imuThread.run(); + } + + return 0; +} diff --git a/examples/nucleo_g474re/ixm42xxx_fifo/project.xml b/examples/nucleo_g474re/ixm42xxx_fifo/project.xml new file mode 100644 index 0000000000..fcf707cd29 --- /dev/null +++ b/examples/nucleo_g474re/ixm42xxx_fifo/project.xml @@ -0,0 +1,16 @@ + + modm:nucleo-g474re + + + + + modm:driver:ixm42xxx + modm:math:geometry + modm:platform:exti + modm:platform:gpio + modm:platform:spi:1 + modm:processing:protothread + modm:processing:timer + modm:build:scons + + \ No newline at end of file