/
MPU9250.h
264 lines (240 loc) · 8.74 KB
/
MPU9250.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
/**
* Invensense MPU-9250 library using the SPI interface
*
* Copyright (C) 2015 Brian Chen
*
* Open source under the MIT License. See LICENSE.txt.
*/
#ifndef MPU9250_h
#define MPU9250_h
#include "Arduino.h"
// #define AK8963FASTMODE
// mpu9250 registers
#define MPUREG_XG_OFFS_TC 0x00
#define MPUREG_YG_OFFS_TC 0x01
#define MPUREG_ZG_OFFS_TC 0x02
#define MPUREG_X_FINE_GAIN 0x03
#define MPUREG_Y_FINE_GAIN 0x04
#define MPUREG_Z_FINE_GAIN 0x05
#define MPUREG_XA_OFFS_H 0x06
#define MPUREG_XA_OFFS_L 0x07
#define MPUREG_YA_OFFS_H 0x08
#define MPUREG_YA_OFFS_L 0x09
#define MPUREG_ZA_OFFS_H 0x0A
#define MPUREG_ZA_OFFS_L 0x0B
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_SELF_TEST_X 0x0D
#define MPUREG_SELF_TEST_Y 0x0E
#define MPUREG_SELF_TEST_Z 0x0F
#define MPUREG_SELF_TEST_A 0x10
#define MPUREG_XG_OFFS_USRH 0x13
#define MPUREG_XG_OFFS_USRL 0x14
#define MPUREG_YG_OFFS_USRH 0x15
#define MPUREG_YG_OFFS_USRL 0x16
#define MPUREG_ZG_OFFS_USRH 0x17
#define MPUREG_ZG_OFFS_USRL 0x18
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG_2 0x1D
#define MPUREG_LP_ACCEL_ODR 0x1E
#define MPUREG_MOT_THR 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_EXT_SENS_DATA_01 0x4A
#define MPUREG_EXT_SENS_DATA_02 0x4B
#define MPUREG_EXT_SENS_DATA_03 0x4C
#define MPUREG_EXT_SENS_DATA_04 0x4D
#define MPUREG_EXT_SENS_DATA_05 0x4E
#define MPUREG_EXT_SENS_DATA_06 0x4F
#define MPUREG_EXT_SENS_DATA_07 0x50
#define MPUREG_EXT_SENS_DATA_08 0x51
#define MPUREG_EXT_SENS_DATA_09 0x52
#define MPUREG_EXT_SENS_DATA_10 0x53
#define MPUREG_EXT_SENS_DATA_11 0x54
#define MPUREG_EXT_SENS_DATA_12 0x55
#define MPUREG_EXT_SENS_DATA_13 0x56
#define MPUREG_EXT_SENS_DATA_14 0x57
#define MPUREG_EXT_SENS_DATA_15 0x58
#define MPUREG_EXT_SENS_DATA_16 0x59
#define MPUREG_EXT_SENS_DATA_17 0x5A
#define MPUREG_EXT_SENS_DATA_18 0x5B
#define MPUREG_EXT_SENS_DATA_19 0x5C
#define MPUREG_EXT_SENS_DATA_20 0x5D
#define MPUREG_EXT_SENS_DATA_21 0x5E
#define MPUREG_EXT_SENS_DATA_22 0x5F
#define MPUREG_EXT_SENS_DATA_23 0x60
#define MPUREG_I2C_SLV0_DO 0x63
#define MPUREG_I2C_SLV1_DO 0x64
#define MPUREG_I2C_SLV2_DO 0x65
#define MPUREG_I2C_SLV3_DO 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_BANK_SEL 0x6D
#define MPUREG_MEM_START_ADDR 0x6E
#define MPUREG_MEM_R_W 0x6F
#define MPUREG_DMP_CFG_1 0x70
#define MPUREG_DMP_CFG_2 0x71
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_WHOAMI 0x75
#define MPUREG_XA_OFFSET_H 0x77
#define MPUREG_XA_OFFSET_L 0x78
#define MPUREG_YA_OFFSET_H 0x7A
#define MPUREG_YA_OFFSET_L 0x7B
#define MPUREG_ZA_OFFSET_H 0x7D
#define MPUREG_ZA_OFFSET_L 0x7E
/* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
#define AK8963_I2C_ADDR 0x0c//0x18
#define AK8963_Device_ID 0x48
// Read-only Reg
#define AK8963_WIA 0x00
#define AK8963_INFO 0x01
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_HXH 0x04
#define AK8963_HYL 0x05
#define AK8963_HYH 0x06
#define AK8963_HZL 0x07
#define AK8963_HZH 0x08
#define AK8963_ST2 0x09
// Write/Read Reg
#define AK8963_CNTL1 0x0A
#define AK8963_CNTL2 0x0B
#define AK8963_ASTC 0x0C
#define AK8963_TS1 0x0D
#define AK8963_TS2 0x0E
#define AK8963_I2CDIS 0x0F
// Read-only Reg ( ROM )
#define AK8963_ASAX 0x10
#define AK8963_ASAY 0x11
#define AK8963_ASAZ 0x12
// Configuration bits mpu9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_2G 0x00
#define BITS_FS_4G 0x08
#define BITS_FS_8G 0x10
#define BITS_FS_16G 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define READ_FLAG 0x80
/* ---- Sensitivity --------------------------------------------------------- */
#define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB
#define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB
#define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB
#define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB
#define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB
#define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB
#define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB
#define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB
#define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB
#define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB
#define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)
class MPU9250 {
public:
// constructor. Default low pass filter of 188Hz
MPU9250(long clock, uint8_t cs, uint8_t low_pass_filter = BITS_DLPF_CFG_188HZ, uint8_t low_pass_filter_acc = BITS_DLPF_CFG_188HZ){
my_clock = clock;
my_cs = cs;
my_low_pass_filter = low_pass_filter;
my_low_pass_filter_acc = low_pass_filter_acc;
}
unsigned int WriteReg(uint8_t WriteAddr, uint8_t WriteData );
unsigned int ReadReg(uint8_t WriteAddr, uint8_t WriteData );
void ReadRegs(uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
bool init(bool calib_gyro = true, bool calib_acc = true);
void read_temp();
void read_acc();
void read_gyro();
unsigned int set_gyro_scale(int scale);
unsigned int set_acc_scale(int scale);
void calib_acc();
void calib_mag();
void select();
void deselect();
unsigned int whoami();
uint8_t AK8963_whoami();
uint8_t get_CNTL1();
void read_mag();
void read_all();
void calibrate(float *dest1, float *dest2);
float acc_divider;
float gyro_divider;
int calib_data[3];
float Magnetometer_ASA[3];
float accel_data[3];
float temperature;
float gyro_data[3];
float mag_data[3];
int16_t mag_data_raw[3];
private:
long my_clock;
uint8_t my_cs;
uint8_t my_low_pass_filter;
uint8_t my_low_pass_filter_acc;
//float randomstuffs[3];
float g_bias[3];
float a_bias[3]; // Bias corrections for gyro and accelerometer
};
#endif