From e1de156b06ff40ed3e40bccc82350e8e86bdfa8a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 1 Dec 2024 07:15:32 +0300 Subject: [PATCH 01/20] Create common interface for different IMU drivers --- IMU.h | 57 +++++++++++++++++++++++++++++++++++++++++++++++++++++ MPU9250.cpp | 7 ++++++- MPU9250.h | 36 ++++++++++++--------------------- 3 files changed, 76 insertions(+), 24 deletions(-) create mode 100644 IMU.h diff --git a/IMU.h b/IMU.h new file mode 100644 index 0000000..f228616 --- /dev/null +++ b/IMU.h @@ -0,0 +1,57 @@ +#pragma once + +#include "Arduino.h" +#include "Wire.h" +#include "SPI.h" + +// IMU driver interface + +class IMUInterface { +public: + enum IMUType { + UNKNOWN, + MPU9250, + MPU9252, + MPU6500, + ICM20948, + }; + enum DlpfBandwidth : int8_t { + DLPF_BANDWIDTH_184HZ = 0x01, + DLPF_BANDWIDTH_92HZ = 0x02, + DLPF_BANDWIDTH_41HZ = 0x03, + DLPF_BANDWIDTH_20HZ = 0x04, + DLPF_BANDWIDTH_10HZ = 0x05, + DLPF_BANDWIDTH_5HZ = 0x06 + }; + enum AccelRange : int8_t { + ACCEL_RANGE_2G = 0x00, + ACCEL_RANGE_4G = 0x08, + ACCEL_RANGE_8G = 0x10, + ACCEL_RANGE_16G = 0x18 + }; + enum GyroRange : int8_t { + GYRO_RANGE_250DPS = 0x00, + GYRO_RANGE_500DPS = 0x08, + GYRO_RANGE_1000DPS = 0x10, + GYRO_RANGE_2000DPS = 0x18 + }; + enum Rate { + RATE_MIN, + RATE_50HZ_APPROX, + RATE_1KHZ_APPROX, + RATE_8KHZ_APPROX, + RATE_MAX + }; + + virtual bool begin() = 0; + virtual void reset() = 0; + virtual uint8_t whoAmI() const = 0; + virtual bool read() = 0; + virtual void waitForData() = 0; + virtual void getAccel(float& x, float& y, float& z) const = 0; + virtual void getGyro(float& x, float& y, float& z) const = 0; + virtual void getMag(float& x, float& y, float& z) const = 0; + virtual float getTemp() const = 0; + virtual bool setRate(Rate rate) = 0; + virtual const char* getModel() const = 0; +}; diff --git a/MPU9250.cpp b/MPU9250.cpp index 1abb464..defc763 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -471,7 +471,12 @@ void MPU9250::getMag(float& x, float& y, float& z) const { y = mag_[1]; z = mag_[2]; } -const char* MPU9250::getType() const { +bool MPU9250::setRate(Rate rate) { + // TODO: + log("Rate setting is not implemented"); + return false; +} +const char* MPU9250::getModel() const { switch (who_am_i_) { case WHOAMI_MPU6500_: return "MPU-6500"; case WHOAMI_MPU9250_: return "MPU-9250"; diff --git a/MPU9250.h b/MPU9250.h index fa550fb..4237eda 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -35,10 +35,11 @@ #include #include "core/core.h" #endif +#include "IMU.h" #include "invensense_imu.h" // NOLINT #include "logger.h" -class MPU9250 : public Logger { +class MPU9250 : public IMUInterface, public Logger { public: /* Sensor and filter settings */ enum I2cAddr : uint8_t { @@ -53,18 +54,6 @@ class MPU9250 : public Logger { DLPF_BANDWIDTH_10HZ = 0x05, DLPF_BANDWIDTH_5HZ = 0x06 }; - enum AccelRange : int8_t { - ACCEL_RANGE_2G = 0x00, - ACCEL_RANGE_4G = 0x08, - ACCEL_RANGE_8G = 0x10, - ACCEL_RANGE_16G = 0x18 - }; - enum GyroRange : int8_t { - GYRO_RANGE_250DPS = 0x00, - GYRO_RANGE_500DPS = 0x08, - GYRO_RANGE_1000DPS = 0x10, - GYRO_RANGE_2000DPS = 0x18 - }; enum WomRate : int8_t { WOM_RATE_0_24HZ = 0x00, WOM_RATE_0_49HZ = 0x01, @@ -88,7 +77,7 @@ class MPU9250 : public Logger { imu_(&spi, cs) {} void config(TwoWire *i2c, const I2cAddr addr); void config(SPIClass *spi, const uint8_t cs); - bool begin(); + bool begin() override; bool enableDrdyInt(); bool disableDrdyInt(); bool setAccelRange(const AccelRange range); @@ -100,13 +89,14 @@ class MPU9250 : public Logger { bool setDlpfBandwidth(const DlpfBandwidth dlpf); inline DlpfBandwidth dlpf_bandwidth() const {return dlpf_bandwidth_;} bool enableWom(int16_t threshold_mg, const WomRate wom_rate); - void reset(); - bool read(); - void waitForData(); - void getAccel(float& x, float& y, float& z) const; - void getGyro(float& x, float& y, float& z) const; - void getMag(float& x, float& y, float& z) const; - const char* getType() const; + void reset() override; + bool read() override; + void waitForData() override; + void getAccel(float& x, float& y, float& z) const override; + void getGyro(float& x, float& y, float& z) const override; + void getMag(float& x, float& y, float& z) const override; + bool setRate(Rate rate) override; + const char* getModel() const override; inline bool new_imu_data() const {return new_imu_data_;} inline float accel_x_mps2() const {return accel_[0];} inline float accel_y_mps2() const {return accel_[1];} @@ -118,8 +108,8 @@ class MPU9250 : public Logger { inline float mag_x_ut() const {return mag_[0];} inline float mag_y_ut() const {return mag_[1];} inline float mag_z_ut() const {return mag_[2];} - inline float die_temp_c() const {return temp_;} - inline uint8_t whoAmI() const {return who_am_i_;} + inline float getTemp() const override {return temp_;} + inline uint8_t whoAmI() const override {return who_am_i_;} private: InvensenseImu imu_; From 6c433ed64219542f3db322f367ec3c56485aeeae Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 04:51:57 +0300 Subject: [PATCH 02/20] Import ICM20948_WE sources https://github.com/wollewald/ICM20948_WE --- ICM20948.cpp | 1031 +++++++++++++++++++++++++++++++++++++++++++ ICM20948.h | 377 ++++++++++++++++ LICENSE_ICM20948_WE | 21 + xyzFloat.cpp | 96 ++++ xyzFloat.h | 32 ++ 5 files changed, 1557 insertions(+) create mode 100644 ICM20948.cpp create mode 100644 ICM20948.h create mode 100644 LICENSE_ICM20948_WE create mode 100644 xyzFloat.cpp create mode 100644 xyzFloat.h diff --git a/ICM20948.cpp b/ICM20948.cpp new file mode 100644 index 0000000..008d1f0 --- /dev/null +++ b/ICM20948.cpp @@ -0,0 +1,1031 @@ +/* + * Code based on: https://github.com/wollewald/ICM20948_WE. + * Author: Wolfgang (Wolle) Ewald. + * License: MIT (see LICENSE_ICM20948_WE file). +*/ + +/******************************************************************** +* This is a library for the 9-axis gyroscope, accelerometer and magnetometer ICM20948. +* +* You'll find an example which should enable you to use the library. +* +* You are free to use it, change it or build on it. In case you like +* it, it would be cool if you give it a star. +* +* If you find bugs, please inform me! +* +* Written by Wolfgang (Wolle) Ewald +* +* Further information can be found on: +* +* https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) +* https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) +* +*********************************************************************/ + +#include "ICM20948_WE.h" + +/************ Basic Settings ************/ + +bool ICM20948_WE::init(){ + if(useSPI){ + pinMode(csPin, OUTPUT); + digitalWrite(csPin, HIGH); + _spi->begin(); + mySPISettings = SPISettings(7000000, MSBFIRST, SPI_MODE0); + } + currentBank = 0; + + reset_ICM20948(); + if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ + delay(2000); + if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ + return false; + } + } + + accOffsetVal.x = 0.0; + accOffsetVal.y = 0.0; + accOffsetVal.z = 0.0; + accCorrFactor.x = 1.0; + accCorrFactor.y = 1.0; + accCorrFactor.z = 1.0; + accRangeFactor = 1.0; + gyrOffsetVal.x = 0.0; + gyrOffsetVal.y = 0.0; + gyrOffsetVal.z = 0.0; + gyrRangeFactor = 1.0; + fifoType = ICM20948_FIFO_ACC; + + sleep(false); + writeRegister8(2, ICM20948_ODR_ALIGN_EN, 1); // aligns ODR + + return true; +} + +void ICM20948_WE::autoOffsets(){ + xyzFloat accRawVal, gyrRawVal; + accOffsetVal.x = 0.0; + accOffsetVal.y = 0.0; + accOffsetVal.z = 0.0; + + setGyrDLPF(ICM20948_DLPF_6); // lowest noise + setGyrRange(ICM20948_GYRO_RANGE_250); // highest resolution + setAccRange(ICM20948_ACC_RANGE_2G); + setAccDLPF(ICM20948_DLPF_6); + delay(100); + + for(int i=0; i<10; i++){ // Allow to get stable values + readSensor(); + delay(10); + } + + for(int i=0; i<50; i++){ + readSensor(); + accRawVal = getAccRawValues(); + accOffsetVal.x += accRawVal.x; + accOffsetVal.y += accRawVal.y; + accOffsetVal.z += accRawVal.z; + delay(10); + } + + accOffsetVal.x /= 50; + accOffsetVal.y /= 50; + accOffsetVal.z /= 50; + accOffsetVal.z -= 16384.0; + + for(int i=0; i<50; i++){ + readSensor(); + gyrRawVal = getGyrRawValues(); + gyrOffsetVal.x += gyrRawVal.x; + gyrOffsetVal.y += gyrRawVal.y; + gyrOffsetVal.z += gyrRawVal.z; + delay(1); + } + + gyrOffsetVal.x /= 50; + gyrOffsetVal.y /= 50; + gyrOffsetVal.z /= 50; + +} + +void ICM20948_WE::setAccOffsets(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){ + accOffsetVal.x = (xMax + xMin) * 0.5; + accOffsetVal.y = (yMax + yMin) * 0.5; + accOffsetVal.z = (zMax + zMin) * 0.5; + accCorrFactor.x = (xMax + abs(xMin)) / 32768.0; + accCorrFactor.y = (yMax + abs(yMin)) / 32768.0; + accCorrFactor.z = (zMax + abs(zMin)) / 32768.0 ; +} + +void ICM20948_WE::setAccOffsets(xyzFloat offset){ + accOffsetVal = offset; +} + +xyzFloat ICM20948_WE::getAccOffsets(){ + return accOffsetVal; +} + +void ICM20948_WE::setGyrOffsets(float xOffset, float yOffset, float zOffset){ + gyrOffsetVal.x = xOffset; + gyrOffsetVal.y = yOffset; + gyrOffsetVal.z = zOffset; +} + +void ICM20948_WE::setGyrOffsets(xyzFloat offset){ + gyrOffsetVal = offset; +} + +xyzFloat ICM20948_WE::getGyrOffsets(){ + return gyrOffsetVal; +} + +uint8_t ICM20948_WE::whoAmI(){ + return readRegister8(0, ICM20948_WHO_AM_I); +} + +void ICM20948_WE::enableAcc(bool enAcc){ + regVal = readRegister8(0, ICM20948_PWR_MGMT_2); + if(enAcc){ + regVal &= ~ICM20948_ACC_EN; + } + else{ + regVal |= ICM20948_ACC_EN; + } + writeRegister8(0, ICM20948_PWR_MGMT_2, regVal); +} + +void ICM20948_WE::setAccRange(ICM20948_accRange accRange){ + regVal = readRegister8(2, ICM20948_ACCEL_CONFIG); + regVal &= ~(0x06); + regVal |= (accRange<<1); + writeRegister8(2, ICM20948_ACCEL_CONFIG, regVal); + accRangeFactor = 1<(((buffer[0]) << 8) | buffer[1]) * 1.0; + accRawVal.y = static_cast(((buffer[2]) << 8) | buffer[3]) * 1.0; + accRawVal.z = static_cast(((buffer[4]) << 8) | buffer[5]) * 1.0; + return accRawVal; +} + +xyzFloat ICM20948_WE::getCorrectedAccRawValues(){ + xyzFloat accRawVal = getAccRawValues(); + accRawVal = correctAccRawValues(accRawVal); + + return accRawVal; +} + +xyzFloat ICM20948_WE::getGValues(){ + xyzFloat gVal, accRawVal; + accRawVal = getCorrectedAccRawValues(); + + gVal.x = accRawVal.x * accRangeFactor / 16384.0; + gVal.y = accRawVal.y * accRangeFactor / 16384.0; + gVal.z = accRawVal.z * accRangeFactor / 16384.0; + return gVal; +} + +xyzFloat ICM20948_WE::getAccRawValuesFromFifo(){ + xyzFloat accRawVal = readICM20948xyzValFromFifo(); + return accRawVal; +} + +xyzFloat ICM20948_WE::getCorrectedAccRawValuesFromFifo(){ + xyzFloat accRawVal = getAccRawValuesFromFifo(); + accRawVal = correctAccRawValues(accRawVal); + + return accRawVal; +} + +xyzFloat ICM20948_WE::getGValuesFromFifo(){ + xyzFloat gVal, accRawVal; + accRawVal = getCorrectedAccRawValuesFromFifo(); + + gVal.x = accRawVal.x * accRangeFactor / 16384.0; + gVal.y = accRawVal.y * accRangeFactor / 16384.0; + gVal.z = accRawVal.z * accRangeFactor / 16384.0; + return gVal; +} + +float ICM20948_WE::getResultantG(xyzFloat gVal){ + float resultant = 0.0; + resultant = sqrt(sq(gVal.x) + sq(gVal.y) + sq(gVal.z)); + + return resultant; +} + +float ICM20948_WE::getTemperature(){ + int16_t rawTemp = static_cast(((buffer[12]) << 8) | buffer[13]); + float tmp = (rawTemp*1.0 - ICM20948_ROOM_TEMP_OFFSET)/ICM20948_T_SENSITIVITY + 21.0; + return tmp; +} + +xyzFloat ICM20948_WE::getGyrRawValues(){ + xyzFloat gyrRawVal; + + gyrRawVal.x = (int16_t)(((buffer[6]) << 8) | buffer[7]) * 1.0; + gyrRawVal.y = (int16_t)(((buffer[8]) << 8) | buffer[9]) * 1.0; + gyrRawVal.z = (int16_t)(((buffer[10]) << 8) | buffer[11]) * 1.0; + + return gyrRawVal; +} + +xyzFloat ICM20948_WE::getCorrectedGyrRawValues(){ + xyzFloat gyrRawVal = getGyrRawValues(); + gyrRawVal = correctGyrRawValues(gyrRawVal); + return gyrRawVal; +} + +xyzFloat ICM20948_WE::getGyrValues(){ + xyzFloat gyrVal = getCorrectedGyrRawValues(); + + gyrVal.x = gyrVal.x * gyrRangeFactor * 250.0 / 32768.0; + gyrVal.y = gyrVal.y * gyrRangeFactor * 250.0 / 32768.0; + gyrVal.z = gyrVal.z * gyrRangeFactor * 250.0 / 32768.0; + + return gyrVal; +} + +xyzFloat ICM20948_WE::getGyrValuesFromFifo(){ + xyzFloat gyrVal; + xyzFloat gyrRawVal = readICM20948xyzValFromFifo(); + + gyrRawVal = correctGyrRawValues(gyrRawVal); + gyrVal.x = gyrRawVal.x * gyrRangeFactor * 250.0 / 32768.0; + gyrVal.y = gyrRawVal.y * gyrRangeFactor * 250.0 / 32768.0; + gyrVal.z = gyrRawVal.z * gyrRangeFactor * 250.0 / 32768.0; + + return gyrVal; +} + +xyzFloat ICM20948_WE::getMagValues(){ + int16_t x,y,z; + xyzFloat mag; + + x = static_cast((buffer[15]) << 8) | buffer[14]; + y = static_cast((buffer[17]) << 8) | buffer[16]; + z = static_cast((buffer[19]) << 8) | buffer[18]; + + mag.x = x * AK09916_MAG_LSB; + mag.y = y * AK09916_MAG_LSB; + mag.z = z * AK09916_MAG_LSB; + + return mag; +} + + +/********* Power, Sleep, Standby *********/ + +void ICM20948_WE::enableCycle(ICM20948_cycle cycle){ + regVal = readRegister8(0, ICM20948_LP_CONFIG); + regVal &= 0x0F; + regVal |= cycle; + + writeRegister8(0, ICM20948_LP_CONFIG, regVal); +} + +void ICM20948_WE::enableLowPower(bool enLP){ // vielleicht besser privat???? + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + if(enLP){ + regVal |= ICM20948_LP_EN; + } + else{ + regVal &= ~ICM20948_LP_EN; + } + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); +} + +void ICM20948_WE::setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg){ + writeRegister8(2, ICM20948_GYRO_CONFIG_2, avg); +} + +void ICM20948_WE::setAccAverageInCycleMode(ICM20948_accAvgLowPower avg){ + writeRegister8(2, ICM20948_ACCEL_CONFIG_2, avg); +} + +void ICM20948_WE::sleep(bool sleep){ + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + if(sleep){ + regVal |= ICM20948_SLEEP; + } + else{ + regVal &= ~ICM20948_SLEEP; + } + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); +} + +/******** Angles and Orientation *********/ + +xyzFloat ICM20948_WE::getAngles(){ + xyzFloat angleVal; + xyzFloat gVal = getGValues(); + if(gVal.x > 1.0){ + gVal.x = 1.0; + } + else if(gVal.x < -1.0){ + gVal.x = -1.0; + } + angleVal.x = (asin(gVal.x)) * 57.296; + + if(gVal.y > 1.0){ + gVal.y = 1.0; + } + else if(gVal.y < -1.0){ + gVal.y = -1.0; + } + angleVal.y = (asin(gVal.y)) * 57.296; + + if(gVal.z > 1.0){ + gVal.z = 1.0; + } + else if(gVal.z < -1.0){ + gVal.z = -1.0; + } + angleVal.z = (asin(gVal.z)) * 57.296; + + return angleVal; +} + +ICM20948_orientation ICM20948_WE::getOrientation(){ + xyzFloat angleVal = getAngles(); + ICM20948_orientation orientation = ICM20948_FLAT; + if(abs(angleVal.x) < 45){ // |x| < 45 + if(abs(angleVal.y) < 45){ // |y| < 45 + if(angleVal.z > 0){ // z > 0 + orientation = ICM20948_FLAT; + } + else{ // z < 0 + orientation = ICM20948_FLAT_1; + } + } + else{ // |y| > 45 + if(angleVal.y > 0){ // y > 0 + orientation = ICM20948_XY; + } + else{ // y < 0 + orientation = ICM20948_XY_1; + } + } + } + else{ // |x| >= 45 + if(angleVal.x > 0){ // x > 0 + orientation = ICM20948_YX; + } + else{ // x < 0 + orientation = ICM20948_YX_1; + } + } + return orientation; +} + +String ICM20948_WE::getOrientationAsString(){ + ICM20948_orientation orientation = getOrientation(); + String orientationAsString = ""; + switch(orientation){ + case ICM20948_FLAT: orientationAsString = "z up"; break; + case ICM20948_FLAT_1: orientationAsString = "z down"; break; + case ICM20948_XY: orientationAsString = "y up"; break; + case ICM20948_XY_1: orientationAsString = "y down"; break; + case ICM20948_YX: orientationAsString = "x up"; break; + case ICM20948_YX_1: orientationAsString = "x down"; break; + } + return orientationAsString; +} + +float ICM20948_WE::getPitch(){ + xyzFloat angleVal = getAngles(); + float pitch = (atan2(-angleVal.x, sqrt(abs((angleVal.y*angleVal.y + angleVal.z*angleVal.z))))*180.0)/M_PI; + return pitch; +} + +float ICM20948_WE::getRoll(){ + xyzFloat angleVal = getAngles(); + float roll = (atan2(angleVal.y, angleVal.z)*180.0)/M_PI; + return roll; +} + + +/************** Interrupts ***************/ + +void ICM20948_WE::setIntPinPolarity(ICM20948_intPinPol pol){ + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if(pol){ + regVal |= ICM20948_INT1_ACTL; + } + else{ + regVal &= ~ICM20948_INT1_ACTL; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948_WE::enableIntLatch(bool latch){ + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if(latch){ + regVal |= ICM20948_INT_1_LATCH_EN; + } + else{ + regVal &= ~ICM20948_INT_1_LATCH_EN; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948_WE::enableClearIntByAnyRead(bool clearByAnyRead){ + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if(clearByAnyRead){ + regVal |= ICM20948_INT_ANYRD_2CLEAR; + } + else{ + regVal &= ~ICM20948_INT_ANYRD_2CLEAR; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948_WE::setFSyncIntPolarity(ICM20948_intPinPol pol){ + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if(pol){ + regVal |= ICM20948_ACTL_FSYNC; + } + else{ + regVal &= ~ICM20948_ACTL_FSYNC; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948_WE::enableInterrupt(ICM20948_intType intType){ + switch(intType){ + case ICM20948_FSYNC_INT: + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + regVal |= ICM20948_FSYNC_INT_MODE_EN; + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); // enable FSYNC as interrupt pin + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x80; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); // enable wake on FSYNC interrupt + break; + + case ICM20948_WOM_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x08; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + regVal |= 0x02; + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + break; + + case ICM20948_DMP_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x02; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_DATA_READY_INT: + writeRegister8(0, ICM20948_INT_ENABLE_1, 0x01); + break; + + case ICM20948_FIFO_OVF_INT: + writeRegister8(0, ICM20948_INT_ENABLE_2, 0x01); + break; + + case ICM20948_FIFO_WM_INT: + writeRegister8(0, ICM20948_INT_ENABLE_3, 0x01); + break; + } +} + +void ICM20948_WE::disableInterrupt(ICM20948_intType intType){ + switch(intType){ + case ICM20948_FSYNC_INT: + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + regVal &= ~ICM20948_FSYNC_INT_MODE_EN; + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x80); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_WOM_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x08); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + regVal &= ~(0x02); + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + break; + + case ICM20948_DMP_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x02); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_DATA_READY_INT: + writeRegister8(0, ICM20948_INT_ENABLE_1, 0x00); + break; + + case ICM20948_FIFO_OVF_INT: + writeRegister8(0, ICM20948_INT_ENABLE_2, 0x00); + break; + + case ICM20948_FIFO_WM_INT: + writeRegister8(0, ICM20948_INT_ENABLE_3, 0x00); + break; + } +} + +uint8_t ICM20948_WE::readAndClearInterrupts(){ + uint8_t intSource = 0; + regVal = readRegister8(0, ICM20948_I2C_MST_STATUS); + if(regVal & 0x80){ + intSource |= 0x01; + } + regVal = readRegister8(0, ICM20948_INT_STATUS); + if(regVal & 0x08){ + intSource |= 0x02; + } + if(regVal & 0x02){ + intSource |= 0x04; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_1); + if(regVal & 0x01){ + intSource |= 0x08; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_2); + if(regVal & 0x01){ + intSource |= 0x10; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_3); + if(regVal & 0x01){ + intSource |= 0x20; + } + return intSource; +} + +bool ICM20948_WE::checkInterrupt(uint8_t source, ICM20948_intType type){ + source &= type; + return source; +} +void ICM20948_WE::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn){ + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + if(womCompEn){ + regVal |= 0x01; + } + else{ + regVal &= ~(0x01); + } + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + writeRegister8(2, ICM20948_ACCEL_WOM_THR, womThresh); +} + +/***************** FIFO ******************/ + +void ICM20948_WE::enableFifo(bool fifo){ + regVal = readRegister8(0, ICM20948_USER_CTRL); + if(fifo){ + regVal |= ICM20948_FIFO_EN; + } + else{ + regVal &= ~ICM20948_FIFO_EN; + } + writeRegister8(0, ICM20948_USER_CTRL, regVal); +} + +void ICM20948_WE::setFifoMode(ICM20948_fifoMode mode){ + if(mode){ + regVal = 0x01; + } + else{ + regVal = 0x00; + } + writeRegister8(0, ICM20948_FIFO_MODE, regVal); +} + +void ICM20948_WE::startFifo(ICM20948_fifoType fifo){ + fifoType = fifo; + writeRegister8(0, ICM20948_FIFO_EN_2, fifoType); +} + +void ICM20948_WE::stopFifo(){ + writeRegister8(0, ICM20948_FIFO_EN_2, 0); +} + +void ICM20948_WE::resetFifo(){ + writeRegister8(0, ICM20948_FIFO_RST, 0x01); + writeRegister8(0, ICM20948_FIFO_RST, 0x00); +} + +int16_t ICM20948_WE::getFifoCount(){ + int16_t regVal16 = static_cast(readRegister16(0, ICM20948_FIFO_COUNT)); + return regVal16; +} + +int16_t ICM20948_WE::getNumberOfFifoDataSets(){ + int16_t numberOfSets = getFifoCount(); + + if((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)){ + numberOfSets /= 6; + } + else if(fifoType==ICM20948_FIFO_ACC_GYR){ + numberOfSets /= 12; + } + + return numberOfSets; +} + +void ICM20948_WE::findFifoBegin(){ + uint16_t count = getFifoCount(); + int16_t start = 0; + + if((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)){ + start = count%6; + for(int i=0; i(readAK09916Register16(AK09916_WIA_1)); +} + +void ICM20948_WE::setMagOpMode(AK09916_opMode opMode){ + writeAK09916Register8(AK09916_CNTL_2, opMode); + delay(10); + if(opMode!=AK09916_PWR_DOWN){ + enableMagDataRead(AK09916_HXL, 0x08); + } +} + +void ICM20948_WE::resetMag(){ + writeAK09916Register8(AK09916_CNTL_3, 0x01); + delay(100); +} + +/************************************************ + Private Functions +*************************************************/ + +void ICM20948_WE::setClockToAutoSelect(){ + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + regVal |= 0x01; + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); + delay(10); +} + +xyzFloat ICM20948_WE::correctAccRawValues(xyzFloat accRawVal){ + accRawVal.x = (accRawVal.x -(accOffsetVal.x / accRangeFactor)) / accCorrFactor.x; + accRawVal.y = (accRawVal.y -(accOffsetVal.y / accRangeFactor)) / accCorrFactor.y; + accRawVal.z = (accRawVal.z -(accOffsetVal.z / accRangeFactor)) / accCorrFactor.z; + + return accRawVal; +} + +xyzFloat ICM20948_WE::correctGyrRawValues(xyzFloat gyrRawVal){ + gyrRawVal.x -= (gyrOffsetVal.x / gyrRangeFactor); + gyrRawVal.y -= (gyrOffsetVal.y / gyrRangeFactor); + gyrRawVal.z -= (gyrOffsetVal.z / gyrRangeFactor); + + return gyrRawVal; +} + +void ICM20948_WE::switchBank(uint8_t newBank){ + if(newBank != currentBank){ + currentBank = newBank; + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(ICM20948_REG_BANK_SEL); + _wire->write(currentBank<<4); + _wire->endTransmission(); + } + else{ + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(ICM20948_REG_BANK_SEL); + _spi->transfer(currentBank<<4); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + } +} + +void ICM20948_WE::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val){ + switchBank(bank); + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->write(val); + _wire->endTransmission(); + } + else{ + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + _spi->transfer(val); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +void ICM20948_WE::writeRegister16(uint8_t bank, uint8_t reg, int16_t val){ + switchBank(bank); + int8_t MSByte = static_cast((val>>8) & 0xFF); + uint8_t LSByte = val & 0xFF; + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->write(MSByte); + _wire->write(LSByte); + _wire->endTransmission(); + } + else{ + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + _spi->transfer(val); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +uint8_t ICM20948_WE::readRegister8(uint8_t bank, uint8_t reg){ + switchBank(bank); + uint8_t regValue = 0; + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(1)); + if(_wire->available()){ + regValue = _wire->read(); + } + } + else{ + reg |= 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + regValue = _spi->transfer(0x00); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + return regValue; +} + +int16_t ICM20948_WE::readRegister16(uint8_t bank, uint8_t reg){ + switchBank(bank); + uint8_t MSByte = 0, LSByte = 0; + int16_t reg16Val = 0; + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(2)); + if(_wire->available()){ + MSByte = _wire->read(); + LSByte = _wire->read(); + } + } + else{ + reg = reg | 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + MSByte = _spi->transfer(0x00); + LSByte = _spi->transfer(0x00); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + reg16Val = (MSByte<<8) + LSByte; + return reg16Val; +} + +void ICM20948_WE::readAllData(uint8_t* data){ + switchBank(0); + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(ICM20948_ACCEL_OUT); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(20)); + if(_wire->available()){ + for(int i=0; i<20; i++){ + data[i] = _wire->read(); + } + } + } + else{ + uint8_t reg = ICM20948_ACCEL_OUT | 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + for(int i=0; i<20; i++){ + data[i] = _spi->transfer(0x00); + } + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +xyzFloat ICM20948_WE::readICM20948xyzValFromFifo(){ + uint8_t fifoTriple[6]; + xyzFloat xyzResult = {0.0, 0.0, 0.0}; + switchBank(0); + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(ICM20948_FIFO_R_W); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(6)); + if(_wire->available()){ + for(int i=0; i<6; i++){ + fifoTriple[i] = _wire->read(); + } + } + } + else{ + uint8_t reg = ICM20948_FIFO_R_W | 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + for(int i=0; i<6; i++){ + fifoTriple[i] = _spi->transfer(0x00); + } + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + + xyzResult.x = (static_cast((fifoTriple[0]<<8) + fifoTriple[1])) * 1.0; + xyzResult.y = (static_cast((fifoTriple[2]<<8) + fifoTriple[3])) * 1.0; + xyzResult.z = (static_cast((fifoTriple[4]<<8) + fifoTriple[5])) * 1.0; + + return xyzResult; +} + +void ICM20948_WE::writeAK09916Register8(uint8_t reg, uint8_t val){ + writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS); // write AK09916 + writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be written to + writeRegister8(3, ICM20948_I2C_SLV0_DO, val); +} + + +uint8_t ICM20948_WE::readAK09916Register8(uint8_t reg){ + enableMagDataRead(reg, 0x01); + enableMagDataRead(AK09916_HXL, 0x08); + regVal = readRegister8(0, ICM20948_EXT_SLV_SENS_DATA_00); + return regVal; +} + +int16_t ICM20948_WE::readAK09916Register16(uint8_t reg){ + int16_t regValue = 0; + enableMagDataRead(reg, 0x02); + regValue = readRegister16(0, ICM20948_EXT_SLV_SENS_DATA_00); + enableMagDataRead(AK09916_HXL, 0x08); + return regValue; +} + +void ICM20948_WE::reset_ICM20948(){ + writeRegister8(0, ICM20948_PWR_MGMT_1, ICM20948_RESET); + delay(10); // wait for registers to reset +} + +void ICM20948_WE::enableI2CMaster(){ + writeRegister8(0, ICM20948_USER_CTRL, ICM20948_I2C_MST_EN); //enable I2C master + writeRegister8(3, ICM20948_I2C_MST_CTRL, 0x07); // set I2C clock to 345.60 kHz + delay(10); +} + +void ICM20948_WE::i2cMasterReset(){ + uint8_t regVal = readRegister8(0, ICM20948_USER_CTRL); + regVal |= ICM20948_I2C_MST_RST; + writeRegister8(0, ICM20948_USER_CTRL, regVal); + delay(10); +} + +void ICM20948_WE::enableMagDataRead(uint8_t reg, uint8_t bytes){ + writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS | AK09916_READ); // read AK09916 + writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be read + writeRegister8(3, ICM20948_I2C_SLV0_CTRL, 0x80 | bytes); //enable read | number of byte + delay(10); +} + \ No newline at end of file diff --git a/ICM20948.h b/ICM20948.h new file mode 100644 index 0000000..875a818 --- /dev/null +++ b/ICM20948.h @@ -0,0 +1,377 @@ +/* + * Code based on: https://github.com/wollewald/ICM20948_WE. + * Author: Wolfgang (Wolle) Ewald. + * License: MIT (see LICENSE_ICM20948_WE file). +*/ + +/****************************************************************************** + * + * This is a library for the 9-axis gyroscope, accelerometer and magnetometer ICM20948. + * + * You'll find several example sketches which should enable you to use the library. + * + * You are free to use it, change it or build on it. In case you like it, it would + * be cool if you give it a star. + * + * If you find bugs, please inform me! + * + * Written by Wolfgang (Wolle) Ewald + * + * Further information can be found on: + * + * https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) + * https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) + * + * + ******************************************************************************/ + +#ifndef ICM20948_WE_H_ +#define ICM20948_WE_H_ + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include +#include +#include "xyzFloat.h" + +/* Enums */ + +typedef enum ICM20948_CYCLE { + ICM20948_NO_CYCLE = 0x00, + ICM20948_GYR_CYCLE = 0x10, + ICM20948_ACC_CYCLE = 0x20, + ICM20948_ACC_GYR_CYCLE = 0x30, + ICM20948_ACC_GYR_I2C_MST_CYCLE = 0x70 +} ICM20948_cycle; + +typedef enum ICM20948_INT_PIN_POL { + ICM20948_ACT_HIGH, ICM20948_ACT_LOW +} ICM20948_intPinPol; + +typedef enum ICM20948_INT_TYPE { + ICM20948_FSYNC_INT = 0x01, + ICM20948_WOM_INT = 0x02, + ICM20948_DMP_INT = 0x04, + ICM20948_DATA_READY_INT = 0x08, + ICM20948_FIFO_OVF_INT = 0x10, + ICM20948_FIFO_WM_INT = 0x20 +} ICM20948_intType; + +typedef enum ICM20948_FIFO_TYPE { + ICM20948_FIFO_ACC = 0x10, + ICM20948_FIFO_GYR = 0x0E, + ICM20948_FIFO_ACC_GYR = 0x1E +} ICM20948_fifoType; + +typedef enum ICM20948_FIFO_MODE_CHOICE { + ICM20948_CONTINUOUS, ICM20948_STOP_WHEN_FULL +} ICM20948_fifoMode; + +typedef enum ICM20948_GYRO_RANGE { + ICM20948_GYRO_RANGE_250, ICM20948_GYRO_RANGE_500, ICM20948_GYRO_RANGE_1000, ICM20948_GYRO_RANGE_2000 +} ICM20948_gyroRange; + +typedef enum ICM20948_DLPF { + ICM20948_DLPF_0, ICM20948_DLPF_1, ICM20948_DLPF_2, ICM20948_DLPF_3, ICM20948_DLPF_4, ICM20948_DLPF_5, + ICM20948_DLPF_6, ICM20948_DLPF_7, ICM20948_DLPF_OFF +} ICM20948_dlpf; + +typedef enum ICM20948_GYRO_AVG_LOW_PWR { + ICM20948_GYR_AVG_1, ICM20948_GYR_AVG_2, ICM20948_GYR_AVG_4, ICM20948_GYR_AVG_8, ICM20948_GYR_AVG_16, + ICM20948_GYR_AVG_32, ICM20948_GYR_AVG_64, ICM20948_GYR_AVG_128 +} ICM20948_gyroAvgLowPower; + +typedef enum ICM20948_ACC_RANGE { + ICM20948_ACC_RANGE_2G, ICM20948_ACC_RANGE_4G, ICM20948_ACC_RANGE_8G, ICM20948_ACC_RANGE_16G +} ICM20948_accRange; + +typedef enum ICM20948_ACC_AVG_LOW_PWR { + ICM20948_ACC_AVG_4, ICM20948_ACC_AVG_8, ICM20948_ACC_AVG_16, ICM20948_ACC_AVG_32 +} ICM20948_accAvgLowPower; + +typedef enum ICM20948_WOM_COMP { + ICM20948_WOM_COMP_DISABLE, ICM20948_WOM_COMP_ENABLE +} ICM20948_womCompEn; + +typedef enum AK09916_OP_MODE { + AK09916_PWR_DOWN = 0x00, + AK09916_TRIGGER_MODE = 0x01, + AK09916_CONT_MODE_10HZ = 0x02, + AK09916_CONT_MODE_20HZ = 0x04, + AK09916_CONT_MODE_50HZ = 0x06, + AK09916_CONT_MODE_100HZ = 0x08 +} AK09916_opMode; + +typedef enum ICM20948_ORIENTATION { + ICM20948_FLAT, ICM20948_FLAT_1, ICM20948_XY, ICM20948_XY_1, ICM20948_YX, ICM20948_YX_1 +} ICM20948_orientation; + +class ICM20948_WE +{ + public: + /* constants */ + + static constexpr uint8_t AK09916_ADDRESS {0x0C}; + + /* Registers ICM20948 USER BANK 0*/ + static constexpr uint8_t ICM20948_WHO_AM_I {0x00}; + static constexpr uint8_t ICM20948_USER_CTRL {0x03}; + static constexpr uint8_t ICM20948_LP_CONFIG {0x05}; + static constexpr uint8_t ICM20948_PWR_MGMT_1 {0x06}; + static constexpr uint8_t ICM20948_PWR_MGMT_2 {0x07}; + static constexpr uint8_t ICM20948_INT_PIN_CFG {0x0F}; + static constexpr uint8_t ICM20948_INT_ENABLE {0x10}; + static constexpr uint8_t ICM20948_INT_ENABLE_1 {0x11}; + static constexpr uint8_t ICM20948_INT_ENABLE_2 {0x12}; + static constexpr uint8_t ICM20948_INT_ENABLE_3 {0x13}; + static constexpr uint8_t ICM20948_I2C_MST_STATUS {0x17}; + static constexpr uint8_t ICM20948_INT_STATUS {0x19}; + static constexpr uint8_t ICM20948_INT_STATUS_1 {0x1A}; + static constexpr uint8_t ICM20948_INT_STATUS_2 {0x1B}; + static constexpr uint8_t ICM20948_INT_STATUS_3 {0x1C}; + static constexpr uint8_t ICM20948_DELAY_TIME_H {0x28}; + static constexpr uint8_t ICM20948_DELAY_TIME_L {0x29}; + static constexpr uint8_t ICM20948_ACCEL_OUT {0x2D}; // accel data registers begin + static constexpr uint8_t ICM20948_GYRO_OUT {0x33}; // gyro data registers begin + static constexpr uint8_t ICM20948_TEMP_OUT {0x39}; + static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_00{0x3B}; + static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_01{0x3C}; + static constexpr uint8_t ICM20948_FIFO_EN_1 {0x66}; + static constexpr uint8_t ICM20948_FIFO_EN_2 {0x67}; + static constexpr uint8_t ICM20948_FIFO_RST {0x68}; + static constexpr uint8_t ICM20948_FIFO_MODE {0x69}; + static constexpr uint8_t ICM20948_FIFO_COUNT {0x70}; + static constexpr uint8_t ICM20948_FIFO_R_W {0x72}; + static constexpr uint8_t ICM20948_DATA_RDY_STATUS {0x74}; + static constexpr uint8_t ICM20948_FIFO_CFG {0x76}; + + /* Registers ICM20948 USER BANK 1*/ + static constexpr uint8_t ICM20948_SELF_TEST_X_GYRO {0x02}; + static constexpr uint8_t ICM20948_SELF_TEST_Y_GYRO {0x03}; + static constexpr uint8_t ICM20948_SELF_TEST_Z_GYRO {0x04}; + static constexpr uint8_t ICM20948_SELF_TEST_X_ACCEL {0x0E}; + static constexpr uint8_t ICM20948_SELF_TEST_Y_ACCEL {0x0F}; + static constexpr uint8_t ICM20948_SELF_TEST_Z_ACCEL {0x10}; + static constexpr uint8_t ICM20948_XA_OFFS_H {0x14}; + static constexpr uint8_t ICM20948_XA_OFFS_L {0x15}; + static constexpr uint8_t ICM20948_YA_OFFS_H {0x17}; + static constexpr uint8_t ICM20948_YA_OFFS_L {0x18}; + static constexpr uint8_t ICM20948_ZA_OFFS_H {0x1A}; + static constexpr uint8_t ICM20948_ZA_OFFS_L {0x1B}; + static constexpr uint8_t ICM20948_TIMEBASE_CORR_PLL {0x28}; + + /* Registers ICM20948 USER BANK 2*/ + static constexpr uint8_t ICM20948_GYRO_SMPLRT_DIV {0x00}; + static constexpr uint8_t ICM20948_GYRO_CONFIG_1 {0x01}; + static constexpr uint8_t ICM20948_GYRO_CONFIG_2 {0x02}; + static constexpr uint8_t ICM20948_XG_OFFS_USRH {0x03}; + static constexpr uint8_t ICM20948_XG_OFFS_USRL {0x04}; + static constexpr uint8_t ICM20948_YG_OFFS_USRH {0x05}; + static constexpr uint8_t ICM20948_YG_OFFS_USRL {0x06}; + static constexpr uint8_t ICM20948_ZG_OFFS_USRH {0x07}; + static constexpr uint8_t ICM20948_ZG_OFFS_USRL {0x08}; + static constexpr uint8_t ICM20948_ODR_ALIGN_EN {0x09}; + static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_1 {0x10}; + static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_2 {0x11}; + static constexpr uint8_t ICM20948_ACCEL_INTEL_CTRL {0x12}; + static constexpr uint8_t ICM20948_ACCEL_WOM_THR {0x13}; + static constexpr uint8_t ICM20948_ACCEL_CONFIG {0x14}; + static constexpr uint8_t ICM20948_ACCEL_CONFIG_2 {0x15}; + static constexpr uint8_t ICM20948_FSYNC_CONFIG {0x52}; + static constexpr uint8_t ICM20948_TEMP_CONFIG {0x53}; + static constexpr uint8_t ICM20948_MOD_CTRL_USR {0x54}; + + /* Registers ICM20948 USER BANK 3*/ + static constexpr uint8_t ICM20948_I2C_MST_ODR_CFG {0x00}; + static constexpr uint8_t ICM20948_I2C_MST_CTRL {0x01}; + static constexpr uint8_t ICM20948_I2C_MST_DELAY_CTRL {0x02}; + static constexpr uint8_t ICM20948_I2C_SLV0_ADDR {0x03}; + static constexpr uint8_t ICM20948_I2C_SLV0_REG {0x04}; + static constexpr uint8_t ICM20948_I2C_SLV0_CTRL {0x05}; + static constexpr uint8_t ICM20948_I2C_SLV0_DO {0x06}; + + /* Registers ICM20948 ALL BANKS */ + static constexpr uint8_t ICM20948_REG_BANK_SEL {0x7F}; + + /* Registers AK09916 */ + static constexpr uint8_t AK09916_WIA_1 {0x00}; // Who I am, Company ID + static constexpr uint8_t AK09916_WIA_2 {0x01}; // Who I am, Device ID + static constexpr uint8_t AK09916_STATUS_1 {0x10}; + static constexpr uint8_t AK09916_HXL {0x11}; + static constexpr uint8_t AK09916_HXH {0x12}; + static constexpr uint8_t AK09916_HYL {0x13}; + static constexpr uint8_t AK09916_HYH {0x14}; + static constexpr uint8_t AK09916_HZL {0x15}; + static constexpr uint8_t AK09916_HZH {0x16}; + static constexpr uint8_t AK09916_STATUS_2 {0x18}; + static constexpr uint8_t AK09916_CNTL_2 {0x31}; + static constexpr uint8_t AK09916_CNTL_3 {0x32}; + + /* Register Bits */ + static constexpr uint8_t ICM20948_RESET {0x80}; + static constexpr uint8_t ICM20948_I2C_MST_EN {0x20}; + static constexpr uint8_t ICM20948_SLEEP {0x40}; + static constexpr uint8_t ICM20948_LP_EN {0x20}; + static constexpr uint8_t ICM20948_BYPASS_EN {0x02}; + static constexpr uint8_t ICM20948_GYR_EN {0x07}; + static constexpr uint8_t ICM20948_ACC_EN {0x38}; + static constexpr uint8_t ICM20948_FIFO_EN {0x40}; + static constexpr uint8_t ICM20948_INT1_ACTL {0x80}; + static constexpr uint8_t ICM20948_INT_1_LATCH_EN {0x20}; + static constexpr uint8_t ICM20948_ACTL_FSYNC {0x08}; + static constexpr uint8_t ICM20948_INT_ANYRD_2CLEAR {0x10}; + static constexpr uint8_t ICM20948_FSYNC_INT_MODE_EN {0x06}; + static constexpr uint8_t AK09916_16_BIT {0x10}; + static constexpr uint8_t AK09916_OVF {0x08}; + static constexpr uint8_t AK09916_READ {0x80}; + + /* Others */ + static constexpr uint16_t AK09916_WHO_AM_I_1 {0x4809}; + static constexpr uint16_t AK09916_WHO_AM_I_2 {0x0948}; + static constexpr uint8_t ICM20948_WHO_AM_I_CONTENT{0xEA}; + static constexpr float ICM20948_ROOM_TEMP_OFFSET {0.0}; + static constexpr float ICM20948_T_SENSITIVITY {333.87}; + static constexpr float AK09916_MAG_LSB {0.1495}; + static constexpr uint8_t ICM20948_I2C_MST_RST {0x02}; + + /* Constructors */ + + ICM20948_WE(uint8_t addr = 0x68) : _wire{&Wire}, i2cAddress{addr}, useSPI{false} {} + ICM20948_WE(TwoWire *w, uint8_t addr = 0x68) : _wire{w}, i2cAddress{addr}, useSPI{false} {} + ICM20948_WE(int cs, bool spi) : _spi{&SPI}, csPin{cs}, useSPI{spi} {} + ICM20948_WE(SPIClass *s, int cs, bool spi) : _spi{s}, csPin{cs}, useSPI{spi} {} + + + /* Basic settings */ + + bool init(); + void autoOffsets(); + void setAccOffsets(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax); + void setAccOffsets(xyzFloat offset); // for writing back previous offsets + xyzFloat getAccOffsets(); + void setGyrOffsets(float xOffset, float yOffset, float zOffset); + void setGyrOffsets(xyzFloat offset); // for writing back previous offsets + xyzFloat getGyrOffsets(); + uint8_t whoAmI(); + void enableAcc(bool enAcc); + void setAccRange(ICM20948_accRange accRange); + void setAccDLPF(ICM20948_dlpf dlpf); + void setAccSampleRateDivider(uint16_t accSplRateDiv); + void enableGyr(bool enGyr); + void setGyrRange(ICM20948_gyroRange gyroRange); + void setGyrDLPF(ICM20948_dlpf dlpf); + void setGyrSampleRateDivider(uint8_t gyrSplRateDiv); + void setTempDLPF(ICM20948_dlpf dlpf); + void setI2CMstSampleRate(uint8_t rateExp); + void setSPIClockSpeed(unsigned long clock); + + + /* x,y,z results */ + + void readSensor(); + xyzFloat getAccRawValues(); + xyzFloat getCorrectedAccRawValues(); + xyzFloat getGValues(); + xyzFloat getAccRawValuesFromFifo(); + xyzFloat getCorrectedAccRawValuesFromFifo(); + xyzFloat getGValuesFromFifo(); + float getResultantG(xyzFloat gVal); + float getTemperature(); + xyzFloat getGyrRawValues(); + xyzFloat getCorrectedGyrRawValues(); + xyzFloat getGyrValues(); + xyzFloat getGyrValuesFromFifo(); + xyzFloat getMagValues(); + + + /* Angles and Orientation */ + + xyzFloat getAngles(); + ICM20948_orientation getOrientation(); + String getOrientationAsString(); + float getPitch(); + float getRoll(); + + + /* Power, Sleep, Standby */ + + void enableCycle(ICM20948_cycle cycle); + void enableLowPower(bool enLP); + void setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg); + void setAccAverageInCycleMode(ICM20948_accAvgLowPower avg); + void sleep(bool sleep); + + + /* Interrupts */ + + void setIntPinPolarity(ICM20948_intPinPol pol); + void enableIntLatch(bool latch); + void enableClearIntByAnyRead(bool clearByAnyRead); + void setFSyncIntPolarity(ICM20948_intPinPol pol); + void enableInterrupt(ICM20948_intType intType); + void disableInterrupt(ICM20948_intType intType); + uint8_t readAndClearInterrupts(); + bool checkInterrupt(uint8_t source, ICM20948_intType type); + void setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn); + + + /* FIFO */ + + void enableFifo(bool fifo); + void setFifoMode(ICM20948_fifoMode mode); + void startFifo(ICM20948_fifoType fifo); + void stopFifo(); + void resetFifo(); + int16_t getFifoCount(); + int16_t getNumberOfFifoDataSets(); + void findFifoBegin(); + + + /* Magnetometer */ + + bool initMagnetometer(); + uint16_t whoAmIMag(); + void setMagOpMode(AK09916_opMode opMode); + void resetMag(); + + protected: + TwoWire *_wire; + SPIClass *_spi; + SPISettings mySPISettings; + uint8_t i2cAddress; + uint8_t currentBank; + uint8_t buffer[20]; + xyzFloat accOffsetVal; + xyzFloat accCorrFactor; + xyzFloat gyrOffsetVal; + uint8_t accRangeFactor; + uint8_t gyrRangeFactor; + uint8_t regVal; // intermediate storage of register values + ICM20948_fifoType fifoType; + int csPin; + bool useSPI; + void setClockToAutoSelect(); + xyzFloat correctAccRawValues(xyzFloat accRawVal); + xyzFloat correctGyrRawValues(xyzFloat gyrRawVal); + void switchBank(uint8_t newBank); + void writeRegister8(uint8_t bank, uint8_t reg, uint8_t val); + void writeRegister16(uint8_t bank, uint8_t reg, int16_t val); + uint8_t readRegister8(uint8_t bank, uint8_t reg); + int16_t readRegister16(uint8_t bank, uint8_t reg); + void readAllData(uint8_t* data); + xyzFloat readICM20948xyzValFromFifo(); + void writeAK09916Register8(uint8_t reg, uint8_t val); + uint8_t readAK09916Register8(uint8_t reg); + int16_t readAK09916Register16(uint8_t reg); + void reset_ICM20948(); + void enableI2CMaster(); + void i2cMasterReset(); + void enableMagDataRead(uint8_t reg, uint8_t bytes); + +}; + +#endif diff --git a/LICENSE_ICM20948_WE b/LICENSE_ICM20948_WE new file mode 100644 index 0000000..4d0bc74 --- /dev/null +++ b/LICENSE_ICM20948_WE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Wolfgang (Wolle) Ewald + +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. \ No newline at end of file diff --git a/xyzFloat.cpp b/xyzFloat.cpp new file mode 100644 index 0000000..2ae7b00 --- /dev/null +++ b/xyzFloat.cpp @@ -0,0 +1,96 @@ +/* + * Code based on: https://github.com/wollewald/ICM20948_WE. + * Author: Wolfgang (Wolle) Ewald. + * License: MIT (see LICENSE_ICM20948_WE file). +*/ + +/* defines the structure xyzFloat which is used for gyroscopes, accelerometers and + magnetometers such as ICM20948, MPU9250, ADXL345, etc. */ + +#include "xyzFloat.h" + +xyzFloat::xyzFloat() + : xyzFloat(0.f, 0.f, 0.f) +{ + // intentionally empty +} + +xyzFloat::xyzFloat(float const x, float const y, float const z) + : x(x) + , y(y) + , z(z) +{ + // intentionally empty +} + +xyzFloat xyzFloat::operator+() const +{ + return *this; +} + +xyzFloat xyzFloat::operator-() const +{ + return xyzFloat{-x, + -y, + -z}; +} + +xyzFloat xyzFloat::operator+(xyzFloat const & summand) const +{ + return xyzFloat{x + summand.x, + y + summand.y, + z + summand.z}; +} + +xyzFloat xyzFloat::operator-(xyzFloat const & subtrahend) const +{ + return xyzFloat{x - subtrahend.x, + y - subtrahend.y, + z - subtrahend.z}; +} + +xyzFloat xyzFloat::operator*(float const operand) const +{ + return xyzFloat{x * operand, + y * operand, + z * operand}; +} + +xyzFloat xyzFloat::operator/(float const divisor) const +{ + return xyzFloat{x / divisor, + y / divisor, + z / divisor}; +} + +xyzFloat & xyzFloat::operator+=(xyzFloat const & summand) +{ + x += summand.x; + y += summand.y; + z += summand.z; + return *this; +} + +xyzFloat & xyzFloat::operator-=(xyzFloat const & subtrahend) +{ + x -= subtrahend.x; + y -= subtrahend.y; + z -= subtrahend.z; + return *this; +} + +xyzFloat & xyzFloat::operator*=(float const operand) +{ + x *= operand; + y *= operand; + z *= operand; + return *this; +} + +xyzFloat & xyzFloat::operator/=(float const divisor) +{ + x /= divisor; + y /= divisor; + z /= divisor; + return *this; +} diff --git a/xyzFloat.h b/xyzFloat.h new file mode 100644 index 0000000..2761bba --- /dev/null +++ b/xyzFloat.h @@ -0,0 +1,32 @@ +/* + * Code based on: https://github.com/wollewald/ICM20948_WE. + * Author: Wolfgang (Wolle) Ewald. + * License: MIT (see LICENSE_ICM20948_WE file). +*/ + +/* defines the structure xyzFloat which is used for gyroscopes, accelerometers and + magnetometers such as ICM20948, MPU9250, ADXL345, etc. */ + +#ifndef XYZ_FLOAT_H_ +#define XYZ_FLOAT_H_ +#include +struct xyzFloat { + float x; + float y; + float z; + + xyzFloat(); + xyzFloat(float const x, float const y, float const z); + + xyzFloat operator+() const; + xyzFloat operator-() const; + xyzFloat operator+(xyzFloat const & summand) const; + xyzFloat operator-(xyzFloat const & subtrahend) const; + xyzFloat operator*(float const operand) const; + xyzFloat operator/(float const divisor) const; + xyzFloat & operator+=(xyzFloat const & summand); + xyzFloat & operator-=(xyzFloat const & subtrahend); + xyzFloat & operator*=(float const operand); + xyzFloat & operator/=(float const divisor); +}; +#endif From d8f5570af573cab771f52926bc2baebdbfd9b675 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 09:30:27 +0300 Subject: [PATCH 03/20] Make basic adaptations of ICM-20948 library to IMUInterface --- ICM20948.cpp | 516 +++++++++++++-------------------------------------- ICM20948.h | 91 +++------ IMU.h | 2 +- MPU9250.h | 2 +- 4 files changed, 162 insertions(+), 449 deletions(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index 008d1f0..63b88cf 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -23,37 +23,37 @@ * *********************************************************************/ -#include "ICM20948_WE.h" +#include "ICM20948.h" /************ Basic Settings ************/ -bool ICM20948_WE::init(){ +bool ICM20948::begin(){ if(useSPI){ + _spi->begin(); + if (csPin == -1) { + // use default CS pin + #ifdef ESP32 + csPin = _spi->pinSS(); + #else + csPin = SS; + #endif + } pinMode(csPin, OUTPUT); digitalWrite(csPin, HIGH); - _spi->begin(); mySPISettings = SPISettings(7000000, MSBFIRST, SPI_MODE0); } currentBank = 0; - reset_ICM20948(); + reset(); if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ delay(2000); if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ + log("Error: incorrect WHO_AM_I value: 0x%02X", whoAmI()); return false; } } - accOffsetVal.x = 0.0; - accOffsetVal.y = 0.0; - accOffsetVal.z = 0.0; - accCorrFactor.x = 1.0; - accCorrFactor.y = 1.0; - accCorrFactor.z = 1.0; accRangeFactor = 1.0; - gyrOffsetVal.x = 0.0; - gyrOffsetVal.y = 0.0; - gyrOffsetVal.z = 0.0; gyrRangeFactor = 1.0; fifoType = ICM20948_FIFO_ACC; @@ -63,88 +63,11 @@ bool ICM20948_WE::init(){ return true; } -void ICM20948_WE::autoOffsets(){ - xyzFloat accRawVal, gyrRawVal; - accOffsetVal.x = 0.0; - accOffsetVal.y = 0.0; - accOffsetVal.z = 0.0; - - setGyrDLPF(ICM20948_DLPF_6); // lowest noise - setGyrRange(ICM20948_GYRO_RANGE_250); // highest resolution - setAccRange(ICM20948_ACC_RANGE_2G); - setAccDLPF(ICM20948_DLPF_6); - delay(100); - - for(int i=0; i<10; i++){ // Allow to get stable values - readSensor(); - delay(10); - } - - for(int i=0; i<50; i++){ - readSensor(); - accRawVal = getAccRawValues(); - accOffsetVal.x += accRawVal.x; - accOffsetVal.y += accRawVal.y; - accOffsetVal.z += accRawVal.z; - delay(10); - } - - accOffsetVal.x /= 50; - accOffsetVal.y /= 50; - accOffsetVal.z /= 50; - accOffsetVal.z -= 16384.0; - - for(int i=0; i<50; i++){ - readSensor(); - gyrRawVal = getGyrRawValues(); - gyrOffsetVal.x += gyrRawVal.x; - gyrOffsetVal.y += gyrRawVal.y; - gyrOffsetVal.z += gyrRawVal.z; - delay(1); - } - - gyrOffsetVal.x /= 50; - gyrOffsetVal.y /= 50; - gyrOffsetVal.z /= 50; - -} - -void ICM20948_WE::setAccOffsets(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){ - accOffsetVal.x = (xMax + xMin) * 0.5; - accOffsetVal.y = (yMax + yMin) * 0.5; - accOffsetVal.z = (zMax + zMin) * 0.5; - accCorrFactor.x = (xMax + abs(xMin)) / 32768.0; - accCorrFactor.y = (yMax + abs(yMin)) / 32768.0; - accCorrFactor.z = (zMax + abs(zMin)) / 32768.0 ; -} - -void ICM20948_WE::setAccOffsets(xyzFloat offset){ - accOffsetVal = offset; -} - -xyzFloat ICM20948_WE::getAccOffsets(){ - return accOffsetVal; -} - -void ICM20948_WE::setGyrOffsets(float xOffset, float yOffset, float zOffset){ - gyrOffsetVal.x = xOffset; - gyrOffsetVal.y = yOffset; - gyrOffsetVal.z = zOffset; -} - -void ICM20948_WE::setGyrOffsets(xyzFloat offset){ - gyrOffsetVal = offset; -} - -xyzFloat ICM20948_WE::getGyrOffsets(){ - return gyrOffsetVal; -} - -uint8_t ICM20948_WE::whoAmI(){ +uint8_t ICM20948::whoAmI() { return readRegister8(0, ICM20948_WHO_AM_I); } -void ICM20948_WE::enableAcc(bool enAcc){ +void ICM20948::enableAcc(bool enAcc){ regVal = readRegister8(0, ICM20948_PWR_MGMT_2); if(enAcc){ regVal &= ~ICM20948_ACC_EN; @@ -155,7 +78,7 @@ void ICM20948_WE::enableAcc(bool enAcc){ writeRegister8(0, ICM20948_PWR_MGMT_2, regVal); } -void ICM20948_WE::setAccRange(ICM20948_accRange accRange){ +void ICM20948::setAccRange(ICM20948_accRange accRange){ regVal = readRegister8(2, ICM20948_ACCEL_CONFIG); regVal &= ~(0x06); regVal |= (accRange<<1); @@ -163,7 +86,7 @@ void ICM20948_WE::setAccRange(ICM20948_accRange accRange){ accRangeFactor = 1<(((buffer[0]) << 8) | buffer[1]) * 1.0; - accRawVal.y = static_cast(((buffer[2]) << 8) | buffer[3]) * 1.0; - accRawVal.z = static_cast(((buffer[4]) << 8) | buffer[5]) * 1.0; - return accRawVal; -} - -xyzFloat ICM20948_WE::getCorrectedAccRawValues(){ - xyzFloat accRawVal = getAccRawValues(); - accRawVal = correctAccRawValues(accRawVal); - - return accRawVal; -} -xyzFloat ICM20948_WE::getGValues(){ - xyzFloat gVal, accRawVal; - accRawVal = getCorrectedAccRawValues(); - - gVal.x = accRawVal.x * accRangeFactor / 16384.0; - gVal.y = accRawVal.y * accRangeFactor / 16384.0; - gVal.z = accRawVal.z * accRangeFactor / 16384.0; - return gVal; +bool ICM20948::setRate(Rate rate) { + // output rate = 1125 Hz / (1 + srd) + // TODO: Implement this function + switch(rate) { + case RATE_MIN: // 1.125 Hz + setAccSampleRateDivider(0x4F); + setGyrSampleRateDivider(0x4F); + setI2CMstSampleRate(0x4F); + return true; + case RATE_50HZ_APPROX: // 37 Hz + setAccSampleRateDivider(0x07); + setGyrSampleRateDivider(0x07); + setI2CMstSampleRate(0x07); + return true; + case RATE_MAX: + case RATE_1KHZ_APPROX: // 1125 Hz + setAccSampleRateDivider(0); + setGyrSampleRateDivider(0); + setI2CMstSampleRate(0); + return true; + default: + log("Unsupported rate setting"); + return false; + } } -xyzFloat ICM20948_WE::getAccRawValuesFromFifo(){ - xyzFloat accRawVal = readICM20948xyzValFromFifo(); - return accRawVal; -} +/************* Results *************/ -xyzFloat ICM20948_WE::getCorrectedAccRawValuesFromFifo(){ - xyzFloat accRawVal = getAccRawValuesFromFifo(); - accRawVal = correctAccRawValues(accRawVal); - - return accRawVal; +bool ICM20948::read(){ + readAllData(buffer); + return true; } -xyzFloat ICM20948_WE::getGValuesFromFifo(){ - xyzFloat gVal, accRawVal; - accRawVal = getCorrectedAccRawValuesFromFifo(); - - gVal.x = accRawVal.x * accRangeFactor / 16384.0; - gVal.y = accRawVal.y * accRangeFactor / 16384.0; - gVal.z = accRawVal.z * accRangeFactor / 16384.0; - return gVal; +void ICM20948::waitForData() { + const static uint8_t RAW_DATA_0_RDY_INT = 0x01; + while (true) { + uint8_t intStatus1 = readRegister8(0, ICM20948_INT_STATUS_1); + bool dataReady = intStatus1 & RAW_DATA_0_RDY_INT; + if (dataReady) break; + } + read(); } -float ICM20948_WE::getResultantG(xyzFloat gVal){ - float resultant = 0.0; - resultant = sqrt(sq(gVal.x) + sq(gVal.y) + sq(gVal.z)); - - return resultant; +void ICM20948::getAccel(float& x, float& y, float& z) const { + x = static_cast(((buffer[0]) << 8) | buffer[1]) * 1.0; + y = static_cast(((buffer[2]) << 8) | buffer[3]) * 1.0; + z = static_cast(((buffer[4]) << 8) | buffer[5]) * 1.0; + // raw to g + x = x * accRangeFactor / 16384.0; + y = y * accRangeFactor / 16384.0; + z = z * accRangeFactor / 16384.0; + // convert to m/s^2 + static constexpr float G = 9.80665f; + x = x * G; + y = y * G; + z = z * G; } -float ICM20948_WE::getTemperature(){ +float ICM20948::getTemp() const { int16_t rawTemp = static_cast(((buffer[12]) << 8) | buffer[13]); float tmp = (rawTemp*1.0 - ICM20948_ROOM_TEMP_OFFSET)/ICM20948_T_SENSITIVITY + 21.0; return tmp; } -xyzFloat ICM20948_WE::getGyrRawValues(){ - xyzFloat gyrRawVal; - - gyrRawVal.x = (int16_t)(((buffer[6]) << 8) | buffer[7]) * 1.0; - gyrRawVal.y = (int16_t)(((buffer[8]) << 8) | buffer[9]) * 1.0; - gyrRawVal.z = (int16_t)(((buffer[10]) << 8) | buffer[11]) * 1.0; - - return gyrRawVal; -} - -xyzFloat ICM20948_WE::getCorrectedGyrRawValues(){ - xyzFloat gyrRawVal = getGyrRawValues(); - gyrRawVal = correctGyrRawValues(gyrRawVal); - return gyrRawVal; +void ICM20948::getGyro(float& x, float& y, float& z) const { + x = (int16_t)(((buffer[6]) << 8) | buffer[7]) * 1.0; + y = (int16_t)(((buffer[8]) << 8) | buffer[9]) * 1.0; + z = (int16_t)(((buffer[10]) << 8) | buffer[11]) * 1.0; + // raw to dps + x = x * gyrRangeFactor * 250.0 / 32768.0; + y = y * gyrRangeFactor * 250.0 / 32768.0; + z = z * gyrRangeFactor * 250.0 / 32768.0; + // convert to rad/s + x = x * DEG_TO_RAD; + y = y * DEG_TO_RAD; + z = z * DEG_TO_RAD; } -xyzFloat ICM20948_WE::getGyrValues(){ - xyzFloat gyrVal = getCorrectedGyrRawValues(); - - gyrVal.x = gyrVal.x * gyrRangeFactor * 250.0 / 32768.0; - gyrVal.y = gyrVal.y * gyrRangeFactor * 250.0 / 32768.0; - gyrVal.z = gyrVal.z * gyrRangeFactor * 250.0 / 32768.0; - - return gyrVal; -} - -xyzFloat ICM20948_WE::getGyrValuesFromFifo(){ - xyzFloat gyrVal; - xyzFloat gyrRawVal = readICM20948xyzValFromFifo(); - - gyrRawVal = correctGyrRawValues(gyrRawVal); - gyrVal.x = gyrRawVal.x * gyrRangeFactor * 250.0 / 32768.0; - gyrVal.y = gyrRawVal.y * gyrRangeFactor * 250.0 / 32768.0; - gyrVal.z = gyrRawVal.z * gyrRangeFactor * 250.0 / 32768.0; - - return gyrVal; -} - -xyzFloat ICM20948_WE::getMagValues(){ - int16_t x,y,z; - xyzFloat mag; - +void ICM20948::getMag(float& x, float& y, float& z) const { x = static_cast((buffer[15]) << 8) | buffer[14]; y = static_cast((buffer[17]) << 8) | buffer[16]; z = static_cast((buffer[19]) << 8) | buffer[18]; - - mag.x = x * AK09916_MAG_LSB; - mag.y = y * AK09916_MAG_LSB; - mag.z = z * AK09916_MAG_LSB; - - return mag; + // correct values + x = x * AK09916_MAG_LSB; + y = y * AK09916_MAG_LSB; + z = z * AK09916_MAG_LSB; } /********* Power, Sleep, Standby *********/ -void ICM20948_WE::enableCycle(ICM20948_cycle cycle){ +void ICM20948::enableCycle(ICM20948_cycle cycle){ regVal = readRegister8(0, ICM20948_LP_CONFIG); regVal &= 0x0F; regVal |= cycle; @@ -364,7 +256,7 @@ void ICM20948_WE::enableCycle(ICM20948_cycle cycle){ writeRegister8(0, ICM20948_LP_CONFIG, regVal); } -void ICM20948_WE::enableLowPower(bool enLP){ // vielleicht besser privat???? +void ICM20948::enableLowPower(bool enLP){ // vielleicht besser privat???? regVal = readRegister8(0, ICM20948_PWR_MGMT_1); if(enLP){ regVal |= ICM20948_LP_EN; @@ -375,15 +267,15 @@ void ICM20948_WE::enableLowPower(bool enLP){ // vielleicht besser privat???? writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); } -void ICM20948_WE::setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg){ +void ICM20948::setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg){ writeRegister8(2, ICM20948_GYRO_CONFIG_2, avg); } -void ICM20948_WE::setAccAverageInCycleMode(ICM20948_accAvgLowPower avg){ +void ICM20948::setAccAverageInCycleMode(ICM20948_accAvgLowPower avg){ writeRegister8(2, ICM20948_ACCEL_CONFIG_2, avg); } -void ICM20948_WE::sleep(bool sleep){ +void ICM20948::sleep(bool sleep){ regVal = readRegister8(0, ICM20948_PWR_MGMT_1); if(sleep){ regVal |= ICM20948_SLEEP; @@ -393,101 +285,10 @@ void ICM20948_WE::sleep(bool sleep){ } writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); } - -/******** Angles and Orientation *********/ - -xyzFloat ICM20948_WE::getAngles(){ - xyzFloat angleVal; - xyzFloat gVal = getGValues(); - if(gVal.x > 1.0){ - gVal.x = 1.0; - } - else if(gVal.x < -1.0){ - gVal.x = -1.0; - } - angleVal.x = (asin(gVal.x)) * 57.296; - - if(gVal.y > 1.0){ - gVal.y = 1.0; - } - else if(gVal.y < -1.0){ - gVal.y = -1.0; - } - angleVal.y = (asin(gVal.y)) * 57.296; - - if(gVal.z > 1.0){ - gVal.z = 1.0; - } - else if(gVal.z < -1.0){ - gVal.z = -1.0; - } - angleVal.z = (asin(gVal.z)) * 57.296; - - return angleVal; -} - -ICM20948_orientation ICM20948_WE::getOrientation(){ - xyzFloat angleVal = getAngles(); - ICM20948_orientation orientation = ICM20948_FLAT; - if(abs(angleVal.x) < 45){ // |x| < 45 - if(abs(angleVal.y) < 45){ // |y| < 45 - if(angleVal.z > 0){ // z > 0 - orientation = ICM20948_FLAT; - } - else{ // z < 0 - orientation = ICM20948_FLAT_1; - } - } - else{ // |y| > 45 - if(angleVal.y > 0){ // y > 0 - orientation = ICM20948_XY; - } - else{ // y < 0 - orientation = ICM20948_XY_1; - } - } - } - else{ // |x| >= 45 - if(angleVal.x > 0){ // x > 0 - orientation = ICM20948_YX; - } - else{ // x < 0 - orientation = ICM20948_YX_1; - } - } - return orientation; -} - -String ICM20948_WE::getOrientationAsString(){ - ICM20948_orientation orientation = getOrientation(); - String orientationAsString = ""; - switch(orientation){ - case ICM20948_FLAT: orientationAsString = "z up"; break; - case ICM20948_FLAT_1: orientationAsString = "z down"; break; - case ICM20948_XY: orientationAsString = "y up"; break; - case ICM20948_XY_1: orientationAsString = "y down"; break; - case ICM20948_YX: orientationAsString = "x up"; break; - case ICM20948_YX_1: orientationAsString = "x down"; break; - } - return orientationAsString; -} - -float ICM20948_WE::getPitch(){ - xyzFloat angleVal = getAngles(); - float pitch = (atan2(-angleVal.x, sqrt(abs((angleVal.y*angleVal.y + angleVal.z*angleVal.z))))*180.0)/M_PI; - return pitch; -} - -float ICM20948_WE::getRoll(){ - xyzFloat angleVal = getAngles(); - float roll = (atan2(angleVal.y, angleVal.z)*180.0)/M_PI; - return roll; -} - /************** Interrupts ***************/ -void ICM20948_WE::setIntPinPolarity(ICM20948_intPinPol pol){ +void ICM20948::setIntPinPolarity(ICM20948_intPinPol pol){ regVal = readRegister8(0, ICM20948_INT_PIN_CFG); if(pol){ regVal |= ICM20948_INT1_ACTL; @@ -498,7 +299,7 @@ void ICM20948_WE::setIntPinPolarity(ICM20948_intPinPol pol){ writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); } -void ICM20948_WE::enableIntLatch(bool latch){ +void ICM20948::enableIntLatch(bool latch){ regVal = readRegister8(0, ICM20948_INT_PIN_CFG); if(latch){ regVal |= ICM20948_INT_1_LATCH_EN; @@ -509,7 +310,7 @@ void ICM20948_WE::enableIntLatch(bool latch){ writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); } -void ICM20948_WE::enableClearIntByAnyRead(bool clearByAnyRead){ +void ICM20948::enableClearIntByAnyRead(bool clearByAnyRead){ regVal = readRegister8(0, ICM20948_INT_PIN_CFG); if(clearByAnyRead){ regVal |= ICM20948_INT_ANYRD_2CLEAR; @@ -520,7 +321,7 @@ void ICM20948_WE::enableClearIntByAnyRead(bool clearByAnyRead){ writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); } -void ICM20948_WE::setFSyncIntPolarity(ICM20948_intPinPol pol){ +void ICM20948::setFSyncIntPolarity(ICM20948_intPinPol pol){ regVal = readRegister8(0, ICM20948_INT_PIN_CFG); if(pol){ regVal |= ICM20948_ACTL_FSYNC; @@ -531,7 +332,7 @@ void ICM20948_WE::setFSyncIntPolarity(ICM20948_intPinPol pol){ writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); } -void ICM20948_WE::enableInterrupt(ICM20948_intType intType){ +void ICM20948::enableInterrupt(ICM20948_intType intType){ switch(intType){ case ICM20948_FSYNC_INT: regVal = readRegister8(0, ICM20948_INT_PIN_CFG); @@ -571,7 +372,7 @@ void ICM20948_WE::enableInterrupt(ICM20948_intType intType){ } } -void ICM20948_WE::disableInterrupt(ICM20948_intType intType){ +void ICM20948::disableInterrupt(ICM20948_intType intType){ switch(intType){ case ICM20948_FSYNC_INT: regVal = readRegister8(0, ICM20948_INT_PIN_CFG); @@ -611,7 +412,7 @@ void ICM20948_WE::disableInterrupt(ICM20948_intType intType){ } } -uint8_t ICM20948_WE::readAndClearInterrupts(){ +uint8_t ICM20948::readAndClearInterrupts(){ uint8_t intSource = 0; regVal = readRegister8(0, ICM20948_I2C_MST_STATUS); if(regVal & 0x80){ @@ -639,11 +440,11 @@ uint8_t ICM20948_WE::readAndClearInterrupts(){ return intSource; } -bool ICM20948_WE::checkInterrupt(uint8_t source, ICM20948_intType type){ +bool ICM20948::checkInterrupt(uint8_t source, ICM20948_intType type){ source &= type; return source; } -void ICM20948_WE::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn){ +void ICM20948::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn){ regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); if(womCompEn){ regVal |= 0x01; @@ -657,7 +458,7 @@ void ICM20948_WE::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn /***************** FIFO ******************/ -void ICM20948_WE::enableFifo(bool fifo){ +void ICM20948::enableFifo(bool fifo){ regVal = readRegister8(0, ICM20948_USER_CTRL); if(fifo){ regVal |= ICM20948_FIFO_EN; @@ -668,7 +469,7 @@ void ICM20948_WE::enableFifo(bool fifo){ writeRegister8(0, ICM20948_USER_CTRL, regVal); } -void ICM20948_WE::setFifoMode(ICM20948_fifoMode mode){ +void ICM20948::setFifoMode(ICM20948_fifoMode mode){ if(mode){ regVal = 0x01; } @@ -678,26 +479,26 @@ void ICM20948_WE::setFifoMode(ICM20948_fifoMode mode){ writeRegister8(0, ICM20948_FIFO_MODE, regVal); } -void ICM20948_WE::startFifo(ICM20948_fifoType fifo){ +void ICM20948::startFifo(ICM20948_fifoType fifo){ fifoType = fifo; writeRegister8(0, ICM20948_FIFO_EN_2, fifoType); } -void ICM20948_WE::stopFifo(){ +void ICM20948::stopFifo(){ writeRegister8(0, ICM20948_FIFO_EN_2, 0); } -void ICM20948_WE::resetFifo(){ +void ICM20948::resetFifo(){ writeRegister8(0, ICM20948_FIFO_RST, 0x01); writeRegister8(0, ICM20948_FIFO_RST, 0x00); } -int16_t ICM20948_WE::getFifoCount(){ +int16_t ICM20948::getFifoCount(){ int16_t regVal16 = static_cast(readRegister16(0, ICM20948_FIFO_COUNT)); return regVal16; } -int16_t ICM20948_WE::getNumberOfFifoDataSets(){ +int16_t ICM20948::getNumberOfFifoDataSets(){ int16_t numberOfSets = getFifoCount(); if((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)){ @@ -710,7 +511,7 @@ int16_t ICM20948_WE::getNumberOfFifoDataSets(){ return numberOfSets; } -void ICM20948_WE::findFifoBegin(){ +void ICM20948::findFifoBegin(){ uint16_t count = getFifoCount(); int16_t start = 0; @@ -731,10 +532,10 @@ void ICM20948_WE::findFifoBegin(){ /************** Magnetometer **************/ -bool ICM20948_WE::initMagnetometer(){ +bool ICM20948::initMagnetometer(){ enableI2CMaster(); resetMag(); - reset_ICM20948(); + reset(); sleep(false); writeRegister8(2, ICM20948_ODR_ALIGN_EN, 1); // aligns ODR @@ -761,11 +562,11 @@ bool ICM20948_WE::initMagnetometer(){ return initSuccess; } -uint16_t ICM20948_WE::whoAmIMag(){ +uint16_t ICM20948::whoAmIMag(){ return static_cast(readAK09916Register16(AK09916_WIA_1)); } -void ICM20948_WE::setMagOpMode(AK09916_opMode opMode){ +void ICM20948::setMagOpMode(AK09916_opMode opMode){ writeAK09916Register8(AK09916_CNTL_2, opMode); delay(10); if(opMode!=AK09916_PWR_DOWN){ @@ -773,7 +574,7 @@ void ICM20948_WE::setMagOpMode(AK09916_opMode opMode){ } } -void ICM20948_WE::resetMag(){ +void ICM20948::resetMag(){ writeAK09916Register8(AK09916_CNTL_3, 0x01); delay(100); } @@ -782,30 +583,14 @@ void ICM20948_WE::resetMag(){ Private Functions *************************************************/ -void ICM20948_WE::setClockToAutoSelect(){ +void ICM20948::setClockToAutoSelect(){ regVal = readRegister8(0, ICM20948_PWR_MGMT_1); regVal |= 0x01; writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); delay(10); } -xyzFloat ICM20948_WE::correctAccRawValues(xyzFloat accRawVal){ - accRawVal.x = (accRawVal.x -(accOffsetVal.x / accRangeFactor)) / accCorrFactor.x; - accRawVal.y = (accRawVal.y -(accOffsetVal.y / accRangeFactor)) / accCorrFactor.y; - accRawVal.z = (accRawVal.z -(accOffsetVal.z / accRangeFactor)) / accCorrFactor.z; - - return accRawVal; -} - -xyzFloat ICM20948_WE::correctGyrRawValues(xyzFloat gyrRawVal){ - gyrRawVal.x -= (gyrOffsetVal.x / gyrRangeFactor); - gyrRawVal.y -= (gyrOffsetVal.y / gyrRangeFactor); - gyrRawVal.z -= (gyrOffsetVal.z / gyrRangeFactor); - - return gyrRawVal; -} - -void ICM20948_WE::switchBank(uint8_t newBank){ +void ICM20948::switchBank(uint8_t newBank){ if(newBank != currentBank){ currentBank = newBank; if(!useSPI){ @@ -825,7 +610,7 @@ void ICM20948_WE::switchBank(uint8_t newBank){ } } -void ICM20948_WE::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val){ +void ICM20948::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val){ switchBank(bank); if(!useSPI){ @@ -844,7 +629,7 @@ void ICM20948_WE::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val){ } } -void ICM20948_WE::writeRegister16(uint8_t bank, uint8_t reg, int16_t val){ +void ICM20948::writeRegister16(uint8_t bank, uint8_t reg, int16_t val){ switchBank(bank); int8_t MSByte = static_cast((val>>8) & 0xFF); uint8_t LSByte = val & 0xFF; @@ -866,7 +651,7 @@ void ICM20948_WE::writeRegister16(uint8_t bank, uint8_t reg, int16_t val){ } } -uint8_t ICM20948_WE::readRegister8(uint8_t bank, uint8_t reg){ +uint8_t ICM20948::readRegister8(uint8_t bank, uint8_t reg){ switchBank(bank); uint8_t regValue = 0; @@ -891,7 +676,7 @@ uint8_t ICM20948_WE::readRegister8(uint8_t bank, uint8_t reg){ return regValue; } -int16_t ICM20948_WE::readRegister16(uint8_t bank, uint8_t reg){ +int16_t ICM20948::readRegister16(uint8_t bank, uint8_t reg){ switchBank(bank); uint8_t MSByte = 0, LSByte = 0; int16_t reg16Val = 0; @@ -920,7 +705,7 @@ int16_t ICM20948_WE::readRegister16(uint8_t bank, uint8_t reg){ return reg16Val; } -void ICM20948_WE::readAllData(uint8_t* data){ +void ICM20948::readAllData(uint8_t* data){ switchBank(0); if(!useSPI){ @@ -947,56 +732,21 @@ void ICM20948_WE::readAllData(uint8_t* data){ } } -xyzFloat ICM20948_WE::readICM20948xyzValFromFifo(){ - uint8_t fifoTriple[6]; - xyzFloat xyzResult = {0.0, 0.0, 0.0}; - switchBank(0); - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(ICM20948_FIFO_R_W); - _wire->endTransmission(false); - _wire->requestFrom(i2cAddress, static_cast(6)); - if(_wire->available()){ - for(int i=0; i<6; i++){ - fifoTriple[i] = _wire->read(); - } - } - } - else{ - uint8_t reg = ICM20948_FIFO_R_W | 0x80; - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - for(int i=0; i<6; i++){ - fifoTriple[i] = _spi->transfer(0x00); - } - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } - - xyzResult.x = (static_cast((fifoTriple[0]<<8) + fifoTriple[1])) * 1.0; - xyzResult.y = (static_cast((fifoTriple[2]<<8) + fifoTriple[3])) * 1.0; - xyzResult.z = (static_cast((fifoTriple[4]<<8) + fifoTriple[5])) * 1.0; - - return xyzResult; -} - -void ICM20948_WE::writeAK09916Register8(uint8_t reg, uint8_t val){ +void ICM20948::writeAK09916Register8(uint8_t reg, uint8_t val){ writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS); // write AK09916 writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be written to writeRegister8(3, ICM20948_I2C_SLV0_DO, val); } -uint8_t ICM20948_WE::readAK09916Register8(uint8_t reg){ +uint8_t ICM20948::readAK09916Register8(uint8_t reg){ enableMagDataRead(reg, 0x01); enableMagDataRead(AK09916_HXL, 0x08); regVal = readRegister8(0, ICM20948_EXT_SLV_SENS_DATA_00); return regVal; } -int16_t ICM20948_WE::readAK09916Register16(uint8_t reg){ +int16_t ICM20948::readAK09916Register16(uint8_t reg){ int16_t regValue = 0; enableMagDataRead(reg, 0x02); regValue = readRegister16(0, ICM20948_EXT_SLV_SENS_DATA_00); @@ -1004,25 +754,25 @@ int16_t ICM20948_WE::readAK09916Register16(uint8_t reg){ return regValue; } -void ICM20948_WE::reset_ICM20948(){ +void ICM20948::reset() { writeRegister8(0, ICM20948_PWR_MGMT_1, ICM20948_RESET); delay(10); // wait for registers to reset } -void ICM20948_WE::enableI2CMaster(){ +void ICM20948::enableI2CMaster(){ writeRegister8(0, ICM20948_USER_CTRL, ICM20948_I2C_MST_EN); //enable I2C master writeRegister8(3, ICM20948_I2C_MST_CTRL, 0x07); // set I2C clock to 345.60 kHz delay(10); } -void ICM20948_WE::i2cMasterReset(){ +void ICM20948::i2cMasterReset(){ uint8_t regVal = readRegister8(0, ICM20948_USER_CTRL); regVal |= ICM20948_I2C_MST_RST; writeRegister8(0, ICM20948_USER_CTRL, regVal); delay(10); } -void ICM20948_WE::enableMagDataRead(uint8_t reg, uint8_t bytes){ +void ICM20948::enableMagDataRead(uint8_t reg, uint8_t bytes){ writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS | AK09916_READ); // read AK09916 writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be read writeRegister8(3, ICM20948_I2C_SLV0_CTRL, 0x80 | bytes); //enable read | number of byte diff --git a/ICM20948.h b/ICM20948.h index 875a818..7fbe3a1 100644 --- a/ICM20948.h +++ b/ICM20948.h @@ -25,8 +25,7 @@ * ******************************************************************************/ -#ifndef ICM20948_WE_H_ -#define ICM20948_WE_H_ +#pragma once #if (ARDUINO >= 100) #include "Arduino.h" @@ -36,7 +35,8 @@ #include #include -#include "xyzFloat.h" +#include "IMU.h" +#include "logger.h" /* Enums */ @@ -106,15 +106,11 @@ typedef enum AK09916_OP_MODE { AK09916_CONT_MODE_100HZ = 0x08 } AK09916_opMode; -typedef enum ICM20948_ORIENTATION { - ICM20948_FLAT, ICM20948_FLAT_1, ICM20948_XY, ICM20948_XY_1, ICM20948_YX, ICM20948_YX_1 -} ICM20948_orientation; - -class ICM20948_WE +class ICM20948 : public IMUInterface, public Logger { - public: + public: /* constants */ - + static constexpr uint8_t AK09916_ADDRESS {0x0C}; /* Registers ICM20948 USER BANK 0*/ @@ -240,23 +236,14 @@ class ICM20948_WE /* Constructors */ - ICM20948_WE(uint8_t addr = 0x68) : _wire{&Wire}, i2cAddress{addr}, useSPI{false} {} - ICM20948_WE(TwoWire *w, uint8_t addr = 0x68) : _wire{w}, i2cAddress{addr}, useSPI{false} {} - ICM20948_WE(int cs, bool spi) : _spi{&SPI}, csPin{cs}, useSPI{spi} {} - ICM20948_WE(SPIClass *s, int cs, bool spi) : _spi{s}, csPin{cs}, useSPI{spi} {} - - + ICM20948(uint8_t addr = 0x68) : _wire{&Wire}, i2cAddress{addr}, useSPI{false} {} + ICM20948(TwoWire *w, uint8_t addr = 0x68) : _wire{w}, i2cAddress{addr}, useSPI{false} {} + ICM20948(SPIClass& s, int cs = -1) : _spi{&s}, csPin{cs}, useSPI{true} {} /* Basic settings */ - bool init(); - void autoOffsets(); - void setAccOffsets(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax); - void setAccOffsets(xyzFloat offset); // for writing back previous offsets - xyzFloat getAccOffsets(); - void setGyrOffsets(float xOffset, float yOffset, float zOffset); - void setGyrOffsets(xyzFloat offset); // for writing back previous offsets - xyzFloat getGyrOffsets(); - uint8_t whoAmI(); + bool begin() override; + void reset() override; + uint8_t whoAmI() override; void enableAcc(bool enAcc); void setAccRange(ICM20948_accRange accRange); void setAccDLPF(ICM20948_dlpf dlpf); @@ -267,35 +254,20 @@ class ICM20948_WE void setGyrSampleRateDivider(uint8_t gyrSplRateDiv); void setTempDLPF(ICM20948_dlpf dlpf); void setI2CMstSampleRate(uint8_t rateExp); - void setSPIClockSpeed(unsigned long clock); - - - /* x,y,z results */ - - void readSensor(); - xyzFloat getAccRawValues(); - xyzFloat getCorrectedAccRawValues(); - xyzFloat getGValues(); - xyzFloat getAccRawValuesFromFifo(); - xyzFloat getCorrectedAccRawValuesFromFifo(); - xyzFloat getGValuesFromFifo(); - float getResultantG(xyzFloat gVal); - float getTemperature(); - xyzFloat getGyrRawValues(); - xyzFloat getCorrectedGyrRawValues(); - xyzFloat getGyrValues(); - xyzFloat getGyrValuesFromFifo(); - xyzFloat getMagValues(); - - - /* Angles and Orientation */ - - xyzFloat getAngles(); - ICM20948_orientation getOrientation(); - String getOrientationAsString(); - float getPitch(); - float getRoll(); - + void setSPIClockSpeed(unsigned long clock); + + bool setRate(Rate rate) override; + + const char* getModel() const override { return "ICM20948"; } + + /* Results */ + + bool read() override; + void waitForData() override; + void getAccel(float& x, float& y, float& z) const override; + float getTemp() const override; + void getGyro(float& x, float& y, float& z) const override; + void getMag(float& x, float& y, float& z) const override; /* Power, Sleep, Standby */ @@ -344,10 +316,7 @@ class ICM20948_WE SPISettings mySPISettings; uint8_t i2cAddress; uint8_t currentBank; - uint8_t buffer[20]; - xyzFloat accOffsetVal; - xyzFloat accCorrFactor; - xyzFloat gyrOffsetVal; + uint8_t buffer[20]; uint8_t accRangeFactor; uint8_t gyrRangeFactor; uint8_t regVal; // intermediate storage of register values @@ -355,23 +324,17 @@ class ICM20948_WE int csPin; bool useSPI; void setClockToAutoSelect(); - xyzFloat correctAccRawValues(xyzFloat accRawVal); - xyzFloat correctGyrRawValues(xyzFloat gyrRawVal); void switchBank(uint8_t newBank); void writeRegister8(uint8_t bank, uint8_t reg, uint8_t val); void writeRegister16(uint8_t bank, uint8_t reg, int16_t val); uint8_t readRegister8(uint8_t bank, uint8_t reg); int16_t readRegister16(uint8_t bank, uint8_t reg); void readAllData(uint8_t* data); - xyzFloat readICM20948xyzValFromFifo(); void writeAK09916Register8(uint8_t reg, uint8_t val); uint8_t readAK09916Register8(uint8_t reg); int16_t readAK09916Register16(uint8_t reg); - void reset_ICM20948(); void enableI2CMaster(); void i2cMasterReset(); void enableMagDataRead(uint8_t reg, uint8_t bytes); }; - -#endif diff --git a/IMU.h b/IMU.h index f228616..b3a8cfe 100644 --- a/IMU.h +++ b/IMU.h @@ -45,7 +45,7 @@ class IMUInterface { virtual bool begin() = 0; virtual void reset() = 0; - virtual uint8_t whoAmI() const = 0; + virtual uint8_t whoAmI() = 0; virtual bool read() = 0; virtual void waitForData() = 0; virtual void getAccel(float& x, float& y, float& z) const = 0; diff --git a/MPU9250.h b/MPU9250.h index 4237eda..5287547 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -109,7 +109,7 @@ class MPU9250 : public IMUInterface, public Logger { inline float mag_y_ut() const {return mag_[1];} inline float mag_z_ut() const {return mag_[2];} inline float getTemp() const override {return temp_;} - inline uint8_t whoAmI() const override {return who_am_i_;} + inline uint8_t whoAmI() override {return who_am_i_;} private: InvensenseImu imu_; From 68af10fd0d95f32815d5454752b00f309ac1d84d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 09:30:38 +0300 Subject: [PATCH 04/20] Fixes to MPU9250 example sketch --- examples/MPU9250/MPU9250.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MPU9250/MPU9250.ino b/examples/MPU9250/MPU9250.ino index 718cae7..a139927 100644 --- a/examples/MPU9250/MPU9250.ino +++ b/examples/MPU9250/MPU9250.ino @@ -9,7 +9,7 @@ void setup() { if (!success) { while (1) { Serial.println("Failed to initialize IMU"); - delay(1000); + delay(5000); } } } @@ -21,7 +21,7 @@ void loop() { IMU.getAccel(ax, ay, az); IMU.getMag(mx, my, mz); // Print data for Serial Plotter: - Serial.print("gx:"); Serial.print(gx); Serial.print(" gy:"); Serial.print(gy); Serial.print(" gz:"); Serial.println(gz); + Serial.print("gx:"); Serial.print(gx); Serial.print(" gy:"); Serial.print(gy); Serial.print(" gz:"); Serial.print(gz); Serial.print(" ax:"); Serial.print(ax); Serial.print(" ay:"); Serial.print(ay); Serial.print(" az:"); Serial.print(az); Serial.print(" mx:"); Serial.print(mx); Serial.print(" my:"); Serial.print(my); Serial.print(" mz:"); Serial.println(mz); delay(50); // slow down the output From 3c542cafc0281a33d12f50dbdd3b28f44744757f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 09:31:46 +0300 Subject: [PATCH 05/20] Add example sketch for ICM20948 --- .github/workflows/build.yaml | 8 ++++++++ examples/ICM20948/ICM20948.ino | 28 ++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+) create mode 100644 examples/ICM20948/ICM20948.ino diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 75ddc8d..26f90ef 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -33,6 +33,14 @@ jobs: run: cd examples/MPU9250 && arduino-cli compile --fqbn esp8266:esp8266:nodemcuv2 - name: Build MPU9250 example for Arduino Nano run: cd examples/MPU9250 && arduino-cli compile --fqbn arduino:avr:nano + - name: Build ICM20948 example for ESP32 + run: cd examples/ICM20948 && arduino-cli compile --fqbn esp32:esp32:esp32 + - name: Build ICM20948 example for ESP32 D1 Mini + run: cd examples/ICM20948 && arduino-cli compile --fqbn esp32:esp32:d1_mini32 + - name: Build ICM20948 example for ESP8266 + run: cd examples/ICM20948 && arduino-cli compile --fqbn esp8266:esp8266:nodemcuv2 + - name: Build ICM20948 example for Arduino Nano + run: cd examples/ICM20948 && arduino-cli compile --fqbn arduino:avr:nano - name: Build SBUS example for ESP32 run: cd examples/SBUS && arduino-cli compile --fqbn esp32:esp32:esp32 - name: Build SBUS example for ESP32 D1 Mini diff --git a/examples/ICM20948/ICM20948.ino b/examples/ICM20948/ICM20948.ino new file mode 100644 index 0000000..c818002 --- /dev/null +++ b/examples/ICM20948/ICM20948.ino @@ -0,0 +1,28 @@ +#include +#include + +ICM20948 IMU(SPI); // no need to specify CS pin, the default pin is used automatically + +void setup() { + Serial.begin(115200); + bool success = IMU.begin(); + if (!success) { + while (1) { + Serial.println("Failed to initialize IMU"); + delay(5000); + } + } +} + +void loop() { + float gx, gy, gz, ax, ay, az, mx, my, mz; + IMU.waitForData(); // blockingly read the data, use IMU.read() for non-blocking read + IMU.getGyro(gx, gy, gz); + IMU.getAccel(ax, ay, az); + IMU.getMag(mx, my, mz); + // Print data for Serial Plotter: + Serial.print("gx:"); Serial.print(gx); Serial.print(" gy:"); Serial.print(gy); Serial.print(" gz:"); Serial.print(gz); + Serial.print(" ax:"); Serial.print(ax); Serial.print(" ay:"); Serial.print(ay); Serial.print(" az:"); Serial.print(az); + Serial.print(" mx:"); Serial.print(mx); Serial.print(" my:"); Serial.print(my); Serial.print(" mz:"); Serial.println(mz); + delay(50); // slow down the output +} From 4d683eefe7ab4ff9f68359bfe19c613db3368cb6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 10:06:14 +0300 Subject: [PATCH 06/20] Rotate ICM20948 data to match MPU-9250 library --- ICM20948.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ICM20948.cpp b/ICM20948.cpp index 63b88cf..219c7f7 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -213,6 +213,12 @@ void ICM20948::getAccel(float& x, float& y, float& z) const { x = x * G; y = y * G; z = z * G; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = y; + y = tmp; + z = -z; } float ICM20948::getTemp() const { @@ -233,6 +239,12 @@ void ICM20948::getGyro(float& x, float& y, float& z) const { x = x * DEG_TO_RAD; y = y * DEG_TO_RAD; z = z * DEG_TO_RAD; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = y; + y = tmp; + z = -z; } void ICM20948::getMag(float& x, float& y, float& z) const { @@ -243,6 +255,12 @@ void ICM20948::getMag(float& x, float& y, float& z) const { x = x * AK09916_MAG_LSB; y = y * AK09916_MAG_LSB; z = z * AK09916_MAG_LSB; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = -y; + y = tmp; + z = z; } From a62882418d5155f112930c6dbfb79bfe6817102a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 10:19:18 +0300 Subject: [PATCH 07/20] Change ICM-20948 library code style --- ICM20948.cpp | 1423 ++++++++++++++++++++++++-------------------------- ICM20948.h | 533 +++++++++---------- 2 files changed, 941 insertions(+), 1015 deletions(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index 219c7f7..f0e0199 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -2,798 +2,759 @@ * Code based on: https://github.com/wollewald/ICM20948_WE. * Author: Wolfgang (Wolle) Ewald. * License: MIT (see LICENSE_ICM20948_WE file). + * Further information can be found on: + * https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) + * https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) */ -/******************************************************************** -* This is a library for the 9-axis gyroscope, accelerometer and magnetometer ICM20948. -* -* You'll find an example which should enable you to use the library. -* -* You are free to use it, change it or build on it. In case you like -* it, it would be cool if you give it a star. -* -* If you find bugs, please inform me! -* -* Written by Wolfgang (Wolle) Ewald -* -* Further information can be found on: -* -* https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) -* https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) -* -*********************************************************************/ - #include "ICM20948.h" -/************ Basic Settings ************/ - -bool ICM20948::begin(){ - if(useSPI){ - _spi->begin(); - if (csPin == -1) { - // use default CS pin - #ifdef ESP32 - csPin = _spi->pinSS(); - #else - csPin = SS; - #endif - } - pinMode(csPin, OUTPUT); - digitalWrite(csPin, HIGH); - mySPISettings = SPISettings(7000000, MSBFIRST, SPI_MODE0); - } - currentBank = 0; - - reset(); - if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ - delay(2000); - if(whoAmI() != ICM20948_WHO_AM_I_CONTENT){ - log("Error: incorrect WHO_AM_I value: 0x%02X", whoAmI()); - return false; - } - } - - accRangeFactor = 1.0; - gyrRangeFactor = 1.0; - fifoType = ICM20948_FIFO_ACC; - - sleep(false); - writeRegister8(2, ICM20948_ODR_ALIGN_EN, 1); // aligns ODR - - return true; +/************ Basic Settings ************/ + +bool ICM20948::begin() { + if (useSPI) { + _spi->begin(); + if (csPin == -1) { + // use default CS pin + #ifdef ESP32 + csPin = _spi->pinSS(); + #else + csPin = SS; + #endif + } + pinMode(csPin, OUTPUT); + digitalWrite(csPin, HIGH); + mySPISettings = SPISettings(7000000, MSBFIRST, SPI_MODE0); + } + currentBank = 0; + + reset(); + if (whoAmI() != ICM20948_WHO_AM_I_CONTENT) { + delay(2000); + if (whoAmI() != ICM20948_WHO_AM_I_CONTENT){ + log("Error: incorrect WHO_AM_I value: 0x%02X", whoAmI()); + return false; + } + } + + accRangeFactor = 1.0; + gyrRangeFactor = 1.0; + fifoType = ICM20948_FIFO_ACC; + + sleep(false); + writeRegister8(2, ICM20948_ODR_ALIGN_EN, 1); // aligns ODR + + return true; } uint8_t ICM20948::whoAmI() { - return readRegister8(0, ICM20948_WHO_AM_I); -} - -void ICM20948::enableAcc(bool enAcc){ - regVal = readRegister8(0, ICM20948_PWR_MGMT_2); - if(enAcc){ - regVal &= ~ICM20948_ACC_EN; - } - else{ - regVal |= ICM20948_ACC_EN; - } - writeRegister8(0, ICM20948_PWR_MGMT_2, regVal); -} - -void ICM20948::setAccRange(ICM20948_accRange accRange){ - regVal = readRegister8(2, ICM20948_ACCEL_CONFIG); - regVal &= ~(0x06); - regVal |= (accRange<<1); - writeRegister8(2, ICM20948_ACCEL_CONFIG, regVal); - accRangeFactor = 1<(((buffer[0]) << 8) | buffer[1]) * 1.0; - y = static_cast(((buffer[2]) << 8) | buffer[3]) * 1.0; - z = static_cast(((buffer[4]) << 8) | buffer[5]) * 1.0; - // raw to g - x = x * accRangeFactor / 16384.0; - y = y * accRangeFactor / 16384.0; - z = z * accRangeFactor / 16384.0; - // convert to m/s^2 + x = static_cast(((buffer[0]) << 8) | buffer[1]) * 1.0; + y = static_cast(((buffer[2]) << 8) | buffer[3]) * 1.0; + z = static_cast(((buffer[4]) << 8) | buffer[5]) * 1.0; + // raw to g + x = x * accRangeFactor / 16384.0; + y = y * accRangeFactor / 16384.0; + z = z * accRangeFactor / 16384.0; + // convert to m/s^2 static constexpr float G = 9.80665f; - x = x * G; - y = y * G; - z = z * G; - // orient axes to match MPU9250 library - // TODO: switch to internal accel and gyro axes - double tmp = x; - x = y; - y = tmp; - z = -z; + x = x * G; + y = y * G; + z = z * G; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = y; + y = tmp; + z = -z; } float ICM20948::getTemp() const { - int16_t rawTemp = static_cast(((buffer[12]) << 8) | buffer[13]); - float tmp = (rawTemp*1.0 - ICM20948_ROOM_TEMP_OFFSET)/ICM20948_T_SENSITIVITY + 21.0; - return tmp; + int16_t rawTemp = static_cast(((buffer[12]) << 8) | buffer[13]); + float tmp = (rawTemp*1.0 - ICM20948_ROOM_TEMP_OFFSET)/ICM20948_T_SENSITIVITY + 21.0; + return tmp; } void ICM20948::getGyro(float& x, float& y, float& z) const { - x = (int16_t)(((buffer[6]) << 8) | buffer[7]) * 1.0; - y = (int16_t)(((buffer[8]) << 8) | buffer[9]) * 1.0; - z = (int16_t)(((buffer[10]) << 8) | buffer[11]) * 1.0; - // raw to dps - x = x * gyrRangeFactor * 250.0 / 32768.0; - y = y * gyrRangeFactor * 250.0 / 32768.0; - z = z * gyrRangeFactor * 250.0 / 32768.0; - // convert to rad/s - x = x * DEG_TO_RAD; - y = y * DEG_TO_RAD; - z = z * DEG_TO_RAD; - // orient axes to match MPU9250 library - // TODO: switch to internal accel and gyro axes - double tmp = x; - x = y; - y = tmp; - z = -z; + x = (int16_t)(((buffer[6]) << 8) | buffer[7]) * 1.0; + y = (int16_t)(((buffer[8]) << 8) | buffer[9]) * 1.0; + z = (int16_t)(((buffer[10]) << 8) | buffer[11]) * 1.0; + // raw to dps + x = x * gyrRangeFactor * 250.0 / 32768.0; + y = y * gyrRangeFactor * 250.0 / 32768.0; + z = z * gyrRangeFactor * 250.0 / 32768.0; + // convert to rad/s + x = x * DEG_TO_RAD; + y = y * DEG_TO_RAD; + z = z * DEG_TO_RAD; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = y; + y = tmp; + z = -z; } void ICM20948::getMag(float& x, float& y, float& z) const { - x = static_cast((buffer[15]) << 8) | buffer[14]; - y = static_cast((buffer[17]) << 8) | buffer[16]; - z = static_cast((buffer[19]) << 8) | buffer[18]; - // correct values - x = x * AK09916_MAG_LSB; - y = y * AK09916_MAG_LSB; - z = z * AK09916_MAG_LSB; - // orient axes to match MPU9250 library - // TODO: switch to internal accel and gyro axes - double tmp = x; - x = -y; - y = tmp; - z = z; + x = static_cast((buffer[15]) << 8) | buffer[14]; + y = static_cast((buffer[17]) << 8) | buffer[16]; + z = static_cast((buffer[19]) << 8) | buffer[18]; + // correct values + x = x * AK09916_MAG_LSB; + y = y * AK09916_MAG_LSB; + z = z * AK09916_MAG_LSB; + // orient axes to match MPU9250 library + // TODO: switch to internal accel and gyro axes + double tmp = x; + x = -y; + y = tmp; + z = z; } -/********* Power, Sleep, Standby *********/ +/********* Power, Sleep, Standby *********/ void ICM20948::enableCycle(ICM20948_cycle cycle){ - regVal = readRegister8(0, ICM20948_LP_CONFIG); - regVal &= 0x0F; - regVal |= cycle; - - writeRegister8(0, ICM20948_LP_CONFIG, regVal); + regVal = readRegister8(0, ICM20948_LP_CONFIG); + regVal &= 0x0F; + regVal |= cycle; + + writeRegister8(0, ICM20948_LP_CONFIG, regVal); } -void ICM20948::enableLowPower(bool enLP){ // vielleicht besser privat???? - regVal = readRegister8(0, ICM20948_PWR_MGMT_1); - if(enLP){ - regVal |= ICM20948_LP_EN; - } - else{ - regVal &= ~ICM20948_LP_EN; - } - writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); +void ICM20948::enableLowPower(bool enLP) { // vielleicht besser privat???? + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + if (enLP) { + regVal |= ICM20948_LP_EN; + } else { + regVal &= ~ICM20948_LP_EN; + } + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); } -void ICM20948::setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg){ - writeRegister8(2, ICM20948_GYRO_CONFIG_2, avg); +void ICM20948::setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg) { + writeRegister8(2, ICM20948_GYRO_CONFIG_2, avg); } -void ICM20948::setAccAverageInCycleMode(ICM20948_accAvgLowPower avg){ - writeRegister8(2, ICM20948_ACCEL_CONFIG_2, avg); +void ICM20948::setAccAverageInCycleMode(ICM20948_accAvgLowPower avg) { + writeRegister8(2, ICM20948_ACCEL_CONFIG_2, avg); } -void ICM20948::sleep(bool sleep){ - regVal = readRegister8(0, ICM20948_PWR_MGMT_1); - if(sleep){ - regVal |= ICM20948_SLEEP; - } - else{ - regVal &= ~ICM20948_SLEEP; - } - writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); +void ICM20948::sleep(bool sleep) { + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + if(sleep) { + regVal |= ICM20948_SLEEP; + } else { + regVal &= ~ICM20948_SLEEP; + } + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); } /************** Interrupts ***************/ -void ICM20948::setIntPinPolarity(ICM20948_intPinPol pol){ - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - if(pol){ - regVal |= ICM20948_INT1_ACTL; - } - else{ - regVal &= ~ICM20948_INT1_ACTL; - } - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); -} - -void ICM20948::enableIntLatch(bool latch){ - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - if(latch){ - regVal |= ICM20948_INT_1_LATCH_EN; - } - else{ - regVal &= ~ICM20948_INT_1_LATCH_EN; - } - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); -} - -void ICM20948::enableClearIntByAnyRead(bool clearByAnyRead){ - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - if(clearByAnyRead){ - regVal |= ICM20948_INT_ANYRD_2CLEAR; - } - else{ - regVal &= ~ICM20948_INT_ANYRD_2CLEAR; - } - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); -} - -void ICM20948::setFSyncIntPolarity(ICM20948_intPinPol pol){ - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - if(pol){ - regVal |= ICM20948_ACTL_FSYNC; - } - else{ - regVal &= ~ICM20948_ACTL_FSYNC; - } - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); -} - -void ICM20948::enableInterrupt(ICM20948_intType intType){ - switch(intType){ - case ICM20948_FSYNC_INT: - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - regVal |= ICM20948_FSYNC_INT_MODE_EN; - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); // enable FSYNC as interrupt pin - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal |= 0x80; - writeRegister8(0, ICM20948_INT_ENABLE, regVal); // enable wake on FSYNC interrupt - break; - - case ICM20948_WOM_INT: - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal |= 0x08; - writeRegister8(0, ICM20948_INT_ENABLE, regVal); - regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); - regVal |= 0x02; - writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); - break; - - case ICM20948_DMP_INT: - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal |= 0x02; - writeRegister8(0, ICM20948_INT_ENABLE, regVal); - break; - - case ICM20948_DATA_READY_INT: - writeRegister8(0, ICM20948_INT_ENABLE_1, 0x01); - break; - - case ICM20948_FIFO_OVF_INT: - writeRegister8(0, ICM20948_INT_ENABLE_2, 0x01); - break; - - case ICM20948_FIFO_WM_INT: - writeRegister8(0, ICM20948_INT_ENABLE_3, 0x01); - break; - } -} - -void ICM20948::disableInterrupt(ICM20948_intType intType){ - switch(intType){ - case ICM20948_FSYNC_INT: - regVal = readRegister8(0, ICM20948_INT_PIN_CFG); - regVal &= ~ICM20948_FSYNC_INT_MODE_EN; - writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal &= ~(0x80); - writeRegister8(0, ICM20948_INT_ENABLE, regVal); - break; - - case ICM20948_WOM_INT: - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal &= ~(0x08); - writeRegister8(0, ICM20948_INT_ENABLE, regVal); - regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); - regVal &= ~(0x02); - writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); - break; - - case ICM20948_DMP_INT: - regVal = readRegister8(0, ICM20948_INT_ENABLE); - regVal &= ~(0x02); - writeRegister8(0, ICM20948_INT_ENABLE, regVal); - break; - - case ICM20948_DATA_READY_INT: - writeRegister8(0, ICM20948_INT_ENABLE_1, 0x00); - break; - - case ICM20948_FIFO_OVF_INT: - writeRegister8(0, ICM20948_INT_ENABLE_2, 0x00); - break; - - case ICM20948_FIFO_WM_INT: - writeRegister8(0, ICM20948_INT_ENABLE_3, 0x00); - break; - } -} - -uint8_t ICM20948::readAndClearInterrupts(){ - uint8_t intSource = 0; - regVal = readRegister8(0, ICM20948_I2C_MST_STATUS); - if(regVal & 0x80){ - intSource |= 0x01; - } - regVal = readRegister8(0, ICM20948_INT_STATUS); - if(regVal & 0x08){ - intSource |= 0x02; - } - if(regVal & 0x02){ - intSource |= 0x04; - } - regVal = readRegister8(0, ICM20948_INT_STATUS_1); - if(regVal & 0x01){ - intSource |= 0x08; - } - regVal = readRegister8(0, ICM20948_INT_STATUS_2); - if(regVal & 0x01){ - intSource |= 0x10; - } - regVal = readRegister8(0, ICM20948_INT_STATUS_3); - if(regVal & 0x01){ - intSource |= 0x20; - } - return intSource; -} - -bool ICM20948::checkInterrupt(uint8_t source, ICM20948_intType type){ - source &= type; - return source; -} -void ICM20948::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn){ - regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); - if(womCompEn){ - regVal |= 0x01; - } - else{ - regVal &= ~(0x01); - } - writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); - writeRegister8(2, ICM20948_ACCEL_WOM_THR, womThresh); +void ICM20948::setIntPinPolarity(ICM20948_intPinPol pol) { + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if (pol) { + regVal |= ICM20948_INT1_ACTL; + } else { + regVal &= ~ICM20948_INT1_ACTL; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948::enableIntLatch(bool latch) { + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if (latch) { + regVal |= ICM20948_INT_1_LATCH_EN; + } else { + regVal &= ~ICM20948_INT_1_LATCH_EN; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948::enableClearIntByAnyRead(bool clearByAnyRead) { + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if (clearByAnyRead){ + regVal |= ICM20948_INT_ANYRD_2CLEAR; + } else { + regVal &= ~ICM20948_INT_ANYRD_2CLEAR; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948::setFSyncIntPolarity(ICM20948_intPinPol pol) { + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + if (pol) { + regVal |= ICM20948_ACTL_FSYNC; + } else{ + regVal &= ~ICM20948_ACTL_FSYNC; + } + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); +} + +void ICM20948::enableInterrupt(ICM20948_intType intType) { + switch(intType){ + case ICM20948_FSYNC_INT: + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + regVal |= ICM20948_FSYNC_INT_MODE_EN; + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); // enable FSYNC as interrupt pin + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x80; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); // enable wake on FSYNC interrupt + break; + + case ICM20948_WOM_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x08; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + regVal |= 0x02; + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + break; + + case ICM20948_DMP_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal |= 0x02; + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_DATA_READY_INT: + writeRegister8(0, ICM20948_INT_ENABLE_1, 0x01); + break; + + case ICM20948_FIFO_OVF_INT: + writeRegister8(0, ICM20948_INT_ENABLE_2, 0x01); + break; + + case ICM20948_FIFO_WM_INT: + writeRegister8(0, ICM20948_INT_ENABLE_3, 0x01); + break; + } +} + +void ICM20948::disableInterrupt(ICM20948_intType intType) { + switch (intType) { + case ICM20948_FSYNC_INT: + regVal = readRegister8(0, ICM20948_INT_PIN_CFG); + regVal &= ~ICM20948_FSYNC_INT_MODE_EN; + writeRegister8(0, ICM20948_INT_PIN_CFG, regVal); + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x80); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_WOM_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x08); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + regVal &= ~(0x02); + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + break; + + case ICM20948_DMP_INT: + regVal = readRegister8(0, ICM20948_INT_ENABLE); + regVal &= ~(0x02); + writeRegister8(0, ICM20948_INT_ENABLE, regVal); + break; + + case ICM20948_DATA_READY_INT: + writeRegister8(0, ICM20948_INT_ENABLE_1, 0x00); + break; + + case ICM20948_FIFO_OVF_INT: + writeRegister8(0, ICM20948_INT_ENABLE_2, 0x00); + break; + + case ICM20948_FIFO_WM_INT: + writeRegister8(0, ICM20948_INT_ENABLE_3, 0x00); + break; + } +} + +uint8_t ICM20948::readAndClearInterrupts() { + uint8_t intSource = 0; + regVal = readRegister8(0, ICM20948_I2C_MST_STATUS); + if (regVal & 0x80) { + intSource |= 0x01; + } + regVal = readRegister8(0, ICM20948_INT_STATUS); + if (regVal & 0x08) { + intSource |= 0x02; + } + if (regVal & 0x02) { + intSource |= 0x04; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_1); + if (regVal & 0x01) { + intSource |= 0x08; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_2); + if (regVal & 0x01) { + intSource |= 0x10; + } + regVal = readRegister8(0, ICM20948_INT_STATUS_3); + if (regVal & 0x01) { + intSource |= 0x20; + } + return intSource; +} + +bool ICM20948::checkInterrupt(uint8_t source, ICM20948_intType type) { + source &= type; + return source; +} + +void ICM20948::setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn) { + regVal = readRegister8(2, ICM20948_ACCEL_INTEL_CTRL); + if(womCompEn) { + regVal |= 0x01; + } else { + regVal &= ~(0x01); + } + writeRegister8(2, ICM20948_ACCEL_INTEL_CTRL, regVal); + writeRegister8(2, ICM20948_ACCEL_WOM_THR, womThresh); } /***************** FIFO ******************/ -void ICM20948::enableFifo(bool fifo){ - regVal = readRegister8(0, ICM20948_USER_CTRL); - if(fifo){ - regVal |= ICM20948_FIFO_EN; - } - else{ - regVal &= ~ICM20948_FIFO_EN; - } - writeRegister8(0, ICM20948_USER_CTRL, regVal); -} - -void ICM20948::setFifoMode(ICM20948_fifoMode mode){ - if(mode){ - regVal = 0x01; - } - else{ - regVal = 0x00; - } - writeRegister8(0, ICM20948_FIFO_MODE, regVal); -} - -void ICM20948::startFifo(ICM20948_fifoType fifo){ - fifoType = fifo; - writeRegister8(0, ICM20948_FIFO_EN_2, fifoType); -} - -void ICM20948::stopFifo(){ - writeRegister8(0, ICM20948_FIFO_EN_2, 0); -} - -void ICM20948::resetFifo(){ - writeRegister8(0, ICM20948_FIFO_RST, 0x01); - writeRegister8(0, ICM20948_FIFO_RST, 0x00); -} - -int16_t ICM20948::getFifoCount(){ - int16_t regVal16 = static_cast(readRegister16(0, ICM20948_FIFO_COUNT)); - return regVal16; -} - -int16_t ICM20948::getNumberOfFifoDataSets(){ - int16_t numberOfSets = getFifoCount(); - - if((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)){ - numberOfSets /= 6; - } - else if(fifoType==ICM20948_FIFO_ACC_GYR){ - numberOfSets /= 12; - } - - return numberOfSets; -} - -void ICM20948::findFifoBegin(){ - uint16_t count = getFifoCount(); - int16_t start = 0; - - if((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)){ - start = count%6; - for(int i=0; i(readAK09916Register16(AK09916_WIA_1)); -} - -void ICM20948::setMagOpMode(AK09916_opMode opMode){ - writeAK09916Register8(AK09916_CNTL_2, opMode); - delay(10); - if(opMode!=AK09916_PWR_DOWN){ - enableMagDataRead(AK09916_HXL, 0x08); - } -} - -void ICM20948::resetMag(){ - writeAK09916Register8(AK09916_CNTL_3, 0x01); - delay(100); -} - -/************************************************ +void ICM20948::enableFifo(bool fifo) { + regVal = readRegister8(0, ICM20948_USER_CTRL); + if (fifo) { + regVal |= ICM20948_FIFO_EN; + } else { + regVal &= ~ICM20948_FIFO_EN; + } + writeRegister8(0, ICM20948_USER_CTRL, regVal); +} + +void ICM20948::setFifoMode(ICM20948_fifoMode mode) { + if (mode) { + regVal = 0x01; + } else { + regVal = 0x00; + } + writeRegister8(0, ICM20948_FIFO_MODE, regVal); +} + +void ICM20948::startFifo(ICM20948_fifoType fifo) { + fifoType = fifo; + writeRegister8(0, ICM20948_FIFO_EN_2, fifoType); +} + +void ICM20948::stopFifo() { + writeRegister8(0, ICM20948_FIFO_EN_2, 0); +} + +void ICM20948::resetFifo() { + writeRegister8(0, ICM20948_FIFO_RST, 0x01); + writeRegister8(0, ICM20948_FIFO_RST, 0x00); +} + +int16_t ICM20948::getFifoCount() { + int16_t regVal16 = static_cast(readRegister16(0, ICM20948_FIFO_COUNT)); + return regVal16; +} + +int16_t ICM20948::getNumberOfFifoDataSets() { + int16_t numberOfSets = getFifoCount(); + + if ((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)) { + numberOfSets /= 6; + } else if (fifoType==ICM20948_FIFO_ACC_GYR) { + numberOfSets /= 12; + } + + return numberOfSets; +} + +void ICM20948::findFifoBegin() { + uint16_t count = getFifoCount(); + int16_t start = 0; + + if ((fifoType == ICM20948_FIFO_ACC) || (fifoType == ICM20948_FIFO_GYR)) { + start = count%6; + for(int i=0; i(readAK09916Register16(AK09916_WIA_1)); +} + +void ICM20948::setMagOpMode(AK09916_opMode opMode) { + writeAK09916Register8(AK09916_CNTL_2, opMode); + delay(10); + if(opMode!=AK09916_PWR_DOWN){ + enableMagDataRead(AK09916_HXL, 0x08); + } +} + +void ICM20948::resetMag() { + writeAK09916Register8(AK09916_CNTL_3, 0x01); + delay(100); +} + +/************************************************ Private Functions *************************************************/ -void ICM20948::setClockToAutoSelect(){ - regVal = readRegister8(0, ICM20948_PWR_MGMT_1); - regVal |= 0x01; - writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); - delay(10); -} - -void ICM20948::switchBank(uint8_t newBank){ - if(newBank != currentBank){ - currentBank = newBank; - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(ICM20948_REG_BANK_SEL); - _wire->write(currentBank<<4); - _wire->endTransmission(); - } - else{ - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(ICM20948_REG_BANK_SEL); - _spi->transfer(currentBank<<4); - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } - } -} - -void ICM20948::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val){ - switchBank(bank); - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(reg); - _wire->write(val); - _wire->endTransmission(); - } - else{ - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - _spi->transfer(val); - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } -} - -void ICM20948::writeRegister16(uint8_t bank, uint8_t reg, int16_t val){ - switchBank(bank); - int8_t MSByte = static_cast((val>>8) & 0xFF); - uint8_t LSByte = val & 0xFF; - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(reg); - _wire->write(MSByte); - _wire->write(LSByte); - _wire->endTransmission(); - } - else{ - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - _spi->transfer(val); - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } -} - -uint8_t ICM20948::readRegister8(uint8_t bank, uint8_t reg){ - switchBank(bank); - uint8_t regValue = 0; - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(reg); - _wire->endTransmission(false); - _wire->requestFrom(i2cAddress, static_cast(1)); - if(_wire->available()){ - regValue = _wire->read(); - } - } - else{ - reg |= 0x80; - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - regValue = _spi->transfer(0x00); - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } - return regValue; -} - -int16_t ICM20948::readRegister16(uint8_t bank, uint8_t reg){ - switchBank(bank); - uint8_t MSByte = 0, LSByte = 0; - int16_t reg16Val = 0; - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(reg); - _wire->endTransmission(false); - _wire->requestFrom(i2cAddress, static_cast(2)); - if(_wire->available()){ - MSByte = _wire->read(); - LSByte = _wire->read(); - } - } - else{ - reg = reg | 0x80; - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - MSByte = _spi->transfer(0x00); - LSByte = _spi->transfer(0x00); - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } - reg16Val = (MSByte<<8) + LSByte; - return reg16Val; -} - -void ICM20948::readAllData(uint8_t* data){ - switchBank(0); - - if(!useSPI){ - _wire->beginTransmission(i2cAddress); - _wire->write(ICM20948_ACCEL_OUT); - _wire->endTransmission(false); - _wire->requestFrom(i2cAddress, static_cast(20)); - if(_wire->available()){ - for(int i=0; i<20; i++){ - data[i] = _wire->read(); - } - } - } - else{ - uint8_t reg = ICM20948_ACCEL_OUT | 0x80; - _spi->beginTransaction(mySPISettings); - digitalWrite(csPin, LOW); - _spi->transfer(reg); - for(int i=0; i<20; i++){ - data[i] = _spi->transfer(0x00); - } - digitalWrite(csPin, HIGH); - _spi->endTransaction(); - } -} - -void ICM20948::writeAK09916Register8(uint8_t reg, uint8_t val){ - writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS); // write AK09916 - writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be written to - writeRegister8(3, ICM20948_I2C_SLV0_DO, val); -} - - -uint8_t ICM20948::readAK09916Register8(uint8_t reg){ - enableMagDataRead(reg, 0x01); - enableMagDataRead(AK09916_HXL, 0x08); - regVal = readRegister8(0, ICM20948_EXT_SLV_SENS_DATA_00); - return regVal; -} - -int16_t ICM20948::readAK09916Register16(uint8_t reg){ - int16_t regValue = 0; - enableMagDataRead(reg, 0x02); - regValue = readRegister16(0, ICM20948_EXT_SLV_SENS_DATA_00); - enableMagDataRead(AK09916_HXL, 0x08); - return regValue; +void ICM20948::setClockToAutoSelect() { + regVal = readRegister8(0, ICM20948_PWR_MGMT_1); + regVal |= 0x01; + writeRegister8(0, ICM20948_PWR_MGMT_1, regVal); + delay(10); +} + +void ICM20948::switchBank(uint8_t newBank) { + if (newBank != currentBank) { + currentBank = newBank; + if (!useSPI) { + _wire->beginTransmission(i2cAddress); + _wire->write(ICM20948_REG_BANK_SEL); + _wire->write(currentBank<<4); + _wire->endTransmission(); + } else { + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(ICM20948_REG_BANK_SEL); + _spi->transfer(currentBank<<4); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + } +} + +void ICM20948::writeRegister8(uint8_t bank, uint8_t reg, uint8_t val) { + switchBank(bank); + + if (!useSPI) { + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->write(val); + _wire->endTransmission(); + } else { + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + _spi->transfer(val); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +void ICM20948::writeRegister16(uint8_t bank, uint8_t reg, int16_t val) { + switchBank(bank); + int8_t MSByte = static_cast((val>>8) & 0xFF); + uint8_t LSByte = val & 0xFF; + + if (!useSPI) { + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->write(MSByte); + _wire->write(LSByte); + _wire->endTransmission(); + } else { + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + _spi->transfer(val); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +uint8_t ICM20948::readRegister8(uint8_t bank, uint8_t reg) { + switchBank(bank); + uint8_t regValue = 0; + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(1)); + if(_wire->available()){ + regValue = _wire->read(); + } + } else { + reg |= 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + regValue = _spi->transfer(0x00); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + return regValue; +} + +int16_t ICM20948::readRegister16(uint8_t bank, uint8_t reg) { + switchBank(bank); + uint8_t MSByte = 0, LSByte = 0; + int16_t reg16Val = 0; + + if (!useSPI) { + _wire->beginTransmission(i2cAddress); + _wire->write(reg); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(2)); + if(_wire->available()){ + MSByte = _wire->read(); + LSByte = _wire->read(); + } + } else { + reg = reg | 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + MSByte = _spi->transfer(0x00); + LSByte = _spi->transfer(0x00); + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } + reg16Val = (MSByte<<8) + LSByte; + return reg16Val; +} + +void ICM20948::readAllData(uint8_t* data) { + switchBank(0); + + if(!useSPI){ + _wire->beginTransmission(i2cAddress); + _wire->write(ICM20948_ACCEL_OUT); + _wire->endTransmission(false); + _wire->requestFrom(i2cAddress, static_cast(20)); + if(_wire->available()){ + for(int i=0; i<20; i++){ + data[i] = _wire->read(); + } + } + } + else{ + uint8_t reg = ICM20948_ACCEL_OUT | 0x80; + _spi->beginTransaction(mySPISettings); + digitalWrite(csPin, LOW); + _spi->transfer(reg); + for(int i=0; i<20; i++){ + data[i] = _spi->transfer(0x00); + } + digitalWrite(csPin, HIGH); + _spi->endTransaction(); + } +} + +void ICM20948::writeAK09916Register8(uint8_t reg, uint8_t val) { + writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS); // write AK09916 + writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be written to + writeRegister8(3, ICM20948_I2C_SLV0_DO, val); +} + + +uint8_t ICM20948::readAK09916Register8(uint8_t reg) { + enableMagDataRead(reg, 0x01); + enableMagDataRead(AK09916_HXL, 0x08); + regVal = readRegister8(0, ICM20948_EXT_SLV_SENS_DATA_00); + return regVal; +} + +int16_t ICM20948::readAK09916Register16(uint8_t reg) { + int16_t regValue = 0; + enableMagDataRead(reg, 0x02); + regValue = readRegister16(0, ICM20948_EXT_SLV_SENS_DATA_00); + enableMagDataRead(AK09916_HXL, 0x08); + return regValue; } void ICM20948::reset() { - writeRegister8(0, ICM20948_PWR_MGMT_1, ICM20948_RESET); - delay(10); // wait for registers to reset + writeRegister8(0, ICM20948_PWR_MGMT_1, ICM20948_RESET); + delay(10); // wait for registers to reset } -void ICM20948::enableI2CMaster(){ - writeRegister8(0, ICM20948_USER_CTRL, ICM20948_I2C_MST_EN); //enable I2C master - writeRegister8(3, ICM20948_I2C_MST_CTRL, 0x07); // set I2C clock to 345.60 kHz - delay(10); +void ICM20948::enableI2CMaster() { + writeRegister8(0, ICM20948_USER_CTRL, ICM20948_I2C_MST_EN); //enable I2C master + writeRegister8(3, ICM20948_I2C_MST_CTRL, 0x07); // set I2C clock to 345.60 kHz + delay(10); } -void ICM20948::i2cMasterReset(){ - uint8_t regVal = readRegister8(0, ICM20948_USER_CTRL); - regVal |= ICM20948_I2C_MST_RST; - writeRegister8(0, ICM20948_USER_CTRL, regVal); - delay(10); +void ICM20948::i2cMasterReset() { + uint8_t regVal = readRegister8(0, ICM20948_USER_CTRL); + regVal |= ICM20948_I2C_MST_RST; + writeRegister8(0, ICM20948_USER_CTRL, regVal); + delay(10); } -void ICM20948::enableMagDataRead(uint8_t reg, uint8_t bytes){ - writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS | AK09916_READ); // read AK09916 - writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be read - writeRegister8(3, ICM20948_I2C_SLV0_CTRL, 0x80 | bytes); //enable read | number of byte - delay(10); +void ICM20948::enableMagDataRead(uint8_t reg, uint8_t bytes) { + writeRegister8(3, ICM20948_I2C_SLV0_ADDR, AK09916_ADDRESS | AK09916_READ); // read AK09916 + writeRegister8(3, ICM20948_I2C_SLV0_REG, reg); // define AK09916 register to be read + writeRegister8(3, ICM20948_I2C_SLV0_CTRL, 0x80 | bytes); //enable read | number of byte + delay(10); } - \ No newline at end of file diff --git a/ICM20948.h b/ICM20948.h index 7fbe3a1..78506f7 100644 --- a/ICM20948.h +++ b/ICM20948.h @@ -2,339 +2,304 @@ * Code based on: https://github.com/wollewald/ICM20948_WE. * Author: Wolfgang (Wolle) Ewald. * License: MIT (see LICENSE_ICM20948_WE file). -*/ - -/****************************************************************************** - * - * This is a library for the 9-axis gyroscope, accelerometer and magnetometer ICM20948. - * - * You'll find several example sketches which should enable you to use the library. - * - * You are free to use it, change it or build on it. In case you like it, it would - * be cool if you give it a star. - * - * If you find bugs, please inform me! - * - * Written by Wolfgang (Wolle) Ewald - * * Further information can be found on: - * * https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) * https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) - * - * - ******************************************************************************/ +*/ #pragma once -#if (ARDUINO >= 100) - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - +#include "Arduino.h" #include #include + #include "IMU.h" #include "logger.h" /* Enums */ typedef enum ICM20948_CYCLE { - ICM20948_NO_CYCLE = 0x00, - ICM20948_GYR_CYCLE = 0x10, - ICM20948_ACC_CYCLE = 0x20, - ICM20948_ACC_GYR_CYCLE = 0x30, - ICM20948_ACC_GYR_I2C_MST_CYCLE = 0x70 + ICM20948_NO_CYCLE = 0x00, + ICM20948_GYR_CYCLE = 0x10, + ICM20948_ACC_CYCLE = 0x20, + ICM20948_ACC_GYR_CYCLE = 0x30, + ICM20948_ACC_GYR_I2C_MST_CYCLE = 0x70 } ICM20948_cycle; typedef enum ICM20948_INT_PIN_POL { - ICM20948_ACT_HIGH, ICM20948_ACT_LOW + ICM20948_ACT_HIGH, ICM20948_ACT_LOW } ICM20948_intPinPol; typedef enum ICM20948_INT_TYPE { - ICM20948_FSYNC_INT = 0x01, - ICM20948_WOM_INT = 0x02, - ICM20948_DMP_INT = 0x04, - ICM20948_DATA_READY_INT = 0x08, - ICM20948_FIFO_OVF_INT = 0x10, - ICM20948_FIFO_WM_INT = 0x20 + ICM20948_FSYNC_INT = 0x01, + ICM20948_WOM_INT = 0x02, + ICM20948_DMP_INT = 0x04, + ICM20948_DATA_READY_INT = 0x08, + ICM20948_FIFO_OVF_INT = 0x10, + ICM20948_FIFO_WM_INT = 0x20 } ICM20948_intType; typedef enum ICM20948_FIFO_TYPE { - ICM20948_FIFO_ACC = 0x10, - ICM20948_FIFO_GYR = 0x0E, - ICM20948_FIFO_ACC_GYR = 0x1E + ICM20948_FIFO_ACC = 0x10, + ICM20948_FIFO_GYR = 0x0E, + ICM20948_FIFO_ACC_GYR = 0x1E } ICM20948_fifoType; typedef enum ICM20948_FIFO_MODE_CHOICE { - ICM20948_CONTINUOUS, ICM20948_STOP_WHEN_FULL + ICM20948_CONTINUOUS, ICM20948_STOP_WHEN_FULL } ICM20948_fifoMode; typedef enum ICM20948_GYRO_RANGE { - ICM20948_GYRO_RANGE_250, ICM20948_GYRO_RANGE_500, ICM20948_GYRO_RANGE_1000, ICM20948_GYRO_RANGE_2000 + ICM20948_GYRO_RANGE_250, ICM20948_GYRO_RANGE_500, ICM20948_GYRO_RANGE_1000, ICM20948_GYRO_RANGE_2000 } ICM20948_gyroRange; typedef enum ICM20948_DLPF { - ICM20948_DLPF_0, ICM20948_DLPF_1, ICM20948_DLPF_2, ICM20948_DLPF_3, ICM20948_DLPF_4, ICM20948_DLPF_5, - ICM20948_DLPF_6, ICM20948_DLPF_7, ICM20948_DLPF_OFF + ICM20948_DLPF_0, ICM20948_DLPF_1, ICM20948_DLPF_2, ICM20948_DLPF_3, ICM20948_DLPF_4, ICM20948_DLPF_5, + ICM20948_DLPF_6, ICM20948_DLPF_7, ICM20948_DLPF_OFF } ICM20948_dlpf; typedef enum ICM20948_GYRO_AVG_LOW_PWR { - ICM20948_GYR_AVG_1, ICM20948_GYR_AVG_2, ICM20948_GYR_AVG_4, ICM20948_GYR_AVG_8, ICM20948_GYR_AVG_16, - ICM20948_GYR_AVG_32, ICM20948_GYR_AVG_64, ICM20948_GYR_AVG_128 + ICM20948_GYR_AVG_1, ICM20948_GYR_AVG_2, ICM20948_GYR_AVG_4, ICM20948_GYR_AVG_8, ICM20948_GYR_AVG_16, + ICM20948_GYR_AVG_32, ICM20948_GYR_AVG_64, ICM20948_GYR_AVG_128 } ICM20948_gyroAvgLowPower; typedef enum ICM20948_ACC_RANGE { - ICM20948_ACC_RANGE_2G, ICM20948_ACC_RANGE_4G, ICM20948_ACC_RANGE_8G, ICM20948_ACC_RANGE_16G + ICM20948_ACC_RANGE_2G, ICM20948_ACC_RANGE_4G, ICM20948_ACC_RANGE_8G, ICM20948_ACC_RANGE_16G } ICM20948_accRange; typedef enum ICM20948_ACC_AVG_LOW_PWR { - ICM20948_ACC_AVG_4, ICM20948_ACC_AVG_8, ICM20948_ACC_AVG_16, ICM20948_ACC_AVG_32 + ICM20948_ACC_AVG_4, ICM20948_ACC_AVG_8, ICM20948_ACC_AVG_16, ICM20948_ACC_AVG_32 } ICM20948_accAvgLowPower; typedef enum ICM20948_WOM_COMP { - ICM20948_WOM_COMP_DISABLE, ICM20948_WOM_COMP_ENABLE + ICM20948_WOM_COMP_DISABLE, ICM20948_WOM_COMP_ENABLE } ICM20948_womCompEn; typedef enum AK09916_OP_MODE { - AK09916_PWR_DOWN = 0x00, - AK09916_TRIGGER_MODE = 0x01, - AK09916_CONT_MODE_10HZ = 0x02, - AK09916_CONT_MODE_20HZ = 0x04, - AK09916_CONT_MODE_50HZ = 0x06, - AK09916_CONT_MODE_100HZ = 0x08 + AK09916_PWR_DOWN = 0x00, + AK09916_TRIGGER_MODE = 0x01, + AK09916_CONT_MODE_10HZ = 0x02, + AK09916_CONT_MODE_20HZ = 0x04, + AK09916_CONT_MODE_50HZ = 0x06, + AK09916_CONT_MODE_100HZ = 0x08 } AK09916_opMode; class ICM20948 : public IMUInterface, public Logger { - public: - /* constants */ - - static constexpr uint8_t AK09916_ADDRESS {0x0C}; - - /* Registers ICM20948 USER BANK 0*/ - static constexpr uint8_t ICM20948_WHO_AM_I {0x00}; - static constexpr uint8_t ICM20948_USER_CTRL {0x03}; - static constexpr uint8_t ICM20948_LP_CONFIG {0x05}; - static constexpr uint8_t ICM20948_PWR_MGMT_1 {0x06}; - static constexpr uint8_t ICM20948_PWR_MGMT_2 {0x07}; - static constexpr uint8_t ICM20948_INT_PIN_CFG {0x0F}; - static constexpr uint8_t ICM20948_INT_ENABLE {0x10}; - static constexpr uint8_t ICM20948_INT_ENABLE_1 {0x11}; - static constexpr uint8_t ICM20948_INT_ENABLE_2 {0x12}; - static constexpr uint8_t ICM20948_INT_ENABLE_3 {0x13}; - static constexpr uint8_t ICM20948_I2C_MST_STATUS {0x17}; - static constexpr uint8_t ICM20948_INT_STATUS {0x19}; - static constexpr uint8_t ICM20948_INT_STATUS_1 {0x1A}; - static constexpr uint8_t ICM20948_INT_STATUS_2 {0x1B}; - static constexpr uint8_t ICM20948_INT_STATUS_3 {0x1C}; - static constexpr uint8_t ICM20948_DELAY_TIME_H {0x28}; - static constexpr uint8_t ICM20948_DELAY_TIME_L {0x29}; - static constexpr uint8_t ICM20948_ACCEL_OUT {0x2D}; // accel data registers begin - static constexpr uint8_t ICM20948_GYRO_OUT {0x33}; // gyro data registers begin - static constexpr uint8_t ICM20948_TEMP_OUT {0x39}; - static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_00{0x3B}; - static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_01{0x3C}; - static constexpr uint8_t ICM20948_FIFO_EN_1 {0x66}; - static constexpr uint8_t ICM20948_FIFO_EN_2 {0x67}; - static constexpr uint8_t ICM20948_FIFO_RST {0x68}; - static constexpr uint8_t ICM20948_FIFO_MODE {0x69}; - static constexpr uint8_t ICM20948_FIFO_COUNT {0x70}; - static constexpr uint8_t ICM20948_FIFO_R_W {0x72}; - static constexpr uint8_t ICM20948_DATA_RDY_STATUS {0x74}; - static constexpr uint8_t ICM20948_FIFO_CFG {0x76}; - - /* Registers ICM20948 USER BANK 1*/ - static constexpr uint8_t ICM20948_SELF_TEST_X_GYRO {0x02}; - static constexpr uint8_t ICM20948_SELF_TEST_Y_GYRO {0x03}; - static constexpr uint8_t ICM20948_SELF_TEST_Z_GYRO {0x04}; - static constexpr uint8_t ICM20948_SELF_TEST_X_ACCEL {0x0E}; - static constexpr uint8_t ICM20948_SELF_TEST_Y_ACCEL {0x0F}; - static constexpr uint8_t ICM20948_SELF_TEST_Z_ACCEL {0x10}; - static constexpr uint8_t ICM20948_XA_OFFS_H {0x14}; - static constexpr uint8_t ICM20948_XA_OFFS_L {0x15}; - static constexpr uint8_t ICM20948_YA_OFFS_H {0x17}; - static constexpr uint8_t ICM20948_YA_OFFS_L {0x18}; - static constexpr uint8_t ICM20948_ZA_OFFS_H {0x1A}; - static constexpr uint8_t ICM20948_ZA_OFFS_L {0x1B}; - static constexpr uint8_t ICM20948_TIMEBASE_CORR_PLL {0x28}; - - /* Registers ICM20948 USER BANK 2*/ - static constexpr uint8_t ICM20948_GYRO_SMPLRT_DIV {0x00}; - static constexpr uint8_t ICM20948_GYRO_CONFIG_1 {0x01}; - static constexpr uint8_t ICM20948_GYRO_CONFIG_2 {0x02}; - static constexpr uint8_t ICM20948_XG_OFFS_USRH {0x03}; - static constexpr uint8_t ICM20948_XG_OFFS_USRL {0x04}; - static constexpr uint8_t ICM20948_YG_OFFS_USRH {0x05}; - static constexpr uint8_t ICM20948_YG_OFFS_USRL {0x06}; - static constexpr uint8_t ICM20948_ZG_OFFS_USRH {0x07}; - static constexpr uint8_t ICM20948_ZG_OFFS_USRL {0x08}; - static constexpr uint8_t ICM20948_ODR_ALIGN_EN {0x09}; - static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_1 {0x10}; - static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_2 {0x11}; - static constexpr uint8_t ICM20948_ACCEL_INTEL_CTRL {0x12}; - static constexpr uint8_t ICM20948_ACCEL_WOM_THR {0x13}; - static constexpr uint8_t ICM20948_ACCEL_CONFIG {0x14}; - static constexpr uint8_t ICM20948_ACCEL_CONFIG_2 {0x15}; - static constexpr uint8_t ICM20948_FSYNC_CONFIG {0x52}; - static constexpr uint8_t ICM20948_TEMP_CONFIG {0x53}; - static constexpr uint8_t ICM20948_MOD_CTRL_USR {0x54}; - - /* Registers ICM20948 USER BANK 3*/ - static constexpr uint8_t ICM20948_I2C_MST_ODR_CFG {0x00}; - static constexpr uint8_t ICM20948_I2C_MST_CTRL {0x01}; - static constexpr uint8_t ICM20948_I2C_MST_DELAY_CTRL {0x02}; - static constexpr uint8_t ICM20948_I2C_SLV0_ADDR {0x03}; - static constexpr uint8_t ICM20948_I2C_SLV0_REG {0x04}; - static constexpr uint8_t ICM20948_I2C_SLV0_CTRL {0x05}; - static constexpr uint8_t ICM20948_I2C_SLV0_DO {0x06}; - - /* Registers ICM20948 ALL BANKS */ - static constexpr uint8_t ICM20948_REG_BANK_SEL {0x7F}; - - /* Registers AK09916 */ - static constexpr uint8_t AK09916_WIA_1 {0x00}; // Who I am, Company ID - static constexpr uint8_t AK09916_WIA_2 {0x01}; // Who I am, Device ID - static constexpr uint8_t AK09916_STATUS_1 {0x10}; - static constexpr uint8_t AK09916_HXL {0x11}; - static constexpr uint8_t AK09916_HXH {0x12}; - static constexpr uint8_t AK09916_HYL {0x13}; - static constexpr uint8_t AK09916_HYH {0x14}; - static constexpr uint8_t AK09916_HZL {0x15}; - static constexpr uint8_t AK09916_HZH {0x16}; - static constexpr uint8_t AK09916_STATUS_2 {0x18}; - static constexpr uint8_t AK09916_CNTL_2 {0x31}; - static constexpr uint8_t AK09916_CNTL_3 {0x32}; - - /* Register Bits */ - static constexpr uint8_t ICM20948_RESET {0x80}; - static constexpr uint8_t ICM20948_I2C_MST_EN {0x20}; - static constexpr uint8_t ICM20948_SLEEP {0x40}; - static constexpr uint8_t ICM20948_LP_EN {0x20}; - static constexpr uint8_t ICM20948_BYPASS_EN {0x02}; - static constexpr uint8_t ICM20948_GYR_EN {0x07}; - static constexpr uint8_t ICM20948_ACC_EN {0x38}; - static constexpr uint8_t ICM20948_FIFO_EN {0x40}; - static constexpr uint8_t ICM20948_INT1_ACTL {0x80}; - static constexpr uint8_t ICM20948_INT_1_LATCH_EN {0x20}; - static constexpr uint8_t ICM20948_ACTL_FSYNC {0x08}; - static constexpr uint8_t ICM20948_INT_ANYRD_2CLEAR {0x10}; - static constexpr uint8_t ICM20948_FSYNC_INT_MODE_EN {0x06}; - static constexpr uint8_t AK09916_16_BIT {0x10}; - static constexpr uint8_t AK09916_OVF {0x08}; - static constexpr uint8_t AK09916_READ {0x80}; - - /* Others */ - static constexpr uint16_t AK09916_WHO_AM_I_1 {0x4809}; - static constexpr uint16_t AK09916_WHO_AM_I_2 {0x0948}; - static constexpr uint8_t ICM20948_WHO_AM_I_CONTENT{0xEA}; - static constexpr float ICM20948_ROOM_TEMP_OFFSET {0.0}; - static constexpr float ICM20948_T_SENSITIVITY {333.87}; - static constexpr float AK09916_MAG_LSB {0.1495}; - static constexpr uint8_t ICM20948_I2C_MST_RST {0x02}; - - /* Constructors */ - - ICM20948(uint8_t addr = 0x68) : _wire{&Wire}, i2cAddress{addr}, useSPI{false} {} - ICM20948(TwoWire *w, uint8_t addr = 0x68) : _wire{w}, i2cAddress{addr}, useSPI{false} {} - ICM20948(SPIClass& s, int cs = -1) : _spi{&s}, csPin{cs}, useSPI{true} {} - /* Basic settings */ - - bool begin() override; - void reset() override; - uint8_t whoAmI() override; - void enableAcc(bool enAcc); - void setAccRange(ICM20948_accRange accRange); - void setAccDLPF(ICM20948_dlpf dlpf); - void setAccSampleRateDivider(uint16_t accSplRateDiv); - void enableGyr(bool enGyr); - void setGyrRange(ICM20948_gyroRange gyroRange); - void setGyrDLPF(ICM20948_dlpf dlpf); - void setGyrSampleRateDivider(uint8_t gyrSplRateDiv); - void setTempDLPF(ICM20948_dlpf dlpf); - void setI2CMstSampleRate(uint8_t rateExp); - void setSPIClockSpeed(unsigned long clock); - - bool setRate(Rate rate) override; - - const char* getModel() const override { return "ICM20948"; } - - /* Results */ - - bool read() override; - void waitForData() override; - void getAccel(float& x, float& y, float& z) const override; - float getTemp() const override; - void getGyro(float& x, float& y, float& z) const override; - void getMag(float& x, float& y, float& z) const override; - - /* Power, Sleep, Standby */ - - void enableCycle(ICM20948_cycle cycle); - void enableLowPower(bool enLP); - void setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg); - void setAccAverageInCycleMode(ICM20948_accAvgLowPower avg); - void sleep(bool sleep); - - - /* Interrupts */ - - void setIntPinPolarity(ICM20948_intPinPol pol); - void enableIntLatch(bool latch); - void enableClearIntByAnyRead(bool clearByAnyRead); - void setFSyncIntPolarity(ICM20948_intPinPol pol); - void enableInterrupt(ICM20948_intType intType); - void disableInterrupt(ICM20948_intType intType); - uint8_t readAndClearInterrupts(); - bool checkInterrupt(uint8_t source, ICM20948_intType type); - void setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn); - - - /* FIFO */ - - void enableFifo(bool fifo); - void setFifoMode(ICM20948_fifoMode mode); - void startFifo(ICM20948_fifoType fifo); - void stopFifo(); - void resetFifo(); - int16_t getFifoCount(); - int16_t getNumberOfFifoDataSets(); - void findFifoBegin(); - - - /* Magnetometer */ - - bool initMagnetometer(); - uint16_t whoAmIMag(); - void setMagOpMode(AK09916_opMode opMode); - void resetMag(); - - protected: - TwoWire *_wire; - SPIClass *_spi; - SPISettings mySPISettings; - uint8_t i2cAddress; - uint8_t currentBank; - uint8_t buffer[20]; - uint8_t accRangeFactor; - uint8_t gyrRangeFactor; - uint8_t regVal; // intermediate storage of register values - ICM20948_fifoType fifoType; - int csPin; - bool useSPI; - void setClockToAutoSelect(); - void switchBank(uint8_t newBank); - void writeRegister8(uint8_t bank, uint8_t reg, uint8_t val); - void writeRegister16(uint8_t bank, uint8_t reg, int16_t val); - uint8_t readRegister8(uint8_t bank, uint8_t reg); - int16_t readRegister16(uint8_t bank, uint8_t reg); - void readAllData(uint8_t* data); - void writeAK09916Register8(uint8_t reg, uint8_t val); - uint8_t readAK09916Register8(uint8_t reg); - int16_t readAK09916Register16(uint8_t reg); - void enableI2CMaster(); - void i2cMasterReset(); - void enableMagDataRead(uint8_t reg, uint8_t bytes); - +public: + /* constants */ + + static constexpr uint8_t AK09916_ADDRESS {0x0C}; + + /* Registers ICM20948 USER BANK 0*/ + static constexpr uint8_t ICM20948_WHO_AM_I {0x00}; + static constexpr uint8_t ICM20948_USER_CTRL {0x03}; + static constexpr uint8_t ICM20948_LP_CONFIG {0x05}; + static constexpr uint8_t ICM20948_PWR_MGMT_1 {0x06}; + static constexpr uint8_t ICM20948_PWR_MGMT_2 {0x07}; + static constexpr uint8_t ICM20948_INT_PIN_CFG {0x0F}; + static constexpr uint8_t ICM20948_INT_ENABLE {0x10}; + static constexpr uint8_t ICM20948_INT_ENABLE_1 {0x11}; + static constexpr uint8_t ICM20948_INT_ENABLE_2 {0x12}; + static constexpr uint8_t ICM20948_INT_ENABLE_3 {0x13}; + static constexpr uint8_t ICM20948_I2C_MST_STATUS {0x17}; + static constexpr uint8_t ICM20948_INT_STATUS {0x19}; + static constexpr uint8_t ICM20948_INT_STATUS_1 {0x1A}; + static constexpr uint8_t ICM20948_INT_STATUS_2 {0x1B}; + static constexpr uint8_t ICM20948_INT_STATUS_3 {0x1C}; + static constexpr uint8_t ICM20948_DELAY_TIME_H {0x28}; + static constexpr uint8_t ICM20948_DELAY_TIME_L {0x29}; + static constexpr uint8_t ICM20948_ACCEL_OUT {0x2D}; // accel data registers begin + static constexpr uint8_t ICM20948_GYRO_OUT {0x33}; // gyro data registers begin + static constexpr uint8_t ICM20948_TEMP_OUT {0x39}; + static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_00{0x3B}; + static constexpr uint8_t ICM20948_EXT_SLV_SENS_DATA_01{0x3C}; + static constexpr uint8_t ICM20948_FIFO_EN_1 {0x66}; + static constexpr uint8_t ICM20948_FIFO_EN_2 {0x67}; + static constexpr uint8_t ICM20948_FIFO_RST {0x68}; + static constexpr uint8_t ICM20948_FIFO_MODE {0x69}; + static constexpr uint8_t ICM20948_FIFO_COUNT {0x70}; + static constexpr uint8_t ICM20948_FIFO_R_W {0x72}; + static constexpr uint8_t ICM20948_DATA_RDY_STATUS {0x74}; + static constexpr uint8_t ICM20948_FIFO_CFG {0x76}; + + /* Registers ICM20948 USER BANK 1*/ + static constexpr uint8_t ICM20948_SELF_TEST_X_GYRO {0x02}; + static constexpr uint8_t ICM20948_SELF_TEST_Y_GYRO {0x03}; + static constexpr uint8_t ICM20948_SELF_TEST_Z_GYRO {0x04}; + static constexpr uint8_t ICM20948_SELF_TEST_X_ACCEL {0x0E}; + static constexpr uint8_t ICM20948_SELF_TEST_Y_ACCEL {0x0F}; + static constexpr uint8_t ICM20948_SELF_TEST_Z_ACCEL {0x10}; + static constexpr uint8_t ICM20948_XA_OFFS_H {0x14}; + static constexpr uint8_t ICM20948_XA_OFFS_L {0x15}; + static constexpr uint8_t ICM20948_YA_OFFS_H {0x17}; + static constexpr uint8_t ICM20948_YA_OFFS_L {0x18}; + static constexpr uint8_t ICM20948_ZA_OFFS_H {0x1A}; + static constexpr uint8_t ICM20948_ZA_OFFS_L {0x1B}; + static constexpr uint8_t ICM20948_TIMEBASE_CORR_PLL {0x28}; + + /* Registers ICM20948 USER BANK 2*/ + static constexpr uint8_t ICM20948_GYRO_SMPLRT_DIV {0x00}; + static constexpr uint8_t ICM20948_GYRO_CONFIG_1 {0x01}; + static constexpr uint8_t ICM20948_GYRO_CONFIG_2 {0x02}; + static constexpr uint8_t ICM20948_XG_OFFS_USRH {0x03}; + static constexpr uint8_t ICM20948_XG_OFFS_USRL {0x04}; + static constexpr uint8_t ICM20948_YG_OFFS_USRH {0x05}; + static constexpr uint8_t ICM20948_YG_OFFS_USRL {0x06}; + static constexpr uint8_t ICM20948_ZG_OFFS_USRH {0x07}; + static constexpr uint8_t ICM20948_ZG_OFFS_USRL {0x08}; + static constexpr uint8_t ICM20948_ODR_ALIGN_EN {0x09}; + static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_1 {0x10}; + static constexpr uint8_t ICM20948_ACCEL_SMPLRT_DIV_2 {0x11}; + static constexpr uint8_t ICM20948_ACCEL_INTEL_CTRL {0x12}; + static constexpr uint8_t ICM20948_ACCEL_WOM_THR {0x13}; + static constexpr uint8_t ICM20948_ACCEL_CONFIG {0x14}; + static constexpr uint8_t ICM20948_ACCEL_CONFIG_2 {0x15}; + static constexpr uint8_t ICM20948_FSYNC_CONFIG {0x52}; + static constexpr uint8_t ICM20948_TEMP_CONFIG {0x53}; + static constexpr uint8_t ICM20948_MOD_CTRL_USR {0x54}; + + /* Registers ICM20948 USER BANK 3*/ + static constexpr uint8_t ICM20948_I2C_MST_ODR_CFG {0x00}; + static constexpr uint8_t ICM20948_I2C_MST_CTRL {0x01}; + static constexpr uint8_t ICM20948_I2C_MST_DELAY_CTRL {0x02}; + static constexpr uint8_t ICM20948_I2C_SLV0_ADDR {0x03}; + static constexpr uint8_t ICM20948_I2C_SLV0_REG {0x04}; + static constexpr uint8_t ICM20948_I2C_SLV0_CTRL {0x05}; + static constexpr uint8_t ICM20948_I2C_SLV0_DO {0x06}; + + /* Registers ICM20948 ALL BANKS */ + static constexpr uint8_t ICM20948_REG_BANK_SEL {0x7F}; + + /* Registers AK09916 */ + static constexpr uint8_t AK09916_WIA_1 {0x00}; // Who I am, Company ID + static constexpr uint8_t AK09916_WIA_2 {0x01}; // Who I am, Device ID + static constexpr uint8_t AK09916_STATUS_1 {0x10}; + static constexpr uint8_t AK09916_HXL {0x11}; + static constexpr uint8_t AK09916_HXH {0x12}; + static constexpr uint8_t AK09916_HYL {0x13}; + static constexpr uint8_t AK09916_HYH {0x14}; + static constexpr uint8_t AK09916_HZL {0x15}; + static constexpr uint8_t AK09916_HZH {0x16}; + static constexpr uint8_t AK09916_STATUS_2 {0x18}; + static constexpr uint8_t AK09916_CNTL_2 {0x31}; + static constexpr uint8_t AK09916_CNTL_3 {0x32}; + + /* Register Bits */ + static constexpr uint8_t ICM20948_RESET {0x80}; + static constexpr uint8_t ICM20948_I2C_MST_EN {0x20}; + static constexpr uint8_t ICM20948_SLEEP {0x40}; + static constexpr uint8_t ICM20948_LP_EN {0x20}; + static constexpr uint8_t ICM20948_BYPASS_EN {0x02}; + static constexpr uint8_t ICM20948_GYR_EN {0x07}; + static constexpr uint8_t ICM20948_ACC_EN {0x38}; + static constexpr uint8_t ICM20948_FIFO_EN {0x40}; + static constexpr uint8_t ICM20948_INT1_ACTL {0x80}; + static constexpr uint8_t ICM20948_INT_1_LATCH_EN {0x20}; + static constexpr uint8_t ICM20948_ACTL_FSYNC {0x08}; + static constexpr uint8_t ICM20948_INT_ANYRD_2CLEAR {0x10}; + static constexpr uint8_t ICM20948_FSYNC_INT_MODE_EN {0x06}; + static constexpr uint8_t AK09916_16_BIT {0x10}; + static constexpr uint8_t AK09916_OVF {0x08}; + static constexpr uint8_t AK09916_READ {0x80}; + + /* Others */ + static constexpr uint16_t AK09916_WHO_AM_I_1 {0x4809}; + static constexpr uint16_t AK09916_WHO_AM_I_2 {0x0948}; + static constexpr uint8_t ICM20948_WHO_AM_I_CONTENT{0xEA}; + static constexpr float ICM20948_ROOM_TEMP_OFFSET {0.0}; + static constexpr float ICM20948_T_SENSITIVITY {333.87}; + static constexpr float AK09916_MAG_LSB {0.1495}; + static constexpr uint8_t ICM20948_I2C_MST_RST {0x02}; + + /* Constructors */ + ICM20948(uint8_t addr = 0x68) : _wire{&Wire}, i2cAddress{addr}, useSPI{false} {} + ICM20948(TwoWire *w, uint8_t addr = 0x68) : _wire{w}, i2cAddress{addr}, useSPI{false} {} + ICM20948(SPIClass& s, int cs = -1) : _spi{&s}, csPin{cs}, useSPI{true} {} + + bool begin() override; + void reset() override; + uint8_t whoAmI() override; + const char* getModel() const override { return "ICM20948"; } + void enableAcc(bool enAcc); + void setAccRange(ICM20948_accRange accRange); + void setAccDLPF(ICM20948_dlpf dlpf); + void setAccSampleRateDivider(uint16_t accSplRateDiv); + void enableGyr(bool enGyr); + void setGyrRange(ICM20948_gyroRange gyroRange); + void setGyrDLPF(ICM20948_dlpf dlpf); + void setGyrSampleRateDivider(uint8_t gyrSplRateDiv); + void setTempDLPF(ICM20948_dlpf dlpf); + void setI2CMstSampleRate(uint8_t rateExp); + void setSPIClockSpeed(unsigned long clock); + bool setRate(Rate rate) override; + + /* Results */ + bool read() override; + void waitForData() override; + void getAccel(float& x, float& y, float& z) const override; + float getTemp() const override; + void getGyro(float& x, float& y, float& z) const override; + void getMag(float& x, float& y, float& z) const override; + + /* Power, Sleep, Standby */ + void enableCycle(ICM20948_cycle cycle); + void enableLowPower(bool enLP); + void setGyrAverageInCycleMode(ICM20948_gyroAvgLowPower avg); + void setAccAverageInCycleMode(ICM20948_accAvgLowPower avg); + void sleep(bool sleep); + + /* Interrupts */ + void setIntPinPolarity(ICM20948_intPinPol pol); + void enableIntLatch(bool latch); + void enableClearIntByAnyRead(bool clearByAnyRead); + void setFSyncIntPolarity(ICM20948_intPinPol pol); + void enableInterrupt(ICM20948_intType intType); + void disableInterrupt(ICM20948_intType intType); + uint8_t readAndClearInterrupts(); + bool checkInterrupt(uint8_t source, ICM20948_intType type); + void setWakeOnMotionThreshold(uint8_t womThresh, ICM20948_womCompEn womCompEn); + + /* FIFO */ + void enableFifo(bool fifo); + void setFifoMode(ICM20948_fifoMode mode); + void startFifo(ICM20948_fifoType fifo); + void stopFifo(); + void resetFifo(); + int16_t getFifoCount(); + int16_t getNumberOfFifoDataSets(); + void findFifoBegin(); + + /* Magnetometer */ + bool initMagnetometer(); + uint16_t whoAmIMag(); + void setMagOpMode(AK09916_opMode opMode); + void resetMag(); + +protected: + TwoWire *_wire; + SPIClass *_spi; + SPISettings mySPISettings; + uint8_t i2cAddress; + uint8_t currentBank; + uint8_t buffer[20]; + uint8_t accRangeFactor; + uint8_t gyrRangeFactor; + uint8_t regVal; // intermediate storage of register values + ICM20948_fifoType fifoType; + int csPin; + bool useSPI; + void setClockToAutoSelect(); + void switchBank(uint8_t newBank); + void writeRegister8(uint8_t bank, uint8_t reg, uint8_t val); + void writeRegister16(uint8_t bank, uint8_t reg, int16_t val); + uint8_t readRegister8(uint8_t bank, uint8_t reg); + int16_t readRegister16(uint8_t bank, uint8_t reg); + void readAllData(uint8_t* data); + void writeAK09916Register8(uint8_t reg, uint8_t val); + uint8_t readAK09916Register8(uint8_t reg); + int16_t readAK09916Register16(uint8_t reg); + void enableI2CMaster(); + void i2cMasterReset(); + void enableMagDataRead(uint8_t reg, uint8_t bytes); }; From 2804ced1cf8467547174601a6c7c87a032f88bcc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 11:25:13 +0300 Subject: [PATCH 08/20] Enable magnetometer in ICM-20984 by default --- ICM20948.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ICM20948.cpp b/ICM20948.cpp index f0e0199..bfec198 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -44,6 +44,8 @@ bool ICM20948::begin() { sleep(false); writeRegister8(2, ICM20948_ODR_ALIGN_EN, 1); // aligns ODR + initMagnetometer(); + return true; } From 1b6e32843a8079e422e8a64656c5852f2a9871df Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 11:25:29 +0300 Subject: [PATCH 09/20] Code style --- ICM20948.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index bfec198..f71226e 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -528,7 +528,7 @@ bool ICM20948::initMagnetometer() { bool initSuccess = false; uint8_t tries = 0; - while(!initSuccess && (tries < 10)){ // max. 10 tries to init the magnetometer + while (!initSuccess && (tries < 10)) { // max. 10 tries to init the magnetometer delay(10); enableI2CMaster(); delay(10); From ba093a3ba71bc5682f99413e6da58570736d68ad Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 11:26:28 +0300 Subject: [PATCH 10/20] Universal interface for setting accel and gyro range --- ICM20948.cpp | 44 +++++++++++++++++++++++++++++++++++++++-- ICM20948.h | 2 ++ IMU.h | 47 ++++++++++++++++++++++---------------------- MPU9250.cpp | 55 ++++++++++++++++++++++++---------------------------- MPU9250.h | 5 +++-- 5 files changed, 95 insertions(+), 58 deletions(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index f71226e..99aba13 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -63,12 +63,32 @@ void ICM20948::enableAcc(bool enAcc) { writeRegister8(0, ICM20948_PWR_MGMT_2, regVal); } -void ICM20948::setAccRange(ICM20948_accRange accRange) { +bool ICM20948::setAccelRange(const AccelRange range) { + int accRange; + switch (range) { + case ACCEL_RANGE_MIN: + case ACCEL_RANGE_2G: + accRange = ICM20948_ACC_RANGE_2G; + break; + case ACCEL_RANGE_4G: + accRange = ICM20948_ACC_RANGE_4G; + break; + case ACCEL_RANGE_8G: + accRange = ICM20948_ACC_RANGE_8G; + break; + case ACCEL_RANGE_16G: + accRange = ICM20948_ACC_RANGE_16G; + break; + default: + log("Unsupported accel range: %d", range); + return false; + } regVal = readRegister8(2, ICM20948_ACCEL_CONFIG); regVal &= ~(0x06); regVal |= (accRange<<1); writeRegister8(2, ICM20948_ACCEL_CONFIG, regVal); accRangeFactor = 1< Date: Wed, 4 Dec 2024 11:29:54 +0300 Subject: [PATCH 11/20] Get rid of duplicating methods in MPU9250 --- MPU9250.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/MPU9250.h b/MPU9250.h index d604b9f..a63391d 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -98,16 +98,7 @@ class MPU9250 : public IMUInterface, public Logger { bool setRate(Rate rate) override; const char* getModel() const override; inline bool new_imu_data() const {return new_imu_data_;} - inline float accel_x_mps2() const {return accel_[0];} - inline float accel_y_mps2() const {return accel_[1];} - inline float accel_z_mps2() const {return accel_[2];} - inline float gyro_x_radps() const {return gyro_[0];} - inline float gyro_y_radps() const {return gyro_[1];} - inline float gyro_z_radps() const {return gyro_[2];} inline bool new_mag_data() const {return new_mag_data_;} - inline float mag_x_ut() const {return mag_[0];} - inline float mag_y_ut() const {return mag_[1];} - inline float mag_z_ut() const {return mag_[2];} inline float getTemp() const override {return temp_;} inline uint8_t whoAmI() override {return who_am_i_;} From f873676ad8a95bb728be5fd3c2839a5d2b5e421d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 11:33:12 +0300 Subject: [PATCH 12/20] Refine setRate signature --- ICM20948.cpp | 2 +- ICM20948.h | 2 +- IMU.h | 2 +- MPU9250.cpp | 2 +- MPU9250.h | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index 99aba13..3c39a76 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -179,7 +179,7 @@ void ICM20948::setSPIClockSpeed(unsigned long clock) { mySPISettings = SPISettings(clock, MSBFIRST, SPI_MODE0); } -bool ICM20948::setRate(Rate rate) { +bool ICM20948::setRate(const Rate rate) { // output rate = 1125 Hz / (1 + srd) // TODO: Implement this function switch(rate) { diff --git a/ICM20948.h b/ICM20948.h index 17c20df..3bc466f 100644 --- a/ICM20948.h +++ b/ICM20948.h @@ -234,7 +234,7 @@ class ICM20948 : public IMUInterface, public Logger void setTempDLPF(ICM20948_dlpf dlpf); void setI2CMstSampleRate(uint8_t rateExp); void setSPIClockSpeed(unsigned long clock); - bool setRate(Rate rate) override; + bool setRate(const Rate rate) override; /* Results */ bool read() override; diff --git a/IMU.h b/IMU.h index 3ff1e72..6260f06 100644 --- a/IMU.h +++ b/IMU.h @@ -49,7 +49,7 @@ class IMUInterface { virtual void getGyro(float& x, float& y, float& z) const = 0; virtual void getMag(float& x, float& y, float& z) const = 0; virtual float getTemp() const = 0; - virtual bool setRate(Rate rate) = 0; + virtual bool setRate(const Rate rate) = 0; virtual bool setAccelRange(const AccelRange range) = 0; virtual bool setGyroRange(const GyroRange range) = 0; virtual const char* getModel() const = 0; diff --git a/MPU9250.cpp b/MPU9250.cpp index 0d57ac1..0141d86 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -466,7 +466,7 @@ void MPU9250::getMag(float& x, float& y, float& z) const { y = mag_[1]; z = mag_[2]; } -bool MPU9250::setRate(Rate rate) { +bool MPU9250::setRate(const Rate rate) { // TODO: log("Rate setting is not implemented"); return false; diff --git a/MPU9250.h b/MPU9250.h index a63391d..a74c1ca 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -95,7 +95,7 @@ class MPU9250 : public IMUInterface, public Logger { void getAccel(float& x, float& y, float& z) const override; void getGyro(float& x, float& y, float& z) const override; void getMag(float& x, float& y, float& z) const override; - bool setRate(Rate rate) override; + bool setRate(const Rate rate) override; const char* getModel() const override; inline bool new_imu_data() const {return new_imu_data_;} inline bool new_mag_data() const {return new_mag_data_;} From cb8c12c7324309f913d1aafd0a27606510915c25 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:25:31 +0300 Subject: [PATCH 13/20] Implement setDLPF method --- ICM20948.cpp | 42 ++++++++++++++++++++++++++++++++++++++++++ ICM20948.h | 1 + IMU.h | 10 +++++----- MPU9250.cpp | 20 ++++++++++++++++++++ MPU9250.h | 1 + 5 files changed, 69 insertions(+), 5 deletions(-) diff --git a/ICM20948.cpp b/ICM20948.cpp index 3c39a76..8fed43e 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -169,6 +169,48 @@ void ICM20948::setTempDLPF(ICM20948_dlpf dlpf) { writeRegister8(2, ICM20948_TEMP_CONFIG, dlpf); } +bool ICM20948::setDLPF(const DLPF dlpf) { + /* ICMD20948 DLPF table: + * + * DLPF 3dB Bandwidth [Hz] Output Rate [Hz] + * 0 246.0 1125/(1+ASRD) (default) + * 1 246.0 1125/(1+ASRD) + * 2 111.4 1125/(1+ASRD) + * 3 50.4 1125/(1+ASRD) + * 4 23.9 1125/(1+ASRD) + * 5 11.5 1125/(1+ASRD) + * 6 5.7 1125/(1+ASRD) + * 7 473.0 1125/(1+ASRD) + * OFF 1209.0 4500 + * + * ASRD = Accelerometer Sample Rate Divider (0...4095) + */ + ICM20948_dlpf val; + switch (dlpf) { + case DLPF_OFF: + val = ICM20948_DLPF_OFF; + break; + case DLPF_MAX: + val = ICM20948_DLPF_7; + break; + case DLPF_100HZ_APPROX: + val = ICM20948_DLPF_2; + break; + case DLPF_50HZ_APPROX: + val = ICM20948_DLPF_3; + break; + case DLPF_MIN: + case DLPF_5HZ_APPROX: + val = ICM20948_DLPF_6; + break; + default: + log("Unsupported DLPF setting"); + return false; + } + setAccDLPF(val); + setGyrDLPF(val); +} + void ICM20948::setI2CMstSampleRate(uint8_t rateExp) { if(rateExp < 16){ writeRegister8(3, ICM20948_I2C_MST_ODR_CFG, rateExp); diff --git a/ICM20948.h b/ICM20948.h index 3bc466f..dd30292 100644 --- a/ICM20948.h +++ b/ICM20948.h @@ -232,6 +232,7 @@ class ICM20948 : public IMUInterface, public Logger void setGyrDLPF(ICM20948_dlpf dlpf); void setGyrSampleRateDivider(uint8_t gyrSplRateDiv); void setTempDLPF(ICM20948_dlpf dlpf); + bool setDLPF(const DLPF dlpf) override; void setI2CMstSampleRate(uint8_t rateExp); void setSPIClockSpeed(unsigned long clock); bool setRate(const Rate rate) override; diff --git a/IMU.h b/IMU.h index 6260f06..0327fb7 100644 --- a/IMU.h +++ b/IMU.h @@ -11,10 +11,10 @@ class IMUInterface { enum DLPF { DLPF_OFF, DLPF_MAX, - DLPF_BANDWIDTH_100HZ_APPROX, - DLPF_BANDWIDTH_50HZ_APPROX, - DLPF_BANDWIDTH_5HZ_APPROX, - DLPF_BANDWIDTH_MIN + DLPF_100HZ_APPROX, + DLPF_50HZ_APPROX, + DLPF_5HZ_APPROX, + DLPF_MIN }; enum AccelRange { ACCEL_RANGE_MIN, @@ -39,7 +39,6 @@ class IMUInterface { RATE_8KHZ_APPROX, RATE_MAX }; - virtual bool begin() = 0; virtual void reset() = 0; virtual uint8_t whoAmI() = 0; @@ -52,5 +51,6 @@ class IMUInterface { virtual bool setRate(const Rate rate) = 0; virtual bool setAccelRange(const AccelRange range) = 0; virtual bool setGyroRange(const GyroRange range) = 0; + virtual bool setDLPF(const DLPF dlpf) = 0; virtual const char* getModel() const = 0; }; diff --git a/MPU9250.cpp b/MPU9250.cpp index 0141d86..a006ea0 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -300,6 +300,26 @@ bool MPU9250::setSrd(const uint8_t srd) { srd_ = srd; return true; } +bool MPU9250::setDLPF(const DLPF dlpf) { + DlpfBandwidth val; + switch (dlpf) { + case DLPF_OFF: + case DLPF_MAX: + val = DLPF_BANDWIDTH_184HZ; + break; + case DLPF_100HZ_APPROX: + val = DLPF_BANDWIDTH_92HZ; + break; + case DLPF_50HZ_APPROX: + val = DLPF_BANDWIDTH_41HZ; + break; + case DLPF_MIN: + case DLPF_5HZ_APPROX: + val = DLPF_BANDWIDTH_5HZ; + break; + } + return setDlpfBandwidth(val); +} bool MPU9250::setDlpfBandwidth(const DlpfBandwidth dlpf) { spi_clock_ = SPI_CFG_CLOCK_; /* Check input is valid and set requested dlpf */ diff --git a/MPU9250.h b/MPU9250.h index a74c1ca..171a73b 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -86,6 +86,7 @@ class MPU9250 : public IMUInterface, public Logger { inline GyroRange gyro_range() const {return gyro_range_;} bool setSrd(const uint8_t srd); inline uint8_t srd() const {return srd_;} + bool setDLPF(const DLPF dlpf) override; bool setDlpfBandwidth(const DlpfBandwidth dlpf); inline DlpfBandwidth dlpf_bandwidth() const {return dlpf_bandwidth_;} bool enableWom(int16_t threshold_mg, const WomRate wom_rate); From 96df228fa34b01a619baf21713c5459e195981b0 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:26:53 +0300 Subject: [PATCH 14/20] Describe ICM-20948 driver in README --- README.md | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ed442cb..ad6b716 100644 --- a/README.md +++ b/README.md @@ -6,8 +6,11 @@ Based on: * [Bolder Flight InvenSense-IMU library](https://github.com/bolderflight/invensense-imu). Original author: Brian Taylor (brian.taylor@bolderflight.com). * [Bolder Flight SBUS library](https://github.com/bolderflight/sbus). Original author: Brian Taylor (brian.taylor@bolderflight.com). +* [ICM20948_WE Arduino library](https://github.com/wollewald/ICM20948_WE/). Original author: Wolfgang Ewald (wolfgang.ewald@wolles-elektronikkiste.de). -## MPU-9250/MPU-6500 IMU +## IMU + +### MPU-9250/MPU-6500 Example for SPI-connected IMU: @@ -45,6 +48,16 @@ Notice, that MPU6500 does not include a magnetometer, so magnetometer data will You can also use `` header and `MPU6500` class, which is an alias for `MPU9250` class. +### ICM-20948 + +The ICM-20948 IMU driver has the same interface. Only the declaration is changed in the example above: + +```cpp +#include + +ICM20948 IMU(SPI); +``` + ### Connection Connecting GY-91 board to ESP32 using VSPI: @@ -65,6 +78,15 @@ Connecting MPU-92.65 board to ESP32 using VSPI: * AD0 (MISO) → IO19 * NCS → IO5 +Connecting ICM-20948 board to ESP32 using VSPI: + +* VCC → 3V3 +* GND → GND +* SCL → IO18 +* SDA (MOSI) → IO23 +* NCS → IO5 +* AD0 (MISO) → IO19 + ### Logging By default, the library logs some of the errors to the serial output. On ESP32, the default serial port is determined by the Core Debug Output mechanism, thus can be changed using `setDebugOutput` method: From 863ec86a35429b7e53435b189383b2b76b0d88c3 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:28:48 +0300 Subject: [PATCH 15/20] Remove unused xyzFloat library --- xyzFloat.cpp | 96 ---------------------------------------------------- xyzFloat.h | 32 ------------------ 2 files changed, 128 deletions(-) delete mode 100644 xyzFloat.cpp delete mode 100644 xyzFloat.h diff --git a/xyzFloat.cpp b/xyzFloat.cpp deleted file mode 100644 index 2ae7b00..0000000 --- a/xyzFloat.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Code based on: https://github.com/wollewald/ICM20948_WE. - * Author: Wolfgang (Wolle) Ewald. - * License: MIT (see LICENSE_ICM20948_WE file). -*/ - -/* defines the structure xyzFloat which is used for gyroscopes, accelerometers and - magnetometers such as ICM20948, MPU9250, ADXL345, etc. */ - -#include "xyzFloat.h" - -xyzFloat::xyzFloat() - : xyzFloat(0.f, 0.f, 0.f) -{ - // intentionally empty -} - -xyzFloat::xyzFloat(float const x, float const y, float const z) - : x(x) - , y(y) - , z(z) -{ - // intentionally empty -} - -xyzFloat xyzFloat::operator+() const -{ - return *this; -} - -xyzFloat xyzFloat::operator-() const -{ - return xyzFloat{-x, - -y, - -z}; -} - -xyzFloat xyzFloat::operator+(xyzFloat const & summand) const -{ - return xyzFloat{x + summand.x, - y + summand.y, - z + summand.z}; -} - -xyzFloat xyzFloat::operator-(xyzFloat const & subtrahend) const -{ - return xyzFloat{x - subtrahend.x, - y - subtrahend.y, - z - subtrahend.z}; -} - -xyzFloat xyzFloat::operator*(float const operand) const -{ - return xyzFloat{x * operand, - y * operand, - z * operand}; -} - -xyzFloat xyzFloat::operator/(float const divisor) const -{ - return xyzFloat{x / divisor, - y / divisor, - z / divisor}; -} - -xyzFloat & xyzFloat::operator+=(xyzFloat const & summand) -{ - x += summand.x; - y += summand.y; - z += summand.z; - return *this; -} - -xyzFloat & xyzFloat::operator-=(xyzFloat const & subtrahend) -{ - x -= subtrahend.x; - y -= subtrahend.y; - z -= subtrahend.z; - return *this; -} - -xyzFloat & xyzFloat::operator*=(float const operand) -{ - x *= operand; - y *= operand; - z *= operand; - return *this; -} - -xyzFloat & xyzFloat::operator/=(float const divisor) -{ - x /= divisor; - y /= divisor; - z /= divisor; - return *this; -} diff --git a/xyzFloat.h b/xyzFloat.h deleted file mode 100644 index 2761bba..0000000 --- a/xyzFloat.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Code based on: https://github.com/wollewald/ICM20948_WE. - * Author: Wolfgang (Wolle) Ewald. - * License: MIT (see LICENSE_ICM20948_WE file). -*/ - -/* defines the structure xyzFloat which is used for gyroscopes, accelerometers and - magnetometers such as ICM20948, MPU9250, ADXL345, etc. */ - -#ifndef XYZ_FLOAT_H_ -#define XYZ_FLOAT_H_ -#include -struct xyzFloat { - float x; - float y; - float z; - - xyzFloat(); - xyzFloat(float const x, float const y, float const z); - - xyzFloat operator+() const; - xyzFloat operator-() const; - xyzFloat operator+(xyzFloat const & summand) const; - xyzFloat operator-(xyzFloat const & subtrahend) const; - xyzFloat operator*(float const operand) const; - xyzFloat operator/(float const divisor) const; - xyzFloat & operator+=(xyzFloat const & summand); - xyzFloat & operator-=(xyzFloat const & subtrahend); - xyzFloat & operator*=(float const operand); - xyzFloat & operator/=(float const divisor); -}; -#endif From 4b4cfca5f10ffe833e23187fe479850d53b7c9f2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:44:31 +0300 Subject: [PATCH 16/20] Add missing return --- ICM20948.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ICM20948.cpp b/ICM20948.cpp index 8fed43e..c7e6d5d 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -209,6 +209,7 @@ bool ICM20948::setDLPF(const DLPF dlpf) { } setAccDLPF(val); setGyrDLPF(val); + return true; } void ICM20948::setI2CMstSampleRate(uint8_t rateExp) { From 69838a2b900f969f895d18c285f6bdc3249c6adf Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:44:50 +0300 Subject: [PATCH 17/20] MPU9250's DLPF disabling is not yet implemented --- MPU9250.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/MPU9250.cpp b/MPU9250.cpp index a006ea0..fda6a5e 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -304,6 +304,8 @@ bool MPU9250::setDLPF(const DLPF dlpf) { DlpfBandwidth val; switch (dlpf) { case DLPF_OFF: + log("Disabling DLPF is not yet implemented"); + return false; case DLPF_MAX: val = DLPF_BANDWIDTH_184HZ; break; From d45154b1747ed2751edb7aa9f890357971647c5b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:44:57 +0300 Subject: [PATCH 18/20] Implement setRate for MPU9250 --- MPU9250.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/MPU9250.cpp b/MPU9250.cpp index fda6a5e..1741ba2 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -489,9 +489,19 @@ void MPU9250::getMag(float& x, float& y, float& z) const { z = mag_[2]; } bool MPU9250::setRate(const Rate rate) { - // TODO: - log("Rate setting is not implemented"); - return false; + // TODO: consider DLPF setting (8 Khz possible without DLPF) + switch (rate) { + case RATE_MIN: + return setSrd(255); // ~4 Hz + case RATE_50HZ_APPROX: + return setSrd(19); // 50 Hz + case RATE_MAX: + case RATE_1KHZ_APPROX: + return setSrd(0); // 1 kHz + default: + log("Unsupported rate setting"); + return false; + } } const char* MPU9250::getModel() const { switch (who_am_i_) { From 69e265fc344334874abf2ceaa2e171fa02b1b416 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:51:45 +0300 Subject: [PATCH 19/20] Remove license file extension --- LICENSE.md => LICENSE | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename LICENSE.md => LICENSE (100%) diff --git a/LICENSE.md b/LICENSE similarity index 100% rename from LICENSE.md rename to LICENSE From 56366df0000f48c30392e7177c99ef10bf0321cb Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 4 Dec 2024 12:54:57 +0300 Subject: [PATCH 20/20] Put all licenses into LICENSE file --- ICM20948.cpp | 2 +- ICM20948.h | 2 +- LICENSE | 69 +++++++++++++++++++++++++++++++++++++++++++-- LICENSE_ICM20948_WE | 21 -------------- 4 files changed, 68 insertions(+), 26 deletions(-) delete mode 100644 LICENSE_ICM20948_WE diff --git a/ICM20948.cpp b/ICM20948.cpp index c7e6d5d..e81399e 100644 --- a/ICM20948.cpp +++ b/ICM20948.cpp @@ -1,7 +1,7 @@ /* * Code based on: https://github.com/wollewald/ICM20948_WE. * Author: Wolfgang (Wolle) Ewald. - * License: MIT (see LICENSE_ICM20948_WE file). + * License: MIT (see LICENSE file). * Further information can be found on: * https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) * https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) diff --git a/ICM20948.h b/ICM20948.h index dd30292..da4438d 100644 --- a/ICM20948.h +++ b/ICM20948.h @@ -1,7 +1,7 @@ /* * Code based on: https://github.com/wollewald/ICM20948_WE. * Author: Wolfgang (Wolle) Ewald. - * License: MIT (see LICENSE_ICM20948_WE file). + * License: MIT (see LICENSE file). * Further information can be found on: * https://wolles-elektronikkiste.de/icm-20948-9-achsensensor-teil-i (German) * https://wolles-elektronikkiste.de/en/icm-20948-9-axis-sensor-part-i (English) diff --git a/LICENSE b/LICENSE index 3aefdce..289dc1b 100644 --- a/LICENSE +++ b/LICENSE @@ -1,10 +1,73 @@ The MIT License (MIT) +Copyright (c) 2024 Oleg Kalachev + +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. + +================================================================================ + +Bolder Flight libraries license: + +The MIT License (MIT) + Copyright (c) 2022 Bolder Flight Systems Inc -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: +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. + +================================================================================ + +ICM20948_WE library license: + +MIT License + +Copyright (c) 2021 Wolfgang (Wolle) Ewald -The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +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 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. +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. diff --git a/LICENSE_ICM20948_WE b/LICENSE_ICM20948_WE deleted file mode 100644 index 4d0bc74..0000000 --- a/LICENSE_ICM20948_WE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2021 Wolfgang (Wolle) Ewald - -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. \ No newline at end of file