Zoltan Hudak / MPU6050

Library for MPU6050 motion sensor with Madgwick filter.

Dependents:   ScilabArduino MPU6050_Hello

MPU6050.h@0:9c2bb0f94c31, 2021-01-18 (annotated)

Committer:
hudakz
Date:
Mon Jan 18 19:44:43 2021 +0000
Revision:
0:9c2bb0f94c31
Library for MPU6050 with Madgwick filter.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hudakz 0:9c2bb0f94c31 1 #ifndef MPU6050_H
hudakz 0:9c2bb0f94c31 2 #define MPU6050_H
hudakz 0:9c2bb0f94c31 3
hudakz 0:9c2bb0f94c31 4 #include "mbed.h"
hudakz 0:9c2bb0f94c31 5 #include "math.h"
hudakz 0:9c2bb0f94c31 6
hudakz 0:9c2bb0f94c31 7 #ifndef M_PI
hudakz 0:9c2bb0f94c31 8 #define M_PI 3.14159265358979323846
hudakz 0:9c2bb0f94c31 9 #endif
hudakz 0:9c2bb0f94c31 10
hudakz 0:9c2bb0f94c31 11 // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
hudakz 0:9c2bb0f94c31 12
hudakz 0:9c2bb0f94c31 13 // Invensense Inc., www.invensense.com
hudakz 0:9c2bb0f94c31 14 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in
hudakz 0:9c2bb0f94c31 15 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
hudakz 0:9c2bb0f94c31 16 //
hudakz 0:9c2bb0f94c31 17 #define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
hudakz 0:9c2bb0f94c31 18 #define YGOFFS_TC 0x01
hudakz 0:9c2bb0f94c31 19 #define ZGOFFS_TC 0x02
hudakz 0:9c2bb0f94c31 20 #define X_FINE_GAIN 0x03 // [7:0] fine gain
hudakz 0:9c2bb0f94c31 21 #define Y_FINE_GAIN 0x04
hudakz 0:9c2bb0f94c31 22 #define Z_FINE_GAIN 0x05
hudakz 0:9c2bb0f94c31 23 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
hudakz 0:9c2bb0f94c31 24 #define XA_OFFSET_L_TC 0x07
hudakz 0:9c2bb0f94c31 25 #define YA_OFFSET_H 0x08
hudakz 0:9c2bb0f94c31 26 #define YA_OFFSET_L_TC 0x09
hudakz 0:9c2bb0f94c31 27 #define ZA_OFFSET_H 0x0A
hudakz 0:9c2bb0f94c31 28 #define ZA_OFFSET_L_TC 0x0B
hudakz 0:9c2bb0f94c31 29 #define SELF_TEST_X 0x0D
hudakz 0:9c2bb0f94c31 30 #define SELF_TEST_Y 0x0E
hudakz 0:9c2bb0f94c31 31 #define SELF_TEST_Z 0x0F
hudakz 0:9c2bb0f94c31 32 #define SELF_TEST_A 0x10
hudakz 0:9c2bb0f94c31 33 #define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050?
hudakz 0:9c2bb0f94c31 34 #define XG_OFFS_USRL 0x14
hudakz 0:9c2bb0f94c31 35 #define YG_OFFS_USRH 0x15
hudakz 0:9c2bb0f94c31 36 #define YG_OFFS_USRL 0x16
hudakz 0:9c2bb0f94c31 37 #define ZG_OFFS_USRH 0x17
hudakz 0:9c2bb0f94c31 38 #define ZG_OFFS_USRL 0x18
hudakz 0:9c2bb0f94c31 39 #define SMPLRT_DIV 0x19
hudakz 0:9c2bb0f94c31 40 #define CONFIG 0x1A
hudakz 0:9c2bb0f94c31 41 #define GYRO_CONFIG 0x1B
hudakz 0:9c2bb0f94c31 42 #define ACCEL_CONFIG 0x1C
hudakz 0:9c2bb0f94c31 43 #define FF_THR 0x1D // Free-fall
hudakz 0:9c2bb0f94c31 44 #define FF_DUR 0x1E // Free-fall
hudakz 0:9c2bb0f94c31 45 #define MOT_THR 0x1F // Motion detection threshold bits [7:0]
hudakz 0:9c2bb0f94c31 46 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
hudakz 0:9c2bb0f94c31 47 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
hudakz 0:9c2bb0f94c31 48 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
hudakz 0:9c2bb0f94c31 49 #define FIFO_EN 0x23
hudakz 0:9c2bb0f94c31 50 #define I2C_MST_CTRL 0x24
hudakz 0:9c2bb0f94c31 51 #define I2C_SLV0_ADDR 0x25
hudakz 0:9c2bb0f94c31 52 #define I2C_SLV0_REG 0x26
hudakz 0:9c2bb0f94c31 53 #define I2C_SLV0_CTRL 0x27
hudakz 0:9c2bb0f94c31 54 #define I2C_SLV1_ADDR 0x28
hudakz 0:9c2bb0f94c31 55 #define I2C_SLV1_REG 0x29
hudakz 0:9c2bb0f94c31 56 #define I2C_SLV1_CTRL 0x2A
hudakz 0:9c2bb0f94c31 57 #define I2C_SLV2_ADDR 0x2B
hudakz 0:9c2bb0f94c31 58 #define I2C_SLV2_REG 0x2C
hudakz 0:9c2bb0f94c31 59 #define I2C_SLV2_CTRL 0x2D
hudakz 0:9c2bb0f94c31 60 #define I2C_SLV3_ADDR 0x2E
hudakz 0:9c2bb0f94c31 61 #define I2C_SLV3_REG 0x2F
hudakz 0:9c2bb0f94c31 62 #define I2C_SLV3_CTRL 0x30
hudakz 0:9c2bb0f94c31 63 #define I2C_SLV4_ADDR 0x31
hudakz 0:9c2bb0f94c31 64 #define I2C_SLV4_REG 0x32
hudakz 0:9c2bb0f94c31 65 #define I2C_SLV4_DO 0x33
hudakz 0:9c2bb0f94c31 66 #define I2C_SLV4_CTRL 0x34
hudakz 0:9c2bb0f94c31 67 #define I2C_SLV4_DI 0x35
hudakz 0:9c2bb0f94c31 68 #define I2C_MST_STATUS 0x36
hudakz 0:9c2bb0f94c31 69 #define INT_PIN_CFG 0x37
hudakz 0:9c2bb0f94c31 70 #define INT_ENABLE 0x38
hudakz 0:9c2bb0f94c31 71 #define DMP_INT_STATUS 0x39 // Check DMP interrupt
hudakz 0:9c2bb0f94c31 72 #define INT_STATUS 0x3A
hudakz 0:9c2bb0f94c31 73 #define ACCEL_XOUT_H 0x3B
hudakz 0:9c2bb0f94c31 74 #define ACCEL_XOUT_L 0x3C
hudakz 0:9c2bb0f94c31 75 #define ACCEL_YOUT_H 0x3D
hudakz 0:9c2bb0f94c31 76 #define ACCEL_YOUT_L 0x3E
hudakz 0:9c2bb0f94c31 77 #define ACCEL_ZOUT_H 0x3F
hudakz 0:9c2bb0f94c31 78 #define ACCEL_ZOUT_L 0x40
hudakz 0:9c2bb0f94c31 79 #define TEMP_OUT_H 0x41
hudakz 0:9c2bb0f94c31 80 #define TEMP_OUT_L 0x42
hudakz 0:9c2bb0f94c31 81 #define GYRO_XOUT_H 0x43
hudakz 0:9c2bb0f94c31 82 #define GYRO_XOUT_L 0x44
hudakz 0:9c2bb0f94c31 83 #define GYRO_YOUT_H 0x45
hudakz 0:9c2bb0f94c31 84 #define GYRO_YOUT_L 0x46
hudakz 0:9c2bb0f94c31 85 #define GYRO_ZOUT_H 0x47
hudakz 0:9c2bb0f94c31 86 #define GYRO_ZOUT_L 0x48
hudakz 0:9c2bb0f94c31 87 #define EXT_SENS_DATA_00 0x49
hudakz 0:9c2bb0f94c31 88 #define EXT_SENS_DATA_01 0x4A
hudakz 0:9c2bb0f94c31 89 #define EXT_SENS_DATA_02 0x4B
hudakz 0:9c2bb0f94c31 90 #define EXT_SENS_DATA_03 0x4C
hudakz 0:9c2bb0f94c31 91 #define EXT_SENS_DATA_04 0x4D
hudakz 0:9c2bb0f94c31 92 #define EXT_SENS_DATA_05 0x4E
hudakz 0:9c2bb0f94c31 93 #define EXT_SENS_DATA_06 0x4F
hudakz 0:9c2bb0f94c31 94 #define EXT_SENS_DATA_07 0x50
hudakz 0:9c2bb0f94c31 95 #define EXT_SENS_DATA_08 0x51
hudakz 0:9c2bb0f94c31 96 #define EXT_SENS_DATA_09 0x52
hudakz 0:9c2bb0f94c31 97 #define EXT_SENS_DATA_10 0x53
hudakz 0:9c2bb0f94c31 98 #define EXT_SENS_DATA_11 0x54
hudakz 0:9c2bb0f94c31 99 #define EXT_SENS_DATA_12 0x55
hudakz 0:9c2bb0f94c31 100 #define EXT_SENS_DATA_13 0x56
hudakz 0:9c2bb0f94c31 101 #define EXT_SENS_DATA_14 0x57
hudakz 0:9c2bb0f94c31 102 #define EXT_SENS_DATA_15 0x58
hudakz 0:9c2bb0f94c31 103 #define EXT_SENS_DATA_16 0x59
hudakz 0:9c2bb0f94c31 104 #define EXT_SENS_DATA_17 0x5A
hudakz 0:9c2bb0f94c31 105 #define EXT_SENS_DATA_18 0x5B
hudakz 0:9c2bb0f94c31 106 #define EXT_SENS_DATA_19 0x5C
hudakz 0:9c2bb0f94c31 107 #define EXT_SENS_DATA_20 0x5D
hudakz 0:9c2bb0f94c31 108 #define EXT_SENS_DATA_21 0x5E
hudakz 0:9c2bb0f94c31 109 #define EXT_SENS_DATA_22 0x5F
hudakz 0:9c2bb0f94c31 110 #define EXT_SENS_DATA_23 0x60
hudakz 0:9c2bb0f94c31 111 #define MOT_DETECT_STATUS 0x61
hudakz 0:9c2bb0f94c31 112 #define I2C_SLV0_DO 0x63
hudakz 0:9c2bb0f94c31 113 #define I2C_SLV1_DO 0x64
hudakz 0:9c2bb0f94c31 114 #define I2C_SLV2_DO 0x65
hudakz 0:9c2bb0f94c31 115 #define I2C_SLV3_DO 0x66
hudakz 0:9c2bb0f94c31 116 #define I2C_MST_DELAY_CTRL 0x67
hudakz 0:9c2bb0f94c31 117 #define SIGNAL_PATH_RESET 0x68
hudakz 0:9c2bb0f94c31 118 #define MOT_DETECT_CTRL 0x69
hudakz 0:9c2bb0f94c31 119 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
hudakz 0:9c2bb0f94c31 120 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
hudakz 0:9c2bb0f94c31 121 #define PWR_MGMT_2 0x6C
hudakz 0:9c2bb0f94c31 122 #define DMP_BANK 0x6D // Activates a specific bank in the DMP
hudakz 0:9c2bb0f94c31 123 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
hudakz 0:9c2bb0f94c31 124 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write
hudakz 0:9c2bb0f94c31 125 #define DMP_REG_1 0x70
hudakz 0:9c2bb0f94c31 126 #define DMP_REG_2 0x71
hudakz 0:9c2bb0f94c31 127 #define FIFO_COUNTH 0x72
hudakz 0:9c2bb0f94c31 128 #define FIFO_COUNTL 0x73
hudakz 0:9c2bb0f94c31 129 #define FIFO_R_W 0x74
hudakz 0:9c2bb0f94c31 130 #define WHO_AM_I_MPU6050 0x75 // Should return 0x68
hudakz 0:9c2bb0f94c31 131
hudakz 0:9c2bb0f94c31 132 // Register bits
hudakz 0:9c2bb0f94c31 133 #define DATA_RDY_INT 0
hudakz 0:9c2bb0f94c31 134 #define I2C_MST_EN 5
hudakz 0:9c2bb0f94c31 135
hudakz 0:9c2bb0f94c31 136 enum AccelScale
hudakz 0:9c2bb0f94c31 137 {
hudakz 0:9c2bb0f94c31 138 AFS_2G,
hudakz 0:9c2bb0f94c31 139 AFS_4G,
hudakz 0:9c2bb0f94c31 140 AFS_8G,
hudakz 0:9c2bb0f94c31 141 AFS_16G
hudakz 0:9c2bb0f94c31 142 };
hudakz 0:9c2bb0f94c31 143
hudakz 0:9c2bb0f94c31 144 enum GyroScale
hudakz 0:9c2bb0f94c31 145 {
hudakz 0:9c2bb0f94c31 146 GFS_250DPS,
hudakz 0:9c2bb0f94c31 147 GFS_500DPS,
hudakz 0:9c2bb0f94c31 148 GFS_1000DPS,
hudakz 0:9c2bb0f94c31 149 GFS_2000DPS
hudakz 0:9c2bb0f94c31 150 };
hudakz 0:9c2bb0f94c31 151
hudakz 0:9c2bb0f94c31 152 class MPU6050
hudakz 0:9c2bb0f94c31 153 {
hudakz 0:9c2bb0f94c31 154 uint8_t _addr;
hudakz 0:9c2bb0f94c31 155 AccelScale _accelScale;
hudakz 0:9c2bb0f94c31 156 GyroScale _gyroScale;
hudakz 0:9c2bb0f94c31 157 I2C _i2c;
hudakz 0:9c2bb0f94c31 158 InterruptIn _interruptIn;
hudakz 0:9c2bb0f94c31 159 float _accelRes;
hudakz 0:9c2bb0f94c31 160 float _gyroRes;
hudakz 0:9c2bb0f94c31 161 int16_t _accelAdc[3]; // Stores the 16-bit signed accelerometer sensor output
hudakz 0:9c2bb0f94c31 162 int16_t _gyroAdc[3]; // Stores the 16-bit signed gyro sensor output
hudakz 0:9c2bb0f94c31 163 float _gyroBias[3] = { 0 };
hudakz 0:9c2bb0f94c31 164 float _accelBias[3] = { 0 }; // Bias corrections for gyro and accelerometer
hudakz 0:9c2bb0f94c31 165 int16_t _tempInt; // Stores the real internal chip temperature in degrees Celsius
hudakz 0:9c2bb0f94c31 166 float _temp;
hudakz 0:9c2bb0f94c31 167 float _selfTest[6];
hudakz 0:9c2bb0f94c31 168
hudakz 0:9c2bb0f94c31 169 // parameters for 6 DoF sensor fusion calculations
hudakz 0:9c2bb0f94c31 170 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
hudakz 0:9c2bb0f94c31 171 const float _gyroDrift = M_PI * (2.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
hudakz 0:9c2bb0f94c31 172 float _beta; // compute beta
hudakz 0:9c2bb0f94c31 173 float _zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
hudakz 0:9c2bb0f94c31 174 float _pitch, _yaw, _roll;
hudakz 0:9c2bb0f94c31 175 float _q[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; // vector to hold quaternion
hudakz 0:9c2bb0f94c31 176 void writeReg(uint8_t reg, uint8_t byte);
hudakz 0:9c2bb0f94c31 177 uint8_t readReg(uint8_t reg);
hudakz 0:9c2bb0f94c31 178 void readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest);
hudakz 0:9c2bb0f94c31 179 int16_t* accelADC();
hudakz 0:9c2bb0f94c31 180 int16_t* gyroADC();
hudakz 0:9c2bb0f94c31 181 int16_t tempADC();
hudakz 0:9c2bb0f94c31 182 public:
hudakz 0:9c2bb0f94c31 183 float accelData[3]; // Stores the real accelerometer sensor output
hudakz 0:9c2bb0f94c31 184 float& accelX = accelData[0]; // Acceleration liases
hudakz 0:9c2bb0f94c31 185 float& accelY = accelData[1];
hudakz 0:9c2bb0f94c31 186 float& accelZ = accelData[2];
hudakz 0:9c2bb0f94c31 187 float gyroData[3]; // Stores the real gyro sensor output
hudakz 0:9c2bb0f94c31 188 float& gyroX = gyroData[0]; // Gyro aliases
hudakz 0:9c2bb0f94c31 189 float& gyroY = gyroData[1];
hudakz 0:9c2bb0f94c31 190 float& gyroZ = gyroData[2];
hudakz 0:9c2bb0f94c31 191
hudakz 0:9c2bb0f94c31 192 MPU6050
hudakz 0:9c2bb0f94c31 193 (
hudakz 0:9c2bb0f94c31 194 uint8_t addr = 0x68,
hudakz 0:9c2bb0f94c31 195 AccelScale accelScale = AFS_2G,
hudakz 0:9c2bb0f94c31 196 GyroScale gyroScale = GFS_250DPS,
hudakz 0:9c2bb0f94c31 197 PinName sdaPin = I2C_SDA,
hudakz 0:9c2bb0f94c31 198 PinName sclPin = I2C_SCL,
hudakz 0:9c2bb0f94c31 199 PinName interruptInPin = NC
hudakz 0:9c2bb0f94c31 200 );
hudakz 0:9c2bb0f94c31 201 virtual ~MPU6050() { }
hudakz 0:9c2bb0f94c31 202 void rise(Callback<void (void)> func);
hudakz 0:9c2bb0f94c31 203 template<typename T>
hudakz 0:9c2bb0f94c31 204 void rise(T* tptr, void (T:: *mptr) (void));
hudakz 0:9c2bb0f94c31 205 void fall(Callback<void (void)> func);
hudakz 0:9c2bb0f94c31 206 template<typename T>
hudakz 0:9c2bb0f94c31 207 void fall(T* tptr, void (T:: *mptr) (void));
hudakz 0:9c2bb0f94c31 208 bool init();
hudakz 0:9c2bb0f94c31 209 bool dataReady();
hudakz 0:9c2bb0f94c31 210 float* accel();
hudakz 0:9c2bb0f94c31 211 float* gyro();
hudakz 0:9c2bb0f94c31 212 float temp();
hudakz 0:9c2bb0f94c31 213
hudakz 0:9c2bb0f94c31 214 // Configure the motion detection control for low power accelerometer mode
hudakz 0:9c2bb0f94c31 215 void lowPowerAccelOnly();
hudakz 0:9c2bb0f94c31 216 void reset();
hudakz 0:9c2bb0f94c31 217
hudakz 0:9c2bb0f94c31 218 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
hudakz 0:9c2bb0f94c31 219 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
hudakz 0:9c2bb0f94c31 220 void calibrate();
hudakz 0:9c2bb0f94c31 221
hudakz 0:9c2bb0f94c31 222 // Accelerometer and gyroscope self test; check calibration wrt factory settings
hudakz 0:9c2bb0f94c31 223 bool selfTestOK(); // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
hudakz 0:9c2bb0f94c31 224 void setGain(float beta, float zeta);
hudakz 0:9c2bb0f94c31 225
hudakz 0:9c2bb0f94c31 226 // Implementation of Sebastian Madgwick's "...efficient orientation filter for inertial/magnetic sensor arrays"
hudakz 0:9c2bb0f94c31 227 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
hudakz 0:9c2bb0f94c31 228 // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
hudakz 0:9c2bb0f94c31 229 // device orientation -- which can be converted to yaw, pitch, and roll.
hudakz 0:9c2bb0f94c31 230 // Useful for stabilizing quadcopters, etc.
hudakz 0:9c2bb0f94c31 231 // The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms
hudakz 0:9c2bb0f94c31 232 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
hudakz 0:9c2bb0f94c31 233 void madgwickFilter(float deltaT);
hudakz 0:9c2bb0f94c31 234
hudakz 0:9c2bb0f94c31 235 // To compute yaw, pitch and roll after after aplying the madgwickFilter
hudakz 0:9c2bb0f94c31 236 float yaw();
hudakz 0:9c2bb0f94c31 237 float pitch();
hudakz 0:9c2bb0f94c31 238 float roll();
hudakz 0:9c2bb0f94c31 239 };
hudakz 0:9c2bb0f94c31 240 #endif // MPU6050_H