Thundersoft / Mbed OS MPU6050_for_TT_Mxx

Creating a project for TT_Mxx

MPU6050/MPU6050.cpp@1:05a211e75d38, 2019-04-26 (annotated)

Committer:
ThunderSoft
Date:
Fri Apr 26 09:46:22 2019 +0000
Revision:
1:05a211e75d38
Parent:
0:53c9fac03371 
"update the mbed-os code to support TT_M4G9"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThunderSoft 0:53c9fac03371 1 #include "MPU6050.h"
ThunderSoft 0:53c9fac03371 2
ThunderSoft 0:53c9fac03371 3
ThunderSoft 0:53c9fac03371 4
ThunderSoft 0:53c9fac03371 5 #if defined(__TT_M4G9__)
ThunderSoft 0:53c9fac03371 6 #define DEFAULT_SDA_PIN PG2
ThunderSoft 0:53c9fac03371 7 #define DEFAULT_SCL_PIN PG3
ThunderSoft 0:53c9fac03371 8 #endif
ThunderSoft 0:53c9fac03371 9
ThunderSoft 0:53c9fac03371 10 #if defined(__TT_M3HQ__)
ThunderSoft 0:53c9fac03371 11 #define DEFAULT_SDA_PIN I2C_SDA
ThunderSoft 0:53c9fac03371 12 #define DEFAULT_SCL_PIN I2C_SCL
ThunderSoft 0:53c9fac03371 13 #endif
ThunderSoft 0:53c9fac03371 14
ThunderSoft 0:53c9fac03371 15
ThunderSoft 0:53c9fac03371 16
ThunderSoft 0:53c9fac03371 17
ThunderSoft 0:53c9fac03371 18 #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
ThunderSoft 0:53c9fac03371 19 #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
ThunderSoft 0:53c9fac03371 20 #define MPU6050_DEFAULT_ADDRESS (MPU6050_ADDRESS_AD0_LOW << 1)
ThunderSoft 0:53c9fac03371 21
ThunderSoft 0:53c9fac03371 22
ThunderSoft 0:53c9fac03371 23 #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
ThunderSoft 0:53c9fac03371 24 #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
ThunderSoft 0:53c9fac03371 25 #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
ThunderSoft 0:53c9fac03371 26 #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
ThunderSoft 0:53c9fac03371 27 #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
ThunderSoft 0:53c9fac03371 28 #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
ThunderSoft 0:53c9fac03371 29 #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
ThunderSoft 0:53c9fac03371 30 #define MPU6050_RA_XA_OFFS_L_TC 0x07
ThunderSoft 0:53c9fac03371 31 #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
ThunderSoft 0:53c9fac03371 32 #define MPU6050_RA_YA_OFFS_L_TC 0x09
ThunderSoft 0:53c9fac03371 33 #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
ThunderSoft 0:53c9fac03371 34 #define MPU6050_RA_ZA_OFFS_L_TC 0x0B
ThunderSoft 0:53c9fac03371 35 #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
ThunderSoft 0:53c9fac03371 36 #define MPU6050_RA_XG_OFFS_USRL 0x14
ThunderSoft 0:53c9fac03371 37 #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
ThunderSoft 0:53c9fac03371 38 #define MPU6050_RA_YG_OFFS_USRL 0x16
ThunderSoft 0:53c9fac03371 39 #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
ThunderSoft 0:53c9fac03371 40 #define MPU6050_RA_ZG_OFFS_USRL 0x18
ThunderSoft 0:53c9fac03371 41 #define MPU6050_RA_SMPLRT_DIV 0x19
ThunderSoft 0:53c9fac03371 42 #define MPU6050_RA_CONFIG 0x1A
ThunderSoft 0:53c9fac03371 43 #define MPU6050_RA_GYRO_CONFIG 0x1B
ThunderSoft 0:53c9fac03371 44 #define MPU6050_RA_ACCEL_CONFIG 0x1C
ThunderSoft 0:53c9fac03371 45 #define MPU6050_RA_FF_THR 0x1D
ThunderSoft 0:53c9fac03371 46 #define MPU6050_RA_FF_DUR 0x1E
ThunderSoft 0:53c9fac03371 47 #define MPU6050_RA_MOT_THR 0x1F
ThunderSoft 0:53c9fac03371 48 #define MPU6050_RA_MOT_DUR 0x20
ThunderSoft 0:53c9fac03371 49 #define MPU6050_RA_ZRMOT_THR 0x21
ThunderSoft 0:53c9fac03371 50 #define MPU6050_RA_ZRMOT_DUR 0x22
ThunderSoft 0:53c9fac03371 51 #define MPU6050_RA_FIFO_EN 0x23
ThunderSoft 0:53c9fac03371 52 #define MPU6050_RA_I2C_MST_CTRL 0x24
ThunderSoft 0:53c9fac03371 53 #define MPU6050_RA_I2C_SLV0_ADDR 0x25
ThunderSoft 0:53c9fac03371 54 #define MPU6050_RA_I2C_SLV0_REG 0x26
ThunderSoft 0:53c9fac03371 55 #define MPU6050_RA_I2C_SLV0_CTRL 0x27
ThunderSoft 0:53c9fac03371 56 #define MPU6050_RA_I2C_SLV1_ADDR 0x28
ThunderSoft 0:53c9fac03371 57 #define MPU6050_RA_I2C_SLV1_REG 0x29
ThunderSoft 0:53c9fac03371 58 #define MPU6050_RA_I2C_SLV1_CTRL 0x2A
ThunderSoft 0:53c9fac03371 59 #define MPU6050_RA_I2C_SLV2_ADDR 0x2B
ThunderSoft 0:53c9fac03371 60 #define MPU6050_RA_I2C_SLV2_REG 0x2C
ThunderSoft 0:53c9fac03371 61 #define MPU6050_RA_I2C_SLV2_CTRL 0x2D
ThunderSoft 0:53c9fac03371 62 #define MPU6050_RA_I2C_SLV3_ADDR 0x2E
ThunderSoft 0:53c9fac03371 63 #define MPU6050_RA_I2C_SLV3_REG 0x2F
ThunderSoft 0:53c9fac03371 64 #define MPU6050_RA_I2C_SLV3_CTRL 0x30
ThunderSoft 0:53c9fac03371 65 #define MPU6050_RA_I2C_SLV4_ADDR 0x31
ThunderSoft 0:53c9fac03371 66 #define MPU6050_RA_I2C_SLV4_REG 0x32
ThunderSoft 0:53c9fac03371 67 #define MPU6050_RA_I2C_SLV4_DO 0x33
ThunderSoft 0:53c9fac03371 68 #define MPU6050_RA_I2C_SLV4_CTRL 0x34
ThunderSoft 0:53c9fac03371 69 #define MPU6050_RA_I2C_SLV4_DI 0x35
ThunderSoft 0:53c9fac03371 70 #define MPU6050_RA_I2C_MST_STATUS 0x36
ThunderSoft 0:53c9fac03371 71 #define MPU6050_RA_INT_PIN_CFG 0x37
ThunderSoft 0:53c9fac03371 72 #define MPU6050_RA_INT_ENABLE 0x38
ThunderSoft 0:53c9fac03371 73 #define MPU6050_RA_DMP_INT_STATUS 0x39
ThunderSoft 0:53c9fac03371 74 #define MPU6050_RA_INT_STATUS 0x3A
ThunderSoft 0:53c9fac03371 75 #define MPU6050_RA_ACCEL_XOUT_H 0x3B
ThunderSoft 0:53c9fac03371 76 #define MPU6050_RA_ACCEL_XOUT_L 0x3C
ThunderSoft 0:53c9fac03371 77 #define MPU6050_RA_ACCEL_YOUT_H 0x3D
ThunderSoft 0:53c9fac03371 78 #define MPU6050_RA_ACCEL_YOUT_L 0x3E
ThunderSoft 0:53c9fac03371 79 #define MPU6050_RA_ACCEL_ZOUT_H 0x3F
ThunderSoft 0:53c9fac03371 80 #define MPU6050_RA_ACCEL_ZOUT_L 0x40
ThunderSoft 0:53c9fac03371 81 #define MPU6050_RA_TEMP_OUT_H 0x41
ThunderSoft 0:53c9fac03371 82 #define MPU6050_RA_TEMP_OUT_L 0x42
ThunderSoft 0:53c9fac03371 83 #define MPU6050_RA_GYRO_XOUT_H 0x43
ThunderSoft 0:53c9fac03371 84 #define MPU6050_RA_GYRO_XOUT_L 0x44
ThunderSoft 0:53c9fac03371 85 #define MPU6050_RA_GYRO_YOUT_H 0x45
ThunderSoft 0:53c9fac03371 86 #define MPU6050_RA_GYRO_YOUT_L 0x46
ThunderSoft 0:53c9fac03371 87 #define MPU6050_RA_GYRO_ZOUT_H 0x47
ThunderSoft 0:53c9fac03371 88 #define MPU6050_RA_GYRO_ZOUT_L 0x48
ThunderSoft 0:53c9fac03371 89 #define MPU6050_RA_EXT_SENS_DATA_00 0x49
ThunderSoft 0:53c9fac03371 90 #define MPU6050_RA_EXT_SENS_DATA_01 0x4A
ThunderSoft 0:53c9fac03371 91 #define MPU6050_RA_EXT_SENS_DATA_02 0x4B
ThunderSoft 0:53c9fac03371 92 #define MPU6050_RA_EXT_SENS_DATA_03 0x4C
ThunderSoft 0:53c9fac03371 93 #define MPU6050_RA_EXT_SENS_DATA_04 0x4D
ThunderSoft 0:53c9fac03371 94 #define MPU6050_RA_EXT_SENS_DATA_05 0x4E
ThunderSoft 0:53c9fac03371 95 #define MPU6050_RA_EXT_SENS_DATA_06 0x4F
ThunderSoft 0:53c9fac03371 96 #define MPU6050_RA_EXT_SENS_DATA_07 0x50
ThunderSoft 0:53c9fac03371 97 #define MPU6050_RA_EXT_SENS_DATA_08 0x51
ThunderSoft 0:53c9fac03371 98 #define MPU6050_RA_EXT_SENS_DATA_09 0x52
ThunderSoft 0:53c9fac03371 99 #define MPU6050_RA_EXT_SENS_DATA_10 0x53
ThunderSoft 0:53c9fac03371 100 #define MPU6050_RA_EXT_SENS_DATA_11 0x54
ThunderSoft 0:53c9fac03371 101 #define MPU6050_RA_EXT_SENS_DATA_12 0x55
ThunderSoft 0:53c9fac03371 102 #define MPU6050_RA_EXT_SENS_DATA_13 0x56
ThunderSoft 0:53c9fac03371 103 #define MPU6050_RA_EXT_SENS_DATA_14 0x57
ThunderSoft 0:53c9fac03371 104 #define MPU6050_RA_EXT_SENS_DATA_15 0x58
ThunderSoft 0:53c9fac03371 105 #define MPU6050_RA_EXT_SENS_DATA_16 0x59
ThunderSoft 0:53c9fac03371 106 #define MPU6050_RA_EXT_SENS_DATA_17 0x5A
ThunderSoft 0:53c9fac03371 107 #define MPU6050_RA_EXT_SENS_DATA_18 0x5B
ThunderSoft 0:53c9fac03371 108 #define MPU6050_RA_EXT_SENS_DATA_19 0x5C
ThunderSoft 0:53c9fac03371 109 #define MPU6050_RA_EXT_SENS_DATA_20 0x5D
ThunderSoft 0:53c9fac03371 110 #define MPU6050_RA_EXT_SENS_DATA_21 0x5E
ThunderSoft 0:53c9fac03371 111 #define MPU6050_RA_EXT_SENS_DATA_22 0x5F
ThunderSoft 0:53c9fac03371 112 #define MPU6050_RA_EXT_SENS_DATA_23 0x60
ThunderSoft 0:53c9fac03371 113 #define MPU6050_RA_MOT_DETECT_STATUS 0x61
ThunderSoft 0:53c9fac03371 114 #define MPU6050_RA_I2C_SLV0_DO 0x63
ThunderSoft 0:53c9fac03371 115 #define MPU6050_RA_I2C_SLV1_DO 0x64
ThunderSoft 0:53c9fac03371 116 #define MPU6050_RA_I2C_SLV2_DO 0x65
ThunderSoft 0:53c9fac03371 117 #define MPU6050_RA_I2C_SLV3_DO 0x66
ThunderSoft 0:53c9fac03371 118 #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
ThunderSoft 0:53c9fac03371 119 #define MPU6050_RA_SIGNAL_PATH_RESET 0x68
ThunderSoft 0:53c9fac03371 120 #define MPU6050_RA_MOT_DETECT_CTRL 0x69
ThunderSoft 0:53c9fac03371 121 #define MPU6050_RA_USER_CTRL 0x6A
ThunderSoft 0:53c9fac03371 122 #define MPU6050_RA_PWR_MGMT_1 0x6B
ThunderSoft 0:53c9fac03371 123 #define MPU6050_RA_PWR_MGMT_2 0x6C
ThunderSoft 0:53c9fac03371 124 #define MPU6050_RA_BANK_SEL 0x6D
ThunderSoft 0:53c9fac03371 125 #define MPU6050_RA_MEM_START_ADDR 0x6E
ThunderSoft 0:53c9fac03371 126 #define MPU6050_RA_MEM_R_W 0x6F
ThunderSoft 0:53c9fac03371 127 #define MPU6050_RA_DMP_CFG_1 0x70
ThunderSoft 0:53c9fac03371 128 #define MPU6050_RA_DMP_CFG_2 0x71
ThunderSoft 0:53c9fac03371 129 #define MPU6050_RA_FIFO_COUNTH 0x72
ThunderSoft 0:53c9fac03371 130 #define MPU6050_RA_FIFO_COUNTL 0x73
ThunderSoft 0:53c9fac03371 131 #define MPU6050_RA_FIFO_R_W 0x74
ThunderSoft 0:53c9fac03371 132 #define MPU6050_RA_WHO_AM_I 0x75
ThunderSoft 0:53c9fac03371 133
ThunderSoft 0:53c9fac03371 134
ThunderSoft 0:53c9fac03371 135
ThunderSoft 0:53c9fac03371 136
ThunderSoft 0:53c9fac03371 137 #define MPU6050_TC_PWR_MODE_BIT 7
ThunderSoft 0:53c9fac03371 138 #define MPU6050_TC_OFFSET_BIT 6
ThunderSoft 0:53c9fac03371 139 #define MPU6050_TC_OFFSET_LENGTH 6
ThunderSoft 0:53c9fac03371 140 #define MPU6050_TC_OTP_BNK_VLD_BIT 0
ThunderSoft 0:53c9fac03371 141
ThunderSoft 0:53c9fac03371 142 #define MPU6050_VDDIO_LEVEL_VLOGIC 0
ThunderSoft 0:53c9fac03371 143 #define MPU6050_VDDIO_LEVEL_VDD 1
ThunderSoft 0:53c9fac03371 144
ThunderSoft 0:53c9fac03371 145 #define MPU6050_CFG_EXT_SYNC_SET_BIT 5
ThunderSoft 0:53c9fac03371 146 #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
ThunderSoft 0:53c9fac03371 147 #define MPU6050_CFG_DLPF_CFG_BIT 2
ThunderSoft 0:53c9fac03371 148 #define MPU6050_CFG_DLPF_CFG_LENGTH 3
ThunderSoft 0:53c9fac03371 149
ThunderSoft 0:53c9fac03371 150 #define MPU6050_EXT_SYNC_DISABLED 0x0
ThunderSoft 0:53c9fac03371 151 #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
ThunderSoft 0:53c9fac03371 152 #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
ThunderSoft 0:53c9fac03371 153 #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
ThunderSoft 0:53c9fac03371 154 #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
ThunderSoft 0:53c9fac03371 155 #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
ThunderSoft 0:53c9fac03371 156 #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
ThunderSoft 0:53c9fac03371 157 #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
ThunderSoft 0:53c9fac03371 158
ThunderSoft 0:53c9fac03371 159 #define MPU6050_DLPF_BW_256 0x00
ThunderSoft 0:53c9fac03371 160 #define MPU6050_DLPF_BW_188 0x01
ThunderSoft 0:53c9fac03371 161 #define MPU6050_DLPF_BW_98 0x02
ThunderSoft 0:53c9fac03371 162 #define MPU6050_DLPF_BW_42 0x03
ThunderSoft 0:53c9fac03371 163 #define MPU6050_DLPF_BW_20 0x04
ThunderSoft 0:53c9fac03371 164 #define MPU6050_DLPF_BW_10 0x05
ThunderSoft 0:53c9fac03371 165 #define MPU6050_DLPF_BW_5 0x06
ThunderSoft 0:53c9fac03371 166
ThunderSoft 0:53c9fac03371 167 #define MPU6050_GCONFIG_FS_SEL_BIT 4
ThunderSoft 0:53c9fac03371 168 #define MPU6050_GCONFIG_FS_SEL_LENGTH 2
ThunderSoft 0:53c9fac03371 169
ThunderSoft 0:53c9fac03371 170 #define MPU6050_GYRO_FS_250 0x00
ThunderSoft 0:53c9fac03371 171 #define MPU6050_GYRO_FS_500 0x01
ThunderSoft 0:53c9fac03371 172 #define MPU6050_GYRO_FS_1000 0x02
ThunderSoft 0:53c9fac03371 173 #define MPU6050_GYRO_FS_2000 0x03
ThunderSoft 0:53c9fac03371 174
ThunderSoft 0:53c9fac03371 175 #define MPU6050_ACONFIG_XA_ST_BIT 7
ThunderSoft 0:53c9fac03371 176 #define MPU6050_ACONFIG_YA_ST_BIT 6
ThunderSoft 0:53c9fac03371 177 #define MPU6050_ACONFIG_ZA_ST_BIT 5
ThunderSoft 0:53c9fac03371 178 #define MPU6050_ACONFIG_AFS_SEL_BIT 4
ThunderSoft 0:53c9fac03371 179 #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
ThunderSoft 0:53c9fac03371 180 #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
ThunderSoft 0:53c9fac03371 181 #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
ThunderSoft 0:53c9fac03371 182
ThunderSoft 0:53c9fac03371 183 #define MPU6050_ACCEL_FS_2 0x00
ThunderSoft 0:53c9fac03371 184 #define MPU6050_ACCEL_FS_4 0x01
ThunderSoft 0:53c9fac03371 185 #define MPU6050_ACCEL_FS_8 0x02
ThunderSoft 0:53c9fac03371 186 #define MPU6050_ACCEL_FS_16 0x03
ThunderSoft 0:53c9fac03371 187
ThunderSoft 0:53c9fac03371 188 #define MPU6050_DHPF_RESET 0x00
ThunderSoft 0:53c9fac03371 189 #define MPU6050_DHPF_5 0x01
ThunderSoft 0:53c9fac03371 190 #define MPU6050_DHPF_2P5 0x02
ThunderSoft 0:53c9fac03371 191 #define MPU6050_DHPF_1P25 0x03
ThunderSoft 0:53c9fac03371 192 #define MPU6050_DHPF_0P63 0x04
ThunderSoft 0:53c9fac03371 193 #define MPU6050_DHPF_HOLD 0x07
ThunderSoft 0:53c9fac03371 194
ThunderSoft 0:53c9fac03371 195 #define MPU6050_TEMP_FIFO_EN_BIT 7
ThunderSoft 0:53c9fac03371 196 #define MPU6050_XG_FIFO_EN_BIT 6
ThunderSoft 0:53c9fac03371 197 #define MPU6050_YG_FIFO_EN_BIT 5
ThunderSoft 0:53c9fac03371 198 #define MPU6050_ZG_FIFO_EN_BIT 4
ThunderSoft 0:53c9fac03371 199 #define MPU6050_ACCEL_FIFO_EN_BIT 3
ThunderSoft 0:53c9fac03371 200 #define MPU6050_SLV2_FIFO_EN_BIT 2
ThunderSoft 0:53c9fac03371 201 #define MPU6050_SLV1_FIFO_EN_BIT 1
ThunderSoft 0:53c9fac03371 202 #define MPU6050_SLV0_FIFO_EN_BIT 0
ThunderSoft 0:53c9fac03371 203
ThunderSoft 0:53c9fac03371 204 #define MPU6050_MULT_MST_EN_BIT 7
ThunderSoft 0:53c9fac03371 205 #define MPU6050_WAIT_FOR_ES_BIT 6
ThunderSoft 0:53c9fac03371 206 #define MPU6050_SLV_3_FIFO_EN_BIT 5
ThunderSoft 0:53c9fac03371 207 #define MPU6050_I2C_MST_P_NSR_BIT 4
ThunderSoft 0:53c9fac03371 208 #define MPU6050_I2C_MST_CLK_BIT 3
ThunderSoft 0:53c9fac03371 209 #define MPU6050_I2C_MST_CLK_LENGTH 4
ThunderSoft 0:53c9fac03371 210
ThunderSoft 0:53c9fac03371 211 #define MPU6050_CLOCK_DIV_348 0x0
ThunderSoft 0:53c9fac03371 212 #define MPU6050_CLOCK_DIV_333 0x1
ThunderSoft 0:53c9fac03371 213 #define MPU6050_CLOCK_DIV_320 0x2
ThunderSoft 0:53c9fac03371 214 #define MPU6050_CLOCK_DIV_308 0x3
ThunderSoft 0:53c9fac03371 215 #define MPU6050_CLOCK_DIV_296 0x4
ThunderSoft 0:53c9fac03371 216 #define MPU6050_CLOCK_DIV_286 0x5
ThunderSoft 0:53c9fac03371 217 #define MPU6050_CLOCK_DIV_276 0x6
ThunderSoft 0:53c9fac03371 218 #define MPU6050_CLOCK_DIV_267 0x7
ThunderSoft 0:53c9fac03371 219 #define MPU6050_CLOCK_DIV_258 0x8
ThunderSoft 0:53c9fac03371 220 #define MPU6050_CLOCK_DIV_500 0x9
ThunderSoft 0:53c9fac03371 221 #define MPU6050_CLOCK_DIV_471 0xA
ThunderSoft 0:53c9fac03371 222 #define MPU6050_CLOCK_DIV_444 0xB
ThunderSoft 0:53c9fac03371 223 #define MPU6050_CLOCK_DIV_421 0xC
ThunderSoft 0:53c9fac03371 224 #define MPU6050_CLOCK_DIV_400 0xD
ThunderSoft 0:53c9fac03371 225 #define MPU6050_CLOCK_DIV_381 0xE
ThunderSoft 0:53c9fac03371 226 #define MPU6050_CLOCK_DIV_364 0xF
ThunderSoft 0:53c9fac03371 227
ThunderSoft 0:53c9fac03371 228 #define MPU6050_I2C_SLV_RW_BIT 7
ThunderSoft 0:53c9fac03371 229 #define MPU6050_I2C_SLV_ADDR_BIT 6
ThunderSoft 0:53c9fac03371 230 #define MPU6050_I2C_SLV_ADDR_LENGTH 7
ThunderSoft 0:53c9fac03371 231 #define MPU6050_I2C_SLV_EN_BIT 7
ThunderSoft 0:53c9fac03371 232 #define MPU6050_I2C_SLV_BYTE_SW_BIT 6
ThunderSoft 0:53c9fac03371 233 #define MPU6050_I2C_SLV_REG_DIS_BIT 5
ThunderSoft 0:53c9fac03371 234 #define MPU6050_I2C_SLV_GRP_BIT 4
ThunderSoft 0:53c9fac03371 235 #define MPU6050_I2C_SLV_LEN_BIT 3
ThunderSoft 0:53c9fac03371 236 #define MPU6050_I2C_SLV_LEN_LENGTH 4
ThunderSoft 0:53c9fac03371 237
ThunderSoft 0:53c9fac03371 238 #define MPU6050_I2C_SLV4_RW_BIT 7
ThunderSoft 0:53c9fac03371 239 #define MPU6050_I2C_SLV4_ADDR_BIT 6
ThunderSoft 0:53c9fac03371 240 #define MPU6050_I2C_SLV4_ADDR_LENGTH 7
ThunderSoft 0:53c9fac03371 241 #define MPU6050_I2C_SLV4_EN_BIT 7
ThunderSoft 0:53c9fac03371 242 #define MPU6050_I2C_SLV4_INT_EN_BIT 6
ThunderSoft 0:53c9fac03371 243 #define MPU6050_I2C_SLV4_REG_DIS_BIT 5
ThunderSoft 0:53c9fac03371 244 #define MPU6050_I2C_SLV4_MST_DLY_BIT 4
ThunderSoft 0:53c9fac03371 245 #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
ThunderSoft 0:53c9fac03371 246
ThunderSoft 0:53c9fac03371 247 #define MPU6050_MST_PASS_THROUGH_BIT 7
ThunderSoft 0:53c9fac03371 248 #define MPU6050_MST_I2C_SLV4_DONE_BIT 6
ThunderSoft 0:53c9fac03371 249 #define MPU6050_MST_I2C_LOST_ARB_BIT 5
ThunderSoft 0:53c9fac03371 250 #define MPU6050_MST_I2C_SLV4_NACK_BIT 4
ThunderSoft 0:53c9fac03371 251 #define MPU6050_MST_I2C_SLV3_NACK_BIT 3
ThunderSoft 0:53c9fac03371 252 #define MPU6050_MST_I2C_SLV2_NACK_BIT 2
ThunderSoft 0:53c9fac03371 253 #define MPU6050_MST_I2C_SLV1_NACK_BIT 1
ThunderSoft 0:53c9fac03371 254 #define MPU6050_MST_I2C_SLV0_NACK_BIT 0
ThunderSoft 0:53c9fac03371 255
ThunderSoft 0:53c9fac03371 256 #define MPU6050_INTCFG_INT_LEVEL_BIT 7
ThunderSoft 0:53c9fac03371 257 #define MPU6050_INTCFG_INT_OPEN_BIT 6
ThunderSoft 0:53c9fac03371 258 #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
ThunderSoft 0:53c9fac03371 259 #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
ThunderSoft 0:53c9fac03371 260 #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
ThunderSoft 0:53c9fac03371 261 #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
ThunderSoft 0:53c9fac03371 262 #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
ThunderSoft 0:53c9fac03371 263 #define MPU6050_INTCFG_CLKOUT_EN_BIT 0
ThunderSoft 0:53c9fac03371 264
ThunderSoft 0:53c9fac03371 265 #define MPU6050_INTMODE_ACTIVEHIGH 0x00
ThunderSoft 0:53c9fac03371 266 #define MPU6050_INTMODE_ACTIVELOW 0x01
ThunderSoft 0:53c9fac03371 267
ThunderSoft 0:53c9fac03371 268 #define MPU6050_INTDRV_PUSHPULL 0x00
ThunderSoft 0:53c9fac03371 269 #define MPU6050_INTDRV_OPENDRAIN 0x01
ThunderSoft 0:53c9fac03371 270
ThunderSoft 0:53c9fac03371 271 #define MPU6050_INTLATCH_50USPULSE 0x00
ThunderSoft 0:53c9fac03371 272 #define MPU6050_INTLATCH_WAITCLEAR 0x01
ThunderSoft 0:53c9fac03371 273
ThunderSoft 0:53c9fac03371 274 #define MPU6050_INTCLEAR_STATUSREAD 0x00
ThunderSoft 0:53c9fac03371 275 #define MPU6050_INTCLEAR_ANYREAD 0x01
ThunderSoft 0:53c9fac03371 276
ThunderSoft 0:53c9fac03371 277 #define MPU6050_INTERRUPT_FF_BIT 7
ThunderSoft 0:53c9fac03371 278 #define MPU6050_INTERRUPT_MOT_BIT 6
ThunderSoft 0:53c9fac03371 279 #define MPU6050_INTERRUPT_ZMOT_BIT 5
ThunderSoft 0:53c9fac03371 280 #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
ThunderSoft 0:53c9fac03371 281 #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
ThunderSoft 0:53c9fac03371 282 #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
ThunderSoft 0:53c9fac03371 283 #define MPU6050_INTERRUPT_DMP_INT_BIT 1
ThunderSoft 0:53c9fac03371 284 #define MPU6050_INTERRUPT_DATA_RDY_BIT 0
ThunderSoft 0:53c9fac03371 285
ThunderSoft 0:53c9fac03371 286 // TODO: figure out what these actually do
ThunderSoft 0:53c9fac03371 287 // UMPL source code is not very obivous
ThunderSoft 0:53c9fac03371 288 #define MPU6050_DMPINT_5_BIT 5
ThunderSoft 0:53c9fac03371 289 #define MPU6050_DMPINT_4_BIT 4
ThunderSoft 0:53c9fac03371 290 #define MPU6050_DMPINT_3_BIT 3
ThunderSoft 0:53c9fac03371 291 #define MPU6050_DMPINT_2_BIT 2
ThunderSoft 0:53c9fac03371 292 #define MPU6050_DMPINT_1_BIT 1
ThunderSoft 0:53c9fac03371 293 #define MPU6050_DMPINT_0_BIT 0
ThunderSoft 0:53c9fac03371 294
ThunderSoft 0:53c9fac03371 295 #define MPU6050_MOTION_MOT_XNEG_BIT 7
ThunderSoft 0:53c9fac03371 296 #define MPU6050_MOTION_MOT_XPOS_BIT 6
ThunderSoft 0:53c9fac03371 297 #define MPU6050_MOTION_MOT_YNEG_BIT 5
ThunderSoft 0:53c9fac03371 298 #define MPU6050_MOTION_MOT_YPOS_BIT 4
ThunderSoft 0:53c9fac03371 299 #define MPU6050_MOTION_MOT_ZNEG_BIT 3
ThunderSoft 0:53c9fac03371 300 #define MPU6050_MOTION_MOT_ZPOS_BIT 2
ThunderSoft 0:53c9fac03371 301 #define MPU6050_MOTION_MOT_ZRMOT_BIT 0
ThunderSoft 0:53c9fac03371 302
ThunderSoft 0:53c9fac03371 303 #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
ThunderSoft 0:53c9fac03371 304 #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
ThunderSoft 0:53c9fac03371 305 #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3
ThunderSoft 0:53c9fac03371 306 #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2
ThunderSoft 0:53c9fac03371 307 #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1
ThunderSoft 0:53c9fac03371 308 #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0
ThunderSoft 0:53c9fac03371 309
ThunderSoft 0:53c9fac03371 310 #define MPU6050_PATHRESET_GYRO_RESET_BIT 2
ThunderSoft 0:53c9fac03371 311 #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
ThunderSoft 0:53c9fac03371 312 #define MPU6050_PATHRESET_TEMP_RESET_BIT 0
ThunderSoft 0:53c9fac03371 313
ThunderSoft 0:53c9fac03371 314 #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
ThunderSoft 0:53c9fac03371 315 #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
ThunderSoft 0:53c9fac03371 316 #define MPU6050_DETECT_FF_COUNT_BIT 3
ThunderSoft 0:53c9fac03371 317 #define MPU6050_DETECT_FF_COUNT_LENGTH 2
ThunderSoft 0:53c9fac03371 318 #define MPU6050_DETECT_MOT_COUNT_BIT 1
ThunderSoft 0:53c9fac03371 319 #define MPU6050_DETECT_MOT_COUNT_LENGTH 2
ThunderSoft 0:53c9fac03371 320
ThunderSoft 0:53c9fac03371 321 #define MPU6050_DETECT_DECREMENT_RESET 0x0
ThunderSoft 0:53c9fac03371 322 #define MPU6050_DETECT_DECREMENT_1 0x1
ThunderSoft 0:53c9fac03371 323 #define MPU6050_DETECT_DECREMENT_2 0x2
ThunderSoft 0:53c9fac03371 324 #define MPU6050_DETECT_DECREMENT_4 0x3
ThunderSoft 0:53c9fac03371 325
ThunderSoft 0:53c9fac03371 326 #define MPU6050_USERCTRL_DMP_EN_BIT 7
ThunderSoft 0:53c9fac03371 327 #define MPU6050_USERCTRL_FIFO_EN_BIT 6
ThunderSoft 0:53c9fac03371 328 #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
ThunderSoft 0:53c9fac03371 329 #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
ThunderSoft 0:53c9fac03371 330 #define MPU6050_USERCTRL_DMP_RESET_BIT 3
ThunderSoft 0:53c9fac03371 331 #define MPU6050_USERCTRL_FIFO_RESET_BIT 2
ThunderSoft 0:53c9fac03371 332 #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
ThunderSoft 0:53c9fac03371 333 #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
ThunderSoft 0:53c9fac03371 334
ThunderSoft 0:53c9fac03371 335 #define MPU6050_PWR1_DEVICE_RESET_BIT 7
ThunderSoft 0:53c9fac03371 336 #define MPU6050_PWR1_SLEEP_BIT 6
ThunderSoft 0:53c9fac03371 337 #define MPU6050_PWR1_CYCLE_BIT 5
ThunderSoft 0:53c9fac03371 338 #define MPU6050_PWR1_TEMP_DIS_BIT 3
ThunderSoft 0:53c9fac03371 339 #define MPU6050_PWR1_CLKSEL_BIT 2
ThunderSoft 0:53c9fac03371 340 #define MPU6050_PWR1_CLKSEL_LENGTH 3
ThunderSoft 0:53c9fac03371 341
ThunderSoft 0:53c9fac03371 342 #define MPU6050_CLOCK_INTERNAL 0x00
ThunderSoft 0:53c9fac03371 343 #define MPU6050_CLOCK_PLL_XGYRO 0x01
ThunderSoft 0:53c9fac03371 344 #define MPU6050_CLOCK_PLL_YGYRO 0x02
ThunderSoft 0:53c9fac03371 345 #define MPU6050_CLOCK_PLL_ZGYRO 0x03
ThunderSoft 0:53c9fac03371 346 #define MPU6050_CLOCK_PLL_EXT32K 0x04
ThunderSoft 0:53c9fac03371 347 #define MPU6050_CLOCK_PLL_EXT19M 0x05
ThunderSoft 0:53c9fac03371 348 #define MPU6050_CLOCK_KEEP_RESET 0x07
ThunderSoft 0:53c9fac03371 349
ThunderSoft 0:53c9fac03371 350 #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
ThunderSoft 0:53c9fac03371 351 #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
ThunderSoft 0:53c9fac03371 352 #define MPU6050_PWR2_STBY_XA_BIT 5
ThunderSoft 0:53c9fac03371 353 #define MPU6050_PWR2_STBY_YA_BIT 4
ThunderSoft 0:53c9fac03371 354 #define MPU6050_PWR2_STBY_ZA_BIT 3
ThunderSoft 0:53c9fac03371 355 #define MPU6050_PWR2_STBY_XG_BIT 2
ThunderSoft 0:53c9fac03371 356 #define MPU6050_PWR2_STBY_YG_BIT 1
ThunderSoft 0:53c9fac03371 357 #define MPU6050_PWR2_STBY_ZG_BIT 0
ThunderSoft 0:53c9fac03371 358
ThunderSoft 0:53c9fac03371 359 #define MPU6050_WAKE_FREQ_1P25 0x0
ThunderSoft 0:53c9fac03371 360 #define MPU6050_WAKE_FREQ_2P5 0x1
ThunderSoft 0:53c9fac03371 361 #define MPU6050_WAKE_FREQ_5 0x2
ThunderSoft 0:53c9fac03371 362 #define MPU6050_WAKE_FREQ_10 0x3
ThunderSoft 0:53c9fac03371 363
ThunderSoft 0:53c9fac03371 364 #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
ThunderSoft 0:53c9fac03371 365 #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
ThunderSoft 0:53c9fac03371 366 #define MPU6050_BANKSEL_MEM_SEL_BIT 4
ThunderSoft 0:53c9fac03371 367 #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
ThunderSoft 0:53c9fac03371 368
ThunderSoft 0:53c9fac03371 369 #define MPU6050_WHO_AM_I_BIT 6
ThunderSoft 0:53c9fac03371 370 #define MPU6050_WHO_AM_I_LENGTH 6
ThunderSoft 0:53c9fac03371 371
ThunderSoft 0:53c9fac03371 372 #define MPU6050_DMP_MEMORY_BANKS 8
ThunderSoft 0:53c9fac03371 373 #define MPU6050_DMP_MEMORY_BANK_SIZE 256
ThunderSoft 0:53c9fac03371 374 #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
ThunderSoft 0:53c9fac03371 375
ThunderSoft 0:53c9fac03371 376
ThunderSoft 0:53c9fac03371 377
ThunderSoft 0:53c9fac03371 378
ThunderSoft 0:53c9fac03371 379 #define SUCCESS_STATUS 0
ThunderSoft 0:53c9fac03371 380 #define ONE_BYTE_LENGTH 1
ThunderSoft 0:53c9fac03371 381 #define TWO_BYTE_LENGTH 2
ThunderSoft 0:53c9fac03371 382 #define LOCATION_ONE 0
ThunderSoft 0:53c9fac03371 383 #define LOCATION_TWO 1
ThunderSoft 0:53c9fac03371 384
ThunderSoft 0:53c9fac03371 385 MPU6050::MPU6050(PinName sda,PinName scl) : I2C(sda,scl)
ThunderSoft 0:53c9fac03371 386 {
ThunderSoft 0:53c9fac03371 387 init_sensor();
ThunderSoft 0:53c9fac03371 388 }
ThunderSoft 0:53c9fac03371 389
ThunderSoft 0:53c9fac03371 390 MPU6050::MPU6050() : I2C(DEFAULT_SDA_PIN,DEFAULT_SCL_PIN)
ThunderSoft 0:53c9fac03371 391 {
ThunderSoft 0:53c9fac03371 392 init_sensor();
ThunderSoft 0:53c9fac03371 393 }
ThunderSoft 0:53c9fac03371 394
ThunderSoft 0:53c9fac03371 395 uint8_t MPU6050::getDeviceID()
ThunderSoft 0:53c9fac03371 396 {
ThunderSoft 0:53c9fac03371 397 char temp_data = MPU6050_RA_WHO_AM_I;
ThunderSoft 0:53c9fac03371 398 int status ;
ThunderSoft 0:53c9fac03371 399 //write address.
ThunderSoft 0:53c9fac03371 400 status = write(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
ThunderSoft 0:53c9fac03371 401 if(status != SUCCESS_STATUS)
ThunderSoft 0:53c9fac03371 402 {
ThunderSoft 0:53c9fac03371 403 return 0xFF;
ThunderSoft 0:53c9fac03371 404 }
ThunderSoft 0:53c9fac03371 405 //
ThunderSoft 0:53c9fac03371 406 status = read(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
ThunderSoft 0:53c9fac03371 407 if(status != SUCCESS_STATUS)
ThunderSoft 0:53c9fac03371 408 {
ThunderSoft 0:53c9fac03371 409 return 0xFF;
ThunderSoft 0:53c9fac03371 410 }
ThunderSoft 0:53c9fac03371 411 return temp_data;
ThunderSoft 0:53c9fac03371 412 }
ThunderSoft 0:53c9fac03371 413
ThunderSoft 0:53c9fac03371 414 uint8_t MPU6050::readByte(uint8_t register_address)
ThunderSoft 0:53c9fac03371 415 {
ThunderSoft 0:53c9fac03371 416 char temp_data = register_address;
ThunderSoft 0:53c9fac03371 417 write(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
ThunderSoft 0:53c9fac03371 418 read(MPU6050_DEFAULT_ADDRESS,&temp_data,ONE_BYTE_LENGTH);
ThunderSoft 0:53c9fac03371 419 return temp_data;
ThunderSoft 0:53c9fac03371 420 }
ThunderSoft 0:53c9fac03371 421
ThunderSoft 0:53c9fac03371 422 uint8_t MPU6050::writeByte(uint8_t register_address,uint8_t value)
ThunderSoft 0:53c9fac03371 423 {
ThunderSoft 0:53c9fac03371 424 char temp_buffer[TWO_BYTE_LENGTH];
ThunderSoft 0:53c9fac03371 425 temp_buffer[LOCATION_ONE] = register_address;
ThunderSoft 0:53c9fac03371 426 temp_buffer[LOCATION_TWO] = value;
ThunderSoft 0:53c9fac03371 427 return write(MPU6050_DEFAULT_ADDRESS,temp_buffer,TWO_BYTE_LENGTH);
ThunderSoft 0:53c9fac03371 428 }
ThunderSoft 0:53c9fac03371 429
ThunderSoft 0:53c9fac03371 430 void MPU6050::init_sensor()
ThunderSoft 0:53c9fac03371 431 {
ThunderSoft 0:53c9fac03371 432 setClockSource(MPU6050_CLOCK_PLL_XGYRO);
ThunderSoft 0:53c9fac03371 433 setFullScaleGyroRange(MPU6050_GYRO_FS_250);
ThunderSoft 0:53c9fac03371 434 setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
ThunderSoft 0:53c9fac03371 435 }
ThunderSoft 0:53c9fac03371 436
ThunderSoft 0:53c9fac03371 437 void MPU6050::setClockSource(uint8_t value)
ThunderSoft 0:53c9fac03371 438 {
ThunderSoft 0:53c9fac03371 439 writeByte(MPU6050_RA_PWR_MGMT_1,value);
ThunderSoft 0:53c9fac03371 440 }
ThunderSoft 0:53c9fac03371 441
ThunderSoft 0:53c9fac03371 442 void MPU6050::setFullScaleAccelRange(uint8_t value)
ThunderSoft 0:53c9fac03371 443 {
ThunderSoft 0:53c9fac03371 444 writeByte(MPU6050_RA_ACCEL_CONFIG,value);
ThunderSoft 0:53c9fac03371 445 }
ThunderSoft 0:53c9fac03371 446
ThunderSoft 0:53c9fac03371 447 void MPU6050::setFullScaleGyroRange(uint8_t value)
ThunderSoft 0:53c9fac03371 448 {
ThunderSoft 0:53c9fac03371 449 writeByte(MPU6050_RA_GYRO_CONFIG,value);
ThunderSoft 0:53c9fac03371 450 }
ThunderSoft 0:53c9fac03371 451
ThunderSoft 0:53c9fac03371 452 uint16_t MPU6050::getX()
ThunderSoft 0:53c9fac03371 453 {
ThunderSoft 0:53c9fac03371 454 return readByte(MPU6050_RA_ACCEL_XOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_XOUT_L);
ThunderSoft 0:53c9fac03371 455 }
ThunderSoft 0:53c9fac03371 456
ThunderSoft 0:53c9fac03371 457 uint16_t MPU6050::getY()
ThunderSoft 0:53c9fac03371 458 {
ThunderSoft 0:53c9fac03371 459 return readByte(MPU6050_RA_ACCEL_YOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_YOUT_L);
ThunderSoft 0:53c9fac03371 460 }
ThunderSoft 0:53c9fac03371 461
ThunderSoft 0:53c9fac03371 462 uint16_t MPU6050::getZ()
ThunderSoft 0:53c9fac03371 463 {
ThunderSoft 0:53c9fac03371 464 return readByte(MPU6050_RA_ACCEL_ZOUT_H) << 8 | readByte(MPU6050_RA_ACCEL_ZOUT_L);
ThunderSoft 0:53c9fac03371 465 }
ThunderSoft 0:53c9fac03371 466
ThunderSoft 0:53c9fac03371 467 uint16_t MPU6050::getGX()
ThunderSoft 0:53c9fac03371 468 {
ThunderSoft 0:53c9fac03371 469 return readByte(MPU6050_RA_GYRO_XOUT_H) << 8 | readByte(MPU6050_RA_GYRO_XOUT_L);
ThunderSoft 0:53c9fac03371 470 }
ThunderSoft 0:53c9fac03371 471
ThunderSoft 0:53c9fac03371 472 uint16_t MPU6050::getGY()
ThunderSoft 0:53c9fac03371 473 {
ThunderSoft 0:53c9fac03371 474 return readByte(MPU6050_RA_GYRO_YOUT_H) << 8 | readByte(MPU6050_RA_GYRO_YOUT_L);
ThunderSoft 0:53c9fac03371 475 }
ThunderSoft 0:53c9fac03371 476
ThunderSoft 0:53c9fac03371 477 uint16_t MPU6050::getGZ()
ThunderSoft 0:53c9fac03371 478 {
ThunderSoft 0:53c9fac03371 479 return readByte(MPU6050_RA_GYRO_ZOUT_H) << 8 | readByte(MPU6050_RA_GYRO_ZOUT_L);
ThunderSoft 0:53c9fac03371 480 }