Library for MPU6050 motion sensor with Madgwick filter.
Dependents: ScilabArduino MPU6050_Hello
Diff: MPU6050.h
- Revision:
- 0:9c2bb0f94c31
diff -r 000000000000 -r 9c2bb0f94c31 MPU6050.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.h Mon Jan 18 19:44:43 2021 +0000 @@ -0,0 +1,240 @@ +#ifndef MPU6050_H +#define MPU6050_H + +#include "mbed.h" +#include "math.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +// Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device + +// Invensense Inc., www.invensense.com +// See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in +// above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor +// +#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD +#define YGOFFS_TC 0x01 +#define ZGOFFS_TC 0x02 +#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B +#define SELF_TEST_X 0x0D +#define SELF_TEST_Y 0x0E +#define SELF_TEST_Z 0x0F +#define SELF_TEST_A 0x10 +#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050? +#define XG_OFFS_USRL 0x14 +#define YG_OFFS_USRH 0x15 +#define YG_OFFS_USRL 0x16 +#define ZG_OFFS_USRH 0x17 +#define ZG_OFFS_USRL 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define FF_THR 0x1D // Free-fall +#define FF_DUR 0x1E // Free-fall +#define MOT_THR 0x1F // Motion detection threshold bits [7:0] +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU6050 0x75 // Should return 0x68 + +// Register bits +#define DATA_RDY_INT 0 +#define I2C_MST_EN 5 + +enum AccelScale +{ + AFS_2G, + AFS_4G, + AFS_8G, + AFS_16G +}; + +enum GyroScale +{ + GFS_250DPS, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; + +class MPU6050 +{ + uint8_t _addr; + AccelScale _accelScale; + GyroScale _gyroScale; + I2C _i2c; + InterruptIn _interruptIn; + float _accelRes; + float _gyroRes; + int16_t _accelAdc[3]; // Stores the 16-bit signed accelerometer sensor output + int16_t _gyroAdc[3]; // Stores the 16-bit signed gyro sensor output + float _gyroBias[3] = { 0 }; + float _accelBias[3] = { 0 }; // Bias corrections for gyro and accelerometer + int16_t _tempInt; // Stores the real internal chip temperature in degrees Celsius + float _temp; + float _selfTest[6]; + + // parameters for 6 DoF sensor fusion calculations + const float _gyroError = M_PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 + const float _gyroDrift = M_PI * (2.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + float _beta; // compute beta + float _zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + float _pitch, _yaw, _roll; + float _q[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; // vector to hold quaternion + void writeReg(uint8_t reg, uint8_t byte); + uint8_t readReg(uint8_t reg); + void readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest); + int16_t* accelADC(); + int16_t* gyroADC(); + int16_t tempADC(); +public: + float accelData[3]; // Stores the real accelerometer sensor output + float& accelX = accelData[0]; // Acceleration liases + float& accelY = accelData[1]; + float& accelZ = accelData[2]; + float gyroData[3]; // Stores the real gyro sensor output + float& gyroX = gyroData[0]; // Gyro aliases + float& gyroY = gyroData[1]; + float& gyroZ = gyroData[2]; + + MPU6050 + ( + uint8_t addr = 0x68, + AccelScale accelScale = AFS_2G, + GyroScale gyroScale = GFS_250DPS, + PinName sdaPin = I2C_SDA, + PinName sclPin = I2C_SCL, + PinName interruptInPin = NC + ); + virtual ~MPU6050() { } + void rise(Callback<void (void)> func); + template<typename T> + void rise(T* tptr, void (T:: *mptr) (void)); + void fall(Callback<void (void)> func); + template<typename T> + void fall(T* tptr, void (T:: *mptr) (void)); + bool init(); + bool dataReady(); + float* accel(); + float* gyro(); + float temp(); + + // Configure the motion detection control for low power accelerometer mode + void lowPowerAccelOnly(); + void reset(); + + // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average + // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. + void calibrate(); + + // Accelerometer and gyroscope self test; check calibration wrt factory settings + bool selfTestOK(); // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass + void setGain(float beta, float zeta); + + // Implementation of Sebastian Madgwick's "...efficient orientation filter for inertial/magnetic sensor arrays" + // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) + // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative + // device orientation -- which can be converted to yaw, pitch, and roll. + // Useful for stabilizing quadcopters, etc. + // The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms + // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + void madgwickFilter(float deltaT); + + // To compute yaw, pitch and roll after after aplying the madgwickFilter + float yaw(); + float pitch(); + float roll(); +}; +#endif // MPU6050_H