MPU6050.cpp

00001 #include "MPU6050.h"
00002 
00003 
00004 
00005 #if defined(__TT_M4G9__)
00006 #define DEFAULT_SDA_PIN PG2
00007 #define DEFAULT_SCL_PIN PG3
00008 #endif
00009 
00010 #if defined(__TT_M3HQ__)
00011 #define DEFAULT_SDA_PIN I2C_SDA
00012 #define DEFAULT_SCL_PIN I2C_SCL
00013 #endif
00014 
00015 
00016 
00017 
00018 #define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
00019 #define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
00020 #define MPU6050_DEFAULT_ADDRESS     (MPU6050_ADDRESS_AD0_LOW << 1)
00021 
00022 
00023 #define MPU6050_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
00024 #define MPU6050_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
00025 #define MPU6050_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
00026 #define MPU6050_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN
00027 #define MPU6050_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN
00028 #define MPU6050_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN
00029 #define MPU6050_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
00030 #define MPU6050_RA_XA_OFFS_L_TC     0x07
00031 #define MPU6050_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS
00032 #define MPU6050_RA_YA_OFFS_L_TC     0x09
00033 #define MPU6050_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
00034 #define MPU6050_RA_ZA_OFFS_L_TC     0x0B
00035 #define MPU6050_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
00036 #define MPU6050_RA_XG_OFFS_USRL     0x14
00037 #define MPU6050_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR
00038 #define MPU6050_RA_YG_OFFS_USRL     0x16
00039 #define MPU6050_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR
00040 #define MPU6050_RA_ZG_OFFS_USRL     0x18
00041 #define MPU6050_RA_SMPLRT_DIV       0x19
00042 #define MPU6050_RA_CONFIG           0x1A
00043 #define MPU6050_RA_GYRO_CONFIG      0x1B
00044 #define MPU6050_RA_ACCEL_CONFIG     0x1C
00045 #define MPU6050_RA_FF_THR           0x1D
00046 #define MPU6050_RA_FF_DUR           0x1E
00047 #define MPU6050_RA_MOT_THR          0x1F
00048 #define MPU6050_RA_MOT_DUR          0x20
00049 #define MPU6050_RA_ZRMOT_THR        0x21
00050 #define MPU6050_RA_ZRMOT_DUR        0x22
00051 #define MPU6050_RA_FIFO_EN          0x23
00052 #define MPU6050_RA_I2C_MST_CTRL     0x24
00053 #define MPU6050_RA_I2C_SLV0_ADDR    0x25
00054 #define MPU6050_RA_I2C_SLV0_REG     0x26
00055 #define MPU6050_RA_I2C_SLV0_CTRL    0x27
00056 #define MPU6050_RA_I2C_SLV1_ADDR    0x28
00057 #define MPU6050_RA_I2C_SLV1_REG     0x29
00058 #define MPU6050_RA_I2C_SLV1_CTRL    0x2A
00059 #define MPU6050_RA_I2C_SLV2_ADDR    0x2B
00060 #define MPU6050_RA_I2C_SLV2_REG     0x2C
00061 #define MPU6050_RA_I2C_SLV2_CTRL    0x2D
00062 #define MPU6050_RA_I2C_SLV3_ADDR    0x2E
00063 #define MPU6050_RA_I2C_SLV3_REG     0x2F
00064 #define MPU6050_RA_I2C_SLV3_CTRL    0x30
00065 #define MPU6050_RA_I2C_SLV4_ADDR    0x31
00066 #define MPU6050_RA_I2C_SLV4_REG     0x32
00067 #define MPU6050_RA_I2C_SLV4_DO      0x33
00068 #define MPU6050_RA_I2C_SLV4_CTRL    0x34
00069 #define MPU6050_RA_I2C_SLV4_DI      0x35
00070 #define MPU6050_RA_I2C_MST_STATUS   0x36
00071 #define MPU6050_RA_INT_PIN_CFG      0x37
00072 #define MPU6050_RA_INT_ENABLE       0x38
00073 #define MPU6050_RA_DMP_INT_STATUS   0x39
00074 #define MPU6050_RA_INT_STATUS       0x3A
00075 #define MPU6050_RA_ACCEL_XOUT_H     0x3B
00076 #define MPU6050_RA_ACCEL_XOUT_L     0x3C
00077 #define MPU6050_RA_ACCEL_YOUT_H     0x3D
00078 #define MPU6050_RA_ACCEL_YOUT_L     0x3E
00079 #define MPU6050_RA_ACCEL_ZOUT_H     0x3F
00080 #define MPU6050_RA_ACCEL_ZOUT_L     0x40
00081 #define MPU6050_RA_TEMP_OUT_H       0x41
00082 #define MPU6050_RA_TEMP_OUT_L       0x42
00083 #define MPU6050_RA_GYRO_XOUT_H      0x43
00084 #define MPU6050_RA_GYRO_XOUT_L      0x44
00085 #define MPU6050_RA_GYRO_YOUT_H      0x45
00086 #define MPU6050_RA_GYRO_YOUT_L      0x46
00087 #define MPU6050_RA_GYRO_ZOUT_H      0x47
00088 #define MPU6050_RA_GYRO_ZOUT_L      0x48
00089 #define MPU6050_RA_EXT_SENS_DATA_00 0x49
00090 #define MPU6050_RA_EXT_SENS_DATA_01 0x4A
00091 #define MPU6050_RA_EXT_SENS_DATA_02 0x4B
00092 #define MPU6050_RA_EXT_SENS_DATA_03 0x4C
00093 #define MPU6050_RA_EXT_SENS_DATA_04 0x4D
00094 #define MPU6050_RA_EXT_SENS_DATA_05 0x4E
00095 #define MPU6050_RA_EXT_SENS_DATA_06 0x4F
00096 #define MPU6050_RA_EXT_SENS_DATA_07 0x50
00097 #define MPU6050_RA_EXT_SENS_DATA_08 0x51
00098 #define MPU6050_RA_EXT_SENS_DATA_09 0x52
00099 #define MPU6050_RA_EXT_SENS_DATA_10 0x53
00100 #define MPU6050_RA_EXT_SENS_DATA_11 0x54
00101 #define MPU6050_RA_EXT_SENS_DATA_12 0x55
00102 #define MPU6050_RA_EXT_SENS_DATA_13 0x56
00103 #define MPU6050_RA_EXT_SENS_DATA_14 0x57
00104 #define MPU6050_RA_EXT_SENS_DATA_15 0x58
00105 #define MPU6050_RA_EXT_SENS_DATA_16 0x59
00106 #define MPU6050_RA_EXT_SENS_DATA_17 0x5A
00107 #define MPU6050_RA_EXT_SENS_DATA_18 0x5B
00108 #define MPU6050_RA_EXT_SENS_DATA_19 0x5C
00109 #define MPU6050_RA_EXT_SENS_DATA_20 0x5D
00110 #define MPU6050_RA_EXT_SENS_DATA_21 0x5E
00111 #define MPU6050_RA_EXT_SENS_DATA_22 0x5F
00112 #define MPU6050_RA_EXT_SENS_DATA_23 0x60
00113 #define MPU6050_RA_MOT_DETECT_STATUS    0x61
00114 #define MPU6050_RA_I2C_SLV0_DO      0x63
00115 #define MPU6050_RA_I2C_SLV1_DO      0x64
00116 #define MPU6050_RA_I2C_SLV2_DO      0x65
00117 #define MPU6050_RA_I2C_SLV3_DO      0x66
00118 #define MPU6050_RA_I2C_MST_DELAY_CTRL   0x67
00119 #define MPU6050_RA_SIGNAL_PATH_RESET    0x68
00120 #define MPU6050_RA_MOT_DETECT_CTRL      0x69
00121 #define MPU6050_RA_USER_CTRL        0x6A
00122 #define MPU6050_RA_PWR_MGMT_1       0x6B
00123 #define MPU6050_RA_PWR_MGMT_2       0x6C
00124 #define MPU6050_RA_BANK_SEL         0x6D
00125 #define MPU6050_RA_MEM_START_ADDR   0x6E
00126 #define MPU6050_RA_MEM_R_W          0x6F
00127 #define MPU6050_RA_DMP_CFG_1        0x70
00128 #define MPU6050_RA_DMP_CFG_2        0x71
00129 #define MPU6050_RA_FIFO_COUNTH      0x72
00130 #define MPU6050_RA_FIFO_COUNTL      0x73
00131 #define MPU6050_RA_FIFO_R_W         0x74
00132 #define MPU6050_RA_WHO_AM_I         0x75
00133 
00134 
00135 
00136 
00137 #define MPU6050_TC_PWR_MODE_BIT     7
00138 #define MPU6050_TC_OFFSET_BIT       6
00139 #define MPU6050_TC_OFFSET_LENGTH    6
00140 #define MPU6050_TC_OTP_BNK_VLD_BIT  0
00141 
00142 #define MPU6050_VDDIO_LEVEL_VLOGIC  0
00143 #define MPU6050_VDDIO_LEVEL_VDD     1
00144 
00145 #define MPU6050_CFG_EXT_SYNC_SET_BIT    5
00146 #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
00147 #define MPU6050_CFG_DLPF_CFG_BIT    2
00148 #define MPU6050_CFG_DLPF_CFG_LENGTH 3
00149 
00150 #define MPU6050_EXT_SYNC_DISABLED       0x0
00151 #define MPU6050_EXT_SYNC_TEMP_OUT_L     0x1
00152 #define MPU6050_EXT_SYNC_GYRO_XOUT_L    0x2
00153 #define MPU6050_EXT_SYNC_GYRO_YOUT_L    0x3
00154 #define MPU6050_EXT_SYNC_GYRO_ZOUT_L    0x4
00155 #define MPU6050_EXT_SYNC_ACCEL_XOUT_L   0x5
00156 #define MPU6050_EXT_SYNC_ACCEL_YOUT_L   0x6
00157 #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L   0x7
00158 
00159 #define MPU6050_DLPF_BW_256         0x00
00160 #define MPU6050_DLPF_BW_188         0x01
00161 #define MPU6050_DLPF_BW_98          0x02
00162 #define MPU6050_DLPF_BW_42          0x03
00163 #define MPU6050_DLPF_BW_20          0x04
00164 #define MPU6050_DLPF_BW_10          0x05
00165 #define MPU6050_DLPF_BW_5           0x06
00166 
00167 #define MPU6050_GCONFIG_FS_SEL_BIT      4
00168 #define MPU6050_GCONFIG_FS_SEL_LENGTH   2
00169 
00170 #define MPU6050_GYRO_FS_250         0x00
00171 #define MPU6050_GYRO_FS_500         0x01
00172 #define MPU6050_GYRO_FS_1000        0x02
00173 #define MPU6050_GYRO_FS_2000        0x03
00174 
00175 #define MPU6050_ACONFIG_XA_ST_BIT           7
00176 #define MPU6050_ACONFIG_YA_ST_BIT           6
00177 #define MPU6050_ACONFIG_ZA_ST_BIT           5
00178 #define MPU6050_ACONFIG_AFS_SEL_BIT         4
00179 #define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
00180 #define MPU6050_ACONFIG_ACCEL_HPF_BIT       2
00181 #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3
00182 
00183 #define MPU6050_ACCEL_FS_2          0x00
00184 #define MPU6050_ACCEL_FS_4          0x01
00185 #define MPU6050_ACCEL_FS_8          0x02
00186 #define MPU6050_ACCEL_FS_16         0x03
00187 
00188 #define MPU6050_DHPF_RESET          0x00
00189 #define MPU6050_DHPF_5              0x01
00190 #define MPU6050_DHPF_2P5            0x02
00191 #define MPU6050_DHPF_1P25           0x03
00192 #define MPU6050_DHPF_0P63           0x04
00193 #define MPU6050_DHPF_HOLD           0x07
00194 
00195 #define MPU6050_TEMP_FIFO_EN_BIT    7
00196 #define MPU6050_XG_FIFO_EN_BIT      6
00197 #define MPU6050_YG_FIFO_EN_BIT      5
00198 #define MPU6050_ZG_FIFO_EN_BIT      4
00199 #define MPU6050_ACCEL_FIFO_EN_BIT   3
00200 #define MPU6050_SLV2_FIFO_EN_BIT    2
00201 #define MPU6050_SLV1_FIFO_EN_BIT    1
00202 #define MPU6050_SLV0_FIFO_EN_BIT    0
00203 
00204 #define MPU6050_MULT_MST_EN_BIT     7
00205 #define MPU6050_WAIT_FOR_ES_BIT     6
00206 #define MPU6050_SLV_3_FIFO_EN_BIT   5
00207 #define MPU6050_I2C_MST_P_NSR_BIT   4
00208 #define MPU6050_I2C_MST_CLK_BIT     3
00209 #define MPU6050_I2C_MST_CLK_LENGTH  4
00210 
00211 #define MPU6050_CLOCK_DIV_348       0x0
00212 #define MPU6050_CLOCK_DIV_333       0x1
00213 #define MPU6050_CLOCK_DIV_320       0x2
00214 #define MPU6050_CLOCK_DIV_308       0x3
00215 #define MPU6050_CLOCK_DIV_296       0x4
00216 #define MPU6050_CLOCK_DIV_286       0x5
00217 #define MPU6050_CLOCK_DIV_276       0x6
00218 #define MPU6050_CLOCK_DIV_267       0x7
00219 #define MPU6050_CLOCK_DIV_258       0x8
00220 #define MPU6050_CLOCK_DIV_500       0x9
00221 #define MPU6050_CLOCK_DIV_471       0xA
00222 #define MPU6050_CLOCK_DIV_444       0xB
00223 #define MPU6050_CLOCK_DIV_421       0xC
00224 #define MPU6050_CLOCK_DIV_400       0xD
00225 #define MPU6050_CLOCK_DIV_381       0xE
00226 #define MPU6050_CLOCK_DIV_364       0xF
00227 
00228 #define MPU6050_I2C_SLV_RW_BIT      7
00229 #define MPU6050_I2C_SLV_ADDR_BIT    6
00230 #define MPU6050_I2C_SLV_ADDR_LENGTH 7
00231 #define MPU6050_I2C_SLV_EN_BIT      7
00232 #define MPU6050_I2C_SLV_BYTE_SW_BIT 6
00233 #define MPU6050_I2C_SLV_REG_DIS_BIT 5
00234 #define MPU6050_I2C_SLV_GRP_BIT     4
00235 #define MPU6050_I2C_SLV_LEN_BIT     3
00236 #define MPU6050_I2C_SLV_LEN_LENGTH  4
00237 
00238 #define MPU6050_I2C_SLV4_RW_BIT         7
00239 #define MPU6050_I2C_SLV4_ADDR_BIT       6
00240 #define MPU6050_I2C_SLV4_ADDR_LENGTH    7
00241 #define MPU6050_I2C_SLV4_EN_BIT         7
00242 #define MPU6050_I2C_SLV4_INT_EN_BIT     6
00243 #define MPU6050_I2C_SLV4_REG_DIS_BIT    5
00244 #define MPU6050_I2C_SLV4_MST_DLY_BIT    4
00245 #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
00246 
00247 #define MPU6050_MST_PASS_THROUGH_BIT    7
00248 #define MPU6050_MST_I2C_SLV4_DONE_BIT   6
00249 #define MPU6050_MST_I2C_LOST_ARB_BIT    5
00250 #define MPU6050_MST_I2C_SLV4_NACK_BIT   4
00251 #define MPU6050_MST_I2C_SLV3_NACK_BIT   3
00252 #define MPU6050_MST_I2C_SLV2_NACK_BIT   2
00253 #define MPU6050_MST_I2C_SLV1_NACK_BIT   1
00254 #define MPU6050_MST_I2C_SLV0_NACK_BIT   0
00255 
00256 #define MPU6050_INTCFG_INT_LEVEL_BIT        7
00257 #define MPU6050_INTCFG_INT_OPEN_BIT         6
00258 #define MPU6050_INTCFG_LATCH_INT_EN_BIT     5
00259 #define MPU6050_INTCFG_INT_RD_CLEAR_BIT     4
00260 #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT  3
00261 #define MPU6050_INTCFG_FSYNC_INT_EN_BIT     2
00262 #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT    1
00263 #define MPU6050_INTCFG_CLKOUT_EN_BIT        0
00264 
00265 #define MPU6050_INTMODE_ACTIVEHIGH  0x00
00266 #define MPU6050_INTMODE_ACTIVELOW   0x01
00267 
00268 #define MPU6050_INTDRV_PUSHPULL     0x00
00269 #define MPU6050_INTDRV_OPENDRAIN    0x01
00270 
00271 #define MPU6050_INTLATCH_50USPULSE  0x00
00272 #define MPU6050_INTLATCH_WAITCLEAR  0x01
00273 
00274 #define MPU6050_INTCLEAR_STATUSREAD 0x00
00275 #define MPU6050_INTCLEAR_ANYREAD    0x01
00276 
00277 #define MPU6050_INTERRUPT_FF_BIT            7
00278 #define MPU6050_INTERRUPT_MOT_BIT           6
00279 #define MPU6050_INTERRUPT_ZMOT_BIT          5
00280 #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
00281 #define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
00282 #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
00283 #define MPU6050_INTERRUPT_DMP_INT_BIT       1
00284 #define MPU6050_INTERRUPT_DATA_RDY_BIT      0
00285 
00286 // TODO: figure out what these actually do
00287 // UMPL source code is not very obivous
00288 #define MPU6050_DMPINT_5_BIT            5
00289 #define MPU6050_DMPINT_4_BIT            4
00290 #define MPU6050_DMPINT_3_BIT            3
00291 #define MPU6050_DMPINT_2_BIT            2
00292 #define MPU6050_DMPINT_1_BIT            1
00293 #define MPU6050_DMPINT_0_BIT            0
00294 
00295 #define MPU6050_MOTION_MOT_XNEG_BIT     7
00296 #define MPU6050_MOTION_MOT_XPOS_BIT     6
00297 #define MPU6050_MOTION_MOT_YNEG_BIT     5
00298 #define MPU6050_MOTION_MOT_YPOS_BIT     4
00299 #define MPU6050_MOTION_MOT_ZNEG_BIT     3
00300 #define MPU6050_MOTION_MOT_ZPOS_BIT     2
00301 #define MPU6050_MOTION_MOT_ZRMOT_BIT    0
00302 
00303 #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
00304 #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
00305 #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
00306 #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
00307 #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
00308 #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0
00309 
00310 #define MPU6050_PATHRESET_GYRO_RESET_BIT    2
00311 #define MPU6050_PATHRESET_ACCEL_RESET_BIT   1
00312 #define MPU6050_PATHRESET_TEMP_RESET_BIT    0
00313 
00314 #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT       5
00315 #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH    2
00316 #define MPU6050_DETECT_FF_COUNT_BIT             3
00317 #define MPU6050_DETECT_FF_COUNT_LENGTH          2
00318 #define MPU6050_DETECT_MOT_COUNT_BIT            1
00319 #define MPU6050_DETECT_MOT_COUNT_LENGTH         2
00320 
00321 #define MPU6050_DETECT_DECREMENT_RESET  0x0
00322 #define MPU6050_DETECT_DECREMENT_1      0x1
00323 #define MPU6050_DETECT_DECREMENT_2      0x2
00324 #define MPU6050_DETECT_DECREMENT_4      0x3
00325 
00326 #define MPU6050_USERCTRL_DMP_EN_BIT             7
00327 #define MPU6050_USERCTRL_FIFO_EN_BIT            6
00328 #define MPU6050_USERCTRL_I2C_MST_EN_BIT         5
00329 #define MPU6050_USERCTRL_I2C_IF_DIS_BIT         4
00330 #define MPU6050_USERCTRL_DMP_RESET_BIT          3
00331 #define MPU6050_USERCTRL_FIFO_RESET_BIT         2
00332 #define MPU6050_USERCTRL_I2C_MST_RESET_BIT      1
00333 #define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0
00334 
00335 #define MPU6050_PWR1_DEVICE_RESET_BIT   7
00336 #define MPU6050_PWR1_SLEEP_BIT          6
00337 #define MPU6050_PWR1_CYCLE_BIT          5
00338 #define MPU6050_PWR1_TEMP_DIS_BIT       3
00339 #define MPU6050_PWR1_CLKSEL_BIT         2
00340 #define MPU6050_PWR1_CLKSEL_LENGTH      3
00341 
00342 #define MPU6050_CLOCK_INTERNAL          0x00
00343 #define MPU6050_CLOCK_PLL_XGYRO         0x01
00344 #define MPU6050_CLOCK_PLL_YGYRO         0x02
00345 #define MPU6050_CLOCK_PLL_ZGYRO         0x03
00346 #define MPU6050_CLOCK_PLL_EXT32K        0x04
00347 #define MPU6050_CLOCK_PLL_EXT19M        0x05
00348 #define MPU6050_CLOCK_KEEP_RESET        0x07
00349 
00350 #define MPU6050_PWR2_LP_WAKE_CTRL_BIT       7
00351 #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH    2
00352 #define MPU6050_PWR2_STBY_XA_BIT            5
00353 #define MPU6050_PWR2_STBY_YA_BIT            4
00354 #define MPU6050_PWR2_STBY_ZA_BIT            3
00355 #define MPU6050_PWR2_STBY_XG_BIT            2
00356 #define MPU6050_PWR2_STBY_YG_BIT            1
00357 #define MPU6050_PWR2_STBY_ZG_BIT            0
00358 
00359 #define MPU6050_WAKE_FREQ_1P25      0x0
00360 #define MPU6050_WAKE_FREQ_2P5       0x1
00361 #define MPU6050_WAKE_FREQ_5         0x2
00362 #define MPU6050_WAKE_FREQ_10        0x3
00363 
00364 #define MPU6050_BANKSEL_PRFTCH_EN_BIT       6
00365 #define MPU6050_BANKSEL_CFG_USER_BANK_BIT   5
00366 #define MPU6050_BANKSEL_MEM_SEL_BIT         4
00367 #define MPU6050_BANKSEL_MEM_SEL_LENGTH      5
00368 
00369 #define MPU6050_WHO_AM_I_BIT        6
00370 #define MPU6050_WHO_AM_I_LENGTH     6
00371 
00372 #define MPU6050_DMP_MEMORY_BANKS        8
00373 #define MPU6050_DMP_MEMORY_BANK_SIZE    256
00374 #define MPU6050_DMP_MEMORY_CHUNK_SIZE   16
00375 
00376 
00377 
00378 
00379 #define SUCCESS_STATUS 0
00380 #define ONE_BYTE_LENGTH 1
00381 #define TWO_BYTE_LENGTH 2
00382 #define LOCATION_ONE 0
00383 #define LOCATION_TWO 1
00384 
00385 MPU6050::MPU6050(PinName sda,PinName scl) : I2C(sda,scl)
00386 {
00387     init_sensor();
00388 }
00389 
00390 MPU6050::MPU6050() : I2C(DEFAULT_SDA_PIN,DEFAULT_SCL_PIN)
00391 {
00392     init_sensor();
00393 }
00394 
00395 uint8_t MPU6050::getDeviceID()
00396 {
00397     char temp_data = MPU6050_RA_WHO_AM_I;
00398     int status ;
00399     //write address.
00400     status = write(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
00401     if(status != SUCCESS_STATUS)
00402     {
00403         return 0xFF;
00404     }
00405     //
00406     status = read(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
00407     if(status != SUCCESS_STATUS)
00408     {
00409         return 0xFF;
00410     }
00411     return temp_data;
00412 }
00413 
00414 uint8_t MPU6050::readByte(uint8_t register_address)
00415 {
00416     char temp_data = register_address;
00417     write(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
00418     read(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
00419     return temp_data;
00420 }
00421 
00422 uint8_t MPU6050::writeByte(uint8_t register_address,uint8_t value)
00423 {
00424     char temp_buffer[TWO_BYTE_LENGTH];
00425     temp_buffer[LOCATION_ONE] = register_address;
00426     temp_buffer[LOCATION_TWO] = value;
00427     return write(MPU6050_DEFAULT_ADDRESS,temp_buffer,TWO_BYTE_LENGTH);
00428 }
00429 
00430 void MPU6050::init_sensor()
00431 {
00432     setClockSource(MPU6050_CLOCK_PLL_XGYRO);
00433     setFullScaleGyroRange(MPU6050_GYRO_FS_250);
00434     setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00435 }
00436 
00437 void MPU6050::setClockSource(uint8_t value)
00438 {
00439     writeByte(MPU6050_RA_PWR_MGMT_1,value);
00440 }
00441 
00442 void MPU6050::setFullScaleAccelRange(uint8_t value)
00443 {
00444     writeByte(MPU6050_RA_ACCEL_CONFIG,value);
00445 }
00446 
00447 void MPU6050::setFullScaleGyroRange(uint8_t value)
00448 {
00449     writeByte(MPU6050_RA_GYRO_CONFIG,value);
00450 }
00451 
00452 uint16_t MPU6050::getX()
00453 {
00454     return readByte(MPU6050_RA_ACCEL_XOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_XOUT_L);
00455 }
00456 
00457 uint16_t MPU6050::getY()
00458 {
00459     return readByte(MPU6050_RA_ACCEL_YOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_YOUT_L);
00460 }
00461 
00462 uint16_t MPU6050::getZ()
00463 {
00464     return readByte(MPU6050_RA_ACCEL_ZOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_ZOUT_L);
00465 }
00466 
00467 uint16_t MPU6050::getGX()
00468 {
00469     return readByte(MPU6050_RA_GYRO_XOUT_H) << 8 | readByte(MPU6050_RA_GYRO_XOUT_L);
00470 }
00471 
00472 uint16_t MPU6050::getGY()
00473 {
00474     return readByte(MPU6050_RA_GYRO_YOUT_H) << 8 | readByte(MPU6050_RA_GYRO_YOUT_L);
00475 }
00476 
00477 uint16_t MPU6050::getGZ()
00478 {
00479     return readByte(MPU6050_RA_GYRO_ZOUT_H) << 8 | readByte(MPU6050_RA_GYRO_ZOUT_L);
00480 }