/
MPU6050.py
371 lines (326 loc) · 18.2 KB
/
MPU6050.py
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
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
#!/usr/bin/python
import smbus
import struct
# import array
import math
# from ctypes import *
bus = smbus.SMBus(1)
class MPU6050Data:
def __init__(self):
self.Gx = 0
self.Gy = 0
self.Gz = 0
self.Temperature = 0
self.Gyrox = 0
self.Gyroy = 0
self.Gyroz = 0
class MPU6050:
AccelerationFactor = 2.0 / 32768.0; # assuming +/- 16G
GyroFactor = 500.0 / 32768.0; # assuming 500 degree / sec
# Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
TemperatureGain = 1.0 / 340.0
TemperatureOffset = 36.53
# converted from Jeff Rowberg code https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.h
MPU6050_ADDRESS = 0x68 # default I2C Address
# register definition
MPU6050_RA_XG_OFFS_TC = 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
MPU6050_RA_YG_OFFS_TC = 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
MPU6050_RA_ZG_OFFS_TC = 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
MPU6050_RA_X_FINE_GAIN = 0x03 # [7:0] X_FINE_GAIN
MPU6050_RA_Y_FINE_GAIN = 0x04 # [7:0] Y_FINE_GAIN
MPU6050_RA_Z_FINE_GAIN = 0x05 # [7:0] Z_FINE_GAIN
MPU6050_RA_XA_OFFS_H = 0x06 # [15:0] XA_OFFS
MPU6050_RA_XA_OFFS_L_TC = 0x07
MPU6050_RA_YA_OFFS_H = 0x08 # [15:0] YA_OFFS
MPU6050_RA_YA_OFFS_L_TC = 0x09
MPU6050_RA_ZA_OFFS_H = 0x0A # [15:0] ZA_OFFS
MPU6050_RA_ZA_OFFS_L_TC = 0x0B
MPU6050_RA_XG_OFFS_USRH = 0x13 # [15:0] XG_OFFS_USR
MPU6050_RA_XG_OFFS_USRL = 0x14
MPU6050_RA_YG_OFFS_USRH = 0x15 # [15:0] YG_OFFS_USR
MPU6050_RA_YG_OFFS_USRL = 0x16
MPU6050_RA_ZG_OFFS_USRH = 0x17 # [15:0] ZG_OFFS_USR
MPU6050_RA_ZG_OFFS_USRL = 0x18
MPU6050_RA_SMPLRT_DIV = 0x19
MPU6050_RA_CONFIG = 0x1A
MPU6050_RA_GYRO_CONFIG = 0x1B
MPU6050_RA_ACCEL_CONFIG = 0x1C
MPU6050_RA_FF_THR = 0x1D
MPU6050_RA_FF_DUR = 0x1E
MPU6050_RA_MOT_THR = 0x1F
MPU6050_RA_MOT_DUR = 0x20
MPU6050_RA_ZRMOT_THR = 0x21
MPU6050_RA_ZRMOT_DUR = 0x22
MPU6050_RA_FIFO_EN = 0x23
MPU6050_RA_I2C_MST_CTRL = 0x24
MPU6050_RA_I2C_SLV0_ADDR = 0x25
MPU6050_RA_I2C_SLV0_REG = 0x26
MPU6050_RA_I2C_SLV0_CTRL = 0x27
MPU6050_RA_I2C_SLV1_ADDR = 0x28
MPU6050_RA_I2C_SLV1_REG = 0x29
MPU6050_RA_I2C_SLV1_CTRL = 0x2A
MPU6050_RA_I2C_SLV2_ADDR = 0x2B
MPU6050_RA_I2C_SLV2_REG = 0x2C
MPU6050_RA_I2C_SLV2_CTRL = 0x2D
MPU6050_RA_I2C_SLV3_ADDR = 0x2E
MPU6050_RA_I2C_SLV3_REG = 0x2F
MPU6050_RA_I2C_SLV3_CTRL = 0x30
MPU6050_RA_I2C_SLV4_ADDR = 0x31
MPU6050_RA_I2C_SLV4_REG = 0x32
MPU6050_RA_I2C_SLV4_DO = 0x33
MPU6050_RA_I2C_SLV4_CTRL = 0x34
MPU6050_RA_I2C_SLV4_DI = 0x35
MPU6050_RA_I2C_MST_STATUS = 0x36
MPU6050_RA_INT_PIN_CFG = 0x37
MPU6050_RA_INT_ENABLE = 0x38
MPU6050_RA_DMP_INT_STATUS = 0x39
MPU6050_RA_INT_STATUS = 0x3A
MPU6050_RA_ACCEL_XOUT_H = 0x3B
MPU6050_RA_ACCEL_XOUT_L = 0x3C
MPU6050_RA_ACCEL_YOUT_H = 0x3D
MPU6050_RA_ACCEL_YOUT_L = 0x3E
MPU6050_RA_ACCEL_ZOUT_H = 0x3F
MPU6050_RA_ACCEL_ZOUT_L = 0x40
MPU6050_RA_TEMP_OUT_H = 0x41
MPU6050_RA_TEMP_OUT_L = 0x42
MPU6050_RA_GYRO_XOUT_H = 0x43
MPU6050_RA_GYRO_XOUT_L = 0x44
MPU6050_RA_GYRO_YOUT_H = 0x45
MPU6050_RA_GYRO_YOUT_L = 0x46
MPU6050_RA_GYRO_ZOUT_H = 0x47
MPU6050_RA_GYRO_ZOUT_L = 0x48
MPU6050_RA_EXT_SENS_DATA_00 = 0x49
MPU6050_RA_EXT_SENS_DATA_01 = 0x4A
MPU6050_RA_EXT_SENS_DATA_02 = 0x4B
MPU6050_RA_EXT_SENS_DATA_03 = 0x4C
MPU6050_RA_EXT_SENS_DATA_04 = 0x4D
MPU6050_RA_EXT_SENS_DATA_05 = 0x4E
MPU6050_RA_EXT_SENS_DATA_06 = 0x4F
MPU6050_RA_EXT_SENS_DATA_07 = 0x50
MPU6050_RA_EXT_SENS_DATA_08 = 0x51
MPU6050_RA_EXT_SENS_DATA_09 = 0x52
MPU6050_RA_EXT_SENS_DATA_10 = 0x53
MPU6050_RA_EXT_SENS_DATA_11 = 0x54
MPU6050_RA_EXT_SENS_DATA_12 = 0x55
MPU6050_RA_EXT_SENS_DATA_13 = 0x56
MPU6050_RA_EXT_SENS_DATA_14 = 0x57
MPU6050_RA_EXT_SENS_DATA_15 = 0x58
MPU6050_RA_EXT_SENS_DATA_16 = 0x59
MPU6050_RA_EXT_SENS_DATA_17 = 0x5A
MPU6050_RA_EXT_SENS_DATA_18 = 0x5B
MPU6050_RA_EXT_SENS_DATA_19 = 0x5C
MPU6050_RA_EXT_SENS_DATA_20 = 0x5D
MPU6050_RA_EXT_SENS_DATA_21 = 0x5E
MPU6050_RA_EXT_SENS_DATA_22 = 0x5F
MPU6050_RA_EXT_SENS_DATA_23 = 0x60
MPU6050_RA_MOT_DETECT_STATUS = 0x61
MPU6050_RA_I2C_SLV0_DO = 0x63
MPU6050_RA_I2C_SLV1_DO = 0x64
MPU6050_RA_I2C_SLV2_DO = 0x65
MPU6050_RA_I2C_SLV3_DO = 0x66
MPU6050_RA_I2C_MST_DELAY_CTRL = 0x67
MPU6050_RA_SIGNAL_PATH_RESET = 0x68
MPU6050_RA_MOT_DETECT_CTRL = 0x69
MPU6050_RA_USER_CTRL = 0x6A
MPU6050_RA_PWR_MGMT_1 = 0x6B
MPU6050_RA_PWR_MGMT_2 = 0x6C
MPU6050_RA_BANK_SEL = 0x6D
MPU6050_RA_MEM_START_ADDR = 0x6E
MPU6050_RA_MEM_R_W = 0x6F
MPU6050_RA_DMP_CFG_1 = 0x70
MPU6050_RA_DMP_CFG_2 = 0x71
MPU6050_RA_FIFO_COUNTH = 0x72
MPU6050_RA_FIFO_COUNTL = 0x73
MPU6050_RA_FIFO_R_W = 0x74
MPU6050_RA_WHO_AM_I = 0x75
ZeroRegister = [
MPU6050_RA_FF_THR, # Freefall threshold of |0mg| LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
MPU6050_RA_FF_DUR, # Freefall duration limit of 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
MPU6050_RA_MOT_THR, # Motion threshold of 0mg LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
MPU6050_RA_MOT_DUR, # Motion duration of 0s LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
MPU6050_RA_ZRMOT_THR, # Zero motion threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
MPU6050_RA_ZRMOT_DUR,
# Zero motion duration threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
MPU6050_RA_FIFO_EN,
# Disable sensor output to FIFO buffer LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
MPU6050_RA_I2C_MST_CTRL,
# AUX I2C setup //Sets AUX I2C to single master control, plus other config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
MPU6050_RA_I2C_SLV0_ADDR,
# Setup AUX I2C slaves LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
MPU6050_RA_I2C_SLV0_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
MPU6050_RA_I2C_SLV0_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
MPU6050_RA_I2C_SLV1_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
MPU6050_RA_I2C_SLV1_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
MPU6050_RA_I2C_SLV1_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
MPU6050_RA_I2C_SLV2_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
MPU6050_RA_I2C_SLV2_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
MPU6050_RA_I2C_SLV2_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
MPU6050_RA_I2C_SLV3_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
MPU6050_RA_I2C_SLV3_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
MPU6050_RA_I2C_SLV3_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
MPU6050_RA_I2C_SLV4_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
MPU6050_RA_I2C_SLV4_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
MPU6050_RA_I2C_SLV4_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_DI, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
MPU6050_RA_INT_PIN_CFG,
# MPU6050_RA_I2C_MST_STATUS //Read-only //Setup INT pin and AUX I2C pass through LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
MPU6050_RA_INT_ENABLE,
# Enable data ready interrupt LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
MPU6050_RA_I2C_SLV0_DO,
# Slave out, dont care LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
MPU6050_RA_I2C_SLV1_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
MPU6050_RA_I2C_SLV2_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
MPU6050_RA_I2C_SLV3_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
MPU6050_RA_I2C_MST_DELAY_CTRL,
# More slave config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
MPU6050_RA_SIGNAL_PATH_RESET,
# Reset sensor signal paths LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
MPU6050_RA_MOT_DETECT_CTRL,
# Motion detection control LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
MPU6050_RA_USER_CTRL,
# Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
MPU6050_RA_CONFIG, # Disable FSync, 256Hz DLPF LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x00);
MPU6050_RA_FF_THR, # Freefall threshold of |0mg| LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
MPU6050_RA_FF_DUR, # Freefall duration limit of 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
MPU6050_RA_MOT_THR, # Motion threshold of 0mg LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
MPU6050_RA_MOT_DUR, # Motion duration of 0s LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
MPU6050_RA_ZRMOT_THR, # Zero motion threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
MPU6050_RA_ZRMOT_DUR,
# Zero motion duration threshold LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
MPU6050_RA_FIFO_EN,
# Disable sensor output to FIFO buffer LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
MPU6050_RA_I2C_MST_CTRL,
# AUX I2C setup //Sets AUX I2C to single master control, plus other config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
MPU6050_RA_I2C_SLV0_ADDR,
# Setup AUX I2C slaves LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
MPU6050_RA_I2C_SLV0_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
MPU6050_RA_I2C_SLV0_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
MPU6050_RA_I2C_SLV1_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
MPU6050_RA_I2C_SLV1_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
MPU6050_RA_I2C_SLV1_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
MPU6050_RA_I2C_SLV2_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
MPU6050_RA_I2C_SLV2_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
MPU6050_RA_I2C_SLV2_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
MPU6050_RA_I2C_SLV3_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
MPU6050_RA_I2C_SLV3_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
MPU6050_RA_I2C_SLV3_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_ADDR, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
MPU6050_RA_I2C_SLV4_REG, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
MPU6050_RA_I2C_SLV4_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
MPU6050_RA_I2C_SLV4_CTRL, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
MPU6050_RA_I2C_SLV4_DI, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
MPU6050_RA_I2C_SLV0_DO,
# Slave out, dont care LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
MPU6050_RA_I2C_SLV1_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
MPU6050_RA_I2C_SLV2_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
MPU6050_RA_I2C_SLV3_DO, # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
MPU6050_RA_I2C_MST_DELAY_CTRL,
# More slave config LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
MPU6050_RA_SIGNAL_PATH_RESET,
# Reset sensor signal paths LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
MPU6050_RA_MOT_DETECT_CTRL,
# Motion detection control LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
MPU6050_RA_USER_CTRL,
# Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
MPU6050_RA_INT_PIN_CFG,
# MPU6050_RA_I2C_MST_STATUS //Read-only //Setup INT pin and AUX I2C pass through LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
MPU6050_RA_INT_ENABLE,
# Enable data ready interrupt LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
MPU6050_RA_FIFO_R_W] # LDByteWriteI2C(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
def setup(self):
self.setSampleRate(1000)
self.setGResolution(2)
# Disable gyro self tests, scale of 500 degrees/s
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_GYRO_CONFIG, 0b00001000)
for loop in self.ZeroRegister:
bus.write_byte_data(self.MPU6050_ADDRESS, loop, 0)
# Sets clock source to gyro reference w/ PLL
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_PWR_MGMT_1, 0b00000010)
# Controls frequency of wakeups in accel low power mode plus the sensor standby modes
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_PWR_MGMT_2, 0x00)
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_INT_ENABLE, 0x01)
def __init__(self):
self.setup()
self.readStatus()
self.fifoCount = 0
def readNFromFifo(self, bytesToRead):
return bus.read_i2c_block_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_R_W, bytesToRead)
def readDataFromFifo(self):
# first check how many bytes in temporary fifo counter
while True:
if self.fifoCount == 0:
self.fifoCount = self.readFifoCount()
# max block transfer in i2c is 32 bytes including the address
# accelerometer, gyro and temperature data=> 7 short = 14 bytes => 31 bytes / 14 = 2
# then it will be 28
if (self.fifoCount > 28):
nCount = 28
else:
nCount = self.fifoCount
GData = bus.read_i2c_block_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_R_W, nCount)
self.fifoCount = self.fifoCount - nCount
return GData
def readData(self):
# read accelerometers , temperature and gyro
GData = bus.read_i2c_block_data(self.MPU6050_ADDRESS, self.MPU6050_RA_ACCEL_XOUT_H, 14)
# convert list of 14 values bytes into MPU6050Data struct in engineering units
return self.convertData(GData)
def convertData(self, ListData):
ShortData = struct.unpack(">hhhhhhh", buffer(bytearray(ListData)))
# lets create the Data Class
AccData = MPU6050Data()
# first 3 short value are Accelerometer
AccData.Gx = ShortData[0] * self.AccelerationFactor
AccData.Gy = ShortData[1] * self.AccelerationFactor
AccData.Gz = ShortData[2] * self.AccelerationFactor
# temperature
AccData.Temperature = ShortData[3] * self.TemperatureGain + self.TemperatureOffset
# and the 3 last ar'e the gyro data
AccData.Gyrox = ShortData[4] * self.GyroFactor
AccData.Gyroy = ShortData[5] * self.GyroFactor
AccData.Gyroz = ShortData[6] * self.GyroFactor
return AccData
def setGResolution(self, value):
# use dictionary to get correct G resolution 2,4,8 or 16G
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_GYRO_CONFIG, {2: 0, 4: 8, 8: 16, 16: 24}[value])
self.AccelerationFactor = value / 32768.0;
# sets the sample rate in samples per second when DLP is used
def setSampleRateWithDLP(self, rate):
rateVal = int((1000 / rate) - 1)
self.SampleRate = 1000.0 / (rate + 1.0)
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_SMPLRT_DIV, rateVal)
def setSampleRate(self, Rate):
SampleReg = int((8000 / Rate) - 1)
self.SampleRate = 8000.0 / (SampleReg + 1.0)
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_SMPLRT_DIV, SampleReg)
def readStatus(self):
return bus.read_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_INT_STATUS)
def readFifoCount(self):
GData = bus.read_i2c_block_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_COUNTH)
# GData=bus.read_i2c_block_data(self.MPU6050_ADDRESS,self.MPU6050_RA_FIFO_COUNTH,2)
self.fifoCount = (GData[0] * 256 + GData[1])
return self.fifoCount
def readFifo(self, ByteCount):
GData = bus.read_i2c_block_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_R_W, ByteCount)
return GData
def resetFifo(self):
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_USER_CTRL, 0b00000000)
pass
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_USER_CTRL, 0b00000100)
pass
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_USER_CTRL, 0b01000000)
def setLowPass5Hz(self):
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_CONFIG, 0b00000110)
def enableFifo(self, flag):
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_EN, 0)
if flag:
self.resetFifo()
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_EN, 0b11111000)
def enableFifoAccelOnly(self, flag):
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_EN, 0)
if flag:
self.resetFifo()
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_EN, 0b00001000)
def disableFifoWrites(self):
bus.write_byte_data(self.MPU6050_ADDRESS, self.MPU6050_RA_FIFO_EN, 0)