MPU6050.h

00001 #ifndef MPU6050_H
00002 #define MPU6050_H
00003 
00004 #include "mbed.h"
00005 #include "math.h"
00006 
00007 #ifndef M_PI
00008 #define M_PI                3.14159265358979323846
00009 #endif
00010 
00011 // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
00012 
00013 // Invensense Inc., www.invensense.com
00014 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in
00015 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
00016 //
00017 #define XGOFFS_TC           0x00                        // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
00018 #define YGOFFS_TC           0x01
00019 #define ZGOFFS_TC           0x02
00020 #define X_FINE_GAIN         0x03                        // [7:0] fine gain
00021 #define Y_FINE_GAIN         0x04
00022 #define Z_FINE_GAIN         0x05
00023 #define XA_OFFSET_H         0x06                        // User-defined trim values for accelerometer
00024 #define XA_OFFSET_L_TC      0x07
00025 #define YA_OFFSET_H         0x08
00026 #define YA_OFFSET_L_TC      0x09
00027 #define ZA_OFFSET_H         0x0A
00028 #define ZA_OFFSET_L_TC      0x0B
00029 #define SELF_TEST_X         0x0D
00030 #define SELF_TEST_Y         0x0E
00031 #define SELF_TEST_Z         0x0F
00032 #define SELF_TEST_A         0x10
00033 #define XG_OFFS_USRH        0x13                        // User-defined trim values for gyroscope; supported in MPU-6050?
00034 #define XG_OFFS_USRL        0x14
00035 #define YG_OFFS_USRH        0x15
00036 #define YG_OFFS_USRL        0x16
00037 #define ZG_OFFS_USRH        0x17
00038 #define ZG_OFFS_USRL        0x18
00039 #define SMPLRT_DIV          0x19
00040 #define CONFIG              0x1A
00041 #define GYRO_CONFIG         0x1B
00042 #define ACCEL_CONFIG        0x1C
00043 #define FF_THR              0x1D                        // Free-fall
00044 #define FF_DUR              0x1E                        // Free-fall
00045 #define MOT_THR             0x1F                        // Motion detection threshold bits [7:0]
00046 #define MOT_DUR             0x20                        // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
00047 #define ZMOT_THR            0x21                        // Zero-motion detection threshold bits [7:0]
00048 #define ZRMOT_DUR           0x22                        // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
00049 #define FIFO_EN             0x23
00050 #define I2C_MST_CTRL        0x24
00051 #define I2C_SLV0_ADDR       0x25
00052 #define I2C_SLV0_REG        0x26
00053 #define I2C_SLV0_CTRL       0x27
00054 #define I2C_SLV1_ADDR       0x28
00055 #define I2C_SLV1_REG        0x29
00056 #define I2C_SLV1_CTRL       0x2A
00057 #define I2C_SLV2_ADDR       0x2B
00058 #define I2C_SLV2_REG        0x2C
00059 #define I2C_SLV2_CTRL       0x2D
00060 #define I2C_SLV3_ADDR       0x2E
00061 #define I2C_SLV3_REG        0x2F
00062 #define I2C_SLV3_CTRL       0x30
00063 #define I2C_SLV4_ADDR       0x31
00064 #define I2C_SLV4_REG        0x32
00065 #define I2C_SLV4_DO         0x33
00066 #define I2C_SLV4_CTRL       0x34
00067 #define I2C_SLV4_DI         0x35
00068 #define I2C_MST_STATUS      0x36
00069 #define INT_PIN_CFG         0x37
00070 #define INT_ENABLE          0x38
00071 #define DMP_INT_STATUS      0x39                        // Check DMP interrupt
00072 #define INT_STATUS          0x3A
00073 #define ACCEL_XOUT_H        0x3B
00074 #define ACCEL_XOUT_L        0x3C
00075 #define ACCEL_YOUT_H        0x3D
00076 #define ACCEL_YOUT_L        0x3E
00077 #define ACCEL_ZOUT_H        0x3F
00078 #define ACCEL_ZOUT_L        0x40
00079 #define TEMP_OUT_H          0x41
00080 #define TEMP_OUT_L          0x42
00081 #define GYRO_XOUT_H         0x43
00082 #define GYRO_XOUT_L         0x44
00083 #define GYRO_YOUT_H         0x45
00084 #define GYRO_YOUT_L         0x46
00085 #define GYRO_ZOUT_H         0x47
00086 #define GYRO_ZOUT_L         0x48
00087 #define EXT_SENS_DATA_00    0x49
00088 #define EXT_SENS_DATA_01    0x4A
00089 #define EXT_SENS_DATA_02    0x4B
00090 #define EXT_SENS_DATA_03    0x4C
00091 #define EXT_SENS_DATA_04    0x4D
00092 #define EXT_SENS_DATA_05    0x4E
00093 #define EXT_SENS_DATA_06    0x4F
00094 #define EXT_SENS_DATA_07    0x50
00095 #define EXT_SENS_DATA_08    0x51
00096 #define EXT_SENS_DATA_09    0x52
00097 #define EXT_SENS_DATA_10    0x53
00098 #define EXT_SENS_DATA_11    0x54
00099 #define EXT_SENS_DATA_12    0x55
00100 #define EXT_SENS_DATA_13    0x56
00101 #define EXT_SENS_DATA_14    0x57
00102 #define EXT_SENS_DATA_15    0x58
00103 #define EXT_SENS_DATA_16    0x59
00104 #define EXT_SENS_DATA_17    0x5A
00105 #define EXT_SENS_DATA_18    0x5B
00106 #define EXT_SENS_DATA_19    0x5C
00107 #define EXT_SENS_DATA_20    0x5D
00108 #define EXT_SENS_DATA_21    0x5E
00109 #define EXT_SENS_DATA_22    0x5F
00110 #define EXT_SENS_DATA_23    0x60
00111 #define MOT_DETECT_STATUS   0x61
00112 #define I2C_SLV0_DO         0x63
00113 #define I2C_SLV1_DO         0x64
00114 #define I2C_SLV2_DO         0x65
00115 #define I2C_SLV3_DO         0x66
00116 #define I2C_MST_DELAY_CTRL  0x67
00117 #define SIGNAL_PATH_RESET   0x68
00118 #define MOT_DETECT_CTRL     0x69
00119 #define USER_CTRL           0x6A                        // Bit 7 enable DMP, bit 3 reset DMP
00120 #define PWR_MGMT_1          0x6B                        // Device defaults to the SLEEP mode
00121 #define PWR_MGMT_2          0x6C
00122 #define DMP_BANK            0x6D                        // Activates a specific bank in the DMP
00123 #define DMP_RW_PNT          0x6E                        // Set read/write pointer to a specific start address in specified DMP bank
00124 #define DMP_REG             0x6F                        // Register in DMP from which to read or to which to write
00125 #define DMP_REG_1           0x70
00126 #define DMP_REG_2           0x71
00127 #define FIFO_COUNTH         0x72
00128 #define FIFO_COUNTL         0x73
00129 #define FIFO_R_W            0x74
00130 #define WHO_AM_I_MPU6050    0x75                        // Should return 0x68
00131 
00132 // Register bits
00133 #define DATA_RDY_INT        0
00134 #define I2C_MST_EN          5
00135 
00136 enum AccelScale
00137 {
00138     AFS_2G,
00139     AFS_4G,
00140     AFS_8G,
00141     AFS_16G
00142 };
00143 
00144 enum GyroScale
00145 {
00146     GFS_250DPS,
00147     GFS_500DPS,
00148     GFS_1000DPS,
00149     GFS_2000DPS
00150 };
00151 
00152 class MPU6050
00153 {
00154     uint8_t     _addr;
00155     AccelScale  _accelScale;
00156     GyroScale   _gyroScale;
00157     I2C         _i2c;
00158     InterruptIn _interruptIn;
00159     float       _accelRes;
00160     float       _gyroRes;
00161     int16_t     _accelAdc[3];                           // Stores the 16-bit signed accelerometer sensor output
00162     int16_t     _gyroAdc[3];                            // Stores the 16-bit signed gyro sensor output
00163     float       _gyroBias[3] = { 0 };
00164     float       _accelBias[3] = { 0 };                  // Bias corrections for gyro and accelerometer
00165     int16_t     _tempInt;                               // Stores the real internal chip temperature in degrees Celsius
00166     float       _temp;
00167     float       _selfTest[6];
00168 
00169     // parameters for 6 DoF sensor fusion calculations
00170     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
00171     const float _gyroDrift = M_PI * (2.0f / 180.0f);    // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
00172     float       _beta;                                  // compute beta
00173     float       _zeta;                                  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
00174     float       _pitch, _yaw, _roll;
00175     float       _q[4] = { 1.0f, 0.0f, 0.0f, 0.0f };     // vector to hold quaternion
00176     void        writeReg(uint8_t reg, uint8_t byte);
00177     uint8_t     readReg(uint8_t reg);
00178     void        readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest);
00179     int16_t*    accelADC();
00180     int16_t*    gyroADC();
00181     int16_t     tempADC();
00182 public:
00183     float   accelData[3];                               // Stores the real accelerometer sensor output
00184     float&  accelX = accelData[0];                          // Acceleration liases
00185     float&  accelY = accelData[1];
00186     float&  accelZ = accelData[2];
00187     float   gyroData[3];                                // Stores the real gyro sensor output
00188     float&  gyroX = gyroData[0];                           // Gyro aliases
00189     float&  gyroY = gyroData[1];
00190     float&  gyroZ = gyroData[2];
00191 
00192     MPU6050
00193     (
00194         uint8_t     addr = 0x68,
00195         AccelScale  accelScale = AFS_2G,
00196         GyroScale   gyroScale = GFS_250DPS,
00197         PinName     sdaPin = I2C_SDA,
00198         PinName     sclPin = I2C_SCL,
00199         PinName     interruptInPin = NC
00200     );
00201     virtual ~MPU6050()  { }
00202     void    rise(Callback<void (void)> func);
00203     template<typename T>
00204     void rise(T* tptr, void (T:: *mptr) (void));
00205     void fall(Callback<void (void)> func);
00206     template<typename T>
00207     void fall(T* tptr, void (T:: *mptr) (void));
00208     bool init();
00209     bool dataReady();
00210     float* accel();
00211     float* gyro();
00212     float temp();
00213 
00214     // Configure the motion detection control for low power accelerometer mode
00215     void lowPowerAccelOnly();
00216     void reset();
00217 
00218     // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
00219     // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
00220     void calibrate();
00221 
00222     // Accelerometer and gyroscope self test; check calibration wrt factory settings
00223     bool selfTestOK();                                  // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
00224     void setGain(float beta, float zeta);
00225 
00226     // Implementation of Sebastian Madgwick's "...efficient orientation filter for inertial/magnetic sensor arrays"
00227     // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
00228     // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
00229     // device orientation -- which can be converted to yaw, pitch, and roll.
00230     // Useful for stabilizing quadcopters, etc.
00231     // The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms
00232     // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
00233     void madgwickFilter(float deltaT);
00234 
00235     // To compute yaw, pitch and roll after after aplying the madgwickFilter
00236     float yaw();
00237     float pitch();
00238     float roll();
00239 };
00240 #endif // MPU6050_H