-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathMPU60x0.h
326 lines (276 loc) · 9.36 KB
/
MPU60x0.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
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
/**
@author: AMOUSSOU Z. Kenneth
@date: 19-08-2018
@version: 1.0
@external-library:
Wire: I2C communication library
*/
#ifndef H_MPU60x0
#define H_MPU60x0
#include <Wire.h>
#if ARDUINO < 100
#include <WProgram.h>
#elif ARDUINO
#include <Arduino.h>
#endif
#ifdef ENERGIA
#include <Energia.h>
#endif
// #define DEBUG
/** Device I2C address **/
#define ADDR_L
#ifdef ADDR_L
#define ADDR 0x68 //AD0: Low
#else
#define ADDR 0x69 //AD0: High
#endif
#define SELF_TEST_X 13
#define SELF_TEST_Y 14
#define SELF_TEST_Z 15
#define SELF_TEST_A 16
#define SMPLRT_DIV 25
#define CONFIG 26
#define GYRO_CONFIG 27
#define ACCEL_CONFIG 28
#define FIFO_EN 35
#define I2C_MST_CTRL 36
#define I2C_SLV0_ADDR 37
#define I2C_SLV0_REG 38
#define I2C_SLV0_CTRL 39
#define I2C_SLV1_ADDR 40
#define I2C_SLV1_REG 41
#define I2C_SLV1_CTRL 42
#define I2C_SLV2_ADDR 43
#define I2C_SLV2_REG 44
#define I2C_SLV2_CTRL 45
#define I2C_SLV3_ADDR 46
#define I2C_SLV3_REG 47
#define I2C_SLV3_CTRL 48
#define I2C_SLV4_ADDR 49
#define I2C_SLV4_REG 50
#define I2C_SLV4_DO 51
#define I2C_SLV4_CTRL 52
#define I2C_SLV4_DI 53
#define I2C_MST_STATUS 54
#define INT_PIN_CFG 55
#define INT_ENABLE 56
#define INT_STATUS 58
#define ACCEL_XOUT_H 59
#define ACCEL_XOUT_L 60
#define ACCEL_YOUT_H 61
#define ACCEL_YOUT_L 62
#define ACCEL_ZOUT_H 63
#define ACCEL_ZOUT_L 64
#define TEMP_OUT_H 65
#define TEMP_OUT_L 66
#define GYRO_XOUT_H 67
#define GYRO_XOUT_L 68
#define GYRO_YOUT_H 69
#define GYRO_YOUT_L 70
#define GYRO_ZOUT_H 71
#define GYRO_ZOUT_L 72
#define EXT_SENS_DATA_00 73
#define EXT_SENS_DATA_01 74
#define EXT_SENS_DATA_02 75
#define EXT_SENS_DATA_03 76
#define EXT_SENS_DATA_04 77
#define EXT_SENS_DATA_05 78
#define EXT_SENS_DATA_06 79
#define EXT_SENS_DATA_07 80
#define EXT_SENS_DATA_08 81
#define EXT_SENS_DATA_09 82
#define EXT_SENS_DATA_10 83
#define EXT_SENS_DATA_11 84
#define EXT_SENS_DATA_12 85
#define EXT_SENS_DATA_13 86
#define EXT_SENS_DATA_14 87
#define EXT_SENS_DATA_15 88
#define EXT_SENS_DATA_16 89
#define EXT_SENS_DATA_17 90
#define EXT_SENS_DATA_18 91
#define EXT_SENS_DATA_19 92
#define EXT_SENS_DATA_20 93
#define EXT_SENS_DATA_21 94
#define EXT_SENS_DATA_22 95
#define EXT_SENS_DATA_23 96
#define I2C_SLV0_DO 99
#define I2C_SLV1_DO 100
#define I2C_SLV2_DO 101
#define I2C_SLV3_DO 102
#define I2C_MST_DELAY_CTRL 103
#define SIGNAL_PATH_RESET 104
#define USER_CTRL 106
#define PWR_MGMT_1 107
#define PWR_MGMT_2 108
#define FIFO_COUNT_H 114
#define FIFO_COUNT_L 115
#define FIFO_R_W 116
#define WHO_AM_I 117
#define GRAVITY 9.81
const uint16_t ACCEL_SENSITIVITY[4] = { 16384 , 8192, 4096 , 2048 };
const uint16_t GYRO_SENSITIVITY[4] = { 1310 , 655 , 328 , 164 }; // x10
typedef struct{
float accelX;
float accelY;
float accelZ;
float temp;
float gyroX;
float gyroY;
float gyroZ;
} IMU_DATA;
class MPU60x0{
public:
MPU60x0();
void begin();
void configure(uint8_t ext_sync, uint8_t digital_low_pass_filter);
bool reset(); // Reset the sensor
uint8_t whoami();
void setSampleRateDivider(uint8_t value);
float getGyroSampleRate();
float getAccelSampleRate();
void setGyroFSR(uint8_t value); // FSR: Full Scale Range
uint8_t getGyroFSR();
void setAccelFSR(uint8_t value);
uint8_t getAccelFSR();
int16_t getGyroX();
int16_t getGyroY();
int16_t getGyroZ();
bool gyroReset();
int16_t getAccelX();
int16_t getAccelY();
int16_t getAccelZ();
bool accelReset();
float getTemp();
bool disableTemp();
bool enableTemp();
bool tempReset();
bool enableGyroTest();
bool enableAccelTest();
bool enableXgFifo();
bool enableYgFifo();
bool enableZgFifo();
bool enableAccelFifo();
bool enableTempFifo();
bool enableSlave0Fifo();
bool enableSlave1Fifo();
bool enableSlave2Fifo();
bool enableSlave3Fifo();
int16_t readFifo();
bool resetFifo();
bool enableFifo();
bool disableFifo();
bool enableSleepMode();
bool disableSleepMode();
void setClock(uint8_t value);
uint8_t gyroXSelfTest();
uint8_t gyroYSelfTest();
uint8_t gyroZSelfTest();
uint8_t accelXSelfTest();
uint8_t accelYSelfTest();
uint8_t accelZSelfTest();
IMU_DATA getData(); // get all the data from the sensor
IMU_DATA read(); // compute sensor data and return them back
bool enableAccelXStandby();
bool enableAccelYStandby();
bool enableAccelZStandby();
bool enableAccelStandby();
bool enableGyroXStandby();
bool enableGyroYStandby();
bool enableGyroZStandby();
bool enableGyroStandby();
bool disableAccelXStandby();
bool disableAccelYStandby();
bool disableAccelZStandby();
bool disableAccelStandby();
bool disableGyroXStandby();
bool disableGyroYStandby();
bool disableGyroZStandby();
bool disableGyroStandby();
bool i2cMultiMasterEnable();
bool i2cMultiMasterDisable();
bool i2cMasterClock(uint8_t clock);
bool i2cSetMasterDelay(uint8_t divider);
uint8_t i2cGetMasterDelay();
float i2cGetSampleRate();
uint8_t i2cMasterStatus();
bool slave0Enable();
bool slave0Disable();
uint8_t getSlave0Register();
bool setSlave0Register(uint8_t address);
bool disableSlave0Register();
bool enableSlave0Register();
void setSlave0WordGrouping(uint8_t order);
bool enableSlave0ByteSwap();
uint8_t getSlave0DataLenght();
void setSlave0DataLength(uint8_t length);
void slave0Write(uint8_t address, uint8_t _register, uint8_t data); // not tested
bool slave1Enable();
bool slave1Disable();
uint8_t getSlave1Register();
bool setSlave1Register(uint8_t address);
bool disableSlave1Register();
bool enableSlave1Register();
void setSlave1WordGrouping(uint8_t order);
bool enableSlave1ByteSwap();
uint8_t getSlave1DataLenght();
void setSlave1DataLength(uint8_t length);
void slave1Write(uint8_t address, uint8_t _register, uint8_t data); // not tested
bool slave2Enable();
bool slave2Disable();
uint8_t getSlave2Register();
bool setSlave2Register(uint8_t address);
bool disableSlave2Register();
bool enableSlave2Register();
void setSlave2WordGrouping(uint8_t order);
bool enableSlave2ByteSwap();
uint8_t getSlave2DataLenght();
void setSlave2DataLength(uint8_t length);
void slave2Write(uint8_t address, uint8_t _register, uint8_t data); // not tested
bool slave3Enable();
bool slave3Disable();
uint8_t getSlave3Register();
bool setSlave3Register(uint8_t address);
bool disableSlave3Register();
bool enableSlave3Register();
void setSlave3WordGrouping(uint8_t order);
bool enableSlave3ByteSwap();
uint8_t getSlave3DataLenght();
void setSlave3DataLength(uint8_t length);
void slave3Write(uint8_t address, uint8_t _register, uint8_t data); // not tested
bool slave4Enable();
bool slave4Disable();
bool enableSlave4Register();
bool disableSlave4Register();
bool setSlave4Register(uint8_t address);
uint8_t getSlave4Register();
bool enableSlave4Interrupt();
bool disableSlave4Interrupt();
int getExternalSensorData(uint8_t index);
bool setInterruptLevel(bool level);
bool setInterruptPinState(bool state);
bool enableInterruptPinLatch();
bool disableInterruptPinLatch();
bool clearInterruptOn(bool reading_status);
bool enableFsyncInterrupt();
bool setFsyncInterruptLevel(bool level);
bool enableMotionDetectionInterrupt();
bool disableMotionDetectionInterrupt();
bool enableFifoOverflowInterrupt();
bool disableFifoOverflowInterrupt();
bool enableI2cMasterInterrupt();
bool disableI2cMasterInterrupt();
bool enableDataReadyInterrupt();
bool disableDataReadyInterrupt();
uint8_t getInterruptStatus();
private:
void _write(uint8_t registerAddr, uint8_t data);
uint8_t _read(uint8_t registerAddr);
void _readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t size);
uint8_t _buffer;
uint8_t _gyroFsr;
uint8_t _accelFsr;
bool _isFSRUpdated;
uint16_t _accel_sensitivity;
uint16_t _gyro_sensitivity;
};
#endif