MPU6050.h

00001 //This file is an adaptation of Kris Winer's MPU6050 library and example code
00002 //See: https://developer.mbed.org/users/onehorse/code/MPU6050IMU/
00003 //Specifically see https://developer.mbed.org/users/onehorse/code/MPU6050IMU/file/e0381ca0edac/main.cpp for license information :)
00004 
00005 #ifndef MPU6050_H
00006 #define MPU6050_H
00007  
00008 #include "mbed.h"
00009 #include "math.h"
00010  
00011  // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
00012 // Invensense Inc., www.invensense.com
00013 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in 
00014 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
00015 //
00016 #define XGOFFS_TC        0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD                 
00017 #define YGOFFS_TC        0x01                                                                          
00018 #define ZGOFFS_TC        0x02
00019 #define X_FINE_GAIN      0x03 // [7:0] fine gain
00020 #define Y_FINE_GAIN      0x04
00021 #define Z_FINE_GAIN      0x05
00022 #define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
00023 #define XA_OFFSET_L_TC   0x07
00024 #define YA_OFFSET_H      0x08
00025 #define YA_OFFSET_L_TC   0x09
00026 #define ZA_OFFSET_H      0x0A
00027 #define ZA_OFFSET_L_TC   0x0B
00028 #define SELF_TEST_X      0x0D
00029 #define SELF_TEST_Y      0x0E    
00030 #define SELF_TEST_Z      0x0F
00031 #define SELF_TEST_A      0x10
00032 #define XG_OFFS_USRH     0x13  // User-defined trim values for gyroscope; supported in MPU-6050?
00033 #define XG_OFFS_USRL     0x14
00034 #define YG_OFFS_USRH     0x15
00035 #define YG_OFFS_USRL     0x16
00036 #define ZG_OFFS_USRH     0x17
00037 #define ZG_OFFS_USRL     0x18
00038 #define SMPLRT_DIV       0x19
00039 #define CONFIG           0x1A
00040 #define GYRO_CONFIG      0x1B
00041 #define ACCEL_CONFIG     0x1C
00042 #define FF_THR           0x1D  // Free-fall
00043 #define FF_DUR           0x1E  // Free-fall
00044 #define MOT_THR          0x1F  // Motion detection threshold bits [7:0]
00045 #define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
00046 #define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
00047 #define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
00048 #define FIFO_EN          0x23
00049 #define I2C_MST_CTRL     0x24   
00050 #define I2C_SLV0_ADDR    0x25
00051 #define I2C_SLV0_REG     0x26
00052 #define I2C_SLV0_CTRL    0x27
00053 #define I2C_SLV1_ADDR    0x28
00054 #define I2C_SLV1_REG     0x29
00055 #define I2C_SLV1_CTRL    0x2A
00056 #define I2C_SLV2_ADDR    0x2B
00057 #define I2C_SLV2_REG     0x2C
00058 #define I2C_SLV2_CTRL    0x2D
00059 #define I2C_SLV3_ADDR    0x2E
00060 #define I2C_SLV3_REG     0x2F
00061 #define I2C_SLV3_CTRL    0x30
00062 #define I2C_SLV4_ADDR    0x31
00063 #define I2C_SLV4_REG     0x32
00064 #define I2C_SLV4_DO      0x33
00065 #define I2C_SLV4_CTRL    0x34
00066 #define I2C_SLV4_DI      0x35
00067 #define I2C_MST_STATUS   0x36
00068 #define INT_PIN_CFG      0x37
00069 #define INT_ENABLE       0x38
00070 #define DMP_INT_STATUS   0x39  // Check DMP interrupt
00071 #define INT_STATUS       0x3A
00072 #define ACCEL_XOUT_H     0x3B
00073 #define ACCEL_XOUT_L     0x3C
00074 #define ACCEL_YOUT_H     0x3D
00075 #define ACCEL_YOUT_L     0x3E
00076 #define ACCEL_ZOUT_H     0x3F
00077 #define ACCEL_ZOUT_L     0x40
00078 #define TEMP_OUT_H       0x41
00079 #define TEMP_OUT_L       0x42
00080 #define GYRO_XOUT_H      0x43
00081 #define GYRO_XOUT_L      0x44
00082 #define GYRO_YOUT_H      0x45
00083 #define GYRO_YOUT_L      0x46
00084 #define GYRO_ZOUT_H      0x47
00085 #define GYRO_ZOUT_L      0x48
00086 #define EXT_SENS_DATA_00 0x49
00087 #define EXT_SENS_DATA_01 0x4A
00088 #define EXT_SENS_DATA_02 0x4B
00089 #define EXT_SENS_DATA_03 0x4C
00090 #define EXT_SENS_DATA_04 0x4D
00091 #define EXT_SENS_DATA_05 0x4E
00092 #define EXT_SENS_DATA_06 0x4F
00093 #define EXT_SENS_DATA_07 0x50
00094 #define EXT_SENS_DATA_08 0x51
00095 #define EXT_SENS_DATA_09 0x52
00096 #define EXT_SENS_DATA_10 0x53
00097 #define EXT_SENS_DATA_11 0x54
00098 #define EXT_SENS_DATA_12 0x55
00099 #define EXT_SENS_DATA_13 0x56
00100 #define EXT_SENS_DATA_14 0x57
00101 #define EXT_SENS_DATA_15 0x58
00102 #define EXT_SENS_DATA_16 0x59
00103 #define EXT_SENS_DATA_17 0x5A
00104 #define EXT_SENS_DATA_18 0x5B
00105 #define EXT_SENS_DATA_19 0x5C
00106 #define EXT_SENS_DATA_20 0x5D
00107 #define EXT_SENS_DATA_21 0x5E
00108 #define EXT_SENS_DATA_22 0x5F
00109 #define EXT_SENS_DATA_23 0x60
00110 #define MOT_DETECT_STATUS 0x61
00111 #define I2C_SLV0_DO      0x63
00112 #define I2C_SLV1_DO      0x64
00113 #define I2C_SLV2_DO      0x65
00114 #define I2C_SLV3_DO      0x66
00115 #define I2C_MST_DELAY_CTRL 0x67
00116 #define SIGNAL_PATH_RESET  0x68
00117 #define MOT_DETECT_CTRL   0x69
00118 #define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
00119 #define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
00120 #define PWR_MGMT_2       0x6C
00121 #define DMP_BANK         0x6D  // Activates a specific bank in the DMP
00122 #define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
00123 #define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
00124 #define DMP_REG_1        0x70
00125 #define DMP_REG_2        0x71
00126 #define FIFO_COUNTH      0x72
00127 #define FIFO_COUNTL      0x73
00128 #define FIFO_R_W         0x74
00129 #define WHO_AM_I_MPU6050 0x75 // Should return 0x68
00130 
00131 // Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
00132 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
00133 #define ADO 0
00134 #if ADO
00135 #define MPU6050_ADDRESS 0x69<<1  // Device address when ADO = 1
00136 #else
00137 #define MPU6050_ADDRESS 0x68<<1  // Device address when ADO = 0
00138 #endif
00139 
00140 #ifndef TRUE
00141 #define TRUE    true
00142 #endif
00143 #ifndef FALSE
00144 #define FALSE   false
00145 #endif
00146 
00147 // Set initial input parameters
00148 enum Ascale {
00149   AFS_2G = 0,
00150   AFS_4G,
00151   AFS_8G,
00152   AFS_16G
00153 };
00154 
00155 enum Gscale {
00156   GFS_250DPS = 0,
00157   GFS_500DPS,
00158   GFS_1000DPS,
00159   GFS_2000DPS
00160 };
00161 
00162 typedef struct {
00163     int ax;
00164     int ay;
00165     int az;
00166     int yaw;
00167     int pitch;
00168     int roll;
00169 } MPU_data_type;
00170 
00171 
00172 class MPU6050 {
00173  
00174     protected:
00175     
00176     private:
00177     
00178         void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
00179 
00180         char readByte(uint8_t address, uint8_t subAddress);
00181 
00182         void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
00183 
00184     public:
00185   //===================================================================================================================
00186 //====== Set of useful function to access acceleratio, gyroscope, and temperature data
00187 //===================================================================================================================
00188 
00189         
00190         void getGres();
00191 
00192         void getAres();
00193 
00194         void readAccelData(int16_t * destination);
00195 
00196         void readGyroData(int16_t * destination);
00197 
00198         int16_t readTempData();
00199 
00200         void LowPowerAccelOnly();
00201 
00202         void resetMPU6050();
00203  
00204         void initMPU6050();
00205 
00206         void calibrateMPU6050(float * dest1, float * dest2);
00207 
00208         void MPU6050SelfTest(float * destination);
00209 
00210         void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz);
00211         
00212         bool motion_sensor_init();
00213   
00214         bool motion_update_data(MPU_data_type *new_data, int current_time_us);
00215          
00216   };
00217   
00218   void MPU6050_set_I2C_freq(int i2c_frequency);
00219   
00220   
00221   
00222   
00223 #endif